用于调整出最优参数

横向偏移问题代码分析.md

@@ -0,0 +1,142 @@
+# 垄沟横向偏移问题 — 代码层面分析
+
+> 基于 `问题诊断报告.md` 的二次审查，对根因优先级提出不同判断。
+
+---
+
+## 现象回顾
+
+- 车头笔直（IMU 航向控制正常）
+- 车身稳定偏向一侧墙壁，无法居中
+
+---
+
+## 代码审查发现
+
+### 发现 1：EKF 的 `side_sensor_inset` fallback 从未实现（主因）
+
+**文件：** `App/est/corridor_ekf.c:451-454`
+
+```c
+float d_center_left  = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;   // = 0.10 + 0.0 = 0.10
+float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;  // = 0.10 + 0.0 = 0.10
+```
+
+**文件：** `App/robot_params.h:96-113`
+
+```c
+#define PARAM_VL53_SIDE_INSET       (-(0.018f))   // 已实测！但从未被 EKF 使用
+#define PARAM_VL53_LEFT_INSET       0.0f           // 未标定
+#define PARAM_VL53_RIGHT_INSET      0.0f           // 未标定
+```
+
+**问题：**
+
+`robot_params.h` 注释写着 `side_sensor_inset` 是"未分侧设置时的默认值"，但 `corridor_ekf.c` 中**没有任何 fallback 逻辑**——它直接用 `left_sensor_inset`（0.0f）和 `right_sensor_inset`（0.0f），完全忽略了已经实测好的 `-0.018f`。
+
+**影响：** EKF 观测方程在双侧有效时退化为：
+
+```
+z_ey = (d_right_avg - d_left_avg) / 2
+```
+
+没有任何安装偏置补偿。如果传感器安装面距车体边缘实际有 18mm 的偏差（`side_sensor_inset = -0.018`），或者左右不对称，EKF **本身就会算出带系统性偏置的 `e_y`**。
+
+### 发现 2：控制器 bypass 绕过 EKF（阻塞性 bug）
+
+**文件：** `App/nav/corridor_ctrl.c:75-85`
+
+```c
+float e_y_ctrl = state->e_y;
+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 结果
+}
+```
+
+**问题：**
+
+当双侧激光都有效时，控制器丢弃 `state->e_y`，改用原始 `(d_right - d_left)/2`。这个公式**不含任何 inset 补偿**。
+
+**关键推论：** 当前 `left/right_sensor_inset` 都是 0.0f 时，EKF 的 `z_ey` 和 bypass 的公式**完全等价**：
+
+```
+EKF:    z_ey = [(W-Rw)/2 + 0 - d_left] + [d_right - (W-Rw)/2 - 0] / 2 = (d_right - d_left) / 2
+Bypass: e_y  = (d_right - d_left) / 2
+```
+
+所以**当前状态下 bypass 不是偏移的直接制造者**——EKF 输出与 bypass 结果几乎相同。但 bypass 的架构问题在于：即使将来标定了 inset，控制器依然用不上——它直接跳过了 EKF 的补偿结果。
+
+### 发现 3：无其他符号/算法错误
+
+- 控制律符号正确：`e_y > 0`（偏左）→ `w_cmd < 0`（右转回中）
+- EKF 预测/更新、Joseph 协方差更新、马氏距离检验均无明显问题
+- IMU 航向观测通道独立且正确
+
+---
+
+## 与原诊断报告的分歧
+
+| 因素 | 原报告评级 | 本次评级 | 理由 |
+|------|-----------|---------|------|
+| 控制器 bypass | **主因** | 阻塞性 bug | 当前 inset=0 时 bypass 与 EKF 输出等价，不是偏移的直接来源；但堵死了标定修复的路 |
+| inset 未生效 | 叠加因素 | **主因** | `-0.018f` 已实测但 EKF 从未使用；left/right=0 导致观测方程无偏置补偿 |
+
+**核心观点：** 原报告说"删 bypass 就一劳永逸"，但如果只删 bypass 而不修 inset，EKF 自身的 `state->e_y` 同样带偏——问题不会改善。
+
+---
+
+## 修复建议
+
+### 修复 1：EKF 添加 inset fallback（立刻见效）
+
+**文件：** `App/est/corridor_ekf.c` 第 451-454 行附近
+
+```c
+// 修改前：
+float d_center_left  = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
+float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
+
+// 修改后：
+float l_inset = (s_cfg.left_sensor_inset  != 0.0f) ? s_cfg.left_sensor_inset  : s_cfg.side_sensor_inset;
+float r_inset = (s_cfg.right_sensor_inset != 0.0f) ? s_cfg.right_sensor_inset : s_cfg.side_sensor_inset;
+float d_center_left  = (W - Rw) / 2.0f + l_inset;
+float d_center_right = (W - Rw) / 2.0f + r_inset;
+```
+
+这样 `-0.018f` 会立即作为两侧默认值生效，消除统一偏置。
+
+### 修复 2：删除控制器 bypass（必须配合修复 1）
+
+**文件：** `App/nav/corridor_ctrl.c` 第 75-85 行
+
+删除 `if (left_ok && right_ok) { ... }` 分支，始终使用 `state->e_y`。
+
+### 修复 3：实车标定分侧 inset（消除左右不对称）
+
+1. 将车精确放在垄沟正中央（卷尺确认）
+2. 记录左右 VL53 前后平均读数
+3. 计算：
+   - `left_inset  = 0.10 - 实测左侧平均读数`
+   - `right_inset = 0.10 - 实测右侧平均读数`
+4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`
+
+---
+
+## 验证方法
+
+### 快速验证（不需要上车）
+
+在调试日志中对比以下两个值：
+- `state->e_y`（EKF 输出）
+- `(d_right - d_left) / 2`（bypass 计算）
+
+如果两者几乎相同，证实 bypass 不是偏移的直接来源，inset 缺失才是。
+
+### 上车验证
+
+修复 1+2 后：
+1. 车应从垄沟正中启动后保持居中
+2. 手推偏后应能自动回中
+3. `wall_escape` 不应频繁触发