李群李代数:SO(3)和SE(3)

群论

定义

群(Group)是一种集合加上一个二元运算

G G G是一个非空集合 ⋅ \cdot 是一个二元运算,若满足如下条件:

  • 封闭性: ∀ a , b ∈ G , a ⋅ b ∈ G \forall a,b\in G, a\cdot b \in G a,bG,abG
  • 结合律: ∀ a , b , c ∈ G , ( a ⋅ b ) ⋅ c = a ⋅ ( b ⋅ c ) \forall a,b,c\in G, (a\cdot b)\cdot c=a\cdot(b\cdot c) a,b,cG,(ab)c=a(bc)
  • 单位元(幺元): ∃ e ∈ G , s . t . ∀ a ∈ G a ⋅ e = e ⋅ a = a \exists e\in G,\enspace s.t.\enspace \forall a\in G\enspace a\cdot e=e\cdot a=a eG,s.t.aGae=ea=a
  • 逆元: ∀ a ∈ G , ∃ a − 1 ∈ G , s . t . a ⋅ a − 1 = a − 1 ⋅ a = e \forall a\in G,\exist a^{-1}\in G,\enspace s.t.\enspace a\cdot a^{-1}=a^{-1}\cdot a=e aG,a1G,s.t.aa1=a1a=e

则称 G G G ⋅ \cdot 构成一个群,记作 ( G , ⋅ ) (G,\cdot) (G,)

  • G G G上二元运算 ⋅ \cdot 为“乘法”
  • a ⋅ b a\cdot b ab a a a b b b的积,简写 a b ab ab
  • 若群 G G G中元素有限,则称其为有限群,反之则称其为无限群
  • 有限群的元素个数称为有限群的阶

子群

对于群 ( G , ⋅ ) (G,\cdot) (G,)存在 H ⊂ G H\subset G HG H ≠ ∅ H\neq \varnothing H=,若 ( H , ⋅ ) (H,\cdot) (H,)也是一个群,则称 H H H G G G的子群。子群的充要条件如下:
H H = H 且 H − 1 = H HH=H且H^{-1}=H HH=HH1=H

李群

李群是一种具有连续(光滑)性质的群,它既是群也是流形。

刚体能够连续地在空间中运动,故而特殊正交群 S O ( n ) SO(n) SO(n)和特殊欧式群 S E ( n ) SE(n) SE(n)为李群。

李代数

定义

每个李群都有与之对应的李代数,李代数描述了李群单位元附近的正切空间性质

李代数由一个集合 V V V,一个数域 F F F以及一个二元运算 [ , ] [,] [,]组成,记为 ( V , F , [ , ] ) (V,F,[,]) (V,F,[,])。其中二元运算 [ , ] [,] [,]被称为李括号,表示了两元素间的差异。

