旋转运动学

速度合成公式

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

ri=ribi+Ribrbr^{i}=r^{i}_{ib}+R_{ib}r^{b}ri=ribi+Ribrb

式中上标表示矢量所在的坐标系，ribir^{i}_{ib}ribi表示 bbb系到iii 系的平移变换矢量，RibR_{ib}Rib表示bbb 系到iii 系的旋转变换矩阵。对上式求解时间导数有：

dridt=d(ribi+Ribrb)dt=dribidt+dRibdtrb+Ribdrbdt=vibi+Rib[ωibb]×rb+Ribvb\begin{aligned}\frac{dr^{i}}{dt}&=\frac{d(r^{i}_{ib}+R_{ib}r^{b})}{dt}\\&=\frac{dr^{i}_{ib}}{dt}+\frac{dR_{ib}}{dt}r^{b}+R_{ib}\frac{dr^{b}}{dt}\\&=v^{i}_{ib}+R_{ib}[\omega ^{b}_{ib}]_{\times}r^{b}+R_{ib}v^{b}\end{aligned}dtdri=dtd(ribi+Ribrb)=dtdribi+dtdRibrb+Ribdtdrb=vibi+Rib[ωibb]×rb+Ribvb

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

dridt=vibi+Rib[ωibb]×rb+Ribvb=vibi+[Ribωibb]×[Ribrb]+Ribvbvi=vibi+ωibi×ri+Ribvb\begin{aligned}\frac{dr^{i}}{dt}&=v^{i}_{ib}+R_{ib}[\omega ^{b}_{ib}]_{\times}r^{b}+R_{ib}v^{b} \\&=v^{i}_{ib}+[R_{ib}\omega^{b}_{ib}]_{\times}[R_{ib}r^{b}]+R_{ib}v^{b}\\v^{i}&=v^{i}_{ib}+\omega ^{i}_{ib}\times r^{i}+R_{ib}v^{b} \end{aligned}dtdrivi=vibi+Rib[ωibb]×rb+Ribvb=vibi+[Ribωibb]×[Ribrb]+Ribvb=vibi+ωibi×ri+Ribvb

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

加速度合成公式

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

dvidt=d(ωibi×ri)dt+d(Ribvb)dt=dωibidt×ri+ωibi×dridt+dRibdtvb+Ribdvbdtai=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab\begin{aligned} \frac{dv^{i}}{dt} &= \frac{d(\omega ^{i}_{ib} \times r^{i})}{dt}+\frac{d(R_{ib}v^{b})}{dt} \\ &=\frac{d\omega ^{i}_{ib}}{dt} \times r^{i}+\omega ^{i}_{ib}\times \frac{dr^{i}}{dt}+\frac{dR_{ib}}{dt}v^{b}+R_{ib} \frac{dv^{b}}{dt}\\ a^{i}&= \dot{\omega}^{i}_{ib}\times r^{i}+\omega ^{i}_{ib} \times v^{i}+\omega ^{i}_{ib}\times r^{i}+R_{ib}a^{b} \end{aligned}dtdviai=dtd(ωibi×ri)+dtd(Ribvb)=dtdωibi×ri+ωibi×dtdri+dtdRibvb+Ribdtdvb=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab

带入速度合成公式有：

ai=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab=ω˙ibi×ri+ωibi×(ωibi×ri+Ribvb)+ωibi×ri+Ribab=ω˙ibi×ri⏟Euler+ωibi×(ωibi×ri)⏟Centrifugal+2ωibi×ri⏟Coriolis+Ribab\begin{aligned}a^{i}&= \dot{\omega}^{i}_{ib}\times r^{i}+\omega ^{i}_{ib} \times v^{i}+\omega ^{i}_{ib}\times r^{i}+R_{ib}a^{b}\\&= \dot{\omega}^{i}_{ib}\times r^{i}+\omega ^{i}_{ib} \times (\omega^{i}_{ib} \times r^{i} +R_{ib}v^{b})+\omega ^{i}_{ib}\times r^{i}+R_{ib}a^{b}\\&= \underbrace{\dot{\omega}^{i}_{ib}\times r^{i}}_{Euler}+\underbrace{\omega ^{i}_{ib} \times (\omega^{i}_{ib} \times r^{i} )}_{Centrifugal}+\underbrace{2\omega ^{i}_{ib}\times r^{i}}_{Coriolis}+R_{ib}a^{b} \end{aligned}ai=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab=ω˙ibi×ri+ωibi×(ωibi×ri+Ribvb)+ωibi×ri+Ribab=Eulerω˙ibi×ri+Centrifugalωibi×(ωibi×ri)+Coriolis2ωibi×ri+Ribab

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

