当前位置：首页 > news >正文

从代码到公式：手把手拆解FAST-LIO状态预测模块（附C++/Eigen实现）

news 2026/6/18 2:32:28

FAST-LIO状态预测模块的数学与代码全景解析

在SLAM系统的开发中，状态预测模块的精度直接影响整个系统的定位效果。FAST-LIO作为基于紧耦合迭代卡尔曼滤波的激光-惯性里程计，其状态预测模块融合了IMU的高频测量与激光雷达的点云数据，实现了精准的状态估计。本文将深入剖析FAST-LIO状态预测模块的数学原理与工程实现，帮助开发者跨越理论到实践的鸿沟。

1. 状态空间建模与变量定义

FAST-LIO的状态空间设计是其核心创新之一。系统状态x包含位置、姿态、速度、传感器偏差等关键信息，完整描述了机器人的运动状态。

1.1 状态变量结构

在C++实现中，状态通过state_ikfom结构体定义：

struct state_ikfom { Eigen::Vector3d pos = Eigen::Vector3d(0,0,0); // 位置 (x,y,z) Sophus::SO3 rot = Sophus::SO3(Eigen::Matrix3d::Identity()); // 旋转矩阵 Sophus::SO3 offset_R_L_I = Sophus::SO3(Eigen::Matrix3d::Identity()); // LiDAR-IMU旋转偏移 Eigen::Vector3d offset_T_L_I = Eigen::Vector3d(0,0,0); // LiDAR-IMU平移偏移 Eigen::Vector3d vel = Eigen::Vector3d(0,0,0); // 速度 Eigen::Vector3d bg = Eigen::Vector3d(0,0,0); // 陀螺仪偏置 Eigen::Vector3d ba = Eigen::Vector3d(0,0,0); // 加速度计偏置 Eigen::Vector3d grav = Eigen::Vector3d(0,0,-G_m_s2); // 重力向量 };

数学上，状态变量可表示为：

x = {p, R, ΔR, ΔT, v, bg, ba, g}

其中：

p ∈ ℝ³：全局坐标系中的位置
R ∈ SO(3)：局部到全局的旋转矩阵
ΔR ∈ SO(3), ΔT ∈ ℝ³：LiDAR到IMU的外参
v ∈ ℝ³：线速度
bg, ba ∈ ℝ³：IMU偏差
g ∈ S²：重力向量（单位球面）

1.2 IMU输入定义

IMU输入数据通过input_ikfom结构体表示：

struct input_ikfom { Eigen::Vector3d acc = Eigen::Vector3d(0,0,0); // 加速度计测量(m/s²) Eigen::Vector3d gyro = Eigen::Vector3d(0,0,0); // 陀螺仪测量(rad/s) };

数学表示为： u = {a, ω}

2. 状态转移函数的实现剖析

状态转移函数f描述了系统状态随时间演化的规律，是预测模块的核心。

2.1 状态转移方程

数学上，状态转移函数定义为：

f(x,u) = [v, ω-bg, 0, 0, R(a-ba)+g, 0, 0, 0]ᵀ

C++实现中的get_f函数对应这一数学表达：

Eigen::Matrix<double, 24, 1> get_f(state_ikfom s, input_ikfom in) { Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero(); Eigen::Vector3d omega = in.gyro - s.bg; // 角速度减去偏差 Eigen::Vector3d a_inertial = s.rot.matrix() * (in.acc - s.ba); // 加速度转到世界系 for(int i=0; i<3; i++) { res(i) = s.vel[i]; // 速度 res(i+3) = omega[i]; // 角速度 res(i+12) = a_inertial[i] + s.grav[i]; // 加速度+重力 } return res; }

关键点解析：

第1-3维：线速度v
第4-6维：角速度ω减去陀螺仪偏差bg
第13-15维：加速度在世界坐标系的投影加上重力

2.2 离散时间积分

在实际实现中，状态预测采用前向欧拉积分：

x_{k+1} = x_k ⊞ (Δt·f(x_k,u_k))

其中⊞为广义加法运算（后文详述），Δt为IMU采样间隔。

3. 雅可比矩阵的计算与实现

雅可比矩阵在误差状态传播中起关键作用，直接影响卡尔曼滤波的精度。

3.1 状态雅可比矩阵Fx

数学上，Fx = ∂f/∂x描述了状态转移函数对状态的敏感性。FAST-LIO中对应的公式为：

Fx = [Exp(-ω̂Δt) 0 0 -A(ω̂Δt)ᵀΔt 0 0 0 I IΔt 0 0 0 -GR̂[â]∧Δt 0 I 0 -GR̂Δt IΔt 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I]

C++实现通过df_dx函数计算：

Eigen::Matrix<double,24,24> df_dx(state_ikfom s, input_ikfom in) { Eigen::Matrix<double,24,24> cov = Eigen::Matrix<double,24,24>::Zero(); cov.block<3,3>(0,12) = Eigen::Matrix3d::Identity(); // ∂v/∂v Eigen::Vector3d acc_ = in.acc - s.ba; cov.block<3,3>(12,3) = -s.rot.matrix() * Sophus::SO3::hat(acc_); // ∂a/∂R cov.block<3,3>(12,18) = -s.rot.matrix(); // ∂a/∂ba cov.block<3,3>(12,21) = Eigen::Matrix3d::Identity(); // ∂a/∂g cov.block<3,3>(3,15) = -Eigen::Matrix3d::Identity(); // ∂ω/∂bg return cov; }

关键块解析：

cov.block<3,3>(0,12)：速度对速度的导数（单位矩阵）
cov.block<3,3>(12,3)：加速度对旋转的导数（涉及叉乘矩阵）
cov.block<3,3>(12,18)：加速度对加速度偏差的导数

3.2 噪声雅可比矩阵Fw

Fw = ∂f/∂w描述了状态转移对过程噪声的敏感性：

Fw = [-A(ω̂Δt)ᵀΔt 0 0 0 0 0 0 0 0 0 -GR̂Δt 0 0 0 0 IΔt 0 0 0 0 0 0 0 0]

C++实现：

Eigen::Matrix<double,24,12> df_dw(state_ikfom s, input_ikfom in) { Eigen::Matrix<double,24,12> cov = Eigen::Matrix<double,24,12>::Zero(); cov.block<3,3>(12,3) = -s.rot.matrix(); // ∂a/∂wa cov.block<3,3>(3,0) = -Eigen::Matrix3d::Identity(); // ∂ω/∂wω cov.block<3,3>(15,6) = Eigen::Matrix3d::Identity(); // ∂bg/∂wbg cov.block<3,3>(18,9) = Eigen::Matrix3d::Identity(); // ∂ba/∂wba return cov; }

4. 流形上的状态更新

FAST-LIO采用流形上的广义加法(⊞)和减法(⊟)来处理SO(3)旋转群的特殊性质。

4.1 广义加法实现

广义加法boxplus实现了状态增量的更新：

state_ikfom boxplus(state_ikfom x, Eigen::Matrix<double,24,1> f_) { state_ikfom x_r; x_r.pos = x.pos + f_.block<3,1>(0,0); x_r.rot = x.rot * Sophus::SO3::exp(f_.block<3,1>(3,0)); x_r.offset_R_L_I = x.offset_R_L_I * Sophus::SO3::exp(f_.block<3,1>(6,0)); x_r.offset_T_L_I = x.offset_T_L_I + f_.block<3,1>(9,0); x_r.vel = x.vel + f_.block<3,1>(12,0); x_r.bg = x.bg + f_.block<3,1>(15,0); x_r.ba = x.ba + f_.block<3,1>(18,0); x_r.grav = x.grav + f_.block<3,1>(21,0); return x_r; }

数学上，对于旋转部分的更新采用指数映射： R' = R·exp(ξ) ，其中ξ∈so(3)为李代数元素

4.2 广义减法实现

广义减法boxminus用于计算状态差异：

vectorized_state boxminus(state_ikfom x1, state_ikfom x2) { vectorized_state x_r = vectorized_state::Zero(); x_r.block<3,1>(0,0) = x1.pos - x2.pos; x_r.block<3,1>(3,0) = Sophus::SO3(x2.rot.matrix().transpose() * x1.rot.matrix()).log(); // 其他分量类似... return x_r; }

旋转差异通过李代数对数映射计算： ξ = log(R₂ᵀR₁)

5. 前向传播与协方差预测

完整的前向传播过程包括状态预测和协方差矩阵更新。

5.1 预测步骤实现

predict函数实现了完整的预测流程：

void predict(double &dt, Eigen::Matrix<double,12,12> &Q, const input_ikfom &i_in) { Eigen::Matrix<double,24,1> f_ = get_f(x_, i_in); // 状态转移 Eigen::Matrix<double,24,24> f_x_ = df_dx(x_, i_in); // 状态雅可比 Eigen::Matrix<double,24,12> f_w_ = df_dw(x_, i_in); // 噪声雅可比 x_ = boxplus(x_, f_ * dt); // 状态预测 f_x_ = Matrix<double,24,24>::Identity() + f_x_ * dt; // 离散化 P_ = (f_x_)*P_*(f_x_).transpose() + (dt*f_w_)*Q*(dt*f_w_).transpose(); // 协方差预测 }