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

140
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,10 +578,52 @@ 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))
{
transition_to(GNAV_TURN_INTO_NEXT, board);
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);