这章主要描述了何描述三维刚体运动，主要围绕两个主题展开：旋转和位移。在书中提到了有四种方法可以描述旋转和位移:旋转矩阵、矩阵、旋转向量、欧拉角、四元数。
旋转矩阵用矩阵的基坐标变换来描述旋转，加上一个向量，就可以完整地表示旋转和平移，但由于旋转矩阵的运算不是线性的，因此改进为由于旋转向量方向是旋转轴的方向，长度为旋转的角度，因此改进为变换矩阵。但这还没完，矩阵的表示法数据冗余度大，并且矩阵必须是正交阵，我们又提出了旋转向量，他的方向与旋转轴一致，长度和旋转角度相同，因此也可以表示旋转和位移。欧拉角比较直观，它一个旋转拆分成分别绕三个坐标轴旋转，可以用于判断和想象旋转是什么样，但是会出现万向锁问题。旋转向量和欧拉角这种三个自由度的表示法，会出现奇异性（例如万向锁），但矩阵表示又不够紧凑，而四元数则兼顾不带奇异性和紧凑型，用一个标量和一个向量组成的四元数字来描述旋转和平移，唯一的缺点可能是计算比较复杂。

slambook第三章.png

1旋转矩阵

1.1公式推导

机器人运动中存在两个坐标系，一个是惯性坐标系（或世界坐标系）其基坐标为
一个是机器人的移动坐标系，其及坐标为

对于同一个三维世界中的某个向量可以分别用这两个坐标系表示:

两边同乘，于是得到旋转矩阵
知道了旋转阵，也就描述了这个旋转，若已知,则可知，反之亦然。是R的逆变换。

1.2旋转矩阵定义

旋转矩阵等价于正交阵:

SO(n)也称为特殊正交群。

1.3齐次坐标和变换矩阵

由旋转矩阵R和一个平移向量t，可以完整表示一个欧式空间所有的变换：

假设我们有多次运算，比如,，那么a到c的变换是
这样的形式在多次变换后十分复杂，于是引入了变换矩阵。

1.3.1齐次坐标

在一般向量末尾加上一维，规定为1，成为齐次坐标，一个齐次坐标数乘一个k，仍然表示同一个点，因此，多个齐次向量可以是相同的：[1,1,1,1]和[2,2,2,2]。但是我们习惯把最后一维强制变为1，于是齐次坐标表示唯一。

1.3.2变换矩阵

加入齐次坐标以后，

那么，多次变换就方便了：

定义：

SE(3)也被称作特殊欧式群。
类似的，变换矩阵的逆也是一个相反的变换。

2.旋转向量

旋转矩阵和变换矩阵用多余3维的数据表示3维的旋转数据，有数据冗余，并且旋转矩阵必须是正交阵，所以提出旋转向量，旋转向量的大小是旋转角度，方向是旋转轴的方向。

2.1旋转向量变为旋转矩阵

2.2由矩阵矩阵得到旋转向量

两边取迹trace，得到旋转角，
再求旋转向量，由于旋转向量绕旋转向量不变，所以.
因此解出向量和旋转角。

3.欧拉角

欧拉角是提供了直观的方式考察旋转，把旋转分解到三个坐标轴上。有多种转角方式包括ZYX,XYZ等，字母的次序代表旋转轴的旋转次序。

3.1ZYX旋转

ZYX旋转（yaw-pitch-roll，偏航-俯仰-滚转），依次绕ZYX旋转。

3.2万向锁

当俯仰角为正负90度时，第一次和第三次旋转的旋转轴会相同因此出现万向锁。

4.四元数

4.1四元数定义

定义一

并且,,满足：

可以看到，按照ijk邻接的循环次序为正，否则为负。
定义二

四元数与旋转向量的关系
旋转向量到四元数

四元数到旋转向量

从式子中而可以看出，旋转角加上2pi，q变为相反数，这说明互为相反数的两个四元数可以表示同一个旋转。

4.2四元数运算

4.2.1加减

4.2.2乘法

四元数的乘法依然得到四元数

4.2.3共轭

共轭相乘，得到一个实四元数

4.2.4模长

4.2.5逆

四元数的逆和四元数相乘可以得到四元数1

并且具有这样的性质

4.2.6数乘和点乘

数乘

点乘

4.3用四元数表示旋转

单位四元数可以表示任意一个旋转
这里将描述如何将一个三维点旋转，p是一个三维空间点，q为四元数，是指定旋转。
第一步用四元数表示三维点:

第二步：用四元数表示这个旋转

第三步：旋转

同理逆变换为
,
他的几何意义是把坐标转到四维空间，再转回来

4.4四元数到旋转矩阵的转换

四元数到旋转矩阵R
设四元数，对应的旋转矩阵R

如何记住这个矩阵？
首先主对角线上那个没有减哪个，减二倍方。对于其他项，矩阵位置对应的在前，剩下的在后，符号联系行列式的计算，主对角线系列加号，副对角线系列是减号。
旋转矩阵R到四元数
注意到表示旋转的四元数都是单位四元数，因此，

2021-08-05十四讲第三章