2、抖动成因分析（上）：传感器噪声模型与时间同步问题

做机器人定位这么多年，我见过最头疼的问题就是轨迹抖动。明明算法看起来没问题，跑起来却像喝醉了酒。说白了，抖动的根源就藏在传感器数据里。今天咱们就掰开揉碎了聊——传感器噪声、采样率不匹配、时间戳同步，这三个坑是怎么把轨迹搞抖的。

2.1 传感器噪声模型：每个传感器都有自己的脾气

先说说噪声。每个传感器都有天生的缺陷，这些缺陷会直接反映在轨迹上。我习惯把噪声分成两类：白噪声和偏置噪声。白噪声是高频随机波动，偏置噪声是低频漂移。两者叠加，就是传感器输出的真实样子。

2.1.1 IMU 噪声模型

IMU 是抖动的主要贡献者。加速度计和陀螺仪的噪声模型通常写成这样：

# IMU 测量模型（简化版）
a_m = a_true + b_a + n_a + g        # 加速度计
w_m = w_true + b_g + n_g            # 陀螺仪

# 其中：
# b_a, b_g : 偏置（随机游走）
# n_a, n_g : 白噪声（高斯分布）
# g        : 重力分量（需要分离）

我在项目中遇到过最典型的场景：IMU 刚上电时偏置稳定，但随着温度升高，偏置开始缓慢漂移。如果不做在线估计，积分出来的姿态就会慢慢偏掉，轨迹自然就抖了。

2.1.2 里程计噪声模型

轮式里程计的噪声主要来自打滑和地面不平。模型可以写成：

# 轮式里程计测量模型
v_m = v_true + n_v + s_v * v_true   # 速度测量
w_m = w_true + n_w + s_w * w_true   # 角速度测量

# n_v, n_w : 白噪声
# s_v, s_w : 尺度因子误差（打滑导致）

嗯，这里要注意：里程计的噪声不是纯高斯分布。打滑时会出现尖峰噪声，也就是突然跳变。这种噪声用卡尔曼滤波很难处理，我一般会用鲁棒核函数来抑制。

2.1.3 激光雷达噪声模型

激光雷达的噪声和距离、反射率、入射角都有关系。模型比较复杂：

# 激光雷达测距噪声模型
r_m = r_true + n_r + b_r(r_true, θ, ρ)

# n_r : 高斯白噪声（σ ≈ 1-3 cm）
# b_r : 系统偏差（与距离、角度、反射率相关）
# θ   : 入射角
# ρ   : 目标反射率

我记得有一次做 AGV 定位，激光雷达在玻璃墙前面直接失效——反射率太低，测距噪声飙到 10 cm 以上。轨迹当场就抖成筛子。后来加了反射率置信度权重，才稳住。

2.1.4 视觉噪声模型

视觉 SLAM 的噪声来源更复杂。特征点提取、匹配误差、相机标定残差，都会贡献到轨迹抖动里。

# 视觉重投影误差模型
e = p_obs - π(P, T_cw) + n_p

# p_obs : 观测到的像素坐标
# π     : 投影函数
# P     : 3D 点坐标
# T_cw  : 相机位姿
# n_p   : 像素噪声（σ ≈ 0.5-2 pixel）

2.2 数据采样率不匹配：多传感器融合的隐形杀手

采样率不匹配，说白了就是传感器们各说各话。IMU 通常 100-1000 Hz，激光雷达 10-20 Hz，相机 10-30 Hz，里程计 50-100 Hz。融合的时候，高频传感器要预测，低频传感器要更新。这个过程中，如果处理不当，就会引入抖动。

我画了一张图，展示多传感器融合时的采样率关系：

你想想看，IMU 每 5 ms 预测一次位姿，但激光雷达每 100 ms 才来一次观测。这 100 ms 里，IMU 积分误差一直在累积。如果激光雷达的观测时间戳不准，或者观测本身有噪声，融合后的轨迹就会在每次更新时出现跳变。

2.3 时间戳同步误差：毫秒级的偏差，厘米级的抖动

时间戳同步，说白了就是让所有传感器用同一个时钟。但实际工程中，这事没那么简单。

我遇到过最坑的情况：IMU 用 ROS 时间戳，激光雷达用硬件时间戳，相机用系统时间戳。三个时间戳的基准不一样，偏差能达到 10-50 ms。对于高速运动的机器人，50 ms 的时间偏差意味着位置偏差可能达到 10 cm 以上。

时间戳同步误差的数学模型：

# 时间戳同步误差模型
t_sensor = t_true + Δt_sync + n_t

# t_true    : 真实时间
# Δt_sync   : 同步偏差（固定偏移 + 漂移）
# n_t       : 时间戳噪声（抖动）

# 同步偏差对轨迹的影响：
# 位置误差 ≈ v * Δt_sync
# 姿态误差 ≈ ω * Δt_sync

# 例如：v = 2 m/s, Δt_sync = 20 ms
# 位置误差 = 2 * 0.02 = 0.04 m = 4 cm

2.4 三种成因的叠加效应

传感器噪声、采样率不匹配、时间戳同步误差，这三个问题很少单独出现。它们会叠加在一起，让轨迹抖动变得非常复杂。

我总结了一个表格，方便你快速定位抖动来源：

抖动特征	可能成因	排查方法
高频随机抖动（> 50 Hz）	IMU 白噪声、视觉像素噪声	Allan 方差分析、频谱分析
低频漂移（< 1 Hz）	IMU 偏置漂移、里程计打滑	在线偏置估计、打滑检测
周期性抖动（固定频率）	采样率不匹配	检查传感器更新频率
突发跳变（非周期）	时间戳同步异常、传感器丢帧	检查时间戳连续性、丢帧率
复合抖动（多种特征混合）	多种成因叠加	逐一隔离测试