ASER/Doc/turning_imu_risk_analysis.md

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

这套策略对单沟模式是强需求，对赛道模式是推荐增强项。