TF、里程计漂移与回环检测

文章目录从一段 rviz 画面说起TF、里程计漂移与回环检测小白科普1. 这是仿真还是实车rviz 里为什么没有小车模型2. 只显示位姿TF 变换是怎么进行的3. TF 变换全过程小白版3.1 先理解“坐标系”3.2 这辆车的 TF 链3.3 每一段谁发布、发布什么3.4 为什么要分 map 和 odom 两层3.5 rviz 里点云是怎么画上去的3.6 自己动手看一遍4. 怎么修正里程计漂移方法一地图匹配定位这辆车用的方法方法二多传感器融合提高里程计本身的精度方法三激光里程计 / 视觉里程计方法四SLAM 建图时的回环修正方法五先把源头误差降下来别忽略小白建议的顺序5. 回环检测在导航中的作用5.1 回环检测是什么5.2 为什么需要它5.3 在不同场景中的作用5.4 常见做法5.5 小白提示6. 融合里程计在回环检测中的作用举例6.1 举例场景6.2 作用 1决定能不能“想到”这里是回环6.3 作用 2给扫描匹配一个好的“初值”6.4 作用 3作为位姿图里的“连接边”6.5 作用 4校验回环是否可信6.6 作用 5修正雷达扫描的运动畸变6.7 两个注意点7. 建图时记录帧间变换回环后修正整个过程7.1 建图过程中记录什么7.2 走的过程中地图是怎么拼的7.3 回到起点检测到回环7.4 修正的是什么7.5 关于“TF”的澄清7.6 小例子7.7 最终得到什么8. 全文串讲从一段 rviz 画面说起TF、里程计漂移与回环检测小白科普本文由一次真实的问答整理而来从“rviz 里为什么没有车模”这个小问题出发一路讲到 TF 坐标变换、里程计漂移修正、融合里程计以及建图时的回环检测。配图见images/目录。1. 这是仿真还是实车rviz 里为什么没有小车模型结论画面不是仿真是真车在跑。右侧是实拍画面一台实体小车在地砖上不是 Gazebo 之类的仿真画面。时间对得上真实时间左下角 ROS Time 为 1788508244.51Wall Time 为 1788508244.55几乎一样ROS Elapsed 与 Wall Elapsed 也基本相同118.09 和 118.06。仿真通常开启use_sim_timeROS Time 是从 0 开始的仿真时钟与 Wall Time 对不上。小车位置用坐标轴表示地图中间的红绿蓝坐标轴对应左侧面板里的/amcl_posePose 显示类型Shape 选 Axes。rviz 里没有加载 RobotModel所以看不到车模。很多实车调试为了省事只显示位姿和代价地图。想在 rviz 里看到车模在 Displays 里添加RobotModel并保证有robot_description参数和robot_state_publisher发布 TF。仿真时也是同样的做法再配合 Gazebo 等仿真器。2. 只显示位姿TF 变换是怎么进行的TF 变换和 rviz 里显示什么没有关系。TF 树是各个节点在后台发布的rviz 只是读取它。这个画面中 TF 的来源大致是变换由谁发布map → odomAMCL定位的核心输出odom → base_link底盘驱动或里程计节点base_link → laserstatic_transform_publisher或 URDF robot_state_publisher没有车模不代表没有静态变换。一行static_transform_publisher就能代替 URDF。rviz 怎么用它Fixed Frame 设为maprviz 用 TF 把每个显示项的坐标系转换到map。激光点云白色点带的是雷达坐标系必须靠map → odom → base_link → laser这条链才能画到地图上。点云能对齐地图上的墙就说明 TF 链是通的。/amcl_pose本身就是map坐标系下的位姿rviz 直接画坐标轴不需要再转换。验证方法rosrun tf view_framesROS 2ros2 run tf2_tools view_frames生成 TF 树图。在 rviz 里添加TF显示项查看各坐标系的轴。3. TF 变换全过程小白版3.1 先理解“坐标系”每个东西都有自己的“视角”。雷达看到“前方 2 米有墙”这个“前方”是以雷达自己为原点说的。要把它画到地图上就得知道雷达在地图里的位置和朝向。TF 就是记录“坐标系 A 相对坐标系 B 在哪、朝向如何”的系统。3.2 这辆车的 TF 链map → odom → base_link → lasermap地图坐标系固定不动相当于“地球”。odom里程计坐标系以车上电后的起点为原点由轮子转了多少圈累积出来。短期很平滑长期会漂移。base_link车身中心跟着车走。laser雷达自身位置固定装在车上所以base_link → laser永远不变。3.3 每一段谁发布、发布什么base_link → laser静态变换只发一次。内容是“雷达装在车中心前方 0.1 米、高 0.2 米”之类的安装参数由人量出来填写。odom → base_link底盘驱动根据轮速或编码器实时算出“车相对启动点走了多远、转了多少角度”每秒发布几十次。map → odomAMCL 发布。它拿雷达扫描去匹配地图算出“车在地图里的真实位置”再倒推出 odom 相对 map 的偏差用来修正里程计漂移。3.4 为什么要分 map 和 odom 两层举例车实际在地图 (5, 3)。里程计因轮子打滑算出车在 odom 系的 (4.8, 3.1)。AMCL 发现这个偏差就把map → odom设为“平移 (0.2, -0.1)”。这样odom → base_link保持平滑连续不会突然跳变对控制很友好map → odom慢慢修正漂移保证长期准确。3.5 rviz 里点云是怎么画上去的雷达扫到一个点在 laser 系坐标是 (2, 0)。rviz 把变换一段段乘起来laser → base_link加上安装偏移→ odom加上车相对起点的位姿→ map加上 AMCL 的修正最后得到这个点在 map 里的坐标画在对应位置。3.6 自己动手看一遍rosrun tf tf_echo map base_linkROS 2ros2 run tf2_ros tf2_echo map base_link实时打印车在地图中的位置和朝向。rviz 里添加 TF 显示项看到各坐标系的小坐标轴和连线。view_frames生成 PDF看到整棵树以及每段由谁发布、发布频率多少。4. 怎么修正里程计漂移核心思路用“不会漂移的外部信息”去纠正“会漂移的里程计”。里程计是累积量误差只增不减必须靠外部参照来修正。方法一地图匹配定位这辆车用的方法AMCL 把雷达扫描和已知地图对比估计车在 map 里的真实位置再通过map → odom补上偏差。里程计本身不变修正体现在map → odom这一层。前提有准确的地图环境没有大变化。方法二多传感器融合提高里程计本身的精度用robot_localizationEKF融合轮速计和 IMU。轮速计管平移IMU 的陀螺仪管转角。转角是漂移的主要来源加 IMU 改善明显。输出的融合里程计替代原始里程计发布odom → base_link。方法三激光里程计 / 视觉里程计用相邻两帧雷达做匹配如rf2o_laser_odometry、laser_scan_matcher得到不依赖轮子的位移估计。轮子打滑时尤其有用。方法四SLAM 建图时的回环修正Cartographer、slam_toolbox 等检测“回到了去过的地方”整条轨迹一起优化消除累积误差。方法五先把源头误差降下来别忽略标定轮距和轮径参数差 1%就会产生持续偏差。阿克曼车注意转向模型前轮转角到实际转弯半径的换算要准。减少打滑地面条件、加速度别太猛。小白建议的顺序先标定轮径、轮距等参数加 IMU用 EKF 融合有地图就用 AMCL 做全局修正。三层叠加里程计短期平滑EKF 提升精度AMCL 长期纠偏。5. 回环检测在导航中的作用先说结论回环检测主要用在建图SLAM阶段纯导航阶段有地图 AMCL一般不用它。5.1 回环检测是什么车走了一大圈回到原来去过的地方系统认出“这里我来过”这就是回环。5.2 为什么需要它里程计和激光匹配都会累积误差。走一圈后估计位置和真实位置对不上建出来的地图会出现走廊“拉长”或“弯曲”回到起点时墙出现重影、对不上。检测到回环后系统知道“现在的位置应该和之前那个位置重合”于是把整条轨迹和地图一起优化位姿图优化把累积误差摊平地图就闭合了。5.3 在不同场景中的作用建图阶段直接决定地图质量。地图准后面导航才准。Cartographer、slam_toolbox 等都有该机制gmapping 靠粒子滤波间接处理没有显式回环。纯导航阶段地图已建好AMCL 每一帧都在和地图比对相当于持续做“全局纠偏”不需要再检测回环。边建图边导航在线 SLAM 导航需要回环检测否则跑得越久地图越歪路径规划会出问题。回环触发时地图会整体调整规划器要能处理这种跳变。重定位车被抱走或丢失定位时识别“我在地图的哪里”原理和回环检测相近。5.4 常见做法激光 SLAM把当前扫描和历史扫描做匹配得分高就判定为回环slam_toolbox 默认开启。视觉 SLAM用词袋模型如 ORB-SLAM 的 DBoW比较图像相似度。5.5 小白提示回环检测可能误判。长走廊、重复的房间很像误检会把地图“拧坏”所以要有一致性校验。这辆车在 rviz 里用的是 AMCL 定位说明地图是提前建好的回环检测是在建那张图的时候起作用。6. 融合里程计在回环检测中的作用举例一句话融合里程计是回环检测的“地基”。它越准回环越容易被发现、越不容易判错修正效果也越好。6.1 举例场景车沿周长 40 米的长方形走廊走一圈回到起点对比两种里程计只用轮速计轮速计 IMU 融合转角漂移约 5°约 0.5°回到起点时位置误差约 1.5 米约 0.3 米数值为示意用来说明量级差异。6.2 作用 1决定能不能“想到”这里是回环SLAM 不会拿当前扫描去比对所有历史帧只会找“估计位置附近”的历史帧作为候选。假设候选半径为 1 米只用轮速计估计位置离起点 1.5 米起点的历史帧不在候选范围回环直接漏检地图出现重影。融合后误差 0.3 米起点历史帧进入候选回环有机会触发。slam_toolbox 中对应参数是loop_search_maximum_distance。里程计太差时调大半径只会让候选变多、误判变多。6.3 作用 2给扫描匹配一个好的“初值”确定候选后要把当前扫描和历史扫描做匹配求相对位姿。匹配是迭代优化需要一个起点初值差 0.3 米很容易收敛到正确结果。初值差 1.5 米在长走廊或重复房间里容易收敛到一个“看起来也对”的错误位置局部最优相当于把地图拧坏。6.4 作用 3作为位姿图里的“连接边”SLAM 后端把每个关键帧当节点相邻节点之间用里程计连一条边起点 ─里程计→ 角1 ─里程计→ 角2 ─里程计→ 角3 ─里程计→ 回到起点附近 ╰──────────────── 回环边这两个应该重合 ────────────────╯检测到回环后优化器发现“里程计链条累积下来说差了 0.3 米回环说应该是 0”于是按各条边的可信度协方差把这 0.3 米分摊到整条链上地图闭合。融合里程计让每条边更可靠要分摊的总误差也更小结果更接近真实。6.5 作用 4校验回环是否可信两个很像的房间里匹配得分很高但回环给出的位置和里程计估计相差 8 米就可以判定是误检并拒绝。融合里程计提供了“合理范围”这把尺子里程计越准范围卡得越窄误检越容易被筛掉。6.6 作用 5修正雷达扫描的运动畸变雷达转一圈要几十毫秒车在动一帧扫描里的点其实是在不同位置采的。用高频融合里程计可以把这些点补偿到同一时刻扫描更干净匹配得分更可靠。6.7 两个注意点融合里程计要有合理的协方差EKF 会输出后端靠它判断“该多信这条边”。协方差给得太乐观回环修正时会不愿意改动里程计链。不要把 SLAM 输出的位姿再喂回 EKF否则同一份信息被重复计算会过度自信。对阿克曼车转角误差是最主要的漂移来源所以加 IMU 的陀螺仪做 yaw 融合对回环检测的提升最明显。7. 建图时记录帧间变换回环后修正整个过程问题“建图中记录不同帧之间的 TF 变换最终回到起点时用回环检测修正整个过程所有帧的变换对吗”基本正确但有两处要精确记录的是“关键帧的位姿”修正的是“位姿图”而不是直接改 TF。7.1 建图过程中记录什么SLAM 不会每一帧都存而是每隔一小段比如走了 0.2 米或转了 10°存一个关键帧包含当时的雷达扫描车在地图里的位姿x, y, yaw。相邻关键帧之间还存一条“相对变换”来源是里程计或扫描匹配例如“关键帧 5 相对关键帧 4前进 0.2 米转 1°”。关键帧是节点相对变换是边合起来就是位姿图。7.2 走的过程中地图是怎么拼的每来一个关键帧就按它当前估计的位姿把扫描“贴”到地图上。此时的位姿是一路累积出来的越往后误差越大。7.3 回到起点检测到回环系统发现当前扫描和关键帧 1 匹配得很好算出两者的相对变换比如“关键帧 200 与关键帧 1 只差 (0.02, 0.01, 0.1°)”。这就新增了一条回环边。7.4 修正的是什么矛盾出现了沿里程计链条累积关键帧 200 离关键帧 1 有 0.3 米回环边说只差 0.02 米。后端做位姿图优化调整所有关键帧的位姿使所有边的误差总和最小。每条边按协方差加权越不可信的边改动越大误差被分摊到整条链上。结果是所有关键帧的位姿都被微调不是只改最后几帧。然后系统用新位姿把所有扫描重新贴一遍地图就闭合了。7.5 关于“TF”的澄清优化的是关键帧在 map 里的位姿不是 TF 树里的odom → base_link。odom → base_link仍然是平滑连续的里程计不会改也不会回溯。实时发布的map → odom在优化后会更新一次所以回环触发时rviz 里车和地图可能“跳”一下这是正常的。7.6 小例子走正方形4 个角的关键帧里程计估计A(0, 0) → B(10, 0) → C(10, 10.3) → D(0.4, 10.3)回到起点附近应回到 A(0, 0)实际估计在 (0.3, 0.2)。回环说“这里就是 A”差 (0.3, 0.2) 被分摊B 改一点点C 多一点D 最多整圈闭合。7.7 最终得到什么建图结束后用优化后的位姿把所有扫描叠加成栅格地图保存为.pgm .yaml。导航时 AMCL 加载的就是这张图。8. 全文串讲环节一句话判断仿真/实车看实拍画面以及 ROS Time 与 Wall Time 是否一致rviz 显示只是“读取” TF车模有无不影响 TFTF 链map → odom → base_link → laser分别由 AMCL、底盘驱动、静态变换发布map 与 odom 分层odom 保平滑map→odom 慢慢纠偏修正漂移标定参数 → IMU EKF 融合 → AMCL 全局纠偏回环检测主要用于建图导航阶段由 AMCL 持续纠偏融合里程计与回环影响候选、初值、边权重、误检校验、扫描去畸变回环修正的对象关键帧位姿图而不是直接改 TF一条主线贯穿全文里程计负责“短期平滑”外部参照地图匹配、回环负责“长期不漂”。融合里程计提高“短期”的质量让“长期修正”更容易、更可靠。