结论：`MultiWheelC\Fleet` 当前的数学链路作为“第一版组件”是成立的，现有 Fleet 数学测试共47个场景全部通过，没有发现明显的坐标系或符号错误。但它还不是可以直接上双车实车的完整工程方案，`FleetStateEstimator` 的等权平均尤其需要增加鲁棒性。 ## 正负平均为0是不是问题 不一定，很多时候这正是期望结果。 假设两辆车反算出的车队中心分别为： ```text 车辆1候选中心：+20mm 车辆2候选中心：-20mm ``` 平均得到： ```text 车队中心：0mm ``` 但代码随后还会计算每辆车相对于这个中心的布局误差： ```text 车辆1相对误差：+20mm 车辆2相对误差：-20mm ``` 所以并不是误差消失了，而是被分成了： ```text 公共分量：车队整体中心运动 相对分量：成员之间的队形变形 ``` 这正好符合当前架构： - 公共分量交给 `FleetController` 控制车队中心； - 相对分量交给 `FleetCoordinator` 减速，以及 `FleetMemberCommandCorrector` 小幅纠偏。 如果两辆车反算结果都是 `+20mm`： ```text 平均中心：+20mm 成员相对误差：接近0 ``` 这表示整个车队共同移动了20mm，而队形没有变，也符合物理意义。 ## 当前不是“无条件直接平均” 当前 [FleetStateEstimator.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Fleet/FleetStateEstimator.cs:277) 实际执行的是： 1. 将成员状态对齐到统一时刻。 2. 每辆车根据固定布局反算候选车队中心。 3. 检查任意两辆车反算的中心位置差和航向差。 4. 差异超过阈值，直接返回状态不可用。 5. 只有所有候选结果基本一致，才平均： - X/Y算术平均； - Yaw圆周平均。 6. 根据融合后的中心重新计算每辆车的布局误差。 所以如果： ```text 车辆1：+200mm 车辆2：-200mm ``` 两者相差400mm，只要超过 `_maximumPositionDisagreementMeters`，代码会在平均前拒绝本周期状态，不会把它们平均成0后继续运行。 Yaw也不是普通平均。例如： ```text +179°和-179° ``` 圆周平均会得到接近 `±180°`，而不是错误地得到 `0°`。 ## 当前平均真正的问题 它隐含了一个假设： > 所有成员定位精度相同、可信度相同，而且没有异常值。 对你目前的 Detour 情况，这个假设不够可靠。 假如真实车队中心是0，而： ```text 车辆1定位正常：0mm 车辆2发生小范围跳变：+60mm ``` 当前等权平均会得到： ```text 估计中心：+30mm ``` 如果60mm还没有超过候选差异阈值，这个错误会进入车队中心控制。 当前估计器存在三个主要限制： - 所有成员等权，没有利用定位质量、数据年龄和历史稳定性； - 单个异常成员超过阈值时，会使整个车队状态不可用，不能隔离坏成员； - 它是单帧估计，没有利用上一周期车队中心和轮速预测判断哪辆车更可信。 而且你目前主要是双车。两辆车互相矛盾时不存在“多数票”： ```text 车1说中心在A 车2说中心在B ``` 只看这两个当前观测，无法判断谁正确。必须引入： - 上一周期车队状态； - 轮速运动预测； - 每辆车定位健康状态； - 数据新鲜度； - 连续多帧确认。 ## 更成熟、适合你的方案 我建议采用： > 带时间预测和异常门控的鲁棒加权 SE(2) 融合。 不需要现在就上复杂的因子图或完整EKF。 ### 第一步：保留当前反算方法 继续让每辆车计算： ```text 候选车队位姿 = 成员实际世界位姿 × 成员固定布局位姿的逆 ``` 这部分当前是正确的，不需要推翻。 ### 第二步：增加上一状态预测 根据上一周期车队中心和轮组速度预测： ```text PredictedFleetPose ``` 然后每辆车的候选中心分别与预测值比较： ```text 位置创新 航向创新 ``` 这样双车出现分歧时，可以判断： ```text 车1与预测连续 车2突然跳变 → 暂时拒绝车2，而不是两车平均或全队立即失败 ``` ### 第三步：成员单独门控 每个成员分别检查： - 状态是否可用； - 数据是否过期； - 相对预测的位置创新是否合理； - 相对预测的航向创新是否合理； - 是否连续多帧异常； - 后续是否连续多帧恢复。 不要只做当前这种“成员之间两两比较”。 ### 第四步：对通过门控的候选加权融合 权重可以先用简单工程等级： ```text 健康且新鲜 → 权重1.0 轻度退化或较旧 → 较低权重 正在异常确认 → 权重0 ``` 以后 Detour 如果能提供协方差，再改成按协方差加权。 对残差使用 Huber 一类鲁棒损失会让正常小误差仍按最小二乘处理，而异常大残差的影响被压低；这是成熟估计库采用的标准做法，[GTSAM官方文档](https://gtsam.org/doxygen/a04439.html)也明确给出了 Huber 在迭代重加权最小二乘中的权重形式。 ### 第五步：输出融合健康等级 建议以后区分： ```text Healthy 所有有效成员一致，正常融合 Degraded 有一个成员被拒绝，暂时依赖剩余成员和预测，整队限速 Unavailable 所有成员都不可信，或双车分歧且无法判断谁正确 短时保持轮速预测，随后停车 ``` ## 更严格的数学形式 成熟版本可以直接求一个最符合全部刚体观测的车队位姿： \[ T_f^*= \arg\min_{T_f} \left[ w_p\|\log(T_{pred}^{-1}T_f)\|^2+ \sum_i w_i\,\rho\left( \|\log((T_fT_{layout,i})^{-1}T_{actual,i})\|^2 \right) \right] \] 它表示： - `T_f`：要求解的车队实际中心； - `T_layout,i`：第i辆车的固定布局； - `T_actual,i`：第i辆车当前测量位姿； - `T_pred`：根据历史和轮速得到的预测中心； - `w_i`：成员可靠性； - `ρ`：Huber等鲁棒损失。 这是“鲁棒刚体拟合”，比单纯平均更完整，同时利用位置和航向。普通最小二乘刚体配准本身就是成熟方法；鲁棒损失是在异常观测下限制单个数据影响的标准扩展。[刚体点集最小二乘原始问题说明](https://ntrl.ntis.gov/NTRL/dashboard/searchResults/titleDetail/PB91135772.xhtml) 如果将来各车能提供可信协方差，但不同SLAM估计之间的相关性未知，可以考虑 Covariance Intersection；它专门处理未知交叉相关性的估计融合。[Julier与Uhlmann的SLAM融合论文](https://www.sciencedirect.com/science/article/abs/pii/S0921889006001436) 但你目前拿不到Detour协方差，所以现在直接上CI没有足够输入。 ## 对当前项目的建议 当前先不要推翻 `FleetStateEstimator`。合理的推进顺序是： 1. 保留现有布局反算、时间对齐、圆周Yaw平均和成员误差计算。 2. 增加“上一车队状态＋轮速”的中心预测。 3. 把两两一致性检查改成“每成员相对预测的独立门控”。 4. 对通过门控的成员做加权圆周平均或小规模Huber融合。 5. 允许单个成员短时降级，不要一帧异常就让整个估计不可用。 6. 双车无法判断谁正确时，短时间用预测，仍无法恢复再停车。 7. 最后再考虑完整的SE(2)鲁棒最小二乘，不需要一开始引入GTSAM或QP。 对整个 `MultiWheelC\Fleet` 的当前评价是： - 布局定义、刚体速度分解、中心控制、相对误差纠偏、最差成员统一减速和全员Ready屏障，结构上没有明显问题； - 当前最大算法缺口就是车队中心融合还不够鲁棒； - 最大工程缺口仍是运行入口、通信、成员超时看门狗、报警汇总和零速命令的实际下发； - `FleetMemberAgent` 目前还缺少独立自动化测试； - 各种阈值目前只在测试构造中出现，尚未通过双车实车数据标定。 所以，平均本身不是错误；“只做等权平均，并在冲突时整队不可用”才是需要下一阶段改进的部分。 将 FleetMemberReport 转成 FleetMemberStateSample 和 FleetMemberSafetyStatus； 将准备命令映射到 FleetMemberAgent； 将成员速度命令映射成 FleetCommand； ShouldStop 时广播 Stop 并让主车本地停车； 命令序号、报告序号和任务编号检查； M层报警和底盘执行失败映射到 FailureCode； 无线端口初始化； 报文序列化、分包、校验和接收循环； 主车本地单调时钟和报文接收时间记录。