From 46e7fcdbb2fd8e4574f8c455dcc056cbf3182eb2 Mon Sep 17 00:00:00 2001
From: nitiantuhao <2062405236@qq.com>
Date: Sat, 4 Apr 2026 23:24:41 +0800
Subject: [PATCH] 1.0

---
 Doc/turning_imu_risk_analysis.md | 212 ++++++++++++++
 审查报告.md                      | 170 +++++++++++
 改进建议报告.md                  | 464 +++++++++++++++++++++++++++++++
 3 files changed, 846 insertions(+)
 create mode 100644 Doc/turning_imu_risk_analysis.md
 create mode 100644 审查报告.md
 create mode 100644 改进建议报告.md

diff --git a/Doc/turning_imu_risk_analysis.md b/Doc/turning_imu_risk_analysis.md
new file mode 100644
index 0000000..8a89b61
--- /dev/null
+++ b/Doc/turning_imu_risk_analysis.md
@@ -0,0 +1,212 @@
+# Turning IMU Risk Analysis
+
+## Background
+
+项目当前的转向完成判定，核心依赖 IMU 的 `yaw_continuous` 相对变化量：
+
+- 单沟模式：`nav_script.c` 中的 `TURN_AT_END` 使用 `imu_yaw_continuous - turn_start_yaw` 判断是否转满 `180°`
+- 赛道模式：`global_nav.c` 中的 `execute_turn()` 使用同样方式判断是否转满 `90°`
+
+这种设计的优点是：
+
+- 不依赖绝对 `yaw == 90°/180°`
+- 可以消掉启动零点偏置
+- 逻辑简单，状态机清晰
+
+但它有一个前提：IMU 在这一次转向过程中的相对角度估计必须足够准确。
+
+## Confirmed Issue In Single-Corridor Mode
+
+单沟测试模式已经确认存在一类风险：
+
+1. 车辆到端后原地转 `180°`
+2. 状态机仅依据 IMU 判断“已经转满”
+3. 如果 IMU 认为已转满，但车体物理上没有真正与沟道重新平行
+4. 返程恢复走廊闭环后，车辆会长期贴一侧墙运行，甚至进一步放大偏差
+
+这个问题在单沟模式中更明显，因为它原本是“转完后立即回到走廊闭环”，中间缓冲很少。
+
+## Why Relative Yaw Is Still Used
+
+虽然有上述风险，转向完成判定仍然不能简单改成“看绝对 `yaw` 是否等于 `90°` 或 `180°`”。
+
+原因是：
+
+1. 绝对 `yaw` 容易带启动零点偏差
+2. 不同位置的目标朝向不同，不能用一个固定绝对角统一判定
+3. 实际需要的是“从当前姿态再转多少度”，而不是“全局朝向是不是某个固定值”
+
+因此，`delta_yaw = current_yaw - start_yaw` 的设计本身是合理的。
+
+真正的风险不在“用差值”，而在“只用 IMU 差值”。
+
+## Root Cause
+
+当以下任一情况存在时，IMU 相对转角可能与真实车体转角不完全一致：
+
+1. 角速度零偏漂移
+2. 转向时的振动或动态加速度影响姿态解算
+3. IMU 安装方向存在小角度误差
+4. 轮胎打滑，导致“轮子转了/IMU变了”但车体未完全对正目标方向
+5. IMU 本身在特定工况下相对角度估计偏快或偏慢
+
+此时状态机会提前结束转向，后续闭环就会在错误初始姿态上接管。
+
+## Single-Corridor Mode Risk
+
+风险等级：High
+
+原因：
+
+1. 转向完成后会很快回到走廊闭环
+2. 同一条沟道原地 `180°` 掉头，对“是否真正与墙平行”要求很高
+3. 若掉头结束时姿态仍有偏差，返程会直接带偏
+
+典型表现：
+
+1. 去程居中正常
+2. 第一次 `180°` 后开始贴右墙或贴左墙
+3. 后续每次掉头都重复同一侧贴边行为
+
+## Global Track Mode Risk
+
+风险等级：Medium
+
+赛道模式比单沟模式更安全，因为它在转向后：
+
+1. 不会立刻进入 `corridor_ctrl`
+2. 会先进入 `GNAV_REACQUIRE`
+3. 会执行 `CorridorFilter_Reset()` 重新建立沟道状态
+
+因此它不太容易复现单沟模式那种“旧状态直接继承导致返程贴边”的故障。
+
+但是，赛道模式仍然存在同类根因风险：
+
+1. `execute_turn()` 的转向完成判定仍主要依赖 IMU 相对转角
+2. `GNAV_REACQUIRE` 当前主要验证：
+   - 侧墙数量是否足够
+   - 左右距离和是否接近走廊宽度
+   - EKF `conf` 是否足够
+3. `GNAV_REACQUIRE` 当前没有显式验证“车身是否已经与侧墙平行”
+
+这意味着：
+
+1. 如果 IMU 认为已经转完 `90°`
+2. 但车体仍略微斜着进入新沟
+3. 只要宽度条件和侧墙可见条件满足，仍可能通过 `REACQUIRE`
+4. 后续以带姿态误差的方式进入沟内闭环
+
+它通常不会像单沟模式那样一下子表现得很剧烈，但可能导致：
+
+1. 进沟后偏一侧
+2. 重捕获不稳
+3. 连接段航向保持偏差累计
+
+## Recommended Strategy
+
+推荐策略是：`IMU coarse turn + wall-based fine alignment`
+
+### Step 1: IMU Coarse Turn
+
+仍然使用 IMU 相对转角做粗转：
+
+- 180° 掉头先转到接近 `180°`
+- 90° 转向先转到接近 `90°`
+
+这样可以快速把姿态大致转到目标方向附近。
+
+### Step 2: Wall-Based Fine Alignment
+
+粗转完成后，不立即恢复直行或闭环，而是利用侧墙前后差做精调。
+
+例如：
+
+- 左侧墙航向误差：`atan2(d_lr - d_lf, Ls)`
+- 右侧墙航向误差：`atan2(d_rf - d_rr, Ls)`
+
+将左右两侧可用观测融合后，得到一个“与墙是否平行”的航向误差。
+
+只有当该误差足够小，才判定“转向真正完成”。
+
+### Why This Works
+
+这样做可以把两类问题拆开：
+
+1. IMU 负责把车快速转到目标附近
+2. 侧墙负责把车最终摆正到与走廊几何一致
+
+即使 IMU 少转几度或多转几度，只要侧墙观测正常，最终仍可以把姿态纠正到可接受范围。
+
+## Recommended Changes
+
+### For Single-Corridor Mode
+
+建议必须具备：
+
+1. IMU 粗转后加入侧墙精调
+2. 精调完成前不恢复走廊直行
+3. 掉头后重建走廊参考：
+   - 重绑 IMU yaw 参考
+   - 清零 `e_th`
+   - 按需要处理 `e_y` 的方向变换
+
+### For Global Track Mode
+
+建议至少做下面其中一项：
+
+1. 在 `TURN_INTO_CORRIDOR` / `TURN_INTO_NEXT` 后加入短暂侧墙精调阶段
+2. 或者在 `GNAV_REACQUIRE` 中增加“墙面航向误差阈值”检查
+
+建议新增的判定条件：
+
+- 若左右侧墙可用，则 `wall_heading_error` 必须小于阈值
+- 否则不允许从 `REACQUIRE` 进入 `CORRIDOR_TRACK`
+
+## Recommended Debug Signals
+
+建议在调试时重点观察这些量：
+
+1. `imu_yaw_continuous`
+2. `turn_start_yaw`
+3. `delta_turned`
+4. `corridor_state.e_y`
+5. `corridor_state.e_th`
+6. `wall_heading_error`
+7. `raw_cmd.w`
+8. `safe_w`
+
+通过这些量可以快速区分：
+
+1. 是 IMU 相对转角本身不准
+2. 还是掉头后状态变换不正确
+3. 还是侧墙几何观测本身有偏差
+
+## Verification Plan
+
+### Single-Corridor Mode
+
+1. 原地转 `180°`，确认 IMU 报告与真实车头角度是否一致
+2. 粗转完成后检查 `wall_heading_error` 是否接近 0
+3. 确认返程起步前车身已基本与沟道平行
+4. 验证返程是否还会长期贴一侧墙
+
+### Global Track Mode
+
+1. 原地/实车执行 `90°` 转向
+2. 检查 `REACQUIRE` 进入条件是否会放过明显姿态误差
+3. 验证进沟后是否存在稳定偏一侧的现象
+4. 验证连接段和下一沟转入时是否存在累计偏航
+
+## Summary
+
+结论不是“不能信 IMU”，而是：
+
+1. IMU 相对转角适合做转向粗判定
+2. 但不能单独作为最终完成条件
+3. 对走廊类任务，最终是否转正应由侧墙几何关系来兜底确认
+
+因此，推荐统一采用：
+
+`IMU coarse turn + wall-based fine alignment`
+
+这套策略对单沟模式是强需求，对赛道模式是推荐增强项。
diff --git a/审查报告.md b/审查报告.md
new file mode 100644
index 0000000..99e5319
--- /dev/null
+++ b/审查报告.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` 的实车联调。
diff --git a/改进建议报告.md b/改进建议报告.md
new file mode 100644
index 0000000..0b303bd
--- /dev/null
+++ b/改进建议报告.md
@@ -0,0 +1,464 @@
+# EKF 改进建议报告
+
+> **基于**: 审查报告 + HANDOFF_v2.md + 全部相关源码精读  
+> **约束条件**: 左右四个 VL53 侧向激光测距数据可信度不高；IMU yaw 准确度非常高  
+> **目标**: 逐条回应审查报告发现，判断是否属实、是否需要修改，并给出具体改进方案
+
+---
+
+## 总体判断
+
+审查报告整体质量很高，8 条发现中 **7 条经代码验证属实**，1 条需要在新约束条件下重新评估优先级。结合"VL53 可信度低、IMU yaw 准确度高"这一关键约束，改进策略需要做出重大调整：**不应恢复侧墙航向观测，而应进一步强化 IMU 主导地位，同时用分侧补偿和自适应噪声解决横向偏置问题**。
+
+---
+
+## 一、逐条回应审查报告
+
+### 1. 横向偏置补偿模型不完整 — "持续偏右"头号嫌疑
+
+**审查报告结论**: 属实  
+**是否需要修改**: **是，优先级最高**
+
+**代码验证**:
+
+```c
+// app_tasks.c:422 — y_offset 硬编码为 0
+.y_offset = 0.0f,
+
+// corridor_ekf.c:382-396 — 只有单一 side_sensor_inset，无分侧区分
+float inset = s_cfg.side_sensor_inset;
+float d_center = (W - Rw) / 2.0f + inset;  // 左右共用同一个 d_center
+```
+
+```c
+// vl53_calibration_config.h:23-51 — 四颗 VL53 的 offset 校准值各不相同
+// 左前: 10000 um, 左后: 10000 um, 右前: 9000 um, 右后: 13000 um
+```
+
+**分析**: 四颗 VL53 的 offset 存在高达 4mm 的差异（9mm vs 13mm），但 EKF 只使用一个 `side_sensor_inset` 且设为 0.0。在 40cm 宽的走廊中每侧仅有 ~10cm 间距，4mm 差异等于 **4% 的系统性横向偏置**。这个偏置会被 EKF 当作"真实居中位置"持续输出给控制器。
+
+加上 VL53 数据本身可信度不高这一约束，这个问题的严重性进一步放大——噪声大的传感器+系统性偏置=控制器持续偏向一侧。
+
+### 2. EKF 取消侧墙航向观测，e_th 几乎完全依赖 IMU
+
+**审查报告结论**: 属实  
+**是否需要修改**: **否，当前设计在新约束下是正确的**
+
+**代码验证**:
+
+```c
+// corridor_ekf.c:345-350 注释明确说明了设计决策
+// 设计决策 (方向 B — IMU 主导航向):
+//   侧墙激光仅用于更新横向位置 e_y，不再构建航向观测 z_eth_L/z_eth_R。
+//   侧墙前后差分 (d_lr-d_lf) 的噪声在 ±2cm 误差下过大，不适合做航向主观测。
+```
+
+**分析**: 审查报告建议"恢复低权重的侧墙航向修正"。但在"VL53 可信度不高"的约束下，这个建议**不应采纳**。理由：
+
+1. 侧墙航向观测依赖同侧前后 VL53 的差分：`atan2(d_lr - d_lf, L_s)`，传感器基线 `L_s = 12cm`。如果单颗 VL53 噪声在 ±2cm 量级，差分噪声将达 ±2.8cm，对应航向噪声 `atan2(0.028, 0.12) ≈ 13°`，这比 IMU yaw 的精度差了一个数量级。
+2. IMU yaw "准确度非常高"已被确认，它是航向估计的最佳来源。
+3. 引入低质量侧墙航向观测反而会**污染** EKF 中高精度的 e_th 估计。
+
+**结论**: 当前"IMU 主导航向"的设计方向 B 在本项目约束下是正确的。不需要恢复侧墙航向观测。
+
+### 3. IMU yaw 参考锁定过早
+
+**审查报告结论**: 属实  
+**是否需要修改**: **需要调整，但策略不同于审查报告建议**
+
+**代码验证**:
+
+```c
+// corridor_filter.c:96-100
+if (!s_imu_yaw_ref_set && out_state->conf >= 0.3f) {
+    s_imu_yaw_ref_rad = imu_yaw_continuous_rad - out_state->ee_th;
+    s_imu_yaw_ref_set = true;
+}
+```
+
+**分析**: 审查报告建议"要求双侧观测稳定、e_y 和 e_th 较小、连续若干拍后再锁"。但考虑到 VL53 可信度低，依赖"双侧观测稳定"来判断是否可以锁定 yaw_ref 本身就不可靠。
+
+更好的方案是：既然 IMU yaw 准确度非常高，**yaw_ref 的锁定应该主要看入沟姿态是否对正**，而不是看 VL53 是否收敛。具体来说：
+
+- 入沟转向阶段 IMU 已经转了 85°+，转向完成时 IMU 的绝对 yaw 值就是走廊方向的最佳估计
+- 可以直接在 `transition_to(GNAV_REACQUIRE)` 时，用 IMU 当前 yaw 作为 yaw_ref（而不是等 VL53 收敛后再锁）
+- conf ≥ 0.3 这个门槛在 VL53 不可靠时意义不大
+
+### 4. CorridorFilter_Update() 覆盖 EKF 的 conf 和诊断字段
+
+**审查报告结论**: 属实，是确定性 bug  
+**是否需要修改**: **是**
+
+**代码验证**:
+
+```c
+// corridor_filter.c:103-106
+if (s_imu_yaw_ref_set) {
+    CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
+    CorridorEKF_GetState(out_state);  // ← 覆盖了之前 Update() 写入的 conf、reject_mask、maha_d2
+    out_state->t_ms = obs->t_ms;
+}
+
+// corridor_ekf.c:568-587 — GetState() 重新计算 conf，只看 P_trace
+float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
+out->conf = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
+out->mahalanobis_d2 = 0.0f;        // ← 清零
+out->obs_reject_mask = 0U;          // ← 清零
+```
+
+**分析**: `CorridorEKF_Update()` 精心计算了包含 `valid_sides`、`reject_mask`、`side_factor` 的 conf，但 `GetState()` 把这些全部丢弃，只用 P_trace 重算。这会让下游（安全层、重捕获判定）对 EKF 健康度的判断偏乐观。
+
+### 5. 转向逻辑对 IMU 失效没有后备路径
+
+**审查报告结论**: 属实  
+**是否需要修改**: **是，但优先级中等（因 IMU 准确度高，实际触发概率低）**
+
+**代码验证**:
+
+```c
+// global_nav.c:223-224
+float imu_yaw = board->imu_yaw_continuous.is_valid
+                ? board->imu_yaw_continuous.value : 0.0f;  // IMU 无效时用 0
+
+// nav_script.c:259-265 — 注释写了 fallback，但没有实现
+// 后备：如果 IMU 离线，退化回 EKF e_th 差值判定。
+float imu_delta_deg = imu_yaw_continuous_deg - s_internal.turn_start_imu_yaw_deg;
+delta_turned = imu_delta_deg * 0.01745329252f;  // 始终用 IMU，无 fallback
+```
+
+**分析**: IMU 准确度高意味着此 bug 实际触发概率极低，但代码逻辑确实存在问题。IMU 无效时 yaw 变成 0，会导致转向角度计算完全错误。应该至少加一个 fail-safe（IMU 无效时立即停车）。
+
+### 6. NavScript 的 EXIT 阶段不可达
+
+**审查报告结论**: 属实，是确定性 bug  
+**是否需要修改**: **是（单沟测试模式下需要修复）**
+
+**代码验证**:
+
+```c
+// nav_script.c:34 — pass_count 在入沟时置为 1
+s_internal.pass_count = 1;
+
+// 后续 CORRIDOR_FORWARD → TURN → CORRIDOR_BACKWARD → TURN → CORRIDOR_FORWARD 循环
+// pass_count 没有递增逻辑，也没有基于 pass_count 跳转到 EXIT 的代码
+```
+
+**分析**: 状态机在 FORWARD ↔ BACKWARD 之间无限循环，永远到不了 EXIT。在单沟测试模式下，机器人会无限往返直到电量耗尽。与赛道模式无关（赛道走 GlobalNav），但作为测试工具应该修复。
+
+### 7. HANDOFF_v2.md 与代码状态漂移
+
+**审查报告结论**: 属实  
+**是否需要修改**: **是**
+
+**代码验证**:
+
+```c
+// HANDOFF_v2.md:582 写的是 USE_GLOBAL_NAV = 1
+// robot_params.h:385 实际值
+#define USE_GLOBAL_NAV   0   // ← 文档说 1，代码是 0
+
+// HANDOFF_v2.md:635 写 kd_theta = 0.1
+// robot_params.h:229 实际值
+#define PARAM_CTRL_KD_THETA   0.4f   // ← 文档说 0.1，代码是 0.4
+
+// HANDOFF_v2.md:636 写 kp_y = 3.0
+// robot_params.h:238 实际值
+#define PARAM_CTRL_KP_Y   4.0f   // ← 文档说 3.0，代码是 4.0
+
+// HANDOFF_v2.md:647 写 d_front_stop = 0.08
+// robot_params.h:282 实际值
+#define PARAM_SAFE_D_FRONT_STOP   0.10f   // ← 文档说 0.08，代码是 0.10
+```
+
+**分析**: 多个关键参数值已漂移。对于正在调试"持续偏右"问题的人来说，如果按文档值理解系统行为会导致误判。
+
+### 8. EKF 接口与实现存在漂移
+
+**审查报告结论**: 属实  
+**是否需要修改**: **是（代码质量问题，不影响功能但影响维护）**
+
+**代码验证**:
+
+```c
+// corridor_msgs.h:13 — 仍声明 3 维观测
+#define EKF_OBS_DIM   3   // [z_ey, z_eth_L, z_eth_R]
+
+// corridor_ekf.h:12-17 — 头文件注释仍描述 3 维观测模型
+//   观测向量：z = [z_ey, z_eth_L, z_eth_R]^T
+
+// corridor_ekf.c — 实际只做 1 维 e_y 更新 + 独立 1DOF IMU yaw 更新
+
+// corridor_ekf.h:81 — K 矩阵仍按 3x3 分配
+float K[EKF_STATE_DIM][EKF_OBS_DIM];   // 3x3，实际只用 3x1
+```
+
+**分析**: `EKF_OBS_DIM` 仍为 3，但实际只做 1DOF 侧墙更新。`innovation[3]` 数组从未被写入。`S[3][3]`, `S_inv[3][3]`, `K[3][3]` 分配了多余空间。不影响运行但浪费 RAM 且误导维护者。
+
+---
+
+## 二、基于"VL53 低可信度 + IMU yaw 高精度"的 EKF 改进方案
+
+### 核心思路
+
+既然 VL53 不可靠而 IMU 非常可靠，改进策略应该是：
+
+1. **航向估计**: 完全信任 IMU，不引入侧墙航向观测（维持现状，审查报告第 2 条建议不采纳）
+2. **横向估计**: 降低 VL53 的信任度（增大 R），但通过分侧补偿消除系统性偏置
+3. **置信度**: 改用更保守的计算方式，不能因为 P_trace 小就认为状态好
+
+### 改进 A: 分侧横向补偿 (解决"持续偏右")
+
+**问题本质**: 左右 VL53 安装不对称+校准值不同，但 EKF 用同一个 `d_center` 处理左右观测。
+
+**改进方案**: 在 `CorridorEKFConfig_t` 中新增 `left_sensor_inset` 和 `right_sensor_inset`，替代单一的 `side_sensor_inset`。
+
+```c
+// corridor_ekf.h — 新增配置字段
+typedef struct {
+    // ... 现有字段 ...
+    float left_sensor_inset;   // 左侧 VL53 内缩距离 (实测)
+    float right_sensor_inset;  // 右侧 VL53 内缩距离 (实测)
+    // 保留 side_sensor_inset 做向后兼容默认值
+} CorridorEKFConfig_t;
+```
+
+```c
+// corridor_ekf.c — Update() 中分别计算左右期望读数
+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;
+
+if (left_ok) {
+    z_ey += d_center_left - ((d_lf + d_lr) / 2.0f) - yoff;
+    valid_sides++;
+}
+if (right_ok) {
+    z_ey += ((d_rf + d_rr) / 2.0f) - d_center_right - yoff;
+    valid_sides++;
+}
+```
+
+**标定方法**: 将机器人手动放在走廊正中央（卷尺确认），记录四颗 VL53 的原始读数。左侧期望读数 = `(走廊宽 - 车宽)/2`，与实际读数的差就是 `left_sensor_inset`，右侧同理。
+
+### 改进 B: 增大 VL53 观测噪声 R_ey (适配低可信度传感器)
+
+**当前值**: `PARAM_EKF_R_EY = 0.002`（相当于 VL53 标准差 ~4.5mm，过于乐观）
+
+**建议值**: 根据 VL53 "可信度不高"的实际表现，建议调大到 `0.01 ~ 0.02`（标准差 ~10-14mm）
+
+```c
+// robot_params.h
+#define PARAM_EKF_R_EY   0.015f   // 从 0.002 提高到 0.015，降低对 VL53 的信任度
+```
+
+**效果**: EKF 会更缓慢地跟踪 VL53 噪声跳变，横向估计更平滑。配合 IMU 高精度航向，控制器输出更稳定。
+
+### 改进 C: 降低 IMU 航向观测噪声 R_eth_imu (充分利用高精度 IMU)
+
+**当前值**: `PARAM_EKF_R_ETH_IMU = 0.01`
+
+**建议值**: 既然 IMU yaw "准确度非常高"，应该给更低的 R 值，让 EKF 更信任 IMU
+
+```c
+// robot_params.h
+#define PARAM_EKF_R_ETH_IMU   0.002f   // 从 0.01 降低到 0.002
+```
+
+**注意**: 当前 R_eth_imu 的注释说"应明显大于侧墙航向观测 R_ETH"，但既然侧墙航向观测已经被取消了，这个约束不再适用。
+
+### 改进 D: 修复 conf 覆盖 bug
+
+```c
+// corridor_filter.c — 改进 CorridorFilter_Update()
+if (s_imu_yaw_ref_set) {
+    CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
+
+    // 修改: 只更新状态值(e_y, e_th, s, P)，不覆盖 conf 和诊断字段
+    CorridorState_t imu_updated;
+    CorridorEKF_GetState(&imu_updated);
+    out_state->e_y  = imu_updated.e_y;
+    out_state->e_th = imu_updated.e_th;
+    out_state->s    = imu_updated.s;
+    memcpy(out_state->P, imu_updated.P, sizeof(out_state->P));
+    // 保留 out_state->conf, obs_reject_mask, mahalanobis_d2 来自 CorridorEKF_Update()
+    out_state->t_ms = obs->t_ms;
+}
+```
+
+### 改进 E: IMU yaw 参考锁定策略优化
+
+既然 IMU yaw 准确度高，锁定策略应改为：
+
+```c
+// corridor_filter.c — 新的锁定策略
+if (imu_yaw_valid) {
+    if (!s_imu_yaw_ref_set) {
+        // 方案: 不等 VL53 收敛，直接在 EKF reset 后的首拍用 IMU 当前 yaw 锁定
+        // 理由: IMU yaw 精度高，转向完成后 IMU 指向即走廊方向
+        // 此时 e_th 应为 0（刚 reset），所以 ref = imu_yaw - 0 = imu_yaw
+        s_imu_yaw_ref_rad = imu_yaw_continuous_rad;  // 不减 e_th，因为刚 reset 为 0
+        s_imu_yaw_ref_set = true;
+    }
+
+    if (s_imu_yaw_ref_set) {
+        CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
+        // ... (用改进 D 的方式更新 out_state)
+    }
+}
+```
+
+**优势**: 消除了审查报告第 3 条中"入沟偏角被固化为参考"的风险。因为 yaw_ref 在 reset 后立即锁定为 IMU 当前值（即转向完成后的方向），不会等到 VL53 不靠谱地"收敛"后才锁。
+
+### 改进 F: VL53 单侧可用时的自适应 R
+
+当只有一侧 VL53 有效时，观测质量明显下降，应该动态增大 R:
+
+```c
+// corridor_ekf.c — Update() 中
+float R_ey = s_cfg.r_ey;
+if (valid_sides == 2) {
+    R_ey *= 0.5f;      // 双侧观测，噪声更低（现有逻辑）
+} else if (valid_sides == 1) {
+    R_ey *= 3.0f;       // 新增: 单侧观测时大幅增大噪声，降低信任度
+}
+```
+
+**理由**: 单侧 VL53 在可信度本就不高的情况下，缺少交叉验证，不应赋予和双侧同等的信任权重。
+
+---
+
+## 三、非 EKF 相关改进
+
+### 改进 G: IMU 失效安全保护
+
+```c
+// global_nav.c — execute_turn() 中加入 IMU 有效性检查
+static void execute_turn(...)
+{
+    if (!board->imu_yaw_continuous.is_valid) {
+        // IMU 失效: 立即停车，不继续盲转
+        out->override_v = 0.0f;
+        out->override_w = 0.0f;
+        out->use_override = true;
+        out->safety_mode = SAFETY_MODE_IDLE;  // 停车
+        // 超时后会被外部超时保护捕获进入 ERROR
+        return;
+    }
+    // ... 正常转向逻辑 ...
+}
+```
+
+### 改进 H: NavScript EXIT 路径修复
+
+```c
+// nav_script.c — 在 CORRIDOR_BACKWARD 到端后，增加 pass_count 递增和退出判断
+case SCRIPT_STAGE_CORRIDOR_BACKWARD: {
+    // ... 到端检测 ...
+    if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
+        s_internal.pass_count++;   // ← 新增: 递增趟数
+        if (s_internal.pass_count >= 3) {  // 例如走 3 趟后退出
+            s_stage = SCRIPT_STAGE_EXIT;
+        } else {
+            s_internal.turn_start_e_th = state->e_th;
+            s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
+            s_internal.turn_started = false;
+            s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
+            s_stage = SCRIPT_STAGE_TURN_AT_END;
+        }
+        out->request_corridor = false;
+    }
+    break;
+}
+```
+
+### 改进 I: EKF 接口维度清理
+
+```c
+// corridor_msgs.h — 更新为实际使用的维度
+#define EKF_OBS_DIM   1   // 实际只做 1DOF 侧墙更新 (z_ey)
+// 或者如果考虑 IMU 独立更新:
+// #define EKF_OBS_DIM_WALL  1
+// #define EKF_OBS_DIM_IMU   1
+```
+
+同时更新 `corridor_ekf.h` 的头部注释，去掉 `z_eth_L`, `z_eth_R` 的描述，标注当前实际是 1DOF 侧墙 + 1DOF IMU 独立更新。
+
+### 改进 J: 文档同步
+
+更新 HANDOFF_v2.md 中以下不一致的参数值:
+
+| 参数 | 文档值 | 代码实际值 | 操作 |
+|------|--------|-----------|------|
+| `USE_GLOBAL_NAV` | 1 | 0 | 更新文档为 0 |
+| `kd_theta` | 0.1 | 0.4 | 更新文档为 0.4 |
+| `kp_y` | 3.0 | 4.0 | 更新文档为 4.0 |
+| `d_front_stop` | 0.08 | 0.10 | 更新文档为 0.10 |
+
+---
+
+## 四、改进优先级排序
+
+| 优先级 | 改进项 | 预期效果 | 工作量 |
+|--------|--------|---------|--------|
+| **P0** | **A: 分侧横向补偿** | 直接解决"持续偏右"根因 | 中 (改 EKF 配置 + 实测标定) |
+| **P0** | **D: 修复 conf 覆盖 bug** | 消除置信度虚高，安全层能正确响应 | 小 (改几行) |
+| **P1** | **B: 增大 R_ey** | 降低 VL53 噪声对控制的影响 | 小 (改一个参数) |
+| **P1** | **C: 降低 R_eth_imu** | 充分利用 IMU 高精度 | 小 (改一个参数) |
+| **P1** | **E: yaw_ref 锁定优化** | 消除入沟偏角固化风险 | 小 (改几行) |
+| **P2** | **F: 单侧自适应 R** | 单侧退化时更鲁棒 | 小 |
+| **P2** | **G: IMU 失效保护** | 防止 IMU 瞬断导致盲转 | 小 |
+| **P2** | **H: NavScript EXIT 修复** | 单沟测试可正常收尾 | 小 |
+| **P3** | **I: 接口维度清理** | 代码可维护性 | 中 |
+| **P3** | **J: 文档同步** | 消除调试误判 | 小 |
+
+---
+
+## 五、建议调试验证流程
+
+### 第 1 步: 实测标定 (P0-A)
+
+1. 将机器人用卷尺**精确**放在走廊正中央
+2. 记录四颗 VL53 的原始读数 (mm)
+3. 计算 `left_sensor_inset` = 理论值 - 实测左侧平均值
+4. 计算 `right_sensor_inset` = 理论值 - 实测右侧平均值
+5. 填入代码
+
+### 第 2 步: 参数调整 (P1-B,C)
+
+1. 修改 `R_ey = 0.015`, `R_eth_imu = 0.002`
+2. 单沟 `USE_GLOBAL_NAV=0` 模式验证
+3. 观察 e_y 估计值是否居中、是否平滑
+
+### 第 3 步: 修复 bug (P0-D, P1-E)
+
+1. 修复 conf 覆盖
+2. 优化 yaw_ref 锁定
+3. 单沟验证：观察入沟后 conf 是否合理、e_th 是否稳定
+
+### 第 4 步: 偏右验证
+
+1. 完成以上修改后，重新做单沟测试
+2. 如果偏右问题消失 → 核心问题已解决
+3. 如果仍有轻微偏向 → 微调 `y_offset` 参数 (之前硬编码 0 的那个)
+
+### 第 5 步: 赛道模式测试
+
+1. 切换 `USE_GLOBAL_NAV=1`
+2. 验证入沟重捕获是否正常
+3. 验证沟内偏右问题是否在多沟遍历中复现
+
+---
+
+## 六、总结
+
+| 审查报告条目 | 是否属实 | 是否采纳建议 | 说明 |
+|-------------|---------|-------------|------|
+| 1. 横向偏置不完整 | **属实** | **采纳并扩展** | 改为分侧补偿 |
+| 2. 恢复侧墙航向观测 | 属实(现象) | **不采纳建议** | VL53 不可靠时恢复航向观测有害 |
+| 3. yaw_ref 锁定过早 | **属实** | **部分采纳** | 改为 IMU 驱动的即时锁定 |
+| 4. conf 覆盖 bug | **属实** | **采纳** | 确定性 bug 必须修复 |
+| 5. IMU 失效无后备 | **属实** | **采纳** | 加 fail-safe 停车 |
+| 6. EXIT 不可达 | **属实** | **采纳** | 确定性 bug |
+| 7. 文档漂移 | **属实** | **采纳** | 对照代码更新 |
+| 8. 接口维度漂移 | **属实** | **采纳** | 清理历史包袱 |
+
+**一句话结论**: 在"VL53 可信度低 + IMU yaw 高精度"的约束下，**正确的改进方向是"强化 IMU 主导 + 分侧标定消偏 + 降低 VL53 权重"，而不是审查报告建议的"恢复侧墙航向观测"**。EKF 的方向 B 设计决策在当前硬件条件下是正确的，只需要补齐横向偏置补偿和修复 conf 覆盖 bug 就能解决"持续偏右"的核心问题。