365 lines
8.2 KiB
Plaintext
365 lines
8.2 KiB
Plaintext
需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：
|
||
|
||
```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错误 |