FAST_LIO原理解读

一、整体框架

请添加图片描述
请添加图片描述

二、前向传播

1、IMU静态初始化

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

均值的更新: A n = A n − 1 + X n − A n − 1 n A_n=A_{n-1}+\frac{X_n-A_{n-1}}{n} An=An1+nXnAn1

方差的更新: V n = n − 1 n 2 ( X n − A n − 1 ) 2 + n − 1 n V n − 1 V_n=\frac{n-1}{n^2}(X_n-A_{n-1})^2+\frac{n-1}{n}V_{n-1} Vn=n2n1(XnAn1)2+nn1Vn1

初始化重力方向: g r a v = − a c c m e a n n o r m ( a c c m e a n ) G grav=-\frac{acc_{mean}}{norm(acc_{mean})}G grav=norm(accmean)accmeanG

初始化陀螺仪零偏: b g = m e a n g y r b_g=mean_{gyr} bg=meangyr

2、传播状态量

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

输入量: u = [ w m T , a m T ] T u=[w_m^T ,a_m^T]^T u=[wmT,amT]T

状态量: x = [ R I G T , p I G T , v I G T , b w T , b a T , g ] T x=[{R_I^G}^T ,{p_I^G}^T,{v_I^G}^T,{b_w}^T,{b_a}^T,g]^T x=[RIGT,pIGT,vIGT,bwT,baT,g]T

噪声量: w = [ n w T , n a T , n b w T , n b a T ] T w=[n_w^T ,n_a^T,n_{bw}^T ,n_{ba}^T]^T w=[nwT,naT,nbwT,nbaT]T

② imu测量模型

测量值 = 真值 + 零偏 + 噪声
a m = a + b a + n a − R G I g w m = w + b w + n w b w ˙ = n b w b a ˙ = n b a a_m=a+b_a+n_a-R_G^Ig \\ w_m=w+b_w+n_w \\ \dot{b_w}=n_{bw} \\ \dot{b_a}=n_{ba} am=a+ba+naRGIgwm=w+bw+nwbw˙=nbwba˙=nba

③ imu运动模型

连续状态下的运动模型:
R ˙ = R w ^ v ˙ = a p ˙ = v \dot{R}=Rw^{\hat{} } \\ \dot{v}=a \\ \dot{p}=v R˙=Rw^v˙=ap˙=v
离散状态下的运动模型(欧拉积分):
R ( t + Δ t ) = R ( t ) E x p ( w ( t ) Δ t ) v ( t + Δ t ) = v ( t ) + a ( t ) Δ t p ( t + Δ t ) = p ( t ) + v ( t ) Δ t + 1 2 a ( t ) Δ t 2 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 R(t+Δt)=R(t)Exp(w(t)Δt)v(t+Δt)=v(t)+a(t)Δtp(t+Δt)=p(t)+v(t)Δt+21a(t)Δt2
将imu测量模型带入imu离散运动模型,得到状态量的传播公式:
R ( t + Δ t ) = R ( t ) E x p [ ( w m − b w − n w ) Δ t ] v ( t + Δ t ) = v ( t ) + [ R ( t ) ( a m − b a − n a ) + g ] Δ t p ( t + Δ t ) = p ( t ) + v ( t ) Δ t + 1 2 a ( t ) Δ t 2 b w ˙ = n b w b a ˙ = n b a 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} R(t+Δt)=R(t)Exp[(wmbwnw)Δt]v(t+Δt)=v(t)+[R(t)(ambana)+g]Δtp(t+Δt)=p(t)+v(t)Δt+21a(t)Δt2bw˙=nbwba˙=nba
对应论文中的形式为:
x i + 1 = x i ⊞ ( Δ t f ( x i , u i , w i ) ) \mathbf{x}_{i+1}=\mathbf{x}_i\boxplus(\Delta t\mathbf{f}(\mathbf{x}_i,\mathbf{u}_i,\mathbf{w}_i)) xi+1=xi(Δtf(xi,ui,wi))

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)= ωmibωinωiGvIiGRIi(amibainai)+Gginbωinbai03×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+1x i+1=(xiΔtf(xi,ui,wi))(x iΔtf(x i,ui,0))Fx x i+Fwwi.
误差量的协方差矩阵:
P ^ i + 1 = F x ~ P ^ i F x ~ T + F w Q F w T ; P ^ 0 = P ‾ k − 1 \widehat{\mathbf{P}}_{i+1}=\mathbf{F}_{\widetilde{\mathbf{x}}}\widehat{\mathbf{P}}_i\mathbf{F}_{\widetilde{\mathbf{x}}}^T+\mathbf{F}_{\mathbf{w}}\mathbf{Q}\mathbf{F}_{\mathbf{w}}^T;\widehat{\mathbf{P}}_0=\overline{\mathbf{P}}_{k-1} P i+1=Fx P iFx T+FwQFwT;P 0=Pk1
其中
F x = [ E x p ( − ω ^ i Δ t ) 0 0 − A ( ω ^ i Δ t ) T Δ t 0 0 0 I I Δ t 0 0 0 − G R ^ I i ⌊ a ^ i ⌋ ∧ Δ t 0 I 0 − G R ^ I i Δ t I Δ t 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F_x}=\begin{bmatrix}\mathrm{Exp}\left(-\widehat{\omega}_i\Delta t\right)&\mathbf{0}&\mathbf{0}&-\mathbf{A}(\widehat{\omega}_i\Delta t)^T\Delta t&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{I}&\mathbf{I}\Delta t&\mathbf{0}&\mathbf{0}&\mathbf{0}\\-^G\widehat{\mathbf{R}}_{I_i}\lfloor\widehat{\mathbf{a}}_i\rfloor\wedge\Delta t&\mathbf{0}&\mathbf{I}&\mathbf{0}&-^G\widehat{\mathbf{R}}_{I_i}\Delta t&\mathbf{I}\Delta t\\\mathbf{0}&0&0&\mathbf{I}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&0&0&\mathbf{0}&\mathbf{I}&\mathbf{0}\\\mathbf{0}&0&0&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix} Fx= Exp(ω iΔt)0GR Iia iΔt0000I00000IΔtI000A(ω iΔt)TΔt00I0000GR IiΔt0I000IΔt00I

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Δt0000000GR IiΔt0000001Δt0000001Δt0

