TD25A 持续融合定位：待解决问题与当前替补方案

状态：T3 全局定位源码取舍已完成离线确认；T2 持续融合尚未完整实现， 尚未实车验收。

本文同时记录 TD25A 定位 T2 的设计决定和 T3 的源码取舍。T3 采用 Jetson 当前实现不等于 T2 的退化闭环已经完成，不能把尚待验证的行为写成稳定功能。

一、已经确定的目标

TD25A 的局部定位最终采用一套持续工作的融合估计器：

• 轮式里程计、IMU、激光里程计始终进入同一套局部状态估计；
• 只有这套估计器可以发布 odom → base_footprint，禁止第二个节点争抢同一条 TF；
• 激光健康时，用激光约束轮式里程计的累计漂移；
• 激光退化或短时丢失时，不停车，继续以轮式里程计和 IMU 推进局部位姿；
• 激光恢复后，自动修正退化期间积累的误差，不要求人工重启定位；
• 检测到车轮打滑时，自动降低轮式里程计权重，更多依靠激光和 IMU，同时发出打滑警告；
• 激光退化和车轮打滑都采用“继续导航、明确提示”的策略，不把普通退化直接升级为停车故障。

导航侧公开的 odom → base_footprint 必须严格二维：

• z = 0；
• roll = 0；
• pitch = 0；
• 只保留平面 x、y 和 yaw。

估计器内部仍保留六自由度状态，供激光匹配、IMU 融合、点云处理和诊断使用。 内部三维状态不得再以第二条、同 child 的 TF 形式对外发布。

二、确定的 TF 职责

主链保持：

map → odom → base_footprint → base_link → sensors

各段职责如下：

TF	责任
map → odom	承担全局定位修正和大误差闭环，不要求局部里程计瞬间跳变
odom → base_footprint	唯一的连续局部二维运动结果，由轮速、IMU、激光融合估计器发布
base_footprint → base_link	固定的离地高度关系
base_link → sensors	机器人实体、雷达、相机、IMU 和机构安装关系

坐标语义也已经确定：

• base_link：机器人运动学基准原点，位于左右驱动轮轴中心；
• base_footprint：base_link 在地面上的垂直投影；
• 链路方向仍为 base_footprint → base_link。

本地 T1 候选已经按这个定义处理：

base_footprint → base_link = [0, 0, 0.099986112]

URDF、Xacro 和 STL 采用 Jetson 版本作为实体基准，仅重新表达基座坐标。 7 个 STL 与 Jetson 逐字节一致；两份展开 URDF 和 Xacro 均通过结构检查， 共 52 个 link、51 个 joint，原有实体与传感器的世界安装位置保持不变。 这些修改目前只存在于本地候选目录，尚未部署到 Jetson。

三、T3 已确认的全局定位后端

2026-07-29 完成 T3 源码取舍：导航模式采用 Jetson GPU 重定位作为生产后端， 由它唯一发布 map → odom。Jetson C++ Open3D 源码保留为人工备用， 但默认不启动，禁止两个后端同时运行。

正常导航通过 nav_robot_bringup.launch.py 的同一个 use_gpu_relocalization 开关选择后端：

开关	启动后端	map → odom 发布者
true（默认）	Jetson GPU 重定位	gpu_relocalization_node
false	Jetson C++ Open3D	global_localization

两个分支分别受 IfCondition 和 UnlessCondition 控制。这个互斥只对统一 bringup 有效；操作者仍不得绕过它，手工同时执行两个独立 launch。

T3 同时确定：

• GPU 和 C++ Open3D 都不得发布 map → motion_link；
• motion_link 只保留 URDF 的 body → motion_link 静态父链；
• /motionlink2map 只作为诊断里程计；
• GPU 对公开 map → odom 做 SE(2) 投影；
• GPU 在定位完成且机器人静止后抑制有界小修正，减少 Nav2/RViz 原地抖动；
• C++ Open3D 使用 base_footprint，并校验 /state_estimation 必须满足 odom → base_footprint；
• C++ Open3D 等待 motion_link ← base_footprint 固定 TF，超时则停止；
• C++ Open3D 的点云源 frame 使用 fastlio_imu_body。

本地候选的 GPU 节点、GPU ICP、GPU launch、C++ Open3D 源码和 launch、 导航总 bringup 六个关键文件，与 Jetson 当前源码 SHA256 一致。新增的本地 契约检查确认了后端互斥、唯一 map → odom、禁止 map → motion_link 和 frame 合约，共 6 项离线检查通过。

这表示“T3 取舍和离线源码合同完成”，不表示重新部署或新增了一次实车验证。 建图模式中的静态 identity 与 SC-PGO map → odom 归属，也不属于这次 导航 T3 结论。

四、退化运行与自动恢复

1. 激光健康

轮速、IMU、激光持续融合。轮速提供短时平滑运动，IMU提供角速度和姿态约束， 激光持续压制轮速积分漂移。

2. 激光退化或丢失

进入“退化运行”，但不停车：

