项目背景

多机器人协同 SLAM（如 DCL-SLAM、Swarm-SLAM 等）通过跨机器人相对观测与约束共享，可以扩大覆盖范围并提升建图精度。但很多系统默认一个隐含前提：各机器人局部里程计在大多数时间是可靠的。

在真实部署中，这个前提经常被打破。典型退化环境包括地下矿道、长廊、仓库过道、对称隧道等：点云/特征在某些自由度方向上约束不足，导致局部估计快速发散。此时协同系统若仍按常规方式引入跨机约束，容易出现“坏节点拖垮好节点”的级联失败。

本项目的目标是：让协同系统在存在退化节点时依旧保持健康节点的轨迹稳定，并尽可能挽救退化节点。

问题定义

当 B 车在退化场景里程计发散后，跨机相对位姿约束会把误差反向传播到 A 车： B 车漂移 → 跨机相对观测因子 → A 车被拉偏 → 全局地图/轨迹崩溃

模块设置

• M1：ROI-Tracker
• M2：Scan-to-Model
• M3：B-spline Relative Velocity Estimator
• M4：Factor Builder

S0 实验场景

场景描述

• 水平地面+交叉墙体+立方体障碍物。
• A 机器人静止，B 机器人在一个方向来回摆动。其中，A 的位姿已知（附带一定高斯误差），A 具有激光雷达传感器，B 没有任何传感器。因此可以采用 SO2 流形约束。

误差参数设置：

• 激光雷达：16线，扫描线高斯误差 0.008m

无 NHC 跟踪效果

• 利用历史的位姿先验作为输入，X-Y 方向定位结果良好，偏航角在特定情况下会定位失误（MAX_ERROR = 0.6 rad, 约等于34度）。
• 观察数据发现，偏航的大误差集中在特定的时间节点，且此时 B 机器人开始远离 A，扫描有效的匹配点从 40 个降低到 30 个左右。

NHC 跟踪效果

• yaw 维度上的误差被大幅度降低，最大误差为 0.1 rad，约等于 5 度 （可以粗略地认为降低了 80%）
• 当然，这依赖于比较强的先验假设，我们需要没有太多的侧向滑坡