多传感器融合定位实战：基于KITTI数据集构建100Hz IMU与相机、激光雷达的滤波融合数据平台

1. 从KITTI数据集到多传感器融合平台为什么需要100Hz IMU在自动驾驶和机器人定位领域KITTI数据集就像是一块瑞士军刀般的存在。这个来自德国卡尔斯鲁厄理工学院和丰田芝加哥技术研究院的开放数据集包含了丰富的城市道路场景传感器数据。但很多朋友在实际使用时会发现原始数据中的IMU频率只有10Hz——这就像用老式机械键盘玩电竞游戏按键响应根本跟不上操作需求。我去年在做一个园区物流车项目时就深有体会。当车辆以30km/h行驶时10Hz的IMU数据意味着每0.8米才能获取一次姿态更新。在急转弯场景下这样的采样率会导致EKF滤波器出现明显的预测滞后。后来我们把IMU频率提升到100Hz后定位轨迹的平滑度立即提升了62%。这就是为什么我们需要对原始KITTI数据进行改造。KITTI的extract版本其实已经包含了100Hz的IMU原始数据但存在两个关键问题一是没有与相机数据时间对齐二是未做去畸变处理。这就好比你有高清摄像头和精密陀螺仪但两者的时钟不同步拍出来的画面和运动轨迹对不上。接下来我要分享的这套方法就是解决这些痛点的完整方案。2. 数据准备与环境搭建2.1 获取正确的数据集版本首先需要登录KITTI官网找到2011_10_03_drive_0027这个经典场景的extract版本数据。注意要下载的是raw_data而非sync版本因为前者包含100Hz的IMU数据。我建议直接使用wget命令批量下载wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_10_03_drive_0027/2011_10_03_drive_0027_extract.zip解压后会看到oxts文件夹这里面就是关键的IMU原始数据。有趣的是KITTI的IMU实际上采用了OXTS RT3003惯性导航系统这个设备本身支持100Hz输出但在生成sync版本时被人为降采样到了10Hz。2.2 Python环境配置由于后续脚本需要用到Python2建议使用conda创建独立环境conda create -n kitti_py2 python2.7 conda activate kitti_py2 pip install numpy pykitti rospkg这里有个坑要注意最新版的pykitti可能不兼容Python2可以指定安装0.3.1版本pip install pykitti0.3.13. 数据预处理实战3.1 时间戳修复与数据重组解压后的extract和sync数据需要合并处理。具体操作是将extract中的oxts重命名为oxts_extract将sync中的oxts重命名为oxts_sync运行时间戳修复脚本这个步骤就像把两个不同时区的时钟校准到同一时间基准。GEYAO大神提供的scripts.py脚本会自动处理时间跳变问题。运行命令如下python2 scripts.py -i 2011_10_03_drive_0027_sync脚本会在后台完成三件重要工作解析GPS周秒时间格式补偿IMU数据的时间偏移生成新的oxts文件夹3.2 ROS bag文件生成接下来使用修改版的kitti2bag工具生成ROS bag文件。这个步骤有几个关键修改点修改了imu消息的时间戳处理逻辑增加了100Hz数据的发布配置修复了点云数据的坐标系问题运行命令如下python2 kitti2bag.py -t 2011_10_03 -r 0027 raw_synced生成的bag文件大约有24GB包含了所有传感器数据。这里有个性能优化技巧如果磁盘空间紧张可以先用rosbag compress进行压缩能减少40%存储空间。4. 数据精修与融合4.1 话题筛选与清理原始bag文件包含大量冗余话题我们需要使用rosbag_filter_gui工具进行清理。安装方法sudo apt install python-qt4 git clone https://github.com/AtsushiSakai/rosbag_filter_gui.git重点保留以下话题/kitti/oxts/imu/kitti/velo/pointcloud/kitti/camera_color_left/image_raw/kitti/oxts/gps/fix特别注意要移除/tf_static话题否则会影响后续的时间同步。这个步骤就像给数据瘦身把不需要的信息剔除掉。4.2 数据合并与频率验证最后使用merge_bags.py脚本合并处理后的bag文件。合并前建议先用rostopic hz检查数据频率rostopic hz /kitti/oxts/imu/extract正常应该看到类似输出average rate: 100.011 min: 0.008s max: 0.012s合并命令示例python2 merge_bags.py final_merged.bag synced_filtered.bag extracted_filtered.bag合并后的数据集就包含了100Hz的IMU数据10Hz的GPS定位10Hz的激光雷达点云10Hz的相机图像这种多频率传感器数据的融合正是扩展卡尔曼滤波等算法的理想输入。在实际项目中我建议先用这个数据集验证算法可行性再迁移到真实硬件平台能节省大量调试时间。