让机器人安全失败：点云中断、TF 超时和控制器崩溃怎么测

机器人巡检仿真------从方法到可验收实现 · 07/10

定位：以真实电厂机器狗巡检项目为贯穿案例，提炼可迁移的机器人巡检仿真开发方法。

适合读者：机器人、自动化、计算机、电子信息等专业学生，以及准备进入 ROS 2、机器人软件、仿真、导航或测试方向的求职者。

上一篇：Gazebo 已经知道机器人在哪，为什么还要做定位融合？

下一篇：一个 90° 旋转如何搞坏整套导航：Gazebo 轮轴 Bug 完整复盘

机器人正朝巡检点前进，控制器突然退出了。导航程序不再发消息，机器人就一定会停下来吗？

这取决于后面的命令链如何处理旧指令。如果下游仍保存着最后一条非零速度，而系统又没有独立的超时保护，"没人再发命令"就可能变成"继续执行旧命令"。点云中断、关键坐标变换失联，也会让机器人在信息不足的情况下继续运动。

上一篇，我们讨论了定位结果如何可信。这一篇继续追问：当系统失去可信输入时，谁来阻止它继续行动？

本文给出故障实验的测量设计，并结合电厂机器狗的历史记录说明哪些结论有证据、哪些还需补齐。

一、先把"停车权"放到明确的位置

如果控制器、安全节点和测试工具都向同一个最终速度话题发布，零指令与非零指令就可能交错到达。不能指望安全节点"多发几次零"就一定压住其他发布者。

案例中把速度链串起来。机器狗保留四足外观，运动由隐藏的差速底盘代理；最终命令交给 Gazebo 差速驱动插件，并非四足步态控制器：

text 复制代码

controller_server
→ /cmd_vel_nav
→ collision_monitor
→ /cmd_vel_raw
→ safety_monitor
→ /cmd_vel
→ Gazebo 差速驱动

collision_monitor 根据启用的观测源和保护区域检查障碍物；safety_monitor 再检查命令是否过期、点云与关键 TF 是否持续到达，以及配置中的速度等约束。底盘接收的是经过这些环节处理的最终指令。

这个设计成立的前提，是控制器和其他命令源不能绕过安全门，直接给底盘发送速度。可以用下面的命令核对最终话题的发布者，再沿桥接与重映射检查实际接线：

bash 复制代码

ros2 topic info /cmd_vel --verbose

即使这样，监控节点自身退出时仍需要下游看门狗兜底。本文的 ROS 软件保护链也不能代替真实设备的驱动器保护或硬件急停。Nav2 官方同样明确，Collision Monitor 不提供硬实时安全认证。

二、命令过期、故障报警和停稳是三件事

先区分几个容易混用的概念：

看门狗：持续检查某个被监控对象是否按预期更新。
超时：超过预先设定的时间，没有等到符合条件的更新。
命令过期清零：旧速度不能再被转发，输出改为零；不一定同时宣布任务故障。
失效安全：系统检测到某类异常后，进入事先定义的受限行为，例如保持零速度。

当前案例启动配置中的部分参数如下。它们是配置值，不是本次实测停车时间：

检查项	配置值	作用
命令过期清零	0.5 s	`/cmd_vel_raw` 长时间不更新时，停止转发旧命令
控制器命令超时	3.0 s	在附加条件满足时判定 `CMD_VEL_TIMEOUT`
点云更新超时	1.0 s	判定 `POINTCLOUD_TIMEOUT`
被监控 TF 更新超时	0.5 s	判定 `TF_TIMEOUT`
`/clock` 报文中断	0.3 s	进入 `CLOCK_STALLED` 安全保持

因此，不能把"控制器超时为 3 秒"解释为"机器人要等 3 秒才收到停车指令"。如果安全门输入先停止更新，0.5 秒的命令过期保护就可能先清零；如果碰撞监控仍在发零，底盘也可能更早收到零指令。究竟是谁先起作用，要看记录。

