ASER/审查报告.md

审查报告

范围

本次审查以 app/ 为主，重点查看了以下模块：

• app/est/corridor_ekf.c/.h
• app/est/corridor_filter.c/.h
• app/nav/corridor_ctrl.c/.h
• app/nav/nav_script.c/.h
• app/nav/global_nav.c/.h
• app/nav/track_map.c/.h
• app/preproc/corridor_preproc.c/.h
• app/app_tasks.c
• app/robot_params.h
• Doc/HANDOFF_v2.md

审查方式是静态代码审查。尝试做构建验证时，当前环境缺少 cmake 命令，未能完成编译验证。

结论

• 单沟测试模式持续偏右 最像是 横向几何/标定偏置 与 IMU 航向参考锁定策略 共同造成，不像单一 kp/kd 参数问题。
• 这类问题会传导到 赛道模式，因为 GlobalNav 进入沟内后复用的仍是同一套 CorridorFilter + CorridorCtrl。
• nav_script 里还存在一个独立的确定性功能 bug：SCRIPT_STAGE_EXIT 基本不可达，单沟模式无法按注释描述正常收尾。

重点发现

1. 横向偏置补偿模型不完整，这是“持续偏右”的头号嫌疑。

位置：app/app_tasks.c:419-424，app/robot_params.h:97，app/est/corridor_ekf.c:382-396，app/est/corridor_ekf.h:55-60，app/VL53L0X_API/platform/vl53_calibration_config.h:23-50

判断：设计缺口兼标定缺口。

影响：

e_y 的观测模型只支持一个全局的 side_sensor_inset，并且 y_offset 在 app_tasks.c 被硬编码为 0.0f。这意味着系统默认假设左右两侧安装完全对称、车体几何中心与传感器几何中心重合、且不存在系统性横向偏置。只要左右侧真实安装、外壳边缘、传感器光轴或标定有几毫米到 1cm 级差异，EKF 就会把“带偏的中心”当作真中心，最终表现为长期偏左或偏右。

补充判断：vl53_calibration_config.h 里的 4 颗 VL53 运行时校准值并不完全一致，但当前 EKF 只有一个共享的 side_sensor_inset，无法表达左右非对称误差。这与“总是偏同一侧”高度一致。

与赛道模式关系：强相关。只要进入 GNAV_CORRIDOR_TRACK，赛道模式会复用同一套横向观测模型。

2. EKF 取消了侧墙航向观测，e_th 几乎完全依赖 IMU；一旦参考锁偏，会形成稳定横向偏置。

位置：app/est/corridor_ekf.c:345-350，app/est/corridor_filter.c:84-109，app/nav/corridor_ctrl.c:45-47

判断：算法设计风险。

影响：

当前实现明确移除了 z_eth_L/z_eth_R 侧墙航向观测，e_th 只由 imu_wz 预测和 IMU yaw 观测约束。控制器又同时使用 e_th 和 e_y 计算转向。如果入沟时车头本来就带有固定偏角，或者 IMU 参考方向锁定在一个有偏姿态上，系统缺少“用走廊墙面把航向拉回真实走廊方向”的机制，最终会收敛到一个偏置平衡点，表现成长期靠右或靠左行驶。

与赛道模式关系：强相关。赛道模式沟内闭环同样使用这套 EKF 和控制器。

3. IMU yaw 参考锁定过早，且没有要求“已经真正对正走廊”。

位置：app/est/corridor_filter.c:96-100

判断：算法设计风险。

影响：

当前只要 imu_yaw_valid 且 out_state->conf >= 0.3f 就锁定 s_imu_yaw_ref_rad。这个门槛偏低，而且没有要求左右观测对称、e_y/e_th 足够小、或连续稳定若干拍。这样一来，如果机器人在入沟初期就带着一个固定的小偏角，后续 IMU 参考会把这个偏角“合法化”，再叠加第 2 条中的 IMU 主导航向设计，很容易演化成固定方向的偏航和偏置。

与赛道模式关系：强相关。GlobalNav 每次 REACQUIRE 后都会重新经历一次相同的锁定过程。

4. CorridorFilter_Update() 会覆盖掉 EKF 已经计算好的 conf 与观测拒绝语义，导致系统对退化状态偏乐观。

位置：app/est/corridor_filter.c:81-109，app/est/corridor_ekf.c:488-496，app/est/corridor_ekf.c:568-587

判断：确定性 bug。

影响：

CorridorEKF_Update() 已把 valid_sides 和 reject_mask 纳入 conf，但 CorridorFilter_Update() 随后调用 CorridorEKF_GetState()，又把 conf 重算成只看 P_trace 的版本，同时把 mahalanobis_d2 和 obs_reject_mask 清成默认值。结果是：单侧退化、观测被拒绝、或观测质量明显变差时，最终输出给导航/安全层的健康度会比实际更乐观，也丢失了关键诊断信息。

与“偏右”关系：不是首因，但会让系统更难及时暴露和处理有偏状态。

与赛道模式关系：相关。赛道模式也复用相同的滤波输出。

5. 转向逻辑对 IMU 失效没有真正后备路径，注释与实现不一致。

位置：app/nav/nav_script.c:259-265，app/app_tasks.c:373-375，app/nav/global_nav.c:223-250

判断：确定性 bug 兼鲁棒性缺陷。

影响：

nav_script.c 的注释写着“如果 IMU 离线，退化回 EKF e_th 差值判定”，但实际代码始终直接用 imu_yaw_continuous_deg 算转角，没有任何 fallback。app_tasks.c 在 IMU 无效时会直接传 0.0f 给脚本，global_nav.c 也在 IMU 无效时把 imu_yaw 当 0.0f 使用。这会让单沟和赛道两套转向逻辑都对 IMU 有单点依赖，IMU 一旦丢失或短时异常，轻则超时，重则转向阶段行为异常。

