移动机器人外参标定实战：IMU、激光雷达与底盘校准流程

做移动机器人的人应该都有过这种感觉：同一套传感器，装到新车上，地图质量立刻见底。点云重影、转弯后轨迹飘、明明走到同一个地方回环就是闭不上，排查到最后，多半不是算法参数没调好，而是外参没标准。这篇工作笔记是“工作笔记”系列的第5篇，记录的正是我给一台双轮差速底盘配单线激光雷达和IMU、用hdl_graph_slam做建图和定位之前，做的一整套外参校准流程。

这个平台是典型的室内移动机器人配置：底盘自带编码器里程计，车上装了一个九轴IMU，前方横梁挂了一个单线TOF激光雷达，雷达扫描平面基本水平。后端用hdl_graph_slam做建图，定位阶段也用同一套外参跑hdl_localization。整个过程涉及IMU相对底盘的安装角标定、底盘轮式里程计自身的参数自检、激光雷达相对底盘的外参求取，最后同步写回hdl_graph_slam的配置并用实测地图检验效果。

如果你正在维护带IMU和单线雷达的差速底盘，或者准备用hdl_graph_slam做建图却被“地图对不齐”折磨，这篇笔记应该能帮你省下不少弯路。下面我按实际操作的先后顺序，把每一步的原理、命令、脚本和踩坑记录都摊开来说。

1. 标定前先把三套坐标系理顺：外参到底在说什么

外参校准的第一步不是拿尺子量，而是先在纸上把坐标系画清楚。我这个平台上有一个明确的底盘坐标系base_link，原点放在底盘旋转中心在地面的投影上，x轴朝前，z轴朝上。IMU装在底盘中心偏前的一块塑料支架上，因为支架和底盘之间的加工和装配公差，它的三个轴并不能严格平行于底盘坐标系的三个轴。激光雷达则装在底盘最前方的铝型材横梁上，扫描平面基本水平，但同样存在角度偏置和平移安装误差。

这三个坐标系之间各自差了那么一点角度和位移，就是外参。外参在校准阶段表现为一个旋转矩阵R和一个平移向量t，合起来写成齐次变换矩阵。放在SLAM里，外参的物理含义是：当雷达或IMU测到一组原始观测时，先把这些观测从传感器坐标系变换到统一的底盘坐标系，再叠加底盘在世界坐标系里的位姿，最终得到世界坐标系下的点云和姿态。任何一个环节的变换矩阵错了，后续所有计算都会带上一个固定但难以追踪的偏差。

在hdl_graph_slam里，这套逻辑体现得很直接。核心因子图框架中，激光scan matching的相邻帧位姿约束、IMU预积分提供的相对姿态约束、回环检测约束都会进入gtsam优化。IMU数据在使用时需要通过外参从IMU坐标系转到激光雷达坐标系；点云在发布时也要经过外参和当前位姿变换。如果IMU外参的yaw反了1°，车跑10米再拐个弯，地图整体就可能偏掉几十厘米，根本没法用。

我习惯把一次完整的标定划分成三个层次，对应三组需要确定的量。

这三步的顺序不建议乱。先把IMU相对底盘的角度压准，再用底盘自身标定结果做参照物去校雷达，逻辑上最顺。一次搞定所有外参看起来很爽，但出错时根本不知道是哪一个矩阵引起的，排查成本反而高得多。

2. 静态标定IMU的roll和pitch：让重力当一次参照物

IMU的roll和pitch最核心的参照物其实是重力。传感器静止的时候，加速度计三个轴的读数合起来就是重力向量在IMU自身坐标系下的投影。如果IMU完全水平安装，理论上ax和ay都接近0，az接近9.8；一旦安装支架歪了，重力就会被分解到x轴或y轴上，滚动角和俯仰角自然就从这些分量里暴露出来。

计算公式不复杂，按常规的欧拉角定义可以写成：

roll = atan2(ay, az) pitch = atan2(-ax, sqrt(ay**2 + az**2))

不同IMU的加速度坐标系正方向有差异，所以这个公式的符号不能直接照抄，最终结果要和实际物理倾斜方向做一次验证：把车头垫高，pitch应该往一个确定方向变；把车体右侧垫高，roll应该往另一个确定方向变。验证通过后再记录数值。

