基本概念
-
刚体的概念
- 在任何力的作用下，体积和形状都不发生改变的物体叫做刚体(Rigid body)。它是力学中的一个科学抽象概念，即理想模型。事实上任何物体受到外力，不可能不改变形状。
在上面的这里理想模型条件下来描述机器人的运动，也就是刚体运动
-
位姿：位置和姿态
-
位置：它是指机器人在当前环境中处于哪个地方
-
姿态：它是指机器人的朝向。
-
通俗理解就是说机器人当前在什么地方，机器人面向哪里
-
-
向量：带箭头的线段，它有大小和方向
-
坐标：坐标是具体的取值，我们只有指定了了在哪个坐标系下，我们讨论坐标才有意义。
- 这里我们来举个例子：该空间的基为\(\left(e_{1}, e_{2}, e_{3}\right)\)，此时\(\left(a_{1}, a_{2}, a_{3}\right)^{\mathrm{T}}\)就为\(a\)在这个基下的坐标。
\[\boldsymbol{a}=\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l}
a_{1} \\
a_{2} \\
a_{3}
\end{array}\right]=a_{1} \boldsymbol{e}_{1}+a_{2} \boldsymbol{e}_{2}+a_{3} \boldsymbol{e}_{3}
\]
-
接下来我们讨论一下向量的运算。
- 内积：\(\boldsymbol{a} \cdot \boldsymbol{b}=\boldsymbol{a}^{\mathrm{T}} \boldsymbol{b}=\sum_{i=1}^{3} a_{i} b_{i}=|\boldsymbol{a}||\boldsymbol{b}| \cos \langle\boldsymbol{a}, \boldsymbol{b}\rangle\)
- 外积：外积的几何意义是生成的向量垂直于向量a和向量b
\[\boldsymbol{a} \times \boldsymbol{b}=\left\|\begin{array}{lll}
\boldsymbol{e}_{1} & \boldsymbol{e}_{2} & \boldsymbol{e}_{3} \\
a_{1} & a_{2} & a_{3} \\
b_{1} & b_{2} & b_{3}
\end{array}\right\|=\left[\begin{array}{c}
a_{2} b_{3}-a_{3} b_{2} \\
a_{3} b_{1}-a_{1} b_{3} \\
a_{1} b_{2}-a_{2} b_{1}
\end{array}\right]=\left[\begin{array}{ccc}
0 & -a_{3} & a_{2} \\
a_{3} & 0 & -a_{1} \\
-a_{2} & a_{1} & 0
\end{array}\right] \boldsymbol{b} \stackrel{\text { def }}{=} \boldsymbol{a}^{\wedge} \boldsymbol{b}
\]
这里我们引入一个反对称符号^，此时可以将\(a\)写成一个矩阵，即为\(\boldsymbol{a}^{\wedge}\)。\(\boldsymbol{a}^{\wedge}\)计作向量向量\(a\)的反对称矩阵。
\[\boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc}
0 & -a_{3} & a_{2} \\
a_{3} & 0 & -a_{1} \\
-a_{2} & a_{1} & 0
\end{array}\right]
\]
该空间的基为\(\left(e_{1}, e_{2}, e_{3}\right)\)，此时\(\left(a_{1}, a_{2}, a_{3}\right)^{\mathrm{T}}\)就为\(a\)在这个基下的坐标。
- 世界坐标系：当前环境中运动的机器人，此时我们设定一个世界坐标系，认为它是固定不动的。
- 移动坐标系：机器人以自己为原点建立的坐标系，但是这个坐标系是随着运动而变化的。通常我们是先获取
该点对移动坐标系的坐标值，在根据机器人位姿变换到世界坐标系中。
-
欧式变换：假设一个向量在各个坐标系下长度和角度都不发生任何变化，我们从坐标系A移动到坐标系B即为欧式变换。
- 通常欧式变换是通过旋转加平移来完成操作。
旋转矩阵
- 假设向量\(a\)从坐标系\(A\)变换到坐标系\(B\)，它在两个两个坐标系下的坐标分别为\(\left[a_{1}, a_{2}, a_{3}\right]^{\mathrm{T}}\)和 \(\left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{\mathrm{T}}\)，，则有
\[\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l}
a_{1} \\
a_{2} \\
a_{3}
\end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c}
a_{1}^{\prime} \\
a_{2}^{\prime} \\
a_{3}^{\prime}
\end{array}\right]
\]
然后我们将其化简，两遍同时左乘\(\left[\begin{array}{c}\boldsymbol{e}_{1}^{\mathrm{T}} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \\ \boldsymbol{e}_{3}^{\mathrm{T}}\end{array}\right]\)，则有
\[\left[\begin{array}{l}
a_{1} \\
a_{2} \\
a_{3}
\end{array}\right]=\left[\begin{array}{lll}
e_{1}^{\mathrm{T}} e_{1}^{\prime} & e_{1}^{\mathrm{T}} e_{2}^{\prime} & e_{1}^{\mathrm{T}} e_{3}^{\prime} \\
e_{2}^{\mathrm{T}} e_{1}^{\prime} & e_{2}^{\mathrm{T}} e_{2}^{\prime} & e_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\
e_{3}^{\mathrm{T}} e_{1}^{\prime} & e_{3}^{\mathrm{T}} e_{2}^{\prime} & e_{3}^{\mathrm{T}} e_{3}^{\prime}
\end{array}\right]\left[\begin{array}{l}
a_{1}^{\prime} \\
a_{2}^{\prime} \\
a_{3}^{\prime}
\end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{R a}^{\prime}
\]
此时\(R\)即为旋转矩阵。
变换矩阵
- 在欧式变换中除了有旋转还要有平移，旋转用矩阵R来表示，平移用向量t来表示，则有
\[a^{\prime}=R a+t
\]
上面这样操作多次显得啰嗦，这里引入其次坐标和变换矩阵。
我们在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标
- 有如下数学关系：
\[\left[\begin{array}{l}
\boldsymbol{a}^{\prime} \\
1
\end{array}\right]=\left[\begin{array}{ll}
\boldsymbol{R} & \boldsymbol{t} \\
\mathbf{0}^{\mathrm{T}} & 1
\end{array}\right]\left[\begin{array}{l}
\boldsymbol{a} \\
1
\end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{T}\left[\begin{array}{l}
\boldsymbol{a} \\
1
\end{array}\right]
\]
此时的\(T\)即为变换矩阵。如果我们现在将向量\(a\)从\(B\)变换到\(A\)（即为反向操作），那么就是将变换矩阵\(T\)进行求逆操作，则有
\[T^{-1}=\left[\begin{array}{cc}
R^{\mathrm{T}} & -R^{\mathrm{T}} t \\
0^{\mathrm{T}} & 1
\end{array}\right]
\]
实践Eigen
上面我们聊了好多理论，接下来我们看一个小demo。Eigen是一个C++开源线性代数库。安装过程如下：
sudo apt-get install libeigen3-dev
- 简单的代码操作如下
#include <iostream>
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
void OperationMatrix()
{
//声明3x3矩阵，并将其初始化
Matrix<float, 3, 3> matrix_33;
matrix_33 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
cout << "matrix 2x3 from 1 to 6: \n" << matrix_33 << endl;
//访问矩阵的元素
cout << "print matrix 3x3: " << endl;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
cout << matrix_33(i, j) << "\t";
cout << endl;
}
// 两种声明向量的方式
Vector3d v_3d;
Matrix<float, 3, 1> vd_3d;
v_3d << 3, 2, 1;
vd_3d << 4, 5, 6;
// 矩阵和向量相乘
Matrix<double, 3, 1> result = matrix_33.cast<double>() * v_3d;
cout << "[1,2,3;4,5,6,7,8,9]*[3,2,1]=" << result.transpose() << endl;
Matrix<float, 3, 1> result2 = matrix_33 * vd_3d;
cout << "[1,2,3;4,5,6,7,8,9]*[4,5,6]: " << result2.transpose() << endl;
}
int main(void)
{
OperationMatrix();
return 0;
}