# 近墙猛修问题记录

## 现象

- 在两侧均为约 `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` 是否跟着立刻增大。

如果这三者时间上连续对应，就可以基本坐实当前分析。