322 lines
8.9 KiB
Markdown
322 lines
8.9 KiB
Markdown
# 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
|
||
- 该需求与当前实现存在结构性差异
|
||
- 从传感器误差特性看，这一需求是合理的
|
||
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导
|