SLAM学习笔记4：三维空间刚体运动

导引：

        我们在笔记2中提到了SLAM问题的基本方程，它由运动方程和观测方程组成。接下来我们要讨论的就是运动方程。

        运动方程用来表述传感器的位姿变化。所谓“位姿”，包含位置和姿态两方面。表述位姿变化的问题就是描述刚体在三维空间中的运动。

        刚体的三维空间运动（对应于“位姿变化”）可以分解为平移运动（对应于“位置变化”）和旋转运动（对应于“姿态变化”）：

        平移运动表述的是刚体位置的变化，可以用向量的加减运算来表示。 旋转运动表述的是刚体姿态的变化，可以用旋转矩阵、旋转向量或者四元数来表示。

一、旋转矩阵、变换矩阵和齐次坐标：

1.欧氏变换与旋转矩阵：

（1）两个坐标系及其转换：

        在描述刚体的三维运动时，常见的做法是设置两个坐标系：惯性坐标系（世界坐标系）和移动坐标系。

        认为惯性坐标系是静止不动的，相机或机器人是移动坐标系。

        对于向量 𝑃：在世界坐标系下其坐标为𝑃𝑤；相机视野下，其坐标为𝑃𝑐。为了转换这两个坐标，要先得到该点针对移动坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。这种变换需要用一个矩阵T来描述。

（2）刚体运动：

        两个坐标系之间的运动由一个旋转和一个平移组成，这种运动称为刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化，仅相差了一个欧式变换。

（3）欧氏变换与旋转矩阵：

        坐标系间的欧氏变换由一次平移和一次旋转组成。