213 lines
6.5 KiB
Markdown
213 lines
6.5 KiB
Markdown
# 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`
|
||
|
||
这套策略对单沟模式是强需求，对赛道模式是推荐增强项。
|