SLAM中刚体的运动描述

简介

物体在真实世界中是运动的，SLAM中求解的机器人位姿就是求解机器人的运动。SLAM的问题就是求解世界坐标系中的静态物理点坐标以及运动的机器人点坐标集合（机器人运动点的集合构成机器人的运动轨迹）。而通常我们获取的信息是以机器人本体为坐标系下的世界点的坐标信息，因此需要完成运动坐标系下的坐标到世界坐标系坐标的转换。

基本概念（SLAM的研究对象）

质点

中学就学过，假象的点，只有位置没有姿态。只有质量没有形状，当只谈论机器人在地图上的那个位置时，可以将其当作质点，只需要知道机器人相对于参考系的平移即可。

刚体

考虑物体的形状，且假设物体形状不发生改变的物体。也就是自身一个点的运动就代表着整个物体的运动。当谈论到机器人的位姿时就需要考虑刚体这个概念。三维空间中物体的描述方式叫做位姿（位置+姿态），其运动描述为平移+旋转。

地图点

将世界看作由一个一个的点构成，得到这些点相对于参考系的位置坐标后就能拼接出一副地图（形状），这个参考系就是世界坐标系，相当于机器人的起点。我们测量到的这些点的信息都是以自身为参考，也就是机器人坐标系。这些点通常是固定的不变的点，处于静态的。

机器人定位

机器人位置：机器人相对于参考系的距离，平移
机器人位姿：机器人的朝向，用机器人相对于参考的旋转来描述
机器人轨迹：机器人位置的集合构成它的运动轨迹

坐标系

世界坐标系：
　　　　用来描述三维世界物体位置信息的参考系，也叫静态坐标系，通常是机器人运动时的起点。描述机器人的位姿，地图点的坐标都要参考该坐标系

机器人坐标系（动态坐标系）：
　　　　通常机器人传感器观测到的世界信息都是以自身为坐标系参考，该坐标系随着机器人的运动而变化，因此需要将某一时刻观测的信息转换到世界坐标系下才能构建地图，而且自身坐标系也是在世界坐标系下进行观测的，自身的坐标系原点在世界坐标系中的集合代表着机器人的运动轨迹

描述运动的方式

刚体在三维世界中的运动可以用一个旋转 + 平移来描述，三维点在不同坐标系下的坐标是不同的，但是他们可以通过坐标系间的变换进行转换。也就是说坐标的转换和坐标系的变换是相关联的，求解时只需要求解坐标系的变换即可。

在SLAM中，由世界坐标系到机器人坐标系的转换，这个变换过程我们叫做机器人的位姿变换，而机器人坐标系在世界坐标系中的坐标和朝向叫做机器人的位姿，机器人本体上有很多坐标系，这里用传感器的自身坐标系代替机器人坐标系，因为这里考虑的是动态坐标和静态坐标之间的变换（在ROS中叫有一个叫做动态坐标发布就是说的这个变换），机器人本体上的坐标系之间相对位置都是不变的，他们之间的变换很简单，ROS中的静态TF变换指的就是这种变换。

描述坐标系之间旋转的方式以及特点

SLAM中我们获得的世界信息（物理点在机器人传感器坐标系下的坐标）都是基于机器人本身坐标系确定的，构建地图需要的是这些坐标是相对于世界坐标系的，因此需要将坐标从机器人坐标系转换到世界坐标系下，我们需要知道的是机器人坐标系相对于世界坐标系的变换（相对位置关系）。由世界坐标系变换到机器人坐标系可以通过旋转+平移来直观的描述。

坐标变换的实质类似于统一单位。

1、旋转矩阵、变换矩阵的推导

（1）向量、向量坐标、基、单位正交基

向量：
　　　　向量是空间中的具有方向的线段，可以来描述刚体，其性质是不变的，不随着坐标系的变化发生改变。
　　　　
向量坐标：
　　　　向量坐标就是坐标系中向量线段起点坐标减去终点坐标，在指定坐标系下才有意义。
　　　　
单位正交基：
　　　　基就是向量，单位长度为1的向量，用来描述向量的方向；
　　　　单位正交基可以理解为笛卡尔坐标系，就是用来描述向量的参照系。
　　　　三维空间中的三个不平行的三个向量基就能够表示整个三维空间中的向量。

