1501 lines
57 KiB
Markdown
1501 lines
57 KiB
Markdown
# ARES 混合导航实施方案
|
||
|
||
> **版本**: v1.0
|
||
> **日期**: 2025 年
|
||
> **依据**: `HANDOFF.md`、`混合导航方案.md`、`map.md`、现有代码分析
|
||
> **本文目标**: 将混合导航方案从概念设计转化为可逐步实施的工程计划，明确每一个文件、每一个结构体、每一个状态转移
|
||
|
||
---
|
||
|
||
## 目录
|
||
|
||
1. [赛道几何精确推演](#1-赛道几何精确推演)
|
||
2. [S 型遍历轨迹精确描述](#2-s-型遍历轨迹精确描述)
|
||
3. [总体架构设计](#3-总体架构设计)
|
||
4. [完整状态机定义](#4-完整状态机定义)
|
||
5. [地图数据模型](#5-地图数据模型)
|
||
6. [新增模块详细设计](#6-新增模块详细设计)
|
||
7. [现有模块改造](#7-现有模块改造)
|
||
8. [数据流水线改造](#8-数据流水线改造)
|
||
9. [参数清单](#9-参数清单)
|
||
10. [文件清单汇总](#10-文件清单汇总)
|
||
11. [分步实施计划](#11-分步实施计划)
|
||
12. [风险与降级策略](#12-风险与降级策略)
|
||
|
||
---
|
||
|
||
## 1. 赛道几何精确推演
|
||
|
||
### 1.1 从 `map.md` 提取的关键尺寸
|
||
|
||
根据地图 (1 字符 = 10cm)，精确测量如下：
|
||
|
||
```
|
||
场地净尺寸: X = 300cm (30格) Y = 390cm (39格，不含启动区)
|
||
|
||
围栏: 1格 = 10cm 厚
|
||
|
||
田垄: 宽 30cm (3格) 长 220cm (22格)
|
||
左端距左围栏内壁 40cm (4格)
|
||
右端距右围栏内壁 40cm (4格)
|
||
|
||
垄沟 (南北向通道):
|
||
每条宽 40cm (4格)
|
||
长度 = 垄背长度 = 220cm (被垄背两侧约束)
|
||
|
||
端部开放区 (垄背两端到围栏):
|
||
左端: 40cm (4格)
|
||
右端: 40cm (4格)
|
||
|
||
启动区: 宽 40cm × 深 100cm，位于场地最下方 (Y=39)，紧邻左侧通道
|
||
入口: 宽 40cm (4格)，位于左下角，从Y=39到围栏外
|
||
```
|
||
|
||
### 1.2 关键几何参数表
|
||
|
||
| 参数 | 值 | 来源 |
|
||
|------|-----|------|
|
||
| 垄沟长度 (沟内可行驶) | **220 cm** | 垄背长度 = 22格 |
|
||
| 垄沟宽度 | **40 cm** | 围栏/垄背间距 |
|
||
| 端部连接通道宽度 (左端) | **40 cm** | 左围栏到垄背左端 |
|
||
| 端部连接通道宽度 (右端) | **40 cm** | 右围栏到垄背右端 |
|
||
| 连接段推进距离 (沟间距) | **70 cm** | 垄背宽 30cm + 垄沟宽 40cm = 70cm，中心到中心 |
|
||
| 入口段长度 (启动区到场内第一个垄沟中心) | **~65 cm** | 从入口边缘到 corridor 1 中心线的距离 |
|
||
| 场地总深度 (Y方向) | **390 cm** | 围栏内净尺寸 |
|
||
| 场地总宽度 (X方向) | **300 cm** | 围栏内净尺寸 |
|
||
|
||
### 1.3 6 条垄沟的 Y 坐标 (中心线，从下往上编号)
|
||
|
||
> 坐标系: 原点在左下围栏内壁角，X 向右，Y 向上
|
||
|
||
从地图分析，自下而上（从入口端开始）：
|
||
|
||
| 垄沟编号 | 两侧约束 | 中心线 Y 坐标 (约) |
|
||
|----------|----------|-------------------|
|
||
| Corridor 1 | 下围栏 ↔ 垄背5 | Y ≈ 20 cm |
|
||
| Corridor 2 | 垄背5 ↔ 垄背4 | Y ≈ 90 cm |
|
||
| Corridor 3 | 垄背4 ↔ 垄背3 | Y ≈ 160 cm |
|
||
| Corridor 4 | 垄背3 ↔ 垄背2 | Y ≈ 230 cm |
|
||
| Corridor 5 | 垄背2 ↔ 垄背1 | Y ≈ 300 cm |
|
||
| Corridor 6 | 垄背1 ↔ 上围栏 | Y ≈ 370 cm |
|
||
|
||
> **注**: 具体 Y 坐标值在实施中不重要，因为我们不做全局定位。关键参数是 **沟间距 70cm** 和 **沟长 220cm**。
|
||
|
||
---
|
||
|
||
## 2. S 型遍历轨迹精确描述
|
||
|
||
### 2.1 S 型路径定义
|
||
|
||
```
|
||
启动区 → 入口 → [左端通道向上] → 右转90° → Corridor 1 (→) → 左转90°
|
||
→ [右端通道向上 70cm] → 左转90° → Corridor 2 (←) → 右转90°
|
||
→ [左端通道向上 70cm] → 右转90° → Corridor 3 (→) → 左转90°
|
||
→ [右端通道向上 70cm] → 左转90° → Corridor 4 (←) → 右转90°
|
||
→ [左端通道向上 70cm] → 右转90° → Corridor 5 (→) → 左转90°
|
||
→ [右端通道向上 70cm] → 左转90° → Corridor 6 (←) → [到左端]
|
||
→ 左转90° → [左端通道向下 回到入口] → 出场 → 回停启动区
|
||
```
|
||
|
||
其中 `(→)` 表示从左端到右端行驶，`(←)` 表示从右端到左端行驶。
|
||
|
||
### 2.2 动作序列表 (完整遍历)
|
||
|
||
| 步骤 | 动作类型 | 传感器依赖 | 终止条件 | 方向参考 |
|
||
|------|---------|-----------|----------|---------|
|
||
| 0 | **启动区等待** | 无 | 外部启动信号 | — |
|
||
| 1 | **入场直线** | IMU航向 + 前激光 | 到达第一个垄沟入口附近（里程计 + 侧向VL53开始有效） | 向上(+Y) |
|
||
| 2 | **右转 90°** | IMU yaw_continuous | 转角达到 90° | 原地转向 |
|
||
| 3 | **重捕获入沟** | VL53 双侧有效 + conf | 锁定走廊结构 | — |
|
||
| 4 | **Corridor 1 跟踪 (→)** | VL53 + IMU + 前激光 | 前激光到端 | 向右(+X) |
|
||
| 5 | **左转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 |
|
||
| 6 | **连接段直行 70cm** | IMU航向 + 里程计 | 里程到 70cm 或侧向开始看到下一沟壁 | 向上(+Y) |
|
||
| 7 | **左转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 |
|
||
| 8 | **重捕获入沟** | VL53 | 锁定 | — |
|
||
| 9 | **Corridor 2 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 | 向左(-X) |
|
||
| 10 | **右转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 |
|
||
| 11 | **连接段直行 70cm** | IMU + 里程计 | 同步骤 6 | 向上(+Y) |
|
||
| 12 | **右转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 |
|
||
| 13 | **重捕获入沟** | VL53 | 锁定 | — |
|
||
| 14 | **Corridor 3 跟踪 (→)** | ... | ... | 向右 |
|
||
| ... | *重复 S 型模式* | ... | ... | ... |
|
||
| 29 | **Corridor 6 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 (左端围栏) | 向左 |
|
||
| 30 | **左转 90° (朝出口)** | IMU | 转角达到 90° | 原地转向 |
|
||
| 31 | **出场直线** | IMU + 里程计 + VL53丢失检测 | 侧向VL53全丢 + 里程足够 | 向下(-Y) |
|
||
| 32 | **回停启动区** | 里程计 + 超时 | 到达启动区范围内 | 减速停车 |
|
||
|
||
### 2.3 转向方向规律
|
||
|
||
关键规律（从垄沟 1 开始，假设 1→2→3→4→5→6 的 S 型）：
|
||
|
||
| 从 → 到 | 行驶方向 | 到端后出沟转向 | 连接段朝向 | 入沟转向 |
|
||
|---------|---------|-------------|-----------|---------|
|
||
| C1(→) → C2 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |
|
||
| C2(←) → C3 | 右→左 | 右转(CW) | 向上 | 右转(CW) |
|
||
| C3(→) → C4 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |
|
||
| C4(←) → C5 | 右→左 | 右转(CW) | 向上 | 右转(CW) |
|
||
| C5(→) → C6 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |
|
||
|
||
**规律**: 奇数沟(→)到端后两次左转；偶数沟(←)到端后两次右转。
|
||
|
||
可用公式表达:
|
||
```c
|
||
// 当前沟结束后的转向方向
|
||
if (corridor_id % 2 == 1) // 奇数沟(→)，在右端出
|
||
turn_direction = TURN_LEFT; // CCW, +90°
|
||
else // 偶数沟(←)，在左端出
|
||
turn_direction = TURN_RIGHT; // CW, -90°
|
||
```
|
||
|
||
---
|
||
|
||
## 3. 总体架构设计
|
||
|
||
### 3.1 三层架构
|
||
|
||
```
|
||
┌───────────────────────────────────────────────────────────────────────┐
|
||
│ 上层: 赛道级导航层 │
|
||
│ global_nav.c/.h + track_map.c/.h │
|
||
│ │
|
||
│ 职责: 知道"当前第几沟、下一沟是谁、该往哪转" │
|
||
│ 输出: ActionRequest_t { action_type, turn_dir, ... } │
|
||
└──────────────────────────────┬────────────────────────────────────────┘
|
||
│
|
||
┌──────────────────────────────▼────────────────────────────────────────┐
|
||
│ 中层: 动作执行层 │
|
||
│ action_executor.c/.h (统一调度器) │
|
||
│ 内部调用: turn_action / straight_action / reacquire │
|
||
│ │
|
||
│ 职责: 把一个"动作请求"变成实际的 v/w 指令序列 │
|
||
│ 输出: RawCmd_t { v, w } │
|
||
└──────────────────────────────┬────────────────────────────────────────┘
|
||
│
|
||
┌──────────────────────────────▼────────────────────────────────────────┐
|
||
│ 下层: 局部闭环层 (已有) │
|
||
│ corridor_ctrl.c + corridor_ekf + corridor_preproc │
|
||
│ │
|
||
│ 职责: 沟内精确居中跟踪 │
|
||
│ 输出: RawCmd_t { v, w } │
|
||
└──────────────────────────────┬────────────────────────────────────────┘
|
||
│
|
||
┌──────────────────────────────▼────────────────────────────────────────┐
|
||
│ 安全仲裁层 (已有，需改造) │
|
||
│ segment_fsm.c │
|
||
│ │
|
||
│ 职责: 根据动作模式做上下文感知的安全裁剪 │
|
||
│ 输出: SegFsmOutput_t { safe_v, safe_w } │
|
||
└──────────────────────────────┬────────────────────────────────────────┘
|
||
│
|
||
CmdSlot → CAN TX
|
||
```
|
||
|
||
### 3.2 调用关系 (navTask 内一次循环)
|
||
|
||
```
|
||
AppTasks_RunNavTask_Impl() {
|
||
// Step 1-3: 不变 (黑板快照 → 预处理 → EKF)
|
||
Blackboard_GetSnapshot(&board);
|
||
CorridorPreproc_ExtractObs(&board, now, &obs);
|
||
CorridorFilter_Update(&obs, wz, vx, dt, yaw, yaw_ok, &state);
|
||
|
||
// Step 4: 【改造】赛道级导航 + 动作执行（替代原 NavScript）
|
||
GlobalNav_Update(&obs, &state, &board, &nav_output);
|
||
// nav_output 包含:
|
||
// - action_type: 当前在执行什么动作
|
||
// - cmd: { v, w } (中层输出)
|
||
// - request_corridor: bool (是否要求走廊控制器接管)
|
||
// - safety_mode: enum (当前动作对安全层的模式要求)
|
||
|
||
// Step 5: 控制律（仅在 request_corridor 时运行）
|
||
if (nav_output.request_corridor)
|
||
CorridorCtrl_Compute(&state, &obs, imu_wz, &raw_cmd);
|
||
else
|
||
raw_cmd = nav_output.cmd;
|
||
|
||
// Step 6: 【改造】安全层（感知当前动作模式）
|
||
SegFsm_Update(&raw_cmd, &obs, &state, nav_output.safety_mode, &fsm_out);
|
||
|
||
// Step 7: 不变
|
||
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0);
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
## 4. 完整状态机定义
|
||
|
||
### 4.1 赛道级状态机 (GlobalNavStage)
|
||
|
||
```c
|
||
typedef enum {
|
||
GNAV_IDLE, // 启动区等待
|
||
GNAV_ENTRY_STRAIGHT, // 入场直线 (从启动区进入场地，向上行驶到第一条沟入口)
|
||
GNAV_TURN_INTO_CORRIDOR, // 转向进入垄沟 (原地转 90°)
|
||
GNAV_REACQUIRE, // 重捕获走廊 (低速前进，等VL53锁定)
|
||
GNAV_CORRIDOR_TRACK, // 沟内闭环跟踪 (现有 corridor_ctrl)
|
||
GNAV_TURN_OUT_OF_CORRIDOR, // 到端后出沟转向 (原地转 90°)
|
||
GNAV_LINK_STRAIGHT, // 连接段直行 (IMU航向保持 + 里程计推进)
|
||
GNAV_TURN_INTO_NEXT, // 转向进入下一条沟 (原地转 90°)
|
||
GNAV_EXIT_STRAIGHT, // 出场直行 (向下回到入口)
|
||
GNAV_DOCK, // 回停启动区
|
||
GNAV_FINISHED, // 终态
|
||
GNAV_ERROR // 异常态 (超时兜底)
|
||
} GlobalNavStage_t;
|
||
```
|
||
|
||
### 4.2 完整状态转移图
|
||
|
||
```
|
||
IDLE
|
||
│ Start()
|
||
▼
|
||
ENTRY_STRAIGHT ──────────────────────── [侧向VL53开始有效 或 里程到达]
|
||
│
|
||
▼
|
||
TURN_INTO_CORRIDOR ──────────────────── [IMU 转角 ≥ 90° - tolerance]
|
||
│
|
||
▼
|
||
REACQUIRE ───────────────────────────── [VL53双侧有效 + conf≥阈值 + 持续N拍]
|
||
│
|
||
▼
|
||
CORRIDOR_TRACK ──────────────────────── [前激光 d_front ≤ 到端阈值]
|
||
│
|
||
▼
|
||
TURN_OUT_OF_CORRIDOR ────────────────── [IMU 转角 ≥ 90° - tolerance]
|
||
│
|
||
├─── [还有未遍历的沟? → 是]
|
||
│ │
|
||
│ ▼
|
||
│ LINK_STRAIGHT ────────────────── [里程 ≥ 连接段距离 或 侧向VL53探到壁]
|
||
│ │
|
||
│ ▼
|
||
│ TURN_INTO_NEXT ───────────────── [IMU 转角 ≥ 90° - tolerance]
|
||
│ │
|
||
│ ▼
|
||
│ REACQUIRE → CORRIDOR_TRACK (循环)
|
||
│
|
||
├─── [最后一条沟完成? → 是]
|
||
│ │
|
||
│ ▼
|
||
│ EXIT_STRAIGHT ────────────────── [侧向VL53全丢 + 里程足够 或 到达出口]
|
||
│ │
|
||
│ ▼
|
||
│ DOCK ─────────────────────────── [里程到 或 超时]
|
||
│ │
|
||
│ ▼
|
||
│ FINISHED
|
||
│
|
||
└─── [超时保护 → 任何阶段超时]
|
||
│
|
||
▼
|
||
ERROR → FINISHED (安全停车)
|
||
```
|
||
|
||
### 4.3 各状态详细行为
|
||
|
||
#### GNAV_IDLE
|
||
```
|
||
行为: v=0, w=0
|
||
安全模式: SAFETY_MODE_IDLE
|
||
进入时: 系统启动默认态
|
||
退出条件: GlobalNav_Start() 被调用
|
||
退出动作: 记录 IMU yaw_ref，初始化里程计基准
|
||
```
|
||
|
||
#### GNAV_ENTRY_STRAIGHT
|
||
```
|
||
行为: v=PARAM_ENTRY_V (0.08 m/s), w=IMU航向保持PD
|
||
w = kp_heading * (yaw_ref - yaw_current) // 简单P控制保直
|
||
安全模式: SAFETY_MODE_STRAIGHT (前后激光防撞生效)
|
||
进入时: 记录起始里程
|
||
退出条件: 以下任一:
|
||
(1) 侧向VL53某侧开始有效 (探到第一条沟的壁)
|
||
(2) 里程 ≥ PARAM_ENTRY_DISTANCE
|
||
(3) 超时 PARAM_ENTRY_TIMEOUT
|
||
退出动作: 记录当前 yaw 为沟口参考
|
||
```
|
||
|
||
#### GNAV_TURN_INTO_CORRIDOR
|
||
```
|
||
行为: v=0, w=turn_dir * PARAM_TURN_OMEGA (±1.0 rad/s)
|
||
接近目标角度时减速: 剩余 < 0.5rad 时线性减速至 0.3 rad/s
|
||
安全模式: SAFETY_MODE_TURN (允许v=0时保留w, 前后激光不触发全停)
|
||
进入时: 记录 turn_start_yaw = IMU yaw_continuous
|
||
从 track_map 查询本次转向方向
|
||
退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE (默认 5°)
|
||
退出动作: EKF 重置; 记录新航向参考
|
||
```
|
||
|
||
#### GNAV_REACQUIRE
|
||
```
|
||
行为: v=PARAM_REACQUIRE_V (0.05 m/s), w=IMU航向保持PD
|
||
安全模式: SAFETY_MODE_STRAIGHT
|
||
进入时: EKF 重置; 清零确认计数器; 记录起始里程
|
||
退出条件: 以下全部持续 PARAM_REACQUIRE_CONFIRM_TICKS 拍 (默认5拍=100ms):
|
||
(1) VL53 双侧(至少3个)有效
|
||
(2) 左右距离和 ≈ 走廊宽度 (±容差 5cm)
|
||
(3) EKF conf ≥ PARAM_REACQUIRE_CONF_THRESH (0.6)
|
||
超时保护: PARAM_REACQUIRE_TIMEOUT (5000ms), 超时 → ERROR
|
||
退出动作: corridor_ctrl 初始化完成，切换到沟内跟踪
|
||
```
|
||
|
||
#### GNAV_CORRIDOR_TRACK
|
||
```
|
||
行为: request_corridor = true (交给现有 corridor_ctrl)
|
||
安全模式: SAFETY_MODE_CORRIDOR (现有 SegFsm 逻辑: 前向减速/停车/E-STOP)
|
||
进入时: 记录起始里程
|
||
退出条件: 以下任一:
|
||
(1) 前激光有效 且 d_front ≤ PARAM_END_DETECT_DIST (0.10m)
|
||
(2) 里程 ≥ PARAM_CORRIDOR_LENGTH_MAX (2.5m, 超长保护)
|
||
退出动作: 停车; corridor_id 对应的遍历标记置位
|
||
```
|
||
|
||
#### GNAV_TURN_OUT_OF_CORRIDOR
|
||
```
|
||
行为: v=0, w=turn_dir * PARAM_TURN_OMEGA
|
||
(转向方向由 track_map 决定)
|
||
安全模式: SAFETY_MODE_TURN
|
||
进入时: 记录 turn_start_yaw
|
||
退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE
|
||
退出动作: 判断是否为最后一条沟: 是 → EXIT_STRAIGHT; 否 → LINK_STRAIGHT
|
||
```
|
||
|
||
#### GNAV_LINK_STRAIGHT
|
||
```
|
||
行为: v=PARAM_LINK_V (0.10 m/s), w=IMU航向保持PD
|
||
安全模式: SAFETY_MODE_STRAIGHT
|
||
进入时: 记录起始里程; 记录航向参考
|
||
退出条件: 以下任一:
|
||
(1) 里程 ≥ PARAM_LINK_DISTANCE (0.70m, 即沟间距)
|
||
(2) 侧向VL53某侧探到新沟壁 (距离在合理范围内)
|
||
(3) 超时 PARAM_LINK_TIMEOUT (8000ms)
|
||
退出动作: 准备进入下一条沟的转向
|
||
```
|
||
|
||
#### GNAV_TURN_INTO_NEXT
|
||
```
|
||
行为: 同 GNAV_TURN_INTO_CORRIDOR (v=0, w=±turn_omega)
|
||
安全模式: SAFETY_MODE_TURN
|
||
进入时: 记录 turn_start_yaw; 从 track_map 查询方向 (同出沟方向)
|
||
退出条件: |delta_yaw| ≥ 90° - PARAM_TURN_TOLERANCE
|
||
退出动作: EKF 重置 → REACQUIRE
|
||
```
|
||
|
||
#### GNAV_EXIT_STRAIGHT
|
||
```
|
||
行为: v=PARAM_EXIT_V (0.15 m/s), w=IMU航向保持PD
|
||
航向目标: 朝向出口方向（向下）
|
||
安全模式: SAFETY_MODE_STRAIGHT (但前向停车距离放宽)
|
||
进入时: 记录起始里程
|
||
退出条件: 以下任一:
|
||
(1) 侧向VL53全部丢失 + 里程 ≥ PARAM_EXIT_RUNOUT (1.5m)
|
||
(2) 里程 ≥ PARAM_EXIT_MAX_DIST (4.0m, 保护)
|
||
(3) 超时 PARAM_EXIT_TIMEOUT (20000ms)
|
||
退出动作: 进入回停
|
||
```
|
||
|
||
#### GNAV_DOCK
|
||
```
|
||
行为: v=PARAM_DOCK_V (0.05 m/s), w=0
|
||
推进一段短距离后停车
|
||
安全模式: SAFETY_MODE_STRAIGHT
|
||
进入时: 记录起始里程
|
||
退出条件: 里程 ≥ PARAM_DOCK_DISTANCE (0.5m) 或超时
|
||
退出动作: 强制零速 → FINISHED
|
||
```
|
||
|
||
#### GNAV_FINISHED
|
||
```
|
||
行为: v=0, w=0 (永久停车)
|
||
安全模式: SAFETY_MODE_IDLE
|
||
终态: 不再转移
|
||
```
|
||
|
||
#### GNAV_ERROR
|
||
```
|
||
行为: v=0, w=0 (紧急停车)
|
||
安全模式: SAFETY_MODE_IDLE
|
||
说明: 任何阶段超时保护触发后进入
|
||
等待 PARAM_ERROR_HOLD_TIME 后 → FINISHED
|
||
```
|
||
|
||
---
|
||
|
||
## 5. 地图数据模型
|
||
|
||
### 5.1 设计原则
|
||
|
||
地图不做全局坐标定位。地图只回答三个问题：
|
||
1. 从第 N 条沟完成后，下一条是第几条？
|
||
2. 这次该往哪转？（左/右）
|
||
3. 当前是不是最后一条沟？
|
||
|
||
### 5.2 数据结构
|
||
|
||
```c
|
||
/* ---- App/nav/track_map.h ---- */
|
||
|
||
#define TRACK_MAP_CORRIDOR_COUNT 6
|
||
#define TRACK_MAP_LINK_DISTANCE_M 0.70f /* 沟间距 (中心到中心) */
|
||
#define TRACK_MAP_CORRIDOR_LENGTH_M 2.20f /* 垄沟标称长度 */
|
||
|
||
typedef enum {
|
||
TRAVEL_DIR_POSITIVE, /* 从左端到右端 (→) */
|
||
TRAVEL_DIR_NEGATIVE /* 从右端到左端 (←) */
|
||
} TravelDirection_t;
|
||
|
||
typedef enum {
|
||
TURN_DIR_LEFT = +1, /* 逆时针 (CCW), w > 0 */
|
||
TURN_DIR_RIGHT = -1 /* 顺时针 (CW), w < 0 */
|
||
} TurnDirection_t;
|
||
|
||
/* 单条垄沟描述 */
|
||
typedef struct {
|
||
uint8_t id; /* 0-5 */
|
||
TravelDirection_t travel_dir; /* 本沟行驶方向 */
|
||
TurnDirection_t exit_turn_dir; /* 出沟时的转向方向 */
|
||
TurnDirection_t entry_turn_dir; /* 入沟时的转向方向 (= exit_turn_dir) */
|
||
bool is_last; /* 是否为最后一条沟 */
|
||
} CorridorDescriptor_t;
|
||
|
||
/* 地图查询接口 */
|
||
typedef struct {
|
||
CorridorDescriptor_t corridors[TRACK_MAP_CORRIDOR_COUNT];
|
||
uint8_t entry_corridor_id; /* 入场后第一条沟 = 0 */
|
||
float link_distance_m; /* 连接段标称距离 */
|
||
float corridor_length_m; /* 垄沟标称长度 */
|
||
} TrackMap_t;
|
||
```
|
||
|
||
### 5.3 地图初始化 (硬编码 S 型遍历表)
|
||
|
||
```c
|
||
/* ---- App/nav/track_map.c ---- */
|
||
|
||
static const TrackMap_t s_map = {
|
||
.corridors = {
|
||
/* id travel_dir exit_turn entry_turn is_last */
|
||
{ 0, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, // C1: →, 到右端后左转
|
||
{ 1, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, // C2: ←, 到左端后右转
|
||
{ 2, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, // C3: →
|
||
{ 3, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, // C4: ←
|
||
{ 4, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, // C5: →
|
||
{ 5, TRAVEL_DIR_NEGATIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, true }, // C6: ←, 最后一条
|
||
},
|
||
.entry_corridor_id = 0,
|
||
.link_distance_m = 0.70f,
|
||
.corridor_length_m = 2.20f,
|
||
};
|
||
|
||
/* 查询 API */
|
||
void TrackMap_Init(void);
|
||
const TrackMap_t* TrackMap_Get(void);
|
||
const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id);
|
||
uint8_t TrackMap_GetNextCorridorId(uint8_t current_id);
|
||
bool TrackMap_IsLastCorridor(uint8_t id);
|
||
TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id);
|
||
TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id);
|
||
```
|
||
|
||
> **为什么 C6 出沟后是左转？**
|
||
> C6(←) 行驶到左端完成后，需要朝向出口（向下）方向离场。从向左(-X)行驶的状态左转90°后，正好朝向下方(-Y)，对准出口。
|
||
|
||
---
|
||
|
||
## 6. 新增模块详细设计
|
||
|
||
### 6.1 `App/nav/global_nav.c/.h` — 赛道级总控
|
||
|
||
这是整个混合导航的核心模块，替代现有 `nav_script.c` 的赛道编排职能。
|
||
|
||
#### 头文件接口
|
||
|
||
```c
|
||
/* ---- App/nav/global_nav.h ---- */
|
||
|
||
#include "corridor_msgs.h"
|
||
#include "track_map.h"
|
||
|
||
/* 安全模式 (传递给 segment_fsm) */
|
||
typedef enum {
|
||
SAFETY_MODE_IDLE, /* 零速，不做任何裁剪 */
|
||
SAFETY_MODE_CORRIDOR, /* 沟内: 前向减速/停车/E-STOP 全开 */
|
||
SAFETY_MODE_TURN, /* 转向: 允许v=0+w≠0, 前向不全停 */
|
||
SAFETY_MODE_STRAIGHT /* 直行段: 前后激光防撞, 无侧向约束 */
|
||
} SafetyMode_t;
|
||
|
||
/* 赛道级导航配置 */
|
||
typedef struct {
|
||
float entry_v; /* 入场速度 m/s */
|
||
float entry_distance; /* 入场里程上限 m */
|
||
uint32_t entry_timeout_ms; /* 入场超时 ms */
|
||
|
||
float turn_omega; /* 转向角速度 rad/s */
|
||
float turn_tolerance_rad; /* 转向完成容差 rad */
|
||
float turn_decel_zone_rad; /* 接近目标角时的减速区 rad */
|
||
float turn_min_omega; /* 减速区最低角速度 rad/s */
|
||
uint32_t turn_timeout_ms; /* 单次转向超时 ms */
|
||
|
||
float reacquire_v; /* 重捕获速度 m/s */
|
||
float reacquire_conf_thresh; /* 重捕获置信度阈值 */
|
||
float reacquire_width_tol; /* 走廊宽度容差 m */
|
||
uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
|
||
uint32_t reacquire_timeout_ms;
|
||
|
||
float corridor_end_detect_dist; /* 到端检测距离 m */
|
||
float corridor_length_max; /* 沟内里程保护上限 m */
|
||
|
||
float link_v; /* 连接段速度 m/s */
|
||
float link_distance; /* 连接段标称距离 m */
|
||
uint32_t link_timeout_ms;
|
||
|
||
float exit_v; /* 出场速度 m/s */
|
||
float exit_runout; /* 出场冲刺距离 m */
|
||
float exit_max_dist; /* 出场里程保护 m */
|
||
uint32_t exit_timeout_ms;
|
||
|
||
float dock_v; /* 回停速度 m/s */
|
||
float dock_distance; /* 回停距离 m */
|
||
|
||
float heading_kp; /* 航向保持P增益 */
|
||
} GlobalNavConfig_t;
|
||
|
||
/* 赛道级导航输出 */
|
||
typedef struct {
|
||
GlobalNavStage_t stage; /* 当前阶段 */
|
||
const char* stage_name; /* 阶段名字符串 (调试) */
|
||
uint8_t corridor_id; /* 当前/目标垄沟 ID */
|
||
uint8_t corridors_done; /* 已完成垄沟数 */
|
||
|
||
bool request_corridor; /* 是否请求沟内闭环 */
|
||
bool use_override; /* 是否用 override 指令 */
|
||
float override_v;
|
||
float override_w;
|
||
|
||
SafetyMode_t safety_mode; /* 当前安全模式 */
|
||
bool active; /* 导航是否在运行 */
|
||
} GlobalNavOutput_t;
|
||
|
||
/* API */
|
||
void GlobalNav_Init(const GlobalNavConfig_t* cfg);
|
||
void GlobalNav_Start(void);
|
||
void GlobalNav_Stop(void);
|
||
void GlobalNav_Reset(void);
|
||
void GlobalNav_Update(
|
||
const CorridorObs_t* obs,
|
||
const CorridorState_t* state,
|
||
const RobotBlackboard_t* board, /* 需要读 IMU yaw_continuous, odom */
|
||
GlobalNavOutput_t* out
|
||
);
|
||
GlobalNavStage_t GlobalNav_GetStage(void);
|
||
```
|
||
|
||
#### 内部状态
|
||
|
||
```c
|
||
/* ---- global_nav.c 内部 ---- */
|
||
|
||
static struct {
|
||
GlobalNavStage_t stage;
|
||
bool running;
|
||
|
||
/* 赛道级 */
|
||
uint8_t current_corridor_id;
|
||
uint8_t corridors_completed;
|
||
|
||
/* 转向 */
|
||
float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */
|
||
float turn_target_delta_deg; /* 90° = ±90° */
|
||
int8_t turn_sign; /* +1 or -1 */
|
||
|
||
/* 里程 */
|
||
float stage_start_odom_s; /* EKF s 或 里程计 at stage entry */
|
||
float stage_start_vx_accum; /* 里程计累积距离 */
|
||
|
||
/* 超时 */
|
||
uint32_t stage_start_ms;
|
||
|
||
/* 重捕获 */
|
||
uint8_t reacquire_ok_count;
|
||
|
||
/* 出场 */
|
||
bool exit_vl53_lost;
|
||
float exit_lost_odom_s;
|
||
|
||
/* 航向保持 */
|
||
float heading_ref_deg; /* 直行段航向参考 */
|
||
|
||
/* 配置引用 */
|
||
GlobalNavConfig_t cfg;
|
||
} s_nav;
|
||
```
|
||
|
||
#### 核心 Update 逻辑 (伪代码)
|
||
|
||
```c
|
||
void GlobalNav_Update(..., GlobalNavOutput_t* out) {
|
||
uint32_t now = HAL_GetTick();
|
||
float imu_yaw_deg = board->imu_yaw_continuous.value;
|
||
float odom_vx = board->odom_vx.value;
|
||
float elapsed_ms = now - s_nav.stage_start_ms;
|
||
|
||
// 默认输出
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->override_v = 0;
|
||
out->override_w = 0;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
out->stage = s_nav.stage;
|
||
out->corridor_id = s_nav.current_corridor_id;
|
||
out->corridors_done = s_nav.corridors_completed;
|
||
out->active = s_nav.running;
|
||
|
||
if (!s_nav.running) return;
|
||
|
||
switch (s_nav.stage) {
|
||
|
||
case GNAV_ENTRY_STRAIGHT:
|
||
out->override_v = s_nav.cfg.entry_v;
|
||
out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, s_nav.cfg.heading_kp);
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
// 退出: 侧向探到壁 或 里程到 或 超时
|
||
if (side_walls_detected(obs) || odom_advanced_enough() || elapsed_ms > s_nav.cfg.entry_timeout_ms)
|
||
transition_to(GNAV_TURN_INTO_CORRIDOR);
|
||
break;
|
||
|
||
case GNAV_TURN_INTO_CORRIDOR:
|
||
case GNAV_TURN_OUT_OF_CORRIDOR:
|
||
case GNAV_TURN_INTO_NEXT:
|
||
execute_turn(obs, state, board, out);
|
||
break;
|
||
|
||
case GNAV_REACQUIRE:
|
||
out->override_v = s_nav.cfg.reacquire_v;
|
||
out->override_w = heading_hold_pd(...);
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
if (check_reacquire_conditions(obs, state))
|
||
s_nav.reacquire_ok_count++;
|
||
else
|
||
s_nav.reacquire_ok_count = 0;
|
||
|
||
if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
|
||
// 重捕获成功，进入沟内跟踪
|
||
transition_to(GNAV_CORRIDOR_TRACK);
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms)
|
||
transition_to(GNAV_ERROR);
|
||
break;
|
||
|
||
case GNAV_CORRIDOR_TRACK:
|
||
out->use_override = false;
|
||
out->request_corridor = true;
|
||
out->safety_mode = SAFETY_MODE_CORRIDOR;
|
||
|
||
// 到端检测
|
||
bool front_valid = obs->valid_mask & CORRIDOR_OBS_MASK_FRONT;
|
||
if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
|
||
s_nav.corridors_completed++;
|
||
transition_to(GNAV_TURN_OUT_OF_CORRIDOR);
|
||
}
|
||
// 里程保护
|
||
if (odom_since_entry() > s_nav.cfg.corridor_length_max)
|
||
transition_to(GNAV_TURN_OUT_OF_CORRIDOR);
|
||
break;
|
||
|
||
case GNAV_LINK_STRAIGHT:
|
||
out->override_v = s_nav.cfg.link_v;
|
||
out->override_w = heading_hold_pd(...);
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
if (odom_since_entry() >= s_nav.cfg.link_distance || side_walls_detected(obs))
|
||
transition_to(GNAV_TURN_INTO_NEXT);
|
||
if (elapsed_ms > s_nav.cfg.link_timeout_ms)
|
||
transition_to(GNAV_ERROR);
|
||
break;
|
||
|
||
case GNAV_EXIT_STRAIGHT:
|
||
out->override_v = s_nav.cfg.exit_v;
|
||
out->override_w = heading_hold_pd(...);
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
// 检测侧向全丢
|
||
if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
|
||
s_nav.exit_vl53_lost = true;
|
||
s_nav.exit_lost_odom_s = current_odom();
|
||
}
|
||
if (s_nav.exit_vl53_lost && (current_odom() - s_nav.exit_lost_odom_s) >= s_nav.cfg.exit_runout)
|
||
transition_to(GNAV_DOCK);
|
||
if (elapsed_ms > s_nav.cfg.exit_timeout_ms)
|
||
transition_to(GNAV_DOCK);
|
||
break;
|
||
|
||
case GNAV_DOCK:
|
||
out->override_v = s_nav.cfg.dock_v;
|
||
out->override_w = 0;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
if (odom_since_entry() >= s_nav.cfg.dock_distance || elapsed_ms > 5000)
|
||
transition_to(GNAV_FINISHED);
|
||
break;
|
||
|
||
case GNAV_FINISHED:
|
||
out->override_v = 0;
|
||
out->override_w = 0;
|
||
out->active = false;
|
||
break;
|
||
|
||
case GNAV_ERROR:
|
||
out->override_v = 0;
|
||
out->override_w = 0;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
if (elapsed_ms > 2000)
|
||
transition_to(GNAV_FINISHED);
|
||
break;
|
||
}
|
||
}
|
||
```
|
||
|
||
#### transition_to 函数
|
||
|
||
```c
|
||
static void transition_to(GlobalNavStage_t next) {
|
||
// 通用: 记录进入时间和里程
|
||
s_nav.stage_start_ms = HAL_GetTick();
|
||
s_nav.stage_start_odom_s = /* 当前里程 */;
|
||
s_nav.reacquire_ok_count = 0;
|
||
|
||
switch (next) {
|
||
case GNAV_TURN_INTO_CORRIDOR: {
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
|
||
s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
break;
|
||
}
|
||
case GNAV_TURN_OUT_OF_CORRIDOR: {
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
|
||
s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
break;
|
||
}
|
||
case GNAV_TURN_INTO_NEXT: {
|
||
uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
|
||
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
|
||
s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
s_nav.current_corridor_id = next_id;
|
||
break;
|
||
}
|
||
case GNAV_REACQUIRE:
|
||
CorridorFilter_Reset(); // EKF 重置
|
||
s_nav.heading_ref_deg = /* 当前 IMU yaw */;
|
||
break;
|
||
|
||
case GNAV_CORRIDOR_TRACK:
|
||
break;
|
||
|
||
case GNAV_LINK_STRAIGHT: {
|
||
s_nav.heading_ref_deg = /* 当前 IMU yaw */;
|
||
break;
|
||
}
|
||
case GNAV_EXIT_STRAIGHT: {
|
||
// 最后一条沟出来后，转向应该已经朝向出口
|
||
s_nav.heading_ref_deg = /* 当前 IMU yaw */;
|
||
s_nav.exit_vl53_lost = false;
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
|
||
s_nav.stage = next;
|
||
}
|
||
```
|
||
|
||
#### 转向执行 (统一的 90° 转向逻辑)
|
||
|
||
```c
|
||
static void execute_turn(const CorridorObs_t* obs, const CorridorState_t* state,
|
||
const RobotBlackboard_t* board, GlobalNavOutput_t* out) {
|
||
float imu_yaw = board->imu_yaw_continuous.value;
|
||
float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
|
||
float target = s_nav.turn_target_delta_deg;
|
||
float remaining_deg = target - delta;
|
||
float remaining_rad = remaining_deg * PARAM_DEG2RAD_FACTOR;
|
||
|
||
float omega = s_nav.cfg.turn_omega;
|
||
|
||
// 接近目标时减速
|
||
if (remaining_rad < s_nav.cfg.turn_decel_zone_rad) {
|
||
float ratio = remaining_rad / s_nav.cfg.turn_decel_zone_rad;
|
||
omega = s_nav.cfg.turn_min_omega + ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
|
||
}
|
||
|
||
out->override_v = 0;
|
||
out->override_w = s_nav.turn_sign * omega;
|
||
out->use_override = true;
|
||
out->safety_mode = SAFETY_MODE_TURN;
|
||
|
||
float tolerance_deg = s_nav.cfg.turn_tolerance_rad * PARAM_RAD2DEG_FACTOR;
|
||
|
||
if (delta >= target - tolerance_deg) {
|
||
// 转向完成
|
||
switch (s_nav.stage) {
|
||
case GNAV_TURN_INTO_CORRIDOR:
|
||
case GNAV_TURN_INTO_NEXT:
|
||
transition_to(GNAV_REACQUIRE);
|
||
break;
|
||
case GNAV_TURN_OUT_OF_CORRIDOR:
|
||
if (TrackMap_IsLastCorridor(s_nav.current_corridor_id))
|
||
transition_to(GNAV_EXIT_STRAIGHT);
|
||
else
|
||
transition_to(GNAV_LINK_STRAIGHT);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 超时保护
|
||
if (HAL_GetTick() - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms)
|
||
transition_to(GNAV_ERROR);
|
||
}
|
||
```
|
||
|
||
### 6.2 `App/nav/track_map.c/.h` — 赛道地图
|
||
|
||
见第 5 节数据结构。实现很简单，就是一个 const 表 + 查询函数。
|
||
|
||
```c
|
||
/* track_map.c */
|
||
#include "track_map.h"
|
||
|
||
static const TrackMap_t s_map = { /* 见 5.3 */ };
|
||
static bool s_initialized = false;
|
||
|
||
void TrackMap_Init(void) {
|
||
s_initialized = true;
|
||
}
|
||
|
||
const TrackMap_t* TrackMap_Get(void) {
|
||
return &s_map;
|
||
}
|
||
|
||
const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id) {
|
||
if (id >= TRACK_MAP_CORRIDOR_COUNT) return &s_map.corridors[0];
|
||
return &s_map.corridors[id];
|
||
}
|
||
|
||
uint8_t TrackMap_GetNextCorridorId(uint8_t current_id) {
|
||
if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) return current_id;
|
||
return current_id + 1;
|
||
}
|
||
|
||
bool TrackMap_IsLastCorridor(uint8_t id) {
|
||
return s_map.corridors[id].is_last;
|
||
}
|
||
|
||
TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id) {
|
||
return s_map.corridors[id].exit_turn_dir;
|
||
}
|
||
|
||
TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id) {
|
||
return s_map.corridors[id].entry_turn_dir;
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
## 7. 现有模块改造
|
||
|
||
### 7.1 `segment_fsm.c/.h` — 安全层增加动作模式感知
|
||
|
||
#### 改动要点
|
||
|
||
1. 新增 `SafetyMode_t` 参数输入
|
||
2. `SegFsm_Update()` 签名变更: 增加 `SafetyMode_t mode` 参数
|
||
3. 根据 mode 决定安全策略
|
||
|
||
#### 改动后的行为表
|
||
|
||
| SafetyMode | 前向减速/停车 | E-STOP (conf低) | w 通过 |
|
||
|------------|-------------|-----------------|--------|
|
||
| IDLE | 不触发 | 不触发 | 清零 |
|
||
| CORRIDOR | ✅ 正常触发 | ✅ 正常触发 | 正常 |
|
||
| TURN | ❌ 不触发 (v=0时不检查前距) | ❌ 不触发 (转向时conf低是正常的) | ✅ 直接通过 |
|
||
| STRAIGHT | ✅ 正常触发 | ❌ 不触发 | 正常 |
|
||
|
||
#### 改动后的函数签名
|
||
|
||
```c
|
||
/* segment_fsm.h 改动 */
|
||
|
||
// 删除旧的:
|
||
// void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs,
|
||
// const CorridorState_t* state, SegFsmOutput_t* out);
|
||
|
||
// 新增:
|
||
void SegFsm_Update(const RawCmd_t* raw_cmd,
|
||
const CorridorObs_t* obs,
|
||
const CorridorState_t* state,
|
||
SafetyMode_t mode, /* 新增 */
|
||
SegFsmOutput_t* out);
|
||
```
|
||
|
||
#### 关键代码改动 (`segment_fsm.c`)
|
||
|
||
```c
|
||
void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs,
|
||
const CorridorState_t* state, SafetyMode_t mode,
|
||
SegFsmOutput_t* out) {
|
||
|
||
if (mode == SAFETY_MODE_IDLE) {
|
||
out->state = SEG_STATE_IDLE;
|
||
out->safe_v = 0;
|
||
out->safe_w = 0;
|
||
return;
|
||
}
|
||
|
||
if (mode == SAFETY_MODE_TURN) {
|
||
// 转向模式: 直接放行，不做前向检查
|
||
out->state = SEG_STATE_CORRIDOR; // 复用 CORRIDOR 表示放行
|
||
out->safe_v = raw_cmd->v;
|
||
out->safe_w = raw_cmd->w;
|
||
// 可选: w 的绝对值做上限裁剪
|
||
return;
|
||
}
|
||
|
||
// CORRIDOR / STRAIGHT 模式: 执行原有逻辑
|
||
// CORRIDOR 额外检查 conf → E-STOP
|
||
// STRAIGHT 不检查 conf
|
||
|
||
if (mode == SAFETY_MODE_CORRIDOR) {
|
||
// 原有 E-STOP 逻辑 (conf 检查)
|
||
if (state->conf < s_cfg.conf_estop_thresh) {
|
||
s_state = SEG_STATE_ESTOP;
|
||
}
|
||
if (s_state == SEG_STATE_ESTOP && state->conf >= 0.5f) {
|
||
s_state = SEG_STATE_CORRIDOR;
|
||
}
|
||
}
|
||
|
||
// 前向距离检查 (CORRIDOR 和 STRAIGHT 都做)
|
||
// ... 现有逻辑不变 ...
|
||
}
|
||
```
|
||
|
||
### 7.2 `nav_script.c/.h` — 降级为测试脚本
|
||
|
||
保留现有代码不删除，但不再在正式比赛流程中使用。
|
||
|
||
改动：
|
||
- 在 `app_tasks.c` 中通过编译开关选择使用 `NavScript` 还是 `GlobalNav`
|
||
- `nav_script.c` 保持现状，用于单垄沟测试验证
|
||
|
||
```c
|
||
/* app_tasks.c 中增加编译开关 */
|
||
#define USE_GLOBAL_NAV 1 /* 1=正式比赛模式 0=单沟测试模式 */
|
||
```
|
||
|
||
### 7.3 `corridor_msgs.h` — 补充枚举和结构
|
||
|
||
新增以下定义：
|
||
|
||
```c
|
||
/* corridor_msgs.h 新增 */
|
||
|
||
/* 赛道级阶段枚举 (已在 global_nav.h 定义，这里仅前向声明) */
|
||
/* #include "global_nav.h" 或直接在 corridor_msgs.h 中定义 */
|
||
|
||
/* 安全模式枚举 (可在此处定义，供 segment_fsm 和 global_nav 共享) */
|
||
typedef enum {
|
||
SAFETY_MODE_IDLE,
|
||
SAFETY_MODE_CORRIDOR,
|
||
SAFETY_MODE_TURN,
|
||
SAFETY_MODE_STRAIGHT
|
||
} SafetyMode_t;
|
||
```
|
||
|
||
> 注意: `SafetyMode_t` 定义应放在 `corridor_msgs.h` 中（数据契约），而非 `global_nav.h` 中，以便 `segment_fsm` 不需要反向依赖 `global_nav`。
|
||
|
||
### 7.4 `corridor_filter.c/.h` — 增加外部 Reset 接口
|
||
|
||
当前 `CorridorFilter_Reset()` 可能不存在或功能不完整。需要确保：
|
||
|
||
```c
|
||
/* corridor_filter.h 确认/新增 */
|
||
void CorridorFilter_Reset(void);
|
||
```
|
||
|
||
该函数应：
|
||
1. 调用 `CorridorEKF_Reset()`
|
||
2. 重置 yaw_ref 状态
|
||
3. 重置 yaw_ref_locked 标志
|
||
|
||
这在每次进入新垄沟时必须调用，否则 EKF 会用旧沟的参考值去更新新沟的状态。
|
||
|
||
### 7.5 `app_tasks.c` — navTask 流水线改造
|
||
|
||
详见第 8 节。
|
||
|
||
---
|
||
|
||
## 8. 数据流水线改造
|
||
|
||
### 8.1 改造后的 navTask 流水线
|
||
|
||
```c
|
||
/* app_tasks.c - AppTasks_RunNavTask_Impl() */
|
||
|
||
void AppTasks_RunNavTask_Impl(void) {
|
||
static uint32_t last_ms = 0;
|
||
uint32_t now = HAL_GetTick();
|
||
float dt = (now - last_ms) * 0.001f;
|
||
if (dt <= 0.0f || dt > 0.1f) dt = 0.02f;
|
||
last_ms = now;
|
||
|
||
/* ========== Step 1: 传感器快照 (不变) ========== */
|
||
RobotBlackboard_t board;
|
||
Blackboard_GetSnapshot(&board);
|
||
|
||
/* ========== Step 2: 预处理 (不变) ========== */
|
||
CorridorObs_t obs;
|
||
CorridorPreproc_ExtractObs(&board, now, &obs);
|
||
|
||
/* ========== Step 3: EKF (不变) ========== */
|
||
float imu_wz_rad = PARAM_DEG2RAD(board.imu_wz.value);
|
||
float odom_vx = board.odom_vx.value;
|
||
float imu_yaw_rad = PARAM_DEG2RAD(board.imu_yaw_continuous.value);
|
||
bool imu_yaw_ok = board.imu_yaw_continuous.is_valid;
|
||
|
||
CorridorState_t corridor_state;
|
||
CorridorFilter_Update(&obs, imu_wz_rad, odom_vx, dt,
|
||
imu_yaw_rad, imu_yaw_ok, &corridor_state);
|
||
|
||
#if USE_GLOBAL_NAV
|
||
/* ========== Step 4: 赛道级导航 (新) ========== */
|
||
GlobalNavOutput_t nav_out;
|
||
GlobalNav_Update(&obs, &corridor_state, &board, &nav_out);
|
||
|
||
/* ========== Step 5: 控制律 ========== */
|
||
RawCmd_t raw_cmd = {0};
|
||
if (nav_out.request_corridor) {
|
||
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd);
|
||
} else if (nav_out.use_override) {
|
||
raw_cmd.v = nav_out.override_v;
|
||
raw_cmd.w = nav_out.override_w;
|
||
}
|
||
|
||
/* ========== Step 6: 安全仲裁 (改造) ========== */
|
||
SegFsmOutput_t fsm_out;
|
||
SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);
|
||
|
||
#else
|
||
/* ---------- 旧版: 单沟测试模式 ---------- */
|
||
NavScriptOutput_t script_out;
|
||
float imu_yaw_deg = board.imu_yaw_continuous.value;
|
||
NavScript_Update(&obs, &corridor_state, imu_yaw_deg, &script_out);
|
||
|
||
RawCmd_t raw_cmd = {0};
|
||
if (script_out.use_override) {
|
||
raw_cmd.v = script_out.override_v;
|
||
raw_cmd.w = script_out.override_w;
|
||
} else if (script_out.request_corridor) {
|
||
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd);
|
||
}
|
||
|
||
SegFsmOutput_t fsm_out;
|
||
SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out);
|
||
#endif
|
||
|
||
/* ========== Step 7: 推送指令 (不变) ========== */
|
||
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0);
|
||
}
|
||
```
|
||
|
||
### 8.2 初始化改造
|
||
|
||
```c
|
||
void AppTasks_Init(void) {
|
||
// ... 现有初始化不变 ...
|
||
|
||
// 新增
|
||
TrackMap_Init();
|
||
|
||
GlobalNavConfig_t gnav_cfg = {
|
||
.entry_v = PARAM_GNAV_ENTRY_V,
|
||
.entry_distance = PARAM_GNAV_ENTRY_DISTANCE,
|
||
.entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT,
|
||
.turn_omega = PARAM_GNAV_TURN_OMEGA,
|
||
.turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE,
|
||
.turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE,
|
||
.turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA,
|
||
.turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT,
|
||
.reacquire_v = PARAM_GNAV_REACQUIRE_V,
|
||
.reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF,
|
||
.reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
|
||
.reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
|
||
.reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT,
|
||
.corridor_end_detect_dist = PARAM_SAFE_D_FRONT_STOP,
|
||
.corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN,
|
||
.link_v = PARAM_GNAV_LINK_V,
|
||
.link_distance = PARAM_GNAV_LINK_DISTANCE,
|
||
.link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT,
|
||
.exit_v = PARAM_GNAV_EXIT_V,
|
||
.exit_runout = PARAM_GNAV_EXIT_RUNOUT,
|
||
.exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST,
|
||
.exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT,
|
||
.dock_v = PARAM_GNAV_DOCK_V,
|
||
.dock_distance = PARAM_GNAV_DOCK_DISTANCE,
|
||
.heading_kp = PARAM_GNAV_HEADING_KP,
|
||
};
|
||
GlobalNav_Init(&gnav_cfg);
|
||
|
||
// ... 延迟启动 ...
|
||
// 在 navTask 中延迟后调用 GlobalNav_Start()
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
## 9. 参数清单
|
||
|
||
### 9.1 新增参数 (`robot_params.h`)
|
||
|
||
```c
|
||
/* ======== P6 — 赛道级导航参数 ======== */
|
||
|
||
/* 入场段 */
|
||
#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 */
|
||
#define PARAM_GNAV_ENTRY_DISTANCE 0.65f /* m — 入场里程上限 */
|
||
#define PARAM_GNAV_ENTRY_TIMEOUT 10000 /* ms — 入场超时 */
|
||
|
||
/* 转向 */
|
||
#define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */
|
||
#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */
|
||
#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */
|
||
#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */
|
||
#define PARAM_GNAV_TURN_TIMEOUT 8000 /* ms — 单次转向超时 */
|
||
|
||
/* 重捕获 */
|
||
#define PARAM_GNAV_REACQUIRE_V 0.05f /* m/s — 重捕获入沟速度 */
|
||
#define PARAM_GNAV_REACQUIRE_CONF 0.6f /* — — 重捕获置信度阈值 */
|
||
#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f /* m — 走廊宽度容差 */
|
||
#define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数 (5×20ms=100ms) */
|
||
#define PARAM_GNAV_REACQUIRE_TIMEOUT 5000 /* ms — 重捕获超时 */
|
||
|
||
/* 沟内 */
|
||
#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.50f /* m — 沟内里程保护上限 */
|
||
|
||
/* 连接段 */
|
||
#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 */
|
||
#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距) */
|
||
#define PARAM_GNAV_LINK_TIMEOUT 8000 /* ms — 连接段超时 */
|
||
|
||
/* 出场 */
|
||
#define PARAM_GNAV_EXIT_V 0.15f /* m/s — 出场速度 */
|
||
#define PARAM_GNAV_EXIT_RUNOUT 1.50f /* m — 出场侧向丢失后冲刺距离 */
|
||
#define PARAM_GNAV_EXIT_MAX_DIST 4.00f /* m — 出场里程保护 */
|
||
#define PARAM_GNAV_EXIT_TIMEOUT 20000 /* ms — 出场超时 */
|
||
|
||
/* 回停 */
|
||
#define PARAM_GNAV_DOCK_V 0.05f /* m/s — 回停速度 */
|
||
#define PARAM_GNAV_DOCK_DISTANCE 0.50f /* m — 回停推进距离 */
|
||
|
||
/* 航向保持 */
|
||
#define PARAM_GNAV_HEADING_KP 0.03f /* — — 航向保持P增益 (输入°，输出rad/s) */
|
||
|
||
/* 编译开关 */
|
||
#define USE_GLOBAL_NAV 1 /* 1=赛道模式 0=单沟测试 */
|
||
```
|
||
|
||
### 9.2 现有参数复用
|
||
|
||
以下现有参数直接复用，无需修改：
|
||
|
||
| 参数 | 用途 |
|
||
|------|------|
|
||
| `PARAM_SAFE_D_FRONT_STOP` | 到端检测距离 (沟内) |
|
||
| `PARAM_SAFE_D_FRONT_APPROACH` | 前向减速预警 |
|
||
| `PARAM_SAFE_CONF_ESTOP` | 沟内 E-STOP 阈值 |
|
||
| `PARAM_CTRL_*` | 沟内控制器增益 |
|
||
| `PARAM_EKF_*` | EKF 滤波参数 |
|
||
| `PARAM_CORRIDOR_WIDTH` | 走廊宽度 |
|
||
|
||
---
|
||
|
||
## 10. 文件清单汇总
|
||
|
||
### 10.1 新增文件
|
||
|
||
| 文件 | 位置 | 职责 | 代码量估计 |
|
||
|------|------|------|-----------|
|
||
| `global_nav.h` | `App/nav/` | 赛道级导航头文件 | ~120 行 |
|
||
| `global_nav.c` | `App/nav/` | 赛道级导航实现 | ~500 行 |
|
||
| `track_map.h` | `App/nav/` | 地图数据结构 | ~60 行 |
|
||
| `track_map.c` | `App/nav/` | 地图查询实现 | ~80 行 |
|
||
|
||
**总计新增: 4 个文件，约 760 行代码**
|
||
|
||
### 10.2 需要修改的文件
|
||
|
||
| 文件 | 改动内容 | 改动量 |
|
||
|------|---------|--------|
|
||
| `App/nav/segment_fsm.h` | `SegFsm_Update()` 签名增加 `SafetyMode_t` 参数 | ~10 行 |
|
||
| `App/nav/segment_fsm.c` | 根据 SafetyMode 分支安全策略 | ~50 行 |
|
||
| `App/preproc/corridor_msgs.h` | 新增 `SafetyMode_t` 枚举定义 | ~10 行 |
|
||
| `App/est/corridor_filter.h` | 确认 `CorridorFilter_Reset()` 接口 | ~5 行 |
|
||
| `App/est/corridor_filter.c` | 实现完整 Reset (含 yaw_ref 重置) | ~15 行 |
|
||
| `App/app_tasks.c` | navTask 流水线替换 + 初始化改造 | ~80 行 |
|
||
| `App/robot_params.h` | 新增 P6 参数组 | ~30 行 |
|
||
| `CMakeLists.txt` | 新增源文件编译 | ~3 行 |
|
||
|
||
**总计修改: 8 个文件，约 200 行改动**
|
||
|
||
### 10.3 不需要改动的文件
|
||
|
||
| 文件 | 说明 |
|
||
|------|------|
|
||
| `App/Can/snc_can_app.c/.h` | 已冻结，勿动 |
|
||
| `App/Contract/robot_blackboard.c/.h` | 接口已足够 |
|
||
| `App/Contract/robot_odom.c/.h` | 接口已足够 |
|
||
| `App/Contract/robot_cmd_slot.c/.h` | 接口已足够 |
|
||
| `App/est/corridor_ekf.c/.h` | 数学核心不变 |
|
||
| `App/preproc/corridor_preproc.c/.h` | 预处理逻辑不变 |
|
||
| `App/nav/corridor_ctrl.c/.h` | 控制律不变 |
|
||
| `App/nav/nav_script.c/.h` | 保留不删，通过编译开关切换 |
|
||
| `App/IMU/hwt101.c/.h` | 驱动不变 |
|
||
| `App/laser/laser_manager.c/.h` | 驱动不变 |
|
||
| `App/VL53L0X_API/**` | 驱动不变 |
|
||
|
||
---
|
||
|
||
## 11. 分步实施计划
|
||
|
||
### Phase 0: 准备工作 (半天)
|
||
|
||
**目标**: 为新模块搭建脚手架，不影响现有功能
|
||
|
||
1. 在 `robot_params.h` 中添加 P6 参数组和 `USE_GLOBAL_NAV` 开关（先设为 0）
|
||
2. 在 `corridor_msgs.h` 中添加 `SafetyMode_t` 枚举
|
||
3. 创建 `track_map.h/.c` 空壳文件
|
||
4. 创建 `global_nav.h/.c` 空壳文件（Init/Start/Stop/Update 都是空实现）
|
||
5. 修改 `CMakeLists.txt` 添加新源文件
|
||
6. 确认编译通过
|
||
|
||
**验证**: `cmake --build build/Debug` 无错误
|
||
|
||
---
|
||
|
||
### Phase 1: 安全层改造 (1天)
|
||
|
||
**目标**: 让安全层支持动作模式，解决 RISK-1
|
||
|
||
1. 修改 `segment_fsm.h`: `SegFsm_Update()` 增加 `SafetyMode_t mode` 参数
|
||
2. 修改 `segment_fsm.c`: 根据 mode 分支安全策略
|
||
3. 修改 `app_tasks.c` 中调用 `SegFsm_Update()` 的地方，旧模式传 `SAFETY_MODE_CORRIDOR`
|
||
|
||
**验证**:
|
||
- `USE_GLOBAL_NAV=0` 编译通过
|
||
- 现有单沟测试行为不变 (回归测试)
|
||
- 手动测试 `SAFETY_MODE_TURN` 模式下 v=0+w≠0 能通过安全层
|
||
|
||
---
|
||
|
||
### Phase 2: 地图模块 (半天)
|
||
|
||
**目标**: 完成 `track_map` 并验证查询逻辑
|
||
|
||
1. 实现 `track_map.c` 的全部查询函数
|
||
2. 在 `global_nav.c` 的 Init 中调用 `TrackMap_Init()`
|
||
|
||
**验证**: 在初始化日志中打印 6 条沟的遍历顺序和转向方向，确认 S 型正确
|
||
|
||
---
|
||
|
||
### Phase 3: 赛道级状态机核心 (2-3天)
|
||
|
||
**目标**: 实现 `global_nav.c` 的完整状态机
|
||
|
||
分步：
|
||
1. 先实现 IDLE → ENTRY_STRAIGHT → TURN_INTO_CORRIDOR → REACQUIRE → CORRIDOR_TRACK 链路
|
||
2. 测试单沟入沟流程
|
||
3. 再实现 CORRIDOR_TRACK → TURN_OUT → LINK_STRAIGHT → TURN_INTO_NEXT → REACQUIRE 链路
|
||
4. 测试双沟 S 型过渡
|
||
5. 最后实现 EXIT_STRAIGHT → DOCK → FINISHED 链路
|
||
6. 测试完整流程
|
||
|
||
**验证**:
|
||
- 用 printf 打印状态转移日志
|
||
- 先在桌面空转 (无传感器) 验证状态机逻辑
|
||
- 再实车单沟测试
|
||
- 再实车双沟测试
|
||
- 最后实车全 6 沟测试
|
||
|
||
---
|
||
|
||
### Phase 4: EKF 重置流程 (半天)
|
||
|
||
**目标**: 确保每次进入新垄沟时 EKF 状态干净
|
||
|
||
1. 确认 `CorridorFilter_Reset()` 完整重置 EKF + yaw_ref
|
||
2. 在 REACQUIRE 进入时调用 Reset
|
||
3. 验证重捕获判据在各种入沟角度下的鲁棒性
|
||
|
||
**验证**: 连续跑 2-3 条沟，观察每条沟入沟后 EKF conf 的收敛曲线
|
||
|
||
---
|
||
|
||
### Phase 5: 流水线集成 (1天)
|
||
|
||
**目标**: `USE_GLOBAL_NAV=1` 时完整流水线打通
|
||
|
||
1. 修改 `app_tasks.c` 的 `AppTasks_RunNavTask_Impl()`
|
||
2. 修改 `AppTasks_Init()` 的初始化流程
|
||
3. 用 `#if USE_GLOBAL_NAV` 保留旧路径
|
||
|
||
**验证**: 全编译 + 实车全流程测试
|
||
|
||
---
|
||
|
||
### Phase 6: 参数调优与鲁棒化 (2-3天)
|
||
|
||
**目标**: 在实车上反复测试并调优
|
||
|
||
重点调优项：
|
||
1. 入场段里程和速度
|
||
2. 转向角速度和容差
|
||
3. 重捕获判据阈值和确认拍数
|
||
4. 连接段里程和超时
|
||
5. 出场检测逻辑
|
||
6. 各阶段超时值
|
||
|
||
---
|
||
|
||
### 总工期估计
|
||
|
||
| 阶段 | 工期 | 累计 |
|
||
|------|------|------|
|
||
| Phase 0: 准备 | 0.5 天 | 0.5 天 |
|
||
| Phase 1: 安全层 | 1 天 | 1.5 天 |
|
||
| Phase 2: 地图 | 0.5 天 | 2 天 |
|
||
| Phase 3: 状态机 | 2-3 天 | 4-5 天 |
|
||
| Phase 4: EKF 重置 | 0.5 天 | 4.5-5.5 天 |
|
||
| Phase 5: 集成 | 1 天 | 5.5-6.5 天 |
|
||
| Phase 6: 调优 | 2-3 天 | 7.5-9.5 天 |
|
||
| **总计** | **~8-10 天** | |
|
||
|
||
---
|
||
|
||
## 12. 风险与降级策略
|
||
|
||
### 12.1 已知风险
|
||
|
||
| 风险 | 严重度 | 说明 | 缓解措施 |
|
||
|------|--------|------|---------|
|
||
| **转向精度不足** | 高 | IMU 漂移导致 90° 转角不准 | 转向后用 VL53 重捕获作为闭环验证；容差设 5° |
|
||
| **连接段偏航** | 高 | IMU 短时漂移 + 轮子打滑导致连接段走歪 | 里程上限保护 + 超时保护 + 侧向VL53早期探测 |
|
||
| **重捕获失败** | 中 | 入沟角度过大导致 VL53 无法同时看到两侧壁 | 重捕获阶段允许单侧先有效，慢速蠕行直到双侧 |
|
||
| **到端检测假阳性** | 中 | 沟内障碍物被误判为端部围栏 | 可加里程下限保护 (已走 > 1.5m 才允许到端) |
|
||
| **出场方向错误** | 低 | 最后一条沟出来后航向偏差过大 | 出场段用 IMU 保持航向 + 里程超限保护 |
|
||
|
||
### 12.2 降级策略
|
||
|
||
| 场景 | 降级方案 |
|
||
|------|---------|
|
||
| 重捕获超时 | → ERROR → 停车 (保得分，不撞) |
|
||
| 连接段超时 | → ERROR → 停车 |
|
||
| 任何阶段 IMU 数据异常 | → ERROR → 停车 |
|
||
| 只能跑 3 条沟 | 修改 `track_map` 的 `is_last` 标志，提前结束 |
|
||
| 来不及实现完整出场 | 最后一条沟结束后直接停车 (放弃回停得分) |
|
||
|
||
### 12.3 不做什么 (明确的非目标)
|
||
|
||
1. **不做 SLAM / 全局定位**: 不需要知道 (x, y) 绝对坐标
|
||
2. **不做路径规划**: S 型路径是硬编码的，不需要搜索
|
||
3. **不做目标物检测/抓取**: 本方案只管导航，不管捡拾
|
||
4. **不重写 EKF**: 现有 EKF 对沟内跟踪已经够用
|
||
5. **不重写驱动层**: 传感器驱动、CAN 协议、里程计全部保留
|
||
6. **不做动态避障**: 沟内假设无动态障碍物
|
||
|
||
---
|
||
|
||
## 附录 A: 完整 S 型遍历动作序列表
|
||
|
||
| # | 状态 | 行为 | v (m/s) | w (rad/s) | 主传感器 | 退出条件 |
|
||
|---|------|------|---------|-----------|---------|---------|
|
||
| 0 | IDLE | 等待 | 0 | 0 | — | Start() |
|
||
| 1 | ENTRY_STRAIGHT | 慢速前进 | 0.08 | PD保直 | IMU | 侧向VL53有效/里程/超时 |
|
||
| 2 | TURN_INTO_CORRIDOR (右转) | 原地转90° | 0 | -1.0 | IMU yaw | 转角≥85° |
|
||
| 3 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效+conf≥0.6 |
|
||
| 4 | CORRIDOR_TRACK (C1→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 5 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 6 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
|
||
| 7 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 8 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
|
||
| 9 | CORRIDOR_TRACK (C2←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 10 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
|
||
| 11 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
|
||
| 12 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
|
||
| 13 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
|
||
| 14 | CORRIDOR_TRACK (C3→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 15 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 16 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
|
||
| 17 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 18 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
|
||
| 19 | CORRIDOR_TRACK (C4←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 20 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
|
||
| 21 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
|
||
| 22 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
|
||
| 23 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
|
||
| 24 | CORRIDOR_TRACK (C5→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 25 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 26 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
|
||
| 27 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 28 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
|
||
| 29 | CORRIDOR_TRACK (C6←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
|
||
| 30 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
|
||
| 31 | EXIT_STRAIGHT | 朝出口直行 | 0.15 | PD保直 | IMU+odom | VL53全丢+里程 |
|
||
| 32 | DOCK | 回停 | 0.05 | 0 | odom | 里程≥0.5m |
|
||
| 33 | FINISHED | 停车 | 0 | 0 | — | 终态 |
|
||
|
||
> **全程共**: 6 次沟内跟踪 + 11 次 90° 转向 + 5 次连接段 + 6 次重捕获 + 1 次入场 + 1 次出场 + 1 次回停 = **33 步动作**
|
||
|
||
---
|
||
|
||
## 附录 B: 关键设计决策记录
|
||
|
||
| 决策 | 选择 | 理由 |
|
||
|------|------|------|
|
||
| 地图用静态表还是动态生成 | **静态表** | 场地固定，运行时不变 |
|
||
| 转向用 90° 还是 180° | **90°** | S 型遍历天然是 90° 转向 |
|
||
| 是否需要全局坐标 (x, y) | **不需要** | 拓扑+里程足够，全局定位反而引入累积误差 |
|
||
| 重捕获判据用 conf 还是 raw VL53 | **两者都用** | conf 依赖 EKF 收敛时间，raw VL53 提供即时几何校验 |
|
||
| EKF 每次入沟是否 Reset | **Reset** | 不同沟的 e_y 参考不同，必须重建 |
|
||
| 航向保持用 PD 还是纯 P | **纯 P** | 连接段短 (<1s)，微分项贡献小，简化设计 |
|
||
| 出场方向如何确定 | **IMU 保持最后转出方向** | 最后一条沟出来后已经朝向出口 |
|
||
| 混合导航模块合几个文件 | **global_nav 1个 + track_map 1个** | 原方案拆 6 个文件过碎，实际耦合度高，合并更清晰 |
|
||
| 是否保留 nav_script | **保留，编译开关切换** | 作为单沟验证脚本仍有价值 |
|
||
|
||
---
|
||
|
||
## 附录 C: 与混合导航方案.md 的差异说明
|
||
|
||
| 混合导航方案.md 建议 | 本方案处理 | 原因 |
|
||
|---------------------|----------|------|
|
||
| 新增 6 个文件 (global_nav_fsm, track_map, lane_transition, reacquire_detector, heading_hold, exit_dock) | **合并为 2 个文件** (global_nav, track_map) | lane_transition / reacquire / heading_hold / exit_dock 的逻辑都嵌入 global_nav 的状态处理中，单独成文件会增加模块间调用复杂度，且每个文件代码量不足 100 行 |
|
||
| 航向不再使用 VL53 差分 | **保留 EKF 中的 VL53 航向观测，但降权** | 完全移除 VL53 航向观测需要改 EKF 数学，风险大；EKF 已有 chi2 拒绝机制，让它自然退化即可 |
|
||
| 赛道级核心状态量列了 10 个变量 | **精简为必要字段** | `stage_progress` 等概念已被 odom_s + elapsed_ms 覆盖 |
|
||
| REACQUIRE_CORRIDOR 作为独立阶段 | **确认保留** | 这是连接沟内闭环与段间动作的关键过渡，必须显式建模 |
|
||
| segment_fsm 区分 4 种动作模式 | **确认采纳，但简化为 SafetyMode_t 枚举** | 不需要传复杂结构体，一个枚举足够 |
|
||
|
||
---
|
||
|
||
> **文档结束**
|
||
> 本方案设计完成后，实施第一步应是 Phase 0（脚手架搭建），确保不破坏现有功能的前提下开始增量开发。
|