SLAM基础——部分预备知识

1 📖 四元数

1-1 🔖 ​四元数介绍

四元数 q 有一个实部和三个虚部。我们把实部写在前：
$$\mathbf{q}={\left[q_0,q_1,q_2,q_3\right]}^T\text{ 或 }\mathbf{q}=[w,x,y,z{]}^T$$

其中 $q_0$ 为实部，${\left[q_1,q_2,q_3\right]}^T$ 为虚部。

因为实部为标量，虚部为矢量，所以也可记为：
$$\mathbf{q}=[s,\mathbf{v}{]}^T$$

四元数和角轴的转换关系：设角轴为 ω 角度大小为θ ，那么它对应的四元数为：
$$\mathbf{q}={\left[\cos \frac{\theta }{2},\mathbf{\omega }\sin \frac{\theta }{2}\right]}^{\mathrm{T}}$$

扩充

单位向量(x,y,z)旋转θ角度后的四元数

$\left(\cos \frac{\theta }{2},x\ast \sin \frac{\theta }{2},y\ast \sin \frac{\theta }{2},z\ast \sin \frac{\theta }{2}\right)$

1-3 🔖 四元数与旋转矩阵的相互转化

设四元数$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$，对应的旋转矩阵 R 为：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$
由旋转矩阵到四元数的转换如下：
$$R=\left[\begin{array}{lll}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
则：
$$q_0=\frac{\sqrt{\mathrm{tr}(R)+1}}{2},q_1=\frac{r_{23}-r_{32}}{4q_0},q_2=\frac{r_{31}-r_{13}}{4q_0},q_3=\frac{r_{12}-r_{21}}{4q_0}$$

由于 q 和 − q 表示同一个旋转，事实上一个 R 对应的四元数表示并不是惟一的。同时，除了上面给出的转换方式之外，还存在其他几种计算方法

1-2 🔖 四元数时间导数

• 世界坐标系 W ；
• IMU 坐标系 I ；
• 相机坐标系 C ；

四元数时间导数：
设初始旋转为 q = [s, v] ，然后，发生了角轴为 ω, θ 的旋转（右乘，对应四元数记作 ∆q ），那么 q 相对该旋转的导数为：(7)
$$\begin{array}{rl}\underset{\theta \to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}-\mathbf{q}}{\theta }& =\underset{\theta \to 0}{\lim }\frac{{\left[s\cos \frac{\theta }{2}-{\mathbf{v}}^T\omega \sin \frac{\theta }{2},s\omega \sin \frac{\theta }{2}+\cos \frac{\theta }{2}\mathbf{v}+\mathbf{v}\times \omega \sin \frac{\theta }{2}\right]}^{\mathrm{T}}-\mathbf{q}}{\theta }\\ & =\underset{\theta \to 0}{\lim }\frac{{\left[s\left(\cos \frac{\theta }{2}-1\right)-{\mathbf{v}}^T\omega \sin \frac{\theta }{2},s\omega \sin \frac{\theta }{2}+\left(\cos \frac{\theta }{2}-1\right)\mathbf{v}+\mathbf{v}\times \omega \sin \frac{\theta }{2}\right]}^{\mathrm{T}}}{\theta }\\ & ={\left[-\frac{1}{2}{\mathbf{v}}^T\mathbf{\omega },\frac{1}{2}s\mathbf{\omega }+\frac{1}{2}\mathbf{v}\times \mathbf{\omega }\right]}^{\mathrm{T}}\\ & =\mathbf{q}\otimes {\left[0,\frac{1}{2}\mathbf{\omega }\right]}^{\mathrm{T}}\end{array}$$

因此，若角速度为 ω ，那么旋转的时间导数即为：(8)

$$\dot{\mathbf{q}}=\mathbf{q}\otimes {\left[0,\frac{1}{2}\omega \right]}^{\mathrm{T}}$$

2 📖 一些求导函数

2-1 🔖 扰动模型

注意：$\theta$表示旋转量，$a$表示旋转轴
$$\varphi =\theta \mathbf{a}$$

$$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{ll}{\mathbf{J}}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_1\text{ is small }\\ J_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1& \text{ if }{\varphi }_2\text{ is small. }\end{array}$$

