Files

2026-04-04 23:24:36 +08:00

37 KiB

Raw Blame History

ARES 项目交接文档 v2

版本: v2.1 — 连接段三信号联合判定
基于: v2.0 + 赛道几何理解修正 + 连接段判定算法升级
更新内容: 修正地图几何理解、修复入场/连接段传感器误用 bug、升级连接段为三信号联合判定
本文目标: 完整描述当前代码库状态，让任何人能在 30 分钟内理解并继续工作

一句话概述
与 v1.0 的核心差异
代码目录结构（当前）
软件架构总览
数据流水线（当前）
赛道级导航状态机详解
安全层改造详解
地图模块详解
EKF 重置机制
编译开关与模式切换
全部可调参数
FreeRTOS 任务一览
文件快速索引
已知问题与待办
实车调试建议

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 状态枚举

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() 逻辑：

// 已转过的角度 = (当前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 阶段低速前进，同时检查三个条件：

至少 3 个侧向传感器有效（4 个 VL53 中至少 3 个返回合法距离）
几何宽度匹配：若左右两侧都有效，则 d_left + d_right + 车宽 与走廊标称宽度 40cm 的误差 ≤ 5cm
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`）

/* 信号 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 枚举作为安全层的"动作语义输入"：

// 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 函数签名变化

// 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 设计原则

地图不做全局坐标，只回答三个问题：

从第 N 条沟完成后，下一条是第几条？
这次该往哪转？（左/右）
当前是不是最后一条沟？

8.3 核心数据

// 单条垄沟描述
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

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 实现

// 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 控制宏

// 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` 未校准	实测传感器内缩距离并填入

给接手者的提醒:

调参前先确认 USE_GLOBAL_NAV 的模式，不同模式下调的参数组不同

v2.0 新增的 P6 参数全部有默认值，实车前必须根据实际场地微调 LINK_DISTANCE 和 ENTRY_DISTANCE

CorridorFilter_Reset() 在每次入沟时自动调用，不需要手动干预

如果出现整体行为异常，先把 USE_GLOBAL_NAV=0 切回单沟模式定位问题

CAN 协议层 (snc_can_app.c) 已冻结，不要修改

37 KiB Raw Blame History Unescape Escape