完善Detour状态估计与轨迹跟踪验证

2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
@@ -0,0 +1,365 @@
+需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：
+
+```text
+单帧错误定位
+持续性的定位重定位/地图修正
+车辆真实的快速运动
+```
+
+对你这种低速停车机器人，可以先采用一套偏安全、容易验证的处理。
+
+## 定位跳变会造成什么影响
+
+假设Detour在50ms内突然跳了10cm：
+
+\[
+v=\frac{0.1}{0.05}=2m/s
+\]
+
+实际车辆可能只有 `0.3m/s`，但差分速度会产生 `2m/s` 的尖峰。
+
+对控制器还有三个直接影响：
+
+- 横向位置跳10cm，横向误差可能瞬间变化10cm。
+- 航向跳5°，航向误差会瞬间变化5°。
+- 全局轨迹投影可能跳到另一条临近或相交的轨迹线段。
+
+Stanley在低速时尤其敏感：
+
+\[
+\delta =
+e_\theta+
+\arctan\left(\frac{k e_y}{v+\varepsilon}\right)
+\]
+
+低速时分母较小，横向误差突然变大，会产生很大的转向指令。因此不能完全不处理。
+
+## 不建议直接低通掉定位跳变
+
+不要简单地对Detour位置做强低通：
+
+```text
+错误位置跳变
+→ 低通缓慢跟过去
+```
+
+这样虽然曲线看起来平滑，但控制器会在一段时间内使用滞后、虚构的位置，可能更加危险。
+
+更好的做法是：
+
+```text
+检测跳变
+→ 暂时不把该帧用于速度差分和控制
+→ 观察后续定位
+→ 判断是单帧异常还是持续重定位
+```
+
+## 第一层：运动学合理性检查
+
+将当前Detour位姿和上一次接受的位姿比较。
+
+位置变化：
+
+\[
+\Delta p=\sqrt{\Delta x^2+\Delta y^2}
+\]
+
+航向变化：
+
+\[
+\Delta\theta=
+|\operatorname{ShortestAngleDifference}|
+\]
+
+允许的最大变化量可以按照车辆物理能力计算：
+
+```csharp
+var maximumAllowedDistance =
+    maximumLinearSpeedMetersPerSecond *
+    deltaTimeSeconds +
+    positionJumpMarginMeters;
+
+var maximumAllowedHeadingChange =
+    maximumAngularSpeedRadiansPerSecond *
+    deltaTimeSeconds +
+    headingJumpMarginRadians;
+```
+
+然后判断：
+
+```csharp
+var positionJump =
+    displacementMeters >
+    maximumAllowedDistance;
+
+var headingJump =
+    headingChangeRadians >
+    maximumAllowedHeadingChange;
+```
+
+你当前小车最高约 `1.2m/s`，假设Detour更新周期为50ms：
+
+```text
+物理最大位移约为：
+1.2 × 0.05 = 0.06m
+```
+
+初期可以额外留出约 `0.03～0.05m` 的定位余量。不过这些阈值最终应根据实际Detour数据确定，不建议永久写死。
+
+## 第二层：不要立即接受异常帧
+
+检测到一个异常帧时，不要立刻改变车辆状态：
+
+```text
+上一正常位置 A
+异常位置 B
+下一帧又回到 A 附近
+```
+
+这种情况说明B很可能是单帧异常，应直接丢弃。
+
+如果后续连续若干帧都稳定在B附近：
+
+```text
+A → B → B附近 → B附近
+```
+
+这更可能是Detour发生了持续性的重定位。
+
+可以使用：
+
+```text
+连续2～3个新定位帧相互一致
+```
+
+作为重新接受定位的条件。
+
+## 第三层：重定位后必须重置速度估计
+
+如果确认新的定位是持续有效的，不能用：
+
+```text
+新位置B - 旧位置A
+```
+
+计算速度，因为A到B是定位修正，不是车辆真实运动。
+
+正确处理是：
+
+```csharp
+_velocityEstimator.Reset(
+    newPose,
+    currentTimestamp);
+```
+
+这会：
+
+- 将新位姿作为新的差分起点。
+- 清除之前的速度历史。
+- 重置三个低通滤波器。
+- 将速度暂时标记为无效或零。
+- 等下一次正常Detour更新后重新开始估计。
+
+## 第四层：控制器应该如何响应
+
+对于实车轨迹跟踪，建议状态分为：
+
+```text
+Valid        正常定位，可以控制
+Suspected    检测到疑似跳变
+Reacquiring  正在确认新的定位
+Stale        定位长时间没有更新
+```
+
+你的第一版不一定需要单独增加复杂枚举，但控制行为至少应该满足：
+
+```text
+正常：
+继续轨迹跟踪
+
+疑似单帧跳变：
+不使用异常帧更新状态
+短时间保持上一状态
+
+连续异常或定位超时：
+停车，不继续使用旧状态运动
+
+确认重定位：
+接受新位姿
+重置速度估计
+重新执行轨迹投影
+确认稳定后恢复控制
+```
+
+停车机器人速度低、场地有限，定位连续异常时停车比盲目继续跟踪更合适。
+
+## 还需要限制轨迹投影进度
+
+即使Detour跳变检测做了，轨迹投影也最好增加进度保护。
+
+当前投影器在整条轨迹上找最近点，如果轨迹自交，车辆可能从：
+
+```text
+当前第20段
+```
+
+突然投影到：
+
+```text
+第80段
+```
+
+后续可以增加一个 `TrajectoryProgressTracker`：
+
+```text
+正常时只在上次线段索引附近搜索
+例如 [previousIndex - 5, previousIndex + 20]
+
+定位重定位后：
+重新执行一次全局搜索
+```
+
+同时限制单周期允许的弧长进度变化：
+
+\[
+|\Delta s|
+\leq
+v_{\max}\Delta t+\text{margin}
+\]
+
+这样可以避免定位轻微波动导致轨迹进度突然跳段。
+
+## 在五个文件中的分工
+
+```text
+DetourVehicleStateProvider
+├─ 检查数值有效性
+├─ 检查定位是否过期
+├─ 检测重复帧
+├─ 检测位置/航向跳变
+├─ 暂存疑似新定位
+└─ 确认重定位后触发Reset
+
+VelocityEstimator2D
+├─ 正常位姿差分
+├─ 真实时间差计算
+├─ 速度滤波
+└─ 提供Reset()
+
+FirstOrderLowPassFilter
+└─ 提供Update()和Reset()
+
+VehicleState
+├─ 保存位姿和速度
+└─ 表示速度估计是否有效
+
+IVehicleStateProvider
+└─ 向控制器返回有效状态或读取失败
+```
+
+因此定位跳变的主要逻辑放在 `DetourVehicleStateProvider`，而不是低通滤波器。
+
+第一版建议先实现：
+
+1. 非有限值检查。
+2. 重复帧识别。
+3. 基于最大速度和最大角速度的跳变检测。
+4. 单帧异常拒绝。
+5. 连续2～3帧确认重定位。
+6. 重定位后重置速度估计。
+7. 长时间定位无效时通知控制器停车。
+
+这套方案足以保护当前Stanley和后续纵向控制，不需要现在就修改SLAM源码。
+
+
+已经给 [DetourVehicleStateProvider.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:233) 增加了基于上一有效速度的预测残差检测。
+
+现在采用两级判断：
+
+```text
+第一级：绝对物理运动边界
+第二级：上一状态恒速预测残差
+```
+
+预测公式为：
+
+\[
+x_{pred}=x_{last}+v_x\Delta t
+\]
+
+\[
+y_{pred}=y_{last}+v_y\Delta t
+\]
+
+\[
+\theta_{pred}=
+\theta_{last}+\omega\Delta t
+\]
+
+实测残差为：
+
+\[
+e_p=
+\sqrt{
+(x_{measured}-x_{pred})^2+
+(y_{measured}-y_{pred})^2
+}
+\]
+
+以及：
+
+\[
+e_\theta=
+\operatorname{ShortestDifference}
+(\theta_{measured},\theta_{pred})
+\]
+
+对应实现在：
+
+- [IsPredictionResidualPlausible()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:536)
+- [PredictPoseInWorld()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:573)
+
+默认阈值为：
+
+```csharp
+位置预测残差：0.04m
+航向预测残差：5°
+```
+
+没有采用最初考虑的30mm，是因为用现有数据离线统计后发现：
+
+| 位置残差阈值 | 触发次数 | 占有效更新比例 |
+|---:|---:|---:|
+| 30mm | 55次 | 1.24% |
+| 35mm | 28次 | 0.63% |
+| 40mm | 13次 | 0.29% |
+| 45mm | 7次 | 0.16% |
+| 50mm | 4次 | 0.09% |
+
+30mm容易把正常Detour波动也判成异常；40mm仍能识别现有数据中约49mm和66mm的明显跳变，同时误触发风险更低。
+
+当前判定逻辑：
+
+```csharp
+if (!isWithinPhysicalBoundary ||
+    !isWithinPredictionResidual)
+{
+    // 进入疑似重定位确认状态。
+}
+```
+
+另外同步修正了跳变恢复逻辑：
+
+- 发生预测残差跳变时，保存当时的预测位姿。
+- 后续定位必须回到预测位姿附近，才能认为是单帧异常后的正常恢复。
+- 不再使用较宽松的绝对物理范围立即放行。
+- 持续远端定位仍需满足3次观测和0.25秒，才按重定位接管。
+- 接管后重置速度估计，避免把坐标修正计算成车辆速度。
+
+第一帧或重定位后的速度还没有建立时，不启用预测残差检测，只使用物理极限检查，避免没有速度基准时误判。
+
+完整构建结果：
+
+- CommonUsage：0警告、0错误
+- MedullaAdapter：0警告、0错误
+- MultiWheelC：0警告、0错误