SLAM公式引出、推导和理解 1-1

---

0 数学表达式

由于相机是在某些时刻采集数据的，所以我们只关心这一段时间机器人的位置和所处地图，将该段时间由连续变为离散，$t=1,\cdots ,K$。各个时刻的位置为 $x_{k=1:K}$，构成了机器人的轨迹。地图是由路标（Landmark）构成的，对应的每一离散时刻，传感器会采集到 $N$ 个路标点 $y_1,\cdots ,y_N$。
运动方程：从 $k-1$ 时刻到 $k$ 时刻，机器人的位置变化
观测方程：在 $k$ 时刻于 $x_k$ 出检测到了某一路标 $y_j$
$$\{\begin{array}{ll}{x}_k=f(x_{k-1},u_k)+w_k,& k=1,\cdots ,K\\ z_{k,j}=h(x_k,y_j)+v_{k,j},& (k,j)\in \mathcal{O}\end{array}\text{\hspace{1em}(0.1)}$$其中 $u_k$ 、$w_k$ 和 $v_k$ 分别表示传感器的输入、运动和观测时的噪声。 $\mathcal{O}$ 是一个集合，记录着在哪个时刻观察到了哪个路标， $z_{k,j}$ 表示在 $k$ 时刻观察到的 $j$ 路标的像素坐标(图片)，$x_k$ 表示为用外参 $R,t$ 描述相机的旋转、平移，$y_j$ 表示路标点在世界坐标系下的坐标 $P_w$ ，$h(x_k,y_j)$ 表示在 $x_k$ 处观察到 $y_j$。
当知道运动测量的读数 $u$ 以及传感器的读数 $z$ 时，如何求解定位问题（估计 $x$）和建图问题（估计 $y$)，即为状态估计（State Estimation）问题。

---

1 位姿表达—运动方程

1.1 基础知识

基
空间的一组基 $(e_1,e_2,e_3)$ ，任意向量 $a$ 在这组基下的坐标为 a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 (1.1.1) a=[e_1,e_2,e_3]\left[\begin{array}{c}a_1 \\a_2\\a_3\end{array}\right]=a_1e_1+a_2e_2+a_3e_3\tag{1.1.1} a=[e1​,e2​,e3​] ​a1​a2​a3​​ ​=a1​e1​+a2​e2​+a3​e3​(1.1.1) $(a_1,a_2,a_3{)}^T$ 称为 $a$ 在此基下的坐标。

内积
对于 $a,b\in {\mathbb{R}}^3$ ，内积写为$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|\cos ⟨a,b⟩\text{\hspace{1em}(1.1.2)}$$ 其中 $⟨a,b⟩$ 指向量 $a,b$ 的夹角。
另外，当 $a$ 为单位向量时，$|a||b|\cos ⟨a,b⟩=|b|\cos ⟨a,b⟩$ 表示为 $b$ 在 $a$ 上的投影。