李代数满足如下条件:

  • 封闭性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall X,Y\in V,\enspace [X,Y]\in V X,YV,[X,Y]V

  • 双线性: ∀ X , Y , Z ∈ V , a , b ∈ F \forall X,Y,Z\in V,\enspace a,b\in F X,Y,ZV,a,bF有:
    [ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z]\\ [Z,aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z][Z,aX+bY]=a[Z,X]+b[Z,Y]

  • 自反性: ∀ X ∈ V , [ X , X ] = 0 \forall X\in V,[X,X]=0 XV,[X,X]=0

  • 雅可比等价: ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Y , [ Z , X ] ] + [ Z , [ X , Y ] ] = 0 \forall X,Y,Z\in V,[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0 X,Y,ZV,[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]=0

  • 反对称性: ∀ X , Y ∈ V , [ X , Y ] = − [ Y , X ] \forall X,Y\in V, [X,Y]=-[Y,X] X,YV,[X,Y]=[Y,X]

推导(SO(3)李代数)

对于任意旋转矩阵 R R R,满足 R R T = I RR^T=I RRT=I。刚体在三维空间中的运动是连续的,随时间而变换,也即:
R ( t ) R ( t ) T = I R(t)R(t)^T=I R(t)R(t)T=I
两侧对时间 t t t进行求导:
R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 R ˙ ( t ) R ( t ) T = − R ( t ) R ˙ ( t ) T R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \begin{aligned} \dot R(t)R(t)^T&+R(t)\dot R(t)^T=0\\ \dot R(t)R(t)^T&=-R(t)\dot R(t)^T\\ \dot R(t)R(t)^T&=-\Bigl(\dot R(t)R(t)^T\Bigr)^T \end{aligned} R˙(t)R(t)TR˙(t)R(t)TR˙(t)R(t)T+R(t)R˙(t)T=0=R(t)R˙(t)T=(R˙(t)R(t)T)T
由此,可知 R ˙ ( t ) R ( t ) T \dot R(t)R(t)^T R˙(t)R(t)T为反对称矩阵,记 R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ \dot R(t)R(t)^T=\phi(t)^\wedge R˙(t)R(t)T=ϕ(t),等式两侧右乘 R ( t ) R(t) R(t)
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) R ˙ ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) \begin{aligned} \dot R(t)&=\phi(t)^\wedge R(t)\\ \dot R(t)&=\begin{bmatrix}0&-\phi_3&\phi_2\\\phi_3&0&-\phi_1\\-\phi_2&\phi_1&0\end{bmatrix}R(t) \end{aligned} R˙(t)R˙(t)=ϕ(t)R(t)=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10R(t)
由上式可知,每对旋转矩阵求一次导数,只需对其左乘一个 ϕ ( t ) ∧ \phi(t)^\wedge ϕ(t)

S O ( 3 ) SO(3) SO(3)的单位元为: t 0 = 0 , R ( 0 ) = I t_0=0,R(0)=I t0=0,R(0)=I,将 R ( t ) R(t) R(t)在单位元附近进行一阶泰勒展开:
R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ R ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ t \begin{aligned} R(t)&\approx R(t_0)+\dot R(t_0)(t-t_0)\\ &=I+\phi(t_0)^\wedge R(t_0)(t-t_0)\\ &=I+\phi(t_0)^\wedge t \end{aligned} R(t)R(t0)+R˙(t0)(tt0)=I+ϕ(t0)R(t0)(tt0)=I+ϕ(t0)t
ϕ \phi ϕ反应了旋转矩阵 R R R的导数性质,称其在 S O ( 3 ) SO(3) SO(3)原点附近的正切空间上。

t 0 t_0 t0附近 ϕ \phi ϕ为常数不变: ϕ ( t 0 ) = ϕ 0 \phi(t_0)=\phi_0 ϕ(t0)=ϕ0,则有:
R ˙ ( t ) = ϕ 0 ∧ R ( t ) \dot R(t)=\phi_0^\wedge R(t) R˙(t)=ϕ0R(t)
在初始条件 R ( 0 ) = I R(0)=I R(0)=I下解微分方程,得到:
R ( t ) = e x p ( ϕ 0 ∧ t ) R(t)=exp(\phi_0^\wedge t) R(t)=exp(ϕ0t)
上式称为李代数 s o ( 3 ) so(3) so(3)的指数映射关系。

李代数so(3)

向量空间: s o ( 3 ) = { Φ = ϕ ∧ ∈ ℜ 3 × 3 ∣ ϕ ∈ ℜ 3 } so(3)=\Bigl\{\Phi=\phi^\wedge\in\Re^{3\times3}\enspace\Big\vert \phi\in\Re^3\Bigr\} so(3)={Φ=ϕ3×3ϕ3}

数域: ℜ \Re