与赛道模式关系：强相关。GlobalNav 的三个转向状态都受此影响。

6. nav_script 的 SCRIPT_STAGE_EXIT 基本不可达，单沟模式无法按设计正常收尾。

位置：app/nav/nav_script.h:52-58，app/nav/nav_script.c:34，app/nav/nav_script.c:188-189，app/nav/nav_script.c:208-209，app/nav/nav_script.c:235-236，app/nav/nav_script.c:351-352，app/nav/nav_script.c:361-387

判断：确定性 bug。

影响：

pass_count 被定义并在入沟时置为 1，但后续没有递增，也没有任何基于趟数切换到 SCRIPT_STAGE_EXIT 的逻辑。当前状态机会在 CORRIDOR_FORWARD -> TURN_AT_END -> CORRIDOR_BACKWARD -> TURN_AT_END -> CORRIDOR_FORWARD 之间循环，EXIT 分支虽然写了实现，但缺少可达路径。这个问题不直接解释“偏右”，但说明单沟模式本身已经与头文件注释和交接文档描述不一致。

与赛道模式关系：无直接影响。赛道模式走的是 GlobalNav，不是 NavScript。

7. HANDOFF_v2.md 与当前代码状态已经发生漂移，调试时不能把它当最终真相。

位置：Doc/HANDOFF_v2.md:582，app/robot_params.h:384-385，Doc/HANDOFF_v2.md:635-650，app/robot_params.h:220-239，app/robot_params.h:282

判断：代码质量问题。

影响：

交接文档写的是 USE_GLOBAL_NAV=1，当前代码实际是 0。文档里列出的若干控制/安全参数值也与 robot_params.h 当前值不一致，例如 kd_theta、kp_y、d_front_stop。这会直接干扰现场排障：如果调试人员按 HANDOFF_v2.md 以为自己在赛道模式、或以为当前增益较保守，结论会偏掉。

8. EKF 接口与实现存在漂移，诊断字段并没有被完整维护。

位置：app/preproc/corridor_msgs.h:42-45，app/est/corridor_ekf.h:10-18，app/est/corridor_ekf.c:130-165，app/app_tasks.c:338-340

判断：代码质量问题。

影响：

CorridorState_t 里保留了 innovation[EKF_OBS_DIM]，头文件也仍然描述了 3 维观测模型，但当前 corridor_ekf.c 实际只做了 e_y 的 1 维侧墙更新，innovation 没有被写入，部分 2x2/3x3 辅助函数也已不再参与主流程。当前这不会直接造成“偏右”，但会让诊断接口带有历史包袱，不利于后续排障和调参。

“持续偏右”专项判断

从代码看，最可能的因果链是：

1. 左右侧真实几何或标定不完全对称，但 EKF 只允许一个共享 side_sensor_inset，且 y_offset=0。
2. 机器人入沟时如果本身就带有固定偏角，CorridorFilter 又会较早锁定 IMU yaw 参考。
3. CorridorEKF 不再使用侧墙航向观测，e_th 会长期保留这类偏差。
4. CorridorCtrl 将 e_th 和 e_y 一起用于转向控制，最终形成稳定的偏右或偏左行驶。

换句话说，这更像“观测模型偏置 + 航向参考偏置 -> 控制器稳定复现偏差”，不是单纯把 kp_y 调大或调小就能根治。

对赛道模式的判断

• 会带入赛道模式的问题：第 1、2、3、4、5、8 条中的共享 EKF/控制问题，以及第 7 条文档漂移带来的调试误判风险。第 6 条不会直接带入赛道模式。
• 不会直接带入赛道模式的问题：NavScript 的 EXIT 不可达，仅限 USE_GLOBAL_NAV=0 的单沟路径。

因此，如果单沟模式已经出现“总是偏右”，我的判断是赛道模式大概率会在沟内闭环段复现相同趋势，只是会被 GlobalNav 的入沟/转向/重捕获逻辑进一步放大或掩盖。

建议处理顺序

1. 先把横向偏置补偿做实。

优先把 y_offset 参数化，不要在 app_tasks.c 固定为 0.0f。同时把左右两侧的安装/外壳/传感器中心偏差重新实测，必要时改成“左/右分侧补偿”，而不是单一 PARAM_VL53_SIDE_INSET。

2. 收紧 IMU yaw 参考锁定条件。

至少要求双侧观测稳定、e_y 和 e_th 较小、连续若干拍后再锁 yaw_ref，避免把入沟瞬间的偏角固化为长期参考。

3. 重新评估“航向仅靠 IMU”的方案。

如果侧墙差分噪声确实偏大，可以考虑恢复一个低权重的侧墙航向修正，或只在双侧质量都好时给 e_th 一个弱约束，而不是完全切断墙面对航向的校正能力。

4. 修复 conf 与诊断覆盖问题。

CorridorFilter_Update() 不应在 IMU 更新后直接覆盖掉 CorridorEKF_Update() 的 conf、obs_reject_mask、mahalanobis_d2。

5. 修复转向阶段的 IMU 失效后备路径。

单沟与赛道两套转向逻辑都应在 IMU 失效时进入明确的 fallback 或 fail-safe，而不是默认把 yaw 当 0.0f 继续算。

6. 修复 NavScript 的退出路径，避免单沟测试脚本本身与设计目标脱节。

最后判断

如果只让我给一句结论：

当前代码里，持续偏右 更可能是 EKF 观测模型与航向参考的系统性偏置，而不是纯控制参数震荡；而且这类问题会传导到 赛道模式，值得先在共享的 CorridorFilter + CorridorCtrl 层解决，再继续做 GlobalNav 的实车联调。