静态采集这一步，我建议不要上来就拿几秒钟的数据算。IMU上电后会有一段温漂期，尤其是陀螺仪零偏会随温度缓慢变化，加速度计相对稳定但也会受振动影响。现场操作流程是：把机器人放在平整地面上，尽量水平，上电后静止至少3分钟，然后用rosbag录90秒左右的imu原始数据。录完之后用脚本把加速度计三轴取平均，再带入上面的公式。

import rosbag import numpy as np bag = rosbag.Bag("imu_static.bag") ax_list, ay_list, az_list = [], [], [] for topic, msg, t in bag.read_messages(topics=["/imu/data_raw"]): ax_list.append(msg.linear_acceleration.x) ay_list.append(msg.linear_acceleration.y) az_list.append(msg.linear_acceleration.z) ax = np.mean(ax_list[100:]) # 跳过前100个采样，避免上电毛刺 ay = np.mean(ay_list[100:]) az = np.mean(az_list[100:]) roll = np.degrees(np.arctan2(ay, az)) pitch = np.degrees(np.arctan2(-ax, np.sqrt(ay**2 + az**2))) print(f"roll={roll:.3f} deg, pitch={pitch:.3f} deg")

我这次实测出来，roll大约0.21°，pitch大约0.33°。这个量级不算大，如果直接忽略，有些场景也能跑，但后续原地旋转标定yaw时，小的roll/pitch偏差会被放大，所以我还是独立标出来并补偿掉了。

这里有个容易忽略的点：如果roll或pitch已经大到两三度，说明机械安装本身就不合格，靠软件外参硬补偿能解决一部分，但支架长期受力后容易再变形。遇到这种情况我会建议先调整机械结构，把支架整平再标。软件补偿是兜底手段，不是让错误安装合法化的理由。

3. 动态标定IMU与底盘的yaw偏差：原地转圈和直线行走

roll和pitch可以用重力当参照，但yaw不行。重力方向绕z轴旋转并不会改变加速度计的读数，所以静态条件下yaw安装角是完全不可观的。必须借助外部航向参考，最常见的参考就是底盘编码器解算出来的转角。

在正式标定yaw之前，要做一次陀螺仪零偏采集。把车静止30秒，取gyro_z的平均值作为零偏，后面积分前先减掉。零偏不扣干净，原地转几圈后积分结果会明显偏大或偏小，直接影响yaw偏差的判定。

标定流程的第一步是原地转圈。找一块摩擦力均匀的空地，让车以一个缓慢但稳定的角速度原地旋转，方向任意，连续转2到3圈。缓慢指的是底盘转速控制在0.3rad/s左右，让轮胎不打滑。打滑会造成编码器少计数，这个误差在标定时非常致命。

底盘的总转角可以用左右轮编码器的位移差除以轮距算出来：

delta_theta_odom = (delta_s_right - delta_s_left) / wheel_base

而IMU的真实总转角，是用扣完零偏的gyro_z按时间积分得到的。如果把这两者的最终差值除以转的圈数，就能反推出yaw安装角的偏差。

import rosbag import numpy as np bag = rosbag.Bag("rotation_test.bag") imu_ts, gyro_z = [], [] odom_ts, delta_s_l, delta_s_r = [], [], [] last_left = None last_right = None for topic, msg, t in bag.read_messages(): if topic == "/imu/data_raw": imu_ts.append(t.to_sec()) gyro_z.append(msg.angular_velocity.z) elif topic == "/odom": if last_left is None: last_left = msg.pose.pose.position.x # 示意，实际用轮速积分 last_right = msg.pose.pose.position.y # 这里需要换成实际编码器累计位移字段 delta_s_l.append(...) delta_s_r.append(...) gyro_bias = np.mean(gyro_z[:200]) dt = np.diff(imu_ts) yaw_imu = np.cumsum((gyro_z[1:] - gyro_bias) * dt) total_yaw_imu = yaw_imu[-1] total_yaw_odom = (sum(delta_s_r) - sum(delta_s_l)) / wheel_base yaw_offset = np.degrees(total_yaw_imu - total_yaw_odom)