群的定义

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

封闭性 ∀g1,g2∈G,g1⋅g2∈G\forall g_1, g_2\in G,\ \ g_1\cdot g_2\in G∀g1,g2∈G, g1⋅g2∈G
结合律 g1⋅(g2⋅g3)=(g1⋅g2)⋅g3g_1\cdot(g_2\cdot g_3)=(g_1\cdot g_2)\cdot g_3g1⋅(g2⋅g3)=(g1⋅g2)⋅g3
存在幺元 ∀g∈G,e⋅g=g⋅e=g\forall g\in G,\ \ e\cdot g=g\cdot e=g∀g∈G, e⋅g=g⋅e=g
存在逆元 ∀g∈G,∃g−1∈G,g⋅g−1=g−1⋅g=e\forall g\in G,\ \ \exists g^{-1}\in G,\ \ g\cdot g^{-1}=g^{-1}\cdot g=e∀g∈G, ∃g−1∈G, g⋅g−1=g−1⋅g=e

建模中常用的群

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

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

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

T=(Rt01)T=\begin{pmatrix}R&t\\0&1\end{pmatrix}T=(R0t1)

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

T−1=(RT−RTt01)T^{-1} =\begin{pmatrix}R^T & -R^Tt \\0 & 1\end{pmatrix}T−1=(RT0−RTt1)

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

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

χ=(Rtp1p2...pk0100...00010...00001...0⋮0000...1)\chi=\begin{pmatrix}R & t & p_1 & p_2 & ... & p_k \\0 & 1 & 0 & 0 & ... & 0 \\0 & 0 & 1 & 0 & ... & 0 \\0 & 0 & 0 & 1 & ... & 0 \\\vdots \\0 & 0 & 0 & 0 & ... & 1\end{pmatrix}χ=⎝⎜⎜⎜⎜⎜⎜⎜⎛R000⋮0t1000p10100p20010...............pk0001⎠⎟⎟⎟⎟⎟⎟⎟⎞

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

χ−1=(RT−RTt−RTp1−RTp2...−RTpk0100...00010...00001...0⋮0000...1)\chi^{-1}= \begin{pmatrix} R^T & -R^Tt & -R^Tp_1 & -R^Tp_2 & ... & -R^Tp_k \\ 0 & 1 & 0 & 0 & ... & 0 \\ 0 & 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & 0 & ... & 1 \end{pmatrix}χ−1=⎝⎜⎜⎜⎜⎜⎜⎜⎛RT000⋮0−RTt1000−RTp10100−RTp20010...............−RTpk0001⎠⎟⎟⎟⎟⎟⎟⎟⎞

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

切空间与李代数

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

SO(3)SO(3)SO(3)的切空间

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