当对一个旋转矩阵$R_2$ （李代数为 ${\varphi }_2$ ）左乘一个微小旋转矩阵$R_1$ （李代数为 ${\varphi }_1$）时，可以近似地看作，在原有的李代数 ${\varphi }_2$ 上，加上了一项${\mathbf{J}}_l{\left({\mathbf{\varphi }}_2\right)}^{-1}{\mathbf{\varphi }}_1$ 。同理，第二个近似描述了右乘一个微小位移的情况。

其中左乘雅可比：
$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$
逆为：
$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

一般直接用下面形式的多:
$${\mathbf{J}}_r(\vec{\varphi })=\mathbf{I}-\frac{1-\cos (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^2}{\vec{\varphi }}^{\wedge }+\frac{\Vert \vec{\varphi }\Vert -\sin (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^3}{\left({\vec{\varphi }}^{\wedge }\right)}^2$$

$${\mathbf{J}}_r^{-1}(\vec{\varphi })=\mathbf{I}+\frac{1}{2}{\vec{\varphi }}^{\wedge }+\left(\frac{1}{\Vert \vec{\varphi }{\Vert }^2}-\frac{1+\cos (\Vert \vec{\varphi }\Vert )}{2\cdot \Vert \vec{\varphi }\Vert \cdot \sin (\Vert \vec{\varphi }\Vert )}\right){\left({\vec{\varphi }}^{\wedge }\right)}^2$$

这种形式的写法，更贴近代码，容易对比

右乘雅可比：
$${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$$

2-1 🔖 旋转矩阵R的左右扰动求导

旋转点的左扰动雅可比：
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\phi \to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\phi \to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\phi \to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\phi }}=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }.\end{array}$$
旋转点的右扰动雅可比：
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\exp \left({\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\mathbf{\phi }}^{\wedge }\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{\phi }}{\phi }=-\mathbf{R}{\mathbf{p}}^{\wedge }\end{array}$$

2-2 🔖 $R_1\ast R_2$对$R_2$求导

下面用到了有扰动雅克比矩阵的近似等式
$$\begin{array}{rl}\frac{\partial {\mathbf{R}}_1{\mathbf{R}}_2}{\partial {\mathbf{R}}_2}& =\underset{\varphi \to 0}{\lim }\frac{\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\exp \left({\varphi }^{\wedge }\right)\right)-\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)+{\mathrm{J}}_r^{-1}\left(\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)\right)\varphi -\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & ={\mathrm{J}}_r^{-1}\left(\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)\right)\end{array}(1)$$

$\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }$ ：将矩阵${\mathbf{R}}_1{\mathbf{R}}_2$变成向量形式（上面整个过程都是在向量形式下进行的求导）

${\mathbf{J}}_r^{-1}\left(\ln {\left(\mathbf{R}{\mathbf{R}}_2\right)}^{\vee }\right)$： 以$\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }$ 为自变量的右雅可比（上面是求逆了）

2-3 🔖 $R_1\ast R_2$对$R_1$求导

下面利用到了伴随矩阵的一个变式
$$\exp \left({\varphi }^{\wedge }\right)\mathbf{R}=\mathbf{R}\exp \left({\left({\mathbf{R}}^T\varphi \right)}^{\wedge }\right)$$

$$\begin{array}{rl}\frac{\partial {\mathbf{R}}_1{\mathbf{R}}_2}{\partial {\mathbf{R}}_1}& =\underset{\varphi \to 0}{\lim }\frac{\ln \left({\mathbf{R}}_1\exp \left({\varphi }^{\wedge }\right){\mathbf{R}}_2\right)-\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\exp \left({\left({\mathbf{R}}_2^T\varphi \right)}^{\wedge }\right)\right)-\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & =J_r^{-1}\left(\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\right)\right){\mathbf{R}}_2^T\end{array}(2)$$

$\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }$ ：将矩阵${\mathbf{R}}_1{\mathbf{R}}_2$变成向量形式（上面整个过程都是在向量形式下进行的求导）

${\mathbf{J}}_r^{-1}\left(\ln {\left(\mathbf{R}{\mathbf{R}}_2\right)}^{\vee }\right)$： 以$\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }$ 为自变量的右雅可比（上面是求逆了）

3 📖 vee（$\vee$）和hat（$\wedge$）

3-1 🔖 vee和hat介绍

hat运算符$\wedge$： （反对称算子）把 3 维实向量映射为 3×3 反对称阵；

vee 运算符$\vee$：把 3×3 反对称映射为 3 维实向量；

