机器人巡检仿真——从方法到可验收实现 · 06/10
定位：以真实电厂机器狗巡检项目为贯穿案例，提炼可迁移的机器人巡检仿真开发方法。
适合读者：机器人、自动化、计算机、电子信息等专业学生，以及准备进入 ROS 2、机器人软件、仿真、导航或测试方向的求职者。
上一篇：一个巡检点能到，五个为什么会乱：Nav2 多航点任务与状态机实战
下一篇：让机器人安全失败：点云中断、TF 超时和控制器崩溃怎么测

上一章，我们让五个巡检点变成了可以取消、重启和验收的连续任务。这时很容易出现一个疑问：Gazebo 明明知道机器人的真实位置，把这个位置交给 Nav2，不就可以导航了吗？

如果要验证任务状态机，这样做可以帮助隔离定位的不确定性。但如果要验证定位算法，直接提供真实位置，就相当于把答案交给了考生。

在电厂机器狗案例的一轮测试中，同一个最后有效评测时刻，轮式里程计与世界真值相差约 7.6 米，融合位置相差约 3.6 厘米。但看整段轨迹，融合的 RMSE 为 0.2447 米，反而大于 GPS 的 0.1429 米。

这就引出了两个问题：仿真已经知道答案，为什么还要估计位置？GPS 已经比较准，融合到底增加了什么？我们从数据边界讲起，再用真实轨迹回答。文末附数据与脚本，没有 ROS 环境也能复算这些指标。

一、先把三种“位置”分开

同样是一个坐标，它可能承担三种完全不同的职责：

数据	含义	谁可以使用
真值	仿真世界里机器人实际处于哪里	传感器模型、独立评测器
观测	传感器在误差、偏置和丢包条件下测到什么	定位算法
估计	算法根据观测推断机器人在哪里	导航、任务和控制模块

传感器模型可以读取世界真值，用来生成模拟观测。例如从世界位姿得到天线位置，转换成经纬度并加入噪声，输出 NavSatFix。

算法则只能看到带误差的观测，不能读取无噪声世界位姿，也不能利用评测器的误差结果在线修正自己。

这里的隔离是信息用途的隔离，不是要求模拟 GPS 与真值之间没有物理关系。二者当然应描述同一个机器人；需要阻止的是待测算法绕过传感器模型，直接取得正确答案。

尤其要避免这条链路：

轮式里程计 → 加噪声 → 模拟 GPS → 与同一份里程计融合

它让两个输入共享同一个里程计误差。即便曲线看起来平滑，也不能据此证明独立的全球定位观测纠正了里程计漂移。

二、RTK、IMU、里程计分别提供什么

可以把三者理解为三个不同的观察角度：

• 轮式里程计根据轮子的运动推算位移和转向，适合提供连续的局部运动信息，但会受到打滑、轮径误差和运动模型误差影响。
• IMU测量角速度、加速度，有些设备还提供姿态估计。它能补充短时运动信息，但不能靠积分无限期保持准确位置；航向是否具有绝对参考，要看具体传感器和估计方式。
• RTK/GNSS提供全球参考下的位置观测，能够约束局部累计误差，但仍可能受遮挡、异常观测和定位状态变化影响。

单个位置点也不能直接告诉机器人“面朝哪里”。不要把 GPS 消息转换成 Odometry 时填入的单位四元数，当成已经测得的真实朝向。

融合需要解决的是：每个输入测到了哪些量、这些量属于哪个坐标系、何时采样、可信度如何，以及输入之间是否存在重复信息。

如果采用 robot_localization，EKF 会通过运动模型预测状态，再用传感器观测校正预测；接收了三种话题并不等于三种信息已经正确参与融合。官方状态估计说明

本案例的这一版实现有明确边界：使用里程计与模拟 GPS 做二维位置互补校正，朝向仍来自里程计，IMU 没有进入这条融合链。 后面解释完整方案时，会把它作为演进方向。

三、让局部运动连续，让全局位置得到校正

上一章介绍过这条坐标链：

map → odom → base_link

odom 下的位姿强调短期连续性，允许长期漂移；map 下的位姿承担全局定位职责，收到新观测后可能发生跳变。定位模块利用已有的 odom → base_link，发布 map → odom，把全局校正接入坐标树。ROS REP-105

本案例采用一个容易检查的简化：map 和 odom 的轴向一致，只估计二维平移偏移。

假设同一时刻，里程计认为机器人位于 (8.0, 2.0)，GPS 观测是 (8.4, 1.8)，那么当次偏移观测为 (0.4, -0.2)。系统不直接把这个带噪声的偏移全部应用，而是更新平滑后的偏移：

# 示意：首次有效 GPS 直接初始化偏移，后续再平滑更新
raw_offset = gps_position - odom_position
offset = alpha * raw_offset + (1 - alpha) * previous_offset
map_position = odom_position + offset

代码中的 alpha 为 0.02。小权重通常让校正更平缓，也会让偏移更新更慢。例如，偏移上次是 (0, 0)，这次观测是 (0.4, -0.2)，更新后只是 (0.008, -0.004)。算法没有立即接受整份修正。实际响应还取决于更新频率、数据时间差和观测是否新鲜。