李括号: [ ϕ a , ϕ b ] = Φ a Φ b − Φ b Φ a [\phi_a,\phi_b]=\Phi_a\Phi_b-\Phi_b\Phi_a [ϕa,ϕb]=ΦaΦbΦbΦa
[ ϕ a , ϕ b ] = Φ a Φ b − Φ b Φ a = ϕ a ∧ ϕ b ∧ − ϕ b ∧ ϕ a ∧ = ( ϕ a ∧ ϕ b ) ∧ ∈ s o ( 3 ) \begin{aligned} {[}\phi_a,\phi_b]&=\Phi_a\Phi_b-\Phi_b\Phi_a\\ &=\phi_a^\land\phi_b^\land-\phi_b^\land\phi_a^\land\\ &=\bigl(\phi_a^\land\phi_b\bigr)^\land\in so(3) \end{aligned} [ϕa,ϕb]=ΦaΦbΦbΦa=ϕaϕbϕbϕa=(ϕaϕb)so(3)
其中 Φ = ϕ ∧ \Phi=\phi^\land Φ=ϕ,为反对称矩阵:
Φ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ ℜ 3 × 3 \Phi=\begin{bmatrix}0&-\phi_3&\phi_2\\\phi_3&0&-\phi_1\\-\phi_2&\phi_1&0\end{bmatrix}\in \Re^{3\times3} Φ=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ103×3

视集合 ℜ 3 \Re^3 3和叉乘运算作为 s e ( 3 ) se(3) se(3)

李代数se(3)

向量空间: s e ( 3 ) = { Ξ = ξ ∧ ∈ ℜ 4 × 4 ∣ ξ ∈ ℜ 6 } se(3)=\Biggl\{\Xi=\xi^\land\in\Re^{4\times4} \Bigg\vert\xi\in\Re^{6}\Biggr\} se(3)={Ξ=ξ4×4ξ6}

数域: ℜ \Re

李括号 [ Ξ 1 , Ξ 2 ] = Ξ 1 Ξ 2 − Ξ 2 Ξ 1 [\Xi_1,\Xi_2]=\Xi_1\Xi_2-\Xi_2\Xi_1 [Ξ1,Ξ2]=Ξ1Ξ2Ξ2Ξ1
[ Ξ a , Ξ b ] = Ξ a Ξ b − Ξ a Ξ b = ξ a ∧ ξ b ∧ − ξ b ∧ ξ a ∧ = ( ξ 1 ⋆ ξ 2 ) ∧ ∈ s e ( 3 ) \begin{aligned} {[}\Xi_a,\Xi_b]&=\Xi_a\Xi_b-\Xi_a\Xi_b\\ &=\xi_a^\land\xi_b^\land-\xi_b^\land\xi_a^\land\\ &=\bigl(\xi_1^{\star}\xi_2\bigr)^\land\in se(3) \end{aligned} [Ξa,Ξb]=ΞaΞbΞaΞb=ξaξbξbξa=(ξ1ξ2)se(3)
其中,参量 ξ \xi ξ用于表示平移加旋转:
ξ = [ ρ ϕ ] , ρ ∈ ℜ 3 , ϕ ∈ s o ( 3 ) \xi=\begin{bmatrix}\rho\\\phi\end{bmatrix},\rho\in\Re^3,\phi\in so(3) ξ=[ρϕ],ρ3,ϕso(3)
ξ ∧ \xi^\land ξ非反对称矩阵,但仍保留记法:
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ ℜ 4 × 4 \xi^\land=\begin{bmatrix}\phi^\land&\rho\\0^T&0\end{bmatrix}\in\Re^{4\times4} ξ=[ϕ0Tρ0]4×4

星计算 ξ ⋆ \xi^\star ξ定义为:
ξ ⋆ = [ ρ ϕ ] ⋆ = [ ϕ ∧ ρ ∧ 0 ϕ ∧ ] ∈ ℜ 6 × 6 \xi^\star=\begin{bmatrix}\rho\\\phi\end{bmatrix}^\star=\begin{bmatrix}\phi^\land&\rho^\land\\0&\phi^\land\end{bmatrix}\in\Re^{6\times6} ξ=[ρϕ]=[ϕ0ρϕ]6×6
视集合 ℜ 6 \Re^6 6和星计算为 s e ( 3 ) se(3) se(3)

