【SLAM十四讲CH3】三维空间刚体运动 学习笔记

CH3 三维空间刚体运动 笔记

旋转矩阵

• 向量在指定坐标系下用坐标表示
  $$\mathbf{a}=[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{\mathbf{e}}_1+a_2{\mathbf{e}}_2+a_3{\mathbf{e}}_3$$

• 内积
  $$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^T\mathbf{b}=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos⟨\mathbf{a},\mathbf{b}⟩$$

• 外积

  两个向量的外积，方向垂直于这两个向量，大小等于两个向量张成的四边形的面积
  $$\mathbf{a}\times \mathbf{b}=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\mathbf{b}\triangleq {\mathbf{a}}^{\wedge }\mathbf{b}$$

• 反对称矩阵

  将外积转化为了矩阵和向量的乘法，变成了线性运算。

$$\mathbf{a}\times \mathbf{b}={\mathbf{a}}^{\wedge }\mathbf{b}$$

$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

• 用外积表示旋转

  右手法则，四根手指由a指向b，拇指方向为旋转向量的方向，大小由两个向量的夹角决定

坐标系的欧氏变换

设定一个不动的惯性坐标系（世界坐标系w)，相机或机器人是移动坐标系c，由一个矩阵T描述两个坐标系之间的转换关系、移动坐标系的运动、两个坐标系下同一点坐标的换算关系。

一个欧氏变换由平移和旋转两部分组成。

• 描述一个向量需要先规定一组基，再在这组基下用一组坐标描述，即按照坐标所述的方式用基向量组合出该向量。在不同的基下，应当有不同的描述，但组合的结果应该相同，都是同一个向量。

  由此有
  $$\left[\begin{array}{c}{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{c}{\mathbf{e}}_1^{{}^{\prime }},{\mathbf{e}}_2^{{}^{\prime }},{\mathbf{e}}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]$$

• 同时左乘$\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$ ,左边变成单位矩阵，右边得到旋转矩阵

$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]\triangleq \mathbf{R}{\mathbf{a}}^{{}^{\prime }}$$

​ 旋转矩阵R由两组基之间的内积组成，一个旋转对应一个旋转矩阵。

• 性质：旋转矩阵是一个行列式为1的正交矩阵，并且行列式为1的正交矩阵是一个旋转矩阵。

  正交阵：${\mathbf{R}}^{\mathbf{T}}={\mathbf{R}}^{-1}$

  其转置（逆）刻画了一个相反的旋转。

• 完整的运动由旋转加平移构成

  ${\mathbf{a}}^{{}^{\prime }}=\mathbf{R}\mathbf{a}+\mathbf{t}$

变换矩阵与齐次坐标

由于${\mathbf{a}}^{{}^{\prime }}=\mathbf{R}\mathbf{a}+\mathbf{t}$的描述方式不符合线性运算，于是引入齐次坐标，和变换矩阵
$$\left[\begin{array}{c}{\mathbf{a}}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$
其中四维向量的最后一位常数表示缩放。
eg:${\left[\begin{array}{c}1,1,1,1\end{array}\right]}^T$和${\left[\begin{array}{c}2,2,2,2\end{array}\right]}^T$表示同一个点

变换矩阵T符合线性要求，有较好的性质：
$$\tilde{\mathbf{b}}={\mathbf{T}}_1\tilde{\mathbf{a}},\tilde{\mathbf{c}}={\mathbf{T}}_2\tilde{\mathbf{b}}\Rightarrow \tilde{\mathbf{c}}={\mathbf{T}}_2{\mathbf{T}}_1\tilde{\mathbf{a}}$$
其逆表示反向的变换
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathbf{T}}& -{\mathbf{R}}^{\mathbf{T}}\mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]$$

旋转向量与欧拉角

用矩阵表示旋转，不够紧凑，矩阵9个值，但仅有三个自由度，冗余浪费。
任意一个旋转都可以用一个旋转轴和一个旋转角来刻画，于是用一个方向与转轴一致，大小与转角一致的向量来描述旋转，该向量称为旋转向量。

⭐️旋转向量到旋转矩阵的过程由罗德格里斯公式给出
$$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^{\mathbf{T}}+sin\theta {\mathbf{n}}^{\wedge }$$
⭐️由旋转矩阵到旋转向量
$$\theta =arccos(\frac{tr(\mathbf{R})-1}{2})$$
由于旋转轴在旋转后不变，因此转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值1对应的特征向量
$$\mathbf{R}\mathbf{n}=\mathbf{n}$$

