IMU运动模型基础：旋转矩阵，李群李代数

旋转运动学

速度合成公式

假设有如上图所示的一静一动两个坐标系，按照习惯，我们记动坐标系为体系$b$，静坐标系为惯性系 $i$。此时有一个物体在空间运动，记其在两个坐标系下的坐标分别为$r^i$ 和$r^b$，两者之间的关系为：

$r^i=r_{ib}^i+R_{ib}{r}^b$

式中上标表示矢量所在的坐标系，$r_{ib}^i$表示 $b$系到$i$ 系的平移变换矢量，$R_{ib}$表示$b$ 系到$i$ 系的旋转变换矩阵。对上式求解时间导数有：

$\begin{array}{rl}\frac{d{r}^i}{dt}& =\frac{d(r_{ib}^i+R_{ib}{r}^b)}{dt}\\ & =\frac{d{r}_{ib}^i}{dt}+\frac{d{R}_{ib}}{dt}{r}^b+R_{ib}\frac{d{r}^b}{dt}\\ & =v_{ib}^i+R_{ib}[{\omega }_{ib}^b{]}_{\times }{r}^b+R_{ib}{v}^b\end{array}$

这里我们引入矢量叉积的一个重要几何性质：
定理1：矢量叉积的旋转不变性。若矩阵$R$为旋转矩阵，则对于矢量叉积$a\times b$有$(Ra)\times (Rb)=R(a\times b)$ 。
利用这一性质，上式可以重写为：

$\begin{array}{rl}\frac{d{r}^i}{dt}& =v_{ib}^i+R_{ib}[{\omega }_{ib}^b{]}_{\times }{r}^b+R_{ib}{v}^b\\ & =v_{ib}^i+[R_{ib}{\omega }_{ib}^b{]}_{\times }[R_{ib}{r}^b]+R_{ib}{v}^b\\ v^i& =v_{ib}^i+{\omega }_{ib}^i\times r^i+R_{ib}{v}^b\end{array}$

上式描述了物体的速度在动坐标系和静坐标系之间的转换关系，又称为速度合成公式或科里奥利方程。由于坐标系原点之间的位移矢量与旋转是相互独立，互不相应的，因此在后续讨论中，将省略式中的$v_{ib}^i$项，只讨论旋转相关部分。

加速度合成公式

进一步地，我们对上式量测继续求时间导数有：

$\begin{array}{rl}\frac{d{v}^i}{dt}& =\frac{d({\omega }_{ib}^i\times r^i)}{dt}+\frac{d(R_{ib}{v}^b)}{dt}\\ & =\frac{d{\omega }_{ib}^i}{dt}\times r^i+{\omega }_{ib}^i\times \frac{d{r}^i}{dt}+\frac{d{R}_{ib}}{dt}{v}^b+R_{ib}\frac{d{v}^b}{dt}\\ a^i& ={\dot{\omega }}_{ib}^i\times r^i+{\omega }_{ib}^i\times v^i+{\omega }_{ib}^i\times r^i+R_{ib}{a}^b\end{array}$

带入速度合成公式有：

$\begin{array}{rl}{a}^i& ={\dot{\omega }}_{ib}^i\times r^i+{\omega }_{ib}^i\times v^i+{\omega }_{ib}^i\times r^i+R_{ib}{a}^b\\ & ={\dot{\omega }}_{ib}^i\times r^i+{\omega }_{ib}^i\times ({\omega }_{ib}^i\times r^i+R_{ib}{v}^b)+{\omega }_{ib}^i\times r^i+R_{ib}{a}^b\\ & =\underset{Euler}{\underbrace{{\dot{\omega }}_{ib}^i\times r^i}}+\underset{Centrifugal}{\underbrace{{\omega }_{ib}^i\times ({\omega }_{ib}^i\times r^i)}}+\underset{Coriolis}{\underbrace{2{\omega }_{ib}^i\times r^i}}+R_{ib}{a}^b\end{array}$

