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

现象描述

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

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

根因分析

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

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

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

#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 注释中的方法：

用卷尺将车精确放在垄沟正中央。
记录左右 VL53 前后平均读数。
计算：
- left_inset = 0.10 - 实测左侧平均读数
- right_inset = 0.10 - 实测右侧平均读数
填入 PARAM_VL53_LEFT_INSET 和 PARAM_VL53_RIGHT_INSET。

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

验证方法

修复后上垄沟测试，预期表现：

笔直 + 居中：车头不偏，车身稳定在垄沟正中间。
故意用手把车推向一侧，松手后应自动回中。
近墙时近墙脱离保护（wall_escape）不应频繁触发——因为车本就在中间，不需要"救"。

4.6 KiB Raw Blame History Unescape Escape