# 垄沟行走无法居中/持续侧墙问题诊断报告

## 现象描述

- 垄沟内可以**笔直向前走**，说明航向控制（IMU）正常。
- 但**无法在垄沟正中行驶**，车身稳定偏向一侧墙壁。
- 表现为：持续侧墙行驶，无法回中。

> **初步判断**：不是 IMU 精度问题（你确认 IMU 精度很高），而是**横向居中控制存在系统性偏差**。

---

## 根因分析

### 1. 核心 Bug：`corridor_ctrl.c` 控制器绕过 EKF 滤波结果

**文件：** `App/nav/corridor_ctrl.c:75-85`

```c
float e_y_ctrl = state->e_y;
bool left_ok = /* 左前+左后有效 */;
bool right_ok = /* 右前+右后有效 */;

if (left_ok && right_ok) {
    float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
    float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
    e_y_ctrl = 0.5f * (d_right - d_left);   // <-- 直接覆盖 EKF 结果！
}
```

**问题：**

当双侧激光都有效时，控制器**丢弃了 EKF 输出**的 `state->e_y`，改用原始的 `(d_right - d_left) / 2` 作为横向偏差。

这个做法隐含一个致命假设：**"车在正中时，左右激光读数相等"**。

但实际情况是：
- 左右 VL53L0X 传感器安装**不可能绝对对称**，哪怕差 2~3mm，在 40cm 垄沟 + 20cm 车体 = 每边仅 10cm 的裕量下，就是 **2~3% 的系统误差**。
- 控制器拿到这个带偏差的 `e_y_ctrl`，认为"偏左"的位置实际上是正中，于是稳定地把车锁在偏右的位置行驶。

**为什么车头能走直但回不了中？**

- `kp_theta * e_th` + `kd_theta * imu_wz` 这组负责**航向**，与激光无关 → 车头笔直。
- `kp_y * e_y_ctrl` 这组负责**横向回中**，但 `e_y_ctrl` 本身带系统性偏置 → 回中目标点本身就是歪的。

---

### 2. 叠加因素：传感器内缩参数 `left/right_sensor_inset` 未标定

**文件：** `App/robot_params.h:107,113`

```c
#define PARAM_VL53_LEFT_INSET       0.0f   // 注释：当前为 0.0 表示未标定
#define PARAM_VL53_RIGHT_INSET      0.0f   // 注释：当前为 0.0 表示未标定
```

虽然控制器 bug 是直接原因，但 `left/right_sensor_inset` 未标定也会影响：

- **EKF 层面：** `corridor_ekf.c:453-454` 用 `d_center_left = (W - Rw)/2 + left_sensor_inset` 计算期望居中读数。`inset = 0` 时，EKF 本身对单侧退化场景的计算也存在偏差。
- **单侧退化时恶性循环：** 车因 bug 偏向一侧 → 近墙激光进入饱和/退化区（< 3cm）→ 系统退化为单侧观测 → 未标定的 `inset` 让 EKF 进一步算错位置 → 偏差放大 → 更贴墙。

---

## 诊断结论

| 因素 | 影响程度 | 说明 |
|------|---------|------|
| **控制器绕过 EKF（Bug）** | **主因** | 双侧有效时直接丢弃滤波结果，用原始差值驱动回中，引入不可消除的安装不对称误差 |
| **left/right_sensor_inset 未标定** | 叠加 | 即使修复 bug，单侧退化场景下 EKF 输出仍不精确；标定后整体鲁棒性更好 |
| IMU 精度 | 无关 | 航向控制正常，IMU 工作良好 |
| 激光硬件 | 无关 | 传感器能正常输出读数，问题在于怎么用读数 |

**总体结论：这是软件/参数问题，不是硬件问题。**

---

## 修复建议

### 方案 A：改代码（推荐，一劳永逸）

删除 `App/nav/corridor_ctrl.c` 中第 `75-85` 行的 `if (left_ok && right_ok)` 分支，**始终使用 `state->e_y`** 作为横向偏差输入控制器。

理由：
- EKF 已经综合了 `corridor_width`、`robot_width`、`left/right_sensor_inset` 做了完整几何补偿。
- EKF 还做了马氏距离异常检测和协方差加权，噪声滤波远好于控制器层直接算差值。
- 控制器不应该"越级"跳过 EKF 做原始观测融合。

### 方案 B：标定参数（也必须做）

按 `robot_params.h` 注释中的方法：
1. 用卷尺将车**精确放在垄沟正中央**。
2. 记录左右 VL53 前后平均读数。
3. 计算：
   - `left_inset = 0.10 - 实测左侧平均读数`
   - `right_inset = 0.10 - 实测右侧平均读数`
4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`。

> 标定完成后，即使不删代码，控制器的原始差值 `d_right - d_left` 也会更接近真实居中偏差（因为 EKF 标定后输出的 `state->e_y` 更准确，但 bug 依然意味着双侧有效时 EKF 被绕过）。

---

## 验证方法

修复后上垄沟测试，预期表现：
1. **笔直 + 居中**：车头不偏，车身稳定在垄沟正中间。
2. 故意用手把车推向一侧，松手后应自动回中。
3. 近墙时近墙脱离保护（`wall_escape`）不应频繁触发——因为车本就在中间，不需要"救"。