SO(3)={R∈R3×3∣RTR=I,det(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}

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

R˙TR+RTR˙=0\dot{R}^TR+R^T\dot{R}=0R˙TR+RTR˙=0

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

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

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

R˙=(0−zyz0−x−yx0)\dot{R}= \begin{pmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{pmatrix}R˙=⎝⎛0z−y−z0xy−x0⎠⎞

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

[xyz]×=(0−zyz0−x−yx0)\begin{bmatrix} x \\ y \\ z \end{bmatrix}_\times= \begin{pmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{pmatrix}⎣⎡xyz⎦⎤×=⎝⎛0z−y−z0xy−x0⎠⎞

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

SE(3)SE(3)SE(3)与SEk+1(3)SE_{k+1}(3)SEk+1(3)的切空间

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

T˙=([ξR]×ξt00)\dot{T}= \begin{pmatrix} [\xi_R]_\times & \xi_t \\ 0 & 0 \end{pmatrix}T˙=([ξR]×0ξt0)

其中ξR,ξt∈R3\xi_R,\xi_t \in \mathbb{R}^3ξR,ξt∈R3。

而SEk+1(3)SE_{k+1}(3)SEk+1(3)的切空间自然就是

χ˙=([ξR]×ξtξp1...ξpk000...0⋮000...0)\dot{\chi}= \begin{pmatrix} [\xi_R]_\times & \xi_t &\xi_{p_1} & ... & \xi_{p_k} \\ 0 & 0 & 0 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 0 \end{pmatrix}χ˙=⎝⎜⎜⎜⎛[ξR]×0⋮0ξt00ξp100.........ξpk00⎠⎟⎟⎟⎞

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

Λ(ξRξtξp1⋮ξpk)=([ξR]×ξtξp1...ξpk000...0⋮000...0)\Lambda \begin{pmatrix} \xi_R\\ \xi_t\\ \xi_{p_1}\\ \vdots\\ \xi_{p_k} \end{pmatrix}= \begin{pmatrix} [\xi_R]_\times & \xi_t &\xi_{p_1} & ... & \xi_{p_k} \\ 0 & 0 & 0 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 0 \end{pmatrix}Λ⎝⎜⎜⎜⎜⎜⎛ξRξtξp1⋮ξpk⎠⎟⎟⎟⎟⎟⎞=⎝⎜⎜⎜⎛[ξR]×0⋮0ξt00ξp100.........ξpk00⎠⎟⎟⎟⎞

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

[X,Y]+[Y,X]=0[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0\begin{aligned} \\ [X,Y]+[Y,X]&=0 \\ [X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]&=0 \end{aligned}[X,Y]+[Y,X][X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0=0

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

指数映射

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

ex=1+x+12!x2+...+1n!xn+...e^x=1+x+\frac{1}{2!}x^2+...+\frac{1}{n!}x^n+...ex=1+x+2!1x2+...+n!1xn+...

将其中的xxx替换为某个矩阵AAA，可得到

eA=I+A+12!A2+...+1n!An+...e^A=I+A+\frac{1}{2!}A^2+...+\frac{1}{n!}A^n+...eA=I+A+2!1A2+...+n!1An+...

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

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

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

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

Exp(ξR)=exp⁡([ξR]×)\text{Exp}(\bf{\xi_R})=\exp([\bf{\xi_R}]_\times)Exp(ξR)=exp([ξR]×)

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

Exp(ξR)=I+θ[u]×+12!θ2[u]×2+...+1n!θn[u]×n+...\text{Exp}(\bf{\xi_R})=I+\theta[\bf{u}]_\times+\frac{1}{2!}\theta^2[\bf{u}]_\times^2+...+\frac{1}{n!}\theta^n[\bf{u}]_\times^n+...Exp(ξR)=I+θ[u]×+2!1θ2[u]×2+...+n!1θn[u]×n+...

注意到

[u]×2=uuT−I[u]×3=−[u]×\begin{aligned} \\ [\bf{u}]_\times^2&=\bf{u}\bf{u}^T-I \\ [\bf{u}]_\times^3&=-[\bf{u}]_\times \end{aligned}[u]×2[u]×3=uuT−I=−[u]×

有

[u]×4=−[u]×2[u]×5=[u]×[u]×6=[u]×2[u]×7=−[u]×...\begin{aligned} \\ [\bf{u}]_\times^4&=-[\bf{u}]_\times^2 \\ [\bf{u}]_\times^5&=[\bf{u}]_\times \\ [\bf{u}]_\times^6&=[\bf{u}]_\times^2 \\ [\bf{u}]_\times^7&=-[\bf{u}]_\times \\ ... \end{aligned}[u]×4[u]×5[u]×6[u]×7...=−[u]×2=[u]×=[u]×2=−[u]×

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

Exp(ξR)=I+(θ−13!θ3+15!θ5−...)[u]×+(12!θ2−14!θ4+...)[u]×2\text{Exp}(\bf{\xi_R})=I+(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-...)[\bf{u}]_\times+(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)[\bf{u}]_\times^2Exp(ξR)=I+(θ−3!1θ3+5!1θ5−...)[u]×+(2!1θ2−4!1θ4+...)[u]×2

又注意到

sin⁡θ=θ−13!θ3+15!θ5−...cos⁡θ=1−12!θ2+14!θ4−...\begin{aligned} \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{aligned}sinθcosθ=θ−3!1θ3+5!1θ5−...=1−2!1θ2+4!1θ4−...

最终可得

Exp(ξR)=I+sin⁡θ[u]×+(1−cos⁡θ)[u]×2\text{Exp}(\bf{\xi_R})=I+\sin\theta[\bf{u}]_\times+(1-\cos\theta)[\bf{u}]_\times^2Exp(ξR)=I+sinθ[u]×+(1−cosθ)[u]×2

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

se(3)到SE(3)se(3)到SE(3)se(3)到SE(3)

设ξ=(ξRT,ξtT)T\xi=(\xi_R^T, \xi_t^T)^Tξ=(ξRT,ξtT)T。同样定义

Exp(ξ)=exp⁡(Λ(ξ))\text{Exp}(\xi)=\exp(\Lambda(\xi))Exp(ξ)=exp(Λ(ξ))

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

Exp(ξ)=(∑1n!θn[u]×n(∑1n!θn−1[u]×n−1)ξt01)\text{Exp}(\xi)= \begin{pmatrix} \sum\frac{1}{n!}\theta^n[\bf{u}]_\times^n & (\sum\frac{1}{n!}\theta^{n-1}[\bf{u}]_\times^{n-1})\xi_t \\ 0 & 1 \end{pmatrix}Exp(ξ)=(∑n!1θn[u]×n0(∑n!1θn−1[u]×n−1)ξt1)

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

Exp(ξ)=(RVξt01)\text{Exp}(\xi)= \begin{pmatrix} R & V\xi_t \\ 0 & 1 \end{pmatrix}Exp(ξ)=(R0Vξt1)

其中

R=I+sin⁡θ[u]×+(1−cos⁡θ)[u]×2V=I+1−cos⁡θθ[u]×+θ−sin⁡θθ[u]×2\begin{aligned} R&=I+\sin\theta[\bf{u}]_\times+(1-\cos\theta)[\bf{u}]_\times^2 \\ V&=I+\frac{1-\cos\theta}{\theta}[\bf{u}]_\times+\frac{\theta-\sin\theta}{\theta}[\bf{u}]_\times^2 \end{aligned}RV=I+sinθ[u]×+(1−cosθ)[u]×2=I+θ1−cosθ[u]×+θθ−sinθ[u]×2

sek+1(3)到SEk+1(3)se_{k+1}(3)到SE_{k+1}(3)sek+1(3)到SEk+1(3)

简单计算即可发现，sek+1(3)se_{k+1}(3)sek+1(3)的指数映射就是se(3)se(3)se(3)的简单扩展，这里直接给出结果。设ξ=(ξRT,ξtT,ξp1T,...,ξpkT)T\xi=(\xi_R^T, \xi_t^T, \xi_{p_1}^T, ..., \xi_{p_k}^T)^Tξ=(ξRT,ξtT,ξp1T,...,ξpkT)T，则

Exp(ξ)=(RVξtVξp1...Vξpk010...0001...0⋮000...1)\text{Exp}(\xi)= \begin{pmatrix} R & V\xi_t &V\xi_{p_1} & ... & V\xi_{p_k} \\ 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & ... & 1 \end{pmatrix}Exp(ξ)=⎝⎜⎜⎜⎜⎜⎛R00⋮0Vξt100Vξp1010............Vξpk001⎠⎟⎟⎟⎟⎟⎞

其中

指数映射的其他性质

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

eAeB=eA+Be^Ae^B=e^{A+B}eAeB=eA+B

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

det(eA)=etr(A)det(e^A)=e^{tr(A)}det(eA)=etr(A)

群伴随

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

SO(3)SO(3)SO(3)的伴随

设R∈SO(3),ξ∈so(3)R\in SO(3), \xi \in so(3)R∈SO(3),ξ∈so(3)，则在RRR的伴随记为AdRAd_RAdR，定义为

R⋅Exp(ξ)=Exp(AdR⋅ξ)⋅RR\cdot \text{Exp}(\xi) = \text{Exp}(Ad_R\cdot \xi)\cdot RR⋅Exp(ξ)=Exp(AdR⋅ξ)⋅R

下面给出AdRAd_RAdR的具体形式。由上式可得

Exp(AdR⋅ξ)=R⋅Exp(ξ)⋅R−1=R(∑n∞1n![ξ]×n)R−1=∑n∞1n!(R[ξ]×R−1)n=exp⁡(R[ξ]×R−1)=exp⁡([Rξ]×)=Exp(Rξ)\begin{aligned} \text{Exp}(Ad_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{aligned}Exp(AdR⋅ξ)=R⋅Exp(ξ)⋅R−1=R(n∑∞n!1[ξ]×n)R−1=n∑∞n!1(R[ξ]×R−1)n=exp(R[ξ]×R−1)=exp([Rξ]×)=Exp(Rξ)

所以有

AdR=RAd_R=RAdR=R

SE(3)与SEk+1(3)SE(3)与SE_{k+1}(3)SE(3)与SEk+1(3)的伴随

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

T=(Rt01)T= \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix}T=(R0t1)

又设ξ=(ξRT,ξtT)T∈se(3)\xi=(\xi_R^T, \xi_t^T)^T \in se(3)ξ=(ξRT,ξtT)T∈se(3)，有

Exp(AdT⋅ξ)=exp⁡(TΛ(ξ)T−1)=exp⁡(R[ξR]×R−1−R[ξR]×R−1t+Rξt00)=exp⁡([RξR]×−[RξR]×t+Rξt00)=exp⁡([RξR]×[t]×RξR+Rξt00)=exp⁡(Λ[(R0[t]×RR)(ξRξt)])=Exp((R0[t]×RR)(ξRξt))\begin{aligned} \text{Exp}(Ad_T\cdot\xi)&=\exp(T\Lambda(\xi) T^{-1}) \\ &=\exp \begin{pmatrix} R[\xi_R]_\times R^{-1} & -R[\xi_R]_\times R^{-1}t+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp \begin{pmatrix} [R\xi_R]_\times & -[R\xi_R]_\times t+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp \begin{pmatrix} [R\xi_R]_\times & [t]_\times R\xi_R+R\xi_t \\ 0 & 0 \end{pmatrix} \\ &=\exp\Bigg( \Lambda \bigg[ \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix} \begin{pmatrix} \xi_R \\ \xi_t \end{pmatrix} \bigg] \Bigg) \\ &=\text{Exp}\Bigg( \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix} \begin{pmatrix} \xi_R \\ \xi_t \end{pmatrix} \Bigg) \end{aligned}Exp(AdT⋅ξ)=exp(TΛ(ξ)T−1)=exp(R[ξR]×R−10−R[ξR]×R−1t+Rξt0)=exp([RξR]×0−[RξR]×t+Rξt0)=exp([RξR]×0[t]×RξR+Rξt0)=exp(Λ[(R[t]×R0R)(ξRξt)])=Exp((R[t]×R0R)(ξRξt))

因此有

AdT=(R0[t]×RR)Ad_T= \begin{pmatrix} R & 0 \\ [t]_\times R & R \end{pmatrix}AdT=(R[t]×R0R)

简单计算可以发现SEk+1(3)SE_{k+1}(3)SEk+1(3)的伴随也是SE(3)SE(3)SE(3)扩展，设χ∈SEk+1(3)\chi \in SE_{k+1}(3)χ∈SEk+1(3)，有

Adχ=(R000...0[t]×RR00...0[p1]×R0R0...0[p2]×R00R...0⋮[pk]×R000...R)Ad_\chi = \begin{pmatrix} 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 \\ \vdots \\ [p_k]_\times R & 0 & 0 & 0 &... & R \end{pmatrix}Adχ=⎝⎜⎜⎜⎜⎜⎜⎜⎛R[t]×R[p1]×R[p2]×R⋮[pk]×R0R00000R00000R0...............0000R⎠⎟⎟⎟⎟⎟⎟⎟⎞

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

SLAM14讲学习笔记（一) 李群李代数基础
第二次翻看<视觉SLAM十四讲>,发现第一次看过的都忘记了,认真分析了一下,我认为是第一次学习没有弄清楚整体的脉络.因此这次做一个小小的总结,希望不要再过几天又忘掉.前面的内容就没有总结的 ...
SLAM基础——李群李代数
李群李代数&雅可比求导 1 李群李代数 1.1 李群李群是一种可微分流形的群,即可以在流形上进行微分操作. 群(G,∘)(\mathcal G,\circ)(G,∘)由一个集合G\mathc ...
SLAM本质剖析番外-李群李代数的微分和导数
0. 简介这几个月,博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享,之前也分享了李群李代数关注核心一文,从现象中解释了李群和李代数表达的含义.但是这还不够,所以这次作者作为SLAM本 ...
SLAM学习--------相机位姿表示-李群李代数
slam 求解相机的位姿求解核心思想:将有约束的李群问题转换成无约束的李代数问题,然后使用高斯牛顿算法或者LM(列文伯格-马夸尔特法)求解. 人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投 ...
多视图几何的数学基础知识的掌握（2）--李群李代数
文章目录前言多视图几何的数学基础知识的掌握(2)--李群李代数三维重建的起源三维欧式空间叉乘与反对称阵刚体运动刚体运动的表示旋转的指数坐标李群$SO(3)$ 李群与李代数李群定义 ...
数学小抄: 李群李代数再回顾 [SLAM十四讲]
前言最近阅读了高翔老师的视觉SLAM十四讲, 也算是了解了当下机器人领域最炙热的机器人领域前沿研究问题大概是怎么样的数学问题. 不得不说视频与书籍真的是深入浅出. 所有第三库无痛安装教程: 知乎链接 ...
SLAM中相机位姿求解（李群李代数）
前言 slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿.举一个重投影例子: ...
李群李代数在计算机视觉中的应用,李群李代数在数据融合算法中的应用分析...
袁治晴摘要:数据融合是提升机器人.无人驾驶.无人机等应用能力的重要手段,一直是前沿技术中研究的一个热点,关于数据融合算法的分析设计,学术界和工程界对此方面进行了长期的研究与讨论,而数据融合算法结合李 ...
李群李代数简明教程
李群李代数简明教程 slam算法中往往需要用到李群李代数的一些知识,最近看到一个李群李代数的简明教程,就翻译下来,有些地方翻译的可能不准,主要是作为一个学习记录. 文章目录李群李代数简明教程 2 ...

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

旋转运动学

速度合成公式

加速度合成公式

群的定义

建模中常用的群

切空间与李代数

SO(3)SO(3)SO(3)的切空间

SE(3)SE(3)SE(3)与SEk+1(3)SE_{k+1}(3)SEk+1(3)的切空间

指数映射

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

se(3)到SE(3)se(3)到SE(3)se(3)到SE(3)

sek+1(3)到SEk+1(3)se_{k+1}(3)到SE_{k+1}(3)sek+1(3)到SEk+1(3)

指数映射的其他性质

群伴随

SO(3)SO(3)SO(3)的伴随

SE(3)与SEk+1(3)SE(3)与SE_{k+1}(3)SE(3)与SEk+1(3)的伴随

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

最新文章

热门文章

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

旋转运动学

速度合成公式

加速度合成公式

群的定义

建模中常用的群

切空间与李代数

SO(3)SO(3)SO(3)的切空间

SE(3)SE(3)SE(3)与SEk+1(3)SE_{k+1}(3)SEk+1​(3)的切空间

指数映射

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

se(3)到SE(3)se(3)到SE(3)se(3)到SE(3)

sek+1(3)到SEk+1(3)se_{k+1}(3)到SE_{k+1}(3)sek+1​(3)到SEk+1​(3)

指数映射的其他性质

群伴随

SO(3)SO(3)SO(3)的伴随

SE(3)与SEk+1(3)SE(3)与SE_{k+1}(3)SE(3)与SEk+1​(3)的伴随

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

最新文章

热门文章

SE(3)SE(3)SE(3)与SEk+1(3)SE_{k+1}(3)SEk+1(3)的切空间

sek+1(3)到SEk+1(3)se_{k+1}(3)到SE_{k+1}(3)sek+1(3)到SEk+1(3)

SE(3)与SEk+1(3)SE(3)与SE_{k+1}(3)SE(3)与SEk+1(3)的伴随