FAST-LIO中的ESKF应用

qq_35971623

已于 2025-03-24 14:55:23 修改

阅读量1.6k

点赞数 28

文章标签：算法

于 2024-09-20 09:23:13 首次发布

本文链接：https://blog.youkuaiyun.com/qq_35971623/article/details/142244300

版权

ESKF原理：ESKF学习笔记-优快云博客

https://zhuanlan.zhihu.com/p/441182819

FAST-LIO原理推导：https://zhuanlan.zhihu.com/p/587500859

FASTLIO代码：GitHub - zlwang7/S-FAST_LIO: A simplified implementation of FAST_LIO (with Chinese note)

FAST-LIO的基本原理和步骤

FAST-LIO以IMU建立运动方程，进行运动方程递推得到里程计定位的预测值（但是由于长时间IMU积分递推存在发散和漂移问题），以Lidar点云与先验地图点云残差作为观测值对IMU预测值进行校准。

1.ESKF预测过程（前向传播）

1.1离散模型

1.1.1离散模型的原理

在已知上一次IMU测量时系统状态 $x_i=[R_i,p_i,v_i,b_{wi},b_{ai},g_i]$ (状态含义[ $R_i$ :IMU到G系的旋转矩阵， $p_i$ :IMU到G系的平移矩阵， $v_i$ :imu的速度， $b_{wi}$ :imu的角速度零偏（对应噪声 $\eta _{bw}$ ）, $b_{ai}$ imu的加速度计零偏（对应噪声 $\eta _{aw}$ ）, $g_i$ :重力加速度])情况下，利用数值积分定理，推导状态方程的离散形式

$x_{i+1}=x_i+x\dot{}_{i} * dt$ (1.1)

$x\dot{}=f(x,u,w)$ (1.2)

$x_{i+1}=x_i\oplus (f(x_i,u_i,w_i)\delta t)$ (1.3)

1.1.2对应代码结合

1）变量的定义

在代码中，状态变量的顺序和数量和论文有一些差异，其变量为 $x=[p,R,R_{LI},T_{LI},v,b_g,b_a,g]$ ,其中， $R_{LI}$ 表示imu系到雷达系的旋转矩阵， $T_{LI}$ 表示imu系到雷达系的平移矩阵(imu和雷达的安装中心和角度不同，通过设置 $R_{LI}$ ， $T_{LI}$ 外参，将imu系的测量值转换到lidar系)

2）离散模型代码

离散方程的f公式定义，此处不考虑噪声影响,且由于状态变量顺序问题和论文中存在一定区别（ESKF中，会对名义状态的噪声进行补偿）

$f(x,u,0)=[v,w_m-b_g,0,0,R_{GI}(a_m-b_a),0,0,0]T$

广义加法对应的代码

$x_{i+1}=x_i\oplus (f(x_i,u_i,w_i)\delta t)$

1.2ESKF预测方程与状态转移矩阵

1.2.1误差状态的转移矩阵

1）状态转移矩阵

使用二元函数的一阶泰勒展开式，对 $x\tilde{}_{i+1}$ 进行展开，得到误差状态的状态方程

其中 $\widetilde{x}_{i+1}$ :表示第i+1时刻X的误差（真实值-估计值）

$\widehat{x}_{i+1}$ :表示第i+1时刻X的估计值（第i+1时刻卡尔曼滤波计算的最优估计值）

${x}_{i+1}$ ：表示第i+1时刻X的真实值

f(x,u,w)是离散模型方程

因为 $\widetilde{x}_{i}$ 是误差， ${w}_{i}$ 是噪声，其真实值接近0，故在 $\widetilde{x}_{i}$ =0， ${w}_{i}$ =0位置处对G进行泰勒展开

$F_x$ 的代码如下，由于代码中的状态变量 $x=[p,R,R_{LI},T_{LI},v,b_g,b_a,g]$ 与论文中的顺序不同，采用某行某列（P行V列）这种形式，将论文中的矩阵元素填入 $F_x$ 中

$F_W$ 的代码也如下，具体的操作过程如上。

2）预测方程：

此处对应步骤1：输入上一时刻的最优估计 $x\bar{}_{k-1}$ , $p\bar{}_{k-1}$ 基于前向传播得到状态预测值（名义状态）和先验误差的协方差矩阵 $P\hat{}_{i+1}$ 。

根据ESKF的原理ESKF学习笔记-优快云博客，其在预测过程中的离散方程 $\delta x_{i+1}=F_x \delta x_i$ 可以忽略（每次在误差状态更新后，误差会加入到名义状态进行补偿，从而更新 $\delta x=0$ ）,协方差矩阵根据上一时刻的协方差 $P\hat{}_i$ 和状态转移矩阵 $F_x$ , $F_w$ 进行推导。

在此还计算了当前的名义状态，名义状态=上一真实状态+系统作用量*dt

2.ESKF的更新过程

2.1残差计算与观测方程推导

2.1.1残差计算

此处对应步骤4迭代中的第3点：计算点云残差和一阶泰勒展开雅可比矩阵

通过KD-tree(IKD-tree)在PCD先验地图中搜索当前点的邻近点，以临近点构建平面，计算当前雷达点（去畸变后的全局系点云）到PCD平面的距离作为观测残差。

2.1.2残差方程线性化

在紧耦合LIO系统中，把雷达的ICP和NDT残差作为观测方程，写入ESKF模型中，由于ICP和DNT算法本身需要迭代才能收敛，所以要对ESKF观测方程进行迭代，对于第i次对ESKF进行迭代的残差方程的计算如下：

1）采用EKF方式，对非线性的残差方程进行一阶泰勒展开，其中雅可比矩阵为：

$H=\frac{\partial h}{\partial x\tilde{}}$

即其等价于观测方程相对于误差状态的雅可比矩阵(ESKF)

$H=\frac{\partial h}{\partial \delta x}$

2）对于残差方程其可以简写为

$h(p(x))=(p-p_a)*n\vec{}$

那么H的计算可以利用链式法则写成

3）对应的代码

由于状态变量和论文的形式不同，代码中与论文公式布局存在一定差异，代码中的变量

$x=[p,R,R_{LI},T_{LI},v,b_g,b_a,g]$

2.1.3误差状态的迭代与名义状态的更新

此处对应步骤4迭代中的第2点和第4点：2）计算迭代过程中先验一阶雅可比矩阵，和误差状态协方差；4）利用误差状态，更新状态估计（名义状态）和误差状态的卡尔曼增益。

在ESKF中，卡尔曼滤波对误差状态进行更新，并将更新后的误差状态输入更新后的名义状态来补偿噪声，得到最终的估计

在FASTLIO中使用迭代ESKF，将第K+1次的误差状态加入第K次的名义状态（残差补偿后），得到新的补偿后的第k+1次名义状态，当误差状态收敛（则认为迭代结束），第k+1次名义状态作为最终的估计输出。(在k=-1初始迭代时，输入的初始名义状态是由前向传播得到的)

$x\hat{}_{k}^{k+1}=x\hat{}_{k}^{k}\oplus x\tilde{}^{k+1}$

为简化计算，K可以使用以下式子等效替换（代码中使用如下K计算）