（2）通过向量的特点推导坐标系之间的旋转关系 旋转矩阵

通过向量的性质可以描述不同坐标系之间的变换。同一个向量在不同坐标系下是不变的。因此可以通过单位正交基来描述向量：

e表示单位正交基（坐标系），a表示两个坐标系下的向量坐标。R的实质就是用ｅ₁来描述e^‘ ，因为空间中的一组基可以可以描述空间中的任何向量，所以求解的R实质就是e^’ 在ｅ₁下的线性组合（坐标），表示了两个坐标系之间的转换

两边变换一下得到R就是坐标系之间的旋转矩阵，：

旋转矩阵的特点：
　　　　１、矩阵为单位正交矩阵、可逆矩阵
　　　 　2、各分量为各向量基的夹角的余弦值
　　　　 3、该矩阵是特殊正交群

（3）旋转+平移描述了坐标系之间的变换 变换矩阵

旋转＋平移刻画了坐标系之间的整体变换，坐标系之间的平移就是左边原点之间的相对位置，直接在原坐标中加上这个平移值即可。

（4）齐次坐标描述了变换矩阵的线性连续变换

当坐标系连续的变换时，变换公式可以依次类推，如下：

但是该递推公式太过于复杂，可以利用齐次坐标将其变换为矩阵的线性变换。
齐次坐标就是在原来的坐标上添加1维，特点是能够使连续变换的非线性公式变成矩阵中的线性变换。T就是整个变换矩阵，由旋转矩阵和平移矩阵构成。

以上就是坐标系之间的转关系，有了这个变换矩阵T，我们就能够进行世界坐标系和机器人坐标系中物理信息的坐标变换了。

这里补充一下，向量之间的变换关系推导出来的变换矩阵，同样也适用于质点之间的坐标系转换（假设这个质点是长度为0的向量）。其中的关系推导可以参考这篇文章，讲的非常细致，其实就是线性代数里边的东西。

2、旋转向量

描述旋转的另外一个方式，也就表示世界坐标系绕某一轴，一次性旋转x角度，变换为另一个坐标系，也就是用转轴和转角的方式来描述。这种方式具有奇异性，转角是360°具有周期性。

旋转向量和旋转矩阵的转换：

罗德里格斯公式:

3、欧拉角

这个更直观，直接就是坐标系绕其自身的三个轴旋转得到转换后的坐标。ROS的TF也是用欧拉角来描述坐标系间的相对位置，其描述顺序为Z,Y,X

绕z轴旋转：偏航 yaw
绕x轴旋转：翻滚 roll
绕y轴旋转：俯仰 pitch

万向锁问题：俯仰角为±90°时，第一次和第三次旋转使用的轴为同一个轴，可以参考这篇文章 ，这篇博客也很细致

4、四元数

该描述旋转的方法很神奇，很有用，没看懂，直接上转换公式。可以参考这些博客

1、四元素表示旋转

任意的单位四元素描述了一个旋转，单位四元素表示的旋转不该表旋转后的向量模。

2、四元素转换为旋转矩阵

3、四元数转换为旋转向量

以上就是描述刚体的所有运动公式推导了

总结

在SLAM中我们需要将机器人观察到的物理信息转换到世界坐标系下，引入了坐标系之间的变换和坐标变换。

我们需要通过坐标之间的变换关系，来求解世界坐标系下的物理点坐标（建图环节）。

相反我们通过传感器获得了不同机器人坐标系下的物理信息点，然后通过对应的物理信息点来求解变换矩阵（位姿、环节定位）。

当然位姿变换T的求解不一定全部通过坐标变换来求解，也有可能采用运动学模型、其他传感器来获取。

结合上一节的相机模型，我们就能够通过视觉相机来对环境进行探索了。其过程如下：
首先相机采集到不同时刻的图像像素坐标信息，通过逆推投影模型获得了物理点在相机坐标系下的坐标（包括深度信息），然后在逆推坐标变换公式，从而得到了不同时刻相机之间的位姿变换，将多个时刻的位姿信息记录下来，便构成了机器人的移动轨迹。