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

基本概念

• 刚体的概念

  • 在任何力的作用下，体积和形状都不发生改变的物体叫做刚体(Rigid body)。它是力学中的一个科学抽象概念，即理想模型。事实上任何物体受到外力，不可能不改变形状。

在上面的这里理想模型条件下来描述机器人的运动，也就是刚体运动

• 位姿：位置和姿态

  • 位置：它是指机器人在当前环境中处于哪个地方

  • 姿态：它是指机器人的朝向。

  • 通俗理解就是说机器人当前在什么地方，机器人面向哪里

• 向量：带箭头的线段，它有大小和方向

• 坐标：坐标是具体的取值，我们只有指定了了在哪个坐标系下，我们讨论坐标才有意义。

  • 这里我们来举个例子：该空间的基为\(\left(e_{1}, e_{2}, e_{3}\right)\)，此时\(\left(a_{1}, a_{2}, a_{3}\right)^{\mathrm{T}}\)就为\(a\)在这个基下的坐标。

• 接下来我们讨论一下向量的运算。

  • 内积：\(\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

这里我们引入一个反对称符号^，此时可以将\(a\)写成一个矩阵，即为\(\boldsymbol{a}^{\wedge}\)。\(\boldsymbol{a}^{\wedge}\)计作向量向量\(a\)的反对称矩阵。

该空间的基为\(\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[\begin{array}{c}\boldsymbol{e}_{1}^{\mathrm{T}} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \\ \boldsymbol{e}_{3}^{\mathrm{T}}\end{array}\right]\)，则有

此时\(R\)即为旋转矩阵。

变换矩阵

• 在欧式变换中除了有旋转还要有平移，旋转用矩阵R来表示，平移用向量t来表示，则有

上面这样操作多次显得啰嗦，这里引入其次坐标和变换矩阵。

我们在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标

• 有如下数学关系：

此时的\(T\)即为变换矩阵。如果我们现在将向量\(a\)从\(B\)变换到\(A\)（即为反向操作），那么就是将变换矩阵\(T\)进行求逆操作，则有

实践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;
}