上式前三项依次为欧拉力、离心力和科氏力，该式表征了运动物体在静坐标系和动坐标系下的加速度变换关系，称为加速度合成公式。

群的定义

设有一个集合$G$，集合中的元素为$g$，元素之间存在乘法运算 $\cdot$ ，称$G$是一个群，如果其满足以下条件：

• 封闭性 $\forall g_1,g_2\in G,g_1\cdot g_2\in G$
• 结合律 $g_1\cdot (g_2\cdot g_3)=(g_1\cdot g_2)\cdot g_3$
• 存在幺元 $\forall g\in G,e\cdot g=g\cdot e=g$
• 存在逆元 $\forall g\in G,\exists g^{-1}\in G,g\cdot g^{-1}=g^{-1}\cdot g=e$

建模中常用的群

只考虑实数域，所有$n\times n$的正交矩阵$R$构成了一个群，称为正交群，记为$O(n)$在这些。正交矩阵中，行列式为正1的矩阵本身也构成一个群，称为特殊正交群，记为$SO(n)$。也就是说$SO(n)$是$O(n)$的一个子群（准确的说是正规子群）。

在$SLAM$以及很多机器人技术中，最常用到的就是$SO(3)$，也就是所有三维旋转矩阵构成的群，可以用来表示机器人或者飞行器的姿态。

当需要描述一个刚体变换（旋转加平移）时，光靠一个旋转矩阵是不够的，当我们使用齐次坐标表示空间中的点时，对该点的刚体变换可用下面的$4\times 4$矩阵表示

$$T=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)$$

其中$R\in {\mathbb{R}}^{3\times 3}$是旋转矩阵，$t\in {\mathbb{R}}^3$是平移矢量。容易验证，矩阵$T$也是满足构成群的四个条件的，其中逆元为

$$T^{-1}=\left(\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right)$$

幺元就是单位阵，因此所有刚体变换矩阵也构成一个群，称为特殊欧式群，记为$SE(3)$。当然也存在$SE(n)$，看做是任意维度下的刚体变换。如果不要求$T$里面的$R$必须是一个旋转矩阵，甚至不要求是正交矩阵，只要可逆就行，那么我们就得到了更一般的仿射群。

为了方便介绍不变卡尔曼滤波，这里对$SE(3)$做一个扩展，我们发现型如

$$\chi =\left(\begin{array}{cccccc}R& t& p_1& p_2& ...& p_k\\ 0& 1& 0& 0& ...& 0\\ 0& 0& 1& 0& ...& 0\\ 0& 0& 0& 1& ...& 0\\ ⋮& & & & & \\ 0& 0& 0& 0& ...& 1\end{array}\right)$$

的矩阵，$(4+k)\times (4+k),p_k\in {\mathbb{R}}^3$，也满足群定义，逆元为

$${\chi }^{-1}=\left(\begin{array}{cccccc}{R}^T& -R^{T}t& -R^T{p}_1& -R^T{p}_2& ...& -R^T{p}_k\\ 0& 1& 0& 0& ...& 0\\ 0& 0& 1& 0& ...& 0\\ 0& 0& 0& 1& ...& 0\\ ⋮& & & & & \\ 0& 0& 0& 0& ...& 1\end{array}\right)$$

幺元也是单位阵，我们将这种矩阵构成的群记为$S{E}_{k+1}(3)$。

切空间与李代数

$SO(3)$和$SE(3)$这类群，还有一个特点是平滑连续，即他们是一个平滑的流形，这类群称为李群。在流形上微分可以得到流形上某一点的切空间（类比于对曲线微分得到切线）。其中在幺元处的切空间最重要（与后面的指数映射有关）。下面分别来求一下$SO(3)，SE(3)$，以及$S{E}_k(3)$在幺元处的切空间。

$SO(3)$的切空间

具体写出$SO(3)$的定义为

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , d e t ( R ) = 1 } SO(3)=\{R\in\mathbb{R}^{3\times 3}|R^TR=I, det(R)=1\} SO(3)={R∈R3×3∣RTR=I,det(R)=1}