指数映射与对数映射

指数映射反应了从李代数到李群的转换,任意矩阵的指数映射可写成一个泰勒展开,其结果仍是一个矩阵:
e x p ( G ) = ∑ n = 0 ∞ 1 n ! G n exp(G)=\sum_{n=0}^\infty\frac{1}{n!}G^n exp(G)=n=0n!1Gn
对数映射则反应了从李群到李代数的转换:
ln ⁡ ( G ) = ∑ n = 0 ∞ ( − 1 ) n n + 1 ( G − I ) n + 1 \ln(G)=\sum_{n=0}^\infty\frac{(-1)^{n}}{n+1}(G-I)^{n+1} ln(G)=n=0n+1(1)n(GI)n+1

李群 S O ( 3 ) SO(3) SO(3)李代数 s o ( 3 ) so(3) so(3)

ϕ \phi ϕ是一个向量,定义其模长为 θ \theta θ方向为 a a a ϕ = θ a \phi=\theta a ϕ=θa。其中, a a a为一个模长为1的单位向量: ∥ a ∥ = 1 \begin{Vmatrix}a\end{Vmatrix}=1 a=1

对于反对称矩阵 a ∧ a^\land a,易得:
式 1 : a ∧ a ∧ = a a T − I 式 2 : a ∧ a ∧ a ∧ = a ∧ ( a a T − I ) = − a ∧ \begin{aligned} 式1:\qquad&a^\land a^\land=aa^T-I\\ 式2:\qquad&a^\land a^\land a^\land=a^\land (aa^T-I)=-a^\land \end{aligned} 1:2:aa=aaTIaaa=a(aaTI)=a

指数映射

对指数映射进行Taylor展开并将上述等式代入:
e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = ∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n 1 : = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 ( a ∧ ) 4 + ⋯ 2 : = ( a a T − a ∧ a ∧ ) + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 a ∧ a ∧ + ⋯ 3 : = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋯   ) a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋯   ) a ∧ a ∧ 4 : = ( I + a ∧ a ∧ ) + sin ⁡ θ a ∧ − cos ⁡ θ a ∧ a ∧ 5 : = ( 1 − cos ⁡ θ ) a ∧ a ∧ + I + sin ⁡ θ a ∧ 6 : = ( 1 − cos ⁡ θ ) ( a a T − I ) + I + sin ⁡ θ a ∧ 7 : = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ \begin{aligned} exp(\phi^\land)&=exp(\theta a^\land)=\sum^\infty_{n=0}\frac{1}{n!}(\theta a^\land)^n\\ 1:\qquad &=I+\theta a^\land + \frac{1}{2!}\theta^2a^\land a^\land +\frac{1}{3!}\theta^3 a^\land a^\land a^\land+\frac{1}{4!}\theta^4(a^\land)^4+\cdots\\ 2:\qquad &=(aa^T-a^\land a^\land)+\theta a^\land+\frac{1}{2!}\theta^2a^\land a^\land -\frac{1}{3!}\theta^3 a^\land-\frac{1}{4!}\theta^4a^\land a^\land+\cdots\\ 3:\qquad &=aa^T+\Bigl(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\Bigr)a^\land-\Bigl(1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots\Bigr)a^\land a^\land\\ 4:\qquad &=(I+a^\land a^\land) +\sin\theta a^\land -\cos\theta a^\land a^\land\\ 5:\qquad &=(1-\cos\theta)a^\land a^\land+I+\sin\theta a^\land\\ 6:\qquad &=(1-\cos\theta)(aa^T-I)+I+\sin\theta a^\land\\ 7:\qquad &=\cos\theta I+(1-\cos\theta)aa^T+\sin\theta a^\land \end{aligned} exp(ϕ)1:2:3:4:5:6:7:=exp(θa)=n=0n!1(θa)n=I+θa+2!1θ2aa+3!1θ3aaa+4!1θ4(a)4+=(aaTaa)+θa+2!1θ2aa3!1θ3a4!1θ4aa+=aaT+(θ3!1θ3+5!1θ5)a(12!1θ2+4!1θ4)aa=(I+aa)+sinθacosθaa=(1cosθ)aa+I+sinθa=(1cosθ)(aaTI)+I+sinθa=cosθI+(1cosθ)aaT+sinθa
上述计算过程的处理思路如下:

  • 第一步:泰勒展开

  • 第二步:将式1代入替换第一步中的 I I I;将式2代入替换 a ∧ a ∧ a ∧ a^\land a^\land a^\land aaa

  • 第三步:对第二步进行整理

  • 第四步:将式1代入替换第三步中的 a a T aa^T aaT;将三角函数的泰勒展开形式写回三角函数形式:
    sin ⁡ θ = θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋯ cos ⁡ θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋯ \begin{aligned} \sin\theta &=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\\ \cos\theta &=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots \end{aligned} sinθcosθ=θ3!1θ3+5!1θ5=12!1θ2+4!1θ4

  • 第五步:对第四步进行整理

  • 第六步:将式1代入替换第五步中 a ∧ a ∧ a^\land a^\land aa

  • 第七步:对第六步进行整理

