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