$${\mathbf{\omega }}^{\wedge }={\left[\begin{array}{l}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]}^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]=\mathbf{W}$$

$${\mathbf{W}}^{\vee }={\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]}^{\vee }=\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]=\mathbf{\omega }$$

关于 hat 运算符的一个性质
$${\mathbf{a}}^{\wedge }\cdot \mathbf{b}=-{\mathbf{b}}^{\wedge }\cdot \mathbf{a},\forall \mathbf{a},\mathbf{b}\in R^3$$

3-2 🔖 exp()和log()操作

指数映射$\exp (\bullet )$将$R^{3\times 1}$映射到$R^{3\times 3}$：
$$\exp \left({\vec{\varphi }}^{\wedge }\right)=\mathbf{I}+\frac{\sin (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }\Vert }{\vec{\varphi }}^{\wedge }+\frac{1-\cos (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^2}{\left({\vec{\varphi }}^{\wedge }\right)}^2$$

当$\vec{\varphi }$是小量时，有一阶近似如下：
$$\exp \left({\vec{\varphi }}^{\wedge }\right)\approx \mathbf{I}+{\vec{\varphi }}^{\wedge }$$

对数映射$\log (\bullet )$将$R^{3\times 3}$映射到$R^{3\times 1}$
$$\log (\mathbf{R})=\frac{\phi \cdot \left(\mathbf{R}-{\mathbf{R}}^T\right)}{2\sin (\phi )}$$

其中$\phi ={\cos }^{-1}\left(\frac{\mathrm{tr}(R)-1}{2}\right)$

其他
$$\log (\mathbf{R}{)}^{\vee }=\phi \cdot \mathbf{a}$$

$\phi$：旋转角

$\mathbf{a}$：旋转轴单位矢量，有$\mathbf{a}={\left(\frac{\mathbf{R}-{\mathbf{R}}^T}{2\sin (\phi )}\right)}^{\vee }$

小量下的近似表达

对于 3 维实向量$\vec{\varphi }$和一个小量$\delta \vec{\varphi }$，有如下性质：
$$\begin{gathered} \mathrm{Exp}(\vec{\varphi }+\delta \vec{\varphi })\approx \mathrm{Exp}(\vec{\varphi })\cdot \mathrm{Exp}\left({\mathbf{J}}_r(\vec{\varphi })\cdot \delta \vec{\varphi }\right) \\ \mathrm{Log}(\mathrm{Exp}(\vec{\varphi })\cdot \mathrm{Exp}(\delta \vec{\varphi }))=\vec{\varphi }+{\mathbf{J}}_r^{-1}(\vec{\varphi })\cdot \delta \vec{\varphi } \end{gathered}$$

注意：
$$\mathrm{E}\mathrm{x}\mathrm{p}\left(\varphi \right)=\exp \left({\varphi }^{\wedge }\right)$$
在等号后面的exp内部还需要取反对称运算，同理 $\mathrm{L}\mathrm{o}\mathrm{g}\left(\mathrm{R}\right)=\mathrm{l}\mathrm{o}\mathrm{g}\left(\mathrm{R}\right)$

其中：
$${\mathbf{J}}_r(\vec{\varphi })=\mathbf{I}-\frac{1-\cos (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^2}{\vec{\varphi }}^{\wedge }+\frac{\Vert \vec{\varphi }\Vert -\sin (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^3}{\left({\vec{\varphi }}^{\wedge }\right)}^2$$

$${\mathbf{J}}_r^{-1}(\vec{\varphi })=\mathbf{I}+\frac{1}{2}{\vec{\varphi }}^{\wedge }+\left(\frac{1}{\Vert \vec{\varphi }{\Vert }^2}-\frac{1+\cos (\Vert \vec{\varphi }\Vert )}{2\cdot \Vert \vec{\varphi }\Vert \cdot \sin (\Vert \vec{\varphi }\Vert )}\right){\left({\vec{\varphi }}^{\wedge }\right)}^2$$

3-3 🔖 一些关于旋转矩阵的性质（⭐️）

R对时间的导数
$$\dot{\mathbf{R}}=\mathbf{R}{\omega }^{\wedge }$$
指数映射的 Adjoint 性质
$$\begin{array}{l}\mathbf{R}\cdot \mathrm{Exp}(\vec{\varphi })\cdot {\mathbf{R}}^T=\exp \left(\mathbf{R}{\vec{\varphi }}^{\wedge }{\mathbf{R}}^T\right)=\mathrm{Exp}(\mathbf{R}\vec{\varphi })\\ \iff \mathrm{Exp}(\vec{\varphi })\cdot \mathbf{R}=\mathbf{R}\cdot \mathrm{Exp}\left({\mathbf{R}}^T\vec{\varphi }\right)\end{array}$$
其他
$$(\mathbf{R}\vec{\varphi }{)}^{\wedge }=\mathbf{R}{\vec{\varphi }}^{\wedge }{\mathbf{R}}^T$$

4 📖欧拉角与旋转矩阵的转换关系

4-1 🔖 基础概念

内在旋转(intrinsic rotations)和外在旋转(extrinsic rotations)

• 内在旋转每次旋转围绕的轴是上次旋转之后坐标系的某个轴。
• 外在旋转每次旋转的轴是固定坐标系中的轴。

内在旋转与外在旋转的转换关系：互换第一次和第三次旋转的位置则两者结果相同。例如Z-Y-X旋转的内部旋转和X-Y-Z旋转的外部旋转的旋转矩阵相同

下图是z-x-z的内在旋转和外在旋转示意图

旋转角度是左手系还是右手系

使用右手的大拇指指向旋转轴，其他4个手指在握拳过程中的指向便是正的角度

• 右手系是逆时针
• 左手系是顺时针

Roll(翻滚角)、Pitch(俯仰角)、Yaw(偏航角)

• 绕x轴旋转角度$\psi$： Roll(翻滚角)
• 绕y轴旋转角度$\theta$：Pitch(俯仰角)
• 绕z轴旋转角度$\varphi$：Yaw(偏航角)

4-2 🔖 不同轴的欧拉角转换成旋转矩阵

蓝色是起始坐标系，而红色的是旋转之后的坐标系

注意：上面的角度符号与下面完全没关系，不要搞混

右手系下，逆时针旋转的角度为正，绕不同轴的旋转结果：
$$\begin{array}{rl}& R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\\ & R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\\ & R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

这里也写上左手系：
$$\begin{array}{rl}& R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right]\\ & R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right]\\ & R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\end{array}$$
即：所有的$\mathbf{s}\mathbf{i}\mathbf{n}\theta$都变号

所以，容易得到，欧拉角转旋转矩阵如下：
$$\begin{array}{rl}R& =R_z(\gamma )R_y(\beta )R_x(\alpha )\\ & =\left[\begin{array}{ccc}\cos \beta \cos \gamma & \sin \alpha \sin \beta \cos \gamma -\cos \alpha \sin \gamma & \cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma \\ \cos \beta \sin \gamma & \sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma & \cos \alpha \sin \beta \sin \gamma -\sin \alpha \cos \gamma \\ -\sin \beta & \sin \alpha \cos \beta & \cos \alpha \cos \beta \end{array}\right]\end{array}$$

即：先绕x轴旋转$\alpha$，再绕y轴旋转$\beta$，最后绕z轴旋转$\gamma$：

线性近似

当$\theta \approx 0$时有$\sin \theta \approx \theta ,\cos \theta \approx 1$因此当$\alpha ,\beta ,\gamma \approx 0$时：
$$\begin{array}{rl}\mathbf{R}(\alpha ,\beta ,\gamma )& \approx \left(\begin{array}{cccc}1& \alpha \beta -\gamma & \alpha \gamma +\beta & 0\\ \gamma & \alpha \beta \gamma +1& \beta \gamma -\alpha & 0\\ -\beta & \alpha & 1& 0\\ 0& 0& 0& 1\end{array}\right)\\ & \approx \left(\begin{array}{cccc}1& -\gamma & \beta & 0\\ \gamma & 1& -\alpha & 0\\ -\beta & \alpha & 1& 0\\ 0& 0& 0& 1\end{array}\right)=\hat{\mathbf{R}}(\alpha ,\beta ,\gamma ).\end{array}$$

4-3 🔖 旋转矩阵求解欧拉角

$$R=\left[\begin{array}{lll}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

对比上一节的旋转矩阵求解得到：
$$\begin{array}{c}{\theta }_x=\mathrm{atan}2\left(r_{32},r_{33}\right)\\ {\theta }_y=\mathrm{atan}2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ {\theta }_z=\mathrm{atan}2\left(r_{21},r_{11}\right)\end{array}$$