回到此案例：我们切断的是点云，不是命令。对应门限是表中的"点云更新超时"，不能拿同为 0.5 秒的命令过期或 TF 门限解释它。多个保护可以同时存在，测试仍要辨认实际生效的环节。

监控循环的默认频率是 20 Hz，周期约 50 毫秒。这只能描述名义检查节奏；调度、通信、桥接与执行器响应都会影响实际延迟。参数越界被发现、最终话题输出零、机器人实际停稳，必须分别测量。

三、仿真暂停时，看门狗不能跟着"睡着"

假设超时判断和定时器都依赖仿真时间。当 /clock 不再推进，机器人世界停住了，检查"已经多久没有消息"的程序也可能不再运行。

案例为此使用两种时间：

用途	使用的时间
定时检查与消息接收间隔	稳定推进的单调时间
运动时间差、传感器数据与运动的对齐	仿真时间

监控定时器采用 STEADY_TIME，点云、TF 和命令回调记录 time.monotonic()。即便仿真时间停止，检查循环仍有机会发现报文中断并持续输出零速度。

前者负责在 ROS 中调度检查，后者是 Python 计算接收间隔的接口；它们都不依赖仿真时钟推进。这里不需要把两种 API 的绝对读数相减，也不能据此直接比较不同主机的时间值。

当 /clock 与传感器一起停发时，代码优先进入 CLOCK_STALLED，暂不把所有传感器都报成独立故障。这样可以把"整个仿真暂停或时钟链路中断"与"仅某个传感器失联"区分开。

不过，当前 /clock 看门狗检查的是报文接收间隔。如果报文持续到达，里面的时间值却一直不变，仅靠接收时间就不能识别这一异常。完整检查还应验证时间值是否推进、是否回退。这是现有保护的边界，不能因为诊断码叫"时钟停滞"，就认为所有时间故障都已覆盖。

同样，代码中的加速度检查基于命令速度及仿真时间差，它不等于测得了机器人本体的物理加速度。

四、机器人为什么会把自己当成障碍物

开始之前前，还要先排除"点云本来就在触发停车"的情况。否则，暂停桥接后机器人仍不动，无法分辨是新的失联保护动作，还是原有误触发一直没有解除。

LiDAR 可能扫到机身、腿部或传感器支架。如果这些点进入碰撞保护区域，碰撞监控可能正确执行停止规则，却让机器人一直无法启动。

案例保留原始点云，另开一条供碰撞监控使用的过滤链：

text 复制代码

/sim/lidar/points
├─ 原始点云继续保留，供其他模块使用
└─ self_filter → /collision_monitor/points
               → collision_monitor

过滤器先把点坐标变换到 base_link，再根据配置的机体平面区域和高度范围删除自身回波。判定使用机体坐标，输出消息仍保留原始点云的坐标系与字段。

但扩大删除区域也会制造感知盲区。调参时应同时检查：自身点是否消失，紧邻机体的墙、设备和临时障碍物是否仍被保留，以及 TF 不可用时过滤器如何退化。当前实现有缓存外参及无缓存时直通原始点云的分支，不能仅凭"过滤器在运行"判断其结果正确。

还要检查 QoS。案例点云链使用 BEST_EFFORT；订阅端如果要求发布端无法满足的可靠性，就可能收不到数据。ROS 2 的 QoS 兼容规则决定端点能否通信，话题名相同不代表消息一定可达。

另一个容易忽略的问题是观测源是否真的启用 。仓库中的普通与融合导航配置，对碰撞监控点云源的启用值并不相同。查实际启动参数与有效配置，比看到配置文件里写着 FootprintStop 更有意义。

五、故障注入要一次只切断一条链

要验证点云中断，就只暂停承担点云桥接的进程，同时确认 /clock 与关键 TF 仍在更新。若把整个桥接进程树一起停掉，看到的可能是时钟故障，无法证明点云超时逻辑生效。

三类用例的实验条件可以这样设计：