外积
外积为
a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 2 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ]  ⁣ b = d e f a ∧ b (1.1.3) a \times b=\left\|\begin{matrix} e_1 &e_2&e_3\\a_1 &a_2&a_3\\b_1 &b_2&b_3\end{matrix}\right\|=\left[\begin{matrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_2\\a_1b_2-a_2b_1\end{matrix}\right]=\left[\begin{matrix} 0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{matrix}\right]\!b\overset{def}{=}a^\wedge b\tag{1.1.3} a×b= ​e1​a1​b1​​e2​a2​b2​​e3​a3​b3​​ ​= ​a2​b3​−a3​b2​a3​b1​−a1​b2​a1​b2​−a2​b1​​ ​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​b=defa∧b(1.1.3)
外积得到的结果是向量，向量方向由右手定则确定，大小为 $|a||b|\sin ⟨a,b⟩$，几何意义以向量为临边的平行四边形的面积，物理意义是两个向量所在面的法向量。对于外积运算可知，$a^{\wedge }$ 为反对称矩阵，反对称矩阵的秩为偶数。
外积的性质
当 $a$ 为单位向量（$\Vert a\Vert =1$）时
$$\begin{array}{rl}& (a^{\wedge }{)}^2=a{a}^T-I\\ & (a^{\wedge }{)}^3=a^{\wedge }(a{a}^T-I)=-a^{\wedge }\\ & (a^{\wedge }{)}^4=a^{\wedge }(-a^{\wedge })=-(a^{\wedge }{)}^2\end{array}\text{\hspace{1em}(1.1.4)}$$

混合积
标量三重积（Scalar triple product）
$a\cdot (b\times c)=b\cdot (c\times a)=c\cdot (a\times b)$
几何意义为三个向量为临边的平行六面体的体积

$(a\cdot (b\times c))a=(a\times b)\times (a\times c)$

向量三重积
$a\times (b\times c)=b(a\cdot c)-c(a\cdot b)$
$(a\times b)\times c=b(a\cdot c)-a(b\cdot c)$

记法：因为等号左边叉乘结果是向量，所以等号右边也是向量。只需记忆等号左边哪两个向量先叉乘和第三个向量的位置关系即可。如 $b,c$ 先叉乘且 $a$ 在其左边，则等号右边表明的向量关系为 $b-c$，点乘是标量，向量顺序（$a\cdot c$ 和 $c\cdot a$一样）无所谓；再如 $a,b$ 先叉乘且 $c$ 在其右边，则等号右边表明的向量关系为 $b-a$。等号右边再将其余向量补全。以第二个向量（$b$）为基准，第二个向量（$b$）减括号里的另一个向量。

$a\times (b\times c)+b\times (c\times a)+c\times (a\times b)=0$

---

1.2 旋转矩阵

习惯上将向量写在变量右侧，即使用的是列向量而非行向量。
对于同一个向量 $a$，在不同基底下的坐标
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] [ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = d e f R a ′ (1.2.1) \begin{aligned}a=[e_1,e_2,e_3]\left[\begin{matrix}a_1 \\a_2\\a_3\end{matrix}\right]&=[e_1^\prime,e_2^\prime,e_3^\prime]\left[\begin{matrix}a_1^\prime \\a_2^\prime\\a_3^\prime\end{matrix}\right]\\ \left[\begin{matrix}a_1\\a_2\\a_3\end{matrix}\right]&=\left[\begin{matrix}e_1^Te_1^\prime &e_1^Te_2^\prime &e_1^Te_3^\prime \\e_2^Te_1^\prime &e_2^Te_2^\prime &e_2^Te_3^\prime \\e_3^Te_1^\prime &e_3^Te_2^\prime &e_3^Te_3^\prime\end{matrix}\right]\left[\begin{matrix} a_1^\prime \\a_2^\prime\\a_3^\prime\end{matrix}\right]\overset{def}{=}Ra^\prime\end{aligned}\tag{1.2.1} a=[e1​,e2​,e3​] ​a1​a2​a3​​ ​ ​a1​a2​a3​​ ​​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​=defRa′​(1.2.1)
旋转矩阵 $R$ （Rotation Matrix）描述了旋转本身（一个基通过旋转矩阵旋转到另一个基）。同时，矩阵各分量为两个基的内积，由于基为单位向量，所以实际上是各基向量夹角的余弦值，也成为方向余弦矩阵（Direction Cosine Matrix）。
$n$ 维旋转矩阵的集合定义为：
$$SO(n)=\{R\in {\mathbb{R}}^{n\times n}|R{R}^T=I,det(R)=1\}\text{\hspace{1em}(1.2.2)}$$
$SO(n)$ 是特殊正交群（Special Orthogonal Group）。$R$ 为正交矩阵，且行列式为 $1$(旋转矩阵的约束条件)。

---

1.3 变换矩阵

为了使旋转矩阵和平移向量为线性变换，引入了齐次坐标（Homogeneous Coordinate）和变换矩阵（Transform Matrix）。
数学技巧：在一个三维向量的末尾添加 $1$，将其变成四维向量，称为齐次坐标（为了满足矩阵的乘法），意义为向量乘以任意非零常数，仍表示为同一向量。 [ k x k y k z k ] ( = k [ x y z 1 ] ) ⇌ [ x y z 1 ] (1.3.1) \left[\begin{matrix}kx\\ky\\kz\\k\end{matrix}\right]\left(=k\left[\begin{matrix}x\\y\\z\\1\end{matrix}\right]\right)\rightleftharpoons \left[\begin{matrix}x\\y\\z\\1\end{matrix}\right]\tag{1.3.1} ​kxkykzk​ ​ ​=k ​xyz1​ ​ ​⇌ ​xyz1​ ​(1.3.1)非齐次坐标转换为齐次坐标之后，有
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\stackrel{def}{=}T\left[\begin{array}{c}a\\ 1\end{array}\right]\text{\hspace{1em}(1.3.2)}$$
$T$ 的集合定义为$$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\right\}\text{\hspace{1em}(1.3.3)}$$
$SE(3)$ 称为特殊欧氏群（Special Euclidean Group）。关于变换矩阵 $T$，具有特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为 $0$ 向量，右下角为 $1$。

