ASER-NAV/横向偏移问题代码分析.md

垄沟横向偏移问题 — 代码层面分析

基于 问题诊断报告.md 的二次审查，对根因优先级提出不同判断。

---

现象回顾

• 车头笔直（IMU 航向控制正常）
• 车身稳定偏向一侧墙壁，无法居中

---

代码审查发现

发现 1：EKF 的 side_sensor_inset fallback 从未实现（主因）

文件： App/est/corridor_ekf.c:451-454

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

#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

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 行附近

// 修改前：
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 不应频繁触发