FAST-LIO中IMU误差模型与噪声参数调优

在FAST-LIO的配置文件中，有一组参数直接影响滤波器的性能：imu_noise（加速度计噪声）、gyr_noise（陀螺仪噪声），以及对应的bias随机游走参数bias_acc_noise和bias_gyr_noise。许多用户在使用时要么直接沿用默认值，要么随意修改，导致系统响应迟钝或漂移过大。理解这些参数的含义并正确设置，是获得稳定定位效果的关键一步。

一、IMU测量模型回顾

IMU的原始测量值包含真值、零偏（bias）和高斯白噪声。模型如下：

text

ω_m = ω + b_g + n_ga_m = a + b_a + n_a - R * g

其中：

• ω_m, a_m：IMU输出的角速度和加速度测量值。

• ω, a：载体真实的角速度和加速度（不含重力）。

• b_g, b_a：陀螺仪和加速度计的零偏，随时间缓慢变化，服从随机游走模型。

• n_g, n_a：高斯白噪声，均值为0，协方差矩阵分别为 diag(σ_g²) 和 diag(σ_a²)。

• R：当前姿态，g是重力向量（世界系下）。

随机游走模型表示bias随时间连续变化，其导数为高斯噪声：

text

b_g_dot = n_bg, b_a_dot = n_ba

n_bg和n_ba的协方差决定了bias变化的剧烈程度。

在FAST-LIO的离散化传播中，噪声项被合并为协方差矩阵Q，需要用户配置的参数正是这些噪声的标准差（或方差）。

二、配置参数的含义与单位

FAST-LIO的YAML配置文件（如avia.yaml）中通常包含以下参数：

yaml

单位说明：

• gyr_noise：对应n_g，单位 rad/s。消费级IMU典型值0.001~0.01，工业级0.0001~0.001。

• acc_noise：对应n_a，单位 m/s²。典型值0.01~0.1。

• gyr_bias_noise：对应n_bg的离散化标准差，单位 rad/s²。数值通常比gyr_noise小一到两个数量级。

• acc_bias_noise：对应n_ba，单位 m/s³。

注意：FAST-LIO中这些参数均代表标准差（σ），而非方差（σ²）。在滤波器内部会计算平方得到协方差。参数文件可能用不同命名（如imu_noise、bias_acc_noise），但含义一致。

三、如何根据IMU数据手册估算初始值

大多数IMU数据手册会提供以下指标：

• Angular Random Walk (ARW)：单位 °/√h 或 °/√s。可以转换为陀螺仪白噪声标准差。

• Rate Noise Density：单位 (°/s)/√Hz。同样可转换。

• Bias Instability：零偏不稳定性，通常远小于白噪声，可用于设定bias随机游走。

转换公式（以陀螺仪为例）：

如果手册给出ARW = 0.1 °/√h，先换算为 rad/√s：0.1 * π/180 / 60 ≈ 2.91e-5 rad/√s。这是噪声密度（连续时间）。离散化时，标准差 ≈ 噪声密度 / √Δt，其中Δt是IMU采样间隔。例如IMU频率200 Hz（Δt=0.005 s），则白噪声标准差 ≈ 2.91e-5 / √0.005 ≈ 4.12e-4 rad/s。

更简便的方式：使用Allen方差分析工具（如imu_utils）在静止数据上直接标定噪声参数，得到的结果可直接用于配置文件。

对于没有手册的廉价IMU，可以参考以下经验初始值：

四、参数设置不当的影响

白噪声（gyr_noise / acc_noise）设太小：滤波器认为IMU测量非常精确，过度相信积分值，导致状态协方差收缩过快。当出现异常运动（如撞击、风扰）或瞬时特征匹配错误时，系统来不及修正，产生持续漂移。典型表现：静止时轨迹稳定，一旦运动就开始偏离真实路径。

白噪声设太大：滤波器认为IMU测量不可靠，倾向于依赖激光雷达观测。此时运动畸变补偿不足，且动态响应滞后，转弯或加减速时位姿估计会出现明显延迟（需要多帧激光数据才能修正）。

Bias随机游走设太小：滤波器假设IMU零偏几乎不变，因此无法跟踪bias的真实缓慢变化。长时间运行后，bias误差累积，导致姿态和速度积分漂移。常见现象：建图开始时效果良好，数分钟后轨迹开始旋转或上翘。

Bias随机游走设太大：滤波器频繁调整bias估计，将正常的运动加速度错误地归因于bias变化，导致状态估计出现高频抖动。

五、基于静止数据计算噪声方差的实用方法

在没有数据手册的情况下，可以通过采集一段静止状态下的IMU数据（建议时长1~2分钟），用简单的统计方法估算噪声参数。

步骤1：采集rosbag。将设备静止放在桌面或地面，录制IMU话题，例如rosbag record -O imu_static.bag /imu/data。

步骤2：分析静止数据。用Python脚本或MATLAB读取bag中的IMU数据，计算角速度和加速度的均值和标准差。均值可作为bias初值，标准差可作为白噪声标准差（但要减去bias影响）。由于静止时真值已知（角速度为0，加速度理论值为重力），统计量的标准差直接对应噪声水平。

示例Python代码片段：

python

import rosbag

import numpy as np

bag = rosbag.Bag('imu_static.bag')

gyr_x, acc_x = [], []

for topic, msg, t in bag.read_messages(topics=['/imu/data']):

gyr_x.append(msg.angular_velocity.x)

acc_x.append(msg.linear_acceleration.x)

gyr_x = np.array(gyr_x)

acc_x = np.array(acc_x)

gyr_std = np.std(gyr_x) # 陀螺仪白噪声标准差 (rad/s)

acc_std = np.std(acc_x) # 加速度计白噪声标准差 (m/s²)

# bias随机游走需要用差值法或Allen方差，近似可设为白噪声的1/10~1/100

注意：此方法测得的白噪声包含了量化噪声和部分bias变化，但作为初始值已经足够。

步骤3：在配置文件中填入计算得到的标准差，然后进行实机测试。如果发现漂移过大，可以适当增大白噪声；如果响应太慢，适当减小白噪声。

六、在线估计与离线参数的关系

FAST-LIO的在线外参标定（extrinsic_est_en）并不改变噪声参数。噪声参数是滤波器的固定超参数，需要在运行前设定。即使开启外参在线估计，IMU噪声参数仍然需要通过离线标定或经验值确定。

此外，FAST-LIO中没有对噪声参数进行自适应调整的机制。一旦噪声参数偏离真实值，滤波器无法自动修正。因此，花一些时间在实际硬件上标定噪声参数是有价值的。

对于追求极限性能的用户，建议使用专门的IMU标定工具（如imu_utils、kalibr）精确计算噪声密度和随机游走参数，然后按上述单位换算填入配置文件。

参考资料

• FAST-LIO论文及代码仓库：hku-mars/FAST_LIO

• imu_utils：https://github.com/gaowenliang/imu_utils

• 《视觉SLAM十四讲》第9章：误差状态卡尔曼滤波的噪声模型

• CSDN博客《FAST-LIO参数配置与调试经验》：提供了各型号IMU的推荐参数值