1.0

审查报告.md

@@ -0,0 +1,170 @@
+# 审查报告
+
+## 范围
+
+本次审查以 `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` 的实车联调。