现在假设$R$是一个随时间变化的量$R(t)$，则$R(t{)}^{T}R(t)=I$两边对时间求导，并令$\partial R/\partial t=\dot{R}$，有

$${\dot{R}}^{T}R+R^T\dot{R}=0$$

由于要求幺元处的切空间，令上式中的$R=I$，得

${\dot{R}}^T+\dot{R}=0$

即$\dot{R}$是一个斜对称矩阵，有如下形式

$\dot{R}=\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right)$

这就是$SO(3)$幺元处的切空间所具有的形式。以上斜对称矩阵实际只有3个维度，我们定义一个映射

${\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}_{\times }=\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right)$

则矢量${\xi }_R=(x,y,z{)}^T$就是我们常说的轴角。矢量的方向代表旋转的轴，矢量的模长代表要旋转的角度。

$SE(3)$与$S{E}_{k+1}(3)$的切空间

有了$$SO(3)$$的切空间后，$$SE(3)$$与$$S{E}_{k+1}(3)$$的切空间就很好求了。幺元处的切空间实际上是幺元在流形上发生微小摄动产生的增量，比如对$$SO(3)$$而言，幺元$$I$$在流形上的微小摄动产生$I+\Lambda ({\xi }_R)$，那么平移量$t$身属于向量空间，微小的摄动仍然是一个三维矢量。因此$$SE(3)$$切空间的形式就是

$\dot{T}=\left(\begin{array}{cc}[{\xi }_R{]}_{\times }& {\xi }_t\\ 0& 0\end{array}\right)$

其中${\xi }_R,{\xi }_t\in {\mathbb{R}}^3$。

而$$S{E}_{k+1}(3)$$的切空间自然就是

$\dot{\chi }=\left(\begin{array}{ccccc}[{\xi }_R{]}_{\times }& {\xi }_t& {\xi }_{p_1}& ...& {\xi }_{p_k}\\ 0& 0& 0& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 0\end{array}\right)$

上面的表达是很冗余的，我们也可以像$SO(3)$那样定义一个映射

$\Lambda \left(\begin{array}{c}{\xi }_R\\ {\xi }_t\\ {\xi }_{p_1}\\ ⋮\\ {\xi }_{p_k}\end{array}\right)=\left(\begin{array}{ccccc}[{\xi }_R{]}_{\times }& {\xi }_t& {\xi }_{p_1}& ...& {\xi }_{p_k}\\ 0& 0& 0& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 0\end{array}\right)$

我们可以发现，所有这些群的切空间都是向量空间，即满足矢量的线性运算（对加法和数乘封闭），实际上，它们还对另外一种运算封闭，被称为李括号，定义为$[X,Y]=XY-YX$，其中$X,Y$是群的切空间的矩阵形式。并可以验证有如下关系成立

$\begin{array}{rl}& \\ [X,Y]+[Y,X]& =0\\ [X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]& =0\end{array}$

如果将李括号也视为一种乘法（和矩阵一样不满足交换律），那么切空间就同时对加法运算和乘法运算封闭，因此构成了一个环$（Ring）！$所以，这样的切空间也被称为李代数。$$SO(3),SE(3),S{E}_{k+1}(3)$$的李代数分别用$$so(3),se(3),s{e}_{k+1}(3)$$表示。

指数映射

先对指数函数做泰勒展开，有

$e^x=1+x+\frac{1}{2!}{x}^2+...+\frac{1}{n!}{x}^n+...$

将其中的$x$替换为某个矩阵$A$，可得到

$e^A=I+A+\frac{1}{2!}{A}^2+...+\frac{1}{n!}{A}^n+...$

可证明上式对任意$n\times n$实矩阵都是收敛的。

