FAST-LIO：从误差状态卡尔曼滤波到高性能激光雷达里程计

1. FAST-LIO与误差状态卡尔曼滤波的完美结合

第一次接触FAST-LIO时，我就被它惊人的实时性能震撼到了。当时我正在调试一台搭载Velodyne VLP-16激光雷达的移动机器人，传统LIO算法在快速转弯时总会出现轨迹漂移。直到尝试了FAST-LIO，这个问题才得到彻底解决。这让我不禁好奇：为什么这个算法能在保持轻量化的同时，实现如此鲁棒的定位效果？答案就藏在它独特的误差状态卡尔曼滤波（ESKF）设计中。

误差状态卡尔曼滤波与传统卡尔曼滤波最大的不同，在于它处理的不是完整状态量，而是状态量的误差项。这就好比我们在纸上写字时，不是直接修改已经写错的内容，而是在旁边用红笔标注修正意见。这种设计带来了三个显著优势：

• 数值稳定性更好：误差量通常远小于状态量本身，避免了浮点数计算的精度损失
• 线性化更准确：在小误差假设下，非线性系统的线性近似更接近真实情况
• 计算效率更高：只需要更新误差状态，减少了不必要的计算开销

在激光雷达里程计的具体场景中，IMU数据的高频特性与激光雷达点云的精确性形成了完美互补。但IMU的零偏和噪声就像调皮的小鬼，总是在你不注意时捣乱。FAST-LIO的聪明之处在于，它用ESKF专门对付这些误差项，让IMU做好"短期精确"的本职工作，而激光雷达则负责"长期可靠"的全局校正。

2. 误差状态方程的数学奥秘

2.1 状态量的分解艺术

让我们用一个日常比喻来理解状态分解：假设你在玩VR游戏，头盔的真实位姿（真值状态xₜ）可以分解为系统预测的位姿（名义状态x）和预测误差（误差状态δx）。在FAST-LIO中，这个数学表达非常优雅：

// 伪代码示例：状态量定义 struct State { Vec3d p; // 位置 Vec3d v; // 速度 Quatd q; // 旋转(四元数) Vec3d bg; // 陀螺零偏 Vec3d ba; // 加速度计零偏 Vec3d g; // 重力向量 };

误差状态的特别之处体现在旋转表示上。不同于直接对旋转矩阵做加减，ESKF使用李代数δθ来表示旋转误差：

Rₜ = R * Exp(δθ)

这个Exp映射正是SO(3)群上的指数映射，它将三维向量转换为旋转矩阵。在实际编程中，我常用Eigen库来实现这个运算：

Eigen::Matrix3d Exp(const Eigen::Vector3d& w) { double theta = w.norm(); if (theta < 1e-7) return Eigen::Matrix3d::Identity(); Eigen::Matrix3d W = skewSymmetric(w/theta); return Eigen::Matrix3d::Identity() + sin(theta)*W + (1-cos(theta))*W*W; }

2.2 运动方程的推导技巧

推导误差状态方程时，有几个关键步骤让我印象深刻。首先是速度误差的推导，需要考虑科氏加速度的影响。具体来说，真实加速度可以表示为：

v̇ₜ = Rₜ(ã - bₐₜ - ηₐ) + gₜ

展开后会出现一个有趣的交叉项 -Rã∧δθ，这实际上反映了机体旋转对加速度测量的影响。我在调试时就曾忽略这个项，导致无人机在快速滚转时出现高度估计漂移。

旋转误差的推导更为精妙。需要利用SO(3)的伴随性质：

R⋅ω∧ = (Rω)∧⋅R

这个性质允许我们将旋转矩阵"穿过"反对称矩阵，就像变魔术一样。最终得到的旋转误差方程：

δθ̇ = -(ω̃ - b_g)∧δθ - δb_g - η_g

揭示了陀螺仪误差是如何传播的。其中-(ω̃ - b_g)∧这个项特别重要，它说明高速旋转时会放大角度误差，这也解释了为什么无人机做快速机动时更容易丢失定位。

3. 离散时间下的预测过程

3.1 名义状态的预测

在实际编程实现时，我发现离散化处理有诸多细节需要注意。以位置预测为例：

p_{k+1} = p_k + v_kΔt + 0.5(R_k(ã - b_a))Δt² + 0.5gΔt²

这个看似简单的公式，在实现时却有几个优化点：

1. Δt的选择：通常取IMU的采样间隔(5-10ms)，过大会导致积分误差累积
2. 加速度处理：需要先扣除零偏b_a，再旋转到世界坐标系
3. 重力补偿：记得重力方向通常是z轴负方向

下面是我在项目中使用的预测代码片段：