故障	注入方式	同时确认
点云中断	对独立点云桥接进程发 `SIGSTOP`	时钟、TF 和测试采集器仍正常
关键 TF 中断	对对应 TF 桥接进程发 `SIGSTOP`	点云与时钟继续更新
控制器崩溃	终止独立控制器进程	自动重启关闭，其他节点仍存活

SIGSTOP 是暂停进程，SIGCONT 可以让其继续执行；终止控制器则用于模拟无法再正常发送收尾命令的退出。通过正常生命周期关闭节点，可能先发出停车指令，不能等价替代崩溃实验。

测试程序应自己启动并持有目标进程的准确 PID，记录进程身份与退出状态。按名称模糊匹配并终止进程，容易误伤其他运行。每轮还应使用独立的 ROS 通信域与 Gazebo 分区，避免外来的时钟或 TF 把失联故障掩盖掉。

注入前必须先确认机器人确实在运动，而且链路健康。机器人本来就没动时，"故障后速度为零"几乎没有证明力。

六、历史用例记录了什么，又没证明什么

先把案例中的已有记录连起来看。报告记载：对点云桥接进程执行了 SIGSTOP，观察到 POINTCLOUD_TIMEOUT、进入恢复状态以及最终 /cmd_vel 为零；执行 SIGCONT 后，点云重新发布，报告记录了恢复。

这条记录描述了"切断输入---保护动作---输入恢复"的过程。但注入前的准确初速、报警与零指令谁先出现、机器人何时真正停稳，都缺少足够的统一时间记录。因此，前面的"先确认运动和基线健康"是重做实验时必须落实的要求，不能倒过来当成这份旧记录已完整保存的事实。

其他两类用例可以与它并排比较。归档审计将这些测试关联到历史冻结版本 0a235a3：

用例	报告记录的故障码	报告记录的行为
TC-05：暂停点云桥接	`POINTCLOUD_TIMEOUT`	最终速度命令归零，恢复点云后退出故障流程
TC-08：暂停 TF 桥接	`TF_TIMEOUT`	最终速度命令归零，恢复后出现导航成功记录
TC-07：终止控制器	`CMD_VEL_TIMEOUT`	控制器未重启，最终速度命令归零，其他节点继续运行

这些记录支持"对应历史用例观察到了超时处理和命令清零"，还不足以给出停车延迟的平均值、最坏值或稳定性保证。点云报告中的"首次停止约 0.6 秒"缺少足够时间基准说明，本文不把它当成检测延迟。

此外，当前仓库的 run_fault_tests.py 对这三类进程级注入保留了 PENDING 分支，原因是独立进程启动和 PID 跟踪尚未接好。历史报告存在，不代表现在执行该脚本就能复现一轮通过；本次也没有执行这些注入。

碰撞监控的记录同样需要谨慎解释：它记录过上游命令非零、下游命令被改为零，但还暴露了里程计与世界位置明显不一致。它可以作为命令拦截的证据，不能扩展成"已验证所有障碍物距离与无碰撞停车"。

七、怎样把"发了零"变成可验收的停车证据

如果重做此案例，需要分别捕捉四类事件：

text 复制代码

t_inject：故障确实发生
t_detect：监控发现异常
t_zero：最终命令进入持续零速
t_stop：机器人实际速度低于门限并保持

只有拿到可比较的内部检测时间，t_detect - t_inject 才能用于描述检测延迟；t_zero 与 t_stop 则分别关注命令响应与实际运动。四类事件不预设先后：命令过期保护可能先清零，故障码稍后才出现。

用一个采集器统一记录"观察时刻"

一种可实施的设计，是把注入控制与数据采集放在同一主机、同一测试进程中，用自身单调时钟记录：

