slam第二讲(三维空间刚体运动)

一.旋转矩阵

1.点和向量&＃xff0c;坐标系

外积&＃xff1a;

表示矩阵a的反对称矩阵&＃xff0c;以此来表示矩阵的外积运算。

2.坐标系间的欧式变换

坐标系1的单位正交基&＃xff1a;[e1,e2,e3]                 点在坐标系1中的坐标&＃xff1a;[a1,a2,a3]

坐标系2的单位正交基&＃xff1a;[e1&＃39;,e2&＃39;,e3&＃39;]               点在坐标系2中的坐标&＃xff1a;[a1&＃39;,a2&＃39;,a3&＃39;]

则转换关系如下&＃xff1a;

将中间的矩阵定义为矩阵R&＃xff0c;R描述了旋转本身&＃xff0c;又称旋转矩阵。

旋转矩阵是一个行列式为1的正交矩阵&＃xff0c;反之&＃xff0c;一个行列式为1的正交矩阵也是一个旋转矩阵。

旋转矩阵的集合定义如下&＃xff1a;

• SO(n)是特殊正交群&＃xff0c;这个集合由n维空间的旋转矩阵组成。
• 旋转矩阵可以描述相机的旋转。
• 刻画了一个相反的旋转

a&＃39;&＃61;Ra&＃43;t&＃xff0c;其中t表示平移&＃xff0c;这是一个完整的欧式变换。

3.变换矩阵与齐次坐标

问题&＃xff1a;a&＃39;&＃61;Ra&＃43;t虽说是一个完整的欧式变换&＃xff0c;但不是一个线性的表示方法&＃xff0c;这会使得多次变换后的运算变得很复杂。

解决方法&＃xff1a;引入齐次坐标和变换矩阵

齐次坐标&＃xff1a;

• 通过添加最后一维&＃xff0c;用四个实数表示了一个三维向量&＃xff0c;这显然多了一个自由度&＃xff0c;但可以将变换写成线性形式。
• 在齐次坐标中&＃xff0c;某个点x的分量同乘一个非0常数k后&＃xff0c;仍然表示同一个点&＃xff0c;故一个点的具体坐标值并不确定&＃xff0c;但当最后一项不为0时&＃xff0c;我们总可以把所有的坐标除以最后一项&＃xff0c;强制最后一项为1&＃xff0c;从而得到一个点的唯一的坐标表示(即转换为非齐次坐标)。

变换矩阵&＃xff1a;

其中T为变换矩阵&＃xff0c;表示a的齐次坐标。

特殊欧式群&＃xff1a;

特点&＃xff1a;左上角为旋转矩阵&＃xff0c;右侧为平移向量&＃xff0c;左下角为0向量&＃xff0c;右下角为1。

与SO(3)一样&＃xff0c;求解该矩阵的逆表示一个反向变换&＃xff1a;

二.旋转向量和欧拉角

存在问题&＃xff1a;

•   SO(3)的旋转矩阵有九个量&＃xff0c;但一次旋转只有3个自由度&＃xff0c;因此这种表示方法是冗余的&＃xff0c;同理变换矩阵用16个量表达了6个自由度的变换&＃xff0c;也是可以有更为紧凑的表示。
• 旋转矩阵自身带有约束&＃xff0c;它是一个行列式为1的正交矩阵&＃xff0c;变换矩阵也是如此&＃xff0c;当想要估计或者优化一个旋转矩阵和变换矩阵时&＃xff0c;这些约束会使得求解变得很困难。

解决方法&＃xff1a;旋转向量

1.旋转向量

任何旋转都可以用一个旋转轴和一个旋转角来表示。我们可以用一个向量&＃xff0c;其方向与旋转轴一致&＃xff0c;而长度等于旋转角&＃xff0c;这种向量称为旋转向量(或轴角&＃xff0c;Axis-Abgle)。

一个旋转轴为n,旋转角为的旋转&＃xff0c;其对应的旋转向量为n.

从旋转向量到旋转矩阵的变换过程由罗德里格斯公式表明。

旋转向量到旋转矩阵&＃xff1a;

旋转矩阵到旋转向量&＃xff1a;

Rn&＃61;n

对于转轴n,由于旋转轴上的向量在旋转后不发生改变&＃xff0c;说明Rn&＃61;n,因此转轴n是矩阵R特征值1对应的特征向量&＃xff0c;求解此方程&＃xff0c;再归一化&＃xff0c;就得到了旋转轴。

2.欧拉角

相对于以上所述的旋转矩阵和旋转向量&＃xff0c;欧拉角便于人类的理解和想象。

• 绕物体的Z轴旋转&＃xff0c;得到偏航角yaw
• 绕旋转之后的Y轴旋转&＃xff0c;得到俯仰角pitch
• 绕旋转之后的X轴旋转&＃xff0c;得到滚转角roll

此时可以用这样一个三维向量描述任何旋转。

重大缺陷&＃xff1a;

会遇到万向锁问题(Gimbal Lock)&＃xff0c;在俯仰角为&＃43;-90度时&＃xff0c;第一次旋转和第三次旋转将使用同一个轴&＃xff0c;使得系统丢失了一个自由度&＃xff08;由三次旋转变成了两次旋转&＃xff09;。

这被称为奇异性问题&＃xff0c;切不可避免&＃xff0c;故slam中很少使用欧拉角表示姿态&＃xff0c;这种表示方法多用于人机相互。

三.四元数

四元数是紧凑的&＃xff0c;也没有奇异性。

一个四元数q拥有一个实部和三个虚部&＃xff0c;表示如下&＃xff1a;

其中i,j,k为四元数的三个虚部。

性质&＃xff1a;

四元数也可以用一个标量和一个向量来表示&＃xff1a;

s称为四元数的实部&＃xff0c;v称为它的虚部&＃xff0c;若v&＃61;0&＃xff0c;则为实四元数&＃xff0c;若s&＃61;0&＃xff0c;则为虚四元数

旋转向量与四元数相互转换&＃xff1a;

旋转向量

旋转轴&＃xff1a;

旋转角&＃xff1a;

四元数&＃xff1a;

反之&＃xff0c;单位四元数到旋转向量&＃xff1a;

任意的旋转都可以由两个互为相反数的四元数表示&＃xff0c;即q和-q这两个四元数表示同一个旋转。

四元数的运算&＃xff1a;

1.加法和减法

2.乘法

向量形式表示&＃xff1a;

四元数乘法通常不可交换&＃xff0c;除非va和vb在R3中共线&＃xff0c;此时外积项为0

3.共轭

4.模长

5.逆

若q为单位四元数&＃xff0c;其逆和共轭就是同一个量。

6.数乘与点乘

数乘&＃xff1a;

点乘&＃xff1a;

用四元数表示旋转 &＃xff1a;

三维空间中一点p&＃61;[0,x,y,z]&＃61;[0,v]

四元数表示旋转

则旋转之后的点p&＃39;可以表示为

四元数到旋转矩阵的转换&＃xff1a;

四元数

对应的旋转矩阵R&＃xff1a;

反之&＃xff0c;若旋转矩阵

则其对应的四元数q为&＃xff1a;

四.相似&＃xff0c;仿射&＃xff0c;射影变换