134 lines
4.7 KiB
Markdown
134 lines
4.7 KiB
Markdown
|
# 近墙猛修问题记录
|
|||
|
|
|
|||
|
|
## 现象
|
|||
|
|
|
|||
|
|
- 在两侧均为约 `40cm` 墙壁的测试环境中，只要车辆初始摆位明显偏向某一侧，就会出现明显的向另一侧猛打方向现象。
|
|||
|
|
- 具体表现为：
|
|||
|
|
- 车身非常靠近左侧墙时，会明显向右转。
|
|||
|
|
- 车身非常靠近右侧墙时，会明显向左转。
|
|||
|
|
- 当距离墙面非常近时，修正会过猛，看起来像“突然拧过去”。
|
|||
|
|
|
|||
|
|
## 当前判断
|
|||
|
|
|
|||
|
|
- “靠左往右修、靠右往左修”这个方向关系本身是符合居中控制逻辑的，不是转向方向定义反了。
|
|||
|
|
- 真正的问题在于：侧向 VL53 在近墙区域的观测仍然被当作可信几何信息使用，导致横向误差 `e_y` 被一次性拉得过大，控制器再把这个大误差直接转换成较大的转向角速度。
|
|||
|
|
- 因此，当前故障更接近“近墙观测过激 -> 状态估计偏大 -> 控制输出过猛”，而不是“状态机无故乱转”。
|
|||
|
|
|
|||
|
|
## 代码链路
|
|||
|
|
|
|||
|
|
### 1. 侧向 VL53 近墙仍可能被判有效
|
|||
|
|
|
|||
|
|
文件：`App/preproc/corridor_preproc.h`
|
|||
|
|
|
|||
|
|
- 当前侧向最小有效距离为：`PREPROC_MIN_SIDE_RANGE_M = 0.02f`
|
|||
|
|
- 也就是只要大于 `2cm`，观测就可能继续进入后续 EKF。
|
|||
|
|
|
|||
|
|
这对 40cm 通道、20cm 车体来说偏激进，因为车辆一旦贴边，`2cm ~ 3cm` 这个区间很容易出现安装偏差、角度误差、近距非线性等问题。
|
|||
|
|
|
|||
|
|
### 2. EKF 直接把侧向距离变成横向偏差观测
|
|||
|
|
|
|||
|
|
文件：`App/est/corridor_ekf.c`
|
|||
|
|
|
|||
|
|
核心逻辑：
|
|||
|
|
|
|||
|
|
```c
|
|||
|
|
float d_center = (W - Rw) / 2.0f + inset;
|
|||
|
|
|
|||
|
|
if (left_ok) {
|
|||
|
|
z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (right_ok) {
|
|||
|
|
z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff;
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- 在当前参数下，理论居中时单侧读数大约为 `0.10m`。
|
|||
|
|
- 如果左侧因为近墙读到 `0.02m ~ 0.03m`，则 `z_ey` 会立刻表现为“明显偏左”，量级可达数厘米。
|
|||
|
|
- 只要该观测没有被马氏距离检验拒绝，就会被吸收到状态 `e_y` 中。
|
|||
|
|
|
|||
|
|
### 3. 控制器直接用 `e_y` 生成转向命令
|
|||
|
|
|
|||
|
|
文件：`App/nav/corridor_ctrl.c`
|
|||
|
|
|
|||
|
|
```c
|
|||
|
|
float w_cmd = -(s_cfg.kp_theta * state->e_th
|
|||
|
|
+ s_cfg.kd_theta * imu_wz
|
|||
|
|
+ s_cfg.kp_y * state->e_y);
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- 当 `e_y` 被近墙观测拉得过大时，`w_cmd` 会立刻增大。
|
|||
|
|
- 因此表现出来就是：
|
|||
|
|
- 贴左墙时明显向右修。
|
|||
|
|
- 贴右墙时明显向左修。
|
|||
|
|
- 靠得非常近时会“修得过猛”。
|
|||
|
|
|
|||
|
|
## 为什么这不是“方向反了”
|
|||
|
|
|
|||
|
|
- 如果控制方向反了，现象应该是：靠左还继续往左，靠右还继续往右。
|
|||
|
|
- 实际观测恰好相反，说明控制方向是对的，只是近墙时误差量级过大。
|
|||
|
|
|
|||
|
|
## 与此前“上电就左转/右转”的关系
|
|||
|
|
|
|||
|
|
- 之前怀疑过状态机提前切换、IMU 保直、前向激光异常等路径。
|
|||
|
|
- 结合实车现象后，当前更值得优先处理的是：
|
|||
|
|
- 车辆一旦被放在明显偏向某一侧的位置，系统会把它当成正常走廊纠偏问题来处理。
|
|||
|
|
- 当侧向 VL53 处于近墙区时，这个纠偏会被放大得过于激进。
|
|||
|
|
|
|||
|
|
这意味着当前问题更像“近墙过激纠偏”，而不是“无条件自主转向”。
|
|||
|
|
|
|||
|
|
## 高概率根因
|
|||
|
|
|
|||
|
|
1. 侧向 VL53 的最小可信距离阈值过低，近墙失真数据仍被当作有效观测。
|
|||
|
|
2. EKF 对单拍横向观测 `z_ey` 缺少足够的近墙限幅或退化策略。
|
|||
|
|
3. 控制层对“近墙高风险状态”没有额外保险，导致 `e_y` 一大就直接猛打方向。
|
|||
|
|
|
|||
|
|
## 建议修复方向
|
|||
|
|
|
|||
|
|
### 方案 A：预处理层加近墙保护
|
|||
|
|
|
|||
|
|
- 提高侧向最小可信距离阈值，不再把极近距离观测直接当正常数据使用。
|
|||
|
|
- 或新增“近墙退化区”，在该区间内降低观测可信度，而不是简单二值有效/无效。
|
|||
|
|
|
|||
|
|
这是最前面的止血点。
|
|||
|
|
|
|||
|
|
### 方案 B：EKF 观测限幅
|
|||
|
|
|
|||
|
|
- 对 `z_ey` 或单拍新息 `y_ey` 增加限幅。
|
|||
|
|
- 防止单次近墙异常读数把 `e_y` 猛拉到过大值。
|
|||
|
|
|
|||
|
|
这是最直接针对“状态一下被拉偏”的修复。
|
|||
|
|
|
|||
|
|
### 方案 C：控制层保险
|
|||
|
|
|
|||
|
|
- 在近墙状态下限制 `w_cmd` 的幅度或变化速度。
|
|||
|
|
- 同时降低线速度，避免“边贴墙边急拧”。
|
|||
|
|
|
|||
|
|
这是最后一道保险，不应单独依赖，但建议保留。
|
|||
|
|
|
|||
|
|
## 建议优先级
|
|||
|
|
|
|||
|
|
1. 预处理层近墙保护
|
|||
|
|
2. EKF 横向观测限幅
|
|||
|
|
3. 控制层近墙保险
|
|||
|
|
|
|||
|
|
## 建议调试观测量
|
|||
|
|
|
|||
|
|
后续复现时建议同步观察以下量：
|
|||
|
|
|
|||
|
|
- 四颗侧向 VL53 原始距离
|
|||
|
|
- `obs.valid_mask`
|
|||
|
|
- `obs.d_lf / d_lr / d_rf / d_rr`
|
|||
|
|
- `corridor_state.e_y`
|
|||
|
|
- `corridor_state.e_th`
|
|||
|
|
- `raw_cmd.w`
|
|||
|
|
- `safe_w`
|
|||
|
|
|
|||
|
|
重点确认：
|
|||
|
|
|
|||
|
|
- 近墙时是否有某一侧距离突然跌到极小值。
|
|||
|
|
- `e_y` 是否在同一时刻明显跃迁。
|
|||
|
|
- `raw_cmd.w` 是否跟着立刻增大。
|
|||
|
|
|
|||
|
|
如果这三者时间上连续对应，就可以基本坐实当前分析。
|