FAST_LIO原理解读_fastlio

一、整体框架

二、前向传播

1、IMU静态初始化

初始化重力方向、陀螺仪零偏

均值的更新：$A_n=A_{n-1}+\frac{X_n-A_{n-1}}{n}$

方差的更新：$V_n=\frac{n-1}{n^2}(X_n-A_{n-1}{)}^2+\frac{n-1}{n}{V}_{n-1}$

初始化重力方向：$grav=-\frac{ac{c}_{mean}}{norm(ac{c}_{mean})}G$

初始化陀螺仪零偏：$b_g=mea{n}_{gyr}$

2、传播状态量

① 输入量、状态量、噪声量

输入量：$u=[w_m^T,a_m^T{]}^T$

状态量：$x=[{R_I^G}^T,{p_I^G}^T,{v_I^G}^T,{b_w}^T,{b_a}^T,g{]}^T$

噪声量：$w=[n_w^T,n_a^T,n_{bw}^T,n_{ba}^T{]}^T$

② imu测量模型

测量值 = 真值 + 零偏 + 噪声
$$\begin{gathered} a_m=a+b_a+n_a-R_G^{I}g \\ {w}_m=w+b_w+n_w \\ \dot{b_w}=n_{bw} \\ \dot{b_a}=n_{ba} \end{gathered}$$

③ imu运动模型

连续状态下的运动模型：
$$\begin{gathered} \dot{R}=R{w}^{\hat{}} \\ \dot{v}=a \\ \dot{p}=v \end{gathered}$$
离散状态下的运动模型（欧拉积分）：
$$\begin{gathered} R(t+\Delta t)=R(t)Exp(w(t)\Delta t) \\ v(t+\Delta t)=v(t)+a(t)\Delta t \\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}a(t)\Delta t^2 \end{gathered}$$
将imu测量模型带入imu离散运动模型，得到状态量的传播公式：
$$\begin{gathered} R(t+\Delta t)=R(t)Exp[(w_m-b_w-n_w)\Delta t] \\ v(t+\Delta t)=v(t)+[R(t)(a_m-b_a-n_a)+g]\Delta t \\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}a(t)\Delta t^2 \\ \dot{b_w}=n_{bw} \\ \dot{b_a}=n_{ba} \end{gathered}$$
对应论文中的形式为：
$${\mathbf{x}}_{i+1}={\mathbf{x}}_i⊞(\Delta t\mathbf{f}({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i))$$

f ( x i , u i , w i ) = [ ω m i − b ω i − n ω i G v I i G R I i ( a m i − b a i − n a i ) + G g i n b ω i n b a i 0 3 × 1 ] \mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)=

$$\begin{bmatrix}\boldsymbol{\omega}_{m_{i}}-\mathbf{b}_{\boldsymbol{\omega}_{i}}-\mathbf{n}_{\boldsymbol{\omega}_{i}}\\^{G}\mathbf{v}_{I_{i}}\\^G\mathbf{R}_{I_i}\left(\mathbf{a}_{m_i}-\mathbf{b}_{\mathbf{a}_i}-\mathbf{n}_{\mathbf{a}_i}\right)+^G\mathbf{g}_i\\\mathbf{n}_{\mathbf{b}\boldsymbol{\omega}_i}\\\mathbf{n}_{\mathbf{b}\mathbf{a}_i}\\\mathbf{0}_{3\times1}\end{bmatrix}$$ f(xi​,ui​,wi​)= ​ωmi​​−bωi​​−nωi​​GvIi​​GRIi​​(ami​​−bai​​−nai​​)+Ggi​nbωi​​nbai​​03×1​​ ​

3、传播误差量的协方差矩阵

误差值 = 真实值 - 估计值
x ~ i + 1 = x i + 1 ⊟ x ^ i + 1 = ( x i ⊞ Δ t f ( x i , u i , w i ) ) ⊟ ( x ^ i ⊞ Δ t f ( x ^ i , u i , 0 ) ) ≃ F x ~ x ~ i + F w w i .