上式构成了指数映射的基础。当矩阵$A$属于某个李群的李代数时，比如$A\in so(3)$，指数映射将其映射到对应的李群上，即$e^A\in SO(3)$。还记得李代数是李群在幺元处的切空间，也就是说，通过指数映射，整个李群完全可以通过幺元处的切空间得到！（实际上，指数映射一般只能将李代数映射到李群的一部分上，但对于$$SO(n),SE(n),S{E}_{k+1}(n)$$而言，能够映射到全部，即任何一个属于这些群的元素，一定能在对应李代数中找到一个对应元素）。下面分别给出$$SO(n),SE(n),S{E}_{k+1}(n)$$指数映射的解析形式。

$so(3)$$到$$SO(3)$

为了能够利用李代数的矢量表示（即轴角），我们一般定义其指数映射为

$\text{Exp}({\xi }_{\mathbf{R}})=\exp ([{\xi }_{\mathbf{R}}{]}_{\times })$

设${\xi }_{\mathbf{R}}=\theta \mathbf{u}$，其中$\theta$是一个标量，代表旋转的角度，$\mathbf{u}$是单位矢量，表示旋转的轴。则利用指数函数的泰勒展开式，有

$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+\theta [\mathbf{u}{]}_{\times }+\frac{1}{2!}{\theta }^2[\mathbf{u}{]}_{\times }^2+...+\frac{1}{\mathbf{n}!}{\theta }^{\mathbf{n}}[\mathbf{u}{]}_{\times }^{\mathbf{n}}+...$

注意到

$\begin{array}{rl}& \\ [\mathbf{u}{]}_{\times }^2& =\mathbf{u}{\mathbf{u}}^{\mathbf{T}}-\mathbf{I}\\ [\mathbf{u}{]}_{\times }^3& =-[\mathbf{u}{]}_{\times }\end{array}$

有

$\begin{array}{rl}& \\ [\mathbf{u}{]}_{\times }^4& =-[\mathbf{u}{]}_{\times }^2\\ [\mathbf{u}{]}_{\times }^5& =[\mathbf{u}{]}_{\times }\\ [\mathbf{u}{]}_{\times }^6& =[\mathbf{u}{]}_{\times }^2\\ [\mathbf{u}{]}_{\times }^7& =-[\mathbf{u}{]}_{\times }\\ ...& \end{array}$

于是我们对展开式分离奇偶项，可得

$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...)[\mathbf{u}{]}_{\times }+(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+...)[\mathbf{u}{]}_{\times }^2$

又注意到

$\begin{array}{rl}\sin \theta & =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...\\ \cos \theta & =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...\end{array}$

最终可得

$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2$

这就是众所周知的罗德里格斯变换。

$se(3)到SE(3)$

设$\xi =({\xi }_R^T,{\xi }_t^T{)}^T$。同样定义

$\text{Exp}(\xi )=\exp (\Lambda (\xi ))$

推导方式与处理$$SO(3)$$时完全一样，首先计算泰勒级数，可以得到

$\text{Exp}(\xi )=\left(\begin{array}{cc}\sum \frac{1}{n!}{\theta }^n[\mathbf{u}{]}_{\times }^{\mathbf{n}}& (\sum \frac{1}{n!}{\theta }^{n-1}[\mathbf{u}{]}_{\times }^{\mathbf{n}-1}){\xi }_{\mathbf{t}}\\ 0& 1\end{array}\right)$

矩阵中左上角元素就是$$so(3)$$的指数映射，而另外一项，同样利用三角函数极数的对照关系，可以得到

$\text{Exp}(\xi )=\left(\begin{array}{cc}R& V{\xi }_t\\ 0& 1\end{array}\right)$

其中

$\begin{array}{rl}R& =I+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2\\ V& =I+\frac{1-\cos \theta }{\theta }[\mathbf{u}{]}_{\times }+\frac{\theta -\sin \theta }{\theta }[\mathbf{u}{]}_{\times }^2\end{array}$

$$s{e}_{k+1}(3)到S{E}_{k+1}(3)$$