实际脚本里odom侧我更建议直接用底盘驱动发布的累计转角，或者自己从左右轮速度和轮距积分。关键是两条轨迹的时间起点和终点要对齐，否则差出来的那一段转角会被误判为安装角偏差。

光转圈还不够，还要走一段直线验证。让车直线向前走3米左右，同时看IMU和底盘各自解算出的航向变化量。如果yaw安装角已经标准，直线行走过程中两者的航向变化应该都很小且一致；如果走直线时IMU解算的航向在慢慢偏，说明yaw偏差没有压干净，或者roll/pitch还有残留。

这里有个很好的判别逻辑：如果IMU的z轴不严格垂直于底盘平面，也就是roll/pitch没标干净，那么原地旋转得到的yaw角度变化和底盘实际转角之间会存在非恒定的比例关系。最典型的表现是按照某个方向转180°和转90°求出的“yaw偏差”不一致。遇到这种现象，就要回头把第2章的roll/pitch再压一遍，而不是强行取平均。

我这台车最终标定的结果，IMU相对底盘的安装角是roll=0.21°、pitch=0.33°、yaw=1.74°。yaw偏差接近两度，一开始我也觉得不至于，但仔细检查发现雷达横梁和底盘之间有一个偏置安装的转接件，确实给IMU支架带偏了。这个数值修正之后，后续建图的转弯稳定性明显上了一个台阶。

4. 轮式里程计参数自检：轮径、轮距和编码器方向不能想当然

底盘轮式里程计虽然严格来说是传感器的内参问题，但在这个系统里它的作用等同于“外参校正”：左右轮轮径决定了位移计算，轮距决定了转角计算，编码器方向决定了里程计的正反。任何一个参数错，底盘向fusion节点输出的odom就带着系统性误差，扫描匹配的初值质量也会被拖垮。

先说轮径标定。让车在光滑平直的地面上走一段已知距离，比如2米。用卷尺在地面标好起点和终点，让车前轮对准起点线，低速直线开到终点线，看底盘odom上报的距离。如果odom报1.95米，说明轮径偏小，按比例修正即可。代码层面的修正一般是把这1.95/2.0的比例乘到轮径上，这个比值在不同车速下基本稳定。

再说轮距标定。让车原地旋转一个已知角度，比如用转角仪或者直接在车体上贴两条参考线，重新定义90°。底盘odom上报的转角可能不是90°，这时候因为左右轮位移差是编码器实打实测出来的，角度不准只能归因于轮距参数。按下面的关系反推实际轮距：

actual_wheel_base = (delta_s_right - delta_s_left) / actual_theta

原地旋转时轮胎和地面的摩擦状态影响很大，所以轮距标定最好也转2到3圈，取平均角度来计算，单次90°容易受到打滑和加速段的影响。

编码器方向检查是我这次差点翻车的地方。底盘驱动发布的速度正方向和IMU定义的正方向如果不一致，后续融合出来的航向永远是不稳定的S型。检查方法很简单：直线往前推车，看odom的x速度是否为正；推车左转，看odom的z轴角速度符号是否和IMU的gyro_z一致。如果发现符号相反，不需要改硬件，在底盘驱动或融合节点里把对应轴取反就行。

这一步标定完成后，底盘odom应该能做到：直行2米误差小于2厘米，原地旋转3圈累计角度误差小于3度。这个精度对于给scan matching提供初值已经足够。如果达不到，先别急着往后走，回去查轮径、轮距和运动学模型里的正负号。

顺便说一句，hdl_graph_slam默认并不强制需要odom话题，它主要靠激光帧间匹配和IMU因子来估计位姿。但我的实际工程中用robot_localization把底盘odom和IMU做了EKF融合，融合结果作为点云配准的初始猜测，同时把原始IMU话题也喂给hdl_graph_slam做因子约束。这就意味着odom质量直接影响scan matching能不能收敛到位，所以底盘参数校准在整个链条里的位置一点不比IMU外参低。

5. 单线激光雷达相对底盘的外参标定：一面墙和一个纸箱

