From 315495bab344e4a8c7f862a92ece48a452bb5c4f Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Sun, 12 Apr 2026 14:30:19 +0800 Subject: [PATCH] 1.0 --- App/nav/global_nav.c | 16 ++++++++++++---- App/robot_params.h | 4 ++-- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c index 6b6c3b1..43f9e96 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -857,12 +857,17 @@ void GlobalNav_Update(const CorridorObs_t* obs, s_nav.link_d_front_valid = true; } - bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f; + /* 连接段入口判定原则： + * 用“看到开口”的那一刻作为唯一锚点，然后固定前冲一小段再转向。 + * 这样每次到沟入口的位置更一致，不再混用多套主触发条件。 + */ + bool odom_guard_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.55f; + /* 前激光仅保留为兜底，不再参与主触发。 */ 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; - laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f); + laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 1.05f); } bool gap_confirmed = false; @@ -875,7 +880,7 @@ void GlobalNav_Update(const CorridorObs_t* obs, } gap_confirmed = (s_nav.link_gap_count >= 5U); - if (gap_confirmed && !s_nav.link_gap_seen) { + if (odom_guard_ok && gap_confirmed && !s_nav.link_gap_seen) { s_nav.link_gap_seen = true; s_nav.link_gap_seen_odom = s_nav.odom_distance_accum; } @@ -887,7 +892,10 @@ void GlobalNav_Update(const CorridorObs_t* obs, >= s_nav.cfg.link_gap_runout; } - if (odom_ok && (laser_ok || (s_nav.link_gap_seen && gap_runout_ok))) { + if (s_nav.link_gap_seen && gap_runout_ok) { + transition_to(GNAV_TURN_INTO_NEXT, board); + } else if (!s_nav.link_gap_seen && laser_ok) { + /* 极端兜底：如果开口检测失效，才允许前激光变化量接管。 */ transition_to(GNAV_TURN_INTO_NEXT, board); } } diff --git a/App/robot_params.h b/App/robot_params.h index af58cfc..3a5f044 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -530,9 +530,9 @@ extern "C" { /* --- 连接段 --- */ /* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */ #define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 (纵向通道内，IMU航向保持) */ -#define PARAM_GNAV_LINK_DISTANCE 0.90f /* m — 连接段标称距离 (沟间距) */ +#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距中心到中心) */ #define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */ -#define PARAM_GNAV_LINK_GAP_RUNOUT 0.06f /* m — 看到下一个沟口后继续前冲距离 */ +#define PARAM_GNAV_LINK_GAP_RUNOUT 0.08f /* m — 看到下一个沟口后继续前冲距离 */ #define PARAM_GNAV_LINK_WALL_TARGET 0.10f /* m — 连接段最小离墙距离（小于10cm才触发保底修正） */ #define PARAM_GNAV_LINK_WALL_KP 3.0f /* — 连接段离墙保底增益 */ #define PARAM_GNAV_LINK_WALL_BLEND 0.5f /* — 预留给其他通道状态，连接段本身不再使用融合 */