SLAM--李群李代数左扰动模型

一、数学基础

对于两个非零向量：

$$a=(x_1,y_1,z_1{)}^{\mathsf{T}},b=(x_2,y_2,z_2{)}^{\mathsf{T}}$$

则向量的内积：
$$⟨b,a⟩=a^{\mathsf{T}}b=x_1{x}_2+y_1{y}_2+z_1{z}_2$$
其结果为一个标量；

向量的外积：
$$\begin{array}{rl}a\times b& =\left|\begin{array}{ccc}i& j& k\\ x_1& y_1& z_1\\ x_2& y_2& z_2\end{array}\right|\\ & =(y_1{z}_2-y_2{z}_1,z_1{x}_2-z_2{x}_1,x_1{y}_2-x_2{y}_1{)}^{\mathsf{T}}\\ & =\left[\begin{array}{ccc}0& -z_1& y_1\\ z_1& 0& -x_1\\ -y_1& x_1& 0\end{array}\right]b\end{array}$$
向量的外积依旧是个向量，其方向为用右手从a转向b，其模长为：
$$\left|a\right|\left|b\right|\sin ⟨a,b⟩$$
令矩阵：
$$a^{\wedge }=\left[\begin{array}{ccc}0& -z_1& y_1\\ z_1& 0& -x_1\\ -y_1& x_1& 0\end{array}\right]$$
我们称其为a向量的反对称矩阵。

 
 

二、旋转矩阵求导

三维的旋转矩阵R构成了一种特殊的正交群SO(3)，变换矩阵T则构成了SE(3):
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ⁡ ( R ) = 1 } S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ R 3 × 3 , t ∈ R 3 } \begin{aligned} SO(3) &= \{ R \in {\mathbb{R}^{3 \times 3}}|R{R^T} = I,\det (R) = 1\} \\ SE(3) &= \{ T = \left[ {\begin{matrix} R&t \\ {{0^T}}&1 \end{matrix}} \right] \in {\mathbb{R}^{4 \times 4}}|R \in {\mathbb{R}^{3 \times 3}},t \in {\mathbb{R}^3}\} \end{aligned} SO(3)SE(3)​={R∈R3×3∣RRT=I,det(R)=1}={T=[R0T​t1​]∈R4×4∣R∈R3×3,t∈R3}​
需要注意的是，它们对加法不封闭：
$$R_1+R_2\notin SO(3),T_1+T_2\notin SE(3)$$

我们首先让旋转矩阵对时间t求导（注意这里是对时间t求导，和后面的左扰动求导参数不同）

因为：
$$R{R}^T=I$$
若旋转矩阵随时间变化,则：
$$R(t)R(t{)}^T=I$$
我们设初始时刻姿态为P0,t时刻姿态为Pt，则：
$$P_t=R(t)P_0$$
两边同时求导得：
$$\frac{\mathrm{d}{P}_t}{\mathrm{d}t}=\frac{\mathrm{d}R(t)}{\mathrm{d}t}{P}_0$$
由理论力学的知识得：
$$V(t)=\omega \times P(t)=\omega \times R(t)\cdot P_0={\omega }^{\wedge }R(t)\cdot P_0$$
位置对t求导即速度：
$$\frac{\mathrm{d}{P}_t}{\mathrm{d}t}=V(t)$$
所以由上式得
$${\omega }^{\wedge }R(t)\cdot P_0=\frac{\mathrm{d}R(t)}{\mathrm{d}t}{P}_0$$
所以有：
$$\frac{\mathrm{d}R(t)}{\mathrm{d}t}={\omega }^{\wedge }R(t)\cdot$$
又假设t0=0时的初始条件为:
$$R(t_0)=I$$
所以由矩阵论的知识可得：
$$R(t)=exp(t\cdot {\omega }^{\wedge })$$
又因为：
$$\varphi =\omega \cdot t$$
所以最后：
$$R(t)=exp({\varphi }^{\wedge })$$
由因为此式对任意时刻都适用，即：
$$R=exp({\varphi }^{\wedge })$$

三、李代数求导

3.1 对李代数求导的意义：