为什么不直接把 GPS 位置交给 Nav2？ 本例的 GPS 适配器提供全局位置，没有测得机器人朝向和速度；它不能仅靠填入一个单位四元数，就承担完整的局部里程计职责。融合架构希望让局部运动信息支撑连续控制，再用全局观测约束漂移。你也可以设计其他定位方案，但都要回答位置、朝向、速度与观测更新之间怎样协作。

这里还有一个容易误读的细节：/odometry/filtered 的 frame_id 仍然是 odom，位置仍保留原始局部坐标；全局校正放在 map → odom 中。

因此，不能直接拿这个话题里的 XY 与世界真值相减，也不能仅凭它与 /odom 数值相同，就认定融合没有作用。评测必须先用对应时刻的 TF，把估计位置转换到共同的 map 坐标系。

一般情况下应组合完整的旋转和平移；本案例只有在旋转为单位旋转的前提下，才能直接相加。

四、案例中的独立 GPS 从哪里来

这版实现的数据流是：

Gazebo 机器狗专属世界位姿
→ /sim/dog_world_pose
→ sim_gps_publisher：坐标转换、噪声、偏置等
→ /sim/rtk/fix
→ rtk_enu_adapter：经纬度转局部坐标
→ /odometry/gps
→ 与 /odom 做互补校正

sim_gps_publisher 不订阅 /odom，也不订阅评测用的 /ground_truth/pose。融合节点只订阅 /odom 和 /odometry/gps，这使传感器生成与算法估计的职责可以分别检查。

模拟器的参数决定了这场“考试”有多难。该次运行版本 b9a944fc 的 gps_noise.yaml 配置如下：

参数	版本配置
东、北两轴高斯噪声标准差	各 0.10 m
高度噪声标准差	0.15 m
更新频率	10 Hz
固定偏置、随机游走强度	均为 0
丢包概率	0
随机种子、噪声开关	42、开启

这是版本中的配置；本地归档未提供该轮完整运行时参数快照，不能据此排除运行中覆盖参数的可能。若两轴独立、零均值且标准差各为 0.10 m，理想条件下二维噪声的 RMSE 约为 √(0.10² + 0.10²) = 0.1414 m。实测 GPS RMSE 0.1429 m 与这一量级接近，不能把它当成复杂遮挡场景下的 RTK 性能结论。这轮也没有覆盖配置中的丢包实验。

适配器使用固定地理原点，将经纬度转换为局部 ENU 坐标：东、北、上。如果地图轴向与 ENU 不一致，还需要旋转到地图坐标。该实现使用局部近似，不能直接推广成跨大范围的高精度测绘方案。

项目曾遇到另一个很隐蔽的问题：世界位姿消息里包含多个实体，实体顺序不稳定。如果直接取第一项，可能把仪表模型的位置当成机器狗的位置。

后续改成机器狗专属位姿话题，避免依赖多实体列表的排列顺序。这说明独立观测不仅要回答“有没有读里程计”，还要回答“读的是不是正确的机器人”。

话题名中的 rtk 表示接口用途；这里的模拟器生成带噪声的位置观测，没有完整模拟卫星、差分解算、多路径或 RTK 固定解状态转换。不能据此声称已经验证了真实 RTK 系统。

五、怎样证明它没有偷看答案

先检查订阅关系：

ros2 node info /sim_gps_publisher
ros2 node info /fused_odom_publisher
ros2 topic info /sim/dog_world_pose --verbose

第一项确认 GPS 生成器使用世界位姿而非里程计；第二项确认融合节点只接收允许的观测；第三项帮助追踪世界位姿的来源。

但静态订阅图还不够。更有说服力的验证方法，是在传给算法的里程计副本上注入已知偏差，同时保留独立 GPS 和评测真值，然后检查：

1. 里程计发生偏移时，GPS 是否仍跟踪世界位置；
2. 全局校正是否朝抵消该偏移的方向变化；
3. 估计与真值的误差是否下降；
4. GPS 中断后，系统是否识别观测过期，而不是继续把旧数据当新数据。

这是一套可以迁移的实验设计。下节展示的是已有运行中的漂移与校正结果，不把它写成已经完成了所有人为偏差和断流实验。

六、真实轨迹：终点误差与全程误差要一起看

下面的图表来自 2026 年 9 月 4 日同一轮五航点运行的保存轨迹，运行版本为 b9a944fc。图表根据原始轨迹重新计算，本文没有重新运行 Gazebo。

里程计仅按起始 20 个配对样本确定一次平移基准，之后保持该基准；GPS 和融合位置按地图坐标直接比较。各序列按时间最近邻配对，拒绝时间差达到 0.3 s 的样本。没有对整条轨迹反复拟合来消除漂移。

先认识三个指标：RMSE 是位置误差平方取平均再开根号，反映全程表现；最大误差检查最偏的一刻；终点误差只看最后有效配对时刻。三个指标回答不同问题。

