ASER-NAV/Doc/近墙猛修问题.md

近墙猛修问题记录

现象

• 在两侧均为约 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

核心逻辑：

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

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 是否跟着立刻增大。

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