简单计算即可发现，$$s{e}_{k+1}(3)$$的指数映射就是$se(3)$的简单扩展，这里直接给出结果。设$\xi =({\xi }_R^T,{\xi }_t^T,{\xi }_{p_1}^T,...,{\xi }_{p_k}^T{)}^T$，则

$\text{Exp}(\xi )=\left(\begin{array}{ccccc}R& V{\xi }_t& V{\xi }_{p_1}& ...& V{\xi }_{p_k}\\ 0& 1& 0& ...& 0\\ 0& 0& 1& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 1\end{array}\right)$

其中

$\begin{array}{rl}R& =I+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2\\ V& =I+\frac{1-\cos \theta }{\theta }[\mathbf{u}{]}_{\times }+\frac{\theta -\sin \theta }{\theta }[\mathbf{u}{]}_{\times }^2\end{array}$

指数映射的其他性质

当两个矩阵满足交换律时，即$$AB=BA$$时，有

$e^A{e}^B=e^{A+B}$

指数映射的行列式与矩阵的迹还有如下一个美妙关系

$det(e^A)=e^{tr(A)}$

群伴随

这里省去很多关于伴随的导出过程，直接给出其定义。

$SO(3)$的伴随

设$R\in SO(3),\xi \in so(3)$，则在$R$的伴随记为$A{d}_R$，定义为

$R\cdot \text{Exp}(\xi )=\text{Exp}(A{d}_R\cdot \xi )\cdot R$

下面给出$A{d}_R$的具体形式。由上式可得

$\begin{array}{rl}\text{Exp}(A{d}_R\cdot \xi )& =R\cdot \text{Exp}(\xi )\cdot R^{-1}\\ & =R(\sum _n^{\infty }\frac{1}{n!}[\xi {]}_{\times }^n)R^{-1}\\ & =\sum _n^{\infty }\frac{1}{n!}(R[\xi {]}_{\times }{R}^{-1}{)}^n\\ & =\exp (R[\xi {]}_{\times }{R}^{-1})\\ & =\exp ([R\xi {]}_{\times })\\ & =\text{Exp}(R\xi )\end{array}$

所以有

$A{d}_R=R$

$SE(3)与S{E}_{k+1}(3)$的伴随

推导方式与$SO(3)$类似，设$T\in SE(3)$，即

$T=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)$

又设$\xi =({\xi }_R^T,{\xi }_t^T{)}^T\in se(3)$，有

$\begin{array}{rl}\text{Exp}(A{d}_T\cdot \xi )& =\exp (T\Lambda (\xi )T^{-1})\\ & =\exp \left(\begin{array}{cc}R[{\xi }_R{]}_{\times }{R}^{-1}& -R[{\xi }_R{]}_{\times }{R}^{-1}t+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\begin{array}{cc}[R{\xi }_R{]}_{\times }& -[R{\xi }_R{]}_{\times }t+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\begin{array}{cc}[R{\xi }_R{]}_{\times }& [t{]}_{\times }R{\xi }_R+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\Lambda \right[\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)\left(\begin{array}{c}{\xi }_R\\ {\xi }_t\end{array}\right)\left]\right)\\ & =\text{Exp}\left(\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)\left(\begin{array}{c}{\xi }_R\\ {\xi }_t\end{array}\right)\right)\end{array}$

因此有

$A{d}_T=\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)$

简单计算可以发现$$S{E}_{k+1}(3)$$的伴随也是$$SE(3)$$扩展，设$\chi \in S{E}_{k+1}(3)$，有

$A{d}_{\chi }=\left(\begin{array}{cccccc}R& 0& 0& 0& ...& 0\\ [t{]}_{\times }R& R& 0& 0& ...& 0\\ [p_1{]}_{\times }R& 0& R& 0& ...& 0\\ [p_2{]}_{\times }R& 0& 0& R& ...& 0\\ ⋮& & & & & \\ [p_k{]}_{\times }R& 0& 0& 0& ...& R\end{array}\right)$