来源	XY RMSE / m	最大误差 / m	终点误差 / m
轮式里程计	4.4731	7.6253	7.5845
独立 GPS	0.1429	0.4335	0.1217
融合位置	0.2447	0.4052	0.0362

这里的“终点误差”指估计位置与世界真值的差，不是机器人与目标航点的距离。这两个指标不能混用。

每路都得到 4324 个配对样本，本次规则下没有丢弃样本。里程计与真值的最大配对时间差为 0.010 s，GPS 为 0.052 s。保存的 fused_map 已由原采集器按世界样本时刻组合 TF 与局部估计，因此二次配对时间差为零；这不表示原始 TF 与里程计没有延迟。时间差可能引入运动误差，但仅凭这个零值，也不能断言评测必然偏向融合。

GPS 全程误差更小，这套融合还值得用吗？ 这轮数据能证明全局观测显著减小了里程计累计位置误差，却没有证明当前融合实现的全程精度优于直接使用 GPS。融合终点误差更小、最大误差略小，也不能抵消 RMSE 更大的事实。

从机制上说，低通平滑可能抑制随机噪声，也可能跟不上变化的偏移。它是解释终点与全程表现不同的一种假设；还需结合速度、偏移更新和观测年龄分析，不能只凭一个终点数值，就认定机器人已经静止收敛。

融合架构希望保留局部连续运动信息，但本例尚未完成 GPS 断流下的独立验证。尤其当前实现会使用最近缓存的观测：若不检查新鲜度，GPS 中断后继续更新旧数据反而可能带来错误。要验证断流价值，必须先补好时间同步与过期观测处理，再单独做实验。

如果沿用历史用例的 RMSE ≤ 0.20 m 门限，这轮的 0.2447 m 未达标，即使终点误差很好，也不能称为该门限下的完整通过。门限应在测试前确定，不能看到结果后换一个有利指标。

保存数据还记录了五个导航 Action 均为 SUCCEEDED。这证明 Action 返回了成功，不能替代真值下的逐点距离核验，更不能替代定位全程精度验收。

七、评测器也可能制造一个“定位故障”

这轮数据曾使用起始几乎静止的样本拟合二维旋转和平移，得到约 89.6° 的旋转，进而算出约 15.5 m 的里程计终点误差。

问题在于：机器人几乎没动，轨迹缺少可用于识别方向的运动信息，旋转拟合会被微小噪声支配。

修正分析按这轮已声明的同轴条件，将旋转固定为零，只确定起始平移，得到 7.5845 m。原始轨迹没有被改写，旧结果与修正说明也保留下来。

这不是“调个角度让结果更好看”。允许哪些对齐自由度，必须由坐标定义和实验设计决定；如果轴向未知，就应先完成标定或采集有足够运动信息的数据，不能随意固定旋转。

定位误差属于整个测量链：数据源、时间配对、坐标变换、对齐方法和指标公式，任何一环出错都可能制造假结论。

八、换到自己的项目，先检查这六件事

1. 真值只供传感器生成和独立评测；算法没有旁路读取答案。
2. GPS 来源独立于轮式里程计，并明确绑定目标机器人。
3. 所有输入使用可比较的时钟、坐标原点、轴向和安装外参。
4. map → odom 与 odom → base_link 各有明确发布者。
5. 同时报告全程误差、最大误差和终点误差，保留配对规则与丢弃样本数。
6. 噪声、偏差、掉线和恢复分别测试，结果只覆盖声明的配置与运行范围。

如果进一步加入 IMU 和完整 EKF，需要重新核对姿态参考、观测分量和协方差。官方 navsat_transform_node 使用里程计、包含可靠航向估计的 IMU 和 GPS，将地理位置转为与世界坐标一致的观测；它与本文的固定原点简化适配器承担的假设不同。官方 GPS 坐标转换说明

当前互补实现还直接使用最近缓存的 GPS 与里程计，缺少严格同步和 GPS 新鲜度门控，也不估计全局航向误差。这些是后续要补齐并验证的能力。

九、结语

Gazebo 知道机器人的真实位置，让我们有了检验定位算法的参照。要让这场检验有意义，就必须把真值、观测和估计分开：算法从带误差的传感器数据中推断位置，独立评测器再核对估计与真值的差距。

本案例展示了独立 GPS 观测对里程计漂移的校正作用，也暴露了当前互补实现的局限。终点误差、全程 RMSE 和最大误差需要共同呈现；数据来源、时间配对和坐标变换也必须能够追溯。只有在明确的工况与门限下完成验证，定位结果才能支撑可信的工程结论。

定位为机器人提供位置判断，安全监控则需要在数据失效时约束它的行为。下一篇《让机器人安全失败：点云中断、TF 超时和控制器崩溃怎么测》，我们将继续讨论如何检测故障、及时停车，并通过故障注入留下可检查的证据。

• 上一篇：一个巡检点能到，五个为什么会乱：Nav2 多航点任务与状态机实战
• 下一篇：让机器人安全失败：点云中断、TF 超时和控制器崩溃怎么测