ROS 2 Humble 多传感器联合标定模块设计与实施文档

适用平台ROS 2 Humble、C17覆盖传感器机械式/固态 LiDAR、RGB/RGB-D 相机、毫米波雷达、IMU、GNSS工程包multi_sensor_calibration主标定文件config/master_calibration.yaml1. 目标与边界该模块不替代 Kalibr、OpenCalib、厂商标定程序或专业优化器而是负责把各类标定工具产生的结果统一管理并完成上线前的自动检查统一保存相机内参、传感器外参和质量状态从主标定文件发布静态 TF检查各传感器时间戳、数据超时和相对时间偏差自动评估 LiDAR–LiDAR / RGB-D 点云的地面高度一致性将 LiDAR 点投影到相机图像生成可视化叠加图根据高分辨率主内参生成裁剪、缩放后的运行分辨率CameraInfo导出可写回 URDF/Xacro 的 fixed-joint 片段形成可重复执行的采集、标定、验证和验收流程。模块按以下原则工作先时间同步 ↓ 再单传感器内参/噪声标定 ↓ 再求外参 ↓ 多帧、多个区域验证 ↓ 写入 master_calibration.yaml ↓ 导出 URDF / static TF ↓ 导航、建图和感知闭环验证2. 对参考标定文档的继承与改进参考文档采用“16 线机械 LiDAR 为基准手工修正固态 LiDAR 与多路 RGB-D 外参”的路线。该路线中的关键经验被保留从 URDF 中读取外参初值将不同传感器点云按时间戳配对最低帧率为 10 Hz 时使用0.1 s作为人工选帧上限在 CloudCompare 中给点云设置不同颜色并应用初始变换在多个地面位置比较两组点云的z值将相邻地面点高度差控制在5 cm以内得到结果后更新 URDF并再次通过 TF 检查多路 RGB-D 使用同一参考 LiDAR检查各相机点云地面高度的一致性。本工程在此基础上增加了以下能力原人工步骤工程化实现手工寻找最近时间戳 PCDselect_nearest_timestamp_pairs.py人工选几个地面点比较 zpointcloud_alignment_validator_node网格化统计只验证单帧ROS 2 在线多帧连续验证容易混淆 TF 方向主文件强制parent - child约定直接修改 URDF先写主标定文件再导出 URDF 片段仅看点云重合同时输出 MAE、RMSE、P95 和匹配网格数相机外参只看 RGB-D 点云增加 LiDAR 到图像投影叠加分辨率变化后手工改内参自动派生CameraInfo2.1 TF 方向必须统一ROS 2 命令ros2 run tf2_ros tf2_echotarget_framesource_frame表示查询source_frame中的点如何变换到target_frame。例如ros2 run tf2_ros tf2_echo laser_link livox1_link查询的是T_laser_livox1而master_calibration.yaml保存的是与 URDF fixed joint 一致的T_parent_child两者不一定是同一个直接变换。禁止把tf2_echo输出不加判断地写入任意origin。应先确认 TF 树路径和变换方向。3. 软件架构multi_sensor_calibration │ ├── calibration_manager_node │ ├── 加载 master_calibration.yaml │ ├── 校验 frame、内参和数值合法性 │ ├── 发布已启用的 static TF │ ├── /reload │ └── /export_urdf │ ├── timestamp_monitor_node │ ├── 检查消息是否超时 │ ├── 检查时间戳倒退 │ └── 检查相对参考传感器的最新时间偏差 │ ├── pointcloud_alignment_validator_node │ ├── 近似时间同步两路 PointCloud2 │ ├── 将目标点云转换到参考坐标系 │ ├── XY 网格中提取最低 z 作为地面高度 │ └── 输出 MAE / RMSE / P95 / PASS │ ├── lidar_camera_projection_validator_node │ ├── 同步图像与点云 │ ├── 读取 CameraInfo │ ├── TF 变换到相机光学坐标系 │ └── 发布深度着色投影图和诊断 │ └── camera_info_deriver_node ├── 读取高分辨率 master intrinsics ├── 应用 crop resize ├── 发布派生 CameraInfo └── 保存标准 camera_info YAML4. 主标定文件4.1 外参定义extrinsic:parent_frame:base_linkchild_frame:livox1_linkxyz:[x,y,z]rpy:[roll,pitch,yaw]publish_static_tf:false含义与 URDF 一致jointtypefixedparentlinkbase_link/childlinklivox1_link/originxyzx y zrpyroll pitch yaw//joint所有角度单位均为弧度。在标定未通过前保持publish_static_tf:false避免样例值或粗测值覆盖机器人现有 TF。4.2 相机内参建议只保存一次最高分辨率、未裁剪传感器模式的主内参intrinsics:frame_id:rgbd1_color_optical_framewidth:1280height:720distortion_model:plumb_bobd:[...]k:[...]r:[...]p:[...]推荐流程原始高分辨率完成一次完整内参和外参标定保存为master_calibration.yaml每种运行分辨率生成派生CameraInfo分辨率变化时外参保持不变每种分辨率运行投影验证误差明显变大时检查裁剪、binning、ISP sensor mode再决定是否重标内参。5. 编译cdsensor_driver_factory_wssource/opt/ros/humble/setup.bash rosdepinstall\--from-paths src\--ignore-src\-r-ycolcon build\--symlink-install\--cmake-args-DCMAKE_BUILD_TYPEReleasesourceinstall/setup.bash6. 标定前检查6.1 坐标系定义建议固定base_link: x 前 y 左 z 上 camera optical frame: x 右 y 下 z 前明确以下 framebase_link laser_link livox1_link rgbd1_color_optical_frame front_radar_link imu_link检查 TF 树ros2 run tf2_tools view_frames ros2 run tf2_ros tf2_echo base_link laser_link ros2 run tf2_ros tf2_echo laser_link livox1_link6.2 时间戳来源逐个确认LiDAR设备时钟、PTP 或主机接收时间相机曝光开始、曝光结束或主机回调时间IMU设备采样时钟RadarCAN/以太网帧时间GNSSPPS/GNSS timeheader.stamp是否在驱动中被二次覆盖。先运行时间监测ros2 run multi_sensor_calibration timestamp_monitor_node\--ros-args\--params-file install/multi_sensor_calibration/share/\multi_sensor_calibration/config/timestamp_monitor.yaml标定数据建议静态 bag验证时间戳稳定、TF、噪声和静态点云动态 bag验证时间偏移对投影和点云重合的影响相机–IMU 标定必须有充分的旋转与平移激励LiDAR–IMU 标定必须包含多方向运动避免仅直线行驶。录制示例ros2 bag record\/rslidar_points\/livox/lidar\/rgbd_filter_1\/sensing/camera/front/image_raw\/sensing/camera/front/camera_info\/imu/data\/tf\/tf_static7. LiDAR–LiDAR / RGB-D 点云标定7.1 人工 CloudCompare 路线安装sudoaptinstallcloudcompare操作顺序使用最近时间戳配对工具找帧将参考点云和目标点云导入 CloudCompare设置不同颜色使用 TF/URDF 外参作为初值应用目标点云到参考坐标系的变换从正视、侧视和俯视三个方向检查在近、中、远以及左右区域检查地面高度微调 roll/pitch再调整 z最后调整 yaw/x/y至少使用多个时刻重复检查将结果写回主标定文件而非直接覆盖生产 URDF。最近时间戳配对python3\src/multi_sensor_calibration/tools/select_nearest_timestamp_pairs.py\/data/lidar\/data/livox_front\--max-diff0.1\--output/data/lidar_livox_pairs.csv7.2 自动地面网格验证启动ros2 run multi_sensor_calibration\pointcloud_alignment_validator_node\--ros-args\--params-file install/multi_sensor_calibration/share/\multi_sensor_calibration/config/pointcloud_alignment.yaml算法参考点云 ───────────────┐ ├─ ROI 过滤 目标点云 ─ TF到参考系 ──┘ ↓ XY 划分网格 ↓ 每个网格取最小 z 近似地面 ↓ 比较相同网格的高度差 ↓ MAE / RMSE / P95 / matched_cells默认验收timestamp skew 0.1 s matched_cells 20 P95(|Δz|) 0.05 mP95 5 cm是对参考文档“多个位置地面 z 差控制在 5 cm 内”的工程化实现。实际阈值应根据雷达量程、地面平整度、时间同步和业务需求重新标定。7.3 不能只看地面只看地面主要约束rollpitchz。还必须利用立柱、墙角、路缘、标定板等非共面结构约束yawxy。推荐最终场景同时包含平整地面 垂直墙面 立柱/墙角 远近目标8. LiDAR–Camera / RGB-D–LiDAR 标定8.1 标定方法可选标定板/AprilTag/ChArUco LiDAR 平面或边缘专用 LiDAR–Camera 标定工具初值来自机械测量与 URDF最后用在线投影节点验证。8.2 投影验证投影节点假设输入为去畸变后的图像ros2 run multi_sensor_calibration\lidar_camera_projection_validator_node\--ros-args\--params-file install/multi_sensor_calibration/share/\multi_sensor_calibration/config/lidar_camera_projection.yaml输出/lidar_camera_projection_validator/overlay /lidar_camera_projection_validator/diagnostics在 RViz2 或图像查看器中检查墙面点是否落在墙面门框、柱子、路缘等边缘是否重合近、中、远距离是否都一致图像中心正确但边缘偏移优先检查内参和畸变全图同方向平移优先检查外参平移一侧对齐另一侧不齐优先检查旋转运动物体或转弯时错位优先检查时间同步。注意节点中的in_bounds_ratio只用于判断数据链路是否可投影不代表外参精度。最终精度必须通过叠加图和目标边缘误差判断。9. 相机分辨率变化与 CameraInfo 派生对于主图像裁剪区域crop (crop_x, crop_y, crop_width, crop_height) target (target_width, target_height)缩放系数sx target_width / crop_width sy target_height / crop_height派生内参fx fx × sx fy fy × sy cx (cx - crop_x) × sx cy (cy - crop_y) × sy启动ros2 launch multi_sensor_calibration\camera_info_deriver.launch.py\calibration_file:/path/master_calibration.yaml\camera_name:rgbd1\target_width:640\target_height:480保存派生文件ros2servicecall\/camera_info_deriver/save\std_srvs/srv/Trigger{}以下情况不能只做比例换算运行模式使用不同传感器读出区域ISP 做了数字防抖或几何矫正binning 与 skipping 规则未知图像经过非等比例缩放驱动自动裁剪但未提供 ROI深度和彩色图使用不同的对齐模式。10. Camera–IMU 与 LiDAR–IMU10.1 Camera–IMU推荐流程相机内参先标定IMU 采样率和噪声参数先确认录制有充分六自由度激励的数据使用 Kalibr 类工具估计外参和时间偏移将结果转换为 ROS frame 定义写入主标定文件在 VIO 中检查静止漂移和快速转动稳定性。常见失败原因运动只有平移、没有旋转画面模糊rolling shutter 未建模IMU 时间戳由主机到达时间代替IMU 单位或轴向错误相机光学 frame 与机身 frame 混用。10.2 LiDAR–IMU推荐静态数据估计 IMU 偏置和噪声动态数据包含绕三个轴旋转使用 LiDAR–IMU 标定工具或 LIO 联合优化通过 FAST-LIO2 等系统检查地图重影检查急转弯和颠簸场景。验收重点静止点云不出现周期摆动快速转弯时墙面不分层长直线与回环后地图不明显错层重力方向与base_linkz 轴一致。11. Radar–LiDAR / Radar–Camera毫米波雷达角分辨率较低建议使用角反射器大型金属平面多个已知位置静态目标低速直线运动目标用于速度方向检查。Radar–LiDAR 流程选择稳定雷达目标 ↓ LiDAR 中提取对应反射器/车辆 ↓ 初值来自机械安装 ↓ 优化 yaw translation ↓ 多距离、多方位重复验证验收近、中、远距离目标位置一致左右目标无系统偏差速度正负方向正确Radar frame 到base_link的轴向定义正确不能用单个正前方目标同时确定所有自由度。12. 标定结果管理12.1 修改与发布编辑master_calibration.yaml校验python3\src/multi_sensor_calibration/tools/validate_master_calibration.py\src/multi_sensor_calibration/config/master_calibration.yaml标定通过后将对应项设为publish_static_tf:true启动ros2 launch multi_sensor_calibration\calibration_manager.launch.py\calibration_file:/path/master_calibration.yaml热加载ros2servicecall\/calibration_manager/reload\std_srvs/srv/Trigger{}静态 TF 在生产系统中不应频繁更新。热加载主要用于调试最终仍建议重启相关节点并完整验证。12.2 导出 URDFros2servicecall\/calibration_manager/export_urdf\std_srvs/srv/Trigger{}默认输出/tmp/calibration_fixed_joints.urdf.xacro确认后再合并到机器人描述包。不要让标定管理器和robot_state_publisher同时发布同名 child frame 的不同静态 TF。13. 验收清单13.1 数据与时间每个话题header.stamp来源明确静态 rosbag 无时间戳倒退动态 rosbag 的配对偏差满足要求PTP/PPS/硬件同步状态已记录运动场景中无明显时间错位。13.2 相机每种原始 sensor mode 完成内参验证重投影误差满足项目要求派生分辨率投影验证通过camera_info.frame_id为光学坐标系RGB 与深度对齐模式明确。13.3 LiDAR–LiDAR / RGB-D 点云时间差不超过配置阈值多帧 P95 地面高度差通过地面、墙面和立柱同时对齐近、中、远区域均检查点云无由运动畸变造成的假错位。13.4 LiDAR–Camera点云可稳定投影到图像中心与边缘都对齐静态和动态场景均检查分辨率变化后重新验证相机畸变模型与图像类型一致。13.5 IMU 与系统闭环IMU 轴向、单位和重力方向正确VIO/LIO 无明显地图重影急转弯时融合稳定URDF 与主标定文件一致导航、障碍物投影、语义融合闭环通过。14. 建议的版本记录每次标定保存calibration_id 机器人序列号 传感器型号与序列号 安装状态/是否拆装 标定日期 数据包路径和 SHA256 使用工具及版本 内参和外参 时间偏移 验证指标 操作人和审核人标定文件建议纳入 Git但原始 rosbag/PCD 使用对象存储或数据版本管理系统保存。15. 当前实现限制点云验证采用最低 z 网格近似地面不是完整 ICP投影节点假设图像已去畸变不在节点内应用畸变模型timestamp_monitor_node比较的是各话题最新时间戳不替代离线相关性时间偏移估计Radar 只提供流程和统一外参管理未实现特定厂商目标关联优化IMU 外参求解仍需外部标定器示例master_calibration.yaml为安全占位值默认不发布 TF当前环境未执行 ROS 2colcon build上线前必须在 Humble 环境编译和 rosbag 回放验证。16. 快速启动顺序# 1. 校验主文件python3 src/multi_sensor_calibration/tools/validate_master_calibration.py\src/multi_sensor_calibration/config/master_calibration.yaml# 2. 启动主标定与验证节点ros2 launch multi_sensor_calibration full_sensor_calibration.launch.py\calibration_file:/path/master_calibration.yaml# 3. 查看诊断ros2 topicecho/calibration_manager/diagnostics ros2 topicecho/timestamp_monitor/diagnostics ros2 topicecho/pointcloud_alignment_validator/diagnostics ros2 topicecho/lidar_camera_projection_validator/diagnostics# 4. 查看投影图ros2 run rqt_image_view rqt_image_view\/lidar_camera_projection_validator/overlay# 5. 验证后导出 URDFros2servicecall /calibration_manager/export_urdf\std_srvs/srv/Trigger{}