单线激光雷达只扫一个平面，在2D建图场景下，它相对底盘的外参最关键的两个量是：雷达坐标系原点在底盘平面上的x/y平移，以及雷达扫描平面相对底盘方向的yaw偏角。roll和pitch虽然也会影响建图，但更好在机械安装阶段控制住，软件标定主要处理yaw。

最朴素的方法是墙面直线法。找一面足够长且平整的墙，把车开到墙前，让车体的正方向尽量垂直于墙面。这时候从激光雷达扫描点里拟合出一条直线，如果雷达安装的yaw没有偏，这条直线在雷达坐标系中应该正好垂直于x轴，也就是直线的方向角与雷达坐标系x轴的夹角接近0；一旦有偏差，直线的方向角就是雷达相对底盘的yaw偏角。

import numpy as np # points是雷达扫描中属于墙面的一串点，shape=(N,2) # 假设墙面在雷达前方，近似为一条直线 A = np.vstack([points[:, 0], np.ones(len(points))]).T k, b = np.linalg.lstsq(A, points[:, 1], rcond=None)[0] yaw_offset_deg = np.degrees(np.arctan(k)) print(f"wall angle in lidar frame: {yaw_offset_deg:.3f} deg")

实测下来这个方法对墙面平整度要求并不苛刻，误差主要来自车体摆正的动作。因为雷达只扫到墙的一小段，直线拟合本身就能把噪声压得很低。要注意的是，墙在雷达坐标系里拟合出来的夹角不仅包含雷达yaw偏差，还包含车体相对墙面没摆正的角度。所以实际操作时我会让人扶着车体，用长直靠尺贴住车侧面对墙，尽可能把“车体垂直墙面”这个条件做好，剩下的夹角就是雷达的yaw偏差。

墙面直线法只能标yaw，平移量还是要另外处理。我的做法是矩形纸箱法。找一个边长已知的矩形纸箱，放在雷达正前方的地面上，位置尽量让纸箱四条边都能被激光扫到。从点云中拟合出纸箱的四个角点，计算纸箱在雷达坐标系下的中心坐标和朝向角；再用卷尺测量纸箱中心在底盘坐标系下的坐标和朝向角。两边一对比，雷达相对底盘的平移和yaw就全出来了。

简单写一个解算思路。假设纸箱在底盘坐标系下的中心位置是(x_base, y_base)，在雷达坐标系下拟合出的中心位置是(x_lidar, y_lidar)，纸箱朝向在底盘坐标系下是0°（人为摆正），在雷达坐标系下拟合出的朝向是psi。那么雷达相对底盘的yaw偏差就是psi，平移量可以通过下面的关系反推：

# R是雷达相对底盘的旋转矩阵，t是待求平移 # p_base = R * p_lidar + t # 已经知道psi = yaw偏差，R可构造出来 # 取两个不同位置的纸箱观测，建立方程组求解t

为了让平移量解算更稳，我会把纸箱在雷达前方摆两个相距几十厘米的不同位置，每个位置各采一组点云并拟合一次，然后用两组中心点坐标联立最小二乘。这样做能把单次测量误差平均掉，比只测一次直接算要可靠得多。

我这台雷达最终的标定结果是：相对底盘沿x轴前移0.25米，y方向的偏移大约1.2厘米，yaw偏角0.5°左右。之前我直接用图纸上的设计值填入配置，结果在长走廊里地图两侧墙面明显出现“劈叉”，修正后重影基本消失。

6. 把标定好的外参写进hdl_graph_slam：配置、验证和那些坑

标定完成只是前半程，把数值准确无误地写进系统才是另一个坑比较多的环节。hdl_graph_slam的launch启动时会加载一组yaml参数文件，里面通常会包含IMU的话题名、是否启用IMU以及传感器之间的外参。不同版本字段名不完全一样，常见的有T_imu_lidar、imu_to_lidar等，含义也有可能是从IMU到雷达还是从雷达到IMU。所以动手改之前，务必先打开你实际仓库里的yaml文件，确认注释里写的变换方向和单位。

我这边的配置文件大致是下面这种结构：