假设一个点的世界坐标p已知，又已知在一个位姿下T（位姿T未知）观测到该点的坐标为z，求出最优的T使得||z-Tp||最小，这样的T就是最优估计；

或者也可以这么解释，如果我们得到一组点的k时刻观测数据Z ，上一个时刻k-1对同一组点观测的数据为P（k-1时刻的位姿与世界坐标重合），此时我们估计它从k-1到k的变化矩阵T，这时候存在着一个误差：
$$e=z-Tp$$
这时候我们需要让这个误差最小，才能让我们估计的T成为最优估计;
所以可以转化为，对于目标函数J(T),使得J(T)最小：
$$\underset{T}{\min }J(T)=\sum _{i=1}^N{\Vert z_i-T{p}_i\Vert }_2^2$$
我们需要对Tp求导，通过导数来判断极小值点，求出最优解；
但由于李群的不可加性，所以只能通过李群对应的李代数来进行求导；例如：
$$\frac{\partial (Tp)}{\partial T}=\underset{\Delta T\to 0}{\lim }\frac{(T+\Delta T)p-Tp}{\Delta T}$$
这个式子是不正确的，因为T+△T不再是SE(3)群，所以要考虑利用其李代数求导；
考虑旋转，平移六个自由度，六个参数：
$$T=T(\varphi ,t),\varphi \in {\mathbb{R}}^3,t\in {\mathbb{R}}^3$$
这个问题就转变为了对下面的式子：
$$\underset{\varphi ,t}{\min }J(\varphi ,t)=\sum _{i=1}^N{\Vert z_i-T(\varphi ,t)p_i\Vert }_2^2$$
由定义的李代数：
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 ∣ ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] } se(3) = \{ \xi = \left[ {\begin{matrix} \rho \\ \phi \end{matrix}} \right] \in {\mathbb{R}^6}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\xi ^ \wedge } = \left[ {\begin{matrix} {{\phi ^ \wedge }}&\rho \\ {{0^T}}&0 \end{matrix}} \right]\} se(3)={ξ=[ρϕ​]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]}
最后变为：
$$\underset{\varphi ,\rho }{\min }J(\varphi ,\rho )=\sum _{i=1}^N{\Vert z_i-\exp ({\varphi }^{\wedge })p_i\Vert }_2^2$$

3.2 在李群SO(3)上的扰动模型：

对旋转矩阵R进行扰动△R，对应在李代数上为δψ，利用李代数求扰动模型：
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =-(Rp{)}^{\wedge }\end{array}$$
不再叙述推导的过程；

3.3 在李群SE(3)上的扰动模型：

首先说明对应的李代数se(3):
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 ∣ ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] } se(3) = \{ \xi = \left[ {\begin{matrix} \rho \\ \phi \end{matrix}} \right] \in {\mathbb{R}^6}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\xi ^ \wedge } = \left[ {\begin{matrix} {{\phi ^ \wedge }}&\rho \\ {{0^T}}&0 \end{matrix}} \right]\} se(3)={ξ=[ρϕ​]∈R6∣ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]}
其中满足：
$$exp({\xi }^{\wedge })=T=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]$$
所以：
$$\begin{array}{rl}\frac{\partial (Tp)}{\partial \delta \xi }& =\frac{\partial (\exp ({\varphi }^{\wedge }))}{\partial \delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\\ & =\left[\begin{array}{cc}{I}_{3\times 3}& -{(Rp+t)}^{\wedge }\\ 0_{1\times 3}^T& 0_{1\times 3}^T\end{array}\right]\end{array}$$

总结

李群李代数的左扰动模型是非常有用的，尤其是在估计位姿的时候。
对于左扰动，我们定义的变换矩阵为：
$$T_{k,k-1}$$
(这个矩阵的物理意义为：上一个观测位姿下观测点的坐标左乘变换矩阵T可以得到下一个观测位姿下的观测点坐标)
$$P_k=T_{k,k-1}\cdot P_{k-1}$$

左扰动定义为：
$$\delta T=\delta T_{k,k-1}$$
所以有：

$$T_{k,k-1}=\delta T_{k,k-1}\cdot T_{k-1}$$
同理对于右扰动模型：

$$T_{k-1}\cdot \delta T_{k-1,k}=T_{k-1,k}$$