ASER-NAV/Doc/HANDOFF.md

ARES 项目交接文档

最后更新: 2025 年
项目性质: B类"马铃薯捡拾机器人竞技"走廊导航系统
本文目标: 让任何人 (人类或 AI) 能在 30 分钟内理解整个项目并开始工作

---

目录

1. 一句话概述
2. 比赛规则速览
3. 硬件架构
4. 软件架构总览
5. 代码目录结构
6. 数据流水线 (核心)
7. 各模块详解
8. FreeRTOS 任务一览
9. CAN 通信协议
10. EKF 算法详解
11. 控制器与安全层
12. 比赛流程状态机
13. 全部可调参数
14. 已知问题与待办
15. 构建与烧录
16. 实车调试流程
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 算法详解。

7.4 控制层

详见 第11节 控制器与安全层。

7.5 导航层

详见 第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. 构建与烧录

构建

# 确保 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