Falling_jasmine/ASER-NAV
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

导航部分效果最好的版本，主分支

Browse Source
This commit is contained in:
nitiantuhao
2026-04-13 23:30:22 +08:00
commit 350fd830f4
1679 changed files with 1464081 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

773
Doc/HANDOFF.md Normal file
View File

@@ -0,0 +1,773 @@
# ARES 项目交接文档
> **最后更新**: 2025 年
> **项目性质**: B类"马铃薯捡拾机器人竞技"走廊导航系统
> **本文目标**: 让任何人 (人类或 AI) 能在 30 分钟内理解整个项目并开始工作
---
## 目录
1. [一句话概述](#1-一句话概述)
2. [比赛规则速览](#2-比赛规则速览)
3. [硬件架构](#3-硬件架构)
4. [软件架构总览](#4-软件架构总览)
5. [代码目录结构](#5-代码目录结构)
6. [数据流水线 (核心)](#6-数据流水线-核心)
7. [各模块详解](#7-各模块详解)
8. [FreeRTOS 任务一览](#8-freertos-任务一览)
9. [CAN 通信协议](#9-can-通信协议)
10. [EKF 算法详解](#10-ekf-算法详解)
11. [控制器与安全层](#11-控制器与安全层)
12. [比赛流程状态机](#12-比赛流程状态机)
13. [全部可调参数](#13-全部可调参数)
14. [已知问题与待办](#14-已知问题与待办)
15. [构建与烧录](#15-构建与烧录)
16. [实车调试流程](#16-实车调试流程)
17. [文件快速索引](#17-文件快速索引)
---
## 1. 一句话概述
一个运行在 **STM32H743 (FreeRTOS)** 上的走廊跟踪导航系统——通过 **4 个 VL53L0X 侧向测距** 做横向/航向定位（鲁棒 EKF），**4 个前后激光** 做防撞和到端检测，PD 控制器纠偏，安全状态机兜底，最终通过 **CAN 总线** 将速度指令发给 STM32F407 底盘控制器。
---
## 2. 比赛规则速览
| 项目 | 规格 |
|------|------|
| 场地尺寸 | 390 cm × 300 cm |
| 垄沟（走廊）宽度 | **40 cm**（允许 ±5% 误差） |
| 田垄数量 | 5 条 |
| 垄间模式需遍历 | **6 条垄沟** |
| 机器人尺寸 | **20 cm × 20 cm**（含外壳） |
| 驱动方式 | 四轮差速 |
| 比赛时间 | 5 分钟 |
| 任务 | 在走廊中往返行驶，捡拾目标物 |
**关键约束**: 走廊 40cm - 车体 20cm = 每边仅 **10cm 余量**。1cm 的传感器偏差就是 10% 的误差。
### 2.1 赛道拓扑理解（垄间模式）
本项目当前最相关的正式比赛模式是 **垄间作业模式**。
#### 赛道几何
- 整个比赛场地尺寸为 `390cm × 300cm`
- 场地内部共有 **5 条田垄**
- 每条田垄：长 `220cm`、宽 `30cm`、高 `12cm`
- 围栏与田垄之间、以及相邻田垄之间，均形成宽 `40cm` 的可通行垄沟
- 场地仅有 **1 个出入口**，宽 `40cm`
- 出入口外侧紧邻 **比赛启动区**，尺寸 `40cm × 100cm`
#### 为什么是 6 条垄沟，不是 5 条
虽然场地内只有 **5 条田垄**，但在垄间模式下，机器人实际需要遍历的是 **6 条垄沟**：
1. 最左侧围栏与第 1 条田垄之间的垄沟
2. 第 1 条田垄与第 2 条田垄之间的垄沟
3. 第 2 条田垄与第 3 条田垄之间的垄沟
4. 第 3 条田垄与第 4 条田垄之间的垄沟
5. 第 4 条田垄与第 5 条田垄之间的垄沟
6. 第 5 条田垄与最右侧围栏之间的垄沟
也就是说：
- **田垄数 = 5**
- **垄间可通行垄沟数 = 6**
这是赛道理解里最容易搞错的地方。若只按“5 条走廊”建模，会与正式比赛要求不一致。
#### 垄间模式下的导航任务本质
在垄间模式下，导航任务不是“单条走廊往返”，而是：
1. 从启动区进入场地
2. 进入某一条垄沟
3. 沿垄沟稳定行驶并完成作业
4. 到达端部后完成转向和换沟
5. 依次遍历 **全部 6 条垄沟**
6. 最终从唯一出入口驶离场地
7. 自主停在启动区内
从导航角度看，赛道是一个 **6 段平行窄走廊 + 端部换沟动作 + 单出入口回停** 的组合问题。
#### 对导航系统的直接要求
- **窄走廊居中跟踪**
走廊宽 `40cm`，若车体宽 `20cm`，理论左右余量各仅 `10cm`
- **到端检测**
能识别走到垄沟尽头，避免撞围栏
- **端部转向**
在垄沟末端稳定完成姿态调整
- **换沟**
从当前垄沟切换到相邻垄沟，而不是原地掉头后回到同一条垄沟
- **赛道级状态机**
知道当前是第几条垄沟、下一条目标是哪条、何时结束遍历
- **最终退出与停区**
驶离场地后必须自主停在启动区，否则可能判 0 分
#### 当前项目与赛道要求的关系
当前 ARES 项目已经具备：
- 单条走廊内的相对定位
- 走廊跟踪控制
- 到端检测
- 原地转向
- 安全停车与防撞
但当前脚本状态机更接近：进入一条走廊、沿走廊前进、到端后转向、再走一趟、然后退出。
它更像 **单垄沟闭环验证系统**，还不是完整的 **6 垄沟遍历赛道导航系统**。
后续若要适配正式比赛，重点不是重写 EKF，而是补上：
- 多垄沟遍历状态机
- 端部换沟策略
- 最终出场与启动区停车逻辑
---
## 3. 硬件架构
### 3.1 双 MCU 体系
```
┌─────────────────────────────────┐ CAN Bus ┌─────────────────────────┐
│ STM32H743 (上位机/主控) │ ◄──────────────► │ STM32F407 (底盘控制器) │
│ 480MHz Cortex-M7, DP-FPU │ 0x100 TX │ LADRC 电机控制 │
│ FreeRTOS + 传感器 + 导航算法 │ 0x181/200 RX │ 四路编码器 + 四路电机 │
└─────────────────────────────────┘ └─────────────────────────┘
```
### 3.2 传感器清单
| 传感器 | 数量 | 用途 | 接口 | 量程 |
|--------|------|------|------|------|
| **VL53L0X** ToF | 4 (左前/左后/右前/右后) | 侧向墙壁测距，走廊跟踪 | I2C1 + I2C2 | 2cm ~ 2m |
| **STP-23L** 激光 | 2 (前/后) | 远距离测距，到端检测 | UART2 / UART4 (230400bps) | 7cm ~ 7.5m |
| **ATK-MS53L1M** 激光 | 2 (前/后) | 近距离补盲 (填 STP 的 7cm 盲区) | UART3 / UART6 (115200bps) | ~4m |
| **HWT101** IMU | 1 | 单轴陀螺仪 (航向角速度 wz) | UART7 | ±2000°/s |
### 3.3 VL53L0X 接线
```
左侧 (I2C2): 右侧 (I2C1):
左前 LF: XSHUT=PB1, 地址=0x62 右前 RF: XSHUT=PD13, 地址=0x56
左后 LR: XSHUT=PC5, 地址=0x64 右后 RR: XSHUT=PD14, 地址=0x58
```
### 3.4 其他 GPIO
| 引脚 | 功能 |
|------|------|
| PE3 | CAN 活动 LED (100ms 翻转) |
---
## 4. 软件架构总览
```
┌─────────────────────────────────────────────────────────────────────────────┐
│ FreeRTOS 任务层 │
│ canTxTask monitorTask navTask laserTestTask vl53Task imuTask │
│ (20ms) (100ms) (20ms) (50ms) (100ms) (10ms) │
└──────┬──────────┬──────────┬──────────┬───────────────┬──────────┬─────────┘
│ │ │ │ │ │
│ CmdSlot │ │ Blackboard (全局传感器数据中心) │
│ Pop │ │ taskENTER_CRITICAL 保护 │
▼ ▼ ▼ │
┌─────────┐ ┌────────┐ ┌───────────────────────────────────┐ │
│CAN 0x100│ │Odom │ │ 导航流水线 (navTask, 20ms) │ │
│→ 底盘 │ │积分 │ │ Preproc → EKF → Script │ │
│ │ │→黑板 │ │ → Ctrl → SafetyFSM → CmdSlot │ │
└─────────┘ └────────┘ └───────────────────────────────────┘ │
│
┌───────────────────────────────────────────────────────────┘
│
各传感器驱动：hwt101.c / vl53_board.c / laser_manager.c
→ Blackboard_Update*()
```
---
## 5. 代码目录结构
```
D:\ARES\
├── App/ ← 【所有业务代码在这里】
│ ├── robot_params.h ← ★ 全局参数配置中心 (唯一的调参入口)
│ ├── app_tasks.c/.h ← 所有 FreeRTOS 任务实现 + 系统初始化
│ ├── retarget.c/.h ← printf → USB CDC 重定向
│ │
│ ├── Can/
│ │ └── snc_can_app.c/.h ← CAN 协议层 (编解码/CRC8/滤波器)【已冻结，勿动】
│ │
│ ├── Contract/ ← 模块间数据契约
│ │ ├── robot_blackboard.c/.h ← 全局传感器数据黑板 (线程安全)
│ │ ├── robot_odom.c/.h ← 差速里程计积分
│ │ ├── robot_cmd_slot.c/.h ← 导航→CAN 指令传递槽
│ │ └── chassis_can_msg.h ← CAN 帧结构体定义
│ │
│ ├── est/ ← 状态估计
│ │ ├── corridor_ekf.c/.h ← ★ 鲁棒 EKF 核心 (3 状态)
│ │ └── corridor_filter.c/.h ← EKF 的兼容包装层
│ │
│ ├── nav/ ← 导航与控制
│ │ ├── corridor_ctrl.c/.h ← PD 走廊控制器
│ │ ├── segment_fsm.c/.h ← 安全状态机 (防撞/急停)
│ │ └── nav_script.c/.h ← 比赛流程编排
│ │
│ ├── preproc/ ← 传感器预处理
│ │ ├── corridor_preproc.c/.h ← 数据清洗 + 互补融合
│ │ └── corridor_msgs.h ← 所有模块间消息结构体
│ │
│ ├── IMU/
│ │ └── hwt101.c/.h ← HWT101 IMU 驱动 (UART DMA)
│ │
│ ├── laser/
│ │ └── laser_manager.c/.h ← 前后激光驱动 (STP+ATK, UART DMA)
│ │
│ └── VL53L0X_API/
│ ├── platform/
│ │ ├── vl53_board.c/.h ← 多传感器管理 + 卡尔曼滤波
│ │ └── vl53l0x_platform.c/.h ← ST API HAL 适配
│ └── core/ ← ST 官方 VL53L0X API (不修改)
│
├── Core/ ← CubeMX 生成的 HAL 初始化代码
├── Drivers/ ← STM32H7 HAL 驱动
├── Middlewares/ ← FreeRTOS + USB
├── USB_DEVICE/ ← CDC 虚拟串口
├── ARES.ioc ← CubeMX 工程文件
├── CMakeLists.txt ← 构建入口
└── STM32H743XX_FLASH.ld ← 链接脚本
```
---
## 6. 数据流水线 (核心)
这是整个系统的心脏，运行在 `navTask` 中，**每 20ms 执行一次**：
```
Step 1: Blackboard_GetSnapshot(&board) 拍摄传感器快照 (无撕裂)
│
Step 2: CorridorPreproc_ExtractObs() 数据清洗
│ • VL53L0X: mm→m, 范围校验 (0.02~2.0m)
│ • STP+ATK: 互补融合 (ATK 填 STP 的 7cm 盲区)
│ • 前后激光偏移补偿 (d -= PARAM_FRONT_LASER_OFFSET)
│ → 输出: CorridorObs_t {d_lf, d_lr, d_rf, d_rr, d_front, d_back, valid_mask}
│
Step 3: CorridorFilter_Update() EKF 状态估计
│ → Predict(odom_vx, imu_wz, dt) 预测步 (IMU wz + 里程计)
│ → Update(obs) 观测步 (VL53 侧向测距)
│ → UpdateIMUYaw(yaw_cont, ref) IMU 航向观测 (独立 1DOF 更新)
│ → 输出: CorridorState_t {e_y, e_th, s, conf}
│
Step 4: NavScript_Update() 比赛流程编排
│ → 决定当前阶段: 入口对准/走廊跟踪/转向/退出
│ → 180° 转弯判定使用 IMU 连续 yaw (非 EKF e_th)
│ → 输出: NavScriptOutput_t {use_override, override_v/w, request_corridor}
│
Step 5: CorridorCtrl_Compute() PD 控制律 (仅在走廊跟踪模式)
│ → w = kp_theta·e_th + kd_theta·(-wz) + kp_y·e_y
│ → v = v_cruise × (1 - 0.4 × |w/w_max|)
│ → 输出: RawCmd_t {v, w}
│
Step 6: SegFsm_Update() 安全仲裁
│ → 前方太近? → 减速/停车
│ → 置信度太低? → 紧急停车
│ → 输出: SegFsmOutput_t {safe_v, safe_w}
│
Step 7: CmdSlot_Push(safe_v, safe_w) 推入指令槽 → canTxTask 取走
```
---
## 7. 各模块详解
### 7.1 传感器驱动层
#### VL53L0X 侧向测距 (`vl53_board.c`)
- 上电时所有 XSHUT 拉低，逐个拉高分配 I2C 地址
- 测距预算 100ms (100000μs)，连续测距模式
- 每个传感器读数经过独立的 **1D 卡尔曼滤波** (Q=10.0, R=14.1)
- 输出 `Vl53BoardSnapshot_t` → `Blackboard_UpdateVl53()`
#### 前后激光测距 (`laser_manager.c`)
- DMA 环形缓冲区轮询，内部自有 FreeRTOS 任务 `LaserTsk` (10ms)
- **STP-23L**: 二进制协议，含校验和；3 帧中值滤波去尖刺
- **ATK-MS53L1M**: ASCII 文本 `"d: 1234\r\n"`；3 帧中值滤波
- 互补融合策略:
- ATK 距离 < 8cm 时无条件信任 ATK (STP 盲区)
- STP 卡在 7cm 时用 ATK
- 两者都活着取更近的 (保守防撞)
- STP 独活但在盲区 → 报失效 (宁可急停也不信假数据)
#### IMU (`hwt101.c`)
- UART DMA 11 字节二进制帧
- 输出: `yaw` (°)、`yaw_continuous` (°) 和 `wz` (°/s)
- `yaw`: 原始偏航角，范围 [-180°, +180°)，跨界时跳变
- `yaw_continuous`: **unwrap 后的连续偏航角**，消除 ±180° 跳变，可直接做差计算任意转角
- 注意: **输出单位是°/s，不是 rad/s**
- `HWT101_ZeroYaw()` 会同时重置 unwrap 状态
### 7.2 数据契约层
#### 黑板 (`robot_blackboard.c`)
- 全局结构体 `g_blackboard`，所有传感器数据汇聚于此
- 所有读写都在 `taskENTER_CRITICAL()` 临界区内
- 消费者调用 `Blackboard_GetSnapshot()` 获取无撕裂快照
- IMU 字段: `imu_yaw` (原始°)、`imu_yaw_continuous` (连续°)、`imu_wz` (°/s)
#### 里程计 (`robot_odom.c`)
- 从 CAN 0x200 获取四轮编码器增量，经 **ISR 侧累加器** 汇聚后由 monitorTask 一次性取走积分
- 消费链路（BUG-8 修复后）:
```
CAN Rx ISR → snc_parse_odom_delta() 每帧累加到 odom_accum（不覆盖）
↓
monitorTask → SNC_CAN_ConsumeOdomDelta() 原子取走累计值并清零
↓
Odom_Update() 仅在有新帧时调用（杜绝重复积分）
```
- 差速运动学:
```
v_left = (fl_delta + rl_delta) / 2 × (π × 0.060 / 500) / dt
v_right = (fr_delta + rr_delta) / 2 × (π × 0.060 / 500) / dt
vx = (v_left + v_right) / 2
wz = (v_right - v_left) / 0.140
```
- 3 帧连续零增量 → 强制 vx=wz=0 (静止检测)
#### 指令槽 (`robot_cmd_slot.c`)
- 单槽无锁设计，`CmdSlot_Push()` 写入，`CmdSlot_Pop()` 读取
- 超过 100ms 未更新 → canTxTask 自动发零速指令 (看门狗)
### 7.3 估计层
详见 [第10节 EKF 算法详解](#10-ekf-算法详解)。
### 7.4 控制层
详见 [第11节 控制器与安全层](#11-控制器与安全层)。
### 7.5 导航层
详见 [第12节 比赛流程状态机](#12-比赛流程状态机)。
---
## 8. FreeRTOS 任务一览
| 任务名 | 周期 | 优先级 | 栈大小 | 职责 |
|--------|------|--------|--------|------|
| `canTxTask` | 20ms | AboveNormal (28) | 2048B | CAN 0x100 硬实时发送；指令槽看门狗 |
| `navTask` | 20ms | AboveNormal (28) | 4096B | 完整导航流水线 (7步) |
| `LaserTsk` (内部) | 10ms | AboveNormal (28) | 4096B | 4路 UART DMA 解析 |
| `monitorTask` | 100ms | Normal (24) | 4096B | CAN 健康监控；里程计更新 |
| `laserTestTask` | 50ms | Normal (24) | 16384B | 激光轮询 + 推送黑板 |
| `vl53Task` | 100ms | Normal (24) | 4096B | VL53L0X 读取 + 推送黑板 |
| `imuTask` | 10ms | BelowNormal (20) | 2048B | IMU 解析 + 推送黑板 |
| `defaultTask` | — | Normal (24) | 512B | USB CDC 初始化；空闲循环 |
**全局配置**: Tick 频率 1000Hz, 堆 65536 字节, 最大优先级 56
---
## 9. CAN 通信协议
### 9.1 TX: H743 → F407
#### 0x100 — 速度指令 (每 20ms 发送)
| 字节 | 字段 | 类型 | 编码 |
|------|------|------|------|
| 0-1 | vx | int16 LE | vx (m/s) × 1000 |
| 2-3 | wz | int16 LE | wz (rad/s) × 1000 |
| 4 | ctrl_flags | uint8 | 控制标志位 |
| 5 | reserved | uint8 | 固定 0 |
| 6 | rolling_counter | uint8 | 单调递增 |
| 7 | crc8 | uint8 | CRC8-SAE-J1850 (bytes[0..6]) |
**硬性约束**: 必须每 ≤150ms 发一次，否则底盘进入 `SAFE_FAULT`。即使停车也要发 vx=0, wz=0。
#### 0x080 — 心跳 (每 20ms, DLC=0)
### 9.2 RX: F407 → H743
#### 0x181 — 底盘状态 (20ms)
| 字节 | 字段 |
|------|------|
| 0 | system_state: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT |
| 1 | system_health: 0=OK, 1=WARNING, 2=FAULT |
| 2-5 | diag_bits (uint32 LE): 通信超时/CAN BUS OFF/电机堵转等 |
| 6 | cmd_age_10ms: 上次有效指令距今 (×10ms) |
| 7 | status_counter |
#### 0x200 — 里程计增量 (~60ms)
| 字节 | 字段 |
|------|------|
| 0-1 | fl_delta_ticks (int16 LE) — 左前轮编码器增量 |
| 2-3 | rl_delta_ticks (int16 LE) — 左后轮 |
| 4-5 | fr_delta_ticks (int16 LE) — 右前轮 |
| 6-7 | rr_delta_ticks (int16 LE) — 右后轮 |
#### 0x184 — 通信诊断 (100ms)
CRC 错误计数、计数器拒绝计数、CAN 丢包等统计信息。
---
## 10. EKF 算法详解
### 状态向量
```
x = [e_y, e_th, s]ᵀ
```
| 状态 | 含义 | 单位 |
|------|------|------|
| `e_y` | 车体中心相对走廊中心线的横向偏差 | m (左偏为正) |
| `e_th` | 车头相对走廊方向的航向偏差 | rad (左偏为正) |
| `s` | 沿走廊行驶里程 | m |
### 预测步 (每 20ms)
```
输入: odom_vx (m/s), imu_wz (rad/s), dt (s)
e_y_new = e_y + vx × sin(e_th) × dt
e_th_new = e_th + wz × dt
s_new = s + vx × cos(e_th) × dt
雅可比矩阵 F:
[1, vx·cos(e_th)·dt, 0]
[0, 1, 0]
[0, -vx·sin(e_th)·dt, 1]
P_pred = F·P·Fᵀ + Q
```
### 观测步
```
d_center = (W - Rw) / 2 + inset
= (0.40 - 0.20) / 2 + 0 = 0.10m (车居中时传感器到墙的距离)
左侧观测:
z_ey = d_center - (d_lf + d_lr)/2 横向偏差
z_eth_L = atan2(d_lr - d_lf, L_s) 航向偏差
右侧观测:
z_ey = (d_rf + d_rr)/2 - d_center 横向偏差
z_eth_R = atan2(d_rf - d_rr, L_s) 航向偏差
双侧有效时: z_ey 取两侧平均 = (d_right - d_left) / 2
→ d_center 被消掉，结果只取决于左右差值
单侧退化时: z_ey = d_center - d_one_side
→ d_center 的准确性至关重要！
IMU 航向观测 (侧墙更新之后独立执行):
z_eth_imu = (imu_yaw_continuous - yaw_ref) × DEG2RAD
yaw_ref 在 EKF 置信度 ≥ 0.5 时首次锁定
使用 1DOF 标量 EKF 更新，R 值 (PARAM_EKF_R_ETH_IMU) 远大于侧墙 R
→ 侧墙有效时 IMU 影响小；侧墙丢失时 IMU 提供航向约束
```
### 鲁棒拒绝
每个观测独立做 **χ² 马氏距离检验**:
- 1 自由度门限 3.84 (95% 置信度)
- 超过门限的观测被标记为异常并跳过更新
### 置信度计算
```
conf = f(协方差迹) × 侧面因子 × (1 - 拒绝比例 × 0.5)
侧面因子: 双侧=1.0, 单侧=0.7
两侧全失效: 协方差膨胀，conf 趋向 0 → 触发 E-STOP
```
---
## 11. 控制器与安全层
### 11.1 PD 走廊控制器
```
w_cmd = kp_theta × e_th + kd_theta × (-imu_wz) + kp_y × e_y
─────────────── ───────────────────── ────────────
航向比例纠偏 航向微分阻尼(用IMU) 横向比例纠偏
w_cmd = clamp(w_cmd, ±1.5 rad/s)
v_cmd = v_cruise × (1 - 0.4 × |w/w_max|) 弯道减速
v_cmd = clamp(v_cmd, 0, 0.3 m/s)
置信度保护:
conf < 0.3 → v × 0.3 (三折)
conf < 0.6 → v × 0.7 (七折)
```
**参数**: kp_theta=2.0, kd_theta=0.1, kp_y=3.0, v_cruise=0.15m/s
### 11.2 安全状态机 (`segment_fsm.c`)
```
conf < 0.1
┌──────────────────┐
▼ │
┌──────┐ Start ┌──────────┐ │ d_front ≤ 0.25m ┌──────────┐
│ IDLE ├────────►│ CORRIDOR ├──┴──────────────────►│ APPROACH │
└──────┘ └────┬─────┘ ◄────────────────── └────┬─────┘
│ d_front > 0.25m │
│ d_front ≤ 0.08m
conf ≥ 0.5 │
┌────┴─────┐ ┌──────▼─────┐
│ E-STOP │ │ STOP │
│ v=0,w=0 │ │ v=0,w=0 │
└──────────┘ └────────────┘
```
| 状态 | 行为 |
|------|------|
| **CORRIDOR** | 放行控制器输出 |
| **APPROACH** | 线性减速: v 从 raw_v 衰减至 0.05m/s (d 从 25cm→8cm) |
| **STOP** | 强制零速 |
| **E-STOP** | 强制零速；conf ≥ 0.5 时自动恢复 |
---
## 12. 比赛流程状态机
> 注意：以下状态机描述的是 **当前固件已实现的单垄沟往返流程**，用于验证走廊跟踪、到端检测、转向与退出链路。
> 它 **不等价于** 正式比赛垄间模式要求的“遍历全部 6 条垄沟”的完整赛道级导航状态机。
```
IDLE → ENTRY_ALIGN → CORRIDOR_FORWARD → TURN_AT_END → CORRIDOR_BACKWARD
↓
(pass_count ≥ 2)
↓
EXIT → FINISHED
```
| 阶段 | 触发条件 | 行为 | 退出条件 |
|------|----------|------|----------|
| **IDLE** | 初始状态 | 零速 | `NavScript_Start()` |
| **ENTRY_ALIGN** | Start() | 慢速前进 0.08m/s，等侧向雷达找到墙 | 双侧有效 + conf≥0.8；或超时 30s |
| **CORRIDOR_FWD** | 入口对准完成 | 走廊控制器跟踪 | d_front ≤ 0.10m (到端) |
| **TURN_AT_END** | 到端 | 原地转 180° (1.0 rad/s, 接近时减速)；使用 IMU 连续 yaw 判定转角 | 转角 ≥ π-0.1 rad |
| **CORRIDOR_BWD** | 第1次转完 | 走廊控制器跟踪 | d_back ≤ 0.10m (到端) |
| **EXIT** | 第2次转完 | 0.5m/s 直线冲出 | 双侧 VL53 全丢 + 行驶 ≥ 2m |
| **FINISHED** | 冲出完成 | 零速停车 | 终态 |
---
## 13. 全部可调参数
所有参数集中在 **`App/robot_params.h`** 中，修改后需重新编译烧录。
### P0 — 几何参数 (必须实测)
| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_ROBOT_WIDTH` | **0.200** | m | 车体外轮廓宽度 |
| `PARAM_ROBOT_LENGTH` | **0.200** | m | 车体外轮廓长度 |
| `PARAM_WHEEL_DIAMETER` | **0.060** | m | 驱动轮外径 |
| `PARAM_WHEEL_TRACK` | **0.140** | m | 左右轮中心距 |
| `PARAM_SENSOR_BASE_LENGTH` | **0.120** | m | 同侧前后 VL53 间距 |
| `PARAM_CORRIDOR_WIDTH` | **0.40** | m | 走廊宽度 |
| `PARAM_FRONT_LASER_OFFSET` | **0.0** | m | 前激光到车头前缘的内缩距离 |
| `PARAM_REAR_LASER_OFFSET` | **0.0** | m | 后激光到车尾后缘的内缩距离 |
| `PARAM_VL53_SIDE_INSET` | **0.0** | m | 侧向 VL53 到车体外壳的内缩距离 |
| `PARAM_ENCODER_CPR` | **500** | — | 编码器每转脉冲数 |
### P2 — EKF 滤波器
| 参数名 | 当前值 | 说明 |
|--------|--------|------|
| `PARAM_EKF_Q_EY` | **0.01** | 横向过程噪声 (越大越信观测) |
| `PARAM_EKF_Q_ETH` | **0.001** | 航向过程噪声 |
| `PARAM_EKF_Q_S` | **0.1** | 里程过程噪声 |
| `PARAM_EKF_R_EY` | **0.002** | 横向观测噪声 (越大越不信雷达) |
| `PARAM_EKF_R_ETH` | **0.001** | 航向观测噪声 (侧墙) |
| `PARAM_EKF_R_ETH_IMU` | **0.01** | 航向观测噪声 (IMU yaw)，远大于侧墙，用于长时约束 |
| `PARAM_EKF_P0_EY` | **0.1** | e_y 初始不确定度 |
| `PARAM_EKF_P0_ETH` | **0.1** | e_th 初始不确定度 |
### P3 — 控制器
| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_CTRL_KP_THETA` | **2.0** | — | 航向比例增益 |
| `PARAM_CTRL_KD_THETA` | **0.1** | — | 航向微分增益 |
| `PARAM_CTRL_KP_Y` | **3.0** | — | 横向比例增益 |
| `PARAM_CTRL_V_CRUISE` | **0.15** | m/s | 巡航速度 |
| `PARAM_CTRL_W_MAX` | **1.5** | rad/s | 最大角速度 |
| `PARAM_CTRL_V_MAX` | **0.3** | m/s | 最大线速度 |
### P4 — 安全与脚本
| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_SAFE_D_FRONT_STOP` | **0.08** | m | 前向强制停车距离 |
| `PARAM_SAFE_D_FRONT_APPROACH` | **0.25** | m | 前向减速预警距离 |
| `PARAM_SAFE_APPROACH_MIN_V` | **0.05** | m/s | 减速区最低速度 |
| `PARAM_SAFE_CONF_ESTOP` | **0.10** | — | E-Stop 置信度阈值 |
| `PARAM_SCRIPT_ENTRY_TIMEOUT` | **30000** | ms | 入口对准超时 |
| `PARAM_SCRIPT_TURN_OMEGA` | **1.0** | rad/s | 转向角速度 |
| `PARAM_SCRIPT_EXIT_RUNOUT` | **2.0** | m | 退出场地后冲距离 |
### P5 — 传感器驱动
| 参数名 | 当前值 | 说明 |
|--------|--------|------|
| `PARAM_VL53_TIMING_BUDGET` | **100000** | VL53L0X 测距预算 (μs) |
| `PARAM_VL53_KALMAN_Q` | **10.0** | VL53 卡尔曼过程噪声 |
| `PARAM_VL53_KALMAN_R` | **14.1** | VL53 卡尔曼观测噪声 |
| `PARAM_IMU_YAW_OFFSET` | **0.0** | IMU 零位偏置 (rad) |
---
## 14. 已知问题与待办
### 严重 (已全部修复 ✅)
| # | 问题 | 状态 | 修复位置 |
|---|------|------|----------|
| **BUG-1** | IMU 输出 `wz` 单位是 **°/s**，但 EKF 预测步按 **rad/s** 使用，导致航向积分放大 57.3 倍 | ✅ 已修复 | `app_tasks.c:305` — 加了 `PARAM_DEG2RAD()` 转换 |
| **BUG-2** | `corridor_filter.c` 中 EKF 的 Q/R/P0 参数全部硬编码，修改 `robot_params.h` 中的 `PARAM_EKF_*` 不生效 | ✅ 已修复 | `corridor_filter.c:40-56` — 改为读取 `PARAM_EKF_*` 宏 |
| **BUG-3** | `AppTasks_Init()` 只调用 `SegFsm_Init()`，未调用 `SegFsm_Start()`，导致安全状态机长期处于 IDLE，输出始终为零速 | ✅ 已修复 | `app_tasks.c:382` — 补加 `SegFsm_Start()` |
| **BUG-4** | `CORRIDOR_BACKWARD` 段转 180° 后正向行驶，但到端检测错误使用 `d_back`（后向雷达），应为 `d_front` | ✅ 已修复 | `nav_script.c:CORRIDOR_BACKWARD` — 改为检查 `d_front` |
| **BUG-5** | `ENTRY_ALIGN` 段超时保护硬编码了 `30000`，`entry_align_timeout` 参数实际不生效 | ✅ 已修复 | `nav_script.c:155` — 改为 `s_cfg.entry_align_timeout` |
| **BUG-6** | `EXIT` 段退出直线速度误用角速度参数 `turn_omega * 0.5f`，调转向速度会意外影响退出速度 | ✅ 已修复 | `nav_script.h` 加 `exit_v` 字段；`nav_script.c:262` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_V`；`robot_params.h` 加宏 |
| **BUG-7** | EKF 卡尔曼增益计算 `K = P·H^T·S⁻¹` 只乘 `S_inv` 对角项，忽略观测间相关性，增益不正确 | ✅ 已修复 | `corridor_ekf.c:576-607` — 改为完整两步矩阵乘法：先算 `PHT = P·H^T`，再算 `K = PHT·S_inv` |
| **BUG-8** | `0x200 Odom Delta` 按"状态型最新值"消费，实为"事件型增量数据"：ISR 直接覆盖旧帧导致**漏积分**（60ms帧率 vs 100ms消费，必然丢帧）；monitorTask 无条件调用 `Odom_Update()` 导致**重复积分**（无新帧时同一份 delta 被积分两次） | ✅ 已修复 | `snc_can_app.h` 新增 `SNC_OdomDeltaAccum_t`；`snc_can_app.c` ISR 改为累加并实现 `SNC_CAN_ConsumeOdomDelta()` 原子取走；`app_tasks.c:monitorTask` 改用新 API，仅在有新帧时调用积分 |
| **BUG-9** | IMU `yaw` 输出范围 [-180°, +180°)，跨界时跳变，且项目完全未使用 `yaw`；180° 转弯判定纯依赖 EKF `e_th` (无侧墙观测时靠 wz 积分漂移) | ✅ 已修复 | `hwt101.c` 新增 yaw unwrap → `yaw_continuous`；`robot_blackboard.h` 新增 `imu_yaw_continuous`；`nav_script.c` 转弯判定改用 IMU 连续 yaw 差值；`corridor_ekf.c` 新增 `CorridorEKF_UpdateIMUYaw()` 1DOF 标量观测更新；`corridor_filter.c` 管理 yaw_ref 参考值并在侧墙更新后注入 IMU 航向观测 |
### 需要实车标定
| # | 问题 | 说明 |
|---|------|------|
| **CAL-1** | `PARAM_FRONT_LASER_OFFSET = 0.0` | 如果传感器在车体内部，停车距离会比预期偏大 |
| **CAL-2** | `PARAM_REAR_LASER_OFFSET = 0.0` | 同上 |
| **CAL-3** | `PARAM_VL53_SIDE_INSET = 0.0` | 如果传感器内缩，单侧退化时会有系统偏差 |
| **CAL-4** | `PARAM_IMU_YAW_OFFSET = 0.0` | 声明了但代码中未使用 |
### 设计风险 / 尚未处理
| # | 问题 | 当前影响 | 建议方向 |
|---|------|----------|----------|
| **RISK-1** | `TURN_AT_END` 原地转向阶段与 `SegFsm` 前向防撞逻辑存在潜在冲突 | `nav_script` 在到端后会输出 `v=0, w!=0` 的原地转向命令；但 `segment_fsm` 仍按“前向距离过近 → STOP”处理，可能把转向角速度也清零，导致机器人到端后想转却被安全层按住 | 后续应为安全层引入“动作语义”或“模式感知”，区分走廊前进与原地转向；转向阶段允许 `v=0` 时保留受限 `w`，而不是沿用普通前向防撞全停策略 |
补充说明：
- 该问题的根因不是传感器噪声，而是 **脚本层与安全层的仲裁语义不一致**
- 侧向 VL53 在端部转向时本来也会偏离正常走廊几何，因此当前已经用 IMU `yaw_continuous` 来判定转角，这部分方向是对的
- 真正需要补的是：**转向阶段的专用安全策略**，否则后续扩展到多垄沟遍历时，端部动作会成为卡点
### 代码质量 (已全部修复 ✅)
| # | 问题 | 状态 | 修复位置 |
|---|------|------|----------|
| **Q-1** | `PARAM_CTRL_SPEED_REDUCTION` 未被控制器读取，硬编码了 `0.4f` | ✅ 已修复 | `corridor_ctrl.h` 加字段 `speed_reduction_k`；`corridor_ctrl.c:59` 读取它；`app_tasks.c` 传入 `PARAM_CTRL_SPEED_REDUCTION` |
| **Q-2** | `PARAM_SCRIPT_ENTRY_V` 未被脚本读取，硬编码了 `0.08f` | ✅ 已修复 | `nav_script.h` 加字段 `entry_align_v`；`nav_script.c:146` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_ENTRY_V` |
| **Q-3** | `PARAM_SCRIPT_EXIT_RUNOUT` 未被脚本读取，硬编码了 `2.0f` | ✅ 已修复 | `nav_script.h` 加字段 `exit_runout_m`；`nav_script.c:275` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_RUNOUT` |
| **Q-4** | `PARAM_VL53_TIMING_BUDGET` 未被使用，直接传字面量 `100000` | ✅ 已修复 | `app_tasks.c:222,232` 改为传 `PARAM_VL53_TIMING_BUDGET` |
| **Q-5** | `exit_start_s` 是函数内 `static` 局部变量，`NavScript_Reset()` 无法清除它 | ✅ 已修复 | 移入 `s_internal` 结构体，`memset(&s_internal, 0, ...)` 统一清零 |
| **Q-6** | 转向角度通过 EKF 的 `e_th` 差值测量，不是绝对角度。如果 EKF 发散或走廊参照丢失，转向判断可能不准 | ✅ 已修复 | `nav_script.c` — 改用 IMU `yaw_continuous` (unwrap 后的连续偏航角) 判定转角，不再依赖 EKF `e_th` |
| **Q-7** | `g_snc_can_app` 被中断写、被任务读，没有互斥保护 (中等风险) | ✅ 部分修复 | `odom_accum` 的取走/清零已通过 `taskENTER_CRITICAL` 保护（BUG-8 修复）；其余字段（status/rpm/diag）仍无保护，但读写均为原子宽度或单次赋值，风险可接受 |
---
## 15. 构建与烧录
### 构建
```bash
# 确保 gcc-arm-none-eabi 在 PATH 中
cmake --preset Debug
cmake --build build/Debug
```
- 输出: `build/Debug/ARES.elf`
- 工具链: `cmake/gcc-arm-none-eabi.cmake`
- C 标准: C11
- 浮点 printf: 已开启 (`-u _printf_float`)
### 烧录
使用 **STM32CubeProgrammer** 或 **OpenOCD** 烧录 `.elf` 文件到 STM32H743。
### 注意事项
- `CMakeLists.txt` 第 38 行有一个大写 `.C` 后缀 (`laser_manager.C`)，在大小写敏感的文件系统上可能会构建失败
- MPU Region 1 配置了 `0x30000000` 32KB 为非缓存区，所有 DMA 缓冲区必须放在此区域 (`.dma_buffer` section)
- USB CDC 用于 `printf` 输出 (通过 `retarget.c`)
---
## 16. 实车调试流程
### 建议顺序
```
P0: 几何参数 → 用卷尺实测，填入 robot_params.h
↓
P1: 里程计标定 → 直线跑 10m，对比编码器累积距离
↓
P2: EKF 调优 → 从保守 Q/R 开始，观察走廊跟踪稳定性
↓
P3: 控制器调参 → 先调 kp_theta/kd_theta (航向)，再调 kp_y (横向)
↓
P4: 安全阈值 → 根据实际场地微调停车/减速距离
```
### 常见问题诊断
| 现象 | 可能原因 | 调整方法 |
|------|----------|----------|
| 车头左右摆动 | kp_theta 过大 或 kd_theta 过小 | 减小 kp_theta 或增大 kd_theta |
| 横向纠偏太慢 | kp_y 过小 | 增大 kp_y |
| 到端刹不住 | d_front_stop 过小 或 approach 区间太短 | 增大 PARAM_SAFE_D_FRONT_STOP |
| 总是急停 | EKF 置信度低，可能是 VL53 数据不稳定 | 增大 R (降低观测信任) 或检查传感器接线 |
| EKF 发散 | Q 过小 (不信观测) 或 R 过小 (过信噪声数据) | 增大 Q 或增大 R |
| 单侧靠墙走 | PARAM_VL53_SIDE_INSET 未校准 | 实测传感器内缩距离并填入 |
---
## 17. 文件快速索引
| 你想做什么 | 去看哪个文件 |
|-----------|-------------|
| 改任何调参数值 | `App/robot_params.h` |
| 理解系统初始化 | `App/app_tasks.c` → `AppTasks_Init()` |
| 理解导航流水线 | `App/app_tasks.c` → `AppTasks_RunNavTask_Impl()` |
| 看 EKF 数学 | `App/est/corridor_ekf.c` |
| 看控制律 | `App/nav/corridor_ctrl.c` |
| 看安全逻辑 | `App/nav/segment_fsm.c` |
| 看比赛编排 | `App/nav/nav_script.c` |
| 看传感器预处理 | `App/preproc/corridor_preproc.c` |
| 看 CAN 协议 | `App/Can/snc_can_app.c` + `App/Contract/chassis_can_msg.h` |
| 看全局数据结构 | `App/Contract/robot_blackboard.c/.h` |
| 看里程计 | `App/Contract/robot_odom.c` |
| 看激光驱动 | `App/laser/laser_manager.c` |
| 看 VL53 驱动 | `App/VL53L0X_API/platform/vl53_board.c` |
| 看 IMU 驱动 | `App/IMU/hwt101.c` |
| 看消息结构定义 | `App/preproc/corridor_msgs.h` |
---
> **给接手者的最后提醒**:
> 1. **先修 BUG-1** (IMU 单位转换)，否则 EKF 完全不可用
> 2. **再修 BUG-2** (EKF 参数硬编码)，否则调参改了白改
> 3. 拿到实车后第一件事：用卷尺量 CAL-1 ~ CAL-3 的三个偏移量
> 4. CAN 协议层 (`snc_can_app.c`) 已冻结，**不要修改**
> 5. IMU `yaw_continuous` 已接入转弯判定，实车调试时对比 `wz` 积分与 `yaw` 哪个更稳，参见 `IMU_YAW_WZ_ISSUE.md`

817
Doc/HANDOFF_v2.md Normal file
View File

@@ -0,0 +1,817 @@
# ARES 项目交接文档 v2
> **版本**: v2.1 — 连接段三信号联合判定
> **基于**: v2.0 + 赛道几何理解修正 + 连接段判定算法升级
> **更新内容**: 修正地图几何理解、修复入场/连接段传感器误用 bug、升级连接段为三信号联合判定
> **本文目标**: 完整描述当前代码库状态，让任何人能在 30 分钟内理解并继续工作
---
## 目录
1. [一句话概述](#1-一句话概述)
2. [与 v1.0 的核心差异](#2-与-v10-的核心差异)
3. [代码目录结构（当前）](#3-代码目录结构当前)
4. [软件架构总览](#4-软件架构总览)
5. [数据流水线（当前）](#5-数据流水线当前)
6. [赛道级导航状态机详解](#6-赛道级导航状态机详解)
7. [安全层改造详解](#7-安全层改造详解)
8. [地图模块详解](#8-地图模块详解)
9. [EKF 重置机制](#9-ekf-重置机制)
10. [编译开关与模式切换](#10-编译开关与模式切换)
11. [全部可调参数](#11-全部可调参数)
12. [FreeRTOS 任务一览](#12-freertos-任务一览)
13. [文件快速索引](#13-文件快速索引)
14. [已知问题与待办](#14-已知问题与待办)
15. [实车调试建议](#15-实车调试建议)
---
## 1. 一句话概述
在 v1.0 的单垄沟闭环基础上，新增了**赛道级混合导航系统**：用固定地图描述 S 型遍历拓扑，用 11 状态的 `GlobalNav` 状态机管理 6 条垄沟的完整遍历，用改造后的安全层支持"动作语义感知"，最终实现从启动区入场 → 遍历全部 6 条垄沟 → 出场回停的完整比赛流程。
---
## 2. 与 v1.0 的核心差异
### 2.1 新增能力
| 能力 | v1.0 | v2.0 |
|------|------|------|
| 单垄沟往返 | ✅ | ✅ |
| 到端检测 | ✅ | ✅ |
| 6 条垄沟 S 型遍历 | ❌ | ✅ |
| 90° 转向（入/出沟） | ❌ | ✅ |
| 连接段三信号联合判定 | ❌ | ✅ |
| 入沟重捕获确认 | ❌ | ✅ |
| 赛道级状态机 | ❌ | ✅ |
| 安全层动作语义感知 | ❌ | ✅ |
| 每次入沟 EKF 重置 | ❌ | ✅ |
| 出场与回停启动区 | ❌ | ✅ |
### 2.2 已解决的已知问题
| 编号 | 问题 | 解决方式 |
|------|------|---------|
| **RISK-1** | 转向阶段安全层卡死 (`v=0, w≠0` 被前向防撞清零) | 新增 `SAFETY_MODE_TURN`，转向时直接放行 |
### 2.3 没有动的部分
以下模块**完全未修改**，行为与 v1.0 一致：
- `corridor_ekf.c/.h` — EKF 数学核心
- `corridor_preproc.c/.h` — 传感器预处理
- `corridor_ctrl.c/.h` — 沟内 PD 控制器
- `nav_script.c/.h` — 保留，通过编译开关可切换回单沟测试
- `snc_can_app.c/.h` — CAN 协议层（已冻结）
- 所有传感器驱动（VL53、激光、IMU）
- 黑板、里程计、指令槽
---
## 3. 代码目录结构（当前）
```
D:\ARES\
├── App/
│ ├── robot_params.h ← ★ 全局参数配置（含新增 P6 赛道级参数）
│ ├── app_tasks.c/.h ← 导航流水线（已改造，#if USE_GLOBAL_NAV 切换）
│ │
│ ├── nav/ ← 导航与控制
│ │ ├── global_nav.c/.h ← ★★ 新增：赛道级总控（11状态机）
│ │ ├── track_map.c/.h ← ★★ 新增：S型遍历地图
│ │ ├── corridor_ctrl.c/.h ← 沟内 PD 控制器（未改）
│ │ ├── segment_fsm.c/.h ← 安全状态机（已改造：SafetyMode）
│ │ └── nav_script.c/.h ← 单沟测试脚本（保留，未改）
│ │
│ ├── est/
│ │ ├── corridor_filter.c/.h ← 已改造：新增 CorridorFilter_Reset()
│ │ └── corridor_ekf.c/.h ← EKF 核心（未改）
│ │
│ ├── preproc/
│ │ ├── corridor_msgs.h ← 已改造：新增 SafetyMode_t 枚举
│ │ └── corridor_preproc.c/.h ← 传感器预处理（未改）
│ │
│ ├── Contract/ ← 数据契约（未改）
│ ├── Can/ ← CAN 协议（已冻结，未改）
│ ├── IMU/ ← IMU 驱动（未改）
│ ├── laser/ ← 激光驱动（未改）
│ └── VL53L0X_API/ ← ToF 驱动（未改）
│
├── Doc/
│ ├── HANDOFF.md ← v1.0 交接文档
│ ├── 混合导航方案.md ← 方案设计文档
│ ├── 实施方案.md ← 详细实施文档
│ ├── HANDOFF_v2.md ← ★ 本文档（v2.0）
│ └── map.md ← 赛道地图
│
├── build/Debug/ARES.elf ← 当前可用固件（编译通过）
└── CMakeLists.txt
```
---
## 4. 软件架构总览
```
┌───────────────────────────────────────────────────────────────────────────┐
│ FreeRTOS 任务层 │
│ canTxTask monitorTask navTask laserTestTask vl53Task imuTask │
│ (20ms) (100ms) (20ms) (50ms) (100ms) (10ms) │
└──────────────────────────┬────────────────────────────────────────────────┘
│
navTask 内每 20ms 执行一次导航流水线
│
┌──────────────────────────▼────────────────────────────────────────────────┐
│ Blackboard (全局传感器数据黑板) │
└──────────────────────────┬────────────────────────────────────────────────┘
│ GetSnapshot()
┌──────────────────────────▼────────────────────────────────────────────────┐
│ CorridorPreproc (传感器清洗 → CorridorObs_t) │
└──────────────────────────┬────────────────────────────────────────────────┘
│
┌──────────────────────────▼────────────────────────────────────────────────┐
│ CorridorFilter / EKF (状态估计 → CorridorState_t {e_y,e_th,s}) │
└──────────────────────────┬────────────────────────────────────────────────┘
│
┌─────────────────┴──────────────────────┐
│ USE_GLOBAL_NAV=1 (赛道模式) │ USE_GLOBAL_NAV=0 (测试模式)
▼ ▼
┌─────────────────────┐ ┌──────────────────────┐
│ GlobalNav │ │ NavScript │
│ (赛道级状态机) │ │ (单沟往返脚本) │
│ 11 个阶段 │ │ IDLE→CORRIDOR→EXIT │
│ ↕ TrackMap │ └──────────────────────┘
└────────┬────────────┘
│ nav_out.safety_mode
│ nav_out.request_corridor / use_override
▼
┌─────────────────────┐ ┌────────────────────┐
│ CorridorCtrl │◄────────│ (request_corridor) │
│ (沟内 PD 控制) │ └────────────────────┘
└────────┬────────────┘
│ RawCmd_t
▼
┌─────────────────────────────────────────────────────────┐
│ SegFsm (安全仲裁) │
│ 感知 SafetyMode → 动作语义相关安全策略 │
│ CORRIDOR: 前向减速/停车/E-STOP 全开 │
│ TURN: 直接放行 (允许 v=0, w≠0) │
│ STRAIGHT: 仅前向防撞，不检查 conf │
│ IDLE: 全部清零 │
└────────┬────────────────────────────────────────────────┘
│ SegFsmOutput_t {safe_v, safe_w}
▼
CmdSlot_Push → canTxTask → CAN 0x100 → STM32F407 底盘
```
---
## 5. 数据流水线（当前）
`navTask` 每 20ms 执行，共 7 步：
```
Step 1: Blackboard_GetSnapshot(&board)
│ 原子快照，无撕裂
│
Step 2: CorridorPreproc_ExtractObs(&board, now_ms, &obs)
│ VL53: mm→m，范围校验
│ STP+ATK: 互补融合
│ → CorridorObs_t {d_lf/d_lr/d_rf/d_rr, d_front, d_back, valid_mask}
│
Step 3: CorridorFilter_Update(&obs, imu_wz, odom_vx, dt, yaw_rad, yaw_ok, &state)
│ EKF 预测 + 侧墙观测更新 + IMU 航向约束
│ → CorridorState_t {e_y, e_th, s, conf}
│
Step 4: GlobalNav_Update(&obs, &state, &board, &nav_out) 【赛道模式】
│ 赛道级状态机推进
│ → GlobalNavOutput_t {stage, safety_mode, request_corridor, override_v/w}
│
Step 5: if (nav_out.request_corridor)
│ CorridorCtrl_Compute(&state, &obs, imu_wz, &raw_cmd) ← 沟内 PD
│ else
│ raw_cmd = {override_v, override_w} ← 上层覆盖
│
Step 6: SegFsm_Update(&raw_cmd, &obs, &state, nav_out.safety_mode, &fsm_out)
│ 感知 SafetyMode 的安全仲裁
│ → SegFsmOutput_t {safe_v, safe_w}
│
Step 7: CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0)
→ canTxTask 取走 → CAN 0x100
```
---
## 6. 赛道级导航状态机详解
### 6.1 文件
| 文件 | 内容 |
|------|------|
| `App/nav/global_nav.h` | 枚举、配置结构、输出结构、API 声明 |
| `App/nav/global_nav.c` | 完整状态机实现（596 行） |
### 6.2 状态枚举
```c
typedef enum {
GNAV_IDLE = 0, // 启动区等待
GNAV_ENTRY_STRAIGHT, // 入场直线
GNAV_TURN_INTO_CORRIDOR, // 第一次入沟转向 (90°)
GNAV_REACQUIRE, // 重捕获走廊
GNAV_CORRIDOR_TRACK, // 沟内闭环跟踪
GNAV_TURN_OUT_OF_CORRIDOR, // 出沟转向 (90°)
GNAV_LINK_STRAIGHT, // 连接段直行
GNAV_TURN_INTO_NEXT, // 入下一条沟转向 (90°)
GNAV_EXIT_STRAIGHT, // 出场直行
GNAV_DOCK, // 回停启动区
GNAV_FINISHED, // 终态
GNAV_ERROR // 异常态 (超时兜底)
} GlobalNavStage_t;
```
### 6.3 完整状态转移图
```
GlobalNav_Start()
│
▼
GNAV_IDLE（启动区等待）
│ 第一个 Update 周期自动
▼
GNAV_ENTRY_STRAIGHT（入场直线） ──── [里程≥0.30m 或 超时10s]
│ (不用VL53：左侧围栏始终有效会误触发)
▼
GNAV_TURN_INTO_CORRIDOR（第一次入沟转向） ── [IMU 转角 ≥ 85°]
│
▼
GNAV_REACQUIRE（重捕获走廊） ─────── [3+传感器有效 + 宽度匹配 + conf≥0.6 持续5拍]
│ [超时5s → ERROR]
▼
GNAV_CORRIDOR_TRACK（沟内闭环跟踪） ───── [d_front≤0.10m + 里程>1.0m]
│ [里程>2.5m 超长保护]
│
┌──────────┘
▼
GNAV_TURN_OUT_OF_CORRIDOR（出沟转向） ────────────── [IMU 转角 ≥ 85°]
│
├── [非最后一条沟]
│ │
│ ▼
│ GNAV_LINK_STRAIGHT（连接段直行） ──────── [B: 前激光变化≥0.595m]
│ │ [或 (A: 里程≥0.595m) AND (C: VL53沟口检测连续2拍)]
│ │ [超时8s → ERROR]
│ ▼
│ GNAV_TURN_INTO_NEXT（入下一条沟转向） ──────── [IMU 转角 ≥ 85°]
│ │
│ └─────────────────── → GNAV_REACQUIRE (循环)
│
└── [最后一条沟 (C6)]
│
▼
GNAV_EXIT_STRAIGHT（出场直行） ─────────── [侧向VL53全丢 + 冲刺≥1.5m]
│ [里程>4.5m 或 超时30s → DOCK]
▼
GNAV_DOCK（回停启动区） ──────────────── [里程≥0.5m 或 超时5s]
│
▼
GNAV_FINISHED（终态） ──────────── 终态，停车
任意阶段超时 → GNAV_ERROR（异常态） → 2s 后 → GNAV_FINISHED（终态）
```
### 6.4 各状态行为速查表
| 状态 | v (m/s) | w (rad/s) | 安全模式 | 传感器依赖 | 退出条件 |
|------|---------|-----------|---------|-----------|---------|
| IDLE（启动区等待） | 0 | 0 | IDLE | — | Start() 触发 |
| ENTRY_STRAIGHT（入场直线） | 0.08 | P保直 | STRAIGHT | IMU航向+前激光 | 里程≥0.30m 或 超时 |
| TURN_INTO_CORRIDOR（第一次入沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° |
| REACQUIRE（重捕获走廊） | 0.05 | P保直 | STRAIGHT | VL53+EKF | 双侧锁定持续5拍 |
| CORRIDOR_TRACK（沟内闭环跟踪） | 0.15 (PD) | PD | CORRIDOR | VL53+IMU+前激光 | d_front≤0.10m+里程>1m |
| TURN_OUT（出沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° |
| LINK_STRAIGHT（连接段直行） | 0.10 | P保直 | STRAIGHT | 前激光+里程+非围栏侧VL53 | 三信号联合判定(见6.8) |
| TURN_INTO_NEXT（入下一条沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° |
| EXIT_STRAIGHT（出场直行） | 0.15 | P保直 | STRAIGHT | IMU+里程+VL53全丢 | VL53全丢+冲刺1.5m |
| DOCK（回停启动区） | 0.05 | 0 | STRAIGHT | 里程计 | 里程到 |
| FINISHED（终态） | 0 | 0 | IDLE | — | 终态 |
| ERROR（异常态） | 0 | 0 | IDLE | — | 2s后→FINISHED |
### 6.5 S 型遍历中的转向方向
场地结构关键理解：
- 垄沟沿 **X 轴（横向）** 分布，长 220cm，宽 40cm
- 左右两端各有一条 **纵向端部通道**（宽 40cm，长 390cm）
- 启动区在左下角，入口对齐左端通道
- C1 在最南端（离入口最近），C6 在最北端
- 机器人从启动区向北进入左端通道，入场距离仅约 10~30cm 即到 C1 入口
端部通道传感器特点：
- **一侧贴围栏**：VL53 能测到（~20cm）
- **另一侧交替出现垄背端面和垄沟开口**：垄背端面处能测到，垄沟开口处测不到（开口通向 220cm 远的对端，远超 VL53 的 1.2m 有效距离）
- 因此端部通道内**不能依赖 EKF**，必须用 IMU 航向保持 + 前/后激光到端检测
从地图推导的 S 型转向规律（北 = Y 递减 = 地图上方）：
| 垄沟 | 行驶方向 | 入沟转向(从纵向通道) | 到端后出沟转向 | 连接段方向 |
|------|---------|-------------------|-------------|-----------|
| C1 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) |
| C2 | ←西 | 左转(CCW) | 右转(CW) | 北行(左端通道) |
| C3 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) |
| C4 | ←西 | 左转(CCW) | 右转(CW) | 北行(左端通道) |
| C5 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) |
| C6 | ←西 | 左转(CCW) | **左转(CCW)→朝南** | **南行出场** |
> **C6 出沟左转说明**: C6 向西走到左端通道，前激光测到左围栏。此时需要朝南回到入口出场，从朝西左转 90° 正好朝南。
### 6.6 转向执行细节
三种转向状态（TURN_INTO_CORRIDOR、TURN_OUT、TURN_INTO_NEXT）共用同一套 `execute_turn()` 逻辑：
```c
// 已转过的角度 = (当前yaw - 起始yaw) × 方向符号(±1)
float delta = (imu_yaw - turn_start_yaw) * turn_sign;
// 减速区: 剩余角度 < 28.6° 时开始线性减速
float omega = turn_omega; // 默认 1.0 rad/s
if (remaining_deg < decel_zone_deg) {
omega = turn_min_omega + ratio * (turn_omega - turn_min_omega);
}
// 最低角速度 0.3 rad/s，防止接近目标时停转
// 完成判定: 已转 ≥ 85° (容差 5°)
if (delta >= 90° - 5°) → 转移到下一状态
```
### 6.7 重捕获判据
进入新垄沟后，`GNAV_REACQUIRE` 阶段低速前进，同时检查三个条件：
1. **至少 3 个侧向传感器有效**（4 个 VL53 中至少 3 个返回合法距离）
2. **几何宽度匹配**：若左右两侧都有效，则 `d_left + d_right + 车宽` 与走廊标称宽度 40cm 的误差 ≤ 5cm
3. **EKF 置信度 ≥ 0.6**
三个条件同时满足，连续 5 拍（100ms），才认为重捕获成功，切换到 `GNAV_CORRIDOR_TRACK`。
### 6.8 连接段三信号联合判定（核心新设计）
#### 场景描述
走完垄沟后，机器人在端部纵向通道里面朝北行驶，目标是走 70cm 到达下一条垄沟的入口。这 70cm 等于一条垄沟宽（40cm）加一条垄背宽（30cm）。
端部通道里的传感器情况：
```
围栏侧（VL53始终有效，不参与判定）
│
┌───────┤ ┌── 垄背端面（VL53有效 ~10cm）
│ 车 │ │
│ │ ─────► │ ← 非围栏侧
│ │ │
└───────┤ └── 垄沟开口（VL53无效 / >1.2m）
│
围栏侧
前激光 ↑ 测到上围栏（距离随北行递减）
后激光 ↓ 测到下围栏（距离随北行递增）
```
非围栏侧 VL53 的信号特征：
- 经过垄背端面时：读数 ≈ `(通道宽 - 车宽) / 2 - VL53内缩` = `(40-20)/2 - 0 = 10cm` → **有效**
- 进入垄沟开口时：射线穿入沟内（沟长 220cm + 对端通道 40cm ≈ 260cm）→ **超出 VL53 有效距离 1.2m** → 返回无效或 0
当车身中心对准垄沟入口时，前后两颗 VL53（间距 12cm）都已进入开口区域（沟口宽 40cm），两颗均应丢失。
#### 三个信号定义
| 信号 | 来源 | 触发条件 | 精度 | 权重 |
|------|------|---------|------|------|
| **A 里程计** | `odom_distance_accum` | `odom >= 0.70m × 0.85 = 0.595m` | 低（打滑） | 低 |
| **B 前激光变化** | `d_front_start - d_front_now` | 变化量 `>= 0.70m × 0.85 = 0.595m` | 高（绝对距离） | 高 |
| **C VL53 沟口检测** | 非围栏侧前/后 VL53 | 读数 `> 0.5m` 或无效，连续 **2 拍**（40ms）| 中（直接探沟口） | 中 |
> **阈值 0.5m 的来源**：正常贴垄背端面时 VL53 读数 ≈ 10cm，进入沟口后 >120cm（超出量程）。0.5m 在两者正中间，充分区分。
> **0.85 容差系数**：为里程计打滑和激光安装偏置留 15% 裕量。
#### 触发逻辑
```
B || (A && C)
```
- **B 单独成立** → 前激光变化量足够，直接触发（最可靠，不依赖其他）
- **A 且 C 同时成立** → 里程计粗定位 + VL53 精确探到沟口，联合校验后触发
#### 哪一侧是"非围栏侧"
由**刚走完的那条沟的行驶方向**决定，在 `LINK_STRAIGHT` 阶段 `current_corridor_id` 还未更新：
| 刚走完的沟 | 到达哪个通道 | 围栏侧 | **非围栏侧（检查这侧）** |
|-----------|------------|--------|----------------------|
| `TRAVEL_DIR_EAST`（向东） | 右端通道 | 右侧 | **左侧 VL53** |
| `TRAVEL_DIR_WEST`（向西） | 左端通道 | 左侧 | **右侧 VL53** |
#### 关键代码（`global_nav.c`）
```c
/* 信号 B: 前激光变化量 */
float d_front_delta = s_nav.link_d_front_start - obs->d_front;
bool laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
/* 信号 A: 里程计 */
bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
/* 信号 C: 非围栏侧 VL53 沟口检测（连续2拍）*/
bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir); // cd = 当前沟
if (gap_now) s_nav.link_gap_count++;
else s_nav.link_gap_count = 0;
bool gap_confirmed = (s_nav.link_gap_count >= 2);
/* 联合判定 */
if (laser_ok || (odom_ok && gap_confirmed))
transition_to(GNAV_TURN_INTO_NEXT, board);
```
---
## 7. 安全层改造详解
### 7.1 改造动机
v1.0 的 `segment_fsm` 统一用前向距离判定安全策略，导致**转向阶段被卡死**（RISK-1）：
- 到端后 `d_front` 很近，`SegFsm` 进入 STOP 状态
- 此时脚本输出 `v=0, w≠0` 期望原地转向
- 但 STOP 状态将 `safe_w` 也清零 → 转不动
### 7.2 改造方案
新增 `SafetyMode_t` 枚举作为安全层的"动作语义输入"：
```c
// corridor_msgs.h
typedef enum {
SAFETY_MODE_IDLE, // 零速，不做任何裁剪
SAFETY_MODE_CORRIDOR, // 完整安全检查（前向减速/停车/E-STOP）
SAFETY_MODE_TURN, // 转向：直接放行，不检查前距和置信度
SAFETY_MODE_STRAIGHT // 直行：仅前向防撞，不检查 conf
} SafetyMode_t;
```
### 7.3 各模式行为
| SafetyMode | 前向减速/停车 | E-STOP (conf<0.1) | w 保留 |
|------------|-------------|------------------|--------|
| IDLE | ❌ | ❌ | ❌ (全清零) |
| CORRIDOR | ✅ | ✅ | ✅ APPROACH 时保留 |
| **TURN** | **❌** | **❌** | **✅ 完全放行** |
| STRAIGHT | ✅ | ❌ | ✅ APPROACH 时保留 |
### 7.4 函数签名变化
```c
// v1.0 (旧)
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SegFsmOutput_t *out);
// v2.0 (新) — 增加 SafetyMode_t mode 参数
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SafetyMode_t mode, // ← 新增
SegFsmOutput_t *out);
```
> **兼容性**: 旧的 `nav_script` 调用路径（`USE_GLOBAL_NAV=0` 时）传入 `SAFETY_MODE_CORRIDOR`，行为与 v1.0 完全一致。
---
## 8. 地图模块详解
### 8.1 文件
| 文件 | 内容 |
|------|------|
| `App/nav/track_map.h` | 数据结构 + API 声明 |
| `App/nav/track_map.c` | 硬编码 S 型遍历表 + 查询实现 |
### 8.2 设计原则
地图**不做全局坐标**，只回答三个问题：
1. 从第 N 条沟完成后，下一条是第几条？
2. 这次该往哪转？（左/右）
3. 当前是不是最后一条沟？
### 8.3 核心数据
```c
// 单条垄沟描述
typedef struct {
uint8_t id; // 0-5
TravelDirection_t travel_dir; // EAST(→) 或 WEST(←)
TurnDirection_t exit_turn_dir; // 出沟转向方向
TurnDirection_t entry_turn_dir; // 入沟转向方向(从纵向通道转入)
bool is_last; // 是否为最后一条
} CorridorDescriptor_t;
```
硬编码的遍历表（`track_map.c`）：
```
C0(C1): →东 entry:右转 exit:左转 is_last:false
C1(C2): ←西 entry:左转 exit:右转 is_last:false
C2(C3): →东 entry:右转 exit:左转 is_last:false
C3(C4): ←西 entry:左转 exit:右转 is_last:false
C4(C5): →东 entry:右转 exit:左转 is_last:false
C5(C6): ←西 entry:左转 exit:左转 is_last:true ← 最后一条，左转朝南出场
```
### 8.4 API
```c
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);
```
---
## 9. EKF 重置机制
### 9.1 为什么每次入沟都要重置
不同垄沟的 `e_y` 参考基准不同（每条沟的"居中"对应的 VL53 读数不同）。如果不重置，EKF 会用上一条沟的历史协方差和参考值去更新新沟的观测，导致：
- 初始几拍置信度虚高（P 矩阵太小）
- 横向偏差 `e_y` 带着上条沟的偏置
### 9.2 实现
```c
// corridor_filter.h — 新增接口
void CorridorFilter_Reset(void);
// corridor_filter.c — 实现
void CorridorFilter_Reset(void) {
if (!s_initialized) return;
CorridorEKF_Reset(); // EKF 状态归零，P 恢复初始值
s_imu_yaw_ref_rad = 0.0f; // 解锁 yaw_ref
s_imu_yaw_ref_set = false; // 等待在新沟中重新锁定
}
```
### 9.3 调用时机
`GlobalNav` 在 `transition_to(GNAV_REACQUIRE, ...)` 时调用，即每次转向完成后、开始寻找新走廊之前。
---
## 10. 编译开关与模式切换
### 10.1 控制宏
```c
// App/robot_params.h
#define USE_GLOBAL_NAV 0 // 1=赛道模式 0=单沟测试模式 (当前为单沟测试)
```
### 10.2 两种模式的差异
| | `USE_GLOBAL_NAV=1` | `USE_GLOBAL_NAV=0` |
|-|-------------------|-------------------|
| 导航模块 | `GlobalNav` | `NavScript` |
| 启动调用 | `GlobalNav_Start()` | `NavScript_Start()` |
| 遍历范围 | 6 条垄沟 S 型 | 单垄沟往返 |
| 安全模式 | 按阶段动态切换 | 固定 `SAFETY_MODE_CORRIDOR` |
| 适用场景 | 正式比赛 | 单条走廊调试 |
### 10.3 切换方法
仅修改 `robot_params.h` 中的宏，**重新编译烧录**即可。所有逻辑通过 `#if USE_GLOBAL_NAV` 在 `app_tasks.c` 中编译时切换，不增加运行时开销。
---
## 11. 全部可调参数
### P0 — 几何参数（实测填写）
| 参数 | 当前值 | 说明 |
|------|--------|------|
| `PARAM_ROBOT_WIDTH` | 0.200 m | 车体宽度 |
| `PARAM_ROBOT_LENGTH` | 0.200 m | 车体长度 |
| `PARAM_WHEEL_DIAMETER` | 0.080 m | 驱动轮直径 |
| `PARAM_WHEEL_TRACK` | 0.140 m | 左右轮中心距 |
| `PARAM_SENSOR_BASE_LENGTH` | 0.120 m | 同侧前后 VL53 间距 |
| `PARAM_CORRIDOR_WIDTH` | 0.40 m | 走廊宽度 |
| `PARAM_FRONT_LASER_OFFSET` | 0.0 m | 前激光到车头偏置 |
| `PARAM_REAR_LASER_OFFSET` | 0.0 m | 后激光到车尾偏置 |
| `PARAM_VL53_SIDE_INSET` | 0.0 m | 侧向 VL53 统一内缩距离 (兼容) |
| `PARAM_VL53_LEFT_INSET` | 0.0 m | 左侧 VL53 内缩距离 (实测后填入!) |
| `PARAM_VL53_RIGHT_INSET` | 0.0 m | 右侧 VL53 内缩距离 (实测后填入!) |
| `PARAM_ENCODER_CPR` | 500 | 编码器每转脉冲数 |
### P2 — EKF 滤波器（调优）
| 参数 | 当前值 | 说明 |
|------|--------|------|
| `PARAM_EKF_Q_EY` | 0.01 | 横向过程噪声 |
| `PARAM_EKF_Q_ETH` | 0.001 | 航向过程噪声 |
| `PARAM_EKF_Q_S` | 0.1 | 里程过程噪声 |
| `PARAM_EKF_R_EY` | 0.015 | 横向观测噪声 (VL53 可信度低，已调大) |
| `PARAM_EKF_R_ETH` | 0.001 | 航向观测噪声（侧墙，已取消使用） |
| `PARAM_EKF_R_ETH_IMU` | 0.002 | 航向观测噪声（IMU，已调小以充分利用高精度 IMU） |
| `PARAM_EKF_P0_EY` | 0.1 | e_y 初始不确定度 |
| `PARAM_EKF_P0_ETH` | 0.1 | e_th 初始不确定度 |
### P3 — 沟内控制器（调优）
| 参数 | 当前值 | 说明 |
|------|--------|------|
| `PARAM_CTRL_KP_THETA` | 2.0 | 航向比例增益 |
| `PARAM_CTRL_KD_THETA` | 0.4 | 航向微分增益 |
| `PARAM_CTRL_KP_Y` | 4.0 | 横向比例增益 |
| `PARAM_CTRL_V_CRUISE` | 0.15 m/s | 巡航速度 |
| `PARAM_CTRL_W_MAX` | 1.5 rad/s | 最大角速度 |
| `PARAM_CTRL_V_MAX` | 0.3 m/s | 最大线速度 |
| `PARAM_CTRL_SPEED_REDUCTION` | 0.4 | 弯道减速系数 |
### P4 — 安全阈值（调优）
| 参数 | 当前值 | 说明 |
|------|--------|------|
| `PARAM_SAFE_D_FRONT_STOP` | 0.10 m | 前向停车距离 |
| `PARAM_SAFE_D_FRONT_APPROACH` | 0.25 m | 前向减速预警距离 |
| `PARAM_SAFE_APPROACH_MIN_V` | 0.05 m/s | 减速区最低速度 |
| `PARAM_SAFE_CONF_ESTOP` | 0.10 | E-Stop 置信度阈值（CORRIDOR模式） |
### P6 — 赛道级导航参数（新增，v2.0）
| 参数 | 当前值 | 说明 |
|------|--------|------|
| `USE_GLOBAL_NAV` | 0 | 编译开关 (当前为单沟测试模式) |
| **入场段** | | |
| `PARAM_GNAV_ENTRY_V` | 0.08 m/s | 入场速度 |
| `PARAM_GNAV_ENTRY_DISTANCE` | 0.30 m | 入场里程上限 (启动区到C1入口仅约10~30cm) |
| `PARAM_GNAV_ENTRY_TIMEOUT` | 10000 ms | 入场超时 |
| **转向** | | |
| `PARAM_GNAV_TURN_OMEGA` | 1.0 rad/s | 转向角速度 |
| `PARAM_GNAV_TURN_TOLERANCE` | 0.087 rad (~5°) | 转向完成容差 |
| `PARAM_GNAV_TURN_DECEL_ZONE` | 0.5 rad (~28°) | 减速区起始角度 |
| `PARAM_GNAV_TURN_MIN_OMEGA` | 0.3 rad/s | 减速区最低角速度 |
| `PARAM_GNAV_TURN_TIMEOUT` | 8000 ms | 单次转向超时 |
| **重捕获** | | |
| `PARAM_GNAV_REACQUIRE_V` | 0.05 m/s | 重捕获速度 |
| `PARAM_GNAV_REACQUIRE_CONF` | 0.6 | 置信度阈值 |
| `PARAM_GNAV_REACQUIRE_WIDTH_TOL` | 0.05 m | 走廊宽度容差 |
| `PARAM_GNAV_REACQUIRE_TICKS` | 5 拍 | 连续确认次数 |
| `PARAM_GNAV_REACQUIRE_TIMEOUT` | 5000 ms | 重捕获超时 |
| **沟内** | | |
| `PARAM_GNAV_CORRIDOR_END_DIST` | 0.10 m | 到端检测距离 |
| `PARAM_GNAV_CORRIDOR_MAX_LEN` | 2.50 m | 沟内里程保护上限 |
| **连接段** | | |
| `PARAM_GNAV_LINK_V` | 0.10 m/s | 连接段速度 |
| `PARAM_GNAV_LINK_DISTANCE` | 0.70 m | 连接段标称距离 |
| `PARAM_GNAV_LINK_TIMEOUT` | 8000 ms | 连接段超时 |
| **出场** | | |
| `PARAM_GNAV_EXIT_V` | 0.15 m/s | 出场速度 |
| `PARAM_GNAV_EXIT_RUNOUT` | 1.50 m | 侧向丢失后冲刺距离 |
| `PARAM_GNAV_EXIT_MAX_DIST` | 4.50 m | 出场里程保护 (纵向通道全长约3.9m) |
| `PARAM_GNAV_EXIT_TIMEOUT` | 30000 ms | 出场超时 (纵向通道距离长，给足时间) |
| **回停** | | |
| `PARAM_GNAV_DOCK_V` | 0.05 m/s | 回停速度 |
| `PARAM_GNAV_DOCK_DISTANCE` | 0.50 m | 回停距离 |
| **其他** | | |
| `PARAM_GNAV_HEADING_KP` | 0.03 | 航向保持 P 增益（°→rad/s） |
---
## 12. FreeRTOS 任务一览
与 v1.0 完全一致，无新增任务：
| 任务名 | 周期 | 优先级 | 职责 |
|--------|------|--------|------|
| `canTxTask` | 20ms | AboveNormal | CAN 0x100 发送；100ms 看门狗 |
| `navTask` | 20ms | AboveNormal | **完整导航流水线（含新 GlobalNav）** |
| `LaserTsk`（内部） | 10ms | AboveNormal | 4 路 UART DMA 激光解析 |
| `monitorTask` | 100ms | Normal | CAN 健康；里程计积分 |
| `laserTestTask` | 50ms | Normal | 激光推送黑板 |
| `vl53Task` | 100ms | Normal | VL53L0X 读取推送黑板 |
| `imuTask` | 10ms | BelowNormal | IMU 解析推送黑板 |
| `defaultTask` | — | Normal | USB CDC 初始化；空闲循环 |
---
## 13. 文件快速索引
| 你想做什么 | 去看哪个文件 |
|-----------|-------------|
| 改调参数值 | `App/robot_params.h` |
| 切换赛道/单沟模式 | `App/robot_params.h` → `USE_GLOBAL_NAV` |
| 看赛道状态机逻辑 | `App/nav/global_nav.c` |
| 看 S 型遍历拓扑表 | `App/nav/track_map.c` |
| 看安全模式定义 | `App/preproc/corridor_msgs.h` → `SafetyMode_t` |
| 看安全层实现 | `App/nav/segment_fsm.c` |
| 看沟内控制律 | `App/nav/corridor_ctrl.c` |
| 看 EKF 数学 | `App/est/corridor_ekf.c` |
| 看 EKF 重置 | `App/est/corridor_filter.c` → `CorridorFilter_Reset()` |
| 看导航流水线入口 | `App/app_tasks.c` → `AppTasks_RunNavTask_Impl()` |
| 看系统初始化 | `App/app_tasks.c` → `AppTasks_Init()` |
| 看 CAN 协议 | `App/Can/snc_can_app.c` |
| 看全局数据结构 | `App/Contract/robot_blackboard.h` |
| 看传感器预处理 | `App/preproc/corridor_preproc.c` |
| 看赛道地图 | `Doc/map.md` |
| 看单沟测试脚本 | `App/nav/nav_script.c` |
---
## 14. 已知问题与待办
### 已在 v2.0 解决
| # | 问题 | 解决方式 |
|---|------|---------|
| **RISK-1** | 转向阶段被安全层卡死 | `SAFETY_MODE_TURN` 直接放行 |
| — | 缺少 6 沟遍历能力 | `global_nav.c` 完整实现 |
| — | 缺少 EKF 入沟重置 | `CorridorFilter_Reset()` |
### v1.0 遗留的已解决 BUG（已全部修复）
BUG-1 至 BUG-9 均在 v1.0 已修复，v2.0 继承已修复状态。详见 `HANDOFF.md`。
### 当前待办
| # | 问题 | 优先级 | 说明 |
|---|------|--------|------|
| **CAL-1** | `PARAM_FRONT_LASER_OFFSET = 0.0` | 高 | 实测填写，影响到端检测精度 |
| **CAL-2** | `PARAM_REAR_LASER_OFFSET = 0.0` | 高 | 同上 |
| **CAL-3** | `PARAM_VL53_SIDE_INSET = 0.0` | 高 | 单侧退化时影响 EKF 精度 |
| **CAL-4** | `PARAM_IMU_YAW_OFFSET = 0.0` | 中 | 声明了但代码未使用 |
| **TODO-1** | `GlobalNav` 里程计时间源 | 中 | 当前用 `imu_wz.timestamp_ms` 估算时间，可改为 `HAL_GetTick()` 从外部传入，更准确 |
| **TODO-2** | 连接段过短/过长自适应 | 低 | 当前固定 0.70m，实际可能因转向角度偏差需要微调 |
| **TODO-3** | 重捕获单侧有效策略 | 低 | 若入沟角度大导致某侧 VL53 暂时无效，当前判定较严格 |
### 设计风险（已知，待注意）
| # | 风险 | 影响 | 说明 |
|---|------|------|------|
| **RISK-2** | 连接段偏航 | 中 | IMU 短时漂移 + 轮滑可能导致连接段走歪，靠里程计上限保护 |
| **RISK-3** | 转向精度 | 中 | IMU 积分漂移，90°实际转角可能有 2-5° 误差，重捕获阶段兜底 |
| **RISK-4** | 入场阶段判据 | 中 | "侧向 VL53 有效"作为进入第一条沟的触发，若传感器偶发有效可能提前转向 |
---
## 15. 实车调试建议
### 建议调试顺序
```
第1步: 几何参数实测 (P0)
→ 卷尺量 PARAM_FRONT/REAR_LASER_OFFSET、PARAM_VL53_SIDE_INSET
第2步: USE_GLOBAL_NAV=0，单沟测试
→ 验证走廊跟踪、到端检测、180°转向（nav_script 旧路径）
第3步: 切换 USE_GLOBAL_NAV=1，测试单次 90° 转向
→ 手动把机器人放在沟内，触发 TURN_OUT，看 IMU 转角是否准确
第4步: 测试重捕获
→ 手动转向后进入 REACQUIRE，看 5 拍锁定时间，调 CONF 阈值
第5步: 测试双沟 S 型 (C1 → C2)
→ 验证连接段长度、入沟对准
第6步: 全 6 沟测试
→ 观察每条沟的入沟质量、完成率
第7步: 调优参数
→ TURN_OMEGA, LINK_DISTANCE, REACQUIRE_TICKS 等
```
### 常见问题诊断（新增 v2.0 相关）
| 现象 | 可能原因 | 处置方法 |
|------|----------|---------|
| 转向后没进下一阶段 | IMU 转角计算偏差 | 检查 `imu_yaw_continuous` 是否正常更新；调小 `TURN_TOLERANCE` |
| 重捕获超时（5s） | 入沟角度偏大，VL53 看不到两侧 | 增大 `REACQUIRE_TIMEOUT`；或降低 `REACQUIRE_TICKS` |
| 连接段走过头 | 里程计打滑 | 适当减小 `LINK_DISTANCE`；靠 VL53 探壁辅助触发 |
| 连接段走不够 | 里程计未积分（odom=0） | 检查 monitorTask 里程计链路 |
| 安全层卡住转向 | `SAFETY_MODE_TURN` 未传入 | 检查 `GlobalNav_Update()` 的 `nav_out.safety_mode` 输出 |
| 出场后未停车 | VL53 未丢失触发 | 调小 `EXIT_RUNOUT`；检查 `all_side_lost()` 判定 |
| 每次入沟偏向一侧 | `PARAM_VL53_SIDE_INSET` 未校准 | 实测传感器内缩距离并填入 |
---
> **给接手者的提醒**:
> 1. 调参前先确认 `USE_GLOBAL_NAV` 的模式，不同模式下调的参数组不同
> 2. v2.0 新增的 P6 参数全部有默认值，实车前**必须根据实际场地微调** `LINK_DISTANCE` 和 `ENTRY_DISTANCE`
> 3. `CorridorFilter_Reset()` 在每次入沟时自动调用，**不需要手动干预**
> 4. 如果出现整体行为异常，先把 `USE_GLOBAL_NAV=0` 切回单沟模式定位问题
> 5. CAN 协议层 (`snc_can_app.c`) 已冻结，**不要修改**

391
Doc/SD日志方案.md Normal file
View File

@@ -0,0 +1,391 @@
# ARES SD 日志方案
## 1. 结论
对当前项目，最可行的一期方案是：
- 用 `SDMMC1 + FatFs` 挂载 SD 卡。
- 新增一个独立的 `logTask` 作为唯一写卡任务。
- `navTask`、`monitorTask`、`imuTask` 等任务只负责把日志记录写入 RAM 队列，绝不直接 `f_write()`。
- 日志格式采用“事件 + 低频快照”的文本 CSV，一期先保证稳定可读，不追求全量高频原始数据。
这个方案最适合当前仓库，因为：
- 现有高频闭环在 `App/app_tasks.c` 的 `navTask` 中以 20ms 周期运行，不能被 SD 写卡阻塞。
- 当前日志只有 `printf -> USB CDC`，没有 FatFs/SDMMC 基础设施。
- 当前系统没有 RTC，日志只能基于 `HAL_GetTick()` 记录相对时间，文件名也不能依赖真实日期。
- `CMakeLists.txt` 已经自动收集 `App/*.c`，新增 `App/log/*.c` 不需要手工改源文件列表。
## 2. 结合当前项目的约束
### 2.1 现有可直接复用的数据源
当前代码里已经有足够多的“运行状态”可用于日志：
- 黑板快照：`App/Contract/robot_blackboard.h`
- 走廊观测：`CorridorObs_t`，定义在 `App/preproc/corridor_msgs.h`
- EKF 状态：`CorridorState_t`，定义在 `App/preproc/corridor_msgs.h`
- 赛道状态机输出：`GlobalNavOutput_t`，定义在 `App/nav/global_nav.h`
- 安全状态机输出：`SegFsmOutput_t`，定义在 `App/nav/segment_fsm.h`
这意味着日志不需要额外造一套状态模型，直接复用现有结构即可。
### 2.2 当前不适合的做法
不建议直接把现在的 `printf` 改成“同时写 USB 和 SD”：
- `printf` 现在走 `App/retarget.c` 的 `_write()`，底层是 USB CDC。
- 如果把 `_write()` 直接改成写文件，会让所有任务都可能进入 FatFs，线程模型会变复杂。
- 文本格式化和写卡延迟都可能拖慢闭环任务。
不建议一期就做“每 20ms 全量原始数据落盘”：
- 对排障来说，状态转移、关键变量、故障瞬间比全量采样更有价值。
- 高频写卡会更容易遇到 H7 的缓存一致性、FatFs 抖动、掉电损坏等问题。
## 3. 推荐总体架构
### 3.1 模块划分
建议新增目录：
- `App/log/sd_log.h`
- `App/log/sd_log.c`
- `App/log/sd_log_port.c` 或 `App/log/sd_log_fs.c`
职责划分：
- `sd_log.c`：日志队列、记录封装、CSV 格式化、刷盘策略。
- `sd_log_fs.c`：FatFs mount/open/write/sync/rotate。
- `app_tasks.c`：在现有任务周期内调用 `SDLog_TryPush...()`。
### 3.2 任务模型
新增一个低优先级 `logTask`：
- 优先级建议低于 `navTask` 和 `canTxTask`
- 周期不固定，阻塞等待日志队列
- 它是系统里唯一允许调用 `f_mount/f_open/f_write/f_sync/f_close` 的任务
生产者任务：
- `navTask`：产出导航快照、状态切换、故障事件
- `monitorTask`：产出 CAN 在线状态、底盘诊断、里程计健康事件
- 启动阶段：产出 boot 配置、固件版本、参数摘要
核心原则：
- 生产者只入队，不阻塞
- 队列满时允许丢弃低优先级快照，但不能卡住控制任务
- 重要事件单独计数，后续在日志里补写 `drop_count`
### 3.3 内存缓冲
一期建议用固定长度消息队列，而不是动态内存：
- 例如 `osMessageQueue`，每条记录 192B 或 256B
- 队列深度 32 到 64 条
- 总 RAM 占用约 8KB 到 16KB，可控
推荐记录结构：
```c
typedef enum {
LOG_REC_BOOT = 0,
LOG_REC_HEALTH,
LOG_REC_NAV_SNAPSHOT,
LOG_REC_STAGE_EVENT,
LOG_REC_FAULT,
} LogRecordType_t;
typedef struct {
uint32_t tick_ms;
uint16_t type;
uint16_t len;
char text[224];
} LogRecord_t;
```
这里故意用“预格式化文本入队”，因为：
- 一期更简单，便于直接在 SD 卡上看内容
- `logTask` 只做批量写盘，不做复杂格式化
- 后续如果需要高频日志，再把 `text` 改成二进制 payload
## 4. 推荐日志内容
### 4.1 一期必须记录的内容
建议只做三类：
1. 启动信息
2. 周期性状态快照
3. 异常/状态切换事件
### 4.2 启动信息
系统启动后创建文件，先写以下内容：
- 固件名、编译日期时间
- `USE_GLOBAL_NAV` 当前值
- 关键参数摘要
- SD 卡 mount 是否成功
- 日志格式版本号
示例：
```text
# ARES log v1
# build=Apr 03 2026 12:00:00
# mode=global_nav
# param.ctrl_v=0.20
# param.safe_front_stop=0.10
```
### 4.3 周期性状态快照
建议由 `navTask` 每 100ms 记录一次，不要每 20ms 都记。
推荐字段：
- `tick_ms`
- `stage` / `stage_name`
- `seg_state`
- `request_corridor`
- `override_v` / `override_w`
- `safe_v` / `safe_w`
- `odom_vx` / `odom_wz`
- `e_y` / `e_th` / `s` / `conf`
- `d_front` / `d_back`
- `d_lf` / `d_lr` / `d_rf` / `d_rr`
- `valid_mask`
- `chassis_online` / `chassis_diag`
推荐 CSV 行示例：
```text
12340,SNAP,GNAV_CORRIDOR_TRACK,CORRIDOR,1,0.000,0.000,0.180,-0.120,0.182,0.01,-0.03,1.42,0.92,0.31,1.85,0.12,0.11,0.10,0.10,0x3F,1,0x00000000
```
### 4.4 事件日志
以下情况必须立即记录，而不是等下一个 100ms 快照：
- `GlobalNav` 阶段切换
- `SegFsm` 状态切换
- 进入 `GNAV_ERROR`
- 进入 `SEG_STATE_ESTOP`
- 底盘离线 / 恢复在线
- SD 卡 mount 失败 / 写入失败 / 队列溢出
推荐示例：
```text
12520,EVENT,GNAV_STAGE,GNAV_REACQUIRE->GNAV_CORRIDOR_TRACK
20840,EVENT,SEG_FSM,CORRIDOR->E-STOP,conf=0.18
20900,FAULT,CHASSIS_OFFLINE,diag=0x00000004
```
## 5. 文件组织与命名
### 5.1 文件命名
因为当前没有 RTC，不建议用日期命名。
推荐方式：
- 目录：`/ARESLOG/`
- 文件名：`BOOT0001.CSV`、`BOOT0002.CSV`...
创建流程：
- 启动时扫描 `/ARESLOG`
- 找到最大序号
- 新建下一个序号文件
### 5.2 文件轮转
建议单文件大小限制 2MB 到 4MB：
- 超过阈值自动新建下一个文件
- 文件轮转只在 `logTask` 内执行
按 100ms 一条快照估算：
- 一条快照约 120B 到 220B
- 约 1.2KB/s 到 2.2KB/s
- 1 小时约 4MB 到 8MB
所以 4MB 一卷是合理的。
## 6. 刷盘策略
### 6.1 建议策略
不要每写一行就 `f_sync()`，也不要一直不刷。
推荐折中：
- RAM 累积 512B 或 1024B 后批量 `f_write()`
- 每 1000ms 做一次 `f_sync()`
- 重大故障事件后立即 `f_sync()` 一次
这样能兼顾三件事：
- 降低 FatFs 抖动
- 降低 SD 卡磨损
- 掉电时最多丢失最近约 1 秒日志
### 6.2 关闭策略
如果系统有明确“任务结束/人工停机”流程，可在停机前调用：
- `SDLog_Flush()`
- `SDLog_Close()`
如果没有，就依赖周期性 `f_sync()`。
## 7. SDMMC1 硬件与驱动建议
### 7.1 一期推荐配置
建议先用最保守配置把链路跑通：
- `SDMMC1`
- 1-bit 总线模式先 bring-up
- FatFs 先用默认配置
- 先不用 DMA 优化
原因：
- H7 上 SDMMC + DCache + DMA 的一致性问题很多
- 一期日志吞吐量很低，1-bit 也足够
- 先把 mount/open/write/read/sync 做稳定，比先追求 4-bit 吞吐更重要
稳定后再考虑：
- 切到 4-bit 模式
- 启用 DMA
- 做 32 字节对齐缓冲区和 cache clean/invalidate
### 7.2 引脚建议
若 PCB 已引出标准 SDMMC1 引脚，可优先使用：
- `PC8` `D0`
- `PC9` `D1`
- `PC10` `D2`
- `PC11` `D3`
- `PC12` `CK`
- `PD2` `CMD`
从当前 `ARES.ioc` 看，这组引脚还没有被现有业务占用，但仍需要你结合原理图再确认一次。
### 7.3 热插拔建议
一期不建议做复杂热插拔。
建议：
- 上电时检测并 mount
- 运行中默认认为卡一直在
- 如果 `f_write()` 连续失败，则置 `sd_offline` 标志并停止写卡
- 需要重新插卡时，用按键命令或调试命令手动 remount
这样实现最小、风险最低。
## 8. 代码落点建议
### 8.1 CubeMX / 自动生成部分
需要改：
- `ARES.ioc`：开启 `SDMMC1`
- `ARES.ioc`：开启 `FATFS`
- 生成对应 `sdmmc.c`、`fatfs.c`、中间件文件
### 8.2 手写业务代码
建议新增：
- `App/log/sd_log.c`
- `App/log/sd_log.h`
建议修改：
- `Core/Src/freertos.c`
- 增加 `logTask`
- `App/app_tasks.c`
- `AppTasks_Init()` 中初始化日志模块
- `AppTasks_RunNavTask_Impl()` 中每 100ms 打一次导航快照
- `AppTasks_RunMonitorTask()` 中记录底盘在线/离线变化
- `App/nav/global_nav.c`
- 在阶段切换点调用事件日志接口，或者暴露状态变化让 `navTask` 记录
- `App/nav/segment_fsm.c`
- 在 `E-STOP/STOP/APPROACH` 切换时记录事件，或者暴露状态变化给上层记录
### 8.3 更推荐的接入方式
为了少改导航核心，我更推荐：
- 在 `navTask` 内缓存“上一次 `stage` / `seg_state`”
- 若本周期发现变化，就从 `app_tasks.c` 发事件日志
这样可以减少对 `global_nav.c` 和 `segment_fsm.c` 的侵入。
## 9. 一期接口建议
建议暴露以下最小 API：
```c
bool SDLog_Init(void);
bool SDLog_Start(void);
void SDLog_Task(void *argument);
void SDLog_TryPushBoot(void);
void SDLog_TryPushHealth(uint32_t tick_ms, bool chassis_online, uint32_t diag);
void SDLog_TryPushNavSnapshot(uint32_t tick_ms,
const RobotBlackboard_t *board,
const CorridorObs_t *obs,
const CorridorState_t *state,
const GlobalNavOutput_t *gnav,
const SegFsmOutput_t *fsm);
void SDLog_TryPushEvent(const char *tag, const char *msg);
```
接口原则：
- 全部 `TryPush`
- 失败直接返回，不阻塞调用方
- 是否丢包由内部计数
## 10. 推荐实施顺序
建议按下面顺序推进，不要一步到位：
1. 在 CubeMX 打开 `SDMMC1 + FatFs`，完成最小 mount/write/read 测试。
2. 新建 `logTask`，只做固定字符串写卡，验证线程模型。
3. 接入启动日志和健康日志。
4. 接入 `navTask` 的 100ms 快照。
5. 接入 `stage` 与 `seg_state` 变化事件。
6. 最后再考虑 4-bit、DMA、热插拔、二进制高频日志。
## 11. 我对这个项目的最终建议
如果你的目标是“记录系统运行状况，便于复盘问题”，我建议你的一期目标定成：
- 能稳定挂载 SD 卡
- 能连续写 30 分钟以上不影响导航
- 能在一份 CSV 里看见：导航阶段、安全状态、EKF 置信度、关键距离、底盘在线状态
- 掉电最多损失 1 秒左右日志
不要把一期目标定成“全量原始数据黑匣子”。
对当前仓库，最小且正确的方案就是：
- `SDMMC1 + FatFs`
- 单写者 `logTask`
- 100ms 状态快照 + 即时事件日志
- CSV 文件 + 周期性 `f_sync()`
这套方案实现量小，调试成本低，而且已经足够覆盖你现在最关心的“系统运行状况记录”。

23
Doc/can通讯协议.md Normal file
View File

@@ -0,0 +1,23 @@
FDR-Core 上 位 机 CAN 协 议 说 明 书 （ 当 前 固 件 实 装 版 ） ⽂ 档 状 态 ： Internal / Current 适 ⽤ 固 件 ： 当 前 上 传 的 f4_can_app.c 实 现 说 明 依 据 ： 以 .c 代 码 ⾏ 为为 准 1. ⽬ 标 与 范 围 本 ⽂ 档 定 义上 位 机 与 底 盘 控 制 器 之 间 的 CAN 通 信 协 议 ， 覆 盖 以 下 内 容 ： 上 位 机 向 底 盘 下 发 速 度 命 令 底 盘 向 上 位 机 上 报 状 态 、 轮 速 、 ⾥ 程 和 通 信 诊 断 命 令 安 全 机 制 ： CRC 、 rolling counter 、 命 令 超 时 故 障 / 警 告 状 态 的 上 报 规 则 2. 总 线 与 编 码 约 定 CAN 类 型 ： 标 准 帧 （ 11-bit ID ） 帧 类 型 ： 数 据 帧 （ RTR=0 ） 字 节 序 ： ⼩ 端 缩 放 规 则 ： vx 、 wz ： int16 = 物理量 * 1000 轮 速 ： RPM 直 接按 int16 发 送 ⾥ 程 ： 增 量 ticks ， 按 int16 发 送 CRC ： CRC8-SAE J1850 poly = 0x1D init = 0xFF xorout = 0xFF ⾮ 反 射 当 前 固 件 在 0x100 控 制 帧 上 ， 对 前 7 个 字 节 计 算 CRC ， 再 放 ⼊ Byte7 。
3. 帧 ID 总 览 ⽅ 向 ID 名 称 实 际 周 期 说 明 Rx 0x080 Heartbeat 上 位 机 ⾃ 定 仅 表 ⽰ 链 路 活 着 ， 不 刷 新 运 动 看 ⻔ 狗 Rx 0x100 Velocity Command 建 议 20ms 速 度 控 制 命 令 ， 必 须 持 续 发 送 Tx 0x181 Status 20ms 状 态 机 / 健 康 等 级 / 诊 断 位 Tx 0x182 Actual RPM 轮 询 ， 约 60ms 实 际 轮 速 Tx 0x183 Target RPM 轮 询 ， 约 60ms ⽬ 标 轮 速 Tx 0x184 Comm Diag 100ms 通 信 统 计 Tx 0x200 Odom Delta 轮 询 ， 约 60ms 四 轮 ⾥ 程 增 量 当 前 固 件 在 CAN_Send_Telemetry_20ms() 中 采 ⽤ 如 下 节 拍 ： 0x181 ： 每 个 20ms 周 期 都 发 0x182 / 0x183 / 0x200 ： 通过 s_telem_slot 三 选 ⼀ 轮 询 发 送 0x184 ： 每 5 个 20ms 周 期 发 ⼀ 次 ， 即 100ms ⼀ 次 4. 上 位 机 -> 底 盘 4.1 0x080 ⼼ 跳 帧 ⽤ 途 ： 仅 表 ⽰ “ 链 路 上 仍 然 有 ⼈ 在 说话 ”
⾏ 为 ： 不 会 刷 新 运 动 看 ⻔ 狗 不 会 延 续 旧 速 度 命 令 当 前 固 件 不 解 析 其 载 荷 内 容 ， 也不 使 ⽤ DLC 内 容 建 议 ： 可发可 不 发 若 发 ， DLC 可 统 ⼀ 为 0 或 8 4.2 0x100 速 度 控 制 帧 ID ： 0x100 DLC ： 8 数 据 定 义 Byte 类 型 字 段 说 明 0~1 int16 LE vx_x1000 线 速 度 ， 单 位 m/s * 1000 2~3 int16 LE wz_x1000 ⻆ 速 度 ， 单 位 rad/s * 1000 4 uint8 ctrl_flags 控 制 标 志 位 ， 当 前 仅 保 存 ， 不 参 与 控 制决 策 5 uint8 reserved 预 留 ， 固 定 填 0 6 uint8 rolling_counter 滚 动 计 数 器 7 uint8 crc8 对 Byte0~6 做 CRC8-SAE J1850 固 件 接 收 后 会 把 vx 、 wz 恢 复 成 浮 点 值 ， 分别 写 ⼊ ： g_robot_ctrl.target_vx g_robot_ctrl.target_wz
并 保 存 ctrl_flags 与 rolling counter 。 接 收 判 定 规 则 1. DLC 必 须 等 于 8 2. CRC 必 须 正 确 3. rolling counter 必 须 满 ⾜ 下 列 规 则 ： 若 尚 未 同 步 ， 或 当 前 不 在 SYSTEM_OPERATIONAL ， 则 ⾸ 帧 直 接接 受 正 常 运 ⾏ 时 ， 新 counter 相 对 上 ⼀ 帧差 值 必 须 在 1..3 之 间 否 则 拒 收 发 送 建 议 推 荐 周 期 ： 20ms （ 50Hz ） 最 低 要 求 ： 不 要 超 过 150ms 不 发 送 合 法 0x100 即 使你 想 停 ⻋ ， 也 建 议 继续 周 期 发 送 ： vx = 0 wz = 0 的 合 法 命 令 帧 因 为 当 前 固 件 只 ⽤ “ 合 法 且 新 鲜 ” 的 0x100 喂 命 令 看 ⻔ 狗 ， 超 时 阈 值 是 150ms 。 5. 底 盘 -> 上 位 机 5.1 0x181 状 态 帧 ID ： 0x181 周 期 ： 20ms DLC ： 8 Byte 类 型 字 段 说 明 0 uint8 system_state 0=BOOTING , 1=OPERATIONAL , 2=SAFE_FAULT 1 uint8 system_health 0=OK , 1=WARNING , 2=FAULT
Byte 类 型 字 段 说 明 2~5 uint32 LE diag_bits 当 前 诊 断 位 图 6 uint8 cmd_age_10ms 距 最 近 ⼀ 次 合 法 0x100 已 过 去 多 少 个 10ms tick 7 uint8 status_counter 状 态 帧 发 送 计 数 器 5.2 0x182 实 际 轮 速 帧 ID ： 0x182 周 期 ： 轮 询 ， 约 60ms DLC ： 8 Byte 类 型 字 段 0~1 int16 LE FL 实 际 RPM 2~3 int16 LE RL 实 际 RPM 4~5 int16 LE FR 实 际 RPM 6~7 int16 LE RR 实 际 RPM 5.3 0x183 ⽬ 标 轮 速 帧 ID ： 0x183 周 期 ： 轮 询 ， 约 60ms DLC ： 8 Byte 类 型 字 段 0~1 int16 LE FL ⽬ 标 RPM 2~3 int16 LE RL ⽬ 标 RPM
Byte 类 型 字 段 4~5 int16 LE FR ⽬ 标 RPM 6~7 int16 LE RR ⽬ 标 RPM 5.4 0x184 通 信 诊 断 帧 ID ： 0x184 周 期 ： 100ms DLC ： 8 Byte 字 段 说 明 0 valid_cmd_total_lsb 合 法 命 令 累 计 低 8 位 1 crc_error_total_lsb CRC 错 误 累 计 低 8 位 2 counter_reject_total_lsb rolling counter 拒 收 累 计 低 8 位 3 can_tx_drop_total_lsb CAN 发 送 丢 帧 累 计 低 8 位 4 busoff_total_lsb Bus-Off 累 计 低 8 位 5 rx_overrun_total_lsb FIFO overrun 累 计 低 8 位 6 last_accepted_counter 最 近 ⼀ 次 接 受 的 rolling counter 7 err_nibbles ⾼ 4 位 = 连 续 counter 错 误 数 ； 低 4 位 = 连 续 CRC 错 误 数 ； 均 饱 和 到 15 5.5 0x200 ⾥ 程 增 量 帧 ID ： 0x200 周 期 ： 轮 询 ， 约 60ms DLC ： 8
Byte 类 型 字 段 0~1 int16 LE FL delta ticks 2~3 int16 LE RL delta ticks 4~5 int16 LE FR delta ticks 6~7 int16 LE RR delta ticks 说 明 ： 这 是 时 间 窗 内 增 量 不 是 累 计 总 值 上 位 机 如 需 总 ⾥ 程 / 总 编 码 器 计 数 ， 需 要 ⾃ ⾏ 累 加 积 分 6. 系 统 状 态 与 健 康 等 级 6.1 system_state 值 名 称 含 义 0 SYSTEM_BOOTING 上 电 后 尚 未 收 到 第 ⼀ 帧 合 法 速 度 命 令 1 SYSTEM_OPERATIONAL 正 常 ⼯ 作 2 SYSTEM_SAFE_FAULT 安 全 保 护 ， ⽬ 标 速 度已 强 制 清 零 6.2 system_health 值 名 称 含 义 0 SYSTEM_HEALTH_OK ⽆ 活 动 中 的 警 告 / 故 障 1 SYSTEM_HEALTH_WARNING 有 警 告 ， 但仍 可 ⼯ 作 2 SYSTEM_HEALTH_FAULT 有 明 确 故 障 ， 通 常已 经 或 应 进 ⼊ 保 护
7. diag_bits 诊 断 位 图 定 义 diag_bits 是 ⼀ 个 32 位 ⼩ 端 位 图 ， ⽬ 前 定 义 如 下 ： bit 宏 名 级 别 含 义 0 DIAG_COMM_TIMEOUT Fatal 速 度 控 制 帧 超 时 1 DIAG_CAN_BUS_OFF Fatal CAN Bus-Off 2 DIAG_CMD_CRC_STORM Fatal 连 续 CRC 错 误 过 多 3 DIAG_CMD_CNT_STORM Fatal 连 续 rolling counter 错 误 过 多 4 DIAG_MOTOR_FL_STALL Fatal 左 前 轮 堵 转 / 失 效 趋 势 5 DIAG_MOTOR_RL_STALL Fatal 左 后 轮 堵 转 / 失 效 趋 势 6 DIAG_MOTOR_FR_STALL Fatal 右 前 轮 堵 转 / 失 效 趋 势 7 DIAG_MOTOR_RR_STALL Fatal 右后 轮 堵 转 / 失 效 趋 势 8 DIAG_CONTROL_SATURATION Warning 控 制 输 出 ⻓ 时 间 顶 满 建 议 上 位 机 把 ： bit0 ~ bit7 视 为 故 障 类 bit8 视 为 警 告 类 8. 安 全 策 略 与 故 障 触 发 条 件 8.1 命 令 超 时 命 令 看 ⻔ 狗 超 时 阈 值 ： 150ms 只 有 合 法 且 新 鲜 的 0x100 才 会 刷 新 0x080 ⼼ 跳 不 会 刷 新 超 时 后 ： 进 ⼊ SAFE_FAULT
置 位 DIAG_COMM_TIMEOUT 8.2 CRC 错 误 ⻛ 暴 若 连 续 CRC 错 误 计 数 达 到 5 进 ⼊ SAFE_FAULT 置 位 DIAG_CMD_CRC_STORM 8.3 Counter 错 误 ⻛ 暴 若 连 续 counter 错 误 计 数 达 到 5 进 ⼊ SAFE_FAULT 置 位 DIAG_CMD_CNT_STORM 8.4 CAN Bus-Off 若 发 ⽣ Bus-Off 计 数 累 加 进 ⼊ SAFE_FAULT 置 位 DIAG_CAN_BUS_OFF 8.5 电 机 堵 转 固 件 每 10ms 做 ⼀ 次 电 机 堵 转 诊 断 。 若 某 轮 持 续 满 ⾜ 以 下 条 件 ： ⽬ 标 RPM >= 40 实 际 RPM <= 8 控 制 输 出 绝 对 值 >= 850 并 持 续 50 个 10ms tick （ 即 500ms ） ， 则 ： 置 对 应 轮 ⼦ 的 stall 位
进 ⼊ SAFE_FAULT 8.6 控 制 饱 和 若 存 在 任 ⼀ 轮 持 续 满 ⾜ ： ⽬ 标 RPM >= 30 控 制 输 出 绝 对 值 >= 980 并 持 续 20 个 10ms tick （ 即 200ms ） ， 则 ： 置 位 DIAG_CONTROL_SATURATION 该 项 属 于 Warning ， 不 单 独 强 制 进 ⼊ SAFE_FAULT 。 9. 上 位 机 实 现 建 议 9.1 速 度 命 令 发 送 建 议 上 位 机 按 如 下 ⽅ 式 实 现 ： 周 期 ： 20ms ID ： 0x100 rolling counter ： 每 帧 ⾃ 增 1 ， uint8 ⾃ 然 回 绕 CRC ： 每 次 发 送 前 重 新 计 算 Byte0~6 的 CRC8-SAE J1850 停 ⻋ 时 不 要 停 ⽌ 发命 令 ， 继续 发 ： vx = 0 wz = 0 推 荐 发 送 频 率 推 荐 ： 50Hz （ 20ms ） 可 接 受 ： 100Hz （ 10ms ） 不 建 议 低 于 10Hz 绝 不 能 超 过 150ms 不 发合 法 0x100
9.2 ⼼ 跳 发 送 ID ： 0x080 可 选 仅 ⽤ 于上 位 机 链 路 监 控 不 要 依 赖 它 维 持 运 动 9.3 状 态 接 收 建 议 重 点 订 阅 ： 0x181 ： 状 态 / 健 康 / 故 障 0x184 ： 通 信 统 计 以 及 按 需 订 阅 ： 0x182 ： 实 际 轮 速 0x183 ： ⽬ 标 轮 速 0x200 ： ⾥ 程 增 量 建 议 上 位 机 ⾄ 少 显 ⽰ ： system_state system_health diag_bits cmd_age_10ms crc_error_total_lsb counter_reject_total_lsb busoff_total_lsb 四 轮 实 际 RPM 四 轮 ⽬ 标 RPM 四 轮 增 量 ticks
10. 0x100 组 帧 参 考 伪代 码 counter = (counter + 1) & 0xFF vx_i16 = round(vx_mps * 1000) wz_i16 = round(wz_radps * 1000) data[0] = vx_i16 & 0xFF data[1] = (vx_i16 >> 8) & 0xFF data[2] = wz_i16 & 0xFF data[3] = (wz_i16 >> 8) & 0xFF data[4] = ctrl_flags data[5] = 0 data[6] = counter data[7] = crc8_j1850(data[0:7]) send(id=0x100, dlc=8, data=data)

192
Doc/code_review_report.md Normal file
View File

@@ -0,0 +1,192 @@
# 导航代码审查报告
日期: 2026-04-03
范围:
- `App/nav/global_nav.c`
- `App/nav/track_map.c`
- `App/nav/track_map.h`
- `App/robot_params.h`
- `App/app_tasks.c`
- `App/preproc/corridor_preproc.c`
## 结论
当前版本的 S 型拓扑和左右转向表整体上与地图理解一致，没有再发现明显的左右方向写反问题。
主要风险集中在以下几类：
- 时间基准过度依赖 IMU 时间戳
- 连续确认逻辑复用了同一帧 VL53 数据
- 入场段过度依赖固定起始摆放位置
- 阶段切换存在 1 个控制周期的旧命令残留
- 连接段提前转向策略较激进
## Findings
### 1. 高: IMU 时间戳卡住时，超时与里程都会冻结
位置:
- `App/nav/global_nav.c:430-445`
- `App/nav/global_nav.c:471`
- `App/nav/global_nav.c:628-630`
- `App/nav/global_nav.c:657-659`
现象:
- `GlobalNav_Update()` 用 `board->imu_wz.timestamp_ms` 作为内部时间基准
- `odom_distance_accum` 的积分和 `elapsed_ms` 的推进都依赖这个时间戳
风险:
- 如果 IMU 仍被判定为在线，但时间戳停更，导航状态机会继续输出控制命令
- 同时阶段超时保护不会推进
- 里程积分也不会推进
可能后果:
- `ENTRY_STRAIGHT`、`LINK_STRAIGHT`、`EXIT_STRAIGHT` 长时间不退出
- 转向超时失效，机器人可能持续原地转
说明:
- 这是行为级问题，不是单纯的调参问题
- 文档中也提到了该项仍是 TODO，但当前实现里确实已经构成运行风险
### 2. 高: “连续 N 拍确认”实际在重复消费同一帧 VL53 数据
位置:
- `App/app_tasks.c:285-286`
- `App/app_tasks.c:299-347`
- `App/nav/global_nav.c:516-523`
- `App/nav/global_nav.c:615-620`
- `App/robot_params.h:398-399`
现象:
- 导航循环周期约为 `20ms`
- VL53 任务推送周期约为 `100ms`
- `REACQUIRE` 的连续 `5` 拍确认和 `LINK_STRAIGHT` 的连续 `2` 拍确认，都是按导航循环计数
风险:
- 同一帧 VL53 观测会被导航层重复读取多次
- 于是“连续确认”并不等于“连续多个独立观测确认”
可能后果:
- `REACQUIRE` 可能只靠 1 帧侧向数据就进入 `CORRIDOR_TRACK`
- 沟口检测的 2 拍确认也可能只是一帧瞬时失效被重复消费
说明:
- 这会削弱你现在新设计的联合判定可靠性
- 当前问题核心不是阈值，而是采样独立性不足
### 3. 中: 入场段强依赖起始摆放位置，缺少几何确认
位置:
- `App/nav/global_nav.c:484-495`
- `App/robot_params.h:382-385`
- `App/app_tasks.c:302-306`
- `Doc/map.md:9`
- `Doc/map.md:53-59`
现象:
- `ENTRY_STRAIGHT` 现在只用 `里程 >= 0.30m 或 超时` 进入第一次右转
- 启动后直接 `GlobalNav_Start()`，没有专门的“出启动区口再开始计段”动作
风险:
- 这要求机器人初始位置必须比较稳定，且接近你假设的起跑点
可能后果:
- 如果车放在 100cm 深启动区内更靠后位置，可能在到达 `C1` 入口前就右转
- 如果车放得更靠前，也可能转得偏晚
说明:
- 当前实现修掉了“侧墙始终有效导致误触发”的问题
- 但引入了“对起点一致性要求很高”的新假设
### 4. 中: 阶段切换发生后，本周期仍可能执行旧阶段指令
位置:
- `App/nav/global_nav.c:242-266`
- `App/nav/global_nav.c:491-495`
- `App/nav/global_nav.c:623-625`
- `App/nav/global_nav.c:706-710`
现象:
- 若本周期内先生成了旧阶段控制命令，再满足切段条件并 `transition_to()`
- `out->stage` 在函数末尾会更新成新阶段
- 但本周期发出去的速度命令可能还是旧阶段的
风险:
- 状态显示与实际执行在一个周期内不完全一致
可能后果:
- 转向完成后多转一个控制周期
- 直行段满足切换条件后，当拍仍会继续向前推进一小段
说明:
- 这通常是 20ms 量级的小偏差
- 但在靠近入口边缘、转向容差较紧时会放大几何误差
### 5. 中: 连接段允许仅凭前激光位移提前触发下一次转向
位置:
- `App/nav/global_nav.c:590-625`
- `App/robot_params.h:406-409`
- `App/nav/track_map.h:36-37`
现象:
- `LINK_STRAIGHT` 的逻辑是 `B || (A && C)`
- 其中 `A` 和 `B` 都采用 `0.70m * 0.85 = 0.595m` 作为触发阈值
- 也就是前激光变化量到达约 `59.5cm` 就可直接触发转向
风险:
- 机器人可能在标称 `70cm` 沟间距之前约 `10.5cm` 就开始转向
可能后果:
- 若前激光初值记录稍晚、转出后航向略偏、或前激光看到的并非理想正对围栏面，可能提前转向
- 提前量叠加转向半径误差后，可能更接近垄背边缘而非下一条沟中心
说明:
- 这不是硬 bug，更像策略上偏激进
- 若实车转向余量很大，可能仍可工作；若几何余量小，则风险会明显上升
## 正向观察
### 1. 转向拓扑表当前与地图理解一致
位置:
- `App/nav/track_map.c:34-47`
说明:
- `C1` 右转入、左转出
- `C2` 左转入、右转出
- 奇偶沟交替
- `C6` 左转出场
这一版没有再看到此前那种第一条沟转向方向写反的问题。
### 2. 连接段已经避免把“贴围栏侧 VL53 常亮”当作入口触发
位置:
- `App/nav/global_nav.c:108-144`
- `App/nav/global_nav.c:557-625`
说明:
- 你已经把“非围栏侧 VL53 沟口检测”显式建模出来
- 同时结合前激光和里程计做联合判定
这比此前的 `side_walls_detected()` 直接触发要合理得多。
## 开放问题
1. 比赛摆车是否保证机器人车头在启动区出口附近，而不是启动区任意位置？
2. 传感器黑板中的 `is_valid` 是否有基于时间戳的失效机制，还是生产者停更后仍可能保持有效？
3. 连接段的目标是“到下一沟中心线再转”，还是“进入下一沟开口就允许转”？当前 0.85 容差会显著影响这个定义。
## 总体评价
当前版本比前一版明显更接近真实场地几何，尤其是：
- 地图方向理解正确
- S 型左右转表正确
- 连接段不再依赖错误的侧墙常亮判据
但如果从比赛稳定性角度看，当前还存在两个最值得优先处理的问题：
- 时间基准不能完全绑死在 IMU 时间戳上
- 连续确认不能重复消费同一帧侧向观测
如果这两点不处理，现场表现会比较依赖传感器健康状态与偶然时序，稳定性风险较高。

61
Doc/map.md Normal file
View File

@@ -0,0 +1,61 @@
```地图
比例尺: 1字符 = 10cm x 10cm
物理尺寸: X轴(横向)净宽 300cm, Y轴(纵向)净深 390cm
图例说明:
[#] : 赛场实体围栏 (外部边界)
[.] : 垄沟/平地通道 (左侧40cm, 右侧40cm, 垄间40cm)
[=] : 垄背凸起 (长220cm, 宽30cm)
[S] : 比赛启动区 (宽40cm, 深100cm，紧邻左侧通道)
X: 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
Y: ------------------------------------------------------------------
0 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
1 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
2 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
3 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
4 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
5 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
6 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
7 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
8 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
9 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
10 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
11 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
12 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
13 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
14 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
15 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
16 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
17 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
18 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
19 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
20 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
21 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
22 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
23 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
24 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
25 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
26 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
27 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
28 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
29 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
30 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
31 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
32 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
33 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
34 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
35 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
36 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
37 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
38 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
39 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
40 | # S S S S # # # # # # # # # # # # # # # # # # # # # # # # # # #
41 S S S S
42 S S S S
43 S S S S <-- 启动区长度 100cm (共10格深)
44 S S S S
45 . . . .
```

212
Doc/turning_imu_risk_analysis.md Normal file
View File

@@ -0,0 +1,212 @@
# 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`
这套策略对单沟模式是强需求，对赛道模式是推荐增强项。

507
Doc/大体方案.md Normal file
View File

@@ -0,0 +1,507 @@
# ASER 平台 B 方案走廊相对定位实现与模块化解耦技术报告
**执行摘要**：本报告围绕你选择的 **B 方案（走廊相对定位 Corridor-relative Estimation）**，在不破坏现有 ASER 工程结构与 **STM32H743（上位机）↔ STM32F407（底盘）既有 CAN 协议**的前提下，给出一套可落地、可扩展、可“先跑通再变强”的实现路线。核心思想是：不把比赛当作“绝对坐标 SLAM”，而把赛场 40 cm 窄通道视为拓扑段落序列，实时估计 **横向偏差 e_y、航向误差 e_θ、沿通道进度 s**，并用侧向测距闭环实现“居中/偏置行走 + 对齐”，以对抗 **赛场尺寸 ±5% 误差、地毯导致的轮滑、测距偶发失效**（比赛规则明确存在地毯模拟松软路面、且尺寸允许误差 ±5%）。同时，将 **0x100 速度指令 20 ms 周期硬实时发送**作为最高优先级工程约束，任何导航/融合/日志都不得影响其节拍（否则底盘可能进入 SAFE_FAULT）。赛场层面，规则要求 **5 分钟限时**且未完全驶出与未停在启动区内可判 0 分，因此系统优先级必须是 **安全与完赛闭环 > 定位美观 > 路径最优**。（赛事通道/出入口 40 cm、启动区、5 分钟限制、尺寸误差 ±5%：见《附件6…比赛规则》1–3 页与 14 页；上位机 CAN 协议关键帧：见《通讯协议》2–4 页、8–10 页；ASER 工程任务/协议层约束：见《aser》1–5 页。）
## 系统约束与现状梳理
### 赛场与任务约束对算法与工程的“硬约束”
比赛场地为 **390 cm × 300 cm**，共有 5 条田垄；围栏与田垄之间、以及相邻田垄之间均为 **40 cm 通道（垄沟）**，出入口宽 **40 cm**，出入口外侧紧邻 **40 cm × 100 cm 启动区**；比赛 **限时 5 分钟**，且规则强调必须驶离场地并停在启动区，否则可能判 0 分；尺寸允许误差 **±5%**（意味着通道宽可能约 38–42 cm，任何“贴边”策略都必须考虑裕量）。这些约束直接决定控制目标应是“别蹭边、别失控、能跑完”，而非追求全局米级地图的一致性。（《附件6…比赛规则》1–3 页、14 页）
### 现有硬件与软件约束
以下为你给定的固定约束（本报告按此设计）：
- 车体：**20×20 cm**，轮厚 **3 cm**；**差速四轮结构**（四电机）。
- 控制器三块：
- **STM32F407（底盘）**：已实现四轮 **LADRC**，通过 CAN 与上位机通信。
- **STM32H743（主控/上位机）**：传感器融合、底盘与机械臂控制（现状上位机）。
- **K230（视觉中枢）**：用于视觉语义/目标等（本报告将其作为增强/降级输入，不进入硬实时链路）。
- ASER 工程形态（来自你上传的 ASER 文档）：基于 STM32CubeMX + FreeRTOS（CMSIS-V2），已建立 **20 ms canTxTask** 与 **100 ms monitorTask**；CAN 协议层在 `App/Can/snc_can_app.c/.h`，并明确要求保持既有协议/中断接收链路，新增功能优先放到 `App/` 目录以避免再生成覆盖；FDCAN 接收链路保持 IRQ→回调→协议解析的单路径。（《aser》1–5 页）
### 既有 CAN 协议是“不可破坏”的硬边界
你已确认 H7↔F4 通信协议“已经制定好”，因此本文不再“重新设计协议”，而是把协议当作 **契约（contract）**：
- 0x100 为速度命令，**DLC=8**，包含 `vx_x1000`、`wz_x1000`、rolling counter、CRC8；要求 **建议 20 ms** 周期持续发送，且 **不得超过 150 ms 不发送合法 0x100**，否则底盘进入 SAFE_FAULT；心跳帧 0x080 **不刷新运动看门狗**。（《通讯协议》2–4 页、8–10 页）
这些要求决定：**所有算法模块必须围绕“20 ms 严格出 v/ω”来组织调度与解耦**。
## 传感器清单、性能与可观测性分析
### 传感器清单（按你给定配置）
- **4 电机编码器**：提供四轮增量/速度，可构造差速里程计，并可用于同侧前后轮差异的 **轮滑检测**（工程上很关键）。
- **高精度单轴 IMU（ω_z 或 θ）**：你给定“短时漂移可忽略”。这对 B 方案非常有利：可把“短段航向保持/原地转向闭环”做得很稳，同时简化滤波状态维度（但仍需讨论风险与退路，见后文）。
- **8 个测距模块**：前后左右各 2 个：
- 左右侧向：**VL53L0X V2**（最远 2 m）。ST 官方产品页说明 VL53L0X 为 ToF 测距，最大可到 **2 m**。citeturn2search3
同时其官方数据手册给出典型“Range profile”：High speed 20 ms、Long range 33 ms、High accuracy 200 ms 等 timing budget 配置（对你做“关键动作更稳、巡航更快”的动态配置非常实用）。citeturn2search41
- 前后向（每端 2 个）：**STP-23L（7 cm–7.5 m）** + **ATK‑MS53L1M（4 m）**。其中 4 m 量程模块工程上高度可能基于 VL53L1X 类 ToF：ST 官方 VL53L1X 产品页给出“up to 4 m、up to 50 Hz”能力，可作为你对 4 m 级传感器的性能参考上限。citeturn0search0
- 结构：**差速四轮** → 上位机输出 (v, ω) 即可驱动底盘；底盘闭环已由 F407 LADRC 负责。
### B 方案的可观测性：为什么侧向双点能同时观测 e_y 与 e_θ
走廊相对定位的关键是：**只关心“相对墙/垄侧”的横向与航向误差**，不强求全局 (x,y)。只要左右两侧各有前后两个测距点（你是每侧 2 个 VL53L0X），就可以构造两个稳定的派生量测：
- **航向误差 e_θ（相对走廊方向）**：利用同侧前后距离差与传感器纵向基线 \(L_s\)，可用
\[
e_{\theta,L} \approx \arctan\left(\frac{d_{Lf}-d_{Lr}}{L_s}\right),\quad
e_{\theta,R} \approx \arctan\left(\frac{d_{Rf}-d_{Rr}}{L_s}\right)
\]
该构造与你上传的《大体方案》对 B 方案与派生量测的定义一致（《大体方案》4–5 页）。
- **横向误差 e_y（相对中心线/偏置线）**：用左右平均间隙差得到
\[
e_y \approx \frac{1}{2}( \bar d_L - \bar d_R) - y_{\text{offset}}
\]
其中 \(y_{\text{offset}}\) 是你可以显式引入的“偏置行走”目标（例如机械臂在右侧外凸时令车辆略偏左，降低擦碰风险）。同样与《大体方案》对 B 方案的描述一致（《大体方案》4–5 页）。
前后向测距（STP-23L / 4 m 模块）主要用于：**到端触发（段落切换）**、安全制动、以及对沿程 \(s\) 的事件校正（见后文）。
## 走廊相对定位与融合算法设计
### 问题建模与坐标系
定义走廊（或垄沟）局部坐标系 \(\{C\}\)：
- \(x_C\)：沿走廊前进方向；
- \(y_C\)：走廊横向（向左为正）；
- 车辆车体坐标 \(\{B\}\)：\(x_B\) 前、\(y_B\) 左。
B 方案输出状态：
\[
\mathbf{x}_c = [e_y,\ e_\theta,\ s]^T
\]
其中：
- \(e_y\)：车辆参考点（建议用车体几何中心或“控制点”）相对走廊目标线（中心线或偏置线）的横向偏差；
- \(e_\theta\)：车辆航向相对走廊方向的误差；
- \(s\)：沿走廊进度（m），用于段落终止触发/动作编排，不作为“强闭环精确位姿”。
### 传感器融合总体策略：分层、分频、强约束
建议采用“三层融合与控制”：
- **层 A（硬实时输出层，20 ms）**：必须产出 (v_cmd, ω_cmd) 并发送 CAN 0x100。无论任何模块异常，这层都不能停摆（最多把速度降到 0 并持续发送合法 0x100）。这是由底盘运动看门狗与超时机制决定的：0x080 心跳不刷新看门狗，只有合法 0x100 刷新；超时阈值 150 ms。 （《通讯协议》2–4 页、8–10 页）
- **层 B（相对定位与走廊闭环，建议 50–100 Hz）**：估计 \(e_y,e_\theta,s\)，并形成走廊跟随控制律；允许降级（只用单侧、只用 IMU 航向保持等），但必须在可控范围内给出指令。
- **层 C（段落状态机/任务决策，10–20 Hz）**：执行段脚本（Segment List）、处理到端触发、重定位行为（扫描/后退重试/停车保护）、以及与机械臂/视觉的协同。
这样做的目的：**把“安全与完赛”从定位精度中解耦**。
### 状态估计实现方案对比：从“极简互补”到“鲁棒 EKF/UKF”
你明确提出希望覆盖 EKF/UKF/粒子滤波。结合 STM32H743 算力与工程风险，本报告建议按阶段实现：
#### 极简方案：互补滤波 + 鲁棒观测融合（优先推荐做 P0）
利用你给定“短时 IMU 漂移可忽略”，可以先不引入复杂滤波矩阵，直接做：
- 航向误差 \(e_\theta\)：
- 预测：\(e_{\theta,k|k-1}=e_{\theta,k-1} + \omega_z \Delta t\)
- 校正：用侧向差分观测 \(e_{\theta,L}, e_{\theta,R}\) 做加权融合（按健康度/置信度权重），再与预测做互补融合：
\[
e_\theta \leftarrow \alpha(e_\theta + \omega_z\Delta t) + (1-\alpha)\cdot e_{\theta,\text{meas}}
\]
- 横向误差 \(e_y\)：
- 直接由 \(\frac{1}{2}(\bar d_L-\bar d_R)-y_{\text{offset}}\) 给出，并做一阶低通（根据 VL53L0X timing budget 动态调整滤波带宽；20 ms 档噪声较大、200 ms 档更稳但延迟更大，可从数据手册的 profile 及误差差异得到工程依据）。citeturn2search41
- 沿程 \(s\)：
- 用编码器里程计积分为主（来自 CAN 0x200 的轮增量 ticks，你需要在 H743 侧累加积分）。 （《通讯协议》5–7 页）
- 用前向测距做“事件校正”：当 \(d_\text{front}\) 小于阈值（接近端部围栏）触发段切换，并把 \(s\) 校正到“段长 - d_front - 安装偏置”。
优点：实现快、数值稳定、适合先把整车跑通；缺点：难以严格输出协方差，也不便于统一做马氏门限。
#### 鲁棒 EKF（推荐做 P1）
若你希望更系统地融合并获得一致的健康度/协方差，则做小维 EKF：
- 状态：\(\mathbf{x}=[e_y,e_\theta,s]^T\) 或扩展为 \([e_y,e_\theta,s,b_g,k_s]\)（陀螺零偏、轮滑比例因子）以吸收轮滑/地毯误差（《大体方案》4 页提到类似扩展思路）。
- 预测模型（简化差速运动学在走廊局部系下的表达）：
\[
\begin{aligned}
e_{y,k+1} &\approx e_{y,k} + v_k\sin(e_{\theta,k})\Delta t \\
e_{\theta,k+1} &\approx e_{\theta,k} + (\omega_{z,k}-b_{g,k})\Delta t \\
s_{k+1} &\approx s_k + v_k\cos(e_{\theta,k})\Delta t
\end{aligned}
\]
其中 \(v_k\) 来自里程计估计或上一周期命令（两者在轮滑情况下要区别对待）。
- 量测模型：
- \(z_{e_y}=\frac{1}{2}(\bar d_L-\bar d_R)-y_{\text{offset}}\)
- \(z_{e_\theta}\)：由左/右侧差分观测（可单侧/双侧）
- \(z_s\)：端部事件约束（触发式更新）
- **异常值剔除（创新门限/χ²）**：
用马氏距离（innovation 的归一化残差）做门限，d² 超限则拒绝该测距更新、降低该传感器健康度，并触发降级/重定位动作。创新卡方检验型鲁棒 EKF 在导航融合领域是成熟手段；例如武汉大学学报给出“基于创新卡方检验的扩展鲁棒卡尔曼滤波”并说明可有效抑制观测粗差、提高稳定性。citeturn5search1
同时马氏距离平方在高斯假设下与 χ² 分布的关系，可作为你选择阈值的理论依据。citeturn5search0
#### UKF（备选 P1/P2）
UKF 的优势是减少雅可比推导错误风险，对非线性更稳健；其系统性论述可参考 Julier & Uhlmann 的综述文章（Proceedings of the IEEE, 2004）。citeturn4search8
但对 MCU 工程而言，UKF 的实现细节（sigma 点、数值稳定、协方差正定维护）仍需严格测试。建议在 EKF 稳定后再引入 UKF，或仅在仿真/离线回放中验证。
#### 粒子滤波（不建议作为主线）
粒子滤波更适合多峰分布或强非线性/非高斯，但你这里的走廊状态空间维度小、观测约束强、且 MCU 实时性要求更硬，粒子滤波的收益通常不如鲁棒 EKF/UKF 明显。建议仅在“视觉地标 + 多段拓扑歧义”场景出现时再考虑。
### 控制律：走廊闭环优先，轨迹跟踪为辅
走廊段推荐直接闭环 \(e_y,e_\theta\) 输出角速度：
\[
\omega_\text{cmd}=k_\theta e_\theta + k_y e_y
\]
线速度：
\[
v_\text{cmd}=\min(v_\text{ref},\ v_\text{safety})
\]
其中 \(v_\text{safety}\) 由安全层根据前向距离与最小侧向间隙硬约束裁剪（见下节）。
对于“入场对准、退出与停回启动区”等非典型走廊段，可以上层用段脚本触发“短距离轨迹跟踪”（如 Pure Pursuit），但其输出仍应统一进入 **命令仲裁器**，最终被安全层裁剪后送给 CAN 0x100。
### 安全层、异常检测与重定位策略
这是 B 方案能否“比赛不判零”的关键（规则对失控/冲出场地会强制罚下，且未驶出/未停在启动区可能 0 分）。《附件6…比赛规则》1、8–9 页
#### 传感器健康度与一致性检查
建议对每个测距传感器维护 `health_score∈[0,1]` 与 `state∈{OK,SUSPECT,FAIL}`，并用以下规则更新（工程可解释、易调参）：
- **有效性**：超量程/无数据/固定值卡死 → 直接 FAIL（可加 N 次确认）。
- **跳变**：\(|d_k-d_{k-1}|>\Delta d_\text{max}\) 且与冗余传感器不一致 → SUSPECT。
- **走廊一致性**：
- 左右平均距离差过大（\(e_y\) 过大）接近阈值 → 触发降速/停车；
- 左右和与走廊宽度模型严重不符（考虑 ±5% 尺寸误差）→ 可能进入入口/出口开阔区或测距误读，切换“入口/开阔区模式”。
#### 轮滑检测（地毯场景必须做）
规则明确会在垄沟中随机铺设地毯模拟松软路面（《附件6…比赛规则》2–3 页），强烈建议把轮滑检测当作“安全与融合共同输入”：
- 同侧前后轮编码器差异过大（四轮提供更多冗余）
- \(|\Delta\theta_\text{enc} - \Delta\theta_\text{imu}|\) 超阈值（IMU 短时漂移可忽略时，这个检测更可靠）
轮滑触发后动作：
- 降低 \(v_\text{max}\)、限制加速度；
- 融合层增大过程噪声 \(Q\) 或降低对里程计的信任；
- 控制层更依赖侧向闭环（\(e_y,e_\theta\)），减少对 \(s\) 的硬依赖。
#### 重定位/恢复动作链（建议用段脚本固化）
当“走廊观测不可用”或“误差超过安全阈值”时，不要让系统继续前冲；建议固化为可重复执行的恢复链：
1. **低速前进 + 侧向对齐**：尝试恢复 \(e_\theta\) 可观测（左右/单侧）。
2. **原地旋转扫描**：转动寻找两侧墙面，使 \(e_y/e_\theta\) 可观测。
3. **后退重试或停车保护**：连续失败 N 次后退出（比赛是否允许等待/重试由你们策略决定，但从“避免失控罚下”角度，停车保护是最后底线）。
这类恢复链在你上传的《大体方案》中已有雏形（《大体方案》13 页），建议工程化为状态机（NORMAL/DEGRADED/RECOVERY/STOP_SAFE）。
### “IMU 短时漂移可忽略”的简化与风险
你给定该条件，可以显著简化实现（例如可不立刻估计 \(b_g\)），但必须明确风险边界：
- 风险来源：温漂、振动、饱和、安装误差、长时间累计偏置。短时可忽略 ≠ 全程可忽略。
- 工程缓解：
- 开机静止校零（估计陀螺零偏）；
- 在走廊段用侧向差分观测周期性校正 \(e_\theta\)（即使不用 EKF，也做互补融合）；
- 若引入 EKF/UKF，建议把 \(b_g\) 作为可选状态并在“长段/高温漂”时开启。
## 通讯协议与硬实时调度约束
### CAN 报文契约（按现有协议原样遵守）
你上传的《通讯协议》给出了当前固件实装版协议要点（标准帧 11-bit、DLC、字节序、缩放、CRC8 等），这里整理为“实现必须满足的契约”：
- **0x100 Velocity Command（H743→F407）**
- DLC=8；`vx,wz` 按 `int16 = 物理量 × 1000`（小端）；包含 `rolling_counter` 与 `CRC8-SAE J1850`（对 Byte0~6 计算，放 Byte7）。
- 推荐周期 **20 ms**；**绝不能超过 150 ms 不发送合法 0x100**（即使停车也继续发 vx=0,wz=0 的合法帧）；rolling counter 在正常运行时要求与上一帧差值在 1..3，否则拒收。
（《通讯协议》2–4 页、8–10 页）
- **0x080 Heartbeat（H743→F407）**：仅表示链路活着，**不刷新运动看门狗**，不能替代 0x100。 （《通讯协议》2–3 页、8–10 页）
- **0x181 Status（F407→H743，20 ms）**：包含 `system_state、system_health、diag_bits、cmd_age_10ms` 等，用于安全降级决策。 （《通讯协议》4–5 页）
- **0x184 Comm Diag（F407→H743，100 ms）**：统计 CRC 错误、counter 拒收、bus-off 等，必须纳入诊断与回放。 （《通讯协议》6 页）
- **0x200 Odom Delta（F407→H743，轮询约 60 ms）**：四轮增量 ticks；上位机需自行累积。 （《通讯协议》5–7 页）
> 说明：你已表示协议既有且固定，因此本报告不建议在 H7↔F4 CAN 上新增“破坏兼容”的帧。若未来要加扩展帧，建议在不影响现有 ID/节拍/接收逻辑的前提下做“可选附加 ID”，并通过版本位/能力位协商（但这属于后续扩展，不是当前 P0 必需）。
### 优先级、周期与超时处理（工程实现建议）
- **最高优先级**：`can_tx_0x100_task`（20 ms，硬实时，禁止被日志/融合阻塞）。
ASER 工程现有结构已采用 20 ms 的 canTxTask 且强调绝对节拍（osDelayUntil）与协议层保持（《aser》1、5 页）。
- **第二优先级**：CAN Rx 中断回调（接收 0x181/0x184/0x200），以“快照/无锁或短临界区”方式写入共享上下文，避免长时间占用 ISR。
- **第三优先级**：控制/估计（50–100 Hz），必须保证“在下一个 0x100 截止前产出最新命令”，但即便估计超时，也要有“上一帧命令 + 安全裁剪”的保底。
- **监控任务（100 ms）**：读取 `cmd_age_10ms、diag_bits、comm diag`，若发现异常趋势（例如 DIAG_COMM_TIMEOUT、bus-off、连续 CRC 错误风暴）立即触发上位机 STOP_SAFE 模式并把 (v,ω) 置零，但仍持续发合法 0x100。 （《通讯协议》8–10 页）
为实现稳定周期，建议使用 FreeRTOS 的 `vTaskDelayUntil()`（按绝对时间唤醒，适合固定频率周期任务）。citeturn3search34
在 ISR→任务的事件通知上，任务通知（`vTaskNotifyGiveFromISR`）通常比信号量更轻，FreeRTOS 手册亦将其描述为更快的替代机制。citeturn2search42
### 关键消息帧示例（十六进制 + 结构体）
下面给出 **0x100** 的一个可复现实例（示例：vx=0.30 m/s，wz=0.80 rad/s，ctrl_flags=0x01，counter=0x10；CRC8=SAE J1850，对 Byte0~6 计算）：
- `vx_x1000 = 300 = 0x012C`（小端：2C 01）
- `wz_x1000 = 800 = 0x0320`（小端：20 03）
- 组合 Byte0~6：`2C 01 20 03 01 00 10`
- 计算得到 `CRC8 = 0xA1`
- 最终 8 字节：
**`2C 01 20 03 01 00 10 A1`**
对应 C 结构体（注意小端序与打包）：
```c
#pragma pack(push,1)
typedef struct {
int16_t vx_x1000; // Byte0-1, m/s * 1000
int16_t wz_x1000; // Byte2-3, rad/s * 1000
uint8_t ctrl_flags; // Byte4
uint8_t reserved; // Byte5, fixed 0
uint8_t rolling_counter; // Byte6
uint8_t crc8; // Byte7, CRC8-SAE J1850 over Byte0..6
} CanCmdVel_0x100_t;
#pragma pack(pop)
```
（0x100 字段定义、CRC 规则、rolling counter 规则、150 ms 超时：见《通讯协议》2–4 页、8–10 页。）
## 软件模块化与解耦方案
### 解耦目标与原则
在 ASER 现有工程约束下（协议层/中断链路/任务框架尽量不动），解耦的关键是把系统切成三类模块：
1. **硬实时链路模块**：唯一职责是“稳定发 0x100”，只从一个“命令槽（Command Slot）”读取最新的 (v,ω)；不做复杂计算、不打印日志。
2. **可实时模块**：估计、控制、预处理，允许在极端情况下掉周期，但必须提供保底输出与超时策略。
3. **非实时模块**：日志、回放、仿真接口、参数调试、统计；永不阻塞硬实时。
ASER 文档明确：CAN 协议层位置固定，并建议新功能放 App/ 目录，且 canTxTask/monitorTask 已存在。（《aser》1–5 页）
### 数据流总览（mermaid）
```mermaid
flowchart LR
subgraph S[传感器/底盘输入]
VL53[VL53L0X x4 侧向测距]
FRONT[前向测距: STP-23L + 4m模块]
BACK[后向测距: STP-23L + 4m模块]
IMU[单轴IMU ωz/θ]
CANRX[CAN Rx IRQ: 0x181/0x184/0x200]
end
subgraph P[预处理与健康度]
PRE[时间戳对齐/滤波/有效性检测/健康度]
OBS[派生观测构造: e_y_meas, e_theta_meas, d_front, d_back]
end
subgraph E[估计与控制]
EST[走廊相对定位滤波器\n(x=[e_y,e_θ,s] 或扩展)]
SEG[段脚本解释器 + 拓扑状态机]
CTRL[走廊控制器/原地转向/入口对准]
SAFE[安全监督器/降级状态机/命令仲裁]
end
subgraph R[硬实时输出]
CANTX[CAN Tx Task\n0x100 @20ms]
end
subgraph L[工具链]
LOG[日志记录/回放/故障注入]
SIM[仿真/传感器注入接口]
end
VL53 --> PRE
FRONT --> PRE
BACK --> PRE
IMU --> PRE
CANRX --> PRE
PRE --> OBS --> EST
CANRX --> EST
EST --> CTRL
SEG --> CTRL
CTRL --> SAFE --> CANTX
CANRX --> SAFE
SAFE --> LOG
OBS --> LOG
EST --> LOG
SIM --> PRE
```
### 模块清单、接口消息、周期与最大延迟（必须表格）
下表给出“建议模块化拆分”，并明确每个模块 I/O、周期与延迟预算。**注意：开发语言、CPU 负载预算未指定**；下表的资源估计按 STM32H743 “小维滤波 + 50–100 Hz 控制”属于轻量任务的常见工程经验给出，最终应以运行时 profile 校准。STM32H743 的 480 MHz Cortex‑M7 与双精度 FPU、TCM RAM 能力，为小维滤波提供了硬件基础。citeturn1search1
| 模块 | 职责 | 输入（消息/接口） | 输出（消息/接口） | 典型周期 | 最大延迟（建议） | 资源估计（H743） |
|---|---|---|---|---|---|---|
| CAN协议适配层（既有） | 按既有协议收发、CRC、上下文维护（禁止破坏兼容） | FDCAN IRQ | `ChassisStatus(0x181)`、`CommDiag(0x184)`、`OdomDelta(0x200)` | IRQ/20ms/轮询 | <2 ms 解析写入 | 低；保持原样 |
| CAN Tx 0x100 硬实时任务 | 固定 20 ms 发送合法 0x100，任何情况下不断流 | `CmdSlot(v,w,flags)` | CAN 0x100 | **20 ms** | **<1 ms 抖动** | 极低；最高优先级 |
| 编码器里程计模块 | ticks 累加、四轮→差速等效、轮滑检测特征 | `OdomDelta(0x200)` | `OdomEst{ds, v, yaw_enc}`、`SlipFeat` | 50–100 Hz（或随0x200） | <20 ms | 低 |
| IMU采集模块 | 采集 ωz/θ、时间戳、静止校零 | SPI/I2C/串口(未指定) | `ImuZ{wz, dt}` | 200–500 Hz（建议） | <5 ms | 低 |
| VL53侧向驱动 | 4 个 VL53L0X 轮询/异步读取、模式切换（20/33/200ms） | I2C | `RangeSideRaw` | 20–50 Hz（视 timing budget） | <40 ms | 低到中（I2C占用） |
| 前后测距驱动 | STP-23L 与 4m 模块读取、冗余校验 | UART/I2C(未指定) | `RangeFrontBackRaw` | 20–50 Hz | <50 ms | 低到中 |
| 预处理与健康度 | 对齐时间戳、滤波、有效性、跳变检测、一致性/走廊模式识别 | Raw ranges、IMU、Odom | `CorridorObs{e_y_meas,e_th_meas,d_front,d_back,valid}`、`SensorHealth` | 50–100 Hz | <20 ms | 低 |
| 走廊相对定位滤波 | 互补/EKF/UKF 输出 e_y/e_θ/s 与置信度 | `CorridorObs`、`ImuZ`、`OdomEst` | `CorridorState{e_y,e_th,s,cov/conf}` | 50–100 Hz | <20–40 ms | 低（小维） |
| 段脚本解释器 | Segment List 执行：走廊段/原地转向/退出停车/失败恢复 | `CorridorState`、`SensorHealth` | `SegmentCmd{mode,v_ref,y_offset,end_trigger}` | 10–20 Hz | <100 ms | 低 |
| 控制器集合 | 走廊闭环、原地转向、入口对准等 | `SegmentCmd`、`CorridorState` | `RawCmd{v,w}` | 50–100 Hz | <20 ms | 低 |
| 安全监督器/仲裁 | 急停/限速/降级/模式切换；融合底盘 diag_bits/cmd_age | `RawCmd`、0x181/0x184、ranges | `CmdSlot(v,w,flags)`、`SafetyState` | 50–100 Hz | **<20 ms**（到0x100） | 低 |
| 日志与回放 | 记录观测/状态/命令/故障；支持离线回放复现实验 | 全部关键消息 | log文件/串口输出 | 5–20 Hz（批量写） | 不得阻塞实时链路 | 中（取决于介质） |
| 测试仿真/注入接口 | PC 仿真或回放注入 ranges/odom/imu；用于HIL | 离线数据/串口/USB | 注入到 PRE | 非实时 | 不影响0x100 | 低 |
**接口命名建议**：把“CAN 帧”与“内部消息”严格区分，例如 `CanCmdVel_0x100_t` 与 `CmdSlot`，避免上层直接依赖 CAN 打包细节，从而实现解耦与可测试性。
## 实现步骤与里程碑
### 分阶段实施步骤清单（可操作）
由于赛期时间表“未指定”，下面以“无特定期限”的工程顺序给出建议；若你们有明确赛期，可按周压缩/并行。
**阶段 P0：跑通闭环（目标：能稳定走通道、不撞、不中断 0x100）**
1. 固化 **0x100 20 ms** 发送硬实时：把 canTxTask 设为最高优先级，只从 `CmdSlot` 取值；加 watchdog/统计，任何异常也持续发送 vx=0,wz=0 的合法帧。（《通讯协议》2–4 页、8–10 页；《aser》1、5 页）
2. 接入 CAN Rx：0x181/0x184/0x200 解析快照写入上下文，monitorTask 先只做可视化打印与超时报警。（《通讯协议》4–7 页；《aser》1–5 页）
3. 侧向 VL53L0X 驱动：先用固定 profile（例如 33 ms 或 30 ms 档），跑出稳定的 `d_Lf,d_Lr,d_Rf,d_Rr`；必要时在关键动作切换到 200 ms 高精度档（官方数据手册给出 profile 与 timing budget）。citeturn2search41
4. 前后测距驱动：至少保证一个“前向安全距离”可用，先实现安全限速/急停。
5. 实现派生观测 \(e_y,e_\theta\) 并闭环：先用互补滤波/低通 + 走廊控制律，让车在 40 cm 通道内稳定跑直线与到端停车。
6. 段脚本最小集：CorridorFollow / TurnInPlace / ExitAndStop（动作少但可跑完全程）。
**阶段 P1：鲁棒性与可调参**
1. 加入健康度、异常检测、降级状态机：单侧可用时切“单侧贴边 + IMU 航向约束”；双侧不可用时低速+停车保护。
2. 加入轮滑检测与自适应策略（地毯段必需）：轮速差与 IMU/里程计不一致触发降速与融合降权。（地毯存在：见《附件6…比赛规则》2–3 页）
3. 引入鲁棒 EKF（或对互补滤波加入创新门限）：用 χ²/马氏距离拒绝异常测距，参考创新卡方检验鲁棒 EKF 的工程化做法。citeturn5search1turn5search0
4. 完善日志与回放：把“每次撞/每次抖动”都能离线复现，支撑快速迭代。
**阶段 P2：增强与赛场集成**
1. K230 输出“结构化低带宽特征”作为增强/降级输入：例如走廊中心偏差、地标识别结果；K230 的多核 RISC‑V 与 KPU 能力适合做推理后输出特征量，而不承担 20 ms 硬实时闭环。citeturn6search3
2. 若需要局部绕障（比赛道具/土块干扰可能导致异常行为），可做“小范围 DWA 风格速度采样”，但务必以安全层为前提；DWA 原始思想是“在速度空间搜索可停、避障、前进最优的 (v,ω)”。citeturn4search0
### 仿真工具与场景建议
未指定你们必须使用 ROS/某仿真器，因此给出三档可选：
- **轻量快速（推荐 P0/P1）**：Python/Matlab 自建 2D 走廊仿真（差速模型 + 墙面 ToF 测距模型 + 噪声/丢包/跳变），用于调 \(k_y,k_\theta\)、异常门限、降级策略；优点是迭代极快、与 B 方案高度匹配。
- **中等复杂（可选）**：Webots/Gazebo 的差速车模型 + 简化距离传感器，验证入口/转向等几何行为。
- **硬件在环 HIL（推荐 P1/P2）**：
- H743 跑真实控制栈；
- 通过“仿真注入接口”把 ranges/imu/odom 注入 PRE 模块（测试编译开关），同时真实发送 CAN 0x100 给 F4，看 F4 是否稳定接受、是否出现 counter/CRC 风暴、cmd_age 是否异常。（《通讯协议》8–10 页）
### 里程碑甘特图（示例：无特定期限，按 6 周节奏展示）
```mermaid
gantt
title ASER B方案（走廊相对定位）里程碑示例（无特定期限）
dateFormat YYYY-MM-DD
axisFormat %m-%d
section P0 跑通（先安全后精度）
0x100 20ms硬实时不掉帧 + 基础安全层 :a1, 2026-03-18, 7d
CAN Rx接入(0x181/0x184/0x200)+里程计 :a2, after a1, 7d
VL53侧向+前后测距驱动 + e_y/e_θ观测 :a3, after a2, 10d
走廊控制器 + 最小段脚本(走廊/转向/退出) :a4, after a3, 10d
section P1 稳定与容错
健康度/异常检测/降级状态机 :b1, after a4, 7d
轮滑检测与自适应降速/融合降权 :b2, after b1, 7d
鲁棒EKF/互补+χ²门限（可选） :b3, after b2, 7d
section 集成与验证
日志回放/自动化测试/故障注入 :c1, after a3, 14d
HIL联调 + 赛场流程演练 + 参数冻结 :c2, after b3, 14d
```
## 性能指标与验证方法
### 建议性能指标（赛前可再定阈值）
结合 40 cm 通道与车辆 20 cm 外形，工程上建议把“安全裕量”明确成指标：
- **走廊横向控制**：
- \(e_y\) RMS（直通道匀速 0.2–0.4 m/s）：建议目标 < 10–20 mm（取决于传感器安装与轮滑程度）；
- \(e_y\) 峰值：不得触发“最小侧向间隙”保护线。
- **航向对齐**：\(e_\theta\) RMS < 1–2°（入口/转向后重新进入走廊的恢复时间也应记录）。
- **端到端延迟**：从侧向测距采样到 0x100 生效的闭环延迟建议 < 40 ms（两周期）；0x100 发送抖动应远小于 20 ms 周期。
- **鲁棒性**：单侧 VL53 临时失效（例如 0.5–1 s）仍能保持不碰撞；前向测距失效时必须降速并可安全停车。
- **完赛相关**：在 5 分钟限制下完成“驶出场地 + 停在启动区”动作（规则强调未完成可 0 分）。 （《附件6…比赛规则》1 页）
### 数据采集需求（支撑可复现实验）
建议每条日志至少包含：
- 时间戳（单调时钟）、段 ID、模式（NORMAL/DEGRADED/RECOVERY/STOP_SAFE）
- 原始测距（8 路）、有效标志、健康度
- 派生观测 \(e_y,e_\theta,d_\text{front},d_\text{back}\)
- 估计状态（\(e_y,e_\theta,s\) 与协方差/置信度）
- 输出命令（RawCmd 与最终 CmdSlot）、0x100 counter/CRC 统计
- 底盘状态（0x181 system_state/health/diag_bits/cmd_age）与 0x184 通信统计
（这些字段在协议中均可获得：见《通讯协议》4–7 页。）
### 单元测试与整车测试用例示例
**单元测试（Host 或 MCU 上跑，推荐“输入→输出可验算”）**
- UT-EST-001：给定理想走廊、已知 \(L_s\)、固定 \(e_\theta\)，构造 \(d_{Lf},d_{Lr}\) 并验证 \(e_{\theta,L}\) 反解误差 < 阈值。
- UT-GATE-001：给定 EKF 创新与协方差，构造离群点，验证 χ² 门限拒绝逻辑；门限选择可参考创新卡方检验鲁棒 EKF 文献。citeturn5search1
- UT-CAN-001：0x100 打包：vx/wz 缩放、小端序、CRC8-J1850 校验与 rolling counter 递增规则；确保 counter 跳变策略不会触发底盘拒收。（《通讯协议》2–4 页）
- UT-SAFE-001：前向距离 < d_stop → v=0；cmd_age_10ms 超阈值或 diag_bits 出现致命位 → STOP_SAFE 并持续发 0x100=0。（《通讯协议》4–5 页、8–10 页）
**整车测试（建议可回归）**
- IT-COR-001（入场对准）：启动区→40 cm 出入口→进入第一通道；指标：不触碰、最大横向误差、耗时。（赛场尺寸：见《附件6…比赛规则》2 页）
- IT-COR-002（走廊直行）：匀速 0.2/0.3/0.4 m/s，统计 \(e_y\) RMS/峰值、\(e_\theta\) RMS。
- IT-END-001（到端触发）：接近端部围栏，验证触发准确率与停车距离一致性。
- IT-SLIP-001（地毯轮滑）：在地毯段运行，验证轮滑检测触发率、降速策略与不碰撞。（地毯：见《附件6…比赛规则》2–3 页）
- IT-DROP-001（侧向遮挡）：遮挡一只 VL53 或一侧两只，验证降级切换时间与稳定性。
- IT-CAN-FAULT-001（CAN 抖动/延迟注入）：验证即使系统负载上升也不会超过 150 ms 不发合法 0x100。（《通讯协议》8–10 页）
## 风险与替代方案
### 主要风险点与缓解
- **通道极窄 + 尺寸误差 ±5%**：通道宽可能缩到约 38 cm，若车辆外廓/轮子外露估计不足，居中也可能刮擦。缓解：把“最小侧向间隙”作为硬安全约束，并支持按段设置 \(y_\text{offset}\)。 （尺寸误差：见《附件6…比赛规则》14 页）
- **测距盲区/遮挡/高反射干扰**：VL53L0X 在高速档噪声更大；缓解：关键动作切高精度 timing budget（200 ms）并降低速度；按 datasheet profile 做“模式-噪声-速度”联动。citeturn2search41
- **前后测距冗余冲突**：两种前向传感器可能在某些目标材质/角度下给不同值；缓解：做一致性选择（例如取更保守的近距离值）+ 门限剔除。
- **地毯轮滑导致里程计失真**：规则明确存在地毯（《附件6…比赛规则》2–3 页）。缓解：轮滑检测触发后降低对里程计的信任、降速并强化侧向闭环。
- **CAN 带宽与实时性**：若日志/调试占用 CPU 或关中断过久，可能导致 0x100 抖动/丢帧；缓解：日志异步队列+独立低优先级 logger task；0x100 发送任务最高优先级，且即使停车也持续发合法帧。（0x100 硬约束：见《通讯协议》2–4 页、8–10 页）
- **计算资源不足**：若引入过重的规划（全局优化/TEB 等），会侵蚀硬实时；缓解：主线坚持 B 方案小维估计+简单控制；局部避障仅做轻量 DWA 风格采样，且可选。citeturn4search0
### 替代/增强方案
- **A 方案度量定位 EKF/UKF 作为评估与恢复辅助**：在不依赖其做走廊闭环的前提下输出 (x,y,θ) 与协方差，用于段落切换一致性检查与日志评估（《大体方案》4 页）。
- **K230 视觉作为测距降级备份**：K230 的双核 RISC‑V + KPU 适合推理后输出结构化特征，作为走廊偏差/地标观测源之一。citeturn6search3
- **局部避障（可选）**：若赛场土块/障碍导致必须绕行，可引入 DWA 思想做速度空间采样；其原始工作在 IEEE Robotics and Automation Magazine 1997 年文章中系统阐述。citeturn4search0
## 代码生成与 AI 协作建议
### 为什么必须“接口契约优先”
你明确提到多数代码将由 AI 生成。要让 AI 代码可控，必须先把系统拆成“可替换模块 + 明确消息契约 + 可回归测试”。否则 AI 很容易在“看似能跑，但破坏实时性/破坏协议/破坏并发安全”的地方踩雷。
ASER 文档中已经明确：协议层与接收链路不要随意改动，新逻辑应放 App/ 并遵守现有任务框架。（《aser》1–5 页）
### 建议的仓库结构与代码模板（示例）
建议在 `App/` 下新增（不动协议层）：
- `App/sensors/`：`vl53_driver.c/.h`、`stp23l_driver.c/.h`、`ms53_driver.c/.h`、`imu_z.c/.h`
- `App/preproc/`：`corridor_obs.c/.h`、`health_monitor.c/.h`
- `App/est/`：`corridor_filter.c/.h`（互补/EKF/UKF 可切换）
- `App/nav/`：`segment_fsm.c/.h`、`corridor_ctrl.c/.h`、`safety_supervisor.c/.h`
- `App/log/`：`logger_task.c/.h`、`replay.c/.h`
- `App/test/`：host 可编译的纯 C 测试（或 Unity/Ceedling/CppUTest）
**接口契约头文件（强制先写）**：例如 `corridor_msgs.h`
```c
#pragma pack(push,1)
typedef struct {
uint32_t t_ms;
float d_lf, d_lr, d_rf, d_rr; // meters
float d_front, d_back; // meters
uint8_t valid_mask; // bitfield
} CorridorObs_t;
typedef struct {
uint32_t t_ms;
float e_y; // meters
float e_th; // radians
float s; // meters
float conf; // 0..1 or covariance proxy
} CorridorState_t;
typedef struct {
uint32_t t_ms;
float v; // m/s
float w; // rad/s
uint8_t flags;
} RawCmd_t;
#pragma pack(pop)
```
### 单元测试规范（让 AI 代码“可验收”）
- **每个模块至少 3 类测试**：正常、边界、异常（离群/丢包/超时）。
- **所有纯算法模块必须可在 Host 编译运行**（不依赖 HAL），把硬件依赖封装成接口（如 `read_range()` 函数指针或 mock）。
- **协议相关测试必须比对字节级输出**：0x100 的 CRC/rolling counter/小端序是典型“AI 容易写错但编译不过不报错”的点。（《通讯协议》2–4 页）
### 自动化生成与审查流程建议
- 先由你/团队写“模块接口契约 + 时序约束表 + 禁止事项”（如“0x100 发送任务不得阻塞”），再让 AI 生成实现。
- CI 至少包含：
1) Host 单元测试；
2) 静态检查（clang-tidy/cppcheck 任选）；
3) 构建固件；
4) （可选）HIL 脚本：注入传感器回放数据，检查 0x100 发送间隔最大值与 counter 连续性。
- Code review checklist（必须人工看）：
- 是否出现长时间关中断/在高优先级任务里打印；
- 是否对共享上下文无保护读写导致撕裂；
- 是否更改了 `snc_can_app` 协议定义/接收链路（ASER 文档明确不建议破坏）。 （《aser》1–5 页）
---
**参考资料（你上传的原始文档）**
- 《附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则》：赛场尺寸、40 cm 通道/出入口、地毯、5 分钟限时、尺寸误差 ±5%（1–3 页、14 页）。
- 《通讯协议》：现有 H7↔F4 CAN 协议、0x100 20 ms/150 ms 超时、CRC8/rolling counter、0x181/0x184/0x200 定义与诊断位（2–10 页）。
- 《aser》：ASER 工程结构、FreeRTOS 任务（20 ms/100 ms）、协议层位置与“不要破坏接收链路/协议层”的约束（1–5 页）。
- 《大体方案》：B 方案状态与派生量测构造、走廊闭环思想、降级/恢复链与实施计划（4–5 页、13–16 页）。
**参考资料（官方/原始文档与论文）**
- ST VL53L0X 产品页（最大 2 m）：citeturn2search3
- ST VL53L0X 数据手册（timing budget 与 profile：20 ms/33 ms/200 ms 等）：citeturn2search41
- ST VL53L1X 产品页（up to 4 m、up to 50 Hz）：citeturn0search0
- STM32H743（480 MHz Cortex‑M7、双精度 FPU、TCM 等特性）：citeturn1search1
- FreeRTOS `vTaskDelayUntil()`（固定频率周期任务的绝对时间阻塞）：citeturn3search34
- FreeRTOS `vTaskNotifyGiveFromISR()`（ISR→任务通知）：citeturn2search42
- 创新 χ² 检验鲁棒 EKF（武汉大学学报）：citeturn5search1
- 马氏距离与 χ² 门限关系、鲁棒策略示例：citeturn5search0
- UKF（Julier & Uhlmann 综述条目）：citeturn4search8
- DWA 原始工作（Fox/Burgard/Thrun 1997）：citeturn4search0
- K230 官方文档（CPU0 Linux/CPU1 RTOS、KPU INT8/INT16 等）：citeturn6search3

1500
Doc/实施方案.md Normal file
View File

File diff suppressed because it is too large Load Diff

549
Doc/混合导航方案.md Normal file
View File

@@ -0,0 +1,549 @@
# 混合导航方案
## 1. 文档目的
本文档用于明确本项目后续正式比赛版导航应采用的总体方案。
目标不是重写当前全部导航代码，而是：
1. 保留现有“垄沟内局部闭环控制”能力
2. 在其上补齐赛道级状态机与段间动作编排
3. 让机器人能够按照固定地图完成 6 条垄沟的 S 型遍历
4. 最终从唯一出口驶离并停回启动区
本文档强调的是“混合导航”：
- 上层使用固定地图和拓扑状态机决定现在该去哪
- 中层使用动作执行器完成转向、连接段推进、再入沟
- 下层使用现有局部传感器闭环完成沟内稳定行驶
它不是纯局部反应式导航，也不是通用 SLAM。
## 2. 已知场地理解
根据 `Doc/map.md`：
- 场地净尺寸约为 `300cm x 390cm`
- 内部有 `5` 条田垄
- 因围栏与田垄、田垄与田垄之间均有通道，所以可通行垄沟实际为 `6` 条
- 启动区位于场地下侧靠左，外接唯一入口
- 各条垄沟是横向分布的长通道
- 垄沟间通过左右两端的短连接段串起来
因此，比赛中的真实轨迹不是“在端部横移搜索下一条沟”，而是：
1. 从启动区进入场地
2. 沿入口直线段前进
3. 到第 1 条垄沟入口附近
4. 原地转 `90°` 入沟
5. 沿垄沟通过
6. 到端后原地转 `90°`
7. 走一小段连接直线
8. 再原地转 `90°` 入下一条垄沟
9. 重复以上动作，形成 **S 型遍历**
10. 全部垄沟完成后离场并回停启动区
## 3. 为什么要做混合导航
当前项目已经具备较强的局部能力：
- 4 路侧向 VL53 做走廊观测
- IMU 提供 `wz` 和 `yaw_continuous`
- EKF / Filter 输出 `e_y`、`e_th`、`conf`
- `corridor_ctrl` 输出沟内控制指令
- `segment_fsm` 负责安全裁剪
- `nav_script` 能做单段脚本验证
但这套能力本质上仍然偏向：
- “单条垄沟怎么跑稳”
- 而不是
- “整张赛道下一步该去哪里”
正式比赛需要解决的核心问题有：
1. 当前正在第几条垄沟
2. 下一条应该进入哪条垄沟
3. 当前应该左转还是右转
4. 什么时候从沟内控制切到端部动作
5. 什么时候从端部动作切回沟内控制
6. 什么时候结束全部遍历并离场
7. 离场后如何回停到启动区
这些问题无法只靠局部测距瞬时值回答，必须引入上层任务状态。
## 4. 混合导航的核心思想
本项目推荐采用：
**固定地图 + 赛道级状态机 + 局部闭环控制**
其中：
- 固定地图负责描述赛道结构
- 状态机负责描述任务推进
- 局部闭环负责把当前这一小段走稳
整体思路是：
- 用地图回答“接下来去哪”
- 用状态机回答“现在该做什么动作”
- 用传感器闭环回答“这一段怎么安全稳定地过去”
## 5. 三层架构
### 5.1 上层：赛道级导航层
职责：
- 记录当前 `corridor_id`
- 决定下一个目标 `target_corridor_id`
- 决定当前阶段
- 决定下一步是左转还是右转
- 在所有阶段之间推进任务
这层不直接控制车轮，只输出“当前应该执行哪种段动作”。
### 5.2 中层：段动作执行层
职责：
- 入场直线推进
- `90°` 原地转向
- 连接段直线推进
- 再次 `90°` 入沟
- 出场段动作
- 回停启动区动作
这层输出当前周期的期望 `v/w`，但仍需经过安全层裁剪。
### 5.3 下层：局部闭环控制层
职责：
- 在垄沟内保持居中
- 控制 `e_y` 和 `e_th`
- 提供局部重捕获判据
- 在当前段可观测时给出稳定闭环
这一层尽量复用现有实现，不重复发明轮子。
## 6. 当前代码与未来架构的对应关系
现有代码可保留并复用的部分：
1. `App/preproc/`
- 继续负责传感器清洗与观测构造
2. `App/est/`
- 继续负责 `e_y / e_th / conf` 估计
3. `App/nav/corridor_ctrl.c`
- 继续负责沟内局部控制
4. `App/Contract/robot_blackboard.*`
- 继续作为全局传感器快照中心
5. `App/Contract/robot_cmd_slot.*`
- 继续作为导航输出到 CAN 的命令槽
6. `App/nav/segment_fsm.*`
- 保留为安全层，但后续必须增加“动作语义感知”
当前不应再承担最终比赛全局职责的部分：
1. `App/nav/nav_script.c`
- 当前更像“单垄沟验证脚本”
- 不适合继续膨胀成完整赛道导航总控
因此后续应新增赛道级模块，而不是把全部逻辑继续堆进 `nav_script.c`。
## 7. 推荐状态机建模
建议把赛道任务拆成以下大阶段：
1. `START_ZONE`
- 启动区待发
2. `ENTRY_STRAIGHT`
- 从启动区经唯一入口进入场地
- 沿左侧入口直线段前进
3. `TURN_INTO_CORRIDOR`
- 到目标垄沟入口后原地转 `90°`
- 对准目标垄沟
4. `CORRIDOR_TRACK`
- 沟内闭环跟踪
- 使用现有 `corridor_ctrl`
5. `TURN_OUT_AT_END`
- 到达当前垄沟末端
- 原地转 `90°` 转向连接段
6. `LINK_STRAIGHT`
- 沿端部连接段直行一小段
- 用 IMU 保持航向
- 用里程计或事件触发控制推进
7. `TURN_INTO_NEXT_CORRIDOR`
- 原地转 `90°`
- 对准下一条垄沟
8. `REACQUIRE_CORRIDOR`
- 低速确认两侧 VL53 是否重新形成合理走廊结构
- 成功后切回 `CORRIDOR_TRACK`
9. `EXIT_FIELD`
- 全部垄沟完成后，朝唯一出口离场
10. `DOCK_START_ZONE`
- 回到启动区并停车
11. `FINISHED`
- 比赛结束
## 8. 赛道级核心状态量
建议赛道级层显式维护以下变量：
- `current_corridor_id`
- `target_corridor_id`
- `total_corridor_count = 6`
- `travel_direction`
- `turn_side`
- `stage`
- `stage_progress`
- `next_turn_is_left`
- `is_final_exit_phase`
- `reacquire_confirm_count`
其中最关键的是：
- 当前在第几条沟
- 下一条是哪条沟
- 这次入沟应该左转还是右转
- 当前处于哪个动作阶段
## 9. 传感器参数与角色分工
### 9.1 左右 VL53L0X
已知参数：
- 每侧 2 个，共 4 个
- 主要用于侧向测距
- 精确测量距离按当前工程经验取 **1.2m 以内**
- 当前已由人工完成标定，但单点测距仍存在约 **±1cm** 的偏差
适合：
- 沟内居中
- 入沟重捕获确认
不适合：
- 作为 `yaw / e_th` 的主观测来源
- 远距离搜索下一条沟入口
- 独立完成赛道级导航
设计含义：
- `VL53` 是近场几何约束传感器
- 只能在“已经接近某条沟”时帮你锁住这条沟
- 不能把“下一条沟在哪里”这个问题压给它
- 由于单点误差量级约为 `±1cm`，同侧前后差分法对噪声非常敏感
- 因此不推荐继续用 `VL53` 前后差分直接计算 `yaw`，航向应主要依赖 `IMU`
### 9.2 前后 STP-23L
已知参数：
- 前后各 1 个
- 有效测距范围 **7cm ~ 7.5m**
适合：
- 到端检测
- 前后安全边界监测
- 开阔区边界辅助判定
- 某些段落的事件触发
不适合：
- 独立判断当前位于哪条垄沟
设计含义：
- `STP` 是远距离边界感知传感器
- 它适合回答“前面/后面还有多远”“是否接近端部或围栏”
- 不适合承担精细入沟定位
### 9.3 前后 ATK-MS53L1M
已知参数：
- 前后各 1 个
- 有效测距范围 **4cm ~ 3.9m**
适合：
- 近距离补盲
- 填补 STP 在近端盲区的不足
- 近场防撞保护
设计含义：
- `ATK` 不是主导航传感器
- 它的核心价值是让前后边界感知在近距离不断层
- 在转向、再入沟、靠近围栏时很重要
### 9.4 IMU
适合：
- 原地转 `90°`
- 连接段航向保持
- 无侧墙阶段的短时姿态约束
### 9.5 编码器 / 里程计
适合：
- 连接段推进量估计
- 段落推进计量
- 动作超时和距离上限保护
注意：
- 地毯和打滑会影响绝对精度
- 不能单独作为最终入沟确认依据
## 10. 各传感器在混合导航中的分工原则
建议按下面的分工使用传感器：
1. 沟内阶段
- 主用：左右 `VL53` 做横向约束，`IMU` 做航向约束
- 辅助：前后激光仅做安全和到端检测
2. 转向阶段
- 主用：`IMU yaw_continuous`
- 辅助：前后激光做安全保护
3. 连接段阶段
- 主用：`IMU + 里程计`
- 辅助：前后 `STP/ATK` 做边界与防撞
4. 再入沟阶段
- 主用：左右 `VL53`
- 辅助：`IMU` 做姿态稳定，前后激光做安全兜底
一句话总结：
- `VL53` 负责“锁住局部走廊”
- `IMU` 负责“航向约束和跨过无墙约束阶段”
- `里程计` 负责“推进量”
- `STP/ATK` 负责“边界和安全”
## 10.1 关于航向观测的专项说明
当前侧向 `VL53L0X` 虽然已经完成标定，但单点测距仍有约 `±1cm` 偏差。
这个精度对于：
- 居中控制
- 左右偏移判断
- 重新捕获一条沟
通常是够用的。
但如果把它直接用于航向估计，例如用同侧前后距离差去推导 `yaw / e_th`，会遇到两个问题：
1. 同侧前后差分属于“小量减小量”，对噪声天然敏感
2. 当前 `±1cm` 的单点误差已经足以让差分航向观测明显抖动
因此推荐原则是：
- `VL53` 负责横向约束和重捕获
- `IMU wz + yaw_continuous` 负责航向估计与转向控制
- 不再把 `VL53` 作为 `yaw` 主观测
## 11. 动作执行原则
### 11.1 沟内阶段
- 主要依赖侧向 VL53 做横向闭环，IMU 做航向闭环
- 使用 `corridor_ctrl`
- 安全层负责限速和急停
### 11.2 转向阶段
- 主要依赖 IMU `yaw_continuous`
- 目标是稳定完成 `90°`
- 安全层不能再沿用普通“前方太近则整段全停”的逻辑
- 必须允许 `v=0, w!=0` 的受限原地转向
### 11.3 连接段阶段
- 主要依赖 IMU 保持连接段朝向
- 使用里程计推进
- 接近预计入口后降速
- 前后 `STP/ATK` 负责边界辅助与防撞
- 进入重捕获阶段等待局部结构恢复
### 11.4 重捕获阶段
判据建议包括：
- 左右两侧 VL53 同时有效
- 左右几何关系符合 40cm 垄沟模型
- `conf` 高于阈值
- 持续若干拍成立
只有重捕获成功后，才允许切回沟内闭环。
## 12. 推荐新增模块
建议新增以下模块。
### 12.1 `App/nav/global_nav_fsm.c/.h`
职责：
- 维护整场比赛任务阶段
- 管理 `corridor_id`
- 决定下一步目标段
- 向下游发布当前动作类型
### 12.2 `App/nav/track_map.c/.h`
职责：
- 固化比赛地图拓扑
- 保存各条垄沟、连接段、入口、出口的相对关系
- 提供“当前完成哪条后下一条是谁”的规则查询
### 12.3 `App/nav/lane_transition.c/.h`
职责：
- 执行端部出沟、连接段推进、再入沟
- 内部管理两个 `90°` 转向和一段连接直线
### 12.4 `App/nav/reacquire_detector.c/.h`
职责：
- 负责判断是否已重新进入目标垄沟
- 对 VL53 几何结构和 `conf` 做持续判定
### 12.5 `App/nav/heading_hold.c/.h`
职责：
- 在无侧墙阶段提供短时航向保持
- 可独立实现，也可并入 `lane_transition`
### 12.6 `App/nav/exit_dock.c/.h`
职责：
- 负责最终离场与启动区停车
## 13. 推荐修改的现有模块
### 13.1 `segment_fsm`
必须补：
- 动作模式输入
- 区分：
- 沟内前进
- 原地转向
- 连接段推进
- 出场段直线
- 否则正式比赛阶段会在端部动作上卡死
### 13.2 `nav_script`
建议定位调整为：
- 临时验证脚本
- 单段测试脚本
- 或过渡期动作编排器
不建议继续作为最终赛道总控。
### 13.3 `corridor_msgs`
应补充：
- 赛道级阶段枚举
- 动作模式枚举
- 重捕获结果结构
- 赛道级状态输出结构
## 14. 推荐实施顺序
### 第 1 步：补底层动作语义
先修好：
- 原地转向安全逻辑
- 局部控制与安全层语义一致性
- 局部测试模式与可观测性
目标：
- 让“走沟、转向、连接直行”都能单独稳定测试
### 第 2 步：加入赛道级状态机
新增：
- `global_nav_fsm`
- `track_map`
目标：
- 系统明确知道“当前第几沟、下一沟是谁、这次该左转还是右转”
### 第 3 步：加入段间动作执行器
新增：
- `lane_transition`
- `heading_hold`
- `reacquire_detector`
目标：
- 从一条沟末端稳定过渡到下一条沟入口并重新入沟
### 第 4 步：补最终出场与回停
新增：
- `exit_dock`
目标：
- 让整场流程闭环，不只是在 6 条沟之间来回
### 第 5 步：统一参数、日志和调试接口
目标：
- 可调
- 可观测
- 可复现
- 可在实地快速定位问题
## 15. 一句话结论
本项目后续不应继续按“单沟脚本补丁”方式扩展。
正确方向应是：
**用固定地图描述赛道，用赛道级状态机管理 S 型遍历，用动作执行器完成两次 90° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。**

133
Doc/近墙猛修问题.md Normal file
View File

@@ -0,0 +1,133 @@
# 近墙猛修问题记录
## 现象
- 在两侧均为约 `40cm` 墙壁的测试环境中，只要车辆初始摆位明显偏向某一侧，就会出现明显的向另一侧猛打方向现象。
- 具体表现为：
- 车身非常靠近左侧墙时，会明显向右转。
- 车身非常靠近右侧墙时，会明显向左转。
- 当距离墙面非常近时，修正会过猛，看起来像“突然拧过去”。
## 当前判断
- “靠左往右修、靠右往左修”这个方向关系本身是符合居中控制逻辑的，不是转向方向定义反了。
- 真正的问题在于：侧向 VL53 在近墙区域的观测仍然被当作可信几何信息使用，导致横向误差 `e_y` 被一次性拉得过大，控制器再把这个大误差直接转换成较大的转向角速度。
- 因此，当前故障更接近“近墙观测过激 -> 状态估计偏大 -> 控制输出过猛”，而不是“状态机无故乱转”。
## 代码链路
### 1. 侧向 VL53 近墙仍可能被判有效
文件：`App/preproc/corridor_preproc.h`
- 当前侧向最小有效距离为：`PREPROC_MIN_SIDE_RANGE_M = 0.02f`
- 也就是只要大于 `2cm`，观测就可能继续进入后续 EKF。
这对 40cm 通道、20cm 车体来说偏激进，因为车辆一旦贴边，`2cm ~ 3cm` 这个区间很容易出现安装偏差、角度误差、近距非线性等问题。
### 2. EKF 直接把侧向距离变成横向偏差观测
文件：`App/est/corridor_ekf.c`
核心逻辑：
```c
float d_center = (W - Rw) / 2.0f + inset;
if (left_ok) {
z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff;
}
if (right_ok) {
z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff;
}
```
- 在当前参数下，理论居中时单侧读数大约为 `0.10m`。
- 如果左侧因为近墙读到 `0.02m ~ 0.03m`，则 `z_ey` 会立刻表现为“明显偏左”，量级可达数厘米。
- 只要该观测没有被马氏距离检验拒绝，就会被吸收到状态 `e_y` 中。
### 3. 控制器直接用 `e_y` 生成转向命令
文件：`App/nav/corridor_ctrl.c`
```c
float w_cmd = -(s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * imu_wz
+ s_cfg.kp_y * state->e_y);
```
- 当 `e_y` 被近墙观测拉得过大时，`w_cmd` 会立刻增大。
- 因此表现出来就是：
- 贴左墙时明显向右修。
- 贴右墙时明显向左修。
- 靠得非常近时会“修得过猛”。
## 为什么这不是“方向反了”
- 如果控制方向反了，现象应该是：靠左还继续往左，靠右还继续往右。
- 实际观测恰好相反，说明控制方向是对的，只是近墙时误差量级过大。
## 与此前“上电就左转/右转”的关系
- 之前怀疑过状态机提前切换、IMU 保直、前向激光异常等路径。
- 结合实车现象后，当前更值得优先处理的是：
- 车辆一旦被放在明显偏向某一侧的位置，系统会把它当成正常走廊纠偏问题来处理。
- 当侧向 VL53 处于近墙区时，这个纠偏会被放大得过于激进。
这意味着当前问题更像“近墙过激纠偏”，而不是“无条件自主转向”。
## 高概率根因
1. 侧向 VL53 的最小可信距离阈值过低，近墙失真数据仍被当作有效观测。
2. EKF 对单拍横向观测 `z_ey` 缺少足够的近墙限幅或退化策略。
3. 控制层对“近墙高风险状态”没有额外保险，导致 `e_y` 一大就直接猛打方向。
## 建议修复方向
### 方案 A：预处理层加近墙保护
- 提高侧向最小可信距离阈值，不再把极近距离观测直接当正常数据使用。
- 或新增“近墙退化区”，在该区间内降低观测可信度，而不是简单二值有效/无效。
这是最前面的止血点。
### 方案 B：EKF 观测限幅
- 对 `z_ey` 或单拍新息 `y_ey` 增加限幅。
- 防止单次近墙异常读数把 `e_y` 猛拉到过大值。
这是最直接针对“状态一下被拉偏”的修复。
### 方案 C：控制层保险
- 在近墙状态下限制 `w_cmd` 的幅度或变化速度。
- 同时降低线速度，避免“边贴墙边急拧”。
这是最后一道保险，不应单独依赖，但建议保留。
## 建议优先级
1. 预处理层近墙保护
2. EKF 横向观测限幅
3. 控制层近墙保险
## 建议调试观测量
后续复现时建议同步观察以下量：
- 四颗侧向 VL53 原始距离
- `obs.valid_mask`
- `obs.d_lf / d_lr / d_rf / d_rr`
- `corridor_state.e_y`
- `corridor_state.e_th`
- `raw_cmd.w`
- `safe_w`
重点确认：
- 近墙时是否有某一侧距离突然跌到极小值。
- `e_y` 是否在同一时刻明显跃迁。
- `raw_cmd.w` 是否跟着立刻增大。
如果这三者时间上连续对应，就可以基本坐实当前分析。

27
Doc/附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md Normal file
View File

@@ -0,0 +1,27 @@
- 1 - “奥凯杯”第十一届国际大学生智能农业装备创新大赛 B 类“马铃薯捡拾机器人竞技”比赛及评审规则 一、比赛规则要点 1 、 马铃薯捡拾机器人可采用垄间作业或跨垄作业模式。 垄间作业模式， 机器人在比赛过程中需遍历所有 6 条垄沟；跨垄作业模式，机器人作业幅宽 只允许跨一个垄背，且比赛过程中需遍历所有 5 个垄背。 2 、比赛需要作业，每支队伍有两次机会，成绩取最优者。参赛机器人需 进行马铃薯捡拾作业，以作业速度和作业效果进行综合成绩评判。比赛中无 作业动作的参赛机器人，比赛成绩以 0 分计。 3 、比赛限时 5 分钟（含违章加罚比赛用时）。从 100 秒预备时间已到 之后，评委发出“起跑”命令后，机器人开始跨越起跑线开始计时，到参赛 机器人所有部位都离开比赛场地出入口终止计时。 在限定时间内未完成比赛 者，比赛成绩以 0 分计。 4 、 比赛结束后， 参赛机器人须驶离场地， 任何部位不得停留在场地内， 否则视为未完成比赛。离开场地后需要自主停在比赛启动区，未自主停止的 视为未完成比赛，自主停止但未完全停在比赛启动区的视为连续超界。 5 、各参赛单位可以派出多支参赛队。但每支参赛队都必须根据比赛要 求，自行设计（或组装）、制作各自的参赛机器人。限定每支参赛队只能有 1 台机器人参赛。 6 、比赛分专本组和硕博组两类，以队伍中学历最高者为分类依据。
- 2 - 二、比赛场地及作业要求 1 、 比赛场地 如图 1 所示。比赛场地为 390cm × 300cm 的区域，共有 5 条田垄。 场地四周用高 12cm 的围栏围住，只留有一个宽 40cm 的出入口，围栏 与田垄之间留有宽 40cm 的通道（垄沟），垄长 220cm 、垄宽 30cm 、垄 高 12cm ，相邻田垄之间也留有宽 40cm 的通道（垄沟）。出入口外侧紧 邻比赛场地的 40cm × 100cm 的区域为比赛启动区，尺寸如图 1 所示： 图 1 比赛场地简图 围栏和田垄可采用钢质或木制材料制作， 田垄为灰色， 围栏为黑色； 地面采用爬行垫铺设，爬行垫标准（ 1. 颜色：灰色； 2. 材质： PE ； 3. 尺
- 3 - 寸：采用 60cm*60cm 标准尺寸进行拼接）。现场比赛时会从 6 条垄 沟中随机抽取两条垄沟，在其中放置宽 40cm 、长 300cm （即与垄沟等 长）的卡其色地毯，以模拟松软路面，地毯材质为丙纶，底部为防滑网 格底，厚度约 5.5mm ，所有场地毛毯布置均相同。如图 2 所示。 图 2 爬行垫及地毯 2 、马铃薯 马铃薯用榉木实心仿真鸽子蛋代替，有棕色和绿色 2 种，分别用 奶黄色和青绿色油漆喷涂，尺寸规格为外径 30mm ，长度 42mm ，重 量约 14g 。具体规格和形状如图 3 所示。
- 4 - 图 3 马铃薯示意图（单位 mm ） 3 、土块 土块使用黄色原积木代替， 因现实中有各种颜色土块， 为使比赛 更接近现实，故允许土块有色差，尺寸为 1.5cm 的立方体，具体如 图 4 。
- 5 - 图 4 土块 4 、 作业要求 参赛机器人需自行设计马铃薯载运装置，比赛时需将马铃薯从 田垄上捡拾起来放到小车上，比赛结束时将其运到比赛启动区，期 间马铃薯不能掉落。 5 、比赛场地布置 每条田垄放置若干颗马铃薯和若干颗土块， 在靠近出入口侧的田垄 长度中心线上每隔 20cm 做标记点，马铃薯在标记点处放置，方向为其 处长度方向与田垄长度中心线 45 °范围内任一方向；土块围绕马铃薯 随机放置，放置范围为以标记点为中心点，长 70mm 、高 75mm 的矩形 区域内随机放置。具体如下： ①专本组 ： 每条田垄放置 2 颗棕色马铃薯、 每颗马铃薯附近随机放 置 4 颗土块，同一级别比赛马铃薯放置标记点位置相同。
- 6 - ②硕博组： 每条田垄放置 2 颗棕色马铃薯、 1 颗绿色马铃薯，绿色 马铃薯为干扰组，不得捡拾，每颗马铃薯附近随机放置 4 颗土块，同一 级别比赛马铃薯放置标记点位置相同，角度在规定范围内随机。 赛前组委会会设计 5 种硕博组场地布置方案并进行密封处理，决 赛现场由裁判或参赛队员现场随机抽取 1 种方案进行布置；其中专本 组绿色马铃薯位置不做任何布置，棕色马铃薯放置标记点位置与硕博 组相同。 三、比赛规则 比赛根据参赛队员中最高学历者设置专本组和硕博组 2 个级别， 分开比赛和统计成绩。每支队伍都有两次机会。 1. 比赛规则 ①根据参赛队伍情况， 设置多块比赛场地。 比赛前组委会会随机为 各参赛队分配一块场地， 各比赛场地内的参赛队抽签确定比赛顺序， 同 一队伍两次机会结束后， 下一队伍开始比赛。 各场地同时进行比赛。 同 一级别比赛的每块比赛场地毛毯、马铃薯的放置位置均相同。 ②比赛成绩根据参赛机器人“作业得分” “走过的通道数” “能 否驶出场地”“能否停在启动区”和“比赛用时”等进行评比。 ③限时 5 分钟内（含违章加罚比赛用时）未完全驶出场地的比 赛成绩为 0 分； ④驶出场地但所有部位都未停止在比赛启动区内（不含启动区 标线）的成绩为 0 分；
- 7 - 2. 相关概念界定 ● 有效作业： 参赛机器人发现某个须捡拾马铃薯，控制作业装置对 其进行作业， 将马铃薯放置到小车上， 且过程中不掉落， 则视为一次有 效作业。 ● 无效作业： 参赛机器人发现某个须捡拾马铃薯，控制作业装置对 其进行作业， 但未将马铃薯放置到小车上， 或者放到小车上但比赛过程 中掉落，则视为一次无效作业。 ● 漏 捡： 参赛机器人在作业过程中未对某个须捡拾马铃薯进行捡拾 作业，则视为一次漏施。 ● 误捡： 参赛机器人在比赛过程中对绿色马铃薯进行作业，致其发 生位移或捡拾成功，则视为一次误捡。 ● 损伤 马铃薯 ： 参赛机器人或作业装置的任何动作导致某个马铃薯 上产生划痕（长度≥ 1cm ）等肉眼可见损伤 ，视为损伤。 ● 损伤、 损坏 2 颗及以内马铃薯等情况从已得作业分数中扣相应 分数； ● 损坏 马铃薯 ： 参赛机器人或作业装置的任何动作导致使马铃薯发 生结构断裂、或使其形状发生严重变形等，视为损坏。 ● 损伤、损坏 3 颗及以上马铃薯，取消比赛成绩，直接罚下。 ● 违章 ：在比赛过程中，如果机器人或作业装置的任何部分超出了 围栏边界，触碰到了围栏、垄测，则判为违章。违章次数为超界次数和 触碰次数之和，比赛时由工作人员现场判别并统计。 ● 驶离比赛场地 ：在完成作业后，参赛机器人需要从出入口离开比 赛场地， 参赛机器人的任何部位不得停留在场地内 （按照俯视投影方法 判断），否则视为未完成比赛，成绩为 0 分。
- 8 - ● 停在比赛启动区 ： 参赛机器人从比赛出口驶出后需要停在比赛启 动区内， 未主动停止的视为未完成比赛， 主动停止但未完全停在比赛启 动区的视为 1 次连续超界，按 2 次违章计算惩罚加时 。 ④比赛过程中不允许使用任何形式的遥控装置，如被裁判发现 或被举报查实，立即取消参赛资格。 ⑤如果机器人或作业装置在比赛过程中出现冲出场地、 失控、 部 件损坏、损坏场地（不含马铃薯、土块）等危险情况，则该参赛机器 人将被立即强制罚下，取消所有比赛成绩。 ⑥比赛开始后， 参赛人员不得以任何理由申请重试， 如因机器人 或作业装置故障而无法在规定时间内完成比赛的，本次比赛以失败 论处。 ⑦如果参赛人员不遵守裁判和工作人员的指示、 指令或警告， 或 做出任何有悖于公平竞争精神的行为，裁判有权直接取消该参赛队 的参赛资格。 3 、比赛过程 ①签到： 所有参赛队都必须在规定时间内到赛场签到， 由评委检 查参赛机器人及其作业装置是否符合比赛要求。检查通过后，关闭 参赛机器人电源，并由工作人员将参赛机器人统一放置在备赛区对 应号位。参赛选手之后不得再进行任何调试，违反者以作弊论处， 取消比赛资格。 ②铺设毛毯： 比赛前由裁判随机抽取确定。 同级别赛场的播种板 位置、毛毯铺设位置和铺设方式完全相同。 ③预备： 评委宣布 “ XX 号机器人进行比赛” 后， 工作人员将 XX 号参赛机器人从备赛区取出，放到比赛启动区。评委宣布“预备”
- 9 - 后，开始预备计时。选手将参赛机器人放到起跑位置，可以给参赛 机器人上电，但参赛机器人的任何部位都不允许超出起跑线。选手 做好起跑准备后告知评委“已就位”。预备时间最长 100 秒。 ④起跑： 评委在参赛选手告知“已就位”之后，评委发出“起 跑”命令后，从小车跨越起始线开始计时；或 100 秒预备时间已到 之后，评委发出“起跑”命令时，开始计时。 参赛选手给参赛机器 人上电（也可提前上电），参赛机器人从比赛启动区出发进入比赛 场地。如在评委发出“起跑”命令之前参赛机器人或其作业装置就 已跨越起跑线则视为抢跑，评委给予警告，并重新起跑。抢跑两次 则比赛以失败论处，计 0 分。 ⑤比赛： 比赛过程中， 由工作人员记录机器人走过的通道数、 作 业得分、违章次数和比赛用时，由裁判确认是否驶出场地和停在启 动区。参赛机器人一旦从出入口驶出，则本次比赛结束。比赛时间 为 5 分钟，参赛机器人超时仍未完成比赛的，比赛也即刻中止。如 果发生机器人冲出场地、部件损坏、损坏场地等情况，裁判有权终 止比赛，且参赛机器人的比赛以失败论处。 ⑥统计和确认成绩： 工作人员统计参赛机器人是否驶出比赛场 地、走过的垄沟 / 垄背数、比赛用时、作业得分 ，参赛选手确认并签 字。如有异议，回放录像确认。 ⑦各参赛队都有两次比赛机会，比赛成绩取最高者。 4 、评分标准 （ 1 ）作业得分计分规则 专本组评分标准
- 10 - 马铃薯颜色 马铃薯 数量 每颗 分值 评分 标准 棕色马铃薯 10 颗 10 有效作业得 10 分 硕博组评分标准 马铃薯颜色 马铃薯 数量 每颗 分值 评分 标准 棕色马铃薯 10 颗 10 有效作业得 10 分 绿色马铃薯 5 颗 20 绿色为干扰组，不得捡 拾，使其发生位移每颗 扣 5 分，捡拾成功每颗 扣 20 分 土块误捡扣分标准 土块误拾扣分标准 （ 最终误捡土块数量占总土块比例） 0% ≤误捡≤ 10% 10% ＜误捡≤ 30% 30% 误捡≤ 50% 50% ＜误捡 不扣分 扣最终作业得分 20 分 扣最终作业得分 40 分 最终作业得分 0 分 ①无效作业、漏捡：不得分也不扣分。 ②误捡： 捡拾成功（将绿色马铃薯捡拾到小车上）每颗扣 20 分 且次数累加，直至最终作业得分 0 分为止。绿色马铃薯发生位移按 5 分 / 颗，从已得作业分中扣除，直至最终作业得分 0 分为止。 ③ 损伤、损坏 2 颗及以内马铃薯 ，按 20 分 / 颗，从已得作业分中 扣除相应分数，直至最终作业得分 0 分为止 。 ④ 损伤、损坏 3 颗 及以上 马铃薯 ，取消比赛成绩，直接罚下。 ⑤ 超出围栏边界或触碰围栏、垄侧，按 10 秒 / 次，加罚比赛用 时。连续刮擦、推行或触碰，都按两次违章计算（ 20 秒 / 次）。 ⑥ 触碰垄背不加罚比赛用时。
- 11 - ⑦如果连续超出围栏边界按 20 秒 / 边计算加罚比赛用时。 （ 2 ）比赛成绩 比赛成绩综合考虑作业用时和作业得分，由两者经归一化处理 后相加得到。选手作业用时为 a ，作业得分为 b ，比赛成绩 S 为： S= max min max min max min 100 0.4 100 0.6 a a b b a a b b − − × × + × × − − 式中： 𝑎𝑎 𝑚𝑚𝑚𝑚𝑚𝑚 —— 所有选手中，作业用时的最大值； 𝑎𝑎 𝑚𝑚𝑖𝑖𝑖𝑖 —— 所有选手中，作业用时的最小值； 𝑏𝑏 𝑚𝑚𝑚𝑚𝑚𝑚 —— 所有选手中，作业得分的最大值； 𝑏𝑏 𝑚𝑚 𝑖𝑖𝑖𝑖 —— 所有选手中，作业得分的最小值。 （ 3 ）获奖比例 决赛期间，大赛委员会根据当年参数规模情况决定各档次获奖 作品的数量。 四、参赛要求 1 、参赛机器人 ①参赛机器人应具有自主行走、 垄间穿行、 识别马铃薯和作业的 能力。 ②作业装置可背负在机器人上， 也可由机器人牵引。 机器人和作 业装置的大小和重量不限，但应尽量小巧，以提高作业灵活性。 ③在整个作业过程中，机器人和作业装置的任何部分都不允许 超出围栏边界，也不允许触碰围栏、垄，更不允许破坏比赛场地。 ④比赛场地周围环境无特殊设置，参赛机器人应能承受周围环 境的光线、噪音和电磁干扰。 ⑤同一参赛单位的任意两台参赛机器人都不可以雷同。如被裁
- 12 - 判质疑雷同，则对该参赛单位的所有类同参赛机器人的队长进行问 辩测试。如被裁判判定为雷同，则取消所有类同参赛机器人的参赛 资格，并判定成绩无效。 2 、参赛团队 ①参赛队员必须为 2025 年 9 月前（含 9 月）正式注册的全日制 非成人教育的普通高等学校和高职高专院校的在校学生，包括专科 生、本科生、硕博研究生。 ②本科及以上院校每个参赛单位最多可派出 8 支参赛队（承办 单位可以多派 5 支队伍），职业院校每个参赛单位最多可派出 5 支 参赛队伍。每位指导教师最多只能指导一支参赛队。 ③每支参赛队都必须根据比赛要求， 自行设计、 制作各自的参赛 机器人。限定每支参赛队只能有 1 台机器人参赛。 ④参赛队员由 2 ～ 5 名学生组成，须为在校专科生、本科生和硕 博研究生，不限学科专业，并指定学历最高者为队长。 ⑤允许最多 2 名队员在准备区内调试机器人。 五、比赛事宜 1 、 比赛过程 （ 1 ）比赛开始前，各队有 100 秒的准备时间，将机器人置于比 赛区域的入口（启动区），并进行必要的调整，机器人可以加电，但 不得运动； （ 2 ）比赛开始，机器人从启动区启动。如在指令前启动机器人 则判为抢跑，给予警告，第二次抢跑的机器人将被罚下； （ 3 ）比赛过程中冲出场地的机器人将直接被罚下，不得重新进 场比赛， 比赛过程中， 如果出现机器人分离， 该机器人被强制罚下。
- 13 - 2 、 重试及断电 （ 1 ）比赛开始后，任何机器人不得申请重试，如因故障而不能 运动，则自动退出比赛，为了机器人的安全和保护场地，裁判有权 将机器人断电并拿出场外； （ 2 ）如机器人在场上出现故障或失控，裁判有权根据现场情况 要求该机器人断电并拿出场地。 3 、 取消比赛资格 参赛队的下列行为会被取消比赛资格。 （ 1 ） 机器人做出危险动作， 危及场上操作手或裁判、 观众安全； （ 2 ）故意损坏比赛场地、道具； （ 3 ）不遵守裁判发出的命令和警告； （ 4 ）做出任何有悖公平竞争精神的行为。 六、比赛安全 安全是机器人比赛持续发展的最重要问题。 因此， 每位参赛者应 特别重视并有义务按照本节的规定在充分采取安全措施的前提下研 制机器人。 第一，所有机器人的制作不应给队员、裁判、工作人员、观众、 设备和比赛场地造成伤害。如果现场裁判认为机器人的行为对人员 或设备有潜在危险，可以禁止该机器人参赛或随时终止比赛。 第二，机器人的结构设计应该考虑到赛前机器人安全检查的方 便性。 第三，禁止使用燃油驱动的发动机、爆炸物、高压气体（超过 0.8MPa ）等。 第四， 在参赛任何时段， 队员都必须充分注意安全问题。 指导教
- 14 - 师或教练应该负起安全指导和监督的责任。参赛期间必须考虑工作 人员和场馆内观众的安全。 七、比赛其它事项 第一， 裁判有权对本规则没有规定的任何行为做出裁决。 在有争 议的情况下，裁判长有权做出最终裁决。 第二，比赛场地及道具尺寸的允许误差为 ±5% 。 第三，重要通知和相关附录后续在官方网站发布。 第四， 比赛将根据报名情况确定赛制， 赛制将在比赛前在官方网 站上发布。 第五， 规则如有更新， 比赛将在官方网站上发布， 以比赛开始前 最后发布的规则为准。 第六，鼓励参赛队在规则允许的范围内以自己的方式装饰机器 人。 第七， 比赛过程中不得使用通讯装置操控机器人， 一旦发现， 以 作弊论处。 第八，如果有需要，比赛将在合适的时间要求各参赛队提交机 器人相关资料、进度报告和录像。 第九，规则的最终解释说明权归大赛委员会所有。 第十一届国际大学生智能农业装备创新大赛委员会 2025 年 9 月 10 日