# IMU 主导航向需求说明

## 1. 背景

当前项目运行场景是小车在走廊/垄沟内行驶，依赖以下传感器进行状态估计与控制：

- HWT101 IMU：提供 `yaw`、`yaw_continuous`、`wz`
- 左右侧 VL53 测距：提供左右前后 4 个侧向距离
- 编码器里程计：提供线速度 `odom_vx`，并参与运动预测

目前系统在 `navTask` 中每 20ms 执行一次状态估计与控制，整体链路见 `HANDOFF.md`。

本说明文档用于明确一个新的需求方向：

- 左右激光测距主要用于横向位置参考
- 航向角估计希望主要依赖 IMU，而不是依赖左右激光前后差分计算得到的航向角

本文档只描述需求、现状和建议，不修改现有代码。

## 2. 当前实现现状

### 2.1 当前 yaw / 航向相关的数据来源

当前系统中的“航向”并不是单一量，而是分成两类：

1. IMU 原始航向信息

- 文件：`App/IMU/hwt101.c`
- IMU 输出：
  - `yaw`：原始偏航角，范围 `[-180, 180)`
  - `yaw_continuous`：对原始 yaw 做 unwrap 后得到的连续角度
  - `wz`：角速度，单位 `deg/s`

2. EKF 中的相对航向误差 `e_th`

- 文件：`App/est/corridor_filter.c`
- 文件：`App/est/corridor_ekf.c`
- `e_th` 表示小车相对当前走廊方向的航向误差，而不是全局绝对航向角

### 2.2 当前 EKF 中航向的计算方式

当前滤波流程如下：

1. 预测步

- 使用 `odom_vx` 和 `imu_wz` 做预测
- 对应代码：`CorridorEKF_Predict(odom_vx, imu_wz, dt)`

2. 侧墙观测更新

- 使用左右侧测距更新横向误差 `e_y`
- 同时也使用左右同侧前后测距差来估计航向 `e_th`

对应观测形式：

- 左侧航向观测：`z_eth_L = atan2(d_lr - d_lf, Ls)`
- 右侧航向观测：`z_eth_R = atan2(d_rf - d_rr, Ls)`

相关代码位置：`App/est/corridor_ekf.c`

3. IMU yaw 观测更新

- 在侧墙更新之后，再用 `imu_yaw_continuous` 做一个独立 1DOF 的航向观测更新
- 该观测形式为：

`z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad`

其中 `imu_yaw_ref_rad` 是在侧墙观测可信时锁定的参考值

相关代码位置：

- `App/est/corridor_filter.c`
- `App/est/corridor_ekf.c`

### 2.3 当前系统对传感器的信任关系

从参数和注释来看，当前系统默认策略是：

- 侧墙激光是走廊内姿态估计的主观测来源
- IMU yaw 是辅助观测，用于长时约束和侧墙观测缺失时兜底

相关参数位于：`App/robot_params.h`

当前默认值：

- `PARAM_EKF_R_EY = 0.002f`
- `PARAM_EKF_R_ETH = 0.001f`
- `PARAM_EKF_R_ETH_IMU = 0.01f`

含义：

- 侧墙航向观测噪声更小，表示当前更信任侧墙推导出的航向角
- IMU yaw 观测噪声更大，表示当前 IMU 在 EKF 中主要是弱约束

### 2.4 当前导航脚本对 IMU yaw 的使用

虽然走廊跟踪阶段的 `e_th` 主要是由 EKF 输出，但 180 度原地转向判定已经直接使用了 IMU 的 `yaw_continuous`。

相关代码位置：`App/nav/nav_script.c`

这说明项目里已经承认一个事实：

- 在“累计转角判定”这类任务上，IMU 连续 yaw 比 EKF 的 `e_th` 更适合做主依据

## 3. 当前方案存在的问题

### 3.1 左右激光更适合测位置，不适合主导航向

用户当前判断是：

- 左右激光测距误差大约在 `+-2cm`
- 这个误差水平对于横向位置参考仍然有价值
- 但对于航向角计算不够稳定，难以直接采纳为主观测

这是一个合理判断。

原因在于，侧墙航向观测本质上来自“同侧前后两个距离的差分”：

- 左侧：`d_lr - d_lf`
- 右侧：`d_rf - d_rr`

差分量本身会放大噪声影响，尤其是在以下条件下：

- 单个传感器误差较大
- 前后基线长度有限
- 墙面不完全平整
- 传感器安装误差存在偏角或偏移

因此，虽然左右激光仍然适合估计：

- 小车是否居中
- 小车偏左还是偏右
- 小车是否位于沟宽的四分之一等目标横向位置

但未必适合继续承担“主航向来源”的角色。

### 3.2 当前结构下，侧墙观测对 e_th 的影响仍然偏强

当前 EKF 的侧墙更新同时包含：

- `e_y` 观测
- `e_th_L` / `e_th_R` 观测

所以只要侧墙数据有效，系统就会直接利用左右前后差分结果去修正航向。

如果侧墙前后差分噪声较大，就可能带来以下问题：

