diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c index 08fb29f..475f09b 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -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); diff --git a/App/nav/track_map.c b/App/nav/track_map.c index 927b37e..2d85b15 100644 --- a/App/nav/track_map.c +++ b/App/nav/track_map.c @@ -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: ← 最后一条, 左转出场 */ + /* id travel_dir exit_turn entry_turn is_last + * -- ---------- --------- ---------- ------- */ + { 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, diff --git a/App/nav/track_map.h b/App/nav/track_map.h index 9b9ae95..2d04380 100644 --- a/App/nav/track_map.h +++ b/App/nav/track_map.h @@ -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; /** 转向方向 */ diff --git a/App/robot_params.h b/App/robot_params.h index 39940c5..e4ea46c 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -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 — 回停速度 */