ParkingRobot/参考文档/detour数据处理.txt

需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：

```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错误