- `e_th` 抖动
- 控制输出 `w` 抖动
- 走廊直行时出现不必要的左右摆动
- IMU 已经给出较平滑航向，但被激光差分估计不断拉扯

## 4. 目标需求

### 4.1 总体需求

希望重新明确传感器分工：

- IMU 主要负责航向角估计
- 左右激光主要负责横向位置参考

更具体地说：

1. 走廊内横向控制

- 左右激光用于判断小车在走廊中的横向位置
- 支持居中行驶
- 支持偏向左/右四分之一位置行驶等策略

2. 走廊内航向控制

- 航向估计应主要依赖 IMU
- 侧墙测距不应继续作为航向主观测来源

3. 转向阶段

- 继续使用 IMU 连续 yaw 作为转角判定主依据

### 4.2 需求表达上的准确表述

如果用更工程化的语言描述该需求，可以表述为：

- “侧墙激光参与横向位置估计，不参与或仅弱参与航向角估计。”
- “航向角 `e_th` 的主来源改为 IMU `wz + yaw_continuous`。”
- “侧墙前后差分得到的航向观测仅作为弱约束、校验项，或直接关闭。”

## 5. 对现有系统的理解结论

基于当前代码实现，可以得出以下判断：

### 5.1 用户的想法与当前实现不一致

当前实现里：

- 左右激光不仅参与横向位置 `e_y`
- 还直接参与航向 `e_th`

而用户期望的是：

- 左右激光只负责横向位置参考
- 航向主要信任 IMU

因此，这不是简单调一个小参数就完全等价的需求，而是状态估计设计思路上的调整。

### 5.2 用户的想法在当前场景下是成立的

若侧墙测距误差确实约为 `+-2cm`，则：

- 用其估计横向偏移仍有意义
- 用其做前后差分计算航向角则很容易噪声偏大

从传感器特性匹配上看，更合理的做法就是：

- 激光负责位置
- IMU 负责航向

### 5.3 需要区分“横向位置”和“航向角”两个子问题

本需求的关键不是“全面抛弃激光”，而是要区分：

- `e_y`：仍可继续信任侧墙测距
- `e_th`：应改为主要信任 IMU

这是本需求最核心的设计点。

## 6. 后续可选改造方向

本节只记录可能的改造方向，不在本次工作中实施。

### 6.1 方向 A：仅通过参数调权，弱化侧墙航向观测

思路：

- 保留现有 EKF 结构不变
- 仅通过增大 `PARAM_EKF_R_ETH`、减小 `PARAM_EKF_R_ETH_IMU` 来让航向估计更偏向 IMU

优点：

- 修改最小
- 风险相对可控
- 可以快速实车验证

缺点：

- 侧墙航向观测仍然存在于主更新流程中
- 只是“变弱”，不是“彻底不参与”

### 6.2 方向 B：结构性调整，侧墙只更新 e_y

思路：

- 修改 EKF 观测模型
- 侧墙测距只用于更新 `e_y`
- `e_th` 仅由 `imu_wz` 预测和 `imu_yaw` 观测约束

优点：

- 最符合本需求原意
- 传感器职责边界清晰

缺点：

- 改动比参数调权大
- 需要重新验证滤波稳定性和控制效果

### 6.3 方向 C：侧墙航向只做低频校验或异常检测

思路：

- 不再把 `z_eth_L/z_eth_R` 作为主 EKF 观测
- 改成仅在长直段、双侧稳定、连续多帧一致时，低频微量校正 IMU 航向
- 或只用于诊断告警，不直接参与状态更新

优点：

- 兼顾 IMU 主导与环境约束
- 有助于抑制纯 IMU 长时漂移

缺点：

- 逻辑更复杂
- 需要额外设计稳定判据

## 7. 推荐结论

如果以当前用户需求为准，推荐设计原则如下：

1. 左右激光负责横向位置，不再主导航向
2. IMU 负责航向主估计
3. 转弯角度继续使用 IMU 连续 yaw 判定
4. 如需保留侧墙航向，也应降为弱约束或校验项，而不是主观测

换句话说，后续如果要正式调整系统，应优先朝这个方向收敛：

- `e_y` 由侧墙激光主导
- `e_th` 由 IMU 主导

## 8. 涉及模块清单

本需求后续若要实施，主要会影响以下模块：

- `App/IMU/hwt101.c`
  - IMU yaw / yaw_continuous / wz 来源
- `App/est/corridor_filter.c`
  - IMU yaw 参考值与更新调用逻辑
- `App/est/corridor_ekf.c`
  - 侧墙航向观测 `z_eth_L/z_eth_R` 的使用方式
- `App/robot_params.h`
  - 观测噪声参数调权
- `App/nav/nav_script.c`
  - 转弯阶段的 IMU yaw 使用逻辑

## 9. 本文档结论摘要

本文档确认以下几点：

- 当前系统现状：侧墙激光不仅用于横向位置，也参与航向角估计
- 用户需求：侧墙激光只作为位置参考，航向主要信任 IMU
- 该需求与当前实现存在结构性差异
- 从传感器误差特性看，这一需求是合理的
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导