02_三维空间刚体运动（上）

基本概念

• 刚体的概念

  • 在任何力的作用下，体积和形状都不发生改变的物体叫做刚体(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;
}