三、反向传播

在这里插入图片描述

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

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

  • it_pcl在世界坐标系下的姿态为: R i = R ∗ E X P ( i m u g r y ∗ d t ) R_i=R*EXP(imu_{gry}*dt) Ri=REXP(imugrydt)

  • it_pcl在世界坐标系下的位置为: P i = p o s + v e l ∗ d t + 0.5 ∗ i m u a c c ∗ d t 2 P_i=pos+vel*dt+0.5*imu_{acc}*dt^2 Pi=pos+veldt+0.5imuaccdt2

  • 运动畸变去除原理:根据补偿后的点在世界坐标系下的位姿 = 补偿前的点在世界坐标系下的位姿
    i m u s t a t e . r o t × ( R L I × p c o m + T L I ) + i m u s t a t e . p o s = R i × ( R L I × p i + t L I ) + P i imu_{state.rot}×(R_L^I×p_{com}+T_L^I)+imu_{state.pos}=R_i×(R_L^I×p_{i}+t_L^I)+P_i imustate.rot×(RLI×pcom+TLI)+imustate.pos=Ri×(RLI×pi+tLI)+Pi

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

1、计算点面残差
  • 先将当前帧从雷达坐标系转换到IMU坐标系,再根据前向传播估计的位姿转换到世界坐标系
    P i G = T I G × T L I × P i L P_i^G=T_I^G×T_L^I×P_i^L PiG=TIG×TLI×PiL

  • 使用ikdtree寻找距离当前点最近的5个点,进行拟合平面 a x + b y + c y + d = 0 ax+by+cy+d=0 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 dax+dby+dcz=1D1xi+D2yi+D3zi=1 x1x2x3x4x5y1y3y3y4y5z1z3z3z4z5 D1D2D3 = 11111 AX=b

​ 利用QR分解求得平面参数 [ D 1 , D 2 , D 3 ] [D_1,D_2,D_3] [D1,D2,D3],计算点到平面的距离 p d 2 pd2 pd2,当前点到激光雷达坐标系的距离 p d 1 pd1 pd1

​ 如果 1 − 0.9 p d 2 p d 1 > 0.9 1 - 0.9\frac{pd2}{pd1}>0.9 10.9pd1pd2>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,Ljnfj)=hj(x kκx kκ,Ljnfj)hj(x kκ,0)+Hjκx kκ+vj=zjκ+Hjκx kκ+vj
其中雅可比矩阵H:
H = [ − n ⃗ G R ^ I ( I R L L p + I T L ) ∧ , n ⃗ , 0 , 0 , 0 , 0 ] H=[-\vec{\boldsymbol{n}}^G\mathbf{\hat{R}}_I(^I\mathbf{R}_L{}^L\mathbf{p}+{}^I\mathbf{T}_L)^\wedge,\vec{\boldsymbol{n}},0,0,0,0] H=[n GR^I(IRLLp+ITL),n ,0,0,0,0]

3、计算新卡尔曼增益

原卡尔曼增益为:
K = P H T ( H P H T + R ) − 1 \mathbf{K}=\mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T+\mathbf{R})^{-1} K=PHT(HPHT+R)1
新卡尔曼增益:
K = ( H T R − 1 H + P − 1 ) − 1 H T R − 1 \mathbf{K}=(\mathbf{H}^T\mathbf{R}^{-1}\mathbf{H}+\mathbf{P}^{-1})^{-1}\mathbf{H}^T\mathbf{R}^{-1} K=(HTR1H+P1)1HTR1

4、状态量的更新

x ^ k κ + 1 = x ^ k κ ⊞ ( − K z k κ − ( I − K H ) ( J κ ) − 1 ( x ^ k κ 日 x ^ k ) ) \widehat{\mathbf{x}}_k^{\kappa+1}=\widehat{\mathbf{x}}_k^\kappa\boxplus\left(-\mathbf{K}\mathbf{z}_k^\kappa-\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\left(\mathbf{J}^\kappa\right)^{-1}\left(\widehat{\mathbf{x}}_k^\kappa\right.\text{日}\widehat{\mathbf{x}}_k)\right) x kκ+1=x kκ(Kzkκ(IKH)(Jκ)1(x kκx k))

5、协方差的更新

P ˉ k = ( I − K H ) P \mathbf{\bar{P}}_k=\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\mathbf{P} Pˉk=(IKH)P

五、地图的更新

1、局部地图的更新

请添加图片描述

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

2、全局地图的更新

  • 将当前帧从雷达坐标系转换到世界坐标系
  • 计算当前点所在体素的中心,如果当前点的最近临点距离体素中心的距离大于体素边长的一半,则该点不需要下采样
  • 如果最近临点到体素中心的距离 < 该点到体素中心的距离,则该点不需要添加
  • 将需要添加的点添加进ikdtree
  • 31
    点赞
  • 17
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值