• 保持轮速与 IMU 推算；
• 保持 odom → base_footprint 连续发布；
• 向上层明确报告激光退化；
• 允许平面位置误差随时间逐渐累积；
• 不重置 odom，不冻结 TF，不制造虚假的“定位仍正常”状态。

如果轮速同时被判断为打滑，则进一步降低轮速权重并发出打滑警告。 此时是否还能安全继续取决于剩余传感器健康度和导航安全边界， 但“单独发现打滑”本身不触发停车。

3. 激光恢复

采用分级闭环修正：

• 小误差：由融合估计器平滑吸收，避免控制器看到突跳；
• 大误差：通过 map → odom 完成全局修正；
• odom → base_footprint 保持局部连续，不直接清零或瞬移；
• 修正完成且健康状态稳定后，自动退出“退化运行”。

这套分级方式兼顾两件事：局部控制需要连续，地图坐标又必须最终回到正确位置。

五、目前的替补方案

在正式 T2 状态机和验收全部完成以前，先沿用 Jetson 现有定位链中的可用部分：

1. FAST-LIO 继续作为 odom → base_footprint 的唯一发布者。
2. 编码器 /odom 继续作为轮速输入参与现有融合。
3. 保持平面导航 TF 开关开启，对外提供二维 odom → base_footprint。
4. 六自由度估计结果通过独立的 /state_estimation 保留，不加入第二条公开 TF 父链。
5. 暂时使用现有轮速创新量、健康度和自适应协方差逻辑降低异常轮速的影响。
6. GPU 全局定位继续维护 map → odom，承担地图坐标中的长期漂移修正。

这个替补方案只表示“已有机制可先用”，不代表下面这些最终能力已经确认：

• 激光退化是否有完整、稳定的状态判定；
• 退化提示是否已经形成统一的 ROS 话题或诊断接口；
• 长时间无激光时轮速与 IMU 是否始终连续；
• 打滑降权是否覆盖湿滑地面、单轮空转和编码器异常；
• 激光恢复后的大小误差分级是否满足控制连续性；
• 大误差是否始终只落在 map → odom，且不会造成 TF 竞争；
• 整套行为是否已经通过实车故障注入测试。

因此当前运行时仍应把定位健康状态当作需要重点观察的诊断项。

六、严格二维公开 TF 留下的待解决问题

这次先确定“Nav2 只使用严格二维公开 TF”，但它会留下一个必须以后解决的接口问题：

激光和 IMU 的内部估计天然包含 z、roll、pitch。一旦公开 odom → base_footprint 把三者强制归零，坡面姿态、车体俯仰、点云去畸变和三维 定位诊断就不能再从这条 TF 读取，否则会把二维导航投影误当成真实三维姿态。

当前替补约定是：

• Nav2、局部/全局代价地图和底盘控制只读取严格二维 TF；
• 激光匹配、点云处理与诊断读取估计器内部状态或 /state_estimation；
• 不为了暴露三维姿态而给 base_footprint 或 base_link 增加第二个动态父节点；
• base_footprint → base_link 仍保持 URDF 中的固定高度，不把实时俯仰混入机器人实体树。

以后需要专门决定一个不破坏 TF 单父链的三维状态接口，并逐项核对所有消费者：

1. 点云去畸变究竟读取哪个时间同步的三维状态；
2. 三维地图匹配和导航投影是否使用同一时刻、同一平移基准；
3. 坡面或地面不平时，二维投影的 x/y/yaw 应如何从三维位姿计算；
4. 是否需要独立诊断 frame；若需要，它不能成为导航主链的另一个父节点；
5. 录包、可视化和故障诊断如何同时显示“真实三维状态”和“导航二维状态”，避免混淆。

在这些问题解决前，严格二维 TF 是导航侧的明确规则，独立六自由度状态是暂时替补。

七、尚待实现的接口与阈值

以下内容尚未最终定值：

• 激光健康、退化、丢失、恢复的判定窗口与迟滞时间；
• “退化运行”和“车轮打滑”的 ROS 诊断话题、消息字段及 UI 提示方式；
• 轮速降权的最小/最大协方差和恢复速度；
• 激光恢复后“小误差平滑吸收”与“大误差转交 map → odom”的分界阈值；
• 大误差修正的最大速度、最大角速度及 Nav2 控制期间的保护方式；
• 多种传感器同时异常时，从“不停车退化”升级到安全停车的最终边界。

这些参数必须由日志和实车故障注入结果决定，不能只凭静态代码推断。

八、后续验收标准

正式宣布 T2 完成前，至少要验证：

1. 全系统只有一个 odom → base_footprint 发布者。
2. 公开 TF 的 z、roll、pitch 在约定误差内始终为零。
3. 人为切断激光输入后，TF 不冻结、不跳零，机器人继续低风险导航并提示“退化运行”。
4. 模拟或制造轮胎打滑后，轮速权重下降、有打滑提示、机器人不因单一打滑事件停车。
5. 激光恢复后，小误差平滑收敛，控制器无明显速度突变。
6. 大误差通过 map → odom 闭环修正，odom → base_footprint 保持连续。
7. 激光丢失、轮速打滑、两者重叠和恢复四类录包都能复现并解释状态切换。
8. Nav2、代价地图和控制器不因三维姿态或恢复修正出现倾斜、跳图或重复 TF。

