这段时间在折腾松灵 Scout mini 底盘上的 3D 建图传感器组合是禾赛 32 线雷达、Xsens MTi-G-710 组合导航模块算法端用 Cartographer 跑 3D 模式。算下来从驱动编译到第一张能用的地图前后花了一周多中间踩了不少坑。很多坑不是算法本身的问题而是坐标系、时间戳、外参和调参的配合问题。这篇文章我把这套系统从硬件连接、驱动安装、TF 配置、Cartographer 3D 参数调整到实际操作流程完整梳理一遍也会把常见报错和排查思路写出来希望能帮到正在用类似底盘和传感器做 Cartographer 建图的同学。1. 整体设计与系统组成1.1 为什么选这一套组合方案先说选型逻辑。松灵 Scout mini 是一台四轮差速 UGV自带电机编码器和官方 ROS 驱动能直接输出轮式里程计和接收速度指令作为移动平台非常省心。但它没有姿态传感器轮式里程计在草地、坡道、打滑场景下会越来越飘所以需要 IMU 做姿态参考和运动预测。Xsens MTi-G-710 是组合导航模块内部有 MEMS 级 IMU 加 GNSS室外能输出经纬度、速度、姿态室内只靠 IMU 也能输出高频率的姿态和角速度用来辅助 Cartographer 建图刚好。激光雷达选了禾赛 32 线这个线数在建设施内外部都够用。16 线太疏在比较远的地方特征不够64 线数据量太大对工控机和 Jetson 的压力偏高。32 线在点云密度和性能之间比较平衡加上禾赛驱动比较成熟点云话题可以直接接到 Cartographer。算法端选择 Cartographer而不是 LOAM、LIO-SAM 那一路原因是 Cartographer 是子图加回环检测的图优化框架对多传感器融合的支持更直接而且能通过配置区分 2D/3D 模式后续如果要转导航还能直接导出 2D 占用栅格地图。整体上这套组合属于“底盘 雷达 高精度 IMU 开源 SLAM”的典型搭配复制成本低遇到问题也容易搜到资料。1.2 硬件连接与通信拓扑硬件连接没有太多玄学关键是理清每条线的供电和数据通道。我这边雷达走以太网通过网线直接到工控机Xsens 用 USB 转串口接在工控机上底盘是 CAN 转 USB 接同一个工控机。三条链路相互独立不共地的问题要特别注意雷达和底盘电机启动瞬间容易把电压拉垮建议雷达和 Xsens 用独立稳压供电别直接挂在底盘动力电池的输出端。通信链路整理如下模块接口数据话题频率用途禾赛 32 线雷达以太网/pandar_points10 Hz3D 点云Xsens MTi-G-710USB 串口/imu/data200 Hz姿态、角速度、加速度松灵 Scout miniCAN/odom50 Hz轮式里程计松灵 Scout miniCAN/cmd_vel10 Hz底盘速度控制这里有个容易忽略的点Xsens 的默认输出可能是 400 Hz但并非越高越好。Cartographer 3D 的 IMU 数据一般 100 到 200 Hz 就够太高反而增加 CPU 负担也容易造成时间队列堆积。我最后把 IMU 配置在 200 Hz稳当也不卡。雷达帧率和 IMU 频率不需要严格整数倍关系但至少保证雷达一帧里能有 10 到 20 个 IMU 数据这样运动补偿才够平滑。1.3 数据流与话题关系整套系统的数据流可以概括为三路传感器输入到 Cartographer再加一组 TF 变换。底盘里程计提供 odom 到 base_link 的里程增量Xsens 提供 imu_link 上的角速度和线加速度雷达点云则是 cartographer_node 直接使用的观测数据。节点内部会根据时间戳把三路数据插值对齐再通过匹配优化得到机器人位姿。很多新手忽略的一点是 Cartographer 3D 模式下点云话题默认是/points2。禾赛驱动默认发布的是/pandar_points所以启动 Cartographer 时要做 remap。如果不做看到 Cartographer 起来但地图一直不更新十有八九就是话题没接上。另外Odometry 话题不是必须的但如果底盘提供了好用的里程计建议接上尤其在 GPS 信号差、IMU 有零漂的室内环境轮式里程计能起到约束作用。2. 环境搭建与驱动编译2.1 软件版本与编译环境软件环境我推荐 Ubuntu 20.04 配 ROS Noetic。Cartographer 对 Melodic 也支持但 Noetic 下的驱动兼容性和第三方依赖更好找。Cartographer 本体建议用源码编译不要只装二进制包因为后续调参和断点排查都要看源码。编译步骤大致如下sudo apt-get update sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow git mkdir -p ~/carto_ws/src cd ~/carto_ws wstool init src wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall wstool update -t src sudo rosdep init rosdep update rosdep install --from-paths src --ignore-src --rosdistronoetic -y catkin_make_isolated --install --use-ninja源码编译对内存和 CPU 要求不高8 G 内存、四核处理器基本能过。唯一要注意的是 protobuf 版本Cartographer 对 protobuf 版本比较挑剔如果系统里装了其他项目的高版本 protobuf最好用虚拟环境或者容器隔离开。否则编译时会出现一堆 ABI 相关报错处理起来很耗时间。2.2 禾赛 32 线雷达驱动编译与配置禾赛官方驱动现在的通用包是HesaiLidar_General_ROS也兼容旧的rslidar_sdk。下载源码后放到 catkin 工作空间编译就行。雷达通过网线连接后需要把工控机网卡 IP 配到和雷达同一网段。以我这台为例雷达默认 IP 是 192.168.1.201工控机网卡改成 192.168.1.102子网掩码 255.255.255.0不需要网关。ifconfig eth0 192.168.1.102 netmask 255.255.255.0 up然后修改驱动配置里的 sensor IP 和 host IP有的版本还需要配置点云坐标系 frame_id我统一设置成laser。编译运行后先用rostopic hz /pandar_points确认频率稳定在 10 Hz 附近。如果一直掉线多半是网线接触问题或供电不稳雷达启动时电流不小建议单独供电。2.3 Xsens MTi-G-710 驱动与数据输出Xsens 的 ROS 驱动推荐用xsens_mti_driver这个开源包它同时兼容 MTi 全系列。驱动依赖libmt库需要先安装 Xsens 官方的 Device API也就是xsensdeviceapi。网上能下载到 Linux 版本解压后设置环境变量再编译 ROS 包。连接串口时注意权限问题常见现象是ls /dev/ttyUSB0能看到设备但没有操作权限。先执行sudo chmod 666 /dev/ttyUSB0再启动roslaunch xsens_mti_driver xsens_mti_driver.launch驱动起来后/imu/data话题会发布传感器融合后的姿态和 IMU 原始数据。我遇到过一个问题Xsens 默认输出的是传感器坐标系不是 ROS 标准坐标系如果不做旋转对齐Cartographer 一开始就会说重力方向不对。这个后面在 TF 配置部分会细说。2.4 松灵底盘驱动与里程计话题松灵官方提供了scout_bringup、scout_base等 ROS 包。Scout mini 底盘通过 CAN 接口通信先把 CAN 接口配置好sudo ip link set can0 type can bitrate 500000 sudo ip link set can0 up确认 CAN 接口起来后启动roslaunch scout_bringup scout_mini_bringup.launch正常启动后会看到/odom、/cmd_vel、/scout_status等话题。底盘默认的 frame_id 一般是odom和base_link这一点比较友好不需要额外改。使用时要注意Scout mini 的里程计来自轮式编码器在硬质地面表现不错但在草地和砂石路上会打滑建图前最好确认一下地面工况。3. 坐标变换、时间同步和 Cartographer 3D 配置3.1 外参标定与 URDF 编写Cartographer 3D 依赖 TF 树获取传感器之间的相对位姿。IMU 和激光雷达在车上的安装位置、朝向必须准确否则建图会越跑越飘。我建议把安装位置固定后用 URDF 写死传感器之间的变换而不是每次手动发布 static_transform_publisher因为 launch 里传参多了容易看错。一个最简 URDF 长这样robot namescout_mini link namebase_link/ link nameimu_link/ link namelaser/ joint namebase_to_imu typefixed parent linkbase_link/ child linkimu_link/ origin xyz0.1 0 0.25 rpy0 0 0/ /joint joint namebase_to_laser typefixed parent linkbase_link/ child linklaser/ origin xyz0.2 0 0.35 rpy0 0 0/ /joint /robot这里面的xyz是从 base_link 中心到各传感器中心的平移rpy是传感器坐标系相对车体坐标系的旋转。具体数值必须以实际量出来的安装位置为准。不要觉得差不多就行毫米级误差可能在短时间看不出来跑到几十米后就会以明显漂移的形式暴露。外参标定可以手工量也可以用lidar_align工具自动优化。手工量的重点是传感器坐标系参考点雷达一般以底面中心为原点Xsens 外壳上也会标参考点。如果 IMU 装歪了几度建图初期可能只是轻微重影越到后面越不可收拾。3.2 时间同步为什么 Cartographer 对时间戳这么敏感Cartographer 内部会维护一个按时间排序的传感器数据队列所有输入数据靠时间戳插入。如果三路传感器的时间基准不一致会出现两类典型问题一是数据乱序导致部分数据被丢弃二是点云和 IMU 对不上匹配结果震荡。最理想的状态是让所有传感器都使用工控机的系统时间。底盘驱动和 Xsens 驱动默认都会打上接收到数据的时间戳雷达驱动有些版本用的是雷达内部时钟这就容易出问题。排查方法很简单rostopic echo /pandar_points/header/stamp -n 1 rostopic echo /imu/data/header/stamp -n 1 rostopic echo /odom/header/stamp -n 1看三个话题的 stamp 是不是都在当前系统时间附近。如果雷达时间戳和系统时间差了太多可以在驱动配置里找时间补偿参数或者用use_system_time方案让节点发布时直接用系统时间。对于实时建图时间戳误差控制在几十毫秒内基本可接受超过一帧雷达周期就麻烦了。我自己遇到过 Xsens 驱动发布的 IMU 时间戳有偶发回跳原因是串口缓冲和解析线程导致数据排队后来把baudrate和timeout参数调了一下再用系统时间统一打戳问题就消失了。3.3 3D LUA 配置核心参数Cartographer 3D 的配置主要在 LUA 文件里。我的scout_3d.lua核心部分如下include cartographer_ros/3d.lua TRAJECTORY_BUILDER_3D.min_range 0.5 TRAJECTORY_BUILDER_3D.max_range 60.0 TRAJECTORY_BUILDER_3D.num_accumulated_range_data 1 TRAJECTORY_BUILDER_3D.use_imu_data true TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching false TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight 5.0 TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight 2.0 TRAJECTORY_BUILDER_3D.motion_filter.max_distance_m 0.1 TRAJECTORY_BUILDER_3D.motion_filter.max_angle_radians 0.02 MAP_BUILDER.num_background_threads 6 return options逐项说下我的调整原因。min_range 0.5是为了滤掉雷达自身外壳、车体和近距离杂点太小会把车体点云扫描进去max_range 60在室外够用太远反而稀疏。num_accumulated_range_data控制一次匹配叠多少帧点云调大为 3 可以增加单帧特征但会引入更多延迟和计算量实时建图时我保持为 1。use_online_correlative_scan_matching是一个很关键的开关。开了之后Cartographer 在匹配前会做一次相关扫描匹配作为初始猜测路面退化场景下更不容易跑飞但 CPU 占用明显上升。如果环境里墙和柱子足够多关掉它问题不大。我在停车场建图时选择关掉在空旷草坪上则打开。ceres_scan_matcher的权重也很讲究。我遇到的场景里平移方向容易飘所以平移权重比旋转高但这不是绝对的要根据建图时观察到的漂移方向来调。motion_filter控制子图插入新关键帧的频率最大值调大子图数量少、计算量低但精度可能下降调小则关键帧更密精度高但计算压力大。3.4 TF 树和话题频率检查启动所有驱动后不要急着跑 Cartographer先做一轮体检。重点看 TF 树是否完整。执行rosrun tf view_frames会生成一个frames.pdf打开后默认应该能见到map - odom - base_link - imu_link - laser。如果底盘驱动没有发布odom - base_linkCartographer 会一直等待里程计或只能靠纯雷达匹配效果很差。如果imu_link - laser没连上Cartographer 的 3D 模式直接起不来或者在控制台输出找不到传感器变换的警告。话题频率检查则执行rostopic hz /pandar_points rostopic hz /imu/data rostopic hz /odom雷达 10 HzIMU 100 到 200 Hz底盘 50 Hz。如果某个话题频率忽高忽低先解决它再谈建图效果。这个步骤花不了两分钟但能省下后面大量排查时间。4. 完整建图实操过程4.1 启动顺序与命令实际操作时启动顺序很有讲究。我的建议是“先底盘再 IMU再雷达最后 Cartographer”这样能保证 Cartographer 启动时有完整的 TF 和所有传感器数据。底盘启动sudo ip link set can0 type can bitrate 500000 sudo ip link set can0 up roslaunch scout_bringup scout_mini_bringup.launchXsens 启动sudo chmod 666 /dev/ttyUSB0 roslaunch xsens_mti_driver xsens_mti_driver.launch雷达启动按驱动说明运行roslaunch hesai_lidar hesai_lidar.launch启动 TF 和 Cartographer这里用一个自定义 launch 文件launch node pkgrobot_state_publisher typerobot_state_publisher namerobot_state_publisher param namerobot_description textfile$(find scout_description)/urdf/scout.urdf/ /node node pkgcartographer_ros typecartographer_node namecartographer_node args-configuration_directory $(find scout_carto)/config -configuration_basename scout_3d.lua outputscreen remap frompoints2 to/pandar_points/ remap fromimu to/imu/data/ remap fromodom to/odom/ /node node pkgcartographer_ros typecartographer_occupancy_grid_node namecartographer_occupancy_grid_node args-resolution 0.05/ /launch注意我在这里把points2重映射到/pandar_points否则 Cartographer 订阅不到雷达数据。robot_state_publisher读取 URDF 后会发布base_link - imu_link - laser的静态变换不需要额外写 static_transform_publisher。4.2 数据采集时的操控手法很多人以为建图就是开着车来回转实际上操控手法对 Cartographer 影响特别大。我的经验是第一件事是让机器人静止三到五秒让 Cartographer 有足够时间完成初始重力估计和 IMU 初始化。如果一启动就猛跑Cartographer 可能还没来得及稳定轨迹就已经歪了。控制底盘时尽量低速匀速。Scout mini 底盘遥控器有档位我一般用低速挡速度控制在一米每秒以内。转弯不要太急轮式里程计在急转弯时打滑明显IMU 也不能完全补偿。遇到长走廊不要闷头往前走走到一半可以做一次原地小角度旋转让雷达扫到走廊两侧的墙这样能增加约束。到了尽头最好沿原路折返一段制造回环让 Cartographer 能修正累积误差。全程盯住 Rviz。如果看到点云出现明显的双层重影不要继续前进停下来判断重影是静态的还是动态的。静态重影一般是外参校准问题动态重影可能是时间戳或里程计问题。在问题没解决之前继续跑只会把地图越建越差。4.3 建图质量判断与地图保存建图过程中可以从三个角度判断质量。第一是 Rviz 里点云和子图的贴合程度子图边缘应该清晰利落第二是 robot 在 map 里的轨迹是否平滑如果出现突然跳动说明匹配在某个时刻丢了第三是保存后的地图是否闭合走过一圈后回到起点地图上的同一面墙应该重合而不是出现错位和双墙。保存地图的常规操作是rosservice call /finish_trajectory 0 rosservice call /write_state {filename: /home/user/maps/scout.pbstream}write_state会把 Cartographer 的内部状态导出为.pbstream文件。如果只需要 2D 栅格地图可以在 Cartographer 运行过程中订阅/map话题然后用 map_server 保存rosrun map_server map_saver -f /home/user/maps/scout_map这样会生成scout_map.pgm和scout_map.yaml可以直接给 navigation 和 move_base 用。如果要做 3D 展示或导入其他软件需要另用cartographer_assets_writer把 pbstream 导出成 PLY 点云这个后续可以再展开。5. 问题排查与调参心得5.1 点云重影和里程计漂移重影问题在我跑这套系统时出现得最多。第一次建图走廊里明明是一面墙点云却出现两条白线拉开接近十几厘米。排查顺序是先让车静止看雷达点云是否稳定如果静止时也有重影那就是雷达点云本身有噪声或者时间戳抖动静止时没问题一动才有重影大概率是里程计或 IMU 的融合出了问题。如果是里程计漂移导致的优先降低里程计对 Cartographer 的影响。Cartographer 里没有像 AMCL 那样直接调里程计权重的参数但可以通过不向 Cartographer 提供/odom话题来绕过底盘里程计让算法退化成雷达加 IMU 的模式。我在沙石路面上试过去掉里程计后反而比加上轮式里程计更不容易漂移。不过底盘码盘在硬质地面上还是很有用的具体要不要用要看地面条件。另一个有效手段是提高num_accumulated_range_data。同一个位置多累积几帧点云再匹配相当于增加了观察密度能有效压低随机噪声。代价是 CPU 占用上升、响应变慢需要在实际场景里试一下看看能否接受。5.2 时间戳乱跳导致的退化时间戳问题有一个非常典型的表现Rviz 里轨迹和地图看着都正常但 Cartographer 控制台一直刷 “Timed out” 或 “Skipped” 一类的日志地图偶尔会整体平移一下。用rostopic hz检查三个话题频率都正常可一查rostopic echo里每个 message 的 stamp 间隔就会发现雷达点云的时间戳有时会往后跳几百毫秒。这种问题根源几乎都在不同传感器时间基准不一致。我当时的处理办法是在雷达驱动里把时间戳改为使用系统时间同时保证 Xsens 驱动不要用传感器内部时间。如果驱动不支持可以写一个简单的 ROS 节点把收到的点云和 IMU 消息的header.stamp统一替换成ros::Time::now()。这个方法只适合实时建图录 bag 后回放时会有问题因为 bag 里的时间应该保持原始时间方便离线回放时统一处理。5.3 IMU 数据异常和坐标系对齐Cartographer 3D 初始化时非常依赖 IMU 的重力方向。如果 Xsens 装在底盘上但 URDF 里没有给好旋转关系Cartographer 可能在启动几秒后就报出“IMU 数据没有重力分量”或类似错误。检查 IMU 是否正常可以先把底盘放在水平地面上用rostopic echo /imu/data -n 5看加速度的 Z 轴分量。如果 Z 轴接近正负 9.8说明基本水平如果重力分量跑到了 X 或 Y 轴说明 IMU 坐标系和 URDF 里的定义不一致。Xsens 默认可能是传感器坐标系需要根据安装方向在 URDF 的 rpy 里做旋转或者修改 Xsens 驱动的输出坐标系配置。我的做法是让 Xsens 输出 ENU 系然后根据实际安装角度换算到车体坐标系再把换算关系写进 URDF。这个步骤做不对后面调多少参数都是白搭。5.4 底盘里程计尺度不准Scout mini 的轮式里程计在平整硬质路面上表现不错但一旦走到草地、泥地或者有坡度的路面车轮打滑会导致里程计输出偏大或偏小。Cartographer 接到这种不准确的里程计后反而会干扰雷达匹配。我踩过的一个典型场景是在草地上建图地图上明显出现一段弯弯曲曲的假轨迹但实际车是直线开的。后来在 launch 里把odom话题的 remap 去掉让 Cartographer 收不到 odom改纯雷达加 IMU 模式问题立刻缓解。另一个思路是把底盘仪表盘打开在地面上量一段十米距离对比/odom里位姿变化是否一致如果不一致可以在底盘驱动里对里程计增益做补偿。但我没有动这一步毕竟底盘驱动更新一下可能又变回去不如直接把 odom 关掉省心。5.5 性能优化与降负载Cartographer 3D 对计算资源的要求比 2D 高不少。我最初在工控机上跑CPU 经常到 80% 以上偶尔还出现实时性不足。后来做了三点优化。第一是给点云做体素滤波。在雷达驱动和 Cartographer 之间加一个pcl_ros/voxel_grid节点把体素设为 0.1 米就能有效减少点云数量。第二是把雷达帧率从 10 Hz 降到 5 Hz建图精度几乎没有肉眼可见的下降但 CPU 占用能降低三分之一。第三是给 Cartographer 多分配后台线程在 LUA 里把MAP_BUILDER.num_background_threads调成 CPU 核心数减一这样充分利用多核资源。如果实时性还是不够建议把数据录制为 rosbag然后用 Cartographer 的离线节点跑roslaunch cartographer_ros offline_node.launch bag_filenames:/path/to/your.bag configuration_directory:/path/to/config configuration_basename:scout_3d.lua离线建图的好处是允许参数试错不用每次开着车反复跑现场。我后续几乎所有参数调试都是这样完成的。6. 最后想说的几点体会整套系统跑通之后我最大的体会是建图效果的决定因素往往不在算法参数而在数据质量。传感器之间的外参和时间戳只要有一个不对后面调多少参数都补不回来。所以每次换场景、拆装设备后我都会重新检查一遍 TF 树的变换和时间戳这个过程看似繁琐但节省的时间远比付出的多。另外一个小技巧是每次建图前先录一段 30 秒的静止数据再去目标场景跑一圈。这段数据既能用来验证传感器是否工作正常也可以在离线环境里反复测试参数省得每次调参都要推着车跑来跑去。Cartographer 3D 这套方案对于中小型园区、地下车库、室内仓库这类结构化场景非常合适但对大面积纯空旷的野外环境还是会有挑战那就要考虑换激光惯性里程计方案了。希望这篇记录能帮你少走一些弯路。