8.9 KiB
IMU 主导航向需求说明
1. 背景
当前项目运行场景是小车在走廊/垄沟内行驶，依赖以下传感器进行状态估计与控制：
- HWT101 IMU：提供
yaw、yaw_continuous、wz - 左右侧 VL53 测距：提供左右前后 4 个侧向距离
- 编码器里程计：提供线速度
odom_vx，并参与运动预测
目前系统在 navTask 中每 20ms 执行一次状态估计与控制，整体链路见 HANDOFF.md。
本说明文档用于明确一个新的需求方向：
- 左右激光测距主要用于横向位置参考
- 航向角估计希望主要依赖 IMU，而不是依赖左右激光前后差分计算得到的航向角
本文档只描述需求、现状和建议，不修改现有代码。
2. 当前实现现状
2.1 当前 yaw / 航向相关的数据来源
当前系统中的“航向”并不是单一量，而是分成两类：
- IMU 原始航向信息
- 文件：
App/IMU/hwt101.c - IMU 输出：
yaw：原始偏航角，范围[-180, 180)yaw_continuous：对原始 yaw 做 unwrap 后得到的连续角度wz：角速度，单位deg/s
- EKF 中的相对航向误差
e_th
- 文件：
App/est/corridor_filter.c - 文件：
App/est/corridor_ekf.c e_th表示小车相对当前走廊方向的航向误差，而不是全局绝对航向角
2.2 当前 EKF 中航向的计算方式
当前滤波流程如下：
- 预测步
- 使用
odom_vx和imu_wz做预测 - 对应代码：
CorridorEKF_Predict(odom_vx, imu_wz, dt)
- 侧墙观测更新
- 使用左右侧测距更新横向误差
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
- IMU yaw 观测更新
- 在侧墙更新之后，再用
imu_yaw_continuous做一个独立 1DOF 的航向观测更新 - 该观测形式为：
z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
其中 imu_yaw_ref_rad 是在侧墙观测可信时锁定的参考值
相关代码位置：
App/est/corridor_filter.cApp/est/corridor_ekf.c
2.3 当前系统对传感器的信任关系
从参数和注释来看，当前系统默认策略是：
- 侧墙激光是走廊内姿态估计的主观测来源
- IMU yaw 是辅助观测，用于长时约束和侧墙观测缺失时兜底
相关参数位于：App/robot_params.h
当前默认值：
PARAM_EKF_R_EY = 0.002fPARAM_EKF_R_ETH = 0.001fPARAM_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 主要负责航向角估计
- 左右激光主要负责横向位置参考
更具体地说：
- 走廊内横向控制
- 左右激光用于判断小车在走廊中的横向位置
- 支持居中行驶
- 支持偏向左/右四分之一位置行驶等策略
- 走廊内航向控制
- 航向估计应主要依赖 IMU
- 侧墙测距不应继续作为航向主观测来源
- 转向阶段
- 继续使用 IMU 连续 yaw 作为转角判定主依据
4.2 需求表达上的准确表述
如果用更工程化的语言描述该需求，可以表述为：
- “侧墙激光参与横向位置估计，不参与或仅弱参与航向角估计。”
- “航向角
e_th的主来源改为 IMUwz + 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. 推荐结论
如果以当前用户需求为准，推荐设计原则如下：
- 左右激光负责横向位置，不再主导航向
- IMU 负责航向主估计
- 转弯角度继续使用 IMU 连续 yaw 判定
- 如需保留侧墙航向，也应降为弱约束或校验项，而不是主观测
换句话说，后续如果要正式调整系统，应优先朝这个方向收敛：
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
- 该需求与当前实现存在结构性差异
- 从传感器误差特性看，这一需求是合理的
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导