只有源码审计、离线回放和低速实车测试全部通过后，本文状态才能从“待解决”改为“已验证”。

九、定点导航恢复动作的倒车安全链（2026-07-29，待实车）

1. 需求与当前决定

定点导航遇到人员或动态障碍进入车体旋转扫掠范围时，先停车并在 UI 显示 30 秒倒计时；提示必须同时说明“正在等待的原因”和“倒计时结束后将做什么”。 障碍在倒计时内离开后，需经过稳定清空判定再重新规划；连续占用 30 秒后， 才允许 MPPI 评估脱困轨迹，并由 UI 报告最终选择、预计移动距离和原因。

恢复阶段不预先把 MPPI 限死为直行、原地转向或某个固定方向；在安全约束已经 被实际实现并验收的方向上，MPPI 可以自由选择前进、后退、弧线或旋转。 每条候选恢复轨迹还必须满足：轨迹执行期间不得缩短与阻挡人员的距离。

这里仍有一个未完成接口：当前系统没有已经验收的人员身份跟踪，暂时不能证明 “同一个阻挡人员”的距离在整条轨迹上单调不减。后续要么补人员跟踪，要么采用 更保守的替代规则，对恢复区域内全部动态或未知障碍执行不接近约束。

2. 为什么目前不能确认倒车安全

2026-07-29 的源码审计和只读现场取样只能证明“后方有部分传感器硬件基础”， 不能证明“自主倒车安全链已经闭环”：

• /chassis/ultrasonic 当次约 42 Hz，connected_mask 与 valid_mask 都为 767（0x2FF）；后向的 CON1、CON5、CON8 在该次取样中有数据， 但尚未做遮挡、断线、冻结值和误报测试，一次有效取样不能当作长期健康结论；
• /td25a/ultrasonic_safety_available、 /td25a/ultrasonic_safety_ready 和 /td25a/ultrasonic_safety_armed 当时均没有发布者；
• 后置 RGB 图像话题 /rear_camera/image_raw/compressed 当时没有发布者， 且 RGB 相机本身不能替代后向深度防撞；
• 当前硬件启动入口只启动底盘驱动，驱动直接消费最终 /cmd_vel；没有一层已经 验收的、失效即停车的超声与点云联合安全过滤；
• 当前 component_poses.yaml 没有完整的超声通道安装映射，旧版安全控制节点 还依赖当前候选中缺失的运动契约，不能直接重新启用；
• 障碍过滤仍存在 frame 合约问题：输入点云实际位于 fastlio_imu_body，部分逻辑却按 base_link 直接计算；在修正安装变换、 自体过滤和动态 footprint 前，不能把其结果作为近车倒车保护证据；
• 旧导航配置的正常 MPPI vx_min = 0，本来就没有完成常规倒车控制验收。

因此，传感器数量并不等于安全链成立。现在缺的是从传感器健康、坐标正确、 障碍判定到最终速度截断的整条闭环，以及对其失败行为的验证。

3. 当前替补限制

定点导航的软件结构和不动车测试可以继续，不必等整套后向安全链完成；但在该链 通过验收以前采用以下边界：

1. 可以保留“MPPI 将来可自由选择倒车”的设计接口；
2. 配置和实车运行中暂时拒绝所有 linear.x < 0 的自主恢复候选；
3. 不把当前超声取样或点云可见性写成“已具备安全倒车”；
4. 旋转扫掠范围被人员占用时仍必须输出零速，并按 30 秒倒计时与 UI 故障提示 规则处理；
5. 允许继续做编译、参数检查、轨迹生成、录包回放和零速联调；禁止以人员充当 倒车障碍物进行未验收实测。

这项临时限制只冻结“自主倒车执行”，不阻塞定点导航其余模块的梳理和实现， 也不改变最终希望由 MPPI 在已验证安全范围内自由选轨迹的目标。

4. 后续需要补齐和验证

正式开放 MPPI 倒车候选前，至少完成：

1. 修正 fastlio_imu_body → base_link 的点云变换，并复核自体过滤、盲区和 动态 footprint；
2. 明确 CON1、CON5、CON8 的真实安装方向、有效量程和覆盖重叠，逐个完成遮挡、 拔线、超时、冻结安全值、异常跳变和恢复测试；
3. 在最终 /cmd_vel 到达底盘驱动前建立唯一的失效即停车安全过滤： 同时检查点云碰撞、超声距离、消息新鲜度和传感器健康；实现形式待定， 不强制重新引入 Gateway；
4. 先用仿真、录包和假消息确认每种异常都会截断速度并向 UI 报告原因；
5. 再用固定软质障碍物进行上电静态和悬空轮测试；
6. 最后在现场重新授权后，才进行有人监护、低速、短距离的实车倒车验收， 初始建议 0.02～0.04 m/s、单次 5～10 cm，不得用真人测试保护边界；
7. 验证全部通过后，才允许移除 linear.x < 0 临时禁用，并把本文状态改为 “已验证”。