SLAM数学篇:李群与李代数

李群与李代数

位姿估计问题

  假设某时刻机器人位姿为 $T$ ，观察到世界坐标系中 $p$ 点，产生观测数据 $z$ ，则有
$$z=Tp+w$$
存在观测噪声 $w$ ，计算观测数据与实际数据的误差，即
$$e=z-Tp$$
假设有 $N$ 个路标点及观测，则寻找一个最优 $T$ ，使得整体误差最小，即
$$\underset{T}{\min }J(T)=\sum _{i=1}^N{||z_i-T{p}_i||}_2^2$$
  这是SLAM核心问题，我们构建了与位姿有关的重投影误差函数，若求最优位姿便应使得误差最小化，自然想到对位姿进行求导。但是因旋转矩阵自身带有约束（正交矩阵且行列式为1），对加法不封闭，将变换矩阵作为优化的变量时，会引入额外的约束。那我们怎么解决这个问题？引入李群与李代数，李代数由向量组成，具有良好的加法运算。通过李群与李代数之间的转换将位姿估计变成无约束优化问题，然后通过高斯牛顿法、列文伯格-马夸尔特法等优化算法简化求解。

李群与李代数

  李群是指具有连续（光滑）性质的群，在SLAM中主要讨论 $SO(n)$ 、$SE(n)$ 两个李群。$SO(n)$ 、$SE(n)$ 是用来描述旋转平移上的变化，随时间变化连续，是矩阵的集合。每个李群都有与之对应的李代数，李代数描述了李群的局部性质，是定义在李群正切空间上的一组向量。

关系推导：
$$\begin{gathered} R{R}^T=I{\Rightarrow }^{微分}{R}^{\prime }(t)R(t{)}^T=-(R^{\prime }(t)R(t{)}^T{)}^T{\Rightarrow }^{反对称矩阵}{R}^{\prime }(t)R(t{)}^T=\varphi (t{)}^{\wedge }\Rightarrow R^{\prime }(t)=\varphi (t{)}^{\wedge }R(t) \\ {\Rightarrow }^{积分}R(t)=e^{{\varphi }_0^{\wedge }t}\Rightarrow R=e^{{\varphi }^{\wedge }} \end{gathered}$$

结果定义：
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , d e t ( R ) = 1 } S E ( 3 ) = { [ R t 0 T 1 ] ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SO(3) = \{R \in \Bbb R^{3\times3}|R^TR = I,det(R)=1\} \\\\ SE(3)=\{ \begin{bmatrix} R & t \\\\ 0^T & 1 \end{bmatrix}| R \in SO(3),t \in \Bbb R^3 \} SO(3)={ R∈R3×3∣RTR=I,det(R)=1}SE(3)={ ⎣ ⎡​R0T​t1​⎦ ⎤​∣R∈SO(3),t∈R3}

s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 } so(3)=\{ \phi \in \Bbb R^3,\Phi=\phi^\wedge \in \Bbb R^{3 \times 3} \} \\\\ se(3)=\{ \xi=\begin{bmatrix} \rho \\\\ \phi \end{bmatrix} \in \Bbb R^6,\rho \in \Bbb R^3,\phi \in so(3),\xi^\wedge=\begin{bmatrix} \phi^\wedge & \rho \\\\ 0^T & 0 \end{bmatrix} \in \Bbb R^4 \} so(3)={ ϕ∈R3,Φ=ϕ∧∈R3×3}se(3)={ ξ=⎣ ⎡​ρϕ​⎦ ⎤​∈R6,ρ∈R3,ϕ∈so(3),ξ∧=⎣ ⎡​ϕ∧0T​ρ0​⎦ ⎤​∈R4}

  此处使用 ”${}^{\wedge }$” 、“ ∨ ^\vee