void predictNominalState(State& state, const IMUData& imu, double dt) { // 旋转积分 Eigen::Vector3d omega = imu.gyro - state.bg; state.q *= deltaQ(omega * dt); // 速度积分 Eigen::Vector3d acc = imu.acc - state.ba; state.v += (state.q * acc + state.g) * dt; // 位置积分 state.p += state.v * dt + 0.5 * (state.q * acc + state.g) * dt * dt; }

3.2 误差状态的协方差传播

误差状态的预测需要考虑噪声传播。FAST-LIO使用了一个巧妙的技巧：将连续时间噪声转换为离散时间噪声。对于陀螺零偏噪声：

σ(η_g) = √Δt ⋅ σ_{bg}

这个√Δt的关系源于布朗运动的统计特性。在实现时，我构建了一个噪声协方差矩阵：

Eigen::Matrix<double,18,18> Q = Eigen::Matrix<double,18,18>::Zero(); Q.block<3,3>(3,3) = (dt*dt)*acc_noise*Eigen::Matrix3d::Identity(); // 速度噪声 Q.block<3,3>(6,6) = (dt*dt)*gyro_noise*Eigen::Matrix3d::Identity(); // 旋转噪声 Q.block<3,3>(9,9) = dt*gyro_bias_noise*Eigen::Matrix3d::Identity(); // 陀螺零偏噪声 Q.block<3,3>(12,12) = dt*acc_bias_noise*Eigen::Matrix3d::Identity(); // 加速度零偏噪声

雅可比矩阵F的构造是预测的核心，它反映了各个误差项之间的耦合关系。特别是速度误差与旋转误差的耦合项 -R(ã - b_a)∧Δt，这个项在实际测试中对系统稳定性影响很大。

4. 激光雷达观测的更新过程

4.1 点云到地图的匹配

FAST-LIO的更新阶段是其性能关键。与传统ICP不同，它采用紧耦合的迭代卡尔曼滤波框架。每次接收到激光雷达数据时，我的处理流程通常是：

1. 点云去畸变：利用预测的IMU轨迹补偿激光雷达运动畸变
2. 特征提取：从原始点云提取平面和边缘特征
3. 数据关联：将特征点与体素地图进行最近邻匹配
4. 残差计算：计算点到平面或点到边的距离残差

观测模型可以表示为：

z = h(x) + v

其中h(x)是非线性观测函数。对于平面特征点，它的残差就是点到匹配平面的距离。这个计算看似简单，但在实现时有几个优化点：

double computePlaneResidual(const Point& point, const VoxelMap& map) { // 查找最近的5个点 auto neighbors = map.findNearestNeighbors(point, 5); // 计算平面参数 Eigen::Vector3d normal; double d; fitPlane(neighbors, normal, d); // 返回点到平面距离 return normal.dot(point) + d; }

4.2 误差状态的观测更新

ESKF更新的精髓在于观测矩阵H的计算。由于我们更新的是误差状态，需要用到链式法则：

H = ∂h/∂x ⋅ ∂x/∂δx

其中第二项∂x/∂δx对于旋转部分特别有趣：

∂R/∂δθ = J_r⁻¹(R)

这个J_r⁻¹是右雅可比矩阵的逆，它反映了旋转矩阵对微小扰动的敏感度。在我的实现中，采用以下近似计算：

Eigen::Matrix3d computeRightJacobianInverse(const Eigen::Vector3d& w) { double theta = w.norm(); if (theta < 1e-7) return Eigen::Matrix3d::Identity(); Eigen::Matrix3d W = skewSymmetric(w); return Eigen::Matrix3d::Identity() + 0.5*W + (1/theta/theta - (1+cos(theta))/(2*theta*sin(theta)))*W*W; }

卡尔曼增益的计算虽然标准，但在资源受限的平台上需要特别注意矩阵求逆的效率。我通常使用QR分解来求解：

Eigen::MatrixXd K = P * H.transpose(); Eigen::MatrixXd S = H * P * H.transpose() + V; K = S.ldlt().solve(K.transpose()).transpose();

5. 误差状态重置与实践技巧

5.1 状态重置的数学细节

误差状态更新后需要合并到名义状态，这个过程看似简单实则暗藏玄机。特别是旋转部分的处理：

R_{k+1} = R_k * Exp(δθ_k)

这个操作在数学上很优雅，但在实现时要注意四元数的乘法顺序。我踩过的坑是不同数学库的四元数乘法约定可能不同，导致结果完全错误。

协方差矩阵的重置更微妙。理论上应该考虑旋转误差带来的切空间变化：

P_reset = J_k * P * J_k^T

其中J_k包含了一个小雅可比矩阵：

J_θ = I - 0.5 δθ_k∧

在实际应用中，我发现当δθ_k很小时，可以省略这个修正，但对高速运动的机器人还是建议保留。

5.2 工程实践中的调参经验

经过多个项目的实战，我总结了一些FAST-LIO的调参经验：

1. IMU噪声参数：不要直接使用数据手册的值，建议通过静态采集标定

   • 陀螺仪噪声：通常1e-4 ~ 1e-3 rad/s/√Hz
   • 加速度计噪声：通常1e-3 ~ 1e-2 m/s²/√Hz

2. 激光雷达匹配权重：平面特征比边缘特征可以给更大权重

   • 平面特征：0.5 ~ 1.0
   • 边缘特征：0.1 ~ 0.3

3. 体素地图分辨率：根据场景动态调整

   • 室内：0.2 ~ 0.5米
   • 室外：0.5 ~ 2.0米

4. 关键帧策略：平移或旋转超过阈值时更新地图

   • 平移阈值：0.1 ~ 0.3米
   • 旋转阈值：5 ~ 10度

下面是我的参数配置文件示例：

# FAST-LIO配置示例 imu: gyro_noise: 0.0001 acc_noise: 0.001 gyro_bias_noise: 0.00001 acc_bias_noise: 0.0001 lidar: plane_resolution: 0.3 edge_resolution: 0.5 plane_weight: 1.0 edge_weight: 0.2 mapping: voxel_size: 0.5 max_range: 50.0 min_range: 1.0

6. 性能优化与创新改进

6.1 计算效率的提升技巧

让FAST-LIO在嵌入式平台实时运行需要一些优化技巧。我常用的方法包括：

1. 体素地图的懒更新：只有当机器人移动一定距离后才更新局部地图
2. 特征点的选择性处理：只处理信息量大的区域（如墙角、边缘）
3. 并行化处理：将点云处理和状态估计分配到不同线程
4. 内存预分配：避免运行时动态内存申请

特别是在资源受限的平台上，我重写了点云处理流水线：

class LidarProcessor { public: void process(const PointCloud::Ptr& cloud) { // 预分配内存 if (tmp_cloud_.size() < cloud->size()) { tmp_cloud_.resize(cloud->size()); } // 并行化特征提取 #pragma omp parallel for for (size_t i = 0; i < cloud->size(); ++i) { extractFeature((*cloud)[i], tmp_cloud_[i]); } // 后续处理... } private: std::vector<FeaturePoint> tmp_cloud_; };

6.2 算法改进的方向

基于ESKF的框架，我尝试过几个有效的改进方案：

1. 运动补偿增强：在高速运动场景，采用二阶运动模型进行点云去畸变
2. 自适应噪声调整：根据运动剧烈程度动态调整IMU噪声参数
3. 多传感器融合：加入轮速计或视觉信息提升平面运动估计
4. 闭环检测集成：结合Scan Context等方法实现后端优化

其中自适应噪声调整的效果特别显著。当检测到机器人剧烈运动时，适当增大过程噪声可以避免滤波器过自信：

if (angular_velocity.norm() > 2.0) { // 高动态场景 Q.block<3,3>(6,6) *= 4.0; // 增大旋转噪声 Q.block<3,3>(3,3) *= 2.0; // 增大速度噪声 }

7. 典型问题与调试方法

7.1 常见问题排查

在部署FAST-LIO时，我遇到过各种奇怪的问题。以下是几个典型案例：

1. 高度持续漂移：通常是加速度计零偏估计不准，建议延长静态初始化时间
2. 快速旋转时发散：检查陀螺仪噪声参数，可能需要增大过程噪声
3. 点云匹配失败：降低体素分辨率或增加特征点数量
4. 计算延迟大：优化点云降采样率和体素地图更新策略

我常用的调试工具包括：

• RViz：实时可视化轨迹和点云匹配情况
• rqt_plot：绘制状态估计和传感器原始数据对比
• gdb：当程序崩溃时进行堆栈跟踪

7.2 实战调试案例

曾经在一个仓储机器人项目中出现过这样的问题：当机器人靠近金属货架时，定位会突然跳变。经过分析发现：

1. 根本原因：金属货架导致IMU受到磁干扰，陀螺仪输出异常
2. 解决方案：
   • 在IMU周围添加磁屏蔽材料
   • 实现异常检测算法，当陀螺仪数据突变时暂时增大噪声参数
   • 增加激光雷达的匹配权重

调试代码示例：

bool checkIMUAbnormal(const IMUData& imu) { static Eigen::Vector3d last_gyro = Eigen::Vector3d::Zero(); double delta = (imu.gyro - last_gyro).norm(); last_gyro = imu.gyro; return delta > 0.5; // 超过0.5 rad/s的变化视为异常 }

这个案例让我深刻认识到，实际部署中传感器数据的可靠性比算法本身更重要。