$$\begin{aligned} \widetilde{\mathbf{x}}_{i+1}& =\mathbf{x}_{i+1}\boxminus\widehat{\mathbf{x}}_{i+1} \\ &=\left(\mathbf{x}_i\boxplus\Delta t\mathbf{f}\left(\mathbf{x}_i,\mathbf{u}_i,\mathbf{w}_i\right)\right)\boxminus\left(\widehat{\mathbf{x}}_i\boxplus\Delta t\mathbf{f}\left(\widehat{\mathbf{x}}_i,\mathbf{u}_i,\mathbf{0}\right)\right) \\ &\simeq\mathbf{F}_{\widetilde{\mathbf{x}}}\widetilde{\mathbf{x}}_i+\mathbf{F}_{\mathbf{w}}\mathbf{w}_i. \end{aligned}$$ x i+1​​=xi+1​⊟x i+1​=(xi​⊞Δtf(xi​,ui​,wi​))⊟(x i​⊞Δtf(x i​,ui​,0))≃Fx ​x i​+Fw​wi​.​
误差量的协方差矩阵：
$${\hat{\mathbf{P}}}_{i+1}={\mathbf{F}}_{\tilde{\mathbf{x}}}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{\tilde{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T;{\hat{\mathbf{P}}}_0={\overline{\mathbf{P}}}_{k-1}$$
其中
$${\mathbf{F}}_{\mathbf{x}}=\left[\begin{array}{cccccc}\mathrm{Exp}\left(-{\hat{\omega }}_i\Delta t\right)& 0& 0& -\mathbf{A}({\hat{\omega }}_i\Delta t{)}^T\Delta t& 0& 0\\ 0& \mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0\\ {-}^G{\hat{\mathbf{R}}}_{I_i}\lfloor {\hat{\mathbf{a}}}_i\rfloor \wedge \Delta t& 0& \mathbf{I}& 0& {-}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& \mathbf{I}\Delta t\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$

F w = [ − A ( ω ^ i Δ t ) T Δ t 0 0 0 0 0 0 0 0 − G R ^ I i Δ t 0 0 0 0 1 Δ t 0 0 0 0 1 Δ t 0 0 0 0 ] \mathbf{F_w}=

$$\begin{bmatrix}-\mathbf{A}\left(\widehat{\boldsymbol{\omega}}_i\Delta t\right)^T\Delta t&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&-^G\widehat{\mathbf{R}}_{I_i}\Delta t&\mathbf{0}&\mathbf{0}\\0&\mathbf{0}&\mathbf{1}\Delta t&\mathbf{0}\\0&\mathbf{0}&\mathbf{0}&\mathbf{1}\Delta t\\0&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix}$$ Fw​= ​−A(ω i​Δt)TΔt00000​00−GR Ii​​Δt000​0001Δt00​00001Δt0​ ​

三、反向传播

• 遍历IMUpose，拿到head帧的旋转R，速度vel，位置pos，拿到tail帧的imu_acc，imu_gry

• 找到时间戳大于head帧的激光点it_pcl，并计算head和it_pcl之间的时间间隔dt

• it_pcl在世界坐标系下的姿态为：$R_i=R\ast EXP(im{u}_{gry}\ast dt)$

• it_pcl在世界坐标系下的位置为：$P_i=pos+vel\ast dt+0.5\ast im{u}_{acc}\ast d{t}^2$

• 运动畸变去除原理：根据补偿后的点在世界坐标系下的位姿 = 补偿前的点在世界坐标系下的位姿
  $$im{u}_{state.rot}\times (R_L^I\times p_{com}+T_L^I)+im{u}_{state.pos}=R_i\times (R_L^I\times p_i+t_L^I)+P_i$$

四、迭代误差状态卡尔曼滤波

1、计算点面残差

• 先将当前帧从雷达坐标系转换到IMU坐标系，再根据前向传播估计的位姿转换到世界坐标系
  $$P_i^G=T_I^G\times T_L^I\times P_i^L$$

• 使用ikdtree寻找距离当前点最近的5个点，进行拟合平面$ax+by+cy+d=0$

  平面方程可进一步写为：

a d x + b d y + c d z = − 1 D 1 x i + D 2 y i + D 3 z i = − 1 [ x 1 y 1 z 1 x 2 y 3 z 3 x 3 y 3 z 3 x 4 y 4 z 4 x 5 y 5 z 5 ] [ D 1 D 2 D 3 ] = [ − 1 − 1 − 1 − 1 − 1 ] A X = b \frac{a}{d}x+\frac{b}{d}y+\frac{c}{d}z=-1 \\ D_1x_i+D_2y_i+D_3z_i=-1 \\

$$\begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_3 & z_3\\ x_3 & y_3 & z_3\\ x_4 & y_4 & z_4\\ x_5 & y_5 & z_5 \end{bmatrix}$$ $$\begin{bmatrix} D_1 \\ D_2 \\ D_3 \end{bmatrix}$$ = $$\begin{bmatrix} -1 \\ -1 \\ -1 \\ -1 \\ -1 \end{bmatrix}$$ \\ AX = b da​x+db​y+dc​z=−1D1​xi​+D2​yi​+D3​zi​=−1 ​x1​x2​x3​x4​x5​​y1​y3​y3​y4​y5​​z1​z3​z3​z4​z5​​ ​ ​D1​D2​D3​​ ​= ​−1−1−1−1−1​ ​AX=b

​ 利用QR分解求得平面参数$[D_1,D_2,D_3]$，计算点到平面的距离$pd2$，当前点到激光雷达坐标系的距离$pd1$，

​ 如果$1-0.9\frac{pd2}{pd1}>0.9$，认为是有效的残差

2、计算观测方程的雅可比矩阵

观测方程：
0 = h j ( x k , L j n f j ) = h j ( x ^ k κ ⊞ x ~ k κ , L j n f j ) ≃ h j ( x ^ k κ , 0 ) + H j κ x ~ k κ + v j = z j κ + H j κ x ~ k κ + v j

$$\begin{aligned} \text{0}& =\mathbf{h}_j\left(\mathbf{x}_k,^{L_j}\mathbf{n}_{f_j}\right)\\ &=\mathbf{h}_j\left(\widehat{\mathbf{x}}_k^\kappa\boxplus\widetilde{\mathbf{x}}_k^\kappa,^{L_j}\mathbf{n}_{f_j}\right) \\ &\simeq\mathbf{h}_j\left(\widehat{\mathbf{x}}_k^\kappa,\mathbf{0}\right)+\mathbf{H}_j^\kappa\widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \\ &=\mathbf{z}_j^\kappa+\mathbf{H}_j^\kappa\widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \end{aligned}$$ 0​=hj​(xk​,Lj​nfj​​)=hj​(x kκ​⊞x kκ​,Lj​nfj​​)≃hj​(x kκ​,0)+Hjκ​x kκ​+vj​=zjκ​+Hjκ​x kκ​+vj​​
其中雅可比矩阵H：
$$H=[-{\vec{\mathbf{n}}}^G{\hat{\mathbf{R}}}_I{(}^I{\mathbf{R}}_L{}^L\mathbf{p}+{}^I{\mathbf{T}}_L{)}^{\wedge },\vec{\mathbf{n}},0,0,0,0]$$

3、计算新卡尔曼增益

原卡尔曼增益为：
$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^T(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$$
新卡尔曼增益：
$$\mathbf{K}=({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}{)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}$$

4、状态量的更新

$${\hat{\mathbf{x}}}_k^{\kappa +1}={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-\mathbf{K}{\mathbf{z}}_k^{\kappa }-\left(\mathbf{I}-\mathbf{KH}\right){\left({\mathbf{J}}^{\kappa }\right)}^{-1}({\hat{\mathbf{x}}}_k^{\kappa }\text{日}{\hat{\mathbf{x}}}_k)\right)$$

5、协方差的更新

$${\bar{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{KH}\right)\mathbf{P}$$

五、地图的更新

1、局部地图的更新

• 局部地图的初始化：以雷达在世界坐标系下的位置为中心，计算局部地图的两个对角的坐标，确定范围
• 计算雷达中心距离局部地图各个面的距离，如果距离小于阈值，则雷达接近边界，需要移动局部地图
• 计算需要移动的距离，更新局部地图的范围，并计算需要删除的范围，利用ikdtree进行删除

2、全局地图的更新

• 将当前帧从雷达坐标系转换到世界坐标系
• 计算当前点所在体素的中心，如果当前点的最近临点距离体素中心的距离大于体素边长的一半，则该点不需要下采样
• 如果最近临点到体素中心的距离 < 该点到体素中心的距离，则该点不需要添加
• 将需要添加的点添加进ikdtree