求逆：
二维矩阵块，“主对调负变号，再除以行列式”。
$$T^{-1}=\frac{1}{R}\left[\begin{array}{cc}1& -t\\ 0^T& R\end{array}\right]=\left[\begin{array}{cc}{R}^{-1}& -R^{-1}t\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$
（$-R^{-1}$ 左乘矩阵，不是右乘是为了使右上块维数对应）

若绕静坐标系（世界坐标系）旋转，则左乘矩阵，即变换矩阵 * 坐标矩阵；若绕动坐标系（自身坐标系）旋转，则右乘矩阵，即坐标矩阵 * 变换矩阵。实际上，对一个图像、点云进行旋转，则均是左乘矩阵实现。
$T_{lr}$ 理解为 $r$ 帧的坐标转换到 $l$ 帧的坐标的变换矩阵，也可以读作 $l$ 帧坐标在 $r$ 帧坐标系下的变换。

---

1.4 旋转向量

旋转矩阵 $R$ （变换矩阵 $T$） 的缺点是 $R$（$T$） 有9（16）个量，但旋转（变换）一次只有3（6）个自由度改变，且 $R$ 本身自带约束条件 —— 引出旋转向量（Rotation Vector）。
事实上，任意旋转都可以用一个旋转轴和一个旋转角度来刻画。于是，使用一个向量：方向为与旋转轴 $n$ （单位向量表示方向）一致，大小为旋转角 $\theta$（弧度制），这种向量称为旋转向量，也叫角轴或轴角（Axis Angle）。

旋转向量转换为旋转矩阵
由罗德里格斯公式（Rodrigues’s Formula）证得：
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }\text{\hspace{1em}(1.4.1)}$$旋转矩阵转换为旋转向量
对罗德里格斯公式两边取迹。$$\begin{array}{rl}tr(R)& =\cos \theta tr(I)+(1-\cos \theta )tr(n{n}^T)+\sin \theta tr(n^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )tr(n^{T}n)\\ & =1+2\cos \theta \end{array}\text{\hspace{1em}(1.4.2)}$$则$$\theta =arc\cos \frac{tr(R)-1}{2}\text{\hspace{1em}(1.4.3)}$$
关于转轴 $n$，旋转轴上的向量在旋转之后不改变$$n=Rn\text{\hspace{1em}(1.4.4)}$$表示转轴 $n$ 是矩阵 $R$ 特征值为 $1$ 对应的特征向量。

---

1.5 欧拉角

旋转矩阵、旋转向量对人类来说表达旋转很不直观。欧拉角用于人机交互。
万向锁使欧拉角具有奇异性。

---

1.6 四元数