最终得到李代数 s o ( 3 ) so(3) so(3)到李群 S O ( 3 ) SO(3) SO(3)的指数映射:
e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ exp(\phi^\land)=exp(\theta a^\land)=\cos\theta I+(1-\cos\theta)aa^T+\sin\theta a^\land exp(ϕ)=exp(θa)=cosθI+(1cosθ)aaT+sinθa
该式被称为罗德里格斯公式,李代数 s o ( 3 ) so(3) so(3)的物理意义就是旋转向量。

对数映射

S O ( 3 ) SO(3) SO(3) s o ( 3 ) so(3) so(3)的对数映射如下:
ϕ = ln ⁡ ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) n + 1 ) ∨ \phi=\ln(R)^\lor=\Biggl(\sum_{n=0}^\infty\frac{(-1)^{n}}{n+1}(R-I)^{n+1}\Biggr)^\lor ϕ=ln(R)=(n=0n+1(1)n(RI)n+1)
对数映射实现由旋转矩阵到旋转向量的转换,可使用刚体变换中计算方式,对指数映射两侧求迹从而简化计算:
θ = arccos ⁡ t r ( R ) − 1 2 R n = n \theta=\arccos\frac{tr(R)-1}{2}\qquad\qquad\qquad\qquad Rn=n θ=arccos2tr(R)1Rn=n

注意点

指数映射是一个满射,不是一个单射。也即每个 S O ( 3 ) SO(3) SO(3)内的元素,都可以找到一个 s o ( 3 ) so(3) so(3)元素与之一一对应;但可能存在多个 s o ( 3 ) so(3) so(3)中的元素,对应到一个 S O ( 3 ) SO(3) SO(3)中元素。

若将旋转角度固定至 ± π \pm\pi ±π中,则可认为李群、李代数中元素一一对应。

李群 S E ( 3 ) SE(3) SE(3)李代数 s e ( 3 ) se(3) se(3)

指数映射

