4. 传感器数据冲突识别：多传感器读数不一致、时间戳对齐、初步过滤

多传感器融合是机器人感知的基础，但不同传感器在时序、精度和噪声特性上存在固有差异。当同一物理量（如距离、速度、温度）被多个传感器测量时，读数差异直接导致状态估计错误和路径规划冲突。本章沿问题排查主线，系统识别数据冲突的根源、对齐时间戳差异，并实施初步过滤。

4.1 现象识别：多传感器读数不一致的具体表现

观察以下典型冲突现象，区分瞬时噪声和系统性偏差：

4.2 数据采集：获取对齐后的原始读数

在采集环节必须记录 硬件时间戳（传感器本地时钟）和 接收时间戳（ROS/系统到达时间），两者之差用来评估传输延迟。以下命令启动记录同时包含两种时间戳的 bag 文件：

# 启用 ROS time_synchronizer，记录完整元数据
rosbag record /sensor/lidar/points /sensor/camera/depth /sensor/imu/data_raw
  --duration 60 --output-name=sensor_conflict_raw.bag

提取指定时间窗口内的传感器读数，并输出为 csv 用于离线对齐：

# 使用 rostopic echo 管道提取时间戳和值
rostopic echo /sensor/imu/data_raw -b sensor_conflict_raw.bag
  --filter 'm.header.stamp.secs > 1650000000 and m.header.stamp.nsecs < 500000000'
  | grep -E "linear_acceleration|angular_velocity" > imu_segment.csv

4.3 原因定位：时间戳对齐与一致性校验

使用时钟域转换和时间戳插值双重方法定位冲突：

1. 时钟域校准：检测传感器是否使用同一系统时钟。若使用独立时钟，需计算偏移量Δt。
2. 数据帧关联：对慢速传感器（如激光雷达 10 Hz）的每个数据帧，在快速传感器（如 IMU 100 Hz）中取最近的前后两帧做线性插值对齐。
3. 一致性评分：计算对齐后残差的 RMS < 标称精度阈值；若超标则标记为冲突点。

检查日志中的时序异常示意：

[WARN] [1650500102.345] Sync latency out of bound: lidar timestamp 1650500102.345, imu timestamp 1650500102.450, delta = 105 ms > threshold 50 ms
[INFO] [1650500102.500] Aligned pair: lidar (t=1650500102.345) imu (t=1650500102.498), interpolation OK

4.4 修复动作：时间戳对齐与初步过滤

采用 软同步（插值对齐） 和 硬同步（硬件触发） 两种方式。软同步适用于异构传感器系统，硬同步需硬件支持。以下 Python 代码执行线性插值对齐 IMU 与激光雷达时间戳：

import numpy as np

def align_sensors(ts_a, val_a, ts_b, val_b):
    """
    对齐传感器B到A的时间戳，使用线性插值
    ts_a: 传感器A时间戳数组（一维）
    val_a: 传感器A测量值
    ts_b: 传感器B时间戳数组
    val_b: 传感器B测量值
    返回 aligned_b: 在ts_a时刻的插值后传感器B值
    """
    aligned_b = np.interp(ts_a, ts_b, val_b)
    return aligned_b

初步过滤条件（基于传感器物理特性）：

4.5 复测：验证对齐与过滤后的数据质量

复测包含两个层面：时间戳对齐精度和数据值一致性。执行以下检查清单：

• ✅ 对齐后残差标准差 ≤ 2×传感器采样间隔
• ✅ 经过初步过滤后，异常点占比 < 3%
• ✅ 人工目视或自动对比：对齐后两传感器在平坦地面的测距偏差 < 标称精度之和
• ✅ 插值后时间戳单调递增，无回退

复测可通过自动化脚本批量运行：

python verify_alignment.py \
  --input bag_files/sensor_conflict_raw.bag \
  --method linear_interpolation \
  --max_delta_ms 50 \
  --max_outlier_ratio 0.03

4.6 预防：构建传感器时间同步与数据质量监控机制

1. 硬件层：优先使用硬件触发（如 PTP/1588），将所有传感器同步到同一网络时钟。
2. 软件层：建立时间戳偏移监控器，每次启动自动校准偏移量。
3. 配置层：在传感器驱动中增加时间戳完整性检查，丢弃时间戳缺失或明显错乱的数据包。
4. 制度层：传感器标定与时间戳对齐作为每次系统上电后的必检步骤，记录对齐误差日志。