旋转向量带有奇异性（$\theta$ 角的周期性）。实际上，找不到不带奇异性的三维向量表述方式。
用复数集合 $\mathbb{C}$ 表示复平面上的向量，而复数的乘法表示复平面上的旋转：乘上复数 $i$ 相当于逆时针把一个复向量旋转 $90^{\circ }$。类比于此，表示三维空间旋转的复数：四元数（Quaternion），是紧凑的、没有奇异性的代数。
一个四元数有一个实部和三个虚部。$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$也可以用一个标量和一个向量表示$$q=[s,v{]}^T，s=q_0\in \mathbb{R}，v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$用单位四元数表示三维空间任意旋转。

四元数的运算

有两个四元数 $q_a=s_a+x_{a}i+y_{a}j+z_{a}k=[s_a,v_a{]}^T$ ，$q_b=s_b+x_{b}i+y_{b}j+z_{b}k=[s_b,v_b{]}^T$

加减法
$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b{]}^T$$
乘法
虚部单位相乘满足右手定则，便于记忆 $ijkijk$ 正向正号，反向负号$$\begin{array}{rl}{q}_a{q}_b& =s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i\\ & +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j\\ & +(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b)k\end{array}$$或$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b{]}^T$$
因为忽略复数单位的情况下，$i$和$i$相乘为$i\times i=-1$可视为负点乘，$i$和$j$相乘为$i\times j=k$可视为叉乘。因此对于实部来说，是负的$v_a^T{v}_b$，虚部是正的。

模长$$\Vert q_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$性质$$\Vert q_a{q}_b\Vert =\Vert q_a\Vert \Vert q_b\Vert$$共轭$$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$$共轭与本身相乘，得到实四元数，实部为模长的平方：$$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0{]}^T$$逆$$q^{-1}=q^{\ast }/\Vert q{\Vert }^2$$以此定义得$$q{q}^{-1}=q^{-1}q=1$$如果 $q$ 为单位四元数，其逆和共轭就是同一个量。同时，乘积的逆具有和矩阵相似得性质：$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}.$$
四元数表示旋转
空间点 $p=[x,y,z]\in {\mathbb{R}}^3$，旋转用单位四元数 $q$ 表示。旋转步骤：首先将 $p$ 转换为与四元数同一形式。由于四元数虚部为向量，故点 $p$ 的四元数形式为（虚四元数）$$p=[0,x,y,z{]}^T=[0,v{]}^T$$旋转之后的点 $p^{\prime }$ 可表示为（类似于矩阵对角化） $$p^{\prime }=qp{q}^{-1}=qp{q}^{\ast }\text{\hspace{1em}(1.6.1)}$$求得点 $p^{\prime }$ 为四元数（虚四元数）形式，最终三维坐标形式为 $p^{\prime }$ 的虚部¹。

四元数到其他旋转的转换
对于式子(1.6.1)来说，要将四元数表示的旋转转换为通常形式（左乘旋转矩阵） $p^{\prime }=Xp$ 的形式，先引入如下定义：
设 $q=[s,v{]}^T$ ，定义符号 ${}^{+}$ 和 ${}^{\oplus }$ 为（分别是左右乘算子）：$$q^{+}=\left[\begin{array}{cc}s& -v^T\\ v& sI+v^{\wedge }\end{array}\right]q^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ v& sI-v^{\wedge }\end{array}\right]$$

故有 $p_1$ 与 $p_2$ 的乘积
$$p_1^{+}{p}_2=p_2^{\oplus }{p}_1{p}_1^{+}{p}_2^{\oplus }=p_2^{\oplus }{p}_1^{+}$$
$uvw$分别为四元数
$$\begin{array}{rl}(u^{+}{)}^T=(u^{+}{)}^{-1}=(u^{-1}{)}^{+}& (u^{\oplus }{)}^T=(u^{\oplus }{)}^{-1}=(u^{-1}{)}^{\oplus }\\ (u^{+}v{)}^{-1}=v^{-1+}{u}^{-1}& (u^{\oplus }v{)}^{-1}=v^{-1\oplus }{u}^{-1}\\ (u^{+}v{)}^{+}w=u^{+}(v^{+}w)=u^{+}{v}^{+}w& (u^{\oplus }v{)}^{\oplus }w=u^{\oplus }(v^{\oplus }w)=u^{\oplus }{v}^{\oplus }w\\ \alpha u^{+}+\beta v^{+}=(\alpha u+\beta v{)}^{+}& \alpha u^{\oplus }+\beta v^{\oplus }=(\alpha u+\beta v{)}^{\oplus }\end{array}$$
其中 $\alpha$ 和 $\beta$ 为标量。