由李代数 s e ( 3 ) se(3) se(3)到李群 S E ( 3 ) SE(3) SE(3)的转换如下:
e x p ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] ≜ [ R J ρ 0 T 1 ] = T \begin{aligned} exp(\xi^\land)&=\begin{bmatrix}\sum^\infty_{n=0}\frac{1}{n!}(\phi^\land)^n&\sum_{n=0}^\infty\frac{1}{(n+1)!}(\phi^\land)^n\rho\\0^T&1\end{bmatrix}\\ &\triangleq \begin{bmatrix}R&J\rho\\0^T&1\end{bmatrix}\\ &=T \end{aligned} exp(ξ)=[n=0n!1(ϕ)n0Tn=0(n+1)!1(ϕ)nρ1][R0TJρ1]=T
式中 J J J称为雅可比矩阵:
J = ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n = ∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n 1 : = I + 1 2 ! θ a ∧ + 1 3 ! θ 2 ( a ∧ ) 2 + 1 4 ! θ 3 ( a ∧ ) 3 + 1 5 ! θ 4 ( a ∧ ) 4 + ⋯ 2 : = I + 1 θ ( 1 2 ! θ 2 a ∧ + 1 3 ! θ 3 ( a ∧ ) 2 + 1 4 ! θ 4 ( a ∧ ) 3 + 1 5 ! θ 5 ( a ∧ ) 4 + ⋯   ) 3 : = I + 1 θ ( 1 2 ! θ 2 a ∧ + 1 3 ! θ 3 ( a ∧ ) 2 − 1 4 ! θ 4 a ∧ − 1 5 ! θ 5 ( a ∧ ) 2 + ⋯   ) 4 : = I + 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯   ) a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯   ) ( a ∧ ) 2 5 : = I + 1 θ ( 1 − cos ⁡ θ ) a ∧ + 1 θ ( θ − sin ⁡ θ ) ( a ∧ ) 2 6 : = I + 1 θ ( 1 − cos ⁡ θ ) a ∧ + 1 θ ( θ − sin ⁡ θ ) ( a a T − I ) 7 : = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ \begin{aligned} J=&\sum_{n=0}^\infty\frac{1}{(n+1)!}(\phi^\land)^n=\sum_{n=0}^\infty\frac{1}{(n+1)!}(\theta a^\land)^n\\ 1:\qquad =&I+\frac{1}{2!}\theta a^\land+\frac{1}{3!}\theta^2 (a^\land)^2+\frac{1}{4!}\theta^3 (a^\land)^3+\frac{1}{5!}\theta^4 (a^\land)^4+\cdots\\ 2:\qquad =&I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2 a^\land+\frac{1}{3!}\theta^3 (a^\land)^2+\frac{1}{4!}\theta^4 (a^\land)^3+\frac{1}{5!}\theta^5 (a^\land)^4+\cdots\Bigr) \\ 3:\qquad =&I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2 a^\land+\frac{1}{3!}\theta^3 (a^\land)^2-\frac{1}{4!}\theta^4 a^\land-\frac{1}{5!}\theta^5 (a^\land)^2+\cdots\Bigr)\\ 4:\qquad =& I+\frac{1}{\theta}\Bigl(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+\cdots\Bigr)a^\land + \frac{1}{\theta}\Bigl(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+\cdots\Bigr)(a^\land)^2 \\ 5:\qquad =& I+\frac{1}{\theta}(1-\cos\theta)a^\land+\frac{1}{\theta}(\theta-\sin\theta)(a^\land)^2\\ 6:\qquad =& I+\frac{1}{\theta}(1-\cos\theta)a^\land+\frac{1}{\theta}(\theta-\sin\theta)(aa^T-I)\\ 7:\qquad =& \frac{\sin\theta}{\theta}I+\Bigl(1-\frac{\sin\theta}{\theta}\Bigr)aa^T+\frac{1-\cos\theta}{\theta}a^\land\\ \end{aligned} J=1:=2:=3:=4:=5:=6:=7:=n=0(n+1)!1(ϕ)n=n=0(n+1)!1(θa)nI+2!1θa+3!1θ2(a)2+4!1θ3(a)3+5!1θ4(a)4+I+θ1(2!1θ2a+3!1θ3(a)2+4!1θ4(a)3+5!1θ5(a)4+)I+θ1(2!1θ2a+3!1θ3(a)24!1θ4a5!1θ5(a)2+)I+θ1(2!1θ24!1θ4+)a+θ1(3!1θ35!1θ5+)(a)2I+θ1(1cosθ)a+θ1(θsinθ)(a)2I+θ1(1cosθ)a+θ1(θsinθ)(aaTI)θsinθI+(1θsinθ)aaT+θ1cosθa
上述计算过程的处理思路如下:

  • 第一步:泰勒展开

  • 第二步:除单位阵 I I I外其余项提出一个因子 1 θ \frac{1}{\theta} θ1

  • 第三步:将式2代入代替 ( a ∧ ) 3 (a^\land)^3 (a)3

  • 第四步:对第三步进行整理,将有共同项的写在一起

  • 第五步:将三角函数的泰勒展开进行整理后代入上式:
    cos ⁡ θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋯ 1 − cos ⁡ θ = 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯ sin ⁡ θ = θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋯ θ − sin ⁡ θ = 1 3 ! θ 3 − 1 5 ! θ 5 + ⋯ \begin{aligned} \cos\theta &=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots\\ 1-\cos\theta&=\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+\cdots\\ \\ \sin\theta &=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\cdots\\ \theta-\sin\theta&=\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+\cdots \end{aligned} cosθ1cosθsinθθsinθ=12!1θ2+4!1θ4=2!1θ24!1θ4+=θ3!1θ3+5!1θ5=3!1θ35!1θ5+

  • 第六步:将式1代入代替 ( a ∧ ) 2 (a^\land)^2 (a)2

  • 第七步:对第六步进行整理

