做移动机器人的人应该都有过这种感觉同一套传感器装到新车上地图质量立刻见底。点云重影、转弯后轨迹飘、明明走到同一个地方回环就是闭不上排查到最后多半不是算法参数没调好而是外参没标准。这篇工作笔记是“工作笔记”系列的第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相对底盘roll、pitch、yaw安装角静态加速度计测roll/pitch动态转圈和直线测yaw底盘轮式里程计左右轮轮径、轮距、编码器方向直线行驶和原地旋转比对激光雷达相对底盘x/y平移、yaw偏角墙面直线法、矩形纸箱法这三步的顺序不建议乱。先把IMU相对底盘的角度压准再用底盘自身标定结果做参照物去校雷达逻辑上最顺。一次搞定所有外参看起来很爽但出错时根本不知道是哪一个矩阵引起的排查成本反而高得多。2. 静态标定IMU的roll和pitch让重力当一次参照物IMU的roll和pitch最核心的参照物其实是重力。传感器静止的时候加速度计三个轴的读数合起来就是重力向量在IMU自身坐标系下的投影。如果IMU完全水平安装理论上ax和ay都接近0az接近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(froll{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相对底盘的安装角是roll0.21°、pitch0.33°、yaw1.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], rcondNone)[0] yaw_offset_deg np.degrees(np.arctan(k)) print(fwall 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 roll/pitch偏大地面点云倾斜scan matching容易失败IMU yaw偏差转弯后地图整体旋转偏置雷达yaw偏差直线墙面变斜长走廊两侧墙“劈叉”雷达x平移错误直行通过门框时点云前缘位置不对回环错开轮距错误原地旋转角度估计偏差地图转角处漂移时间戳不同步点云与IMU融合出现随机抖动状似外参不稳改配置时有个常见坑如果你在测试时发现外参填对了地图依然歪先检查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、底盘车轮、单激光雷达——在移动机器人上几乎永远不会被拆开单独讨论但它们之间没有一次性的“装好就不用管”的关系。换一个支架、拆装一次横梁、甚至只是运输颠簸过都可能导致微小的外参漂移。把这些漂移的规律记录成笔记建立一套可复现的标定流程才是长期维护一台机器人最值得投入的部分。