则
$$\begin{array}{rl}{p}^{\prime }& =Xp\\ & =qp{q}^{-1}=q(q^{-1}{)}^{\oplus }p\\ & =q^{+}(q^{-1}{)}^{\oplus }p\end{array}\text{\hspace{1em}(1.6.2)}$$
（最后一步是因为倒数第二步 $q$ 维数 $2\times 1$，而 $(q^{-1}{)}^{\oplus }$ 维数为 $2\times 2$ 不满足矩阵乘法）
$$q^{+}(q^{-1}{)}^{\oplus }=\left[\begin{array}{cc}1& 0\\ 0^T& v{v}^T+s^{2}I+2s{v}^{\wedge }+(v^{\wedge }{)}^2\end{array}\right]$$令$$R=v{v}^T+s^{2}I+2s{v}^{\wedge }+(v^{\wedge }{)}^2\text{\hspace{1em}(1.6.3)}$$因为 $p^{\prime }$ 和 $p$ 为虚四元数，有$$\begin{array}{rl}{p}^{\prime }& =q^{+}(q^{-1}{)}^{\oplus }p\\ \left[\begin{array}{c}0\\ v^{\prime }\end{array}\right]& =\left[\begin{array}{cc}1& 0\\ 0^T& R\end{array}\right]\left[\begin{array}{c}0\\ v\end{array}\right]\\ v^{\prime }& =Rv\end{array}\text{\hspace{1em}(1.6.4)}$$则，右下角的分块矩阵给出了四元数到旋转矩阵的关系。
对式子(1.6.3)求迹，得$$\begin{array}{rl}tr(R)& =tr(v{v}^T)+3s^2+tr((v^{\wedge }{)}^2)\\ & =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ & =4s^2-1\end{array}\text{\hspace{1em}(1.6.5)}$$（$s$ （四元数）到 $R$ （旋转矩阵）的关系)
四元数到旋转向量的关系为四元数到旋转矩阵加上旋转矩阵到旋转向量的关系，所以由式子(1.4.3)得$$\begin{array}{rl}\theta & =arc\cos \frac{tr(R)-1}{2}\\ & =arc\cos (2s^2-1)\end{array}$$有二倍角公式 $\cos 2\theta =2{\cos }^2\theta -1$
则$$\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1$$所以$$\theta =2arc\cos s\text{\hspace{1em}(1.6.6)}$$
$s$ 为由单位四元数 $q$ 的实部，由于旋转向量在旋转前后是不变的，所以旋转向量的方向可由 $q$ 的虚部 $v$ 表示 $[n_x,n_y,n_z{]}^T=[v_1,v_2,v_3{]}^T/\sin \frac{\theta }{2}$
（因为 $n$ 为单位向量 ，所以用单位四元数 $q$ 的虚部表示时需要将虚部单位化，又由式子(1.6.6)可得 $s=\cos \frac{\theta }{2}$，所以 $v$ 的模长为 $\sin \frac{\theta }{2}$）
$$\{\begin{array}{l}\theta =2arc\cos s\\ [n_x,n_y,n_z{]}^T=[v_1,v_2,v_3{]}^T/\sin \frac{\theta }{2}\end{array}\text{\hspace{1em}(1.6.7)}$$

---

A$\to$B	旋转矩阵(B)	旋转向量(B)	四元数(B)
旋转矩阵(A)	N/A	(1.4.3)(1.4.4)	很繁琐
旋转向量(A)	(1.4.1)	N/A	(1.6.7)
四元数(A)	(1.6.3)	(1.6.7)	N/A

---

---

1. 参考四元数和旋转(Quaternion & rotation) 3.4 ↩︎