由此得到雅可比矩阵的表达式:
J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ J=\frac{\sin\theta}{\theta}I+\Bigl(1-\frac{\sin\theta}{\theta}\Bigr)aa^T+\frac{1-\cos\theta}{\theta}a^\land J=θsinθI+(1θsinθ)aaT+θ1cosθa

对数映射

S E ( 3 ) SE(3) SE(3) s e ( 3 ) se(3) se(3)的转换,如下:
ξ = ln ⁡ ( T ) ∨ \xi=\ln(T)^\lor ξ=ln(T)
在进行实际转换时,使用对数映射较为复杂。一般用 T T T左上角旋转矩阵 R R R求解旋转向量;再由右侧平移向量计算平移部分:
t = J ρ t=J\rho t=Jρ

总结

在这里插入图片描述

  • 15
    点赞
  • 39
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
抱歉,我之前的回答中的 `mianif::SO2d` 是一个错误的命名。这个名称应该是 `manif::SO2d`。 `manif::SO2d` 是 `manif` 库中的一个类,用于表示二维空间中的旋转。它是基于李群理论的实现,用于处理刚体运动和姿态估计问题。 `manifSO2d` 类提供了一些方法和操作,可以进行旋转的合、插值、逆运算等。它还可以与其他旋转表示(如旋转矩阵、四元数等)之间进行转换。 以下是一个示例代码,演示了如何使用 `manif::SO2d` 类来表示和操作二维旋转: ```cpp #include <iostream> #include <manif/so2.h> int main() { manif::SO2d rotation(1.0); // 创建一个表示旋转角度为 1.0 的 SO2d 对象 // 输出旋转矩阵表示 std::cout << "Rotation matrix:\n" << rotation.matrix() << std::endl; // 旋转角度加法 manif::SO2d rotation2(0.5); manif::SO2d rotation_sum = rotation + rotation2; std::cout << "Rotation sum:\n" << rotation_sum.matrix() << std::endl; // 旋转角度乘法 double scalar = 2.0; manif::SO2d rotation_scaled = scalar * rotation; std::cout << "Scaled rotation:\n" << rotation_scaled.matrix() << std::endl; return 0; } ``` 上述示例中,我们创建了一个 `manif::SO2d` 对象 `rotation`,表示一个旋转角度为 1.0 的二维旋转。然后我们可以使用 `matrix()` 方法获取其对应的旋转矩阵表示,并进行旋转角度的加法和乘法操作。 请注意,为了使用 `manif::SO2d` 类,您需要在代码中包含正确的头文件,并将 `manif` 库链接到您的项目中。确保正确配置和安装了 `manif` 库以便使用它。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值