3、传感器噪声与滤波：IMU噪声模型、GPS跳变、Lidar点云抖动、传感器融合中的时间戳对齐

传感器噪声，说白了就是轨迹抖动的万恶之源。我做了这么多年自动驾驶，见过太多因为传感器没处理好导致轨迹像“醉汉走路”的案例。这一章咱们就把几个核心传感器的噪声问题掰开揉碎，看看怎么治它们。

3.1 IMU噪声模型：别被“高频率”骗了

IMU这东西，频率高（200Hz+），但噪声也大。很多人觉得IMU数据“实时”，就直接拿来积分算位姿，结果几秒后就漂到天边去了。

IMU的噪声主要分两类：

• 白噪声（White Noise）：高频随机波动，像收音机里的沙沙声。这个好办，低通滤波就能压下去。
• 随机游走（Random Walk / Bias Instability）：零偏缓慢漂移，这才是要命的。你想想看，加速度计零偏如果漂了0.01 m/s²，积分10秒就是0.5米的位移误差。

我个人习惯用Allan方差来标定IMU的噪声参数。拿到一个IMU芯片，先让它静止采集1小时数据，画Allan方差曲线，读出角度随机游走（ARW）和速率随机游走（RRW）。

# 伪代码：Allan方差计算核心
def allan_variance(data, tau):
    # data: 静止采集的角速度/加速度序列
    # tau: 积分时间窗口
    N = len(data)
    n = int(tau / dt)  # 窗口内的样本数
    # 分段平均
    segments = [np.mean(data[i:i+n]) for i in range(0, N-n, n)]
    # 计算相邻段差值的方差
    var = 0.5 * np.var(np.diff(segments))
    return var

# 实际项目中，我会用这个曲线去拟合噪声参数
# 斜率 -0.5 段 → 白噪声强度
# 斜率 +0.5 段 → 随机游走系数

3.2 GPS跳变：城市峡谷里的“幽灵”

GPS跳变，我遇到过太多次了。明明车在直道上走，GPS突然往左跳了2米，轨迹直接画了个“之”字。为什么会这样？

• 多路径效应：高楼反射信号，伪距测量出现几米到几十米的误差。
• 卫星几何变化：城市峡谷里卫星被遮挡，PDOP值飙升，定位精度从2米掉到10米。
• 接收机状态切换：从RTK固定解掉到浮点解，或者从3D定位掉到2D定位，跳变幅度能到几米。

我建议的做法是：

1. 速度/加速度门限检测：如果GPS位置变化对应的速度超过物理极限（比如 > 20 m/s²），直接标记为异常。
2. 结合IMU做预测校验：用IMU积分预测当前位置，如果GPS观测值与预测值偏差超过阈值（比如5米），降低该观测的权重。
3. 使用载波相位平滑伪距：这个能有效抑制多路径噪声，但需要接收机支持原始观测量输出。

// C++ 伪代码：GPS跳变检测
struct GPSMeasurement {
    double lat, lon, alt;
    double speed;       // 地面速度 m/s
    int fix_type;       // 0=无, 1=单点, 2=差分, 4=RTK固定, 5=RTK浮点
    double pdop;
};

bool isGPSJump(const GPSMeasurement& curr, const GPSMeasurement& prev, double dt) {
    double distance = haversineDistance(curr, prev);
    double implied_speed = distance / dt;
    // 物理约束：正常车辆加速度 < 5 m/s²
    if (implied_speed > 30.0) {  // 108 km/h 以上跳变
        return true;
    }
    // 固定解→浮点解 切换时特别警惕
    if (prev.fix_type == 4 && curr.fix_type <= 2) {
        return true;
    }
    // PDOP 突变
    if (curr.pdop > 3.0 && curr.pdop > 2 * prev.pdop) {
        return true;
    }
    return false;
}

3.3 Lidar点云抖动：机械旋转的“原罪”

Lidar点云抖动，主要是机械旋转结构带来的。16线、32线、64线，只要是旋转式的，就逃不掉这个问题。

抖动来源：

抖动类型	原因	典型幅度	频率特征
电机转速波动	电机控制不稳，导致扫描角度不均匀	0.1~0.5°	与转速基频相关
振动耦合	车辆振动传递到Lidar，导致点云整体晃动	1~5 cm	与路面激励相关
时间戳抖动	每个点的时间戳不准确，导致运动畸变	0.5~2 ms	随机分布
反射面干扰	高反物体（玻璃、车漆）导致测距跳变	5~20 cm	空间局部

处理Lidar点云抖动，我一般分三步走：

1. 运动畸变补偿：利用IMU或轮速计，给每个点赋予精确的时间戳，然后补偿到统一坐标系下。
2. 点云去畸变：对于旋转式Lidar，用匀速运动模型或IMU辅助，把一帧内的点都变换到起始时刻。
3. 离群点滤波：用半径滤波或统计滤波，剔除那些明显偏离局部点云分布的点。

// 运动畸变补偿核心逻辑
// 假设一帧点云的时间范围 [t_start, t_end]
// 每个点有原始时间戳 t_i
// 利用IMU积分得到 t_i 时刻的位姿 T_i
for (auto& point : cloud.points) {
    double t_i = point.timestamp;
    // 插值得到 t_i 时刻的位姿
    Eigen::Isometry3d T_i = interpolatePose(t_start, t_end, t_i);
    // 将点变换到 t_start 时刻
    Eigen::Vector3d p_compensated = T_i.inverse() * point.position;
    point.position = p_compensated;
}

3.4 时间戳对齐：传感器融合的“命门”

时间戳对齐，说白了就是让所有传感器说“同一时间的话”。我见过太多融合算法写得漂亮，但时间没对齐，结果轨迹抖得没法看。

常见的对齐方式：

• 硬件同步（Hardware Sync）：通过PPS信号或触发线，让多个传感器在同一时刻采集数据。精度最高（微秒级），但需要硬件支持。
• 软件插值（Software Interpolation）：以主传感器（通常是IMU或Lidar）的时间为基准，对其他传感器数据进行线性插值或样条插值。精度取决于传感器频率和运动剧烈程度。
• 时间戳修正（Timestamp Correction）：有些传感器（如USB摄像头）的时间戳可能延迟或抖动，需要先做时间戳校准。

// 时间戳对齐示例：以IMU时间为基准，插值GPS数据
struct TimedPose {
    double timestamp;
    Eigen::Vector3d position;
};

TimedPose interpolateGPS(const std::vector<TimedPose>& gps_data, double t_target) {
    // 找到 t_target 前后的两个GPS观测
    auto it = std::lower_bound(gps_data.begin(), gps_data.end(), t_target,
                               [](const TimedPose& p, double t) { return p.timestamp < t; });
    if (it == gps_data.begin() || it == gps_data.end()) {
        return *it;  // 边界情况直接返回
    }
    auto prev = it - 1;
    auto next = it;
    double alpha = (t_target - prev->timestamp) / (next->timestamp - prev->timestamp);
    TimedPose result;
    result.timestamp = t_target;
    result.position = prev->position + alpha * (next->position - prev->position);
    return result;
}

3.5 本章知识体系

下面这张图把传感器噪声与滤波的核心逻辑串起来了。你想想看，从原始传感器数据到最终平滑轨迹，中间要过多少关？