欧拉角

把一次旋转分解成三次绕不同轴的旋转，分解方式有很多种，定义也有很多种。最常见的一种定义方式为“偏航 俯仰 滚转”

1.物体绕z轴旋转，得到偏航角yaw
2.物体绕旋转后的Y轴旋转，得到俯仰角pitch
3.物体绕旋转后的X轴旋转，得到滚转交roll

写成向量用$[r,p,y{]}^T$来描述

欧拉角会遇到万向锁问题，当第二次绕Y轴旋转为90°时，第一次和第三次旋转会使用同一个轴，成为奇异性问题。

很少在程序中直接使用欧拉角，常用于人机交互界面，便于理解。

四元数

复数的扩展，紧凑，无奇异性

定义：
四元数是形如$q=s+xi+yj+zk=[s,\mathbf{v}]$的表达式，其中a,b,c,d均为实数，而i,j,k满足如下运算规则

$\times$	1	i	j	k
1	1	i	j	k
i	i	-1	k	-j
j	j	-k	-1	i
k	k	j	-i	-1

共轭：对于q=a+bi+cj+dk，其共轭为q*=a-bi-cj-dk

模长：$||q||=\sqrt{qq\ast }=\sqrt{q\ast q}=\sqrt{a^2+b^2+c^2+d^2}$

逆：$q^{-1}=\frac{1}{||q||}q\ast$

加减：实部虚部分别加减
${\mathbf{q}}_{\mathbf{a}}\pm {\mathbf{q}}_{\mathbf{b}}=[s_a\pm s_b,{\mathbf{v}}_{\mathbf{a}}\pm {\mathbf{v}}_{\mathbf{b}}]$

乘法（叉乘）：各相分别相乘再相加
写成向量形式为：${\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=[s_a{s}_b-{\mathbf{v}}_{\mathbf{a}}^{\mathbf{T}}{\mathbf{v}}_{\mathbf{b}},s_a{\mathbf{v}}_{\mathbf{b}}+s_b{\mathbf{v}}_{\mathbf{A}}+{\mathbf{v}}_{\mathbf{a}}\times {\mathbf{v}}_{\mathbf{b}}]$

点乘：${\mathbf{q}}_1\cdot {\mathbf{q}}_2=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$

四元数表示旋转

假设一个绕单位向量$\mathbf{n}=[n_x,n_y,n_z]$进行角度为$\theta$的旋转，则表示这个旋转的四元数为：
$$\mathbf{q}=[cos\frac{\theta }{2},sin\frac{\theta }{2}(n_x,n_y,n_z)]$$
对于一个四元数$\mathbf{q}$所表示的旋转，其转轴和转角为：
KaTeX parse error: Unknown column alignment: * at position 23: …{\begin{array}{*̲*lr**}\theta=2a…
对三维空间点$\mathbf{p}=[x,y,z]$进行一个轴为$\mathbf{n}$角为$\theta$的旋转：
首先将三维空间点$\mathbf{p}$用一个虚四元数描述：$\mathbf{p}=[0,x,y,z]=[0,\mathbf{v}]$
用四元数表示旋转为：$\mathbf{q}=[cos\frac{\theta }{2},\mathbf{n}sin\frac{\theta }{2}]$
则旋转后的点为：
$${\mathbf{p}}^{{}^{\prime }}=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$$

四元数与旋转矩阵的转化

相似 放射 射影变换

相似

允许物体进行均匀缩放，立方体经过相似变换依然为立方体，但大小发生了变化。
矩阵表示为：
$${\mathbf{T}}_{\mathbf{s}}=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$

放射变换

立方体经过放射变换后，不再是方的，但各个面仍然是平行四边形。
矩阵表示为：
$${\mathbf{T}}_{\mathbf{A}}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$
其中$\mathbf{A}$只要求是可逆矩阵，不要求是正交矩阵。

射影变换

真实世界到相机照片的变换是一个射影变换，其矩阵形式为
$${\mathbf{T}}_{\mathbf{P}}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right]$$
左上角为可逆矩阵$\mathbf{A}$，右上为平移$\mathbf{t}$，左下缩放${\mathbf{a}}^T$