271 lines
8.3 KiB
Plaintext
271 lines
8.3 KiB
Plaintext
结论：`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；
|
||
无线端口初始化；
|
||
报文序列化、分包、校验和接收循环；
|
||
主车本地单调时钟和报文接收时间记录。 |