FAST_LIO原理解读

一、整体框架

二、前向传播

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))$$

$$\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)=\left[\begin{array}{c}{\mathbf{\omega }}_{m_i}-{\mathbf{b}}_{{\mathbf{\omega }}_i}-{\mathbf{n}}_{{\mathbf{\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}{\mathbf{\omega }}_i}\\ {\mathbf{n}}_{\mathbf{b}{\mathbf{a}}_i}\\ 0_{3\times 1}\end{array}\right]$$

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

误差值 = 真实值 - 估计值
$$\begin{array}{rl}{\tilde{\mathbf{x}}}_{i+1}& ={\mathbf{x}}_{i+1}\boxminus {\hat{\mathbf{x}}}_{i+1}\\ & =\left({\mathbf{x}}_i⊞\Delta t\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)\boxminus \left({\hat{\mathbf{x}}}_i⊞\Delta t\mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right)\\ & \simeq {\mathbf{F}}_{\tilde{\mathbf{x}}}{\tilde{\mathbf{x}}}_i+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_i.\end{array}$$
误差量的协方差矩阵：
$${\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]$$

$${\mathbf{F}}_{\mathbf{w}}=\left[\begin{array}{cccc}-\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& {-}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& 0& 0\\ 0& 0& 1\Delta t& 0\\ 0& 0& 0& 1\Delta t\\ 0& 0& 0& 0\end{array}\right]$$

三、反向传播

• 遍历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$

  平面方程可进一步写为：

$$\begin{gathered} \frac{a}{d}x+\frac{b}{d}y+\frac{c}{d}z=-1 \\ {D}_1{x}_i+D_2{y}_i+D_3{z}_i=-1 \\ \left[\begin{array}{ccc}{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{array}\right]\left[\begin{array}{c}{D}_1\\ D_2\\ D_3\end{array}\right]=\left[\begin{array}{c}-1\\ -1\\ -1\\ -1\\ -1\end{array}\right] \\ AX=b \end{gathered}$$

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

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

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

观测方程：
$$\begin{array}{rl}\text{0}& ={\mathbf{h}}_j\left({\mathbf{x}}_k{,}^{L_j}{\mathbf{n}}_{f_j}\right)\\ & ={\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }{,}^{L_j}{\mathbf{n}}_{f_j}\right)\\ & \simeq {\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa },0\right)+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\\ & ={\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\end{array}$$
其中雅可比矩阵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