记录项	采集方式
`t_request` / `t_inject_confirmed`	发信号前记录请求时间；查询准确 PID 的暂停或退出状态后，另记确认时间
`t_fault_observed`	订阅 `/safety/status`，第一次收到目标故障码时记录
零命令窗口	订阅最终 `/cmd_vel`，记录线、角速度均低于约定门限的窗口起点与确认时刻
停稳窗口	使用独立运动观测，记录实际线、角速度满足门限的窗口起点与确认时刻

这样可以比较 t_fault_observed - t_request 等采集器观察延迟。但第一次看到报警不等于节点内部检测时刻，发出信号也不等于故障立刻生效；请求与确认应一起保留。只有另行对齐内部事件时间，才能进一步拆分检测、通信与调度开销。

实际速度从哪里来

仿真中可以单独桥接机器人的世界位姿，用相邻位置差除以仿真采样时间差得到平面速度，并结合姿态变化检查转动；也可以接入独立测速来源。不要用期望速度代替真实运动，原地旋转也不能判为停稳。

采集器同时保存原消息时间戳与自身接收时间。位置差分窗口、坐标系、速度门限、保持时长及最大允许消息间隔都要写入记录。窗口内断流应标为未知，不能凭"没收到非零消息"认定持续为零。若把窗口确认时刻记作 t_stop，报告的响应时间就包含保持时长，应与窗口起点分别列出。

再从世界轨迹统计停车距离，记录初速与重复次数。接收时间统一，只解决观察记录的共同基准，并未消除桥接延迟。仿真暂停时世界本来就不推进，"位置没变"也不能证明物理制动能力。以上是补测方案，本次没有新增实测结果。

观察对象也要选对。当前速度诊断中的 final_lin 来自内部滤波变量，并非最终发布的 /cmd_vel 实测值。故障分支可以直接发布零 Twist，而该内部变量尚未变零。因此，测试应实际订阅最终话题，同时独立检查真实运动，不能只抄一个名字带 final 的字段。

八、恢复也是安全逻辑的一部分

故障后保持零速度，还要决定谁有权恢复运动。候选条件包括：输入重新连续有效、关键 TF 可用、控制器确实恢复、旧目标已处理、新命令属于当前任务。

案例有 FAULTED 与 RECOVERING 状态，在这些处理分支中持续发布零。当前启动入口打开了自动恢复，而导航 YAML 中另有参考配置；运行策略应以节点实际参数为准。

实际执行的报告记录了恢复，但回到 NORMAL 仍不等于恢复条件已经完整验证。当前实现还需针对以下问题补测：

控制器是否真的恢复？ 恢复函数未完整复核命令活性，传感器恢复不能代替控制器恢复。
每条关键 TF 是否都健康？ 多个被监控关系共用更新时间，一条更新可能掩盖另一条失联。
活性是否来自控制器自身？ 巡检状态也会刷新其活性时间，崩溃判定还有末次线速度等附加条件。

这些边界用例应分别验证，不能由一次停车成功推导出所有恢复场景可靠。

迁移到其他项目时，可以先带走这六个检查点：

最终命令来源明确，各种控制模式都经过安全仲裁。
旧命令有过期策略，安全节点自身失效有下游保护。
接收活性、数据时间戳和关键 TF 关系分别检查。
自过滤删除自身点，同时验证近处真实障碍物仍可见。
故障实验只切断目标链，并核对检测、零速、停稳与距离。
故障解除与恢复授权分开验证，避免旧指令或旧任务重新生效。

结语

机器人能够完成巡检，说明正常流程有了结果；当感知、坐标或控制失效时，系统如何收住动作，则决定了异常流程能否被解释和验证。

这套案例提供了明确的命令链、独立计时的活性检测和历史故障记录，也留下了进程注入自动化、逐项健康检查与物理停稳测量的补齐方向。可信的安全结论，需要从"代码里有保护"继续走到"故障确实发生、保护确实动作、机器人行为符合门限"。

上一篇：Gazebo 已经知道机器人在哪，为什么还要做定位融合？
下一篇：一个 90° 旋转如何搞坏整套导航：Gazebo 轮轴 Bug 完整复盘