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

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-03 13:46:45 +08:00
parent 1bd0a73a73
commit 7afa32f7dc
4 changed files with 180 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
App/nav/global_nav.c
View File

@@ -2,10 +2,17 @@
* @file global_nav.c
* @brief 赛道级总控 — 混合导航状态机实现
*
* 实现 6 条垄沟 S 型遍历:
* IDLE → ENTRY → TURN_IN → REACQUIRE → CORRIDOR_TRACK →
* TURN_OUT → LINK → TURN_IN_NEXT → REACQUIRE → ...
* → EXIT → DOCK → FINISHED
* 场地结构: 垄沟沿X轴(横向)分布，左右两端各有纵向端部通道
* 启动区在左下角，入口对齐左端通道
*
* S 型遍历:
* 入场(北行)→右转入C1(→东)→右端到端→左转→北行→左转入C2(←西)
* →左端到端→右转→北行→右转入C3(→东)→...→C6(←西)
* →左端到端→左转(朝南)→南行出场→回停启动区
*
* 端部通道特点:
* - 一侧贴围栏(VL53能测到), 另一侧交替出现垄背端面和垄沟开口
* - 不能依赖双侧VL53做EKF, 必须用IMU航向保持+前后激光到端检测
*/
#include "global_nav.h"
@@ -49,6 +56,11 @@ static struct {
/* 航向保持 */
float heading_ref_deg;
/* 连接段: 多传感器辅助 */
float link_d_front_start; /* 进入连接段时前激光读数 (m) */
bool link_d_front_valid; /* 进入时前激光是否有效 */
uint8_t link_gap_count; /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */
/* EKF 进度保存 */
float corridor_entry_s;
@@ -83,7 +95,7 @@ static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
return gnav_clampf(w, -1.0f, 1.0f);
}
/** 检查侧向 VL53 是否探到壁 (至少有一侧的前后都有效) */
/** 检查侧向 VL53 是否探到壁 (至少有一侧的前后都有效) — 仅用于重捕获 */
static bool side_walls_detected(const CorridorObs_t* obs)
{
bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
@@ -93,6 +105,44 @@ static bool side_walls_detected(const CorridorObs_t* obs)
return left_ok || right_ok;
}
/**
* @brief 检测非围栏侧 VL53 是否发现垄沟开口 (连接段沟口辅助判定)
*
* 在纵向端部通道面朝北行驶时：
* - 刚走完向东(EAST)的沟 → 当前在右端通道 → 右侧贴围栏 → 检查左侧VL53
* - 刚走完向西(WEST)的沟 → 当前在左端通道 → 左侧贴围栏 → 检查右侧VL53
*
* 经过垄背端面时: VL53 测到约 (通道宽/2 - 车宽/2 - VL53内缩) ≈ 10cm → 有效
* 经过垄沟开口时: VL53 射入沟内 220cm+ → 超出有效距离 → 无效或读数 > 1.2m
*
* 因为 VL53 前后两颗间距 12cm，车身 20cm 宽，沟口 40cm 宽，
* 当车身中心对准沟口时，前后 VL53 都已进入开口区域，两颗均应丢失。
* 但过渡区有边缘效应，所以只要求 "前后至少一颗丢失" 即视为探到沟口。
*
* @param obs 当前观测
* @param prev_travel_dir 刚走完的那条沟的行驶方向 (EAST/WEST)
* @return true = 非围栏侧检测到开口
*/
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
TravelDirection_t prev_travel_dir)
{
if (prev_travel_dir == TRAVEL_DIR_EAST) {
/* 在右端通道，右侧贴围栏 → 检查左侧 VL53 */
bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
|| (obs->d_lf > 0.5f); /* >50cm 视为沟口 (正常贴壁约10cm) */
bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
|| (obs->d_lr > 0.5f);
return lf_lost || lr_lost;
} else {
/* 在左端通道，左侧贴围栏 → 检查右侧 VL53 */
bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
|| (obs->d_rf > 0.5f);
bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
|| (obs->d_rr > 0.5f);
return rf_lost || rr_lost;
}
}
/** 检查侧向 VL53 是否全丢 */
static bool all_side_lost(const CorridorObs_t* obs)
{
@@ -276,6 +326,9 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
case GNAV_LINK_STRAIGHT:
s_nav.heading_ref_deg = imu_yaw;
s_nav.link_d_front_start = 0.0f;
s_nav.link_d_front_valid = false; /* 首拍再记录 */
s_nav.link_gap_count = 0;
break;
case GNAV_EXIT_STRAIGHT:
@@ -420,7 +473,13 @@ void GlobalNav_Update(const CorridorObs_t* obs,
switch (s_nav.stage) {
/* ============================================================
* 入场直线
* 入场直线 (从启动区沿左端纵向通道北行)
*
* 传感器情况：
* - 左侧 VL53 贴围栏，始终有效（不能用来判"到了C1入口"）
* - 右侧 VL53 一出启动区就对着C1开口(260cm)，测不到
* - 入场距离极短（启动区入口到C1入口仅约 10~30cm）
* - 主要靠里程计推进足够距离后即可转向
* ============================================================ */
case GNAV_ENTRY_STRAIGHT:
out->override_v = s_nav.cfg.entry_v;
@@ -428,8 +487,8 @@ void GlobalNav_Update(const CorridorObs_t* obs,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (side_walls_detected(obs) ||
odom_since_entry() >= s_nav.cfg.entry_distance ||
/* 里程计 + 超时判定 (不用 side_walls_detected，因为左侧围栏始终有效会误触发) */
if (odom_since_entry() >= s_nav.cfg.entry_distance ||
elapsed_ms > s_nav.cfg.entry_timeout_ms)
{
transition_to(GNAV_TURN_INTO_CORRIDOR, board);
@@ -496,7 +555,22 @@ void GlobalNav_Update(const CorridorObs_t* obs,
break;
/* ============================================================
* 连接段直行
* 连接段直行 (纵向端部通道北行, 方案2: 三信号联合判定)
*
* 传感器情况（转向完成后面朝北）：
* - 前激光（朝北）→ 上围栏，d_front 随北行递减 [精度高]
* - 后激光（朝南）→ 下围栏，d_back 随北行递增 [精度高]
* - 围栏侧 VL53 → 始终有效 (~10cm) [不用于判定]
* - 非围栏侧 VL53 → 贴垄背端面时有效，到垄沟开口时丢失 [沟口标志]
*
* 信号定义：
* A: 里程计 odom >= link_distance * 0.85 (打滑衰减, 权重低)
* B: 前激光变化 d_front缩小 >= link_distance * 0.85 (权重高)
* C: 非围栏侧VL53 丢失/跳到>50cm，连续2拍确认 (直接探测沟口, 权重中)
*
* 触发逻辑: B || (A && C)
* - 前激光变化量足够 → 直接触发（最可靠的单一信号）
* - 里程计到位 + VL53探到沟口 → 联合触发（互相校验）
* ============================================================ */
case GNAV_LINK_STRAIGHT:
out->override_v = s_nav.cfg.link_v;
@@ -504,11 +578,53 @@ void GlobalNav_Update(const CorridorObs_t* obs,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (odom_since_entry() >= s_nav.cfg.link_distance ||
side_walls_detected(obs))
{
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
/* 首拍记录前激光基准值 */
if (!s_nav.link_d_front_valid && front_valid) {
s_nav.link_d_front_start = obs->d_front;
s_nav.link_d_front_valid = true;
}
/* ---- 信号 A: 里程计 (考虑打滑, 用 85% 容差) ---- */
bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
/* ---- 信号 B: 前激光变化量 (高精度) ---- */
bool laser_ok = false;
if (s_nav.link_d_front_valid && front_valid) {
float d_front_delta = s_nav.link_d_front_start - obs->d_front;
/* 车身中心到前激光有偏置(FRONT_LASER_OFFSET)，但这里用的是差值，偏置抵消 */
laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
}
/* ---- 信号 C: 非围栏侧 VL53 沟口检测 (需连续2拍确认) ----
*
* 判定阈值 0.5m 的来源:
* 正常贴垄背端面时 VL53 读数 ≈ (通道宽/2 - 车宽/2 - VL53内缩)
* = (0.40/2 - 0.20/2 - 0.0)
* = 0.10m
* 到垄沟开口时 VL53 读数 > 1.2m (超出有效距离) 或无效
* 阈值 0.5m 在两者之间，足够区分
*/
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
/* 在 LINK_STRAIGHT 阶段，current_corridor_id 仍是刚走完的沟 (尚未更新)
所以 cd->travel_dir 就是刚走完那条沟的方向，直接用来判断当前在哪个端部通道 */
bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir);
if (gap_now) {
if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
} else {
s_nav.link_gap_count = 0;
}
bool gap_confirmed = (s_nav.link_gap_count >= 2); /* 连续2拍 (40ms @ 20ms周期) */
/* ---- 联合判定: B || (A && C) ---- */
if (laser_ok || (odom_ok && gap_confirmed))
{
transition_to(GNAV_TURN_INTO_NEXT, board);
}
}
if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
transition_to(GNAV_ERROR, board);
}

38
App/nav/track_map.c
View File

@@ -2,27 +2,45 @@
* @file track_map.c
* @brief 赛道地图 — S 型遍历拓扑表
*
* S 型路径:
* C1(→) → 左转 → 连接 → 左转 → C2(←) → 右转 → 连接 → 右转 →
* C3(→) → 左转 → 连接 → 左转 → C4(←) → 右转 → 连接 → 右转 →
* C5(→) → 左转 → 连接 → 左转 → C6(←) → 左转 → 出场
* 正确的场地结构 (参见 Doc/map.md):
* - 垄沟沿 X 轴（横向）分布，长 220cm
* - 左右两端各有纵向端部通道，宽 40cm
* - 启动区在左下角，入口对齐左端通道
* - C1 最靠近入口(南侧)，C6 最远(北侧)
*
* S 型路径 (以"北=Y递减=地图上方"为参考):
* 入场→左端通道→右转入C1(→东)→右端到端→左转(朝北)
* →北行70cm→左转入C2(←西)→左端到端→右转(朝北)
* →北行70cm→右转入C3(→东)→...
* →C6(←西)→左端到端→左转(朝南)→南行出场
*
* entry_turn_dir: 从纵向通道(朝北)转入垄沟的方向
* exit_turn_dir: 从垄沟到端后转入纵向通道的方向
*/
#include "track_map.h"
/* =========================================================
* 硬编码 S 型遍历表
*
* 转向方向推导 (北=机器人沿纵向通道前进方向):
* C1: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
* C2: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
* C3: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
* C4: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
* C5: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
* C6: 右端通道朝北→左转→朝西入沟; 到左端→左转→朝南(出场)
* ========================================================= */
static const TrackMap_t s_map = {
.corridors = {
/* id travel_dir exit_turn entry_turn is_last
* -- ---------- --------- ---------- ------- */
{ 0, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C1: → 到右端后左转 */
{ 1, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, /* C2: ← 到左端后右转 */
{ 2, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C3: → */
{ 3, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, /* C4: ← */
{ 4, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C5: → */
{ 5, TRAVEL_DIR_NEGATIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, true }, /* C6: ← 最后一条, 左转出场 */
{ 0, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C1: →东, 从左端右转入, 到右端左转出 */
{ 1, TRAVEL_DIR_WEST, TURN_DIR_RIGHT, TURN_DIR_LEFT, false }, /* C2: ←西, 从右端左转入, 到左端右转出 */
{ 2, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C3: →东 */
{ 3, TRAVEL_DIR_WEST, TURN_DIR_RIGHT, TURN_DIR_LEFT, false }, /* C4: ←西 */
{ 4, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C5: →东 */
{ 5, TRAVEL_DIR_WEST, TURN_DIR_LEFT, TURN_DIR_LEFT, true }, /* C6: ←西, 从右端左转入, 到左端左转出(朝南出场) */
},
.entry_corridor_id = 0,
.link_distance_m = TRACK_MAP_LINK_DISTANCE_M,

15
App/nav/track_map.h
View File

@@ -2,6 +2,17 @@
* @file track_map.h
* @brief 赛道地图 — 固化 S 型遍历拓扑
*
* 赛道几何 (参见 Doc/map.md):
* - 场地 300cm(X) × 390cm(Y)，5 条横向垄背把场地切成 6 条横向垄沟
* - 垄沟沿 X 轴方向，长 220cm，宽 40cm
* - 左右两端各有一条纵向端部通道 (宽 40cm，长 390cm)
* - 启动区在左下角，入口对齐左端通道
* - 垄沟1(最靠近入口) 到 垄沟6(最远离入口) 自下而上排列
*
* S 型遍历:
* 入场→左端通道→右转入C1(→)→右端到端→左转→北行→左转入C2(←)
* →左端到端→右转→北行→右转入C3(→)→...→C6(←)→左端→左转→南行出场
*
* 地图不做全局坐标定位，只回答三个问题：
* 1. 从第 N 条沟完成后，下一条是第几条？
* 2. 这次该往哪转？(左/右)
@@ -31,8 +42,8 @@ extern "C" {
/** 沟内行驶方向 */
typedef enum {
TRAVEL_DIR_POSITIVE = 0, /* 从左端到右端 (→) */
TRAVEL_DIR_NEGATIVE = 1 /* 从右端到左端 (←) */
TRAVEL_DIR_EAST = 0, /* 从左端到右端 (→, +X) 奇数沟 */
TRAVEL_DIR_WEST = 1 /* 从右端到左端 (←, -X) 偶数沟 */
} TravelDirection_t;
/** 转向方向 */

15
App/robot_params.h
View File

@@ -379,8 +379,9 @@ extern "C" {
#define USE_GLOBAL_NAV 1
/* --- 入场段 --- */
#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 */
#define PARAM_GNAV_ENTRY_DISTANCE 0.65f /* m — 入场里程上限 */
/* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */
#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 (沿左端纵向通道向北) */
#define PARAM_GNAV_ENTRY_DISTANCE 0.30f /* m — 入场里程上限 (启动区到C1入口约10~30cm) */
#define PARAM_GNAV_ENTRY_TIMEOUT 10000U /* ms — 入场超时 */
/* --- 转向 --- */
@@ -402,15 +403,17 @@ extern "C" {
#define PARAM_GNAV_CORRIDOR_END_DIST 0.10f /* m — 到端检测距离 */
/* --- 连接段 --- */
#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 */
/* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */
#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 (纵向通道内，IMU航向保持) */
#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距) */
#define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */
/* --- 出场 --- */
#define PARAM_GNAV_EXIT_V 0.15f /* m/s — 出场速度 */
/* C6完成后在左端通道左转朝南，沿纵向通道南行回到入口，距离约390cm */
#define PARAM_GNAV_EXIT_V 0.15f /* m/s — 出场速度 (沿左端通道向南) */
#define PARAM_GNAV_EXIT_RUNOUT 1.50f /* m — 出场侧向丢失后冲刺距离 */
#define PARAM_GNAV_EXIT_MAX_DIST 4.00f /* m — 出场里程保护 */
#define PARAM_GNAV_EXIT_TIMEOUT 20000U /* ms — 出场超时 */
#define PARAM_GNAV_EXIT_MAX_DIST 4.50f /* m — 出场里程保护 (通道全长约3.9m) */
#define PARAM_GNAV_EXIT_TIMEOUT 30000U /* ms — 出场超时 (距离长，给足时间) */
/* --- 回停 --- */
#define PARAM_GNAV_DOCK_V 0.05f /* m/s — 回停速度 */