imu_topic: /imu/data_raw enable_imu: true imu_time_offset: 0.0 # 外参定义：IMU坐标系在Lidar坐标系下的位姿 T_imu_lidar: x: -0.12 y: 0.0 z: 0.08 roll: 0.0037 # 约0.21° pitch: 0.0058 # 约0.33° yaw: 1.7396 # 度，这里按实际yaml单位填写

同时，激光雷达相对底盘的外参通常通过TF树体现，也就是base_link到laser的静态变换。这个TF可以由static_transform_publisher发布，也可以在机器人的URDF里定义。关键点是：TF里的值必须和你在标定脚本里求出的值一致，否则你标定用的坐标系和SLAM运行时实际使用的坐标系就对不上。

填好配置后，不要急着直接跑长距离建图。先在rviz里做一个短距离的往返验证：让车沿直线前进3米再倒回来，然后原地转一圈。观察点云里墙面和障碍物是否在整个运动过程中保持稳定。如果外参正确，原地旋转时地图不会飘，墙线始终是一条线；如果雷达yaw偏了，车一转起来，点云就会在边缘处分离出重影。

我习惯再做一个“回环验证”。让车在一个约5米见方的矩形路径上走一圈，回到起点。标定好的系统在回环处应该能闭合，地图边缘误差控制在厘米级。如果回环错开十几厘米，优先怀疑的目标顺序是：雷达外参yaw、IMU yaw、轮距参数。用排除法依次验证，而不是盲目调hdl_graph_slam的优化参数。

改配置时有个常见坑：如果你在测试时发现外参填对了，地图依然歪，先检查IMU和激光雷达的时间戳对齐情况。我遇到过类似的情况，IMU的时间戳比雷达晚了大约40毫秒，造成的结果和雷达yaw偏0.3°非常像——车一转弯地图就飘。后来我在rosparam里配了imu_time_offset做补偿，问题立刻消失。时间同步和外参问题在现象上会互相掩护，排查时要同时盯。

另外，hdl_graph_slam的坐标系约定和部分机器人的底盘坐标系定义不同，这一点容易被忽略。有的底盘base_link是z轴朝上，但雷达TF发布时yaw方向定义和IMU的yaw方向定义相反。写配置前先做一个简单自检：原地顺时针转一圈，确认odom和imu的z轴角速度在rviz里正方向一致，再填入外参数值，能省去后面很多无效调试。

7. 这轮标定之后，哪些教训最值钱

整套流程走完，我最想分享的其实不是哪条命令，而是三个判断原则。

第一，标定顺序不能乱。先IMU静态roll/pitch，再IMU和底盘动态yaw，然后底盘轮径轮距和方向，最后雷达外参。每一步的动态验证都依赖上一步的结果作参照，顺序颠倒会让误差像滚雪球一样变大，最后根本分不清是哪个环节出的问题。按顺序来，每一步都有可解释的物理意义，出错了也知道回退到哪一步。

第二，软件补偿永远替代不了机械安装。这次IMU的roll/pitch虽然只有零点几度，但支架本身是塑料打印件，温度变化之后尺寸会有轻微变形。我在现场标定得到的数值，和两周前在办公室标定的结果差出了0.1°左右。如果能把这个量级的变形通过机械设计消除，比反复修外参更省心。对精度要求高的场合，建议用金属支架并增加定位销。

第三，外参标定的“合格标准”应该由应用场景来定。如果只是做低速巡检，雷达yaw偏差0.5°可能完全够用；如果是做高精度对接或货架识别，yaw偏差超过0.1°就会导致末端定位超差。所以每次标定完成后，我都会在同一个场景里分别记录“标定前”和“标定后”的地图重影量、回环闭合误差，并把它们写进工作笔记。有了基线数据，下次再遇到地图异常时，就能快速判断是外参退化了，还是算法参数需要重调。

这次标定涉及的三个对象——IMU、底盘车轮、单激光雷达——在移动机器人上几乎永远不会被拆开单独讨论，但它们之间没有一次性的“装好就不用管”的关系。换一个支架、拆装一次横梁、甚至只是运输颠簸过，都可能导致微小的外参漂移。把这些漂移的规律记录成笔记，建立一套可复现的标定流程，才是长期维护一台机器人最值得投入的部分。