# 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`

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