774 lines
35 KiB
Markdown
774 lines
35 KiB
Markdown
# 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`
|