三维空间刚体特性

一、旋转矩阵

1.1 向量坐标

在线性代数中，基（也称为基底）是描述、刻画向量空间的基本工具。向量空间的基是它的一个特殊的子集，基的元素称为基向量。向量空间中任意一个元素，都可以唯一地表示成基向量的线性组合。如果基中元素个数有限，就称向量空间为有限维向量空间，将元素的个数称作向量空间的维数。

用线性代数的知识来说，我们找到了三维空间下一组基（e1,e2,e3）,顾名思义，基就是张成这个空间的一组线性无关的向量，也叫基地，任意向量在这组基下都存在一个坐标，我们把向量a定义如下：

这里的(a1,a2,a3)称为向量a在此基下的坐标，坐标的取值，一是和向量本身有关，二适合坐标系的选取（基）有关

• 内积（内积为一个数）

表征为对于坐标相乘再相加

• 外积（外积为一组坐标）

表征为与两个向量组成平面相垂直的向量

具体计算如下：

a = (a1,a2,a3) ,b = (b1,b2,b3)

整理之后即得到前式

从上式不难发现，a^为一个反对称矩阵，观察其特点，其为3*3的矩阵，有九个元素，但由于对角线都为0，并且还上下对称，因此我们只用三个元素就能表达这个反对称矩阵，也就是说这个反对称矩阵只有三个维度

1.2 坐标系之间的转换

坐标系之间的转换称为欧式变换（由旋转和平移组成）：核心为坐标变换但是 向量本身不变

1.2.1 欧式变换

比如大地坐标系与人的坐标系之间，我们可以通过一系列的动作，把我们的坐标系体，调整到与大地坐标系重合的位置，这个过程，就叫做欧式变换

欧式变换由旋转和平移组成，首先考虑旋转，如下表示

由上述变换可知，向量a本身没有改变，在不同的坐标系（基）下，坐标发生了变化，所以来说欧式变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行移动或旋转，不改变它自身的样子，其他变换则会改变它的外形

右边可以得到一个矩阵R，我们称这个矩阵为旋转矩阵

记下来考虑平移，世界坐标系中的向量a,经过一次旋转和平移得到a’向量

所以，我们用一个旋转矩阵R和一个平移向量t就可以完整描述一个欧式空间下的坐标变换关系

实际当中，我定义了坐标系1、2，那么向量a在两个坐标系下的坐标为a1,a2，它们之间的关系为：

上式中R12指的是把坐标系2的向量变换的到坐标系1中，R21即为从坐标系2到坐标系1的变换；t12指的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，但值得注意的是，t12不等于-t21,而是和两个坐标系的旋转也有关系

二、变换矩阵与齐次坐标

对于上述坐标变换，假如我们要进行多次变换，比如把a联系进行两次变换，先变成b，再变成c ，其中旋转矩阵和平移矩阵分别为R1 R2 t1 t2 ：

则从a到c的变换为：

这样的变换无疑是繁杂的，因此我们引入了齐次坐标和变换矩阵；我们在三维向量的末尾加上1，使其变为四维向量，称为齐次坐标，重写上式为：

我们成矩阵T即为变换矩阵，这样依靠齐次坐标和变换矩阵，两次变换的叠加就可以拥有很简洁的形式：

变换矩阵T,其结构为：左上角为旋转矩阵，右上角为平移矩阵，左下角为0，右下角为1，这种矩阵又称为特殊欧式群：

与SO(3)一样，求解该矩阵的逆表示一个反向的变换：

同样，我们用T12这样的写法来表示从坐标系2到1的变换

SO(3):三维旋转矩阵 如果将平移和旋转放在一个矩阵中即得 SE(3):变换矩阵（是一个四维矩阵）

三、旋转向量与欧拉角

3.1 旋转向量

SO(3)的旋转矩阵有9个量，但一次旋转只有三个自由度，变换矩阵有16个量，但是只表达了6个自由度的变换，这种方法是冗余的，且进行估计和优化时，旋转矩阵和变换矩阵较难求，因此，引入了旋转向量

任意旋转都可以用一个旋转轴和一个旋转角来刻画；我们可以使用一个向量，其方向与旋转轴一致，长度等于旋转角 ，这样的向量，称为旋转向量(或轴角，AngleAxis),这样的话，只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换，这时的维数正好是六维。

本来一个旋转由R表示，现在有了旋转角，我们假设旋转轴是一个单位长度的向量n， 角度为[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-McxucTPd-1665451491225)(https://gitee.com/xu-yanbo/picture/raw/master/gif.latex)]那么[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-1nOMyRh5-1665451491226)(https://gitee.com/xu-yanbo/picture/raw/master/gif.latex)] 就是一个长度等于旋转角，方向与旋转轴一致的向量。我们就可以用来表示一个旋转。这种表示方法与原来的旋转矩阵之间的联系：由罗德里格斯公式可以互相转换：

tips:旋转向量的矩阵形式即为旋转矩阵

3.2 欧拉角

旋转向量与旋转矩阵虽说能描述旋转，但都不直接，故须引入欧拉角

欧拉角提供了一种非常直观的方式来 描述旋转——它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转;由于绕不同的轴旋转所得到的欧拉角是不同的，所以欧拉角在使用的时候必须要先指明旋转的顺序；还需要区分每次旋转是绕固定轴旋转的，还是绕旋转之后的轴旋转的。假设一个刚体的前方（朝向我们的方向）为 X 轴，右侧为 Y 轴，上方为 Z 轴，如下图所示。那么，ZYX转角相当于把任意旋转分解成以下三个轴上的转角:

• 绕物体的z轴旋转，得到偏航角yaw
• 绕旋转之后的y轴旋转，得到俯仰角pitch
• 绕旋转之后的x轴旋转，得到滚转角 roll
  
• RPY角
  绕参考坐标系，绕定轴X（Roll）—Y（Pitch）—Z（Yaw）旋转，旋转矩阵左乘。
  顺序：绕X转γ，绕Y转β，绕Z转α
  公式：R(γ,β,α) = Rot(z,α)Rot(y,β)Rot(x,γ)
• 欧拉角
  绕自身坐标系，绕动轴Z—Y—X旋转，旋转矩阵右乘
  顺序：绕Z转α，绕Y转β，绕X转γ
  公式：R(α,β,γ) = Rot(z,α)Rot(y,β)Rot(x,γ)

3.3 四元数

旋转矩阵具有冗余性，欧拉角与旋转向量具有奇异性，因此，引入了一种类似于复数的代数：四元数 但其表示不够直观，且其运算稍复杂

四元数简单可以理解为一种扩展的复数；

人们也用一个标量和一个向量来表示四元数：

在这里，s称为四元数的实部，v称为它的虚部，如果一个四元数的虚部为0，则称为实四元数，反之称为虚四元数。四元数的虚部即为该点的坐标

二维情况下：旋转可以由单位复数来描述；乘i就是绕i轴旋转90度

三维情况下：旋转可以由单位四元数来描述；乘i就是绕i轴旋转180度

四元数的运算和复数基本一样，四元数共轭即是把虚部取成相反数，常见的有四则运算、共轭、求逆、数乘等。暂不一一举例

补充总结

• 变换矩阵可直接进行坐标变换
• 位姿实质上就是变换矩阵，一般可构成世界坐标系到参考坐标系的变换
• 通过旋转向量、旋转矩阵、欧拉角、四元数都可进行旋转变换，且可互相转化

以上内容参考《视觉slam十四讲》，谢谢！！！