From 1bd0a73a73656e77679eeac2896fd58c3048d3fa Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Fri, 3 Apr 2026 08:56:26 +0800 Subject: [PATCH] 1.0 --- App/app_tasks.c | 96 +- App/est/corridor_filter.c | 15 + App/est/corridor_filter.h | 10 + App/nav/global_nav.c | 596 +++++++++++++ App/nav/global_nav.h | 138 +++ App/nav/segment_fsm.c | 65 +- App/nav/segment_fsm.h | 2 + App/nav/track_map.c | 78 ++ App/nav/track_map.h | 80 ++ App/preproc/corridor_msgs.h | 10 + App/robot_params.h | 48 + Doc/CODE_REVIEW.md | 397 --------- Doc/GLOBAL_NAV_REQUIREMENT.md | 667 -------------- Doc/HYBRID_NAVIGATION_GUIDE.md | 458 ---------- Doc/IMU_YAW_REQUIREMENT.md | 321 ------- Doc/实施方案.md | 1500 ++++++++++++++++++++++++++++++++ Doc/混合导航方案.md | 549 ++++++++++++ 17 files changed, 3147 insertions(+), 1883 deletions(-) create mode 100644 App/nav/global_nav.c create mode 100644 App/nav/global_nav.h create mode 100644 App/nav/track_map.c create mode 100644 App/nav/track_map.h delete mode 100644 Doc/CODE_REVIEW.md delete mode 100644 Doc/GLOBAL_NAV_REQUIREMENT.md delete mode 100644 Doc/HYBRID_NAVIGATION_GUIDE.md delete mode 100644 Doc/IMU_YAW_REQUIREMENT.md create mode 100644 Doc/实施方案.md create mode 100644 Doc/混合导航方案.md diff --git a/App/app_tasks.c b/App/app_tasks.c index b98b251..3e78836 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -19,6 +19,8 @@ #include "nav/corridor_ctrl.h" #include "nav/segment_fsm.h" #include "nav/nav_script.h" +#include "nav/global_nav.h" +#include "nav/track_map.h" extern osMutexId_t logMutexHandle; /* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */ @@ -297,9 +299,14 @@ void AppTasks_RunNavTask_Impl(void *argument) const uint32_t period_ticks = AppTasks_MsToTicks(20U); uint32_t last_ms = HAL_GetTick(); - /* 等传感器全部就绪再启动脚本 (避免刚上电全是脏数据) */ + /* 等传感器全部就绪再启动 (避免刚上电全是脏数据) */ osDelay(500); - NavScript_Start(); // 开始执行比赛脚本 + +#if USE_GLOBAL_NAV + GlobalNav_Start(); /* 开始赛道级导航 */ +#else + NavScript_Start(); /* 单沟测试模式 */ +#endif for (;;) { uint32_t now_ms = HAL_GetTick(); @@ -317,11 +324,11 @@ void AppTasks_RunNavTask_Impl(void *argument) CorridorObs_t obs; CorridorPreproc_ExtractObs(&board, now_ms, &obs); - /* --- Step 3: 互补滤波 → 走廊状态估计 --- */ + /* --- Step 3: EKF → 走廊状态估计 --- */ /* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */ float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f; float imu_wz = PARAM_DEG2RAD(imu_wz_raw); - float odom_vx = board.odom_vx; /* 里程计已接入，由 monitorTask 调用 Odom_Update() 更新 */ + float odom_vx = board.odom_vx; /* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */ float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid @@ -332,34 +339,62 @@ void AppTasks_RunNavTask_Impl(void *argument) CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s, imu_yaw_cont_rad, imu_yaw_ok, &corridor_state); - /* --- Step 4: 段脚本执行器 → 决定当前阶段和目标 --- */ +#if USE_GLOBAL_NAV + /* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */ + + /* --- Step 4: 赛道级导航 --- */ + GlobalNavOutput_t nav_out; + GlobalNav_Update(&obs, &corridor_state, &board, &nav_out); + + /* --- Step 5: 控制律 --- */ + RawCmd_t raw_cmd; + memset(&raw_cmd, 0, sizeof(raw_cmd)); + raw_cmd.t_ms = now_ms; + + if (nav_out.request_corridor) { + /* 沟内闭环：使用走廊控制器 */ + CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd); + } else if (nav_out.use_override) { + /* 赛道级覆盖：直接用导航输出 */ + raw_cmd.v = nav_out.override_v; + raw_cmd.w = nav_out.override_w; + } + /* else: raw_cmd 已是零速 */ + + /* --- Step 6: 安全仲裁 (带动作模式感知) --- */ + SegFsmOutput_t fsm_out; + SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out); + +#else + /* ========== 单沟测试模式 (原 nav_script) ========== */ + + /* --- Step 4: 段脚本执行器 --- */ NavScriptOutput_t script_out; float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid ? board.imu_yaw_continuous.value : 0.0f; NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out); - /* --- Step 5: 控制律 → 计算期望 v, w --- */ + /* --- Step 5: 控制律 --- */ RawCmd_t raw_cmd; if (script_out.use_override) { - /* 脚本覆盖模式：直接用脚本的输出（入口对准/原地转向/退出等） */ raw_cmd.t_ms = now_ms; raw_cmd.v = script_out.override_v; raw_cmd.w = script_out.override_w; raw_cmd.flags = 0U; } else if (script_out.request_corridor) { - /* 走廊跟踪模式：使用走廊控制器 */ CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd); } else { - /* 默认停车 */ raw_cmd.t_ms = now_ms; raw_cmd.v = 0.0f; raw_cmd.w = 0.0f; raw_cmd.flags = 0U; } - /* --- Step 6: 段状态机 → 安全仲裁 --- */ + /* --- Step 6: 安全仲裁 (CORRIDOR 模式，兼容旧行为) --- */ SegFsmOutput_t fsm_out; - SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out); + SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out); + +#endif /* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */ CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U); @@ -408,7 +443,43 @@ void AppTasks_Init(void) SegFsm_Init(&fsm_cfg); SegFsm_Start(); /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */ - /* --- 初始化段脚本执行器 --- */ +#if USE_GLOBAL_NAV + /* --- 初始化赛道地图 --- */ + TrackMap_Init(); + + /* --- 初始化赛道级导航状态机 --- */ + GlobalNavConfig_t gnav_cfg = { + .entry_v = PARAM_GNAV_ENTRY_V, + .entry_distance = PARAM_GNAV_ENTRY_DISTANCE, + .entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT, + .turn_omega = PARAM_GNAV_TURN_OMEGA, + .turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE, + .turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE, + .turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA, + .turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT, + .reacquire_v = PARAM_GNAV_REACQUIRE_V, + .reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF, + .reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL, + .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS, + .reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT, + .corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST, + .corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN, + .link_v = PARAM_GNAV_LINK_V, + .link_distance = PARAM_GNAV_LINK_DISTANCE, + .link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT, + .exit_v = PARAM_GNAV_EXIT_V, + .exit_runout = PARAM_GNAV_EXIT_RUNOUT, + .exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST, + .exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT, + .dock_v = PARAM_GNAV_DOCK_V, + .dock_distance = PARAM_GNAV_DOCK_DISTANCE, + .heading_kp = PARAM_GNAV_HEADING_KP, + .corridor_width = PARAM_CORRIDOR_WIDTH, + }; + GlobalNav_Init(&gnav_cfg); + +#else + /* --- 初始化段脚本执行器 (单沟测试模式) --- */ NavScriptConfig_t script_cfg = { .turn_target_angle = 3.14159265f, /* 固定：180度转向 */ .turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */ @@ -420,4 +491,5 @@ void AppTasks_Init(void) .exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */ }; NavScript_Init(&script_cfg); +#endif } diff --git a/App/est/corridor_filter.c b/App/est/corridor_filter.c index 64484c1..2971dfb 100644 --- a/App/est/corridor_filter.c +++ b/App/est/corridor_filter.c @@ -106,3 +106,18 @@ void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx } } } + +/* ========================================================= + * 重置 (进入新垄沟时调用) + * ========================================================= */ +void CorridorFilter_Reset(void) +{ + if (!s_initialized) return; + + /* 重置 EKF 内核: 状态归零, 协方差恢复初始值 */ + CorridorEKF_Reset(); + + /* 解锁 IMU yaw 参考值, 等待在新沟中重新锁定 */ + s_imu_yaw_ref_rad = 0.0f; + s_imu_yaw_ref_set = false; +} diff --git a/App/est/corridor_filter.h b/App/est/corridor_filter.h index 69643ff..bbf94ac 100644 --- a/App/est/corridor_filter.h +++ b/App/est/corridor_filter.h @@ -39,6 +39,16 @@ extern "C" { float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid, CorridorState_t *out_state); + /** + * @brief 重置滤波器状态 (进入新垄沟时必须调用) + * + * 重置内容： + * - EKF 状态向量清零 (e_y=0, e_th=0, s=0) + * - 协方差恢复到初始值 + * - IMU yaw 参考值解锁，等待重新锁定 + */ + void CorridorFilter_Reset(void); + #ifdef __cplusplus } #endif diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c new file mode 100644 index 0000000..08fb29f --- /dev/null +++ b/App/nav/global_nav.c @@ -0,0 +1,596 @@ +/** + * @file global_nav.c + * @brief 赛道级总控 — 混合导航状态机实现 + * + * 实现 6 条垄沟 S 型遍历: + * IDLE → ENTRY → TURN_IN → REACQUIRE → CORRIDOR_TRACK → + * TURN_OUT → LINK → TURN_IN_NEXT → REACQUIRE → ... + * → EXIT → DOCK → FINISHED + */ + +#include "global_nav.h" +#include "est/corridor_filter.h" +#include "preproc/corridor_preproc.h" +#include "robot_params.h" +#include +#include + +/* ========================================================= + * 内部状态 + * ========================================================= */ +static struct { + GlobalNavStage_t stage; + bool running; + bool initialized; + + /* 赛道级 */ + uint8_t current_corridor_id; + uint8_t corridors_completed; + + /* 转向 */ + float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */ + float turn_target_delta_deg; /* 90° */ + int8_t turn_sign; /* +1 (CCW) or -1 (CW) */ + + /* 里程 (用里程计积分距离) */ + float stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */ + float odom_distance_accum; /* 运行中持续积分的里程 */ + + /* 超时 */ + uint32_t stage_start_ms; + + /* 重捕获 */ + uint8_t reacquire_ok_count; + + /* 出场 */ + bool exit_vl53_lost; + float exit_lost_distance; + + /* 航向保持 */ + float heading_ref_deg; + + /* EKF 进度保存 */ + float corridor_entry_s; + + /* 配置 */ + GlobalNavConfig_t cfg; +} s_nav; + +/* 上一个周期的里程计速度，用于积分 */ +static float s_last_odom_vx = 0.0f; +static uint32_t s_last_update_ms = 0; + +/* ========================================================= + * 辅助函数 + * ========================================================= */ +static inline float gnav_clampf(float val, float lo, float hi) +{ + if (val < lo) return lo; + if (val > hi) return hi; + return val; +} + +static inline float gnav_fabsf(float x) +{ + return x < 0.0f ? -x : x; +} + +/** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */ +static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp) +{ + float err_deg = ref_yaw_deg - current_yaw_deg; + float w = kp * err_deg; /* kp 把 ° 映射到 rad/s */ + return gnav_clampf(w, -1.0f, 1.0f); +} + +/** 检查侧向 VL53 是否探到壁 (至少有一侧的前后都有效) */ +static bool side_walls_detected(const CorridorObs_t* obs) +{ + bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_LR); + bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_RR); + return left_ok || right_ok; +} + +/** 检查侧向 VL53 是否全丢 */ +static bool all_side_lost(const CorridorObs_t* obs) +{ + uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR | + CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR; + return (obs->valid_mask & side_mask) == 0U; +} + +/** 检查重捕获条件 */ +static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state) +{ + /* 条件 1: 至少 3 个侧向传感器有效 */ + uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR | + CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR; + uint8_t active = obs->valid_mask & side_mask; + int count = 0; + for (int i = 0; i < 4; i++) { + if (active & (1U << i)) count++; + } + if (count < 3) return false; + + /* 条件 2: 左右距离和 ≈ 走廊宽度 */ + bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_LR); + bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_RR); + + if (left_ok && right_ok) { + float d_left = (obs->d_lf + obs->d_lr) * 0.5f; + float d_right = (obs->d_rf + obs->d_rr) * 0.5f; + float total_width = d_left + d_right + PARAM_ROBOT_WIDTH; + float err = gnav_fabsf(total_width - s_nav.cfg.corridor_width); + if (err > s_nav.cfg.reacquire_width_tol) return false; + } + + /* 条件 3: EKF 置信度 */ + if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false; + + return true; +} + +/** 获取当前阶段的已行驶里程 */ +static float odom_since_entry(void) +{ + return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum; +} + +/** 阶段转移 — 通用初始化 */ +static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board); + +/* ========================================================= + * 阶段名称表 + * ========================================================= */ +static const char* const s_stage_names[] = { + "IDLE", + "ENTRY_STRAIGHT", + "TURN_INTO_CORRIDOR", + "REACQUIRE", + "CORRIDOR_TRACK", + "TURN_OUT", + "LINK_STRAIGHT", + "TURN_INTO_NEXT", + "EXIT_STRAIGHT", + "DOCK", + "FINISHED", + "ERROR" +}; + +/* ========================================================= + * 转向执行 (统一的 90° 转向逻辑) + * ========================================================= */ +static void execute_turn(const CorridorObs_t* obs, + const CorridorState_t* state, + const RobotBlackboard_t* board, + uint32_t now_ms, + GlobalNavOutput_t* out) +{ + float imu_yaw = board->imu_yaw_continuous.is_valid + ? board->imu_yaw_continuous.value : 0.0f; + + /* 已转过的角度 (取绝对值) */ + float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign; + float target = s_nav.turn_target_delta_deg; + float remaining_deg = target - delta; + + float omega = s_nav.cfg.turn_omega; + + /* 接近目标时减速 */ + float decel_zone_deg = PARAM_RAD2DEG(s_nav.cfg.turn_decel_zone_rad); + if (remaining_deg < decel_zone_deg && decel_zone_deg > 0.01f) { + float ratio = remaining_deg / decel_zone_deg; + if (ratio < 0.0f) ratio = 0.0f; + omega = s_nav.cfg.turn_min_omega + + ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega); + } + + out->override_v = 0.0f; + out->override_w = (float)s_nav.turn_sign * omega; + out->use_override = true; + out->request_corridor = false; + out->safety_mode = SAFETY_MODE_TURN; + + /* 转向完成判定 */ + float tolerance_deg = PARAM_RAD2DEG(s_nav.cfg.turn_tolerance_rad); + if (delta >= target - tolerance_deg) { + switch (s_nav.stage) { + case GNAV_TURN_INTO_CORRIDOR: + case GNAV_TURN_INTO_NEXT: + transition_to(GNAV_REACQUIRE, board); + break; + case GNAV_TURN_OUT_OF_CORRIDOR: + if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) { + transition_to(GNAV_EXIT_STRAIGHT, board); + } else { + transition_to(GNAV_LINK_STRAIGHT, board); + } + break; + default: + break; + } + return; + } + + /* 超时保护 */ + if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) { + transition_to(GNAV_ERROR, board); + } +} + +/* ========================================================= + * 阶段转移 + * ========================================================= */ +static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) +{ + float imu_yaw = (board != NULL && board->imu_yaw_continuous.is_valid) + ? board->imu_yaw_continuous.value : 0.0f; + + /* 通用: 记录进入时间和里程 */ + s_nav.stage_start_ms = s_last_update_ms; + s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum; + s_nav.reacquire_ok_count = 0; + + switch (next) { + case GNAV_ENTRY_STRAIGHT: + s_nav.heading_ref_deg = imu_yaw; + s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id; + break; + + case GNAV_TURN_INTO_CORRIDOR: { + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id); + s_nav.turn_sign = (int8_t)cd->entry_turn_dir; + s_nav.turn_start_yaw_deg = imu_yaw; + s_nav.turn_target_delta_deg = 90.0f; + break; + } + case GNAV_TURN_OUT_OF_CORRIDOR: { + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id); + s_nav.turn_sign = (int8_t)cd->exit_turn_dir; + s_nav.turn_start_yaw_deg = imu_yaw; + s_nav.turn_target_delta_deg = 90.0f; + break; + } + case GNAV_TURN_INTO_NEXT: { + uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id); + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id); + s_nav.turn_sign = (int8_t)cd->entry_turn_dir; + s_nav.turn_start_yaw_deg = imu_yaw; + s_nav.turn_target_delta_deg = 90.0f; + s_nav.current_corridor_id = next_id; + break; + } + case GNAV_REACQUIRE: + /* EKF 重置: 新沟的 e_y 参考不同，必须重建 */ + CorridorFilter_Reset(); + s_nav.heading_ref_deg = imu_yaw; + break; + + case GNAV_CORRIDOR_TRACK: + s_nav.corridor_entry_s = 0.0f; /* EKF 已 reset, s 从 0 开始 */ + break; + + case GNAV_LINK_STRAIGHT: + s_nav.heading_ref_deg = imu_yaw; + break; + + case GNAV_EXIT_STRAIGHT: + s_nav.heading_ref_deg = imu_yaw; + s_nav.exit_vl53_lost = false; + s_nav.exit_lost_distance = 0.0f; + break; + + case GNAV_DOCK: + break; + + case GNAV_FINISHED: + s_nav.running = false; + break; + + case GNAV_ERROR: + break; + + default: + break; + } + + s_nav.stage = next; +} + +/* ========================================================= + * 公开 API + * ========================================================= */ + +void GlobalNav_Init(const GlobalNavConfig_t* cfg) +{ + memset(&s_nav, 0, sizeof(s_nav)); + s_nav.cfg = *cfg; + s_nav.stage = GNAV_IDLE; + s_nav.running = false; + s_nav.initialized = true; + s_last_odom_vx = 0.0f; + s_last_update_ms = 0; +} + +void GlobalNav_Start(void) +{ + if (!s_nav.initialized) return; + s_nav.running = true; + s_nav.corridors_completed = 0; + s_nav.odom_distance_accum = 0.0f; + s_nav.stage_entry_odom_vx_accum = 0.0f; + s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id; + /* 不在这里 transition_to，因为还没有 board 数据。 + 下一个 Update 周期里发现 running && IDLE 时再 transition。 */ + s_nav.stage = GNAV_IDLE; +} + +void GlobalNav_Stop(void) +{ + s_nav.running = false; + s_nav.stage = GNAV_FINISHED; +} + +void GlobalNav_Reset(void) +{ + s_nav.stage = GNAV_IDLE; + s_nav.running = false; + s_nav.corridors_completed = 0; + s_nav.odom_distance_accum = 0.0f; +} + +GlobalNavStage_t GlobalNav_GetStage(void) +{ + return s_nav.stage; +} + +const char* GlobalNav_GetStageName(GlobalNavStage_t stage) +{ + if (stage <= GNAV_ERROR) { + return s_stage_names[stage]; + } + return "UNKNOWN"; +} + +/* ========================================================= + * 核心 Update + * ========================================================= */ +void GlobalNav_Update(const CorridorObs_t* obs, + const CorridorState_t* state, + const RobotBlackboard_t* board, + GlobalNavOutput_t* out) +{ + if (!s_nav.initialized) { + memset(out, 0, sizeof(*out)); + return; + } + + uint32_t now_ms = s_last_update_ms; + /* 用 HAL_GetTick 代理 — 实际上 app_tasks 会传入 board 里的时间 */ + { + /* 里程计积分: Δd = vx * dt */ + float odom_vx = board->odom_vx; + if (s_last_update_ms > 0) { + /* 使用 board 中的时间戳估计当前时间 */ + uint32_t cur_ms = board->imu_wz.timestamp_ms; + if (cur_ms == 0) cur_ms = s_last_update_ms + 20U; + float dt = (float)(cur_ms - s_last_update_ms) * 0.001f; + if (dt > 0.0f && dt < 0.5f) { + s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt; + } + now_ms = cur_ms; + } else { + now_ms = board->imu_wz.timestamp_ms; + if (now_ms == 0) now_ms = 1; + } + s_last_update_ms = now_ms; + s_last_odom_vx = odom_vx; + } + + float imu_yaw_deg = board->imu_yaw_continuous.is_valid + ? board->imu_yaw_continuous.value : 0.0f; + + /* 默认输出 */ + out->use_override = true; + out->request_corridor = false; + out->override_v = 0.0f; + out->override_w = 0.0f; + out->safety_mode = SAFETY_MODE_IDLE; + out->stage = s_nav.stage; + out->corridor_id = s_nav.current_corridor_id; + out->corridors_done = s_nav.corridors_completed; + out->active = s_nav.running; + out->stage_name = GlobalNav_GetStageName(s_nav.stage); + + if (!s_nav.running) return; + + /* IDLE → 自动进入 ENTRY_STRAIGHT */ + if (s_nav.stage == GNAV_IDLE) { + transition_to(GNAV_ENTRY_STRAIGHT, board); + out->stage = s_nav.stage; + out->stage_name = GlobalNav_GetStageName(s_nav.stage); + } + + uint32_t elapsed_ms = now_ms - s_nav.stage_start_ms; + + switch (s_nav.stage) { + + /* ============================================================ + * 入场直线 + * ============================================================ */ + case GNAV_ENTRY_STRAIGHT: + out->override_v = s_nav.cfg.entry_v; + out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (side_walls_detected(obs) || + odom_since_entry() >= s_nav.cfg.entry_distance || + elapsed_ms > s_nav.cfg.entry_timeout_ms) + { + transition_to(GNAV_TURN_INTO_CORRIDOR, board); + } + break; + + /* ============================================================ + * 三种转向状态统一处理 + * ============================================================ */ + case GNAV_TURN_INTO_CORRIDOR: + case GNAV_TURN_OUT_OF_CORRIDOR: + case GNAV_TURN_INTO_NEXT: + execute_turn(obs, state, board, now_ms, out); + break; + + /* ============================================================ + * 重捕获走廊 + * ============================================================ */ + case GNAV_REACQUIRE: + out->override_v = s_nav.cfg.reacquire_v; + out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (check_reacquire(obs, state)) { + s_nav.reacquire_ok_count++; + } else { + s_nav.reacquire_ok_count = 0; + } + + if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) { + transition_to(GNAV_CORRIDOR_TRACK, board); + } + if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) { + transition_to(GNAV_ERROR, board); + } + break; + + /* ============================================================ + * 沟内闭环跟踪 (交给 corridor_ctrl) + * ============================================================ */ + case GNAV_CORRIDOR_TRACK: + out->use_override = false; + out->request_corridor = true; + out->safety_mode = SAFETY_MODE_CORRIDOR; + + /* 到端检测 */ + { + bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; + if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) { + /* 里程下限保护: 至少走了 1.0m 才允许认定到端，避免假阳性 */ + float corridor_odom = odom_since_entry(); + if (corridor_odom > 1.0f) { + s_nav.corridors_completed++; + transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board); + } + } + } + /* 里程超长保护 */ + if (odom_since_entry() > s_nav.cfg.corridor_length_max) { + s_nav.corridors_completed++; + transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board); + } + break; + + /* ============================================================ + * 连接段直行 + * ============================================================ */ + case GNAV_LINK_STRAIGHT: + out->override_v = s_nav.cfg.link_v; + out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + 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); + } + if (elapsed_ms > s_nav.cfg.link_timeout_ms) { + transition_to(GNAV_ERROR, board); + } + break; + + /* ============================================================ + * 出场直行 + * ============================================================ */ + case GNAV_EXIT_STRAIGHT: + out->override_v = s_nav.cfg.exit_v; + out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + /* 检测侧向全丢 */ + if (!s_nav.exit_vl53_lost && all_side_lost(obs)) { + s_nav.exit_vl53_lost = true; + s_nav.exit_lost_distance = s_nav.odom_distance_accum; + } + if (s_nav.exit_vl53_lost) { + float since_lost = s_nav.odom_distance_accum - s_nav.exit_lost_distance; + if (since_lost >= s_nav.cfg.exit_runout) { + transition_to(GNAV_DOCK, board); + } + } + /* 里程上限保护 */ + if (odom_since_entry() >= s_nav.cfg.exit_max_dist) { + transition_to(GNAV_DOCK, board); + } + if (elapsed_ms > s_nav.cfg.exit_timeout_ms) { + transition_to(GNAV_DOCK, board); + } + break; + + /* ============================================================ + * 回停启动区 + * ============================================================ */ + case GNAV_DOCK: + out->override_v = s_nav.cfg.dock_v; + out->override_w = 0.0f; + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (odom_since_entry() >= s_nav.cfg.dock_distance || + elapsed_ms > 5000U) + { + transition_to(GNAV_FINISHED, board); + } + break; + + /* ============================================================ + * 终态 + * ============================================================ */ + case GNAV_FINISHED: + out->override_v = 0.0f; + out->override_w = 0.0f; + out->safety_mode = SAFETY_MODE_IDLE; + out->active = false; + break; + + /* ============================================================ + * 异常态 + * ============================================================ */ + case GNAV_ERROR: + out->override_v = 0.0f; + out->override_w = 0.0f; + out->safety_mode = SAFETY_MODE_IDLE; + + if (elapsed_ms > 2000U) { + transition_to(GNAV_FINISHED, board); + } + break; + + default: + out->override_v = 0.0f; + out->override_w = 0.0f; + break; + } + + /* 更新输出阶段 (可能在 switch 内已经 transition) */ + out->stage = s_nav.stage; + out->stage_name = GlobalNav_GetStageName(s_nav.stage); + out->corridor_id = s_nav.current_corridor_id; + out->corridors_done = s_nav.corridors_completed; + out->active = s_nav.running; +} diff --git a/App/nav/global_nav.h b/App/nav/global_nav.h new file mode 100644 index 0000000..21c8d9a --- /dev/null +++ b/App/nav/global_nav.h @@ -0,0 +1,138 @@ +/** + * @file global_nav.h + * @brief 赛道级总控 — 混合导航状态机 + * + * 三层架构中的最上层： + * 上层 (本模块): 知道"当前第几沟、下一沟是谁、该往哪转" + * 中层 (内嵌): 转向/连接段/重捕获动作执行 + * 下层 (已有): 沟内 corridor_ctrl 闭环 + * + * 替代原 nav_script.c 的赛道编排职能。 + */ + +#ifndef GLOBAL_NAV_H +#define GLOBAL_NAV_H + +#include +#include +#include "preproc/corridor_msgs.h" +#include "Contract/robot_blackboard.h" +#include "nav/track_map.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================= + * 赛道级阶段枚举 + * ========================================================= */ +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; + +/* ========================================================= + * 配置参数 + * ========================================================= */ +typedef struct { + /* 入场段 */ + float entry_v; /* 入场速度 m/s */ + float entry_distance; /* 入场里程上限 m */ + uint32_t entry_timeout_ms; /* 入场超时 ms */ + + /* 转向 */ + float turn_omega; /* 转向角速度 rad/s */ + float turn_tolerance_rad; /* 转向完成容差 rad */ + float turn_decel_zone_rad; /* 接近目标角时的减速区 rad */ + float turn_min_omega; /* 减速区最低角速度 rad/s */ + uint32_t turn_timeout_ms; /* 单次转向超时 ms */ + + /* 重捕获 */ + float reacquire_v; /* 重捕获速度 m/s */ + float reacquire_conf_thresh; /* 重捕获置信度阈值 */ + float reacquire_width_tol; /* 走廊宽度容差 m */ + uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */ + uint32_t reacquire_timeout_ms; + + /* 沟内 */ + float corridor_end_detect_dist; /* 到端检测距离 m */ + float corridor_length_max; /* 沟内里程保护上限 m */ + + /* 连接段 */ + float link_v; /* 连接段速度 m/s */ + float link_distance; /* 连接段标称距离 m */ + uint32_t link_timeout_ms; + + /* 出场 */ + float exit_v; /* 出场速度 m/s */ + float exit_runout; /* 出场冲刺距离 m */ + float exit_max_dist; /* 出场里程保护 m */ + uint32_t exit_timeout_ms; + + /* 回停 */ + float dock_v; /* 回停速度 m/s */ + float dock_distance; /* 回停距离 m */ + + /* 航向保持 */ + float heading_kp; /* 航向保持P增益 */ + + /* 走廊宽度 (用于重捕获判定) */ + float corridor_width; /* 走廊宽度 m */ +} GlobalNavConfig_t; + +/* ========================================================= + * 输出结构 + * ========================================================= */ +typedef struct { + GlobalNavStage_t stage; /* 当前阶段 */ + const char* stage_name; /* 阶段名字符串 (调试) */ + uint8_t corridor_id; /* 当前/目标垄沟 ID */ + uint8_t corridors_done; /* 已完成垄沟数 */ + + bool request_corridor; /* 是否请求沟内闭环 */ + bool use_override; /* 是否用 override 指令 */ + float override_v; + float override_w; + + SafetyMode_t safety_mode; /* 当前安全模式 */ + bool active; /* 导航是否在运行 */ +} GlobalNavOutput_t; + +/* ========================================================= + * API + * ========================================================= */ +void GlobalNav_Init(const GlobalNavConfig_t* cfg); +void GlobalNav_Start(void); +void GlobalNav_Stop(void); +void GlobalNav_Reset(void); + +/** + * @brief 核心函数：每个导航周期调用一次 + * @param obs 预处理层的观测快照 + * @param state EKF 走廊状态 + * @param board 黑板快照 (读 IMU yaw_continuous, odom) + * @param out 导航输出 + */ +void GlobalNav_Update(const CorridorObs_t* obs, + const CorridorState_t* state, + const RobotBlackboard_t* board, + GlobalNavOutput_t* out); + +GlobalNavStage_t GlobalNav_GetStage(void); +const char* GlobalNav_GetStageName(GlobalNavStage_t stage); + +#ifdef __cplusplus +} +#endif + +#endif /* GLOBAL_NAV_H */ diff --git a/App/nav/segment_fsm.c b/App/nav/segment_fsm.c index 1e6a256..a773a44 100644 --- a/App/nav/segment_fsm.c +++ b/App/nav/segment_fsm.c @@ -48,6 +48,7 @@ const char* SegFsm_GetStateName(SegFsmState_t s) void SegFsm_Update(const RawCmd_t *raw_cmd, const CorridorObs_t *obs, const CorridorState_t *state, + SafetyMode_t mode, SegFsmOutput_t *out) { if (!s_initialized) { @@ -58,18 +59,9 @@ void SegFsm_Update(const RawCmd_t *raw_cmd, } /* ======================================================== - * 1. 全局安全断言：无论在什么状态，满足条件直接超驰 + * 0. IDLE 模式：零速，不做任何裁剪 * ======================================================== */ - - /* 1a. 置信度极低 → 紧急停车 */ - if (state->conf < s_cfg.conf_estop_thresh) { - s_state = SEG_STATE_ESTOP; - } - - /* ======================================================== - * 2. 待命状态：什么都不做 - * ======================================================== */ - if (s_state == SEG_STATE_IDLE) { + if (mode == SAFETY_MODE_IDLE) { out->state = SEG_STATE_IDLE; out->safe_v = 0.0f; out->safe_w = 0.0f; @@ -77,14 +69,43 @@ void SegFsm_Update(const RawCmd_t *raw_cmd, } /* ======================================================== - * 3. 紧急停车状态：等待置信度恢复 + * 1. TURN 模式：原地转向，直接放行，不检查前距和置信度 + * 解决 RISK-1: 转向阶段不会被安全层卡死 * ======================================================== */ + if (mode == SAFETY_MODE_TURN) { + s_state = SEG_STATE_CORRIDOR; + out->state = SEG_STATE_CORRIDOR; + out->safe_v = raw_cmd->v; + out->safe_w = raw_cmd->w; + return; + } + + /* ======================================================== + * 2. CORRIDOR 模式：完整安全检查 (前向 + 置信度) + * STRAIGHT 模式：前向检查，但不检查置信度 + * ======================================================== */ + + /* 2a. 置信度检查：仅 CORRIDOR 模式下生效 */ + if (mode == SAFETY_MODE_CORRIDOR) { + if (state->conf < s_cfg.conf_estop_thresh) { + s_state = SEG_STATE_ESTOP; + } + } + + /* 待命状态 */ + if (s_state == SEG_STATE_IDLE) { + out->state = SEG_STATE_IDLE; + out->safe_v = 0.0f; + out->safe_w = 0.0f; + return; + } + + /* 紧急停车状态：等待置信度恢复 (仅 CORRIDOR 模式可能进入) */ if (s_state == SEG_STATE_ESTOP) { out->state = SEG_STATE_ESTOP; out->safe_v = 0.0f; out->safe_w = 0.0f; - /* 自动恢复开关：当置信度重新回到安全区且前方有路，恢复巡航 */ if (state->conf >= 0.5f) { s_state = SEG_STATE_CORRIDOR; } @@ -92,45 +113,34 @@ void SegFsm_Update(const RawCmd_t *raw_cmd, } /* ======================================================== - * 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP + * 3. 前向距离判定：CORRIDOR 和 STRAIGHT 都做 * ======================================================== */ bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; float d_front = obs->d_front; if (front_valid) { if (d_front <= s_cfg.d_front_stop) { - /* 距离过近：到端停车 */ s_state = SEG_STATE_STOP; } else if (d_front <= s_cfg.d_front_approach) { - /* 进入减速预警区 */ s_state = SEG_STATE_APPROACH; } else { - /* 前方安全：正常走廊跟踪 */ if (s_state == SEG_STATE_STOP || s_state == SEG_STATE_APPROACH) { s_state = SEG_STATE_CORRIDOR; } } - } else { - /* 前向雷达失效：保守策略，保持当前状态但降速 - * (不往 STOP 跳，避免前雷达偶发掉线就急停) */ } /* ======================================================== - * 5. 根据当前状态输出最终安全速度 + * 4. 根据当前状态输出最终安全速度 * ======================================================== */ switch (s_state) { case SEG_STATE_CORRIDOR: { - /* 正常放行控制器输出 */ out->safe_v = raw_cmd->v; out->safe_w = raw_cmd->w; break; } case SEG_STATE_APPROACH: { - /* 线性插值减速: - * 当 d_front 从 approach 距离滑向 stop 距离时， - * 速度从 raw_cmd->v 线性衰减至 approach_min_v - */ float range = s_cfg.d_front_approach - s_cfg.d_front_stop; float ratio = 1.0f; if (range > 0.001f) { @@ -139,13 +149,12 @@ void SegFsm_Update(const RawCmd_t *raw_cmd, } float v_scaled = s_cfg.approach_min_v + ratio * (raw_cmd->v - s_cfg.approach_min_v); out->safe_v = seg_clampf(v_scaled, 0.0f, raw_cmd->v); - out->safe_w = raw_cmd->w; // 角速度不限，仍然允许纠偏 + out->safe_w = raw_cmd->w; break; } case SEG_STATE_STOP: default: { - /* 到端停车 / 紧急停车：零速 */ out->safe_v = 0.0f; out->safe_w = 0.0f; break; diff --git a/App/nav/segment_fsm.h b/App/nav/segment_fsm.h index bb9e914..08a8744 100644 --- a/App/nav/segment_fsm.h +++ b/App/nav/segment_fsm.h @@ -54,11 +54,13 @@ void SegFsm_Start(void); * @param raw_cmd 走廊控制器的原始输出 (v, w) * @param obs 预处理层的观测快照 (提供 d_front 和 valid_mask) * @param state 滤波器的状态输出 (提供 conf 置信度) + * @param mode 安全模式 (CORRIDOR/TURN/STRAIGHT/IDLE)，由赛道级导航指定 * @param out 安全仲裁后的最终输出 */ void SegFsm_Update(const RawCmd_t *raw_cmd, const CorridorObs_t *obs, const CorridorState_t *state, + SafetyMode_t mode, SegFsmOutput_t *out); /** diff --git a/App/nav/track_map.c b/App/nav/track_map.c new file mode 100644 index 0000000..927b37e --- /dev/null +++ b/App/nav/track_map.c @@ -0,0 +1,78 @@ +/** + * @file track_map.c + * @brief 赛道地图 — S 型遍历拓扑表 + * + * S 型路径: + * C1(→) → 左转 → 连接 → 左转 → C2(←) → 右转 → 连接 → 右转 → + * C3(→) → 左转 → 连接 → 左转 → C4(←) → 右转 → 连接 → 右转 → + * C5(→) → 左转 → 连接 → 左转 → C6(←) → 左转 → 出场 + */ + +#include "track_map.h" + +/* ========================================================= + * 硬编码 S 型遍历表 + * ========================================================= */ +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: ← 最后一条, 左转出场 */ + }, + .entry_corridor_id = 0, + .link_distance_m = TRACK_MAP_LINK_DISTANCE_M, + .corridor_length_m = TRACK_MAP_CORRIDOR_LENGTH_M, +}; + +/* ========================================================= + * API 实现 + * ========================================================= */ + +void TrackMap_Init(void) +{ + /* 静态表，无需运行时初始化 */ +} + +const TrackMap_t* TrackMap_Get(void) +{ + return &s_map; +} + +const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id) +{ + if (id >= TRACK_MAP_CORRIDOR_COUNT) { + return &s_map.corridors[0]; + } + return &s_map.corridors[id]; +} + +uint8_t TrackMap_GetNextCorridorId(uint8_t current_id) +{ + if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) { + return current_id; /* 已是最后一条 */ + } + return current_id + 1; +} + +bool TrackMap_IsLastCorridor(uint8_t id) +{ + if (id >= TRACK_MAP_CORRIDOR_COUNT) return true; + return s_map.corridors[id].is_last; +} + +TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id) +{ + if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT; + return s_map.corridors[id].exit_turn_dir; +} + +TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id) +{ + if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT; + return s_map.corridors[id].entry_turn_dir; +} diff --git a/App/nav/track_map.h b/App/nav/track_map.h new file mode 100644 index 0000000..9b9ae95 --- /dev/null +++ b/App/nav/track_map.h @@ -0,0 +1,80 @@ +/** + * @file track_map.h + * @brief 赛道地图 — 固化 S 型遍历拓扑 + * + * 地图不做全局坐标定位，只回答三个问题： + * 1. 从第 N 条沟完成后，下一条是第几条？ + * 2. 这次该往哪转？(左/右) + * 3. 当前是不是最后一条沟？ + */ + +#ifndef TRACK_MAP_H +#define TRACK_MAP_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================= + * 常量 + * ========================================================= */ +#define TRACK_MAP_CORRIDOR_COUNT 6 /* 总共 6 条垄沟 */ +#define TRACK_MAP_LINK_DISTANCE_M 0.70f /* 沟间距 (中心到中心) */ +#define TRACK_MAP_CORRIDOR_LENGTH_M 2.20f /* 垄沟标称长度 */ + +/* ========================================================= + * 枚举 + * ========================================================= */ + +/** 沟内行驶方向 */ +typedef enum { + TRAVEL_DIR_POSITIVE = 0, /* 从左端到右端 (→) */ + TRAVEL_DIR_NEGATIVE = 1 /* 从右端到左端 (←) */ +} TravelDirection_t; + +/** 转向方向 */ +typedef enum { + TURN_DIR_LEFT = +1, /* 逆时针 (CCW), w > 0 */ + TURN_DIR_RIGHT = -1 /* 顺时针 (CW), w < 0 */ +} TurnDirection_t; + +/* ========================================================= + * 数据结构 + * ========================================================= */ + +/** 单条垄沟描述 */ +typedef struct { + uint8_t id; /* 0-5 */ + TravelDirection_t travel_dir; /* 本沟行驶方向 */ + TurnDirection_t exit_turn_dir; /* 出沟时的转向方向 */ + TurnDirection_t entry_turn_dir; /* 入沟时的转向方向 */ + bool is_last; /* 是否为最后一条沟 */ +} CorridorDescriptor_t; + +/** 完整赛道地图 */ +typedef struct { + CorridorDescriptor_t corridors[TRACK_MAP_CORRIDOR_COUNT]; + uint8_t entry_corridor_id; /* 入场后第一条沟 = 0 */ + float link_distance_m; /* 连接段标称距离 */ + float corridor_length_m; /* 垄沟标称长度 */ +} TrackMap_t; + +/* ========================================================= + * 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); + +#ifdef __cplusplus +} +#endif + +#endif /* TRACK_MAP_H */ diff --git a/App/preproc/corridor_msgs.h b/App/preproc/corridor_msgs.h index d280089..a56640f 100644 --- a/App/preproc/corridor_msgs.h +++ b/App/preproc/corridor_msgs.h @@ -55,4 +55,14 @@ typedef struct { uint8_t flags; // 控制特殊标志位 } RawCmd_t; +/* ========================================================= + * 安全模式枚举 (赛道级导航 → 安全状态机 的模式指示) + * ========================================================= */ +typedef enum { + SAFETY_MODE_IDLE = 0, // 零速，不做任何裁剪 + SAFETY_MODE_CORRIDOR, // 沟内: 前向减速/停车/E-STOP 全开 + SAFETY_MODE_TURN, // 转向: 允许 v=0+w!=0, 前向不全停, 不检查 conf + SAFETY_MODE_STRAIGHT // 直行段: 前后激光防撞, 不检查 conf +} SafetyMode_t; + #endif // CORRIDOR_MSGS_H \ No newline at end of file diff --git a/App/robot_params.h b/App/robot_params.h index c560c25..39940c5 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -371,6 +371,54 @@ extern "C" { */ #define PARAM_IMU_YAW_OFFSET 0.0f +/* ========================================================= + * 【P6】赛道级导航参数 (混合导航方案) + * ========================================================= */ + +/** @brief 编译开关：1=赛道模式(6沟S型遍历) 0=单沟测试模式(原nav_script) */ +#define USE_GLOBAL_NAV 1 + +/* --- 入场段 --- */ +#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 */ +#define PARAM_GNAV_ENTRY_DISTANCE 0.65f /* m — 入场里程上限 */ +#define PARAM_GNAV_ENTRY_TIMEOUT 10000U /* ms — 入场超时 */ + +/* --- 转向 --- */ +#define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */ +#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */ +#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */ +#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */ +#define PARAM_GNAV_TURN_TIMEOUT 8000U /* ms — 单次转向超时 */ + +/* --- 重捕获 --- */ +#define PARAM_GNAV_REACQUIRE_V 0.05f /* m/s — 重捕获入沟速度 */ +#define PARAM_GNAV_REACQUIRE_CONF 0.6f /* — 重捕获置信度阈值 */ +#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f /* m — 走廊宽度容差 */ +#define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数 (5×20ms=100ms) */ +#define PARAM_GNAV_REACQUIRE_TIMEOUT 5000U /* ms — 重捕获超时 */ + +/* --- 沟内 --- */ +#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.50f /* m — 沟内里程保护上限 */ +#define PARAM_GNAV_CORRIDOR_END_DIST 0.10f /* m — 到端检测距离 */ + +/* --- 连接段 --- */ +#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 */ +#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距) */ +#define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */ + +/* --- 出场 --- */ +#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_DOCK_V 0.05f /* m/s — 回停速度 */ +#define PARAM_GNAV_DOCK_DISTANCE 0.50f /* m — 回停推进距离 */ + +/* --- 航向保持 --- */ +#define PARAM_GNAV_HEADING_KP 0.03f /* — — 航向保持P增益 (输入°，输出rad/s) */ + /* ========================================================= * 宏工具函数 * ========================================================= */ diff --git a/Doc/CODE_REVIEW.md b/Doc/CODE_REVIEW.md deleted file mode 100644 index c6eb5a6..0000000 --- a/Doc/CODE_REVIEW.md +++ /dev/null @@ -1,397 +0,0 @@ -# ARES 项目代码审查报告 - -> **审查日期**: 2025 年 -> **审查范围**: `/App` 目录下全部业务代码（排除 `VL53L0X_API/core` ST 官方库） -> **审查基准**: HANDOFF.md 中的设计描述与已修复 BUG 列表 -> **审查重点**: 残留 BUG、潜在运行时风险、代码质量问题 - ---- - -## 审查总结 - -| 严重等级 | 数量 | 说明 | -|---------|------|------| -| 🔴 严重 (可能导致功能错误/崩溃) | 4 | 运行时会导致错误行为 | -| 🟠 中等 (潜在风险/隐患) | 6 | 特定条件下可能触发问题 | -| 🟡 低 (代码质量/可维护性) | 7 | 不影响功能但应改善 | - -**总体评价**: 项目架构设计清晰，HANDOFF.md 中列出的 9 个严重 BUG 和 7 个质量问题均已正确修复。但仍存在若干残留问题，以下逐一分析。 - ---- - -## 🔴 严重问题 - -### S-1: VL53L0X 卡尔曼滤波 Q/R 参数硬编码，未使用 `robot_params.h` 配置 - -**文件**: `VL53L0X_API/platform/vl53_board.c:84` - -```c -/* 初始化卡尔曼滤波器：默认 Q=1.0, R=9.5 */ // ← 注释过期 -vl53_kalman_init(&board->kf[i], 10.0f, 14.1f); // ← 硬编码值 -``` - -`robot_params.h` 中定义了 `PARAM_VL53_KALMAN_Q = 10.0f` 和 `PARAM_VL53_KALMAN_R = 14.1f`，但 `vl53_board.c` 没有 `#include "robot_params.h"`，而是直接写死了 `10.0f` 和 `14.1f`。虽然当前值恰好与参数一致，但**修改 `robot_params.h` 中的 `PARAM_VL53_KALMAN_Q/R` 不会生效**，与 BUG-2 (EKF 参数硬编码) 属于同类问题。 - -**修复建议**: -```c -#include "robot_params.h" -// ... -vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R); -``` - ---- - -### S-2: `LASER_SIMPLE_GetSnapshot()` 返回指针无线程安全保护，存在数据撕裂风险 - -**文件**: `laser/laser_manager.c:277-279` - -```c -const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void) { - return &g_snapshot; // ← 返回原始指针，无拷贝、无临界区保护 -} -``` - -消费者 `AppTasks_RunLaserTestTask_Impl()` 调用此函数获取指针后直接传给 `Blackboard_UpdateLaser(snap)`。由于 `LaserTask`（内部10ms任务）在 `taskENTER_CRITICAL()` 中**逐通道写入** `g_snapshot.ch[i]`，而消费者拿到的是裸指针，读取可能发生在写入的间隙——比如前两个通道已更新、后两个通道还是旧值——构成**部分撕裂**。 - -虽然 `Blackboard_UpdateLaser` 内部有临界区保护，但问题出在**读 g_snapshot 的时候没有保护**。当前架构中 `laserTestTask` 以 50ms 周期读取、`LaserTask` 以 10ms 周期写入，两者优先级相同，可能发生抢占。 - -**影响等级**: 中高。实际撕裂概率取决于临界区外的读取窗口大小。由于 `laser_simple_snapshot_t` 只包含 4 个 `laser_simple_data_t`，单次 `memcpy` 耗时极短，实际风险较低。但从防御性编程角度应修复。 - -**修复建议**: -```c -void LASER_SIMPLE_GetSnapshotCopy(laser_simple_snapshot_t *out) { - taskENTER_CRITICAL(); - *out = g_snapshot; - taskEXIT_CRITICAL(); -} -``` - ---- - -### S-3: `snc_parse_odom_delta()` 在 ISR 中写 `odom_accum` 无临界区保护 - -**文件**: `Can/snc_can_app.c:148-173` (在 `SNC_CAN_RxFifo0Callback` 即 CAN ISR 中被调用) - -```c -static void snc_parse_odom_delta(const uint8_t *d) -{ - // ... 直接写 g_snc_can_app.odom_accum 的各个字段 ... - SNC_OdomDeltaAccum_t *acc = &g_snc_can_app.odom_accum; - acc->fl_accum += (int32_t)snc_read_i16_le(d[0], d[1]); - // ... - if (acc->frame_count < 255U) { - acc->frame_count++; - } -} -``` - -而消费侧 `SNC_CAN_ConsumeOdomDelta()` 使用 `taskENTER_CRITICAL()` 保护读取和清零。问题是：**`taskENTER_CRITICAL()` 在 Cortex-M 上通过 BASEPRI 屏蔽中断，只屏蔽优先级不高于 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 的中断**。如果 CAN FIFO0 中断的优先级高于此阈值（即数值更小），那么 `snc_parse_odom_delta()` 可能在消费者持有临界区期间执行，导致**竞态条件**：消费者清零后，ISR 立刻写入新数据，然后消费者返回的 `snapshot` 中 `frame_count=0` 但 `accum` 值已非零。 - -**影响等级**: 取决于 CAN 中断优先级配置。如果 CAN 中断优先级在 FreeRTOS 管理范围内（优先级数值 ≥ `configMAX_SYSCALL_INTERRUPT_PRIORITY`），则 `taskENTER_CRITICAL()` 可以正确屏蔽它，问题不存在。**需要检查 CubeMX 中 FDCAN1 中断优先级配置**。 - -**修复建议**: 确认 FDCAN1 中断优先级 ≥ `configMAX_SYSCALL_INTERRUPT_PRIORITY`。或在 `snc_parse_odom_delta()` 中也使用 `taskENTER_CRITICAL()`（但 ISR 中应使用 `taskENTER_CRITICAL_FROM_ISR()`）。 - ---- - -### S-4: `corridor_msgs.h` 使用 `#pragma pack(push, 1)` 导致 EKF 矩阵和浮点运算性能损失及潜在对齐异常 - -**文件**: `preproc/corridor_msgs.h:8,61` - -```c -#pragma pack(push, 1) -// ... -typedef struct { - float P[3][3]; // 36 字节的浮点矩阵，被强制 1 字节对齐 - float innovation[3]; // 12 字节 - float mahalanobis_d2; - // ... -} CorridorState_t; -// ... -#pragma pack(pop) -``` - -`CorridorState_t` 包含大量 `float` 和 `float[3][3]` 数组，被 `#pragma pack(push, 1)` 强制 1 字节对齐。在 Cortex-M7 上： -1. **性能损失**: 未对齐的 float 访问会触发硬件 unaligned access 处理，速度比对齐访问慢数倍。这个结构体在 navTask 的 20ms 循环中被高频读写，影响实时性能。 -2. **FPU 指令风险**: 某些 FPU 指令（如 `VLDM`/`VSTM`）**要求 4 字节对齐**，不对齐会触发 UsageFault。虽然 GCC 通常会生成安全的加载指令，但编译器优化可能引入向量化或批量加载指令。 - -`CorridorObs_t` 和 `RawCmd_t` 也受影响，但内部结构更简单，风险较低。 - -**修复建议**: `#pragma pack(push, 1)` 仅用于需要匹配物理总线帧格式的结构体（如 `chassis_can_msg.h` 中的 CAN 帧，那是正确的用法）。`corridor_msgs.h` 中的结构体是内存中的数据传递，**不需要 pack(1)**，应删除。 - ---- - -## 🟠 中等问题 - -### M-1: IMU 帧解析状态机缺少帧类型 0x55 之后的 header byte 校验 - -**文件**: `IMU/hwt101.c:64-68` - -```c -if (s_frame_idx == 0 && byte != 0x55) continue; -s_frame[s_frame_idx++] = byte; -if (s_frame_idx >= 11) { -``` - -当前状态机只检查首字节 `0x55`，然后无条件接收后续 10 字节。如果数据流中出现非帧头的 `0x55`（比如校验和恰好是 `0x55`），会导致帧错位。虽然最后有校验和检查可以过滤错误帧，但会**浪费 10 个字节的解析窗口**，在高频数据流中可能导致短暂的数据更新延迟。 - -**修复建议**: 在 `s_frame_idx == 1` 时检查第二字节是否为有效帧类型 (`0x51`/`0x52`/`0x53`)，不匹配则 `s_frame_idx = 0` 重新寻帧。 - ---- - -### M-2: `nav_script.c` 转向方向始终为正，不支持反向 (顺时针) 180° 转弯 - -**文件**: `nav/nav_script.c:202,229` - -```c -float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度) -// ... -float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f; -``` - -`turn_target_angle` 在 `app_tasks.c` 中始终被初始化为 `3.14159265f`（正值），所以 `turn_dir` 永远为 `1.0f`（逆时针）。如果因场地布局需要顺时针转弯，当前代码无法支持。更关键的是，**两次 180° 转弯方向相同**——第一次到端逆时针转，第二次到端又逆时针转，结果机器人回到出发方向。这可能是设计意图（转完 180° 后继续正向走），但如果走廊空间不对称，固定转向方向可能导致转弯时撞墙。 - -**修复建议**: 考虑根据当前方向或走廊几何自动选择转向方向，或至少在 `NavScriptConfig_t` 中加一个 `turn_direction` 参数。 - ---- - -### M-3: `Odom_GetSpeed()` 在临界区外读取 `s_odom.vx` 和 `s_odom.wz` - -**文件**: `Contract/robot_odom.c:122-124` - -```c -void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status) -{ - if (out_vx != NULL) *out_vx = s_odom.vx; // ← 临界区外 - if (out_wz != NULL) *out_wz = s_odom.wz; // ← 临界区外 - if (out_status != NULL) { - lock_odom(); // ← 只有 status 在临界区内 - // ... - unlock_odom(); - } -} -``` - -`s_odom.vx` 和 `s_odom.wz` 是 `float` 类型，在 Cortex-M7 上 32 位对齐的 float 读写是原子的，所以单独读取不会撕裂。但 `vx` 和 `wz` 之间不是原子的——可能读到旧的 `vx` 和新的 `wz`。不过此函数当前未被任何代码调用（速度通过 `Blackboard_UpdateOdom` 传递），所以实际影响为零。 - -**修复建议**: 将 `vx/wz` 的读取也放入 `lock_odom()` 中，或标注此函数为 `@deprecated`。 - ---- - -### M-4: `SNC_CAN_SendHeartbeat()` 使用零长度数组 - -**文件**: `Can/snc_can_app.c:194` - -```c -HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void) -{ - uint8_t data[0]; // ← 零长度数组，C11 标准未定义行为 - HAL_StatusTypeDef ret = snc_fdcan_add_tx_std(SNC_CAN_ID_HEARTBEAT, data, 0U); -``` - -零长度数组 `uint8_t data[0]` 在 C11 标准中是未定义行为（C99 的 flexible array member 语法是 `[]` 且只能在结构体末尾）。虽然 GCC 作为扩展支持它，且 `snc_fdcan_add_tx_std` 传入 `dlc_bytes=0` 不会实际读取 `data`，但这仍属于未定义行为。 - -**修复建议**: 改为 `uint8_t data[1] = {0};` 或直接传 `NULL`（需确认 HAL 是否接受 NULL data）。 - ---- - -### M-5: 安全状态机 (SegFsm) 在脚本覆盖模式下仍然生效，可能干扰转弯等特殊动作 - -**文件**: `app_tasks.c` navTask 流水线 Step 5-6 - -```c -/* Step 5: 控制律 */ -if (script_out.use_override) { - raw_cmd.v = script_out.override_v; // 脚本指定速度 - raw_cmd.w = script_out.override_w; -} else if (script_out.request_corridor) { - CorridorCtrl_Compute(..., &raw_cmd); -} - -/* Step 6: 段状态机 -> 安全仲裁 */ -SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out); -``` - -当脚本处于 `TURN_AT_END` 阶段时，`use_override = true`，输出 `v=0, w=turn_omega`（原地转向）。但此时 EKF 因侧墙观测丢失，`conf` 可能降到 `< 0.1`，触发 `SegFsm` 进入 `E-STOP`，**强制将转向角速度归零**，导致机器人无法完成转弯。 - -虽然 `E-STOP` 有自动恢复机制（`conf >= 0.5` 时恢复），但在转弯过程中侧墙持续不可见，`conf` 不会恢复，造成**死锁**：转不了弯 → 永远看不到墙 → 永远 E-STOP。 - -**影响等级**: 实车中度风险。取决于转弯时 EKF 的 IMU 航向观测是否能维持 `conf >= 0.1`。如果 IMU 航向观测能阻止 `conf` 跌破阈值，则不会触发。但这是一个脆弱的依赖。 - -**修复建议**: 在 `SegFsm_Update()` 中增加一个 bypass 标志，当脚本处于 `use_override` 模式时跳过 E-STOP 判定；或仅在 `request_corridor` 为 true 时检查置信度。 - ---- - -### M-6: `process_complementary_laser()` 中 ATK 距离值无上限校验 - -**文件**: `preproc/corridor_preproc.c:30-73` - -```c -static bool process_complementary_laser(const SensorItem_t *stp, const SensorItem_t *atk, float *out_m) -{ - // ... - float atk_m = atk->value / 1000.0f; - // ATK 没有像 VL53 那样的量程校验 (0.02~2.0m) - // 如果 ATK 吐出异常大值 (如 65535mm)，会被当作有效数据 -``` - -`process_side_laser()` 对 VL53L0X 数据做了 `[0.02m, 2.0m]` 范围校验，但 `process_complementary_laser()` 对 ATK 和 STP 数据**没有上限校验**。如果 ATK 传感器吐出异常大值（如掉线前最后一帧的乱码），这个值会被当作有效距离传递给安全状态机，可能导致**该停车时不停车**。 - -**修复建议**: 添加 ATK/STP 的量程校验，如 `atk_m > 0.0f && atk_m <= 8.0f` (ATK 标称量程 4m，留双倍余量)。 - ---- - -## 🟡 低优先级 / 代码质量问题 - -### L-1: `PARAM_IMU_YAW_OFFSET` 声明但未使用 - -**文件**: `robot_params.h:100` - -```c -#define PARAM_IMU_YAW_OFFSET 0.0f // 声明了但代码中未使用 -``` - -HANDOFF.md CAL-4 已标注此问题。如果 IMU 安装有固定偏角，此参数应在 `HWT101_Process()` 或 `corridor_filter.c` 中使用。当前为死代码。 - ---- - -### L-2: `corridor_filter.c` 中 `s_imu_yaw_ref_set` 在 Reset/Init 时未重置 - -**文件**: `est/corridor_filter.c:23-24` - -```c -static float s_imu_yaw_ref_rad = 0.0f; -static bool s_imu_yaw_ref_set = false; -``` - -`CorridorFilter_Init()` 不会重置 `s_imu_yaw_ref_set`。如果比赛中途调用 `NavScript_Reset()` → `CorridorFilter_Init()` 重新初始化，旧的 `s_imu_yaw_ref_rad` 会保留，导致 IMU 航向观测使用过时的参考值。 - -**修复建议**: 在 `CorridorFilter_Init()` 末尾添加： -```c -s_imu_yaw_ref_rad = 0.0f; -s_imu_yaw_ref_set = false; -``` - ---- - -### L-3: `retarget.c` 中 `_write()` 的忙等循环可能阻塞调用任务 - -**文件**: `retarget.c:44-64` - -```c -int _write(int file, char *ptr, int len) -{ - while (1) { - result = CDC_Transmit_FS((uint8_t *)ptr, (uint16_t)len); - if (result == USBD_OK) return len; - if ((HAL_GetTick() - start_tick) > 20U) return 0; - if (osKernelGetState() == osKernelRunning) osDelay(1); - } -} -``` - -如果 USB CDC 端口忙碌（Host 未连接或 buffer 满），`printf` 会阻塞当前任务最多 20ms。对于 `navTask`（20ms 周期）和 `canTxTask`（20ms 周期），一次 `printf` 阻塞就可能导致**整个控制周期跳过**。当前 `App_PrintStatus()` 已被注释掉，但如果未来取消注释或在其他任务中添加 `printf`，可能造成问题。 - -**修复建议**: 在实时任务中避免使用 `printf`，或将 `_write` 改为非阻塞（丢弃模式）。 - ---- - -### L-4: 多个模块重复定义 `clampf()` 静态内联函数 - -**文件**: -- `est/corridor_ekf.c:42` -- `nav/corridor_ctrl.c:12` -- `nav/nav_script.c:34` -- `nav/segment_fsm.c:12` - -四个文件各自定义了相同的 `static inline float clampf()`。虽然由于 `static` 链接属性不会导致链接错误，但违反 DRY 原则。 - -**修复建议**: 提取到公共头文件 `robot_params.h` 或新建 `utils.h`。 - ---- - -### L-5: `CorridorObs_t.valid_mask` 类型为 `uint8_t` 但掩码使用 bit 0-5 共 6 位 - -**文件**: `preproc/corridor_msgs.h:31` 和 `corridor_preproc.h:10-15` - -当前 6 个掩码位刚好在 `uint8_t` 范围内（最大 bit 5 = 0x20），但余量很小。如果未来添加更多传感器（如第二组 VL53），很容易溢出。不紧急但值得注意。 - ---- - -### L-6: `vl53_board.c:84` 注释与实际值不一致 - -```c -/* 初始化卡尔曼滤波器：默认 Q=1.0, R=9.5 */ // ← 注释说 Q=1.0, R=9.5 -vl53_kalman_init(&board->kf[i], 10.0f, 14.1f); // ← 实际值 Q=10.0, R=14.1 -``` - -注释是旧版残留，与当前代码不符。 - ---- - -### L-7: `nav_script.c:285` 使用 `exit_start_s == 0.0f` 判断是否已触发，但 `s` 初始值就是 0 - -```c -if (s_internal.exit_start_s == 0.0f) { - s_internal.exit_start_s = state->s; -} -``` - -如果恰好在 `state->s ≈ 0.0` 时进入 EXIT 阶段（理论上不会，因为已经往返走过垄沟），`exit_start_s` 会被设为 0，然后下次循环 `== 0.0f` 再次为 true，重复赋值但不会出错（`state->s` 每次都差不多）。实际上因为 `memset(&s_internal, 0, ...)` 已将其初始化为 0.0，如果 EXIT 阶段被跳过再重入，可能出现意外行为。 - -**修复建议**: 使用 `bool exit_triggered` 标志代替浮点数零值判断。 - ---- - -## 架构设计审查 - -### 优点 - -1. **分层清晰**: 传感器驱动 → 黑板 → 预处理 → EKF → 控制 → 安全 → 指令槽，每层职责明确。 -2. **线程安全设计合理**: 黑板的 `taskENTER_CRITICAL` + snapshot 模式有效防止了数据撕裂。 -3. **传感器互补融合策略** (`process_complementary_laser`): STP+ATK 的互补逻辑考虑了盲区、单体故障、保守防撞等多种场景，设计周到。 -4. **EKF 实现质量高**: 鲁棒 χ² 检验、分级观测更新（1/2/3 DOF）、协方差保护等机制完备。BUG-7（卡尔曼增益矩阵乘法）的修复正确。 -5. **参数集中管理**: `robot_params.h` 作为唯一调参入口，大部分参数已正确引用（除 S-1 遗漏）。 -6. **里程计累加器设计** (BUG-8 修复): ISR 累加 + 原子取走清零的设计有效解决了漏积分/重复积分问题。 - -### 可改进方向 - -1. **缺少 Watchdog**: 系统没有硬件看门狗 (IWDG/WWDG)。如果任何任务死锁或 HardFault，MCU 将永久挂起而不会自动重启。建议在空闲任务中喂看门狗。 -2. **缺少运行时错误日志**: 当前的错误处理主要是"静默忽略"或"返回零值"。建议增加一个轻量级错误计数器或环形日志 buffer，方便赛后分析。 -3. **EKF 和安全状态机没有联动**: 如 M-5 所述，脚本覆盖模式下安全状态机可能干扰正常流程。建议在 `SegFsmOutput_t` 中增加 bypass 机制。 -4. **`laserTestTask` 的 `osDelay(50)` 不精确**: 使用 `osDelay` 而非 `osDelayUntil`，任务执行时间会累加到周期中，导致实际周期 > 50ms。对于激光数据更新频率有影响。 - ---- - -## 已修复 BUG 验证 - -| HANDOFF 编号 | 修复内容 | 验证结果 | -|-------------|---------|---------| -| BUG-1 | IMU wz deg→rad 转换 | ✅ `app_tasks.c:305` 使用 `PARAM_DEG2RAD()` | -| BUG-2 | EKF Q/R/P0 从 params 读取 | ✅ `corridor_filter.c:50-66` 使用 `PARAM_EKF_*` | -| BUG-3 | `SegFsm_Start()` 调用 | ✅ `app_tasks.c:382` | -| BUG-4 | BACKWARD 段使用 d_front | ✅ `nav_script.c:254` | -| BUG-5 | 超时使用配置参数 | ✅ `nav_script.c:157` | -| BUG-6 | EXIT 速度独立参数 | ✅ `nav_script.c:273` 使用 `s_cfg.exit_v` | -| BUG-7 | 完整矩阵乘法 K=PHT*S_inv | ✅ `corridor_ekf.c:576-607` 两步乘法 | -| BUG-8 | 里程计累加器模式 | ✅ `snc_can_app.c` ISR 累加 + `ConsumeOdomDelta` 原子取走 | -| BUG-9 | IMU yaw unwrap + 连续角度 | ✅ `hwt101.c` unwrap 逻辑 + `nav_script.c` 使用 IMU yaw | -| Q-1~Q-7 | 各项代码质量修复 | ✅ 全部验证通过 | - ---- - -## 修复优先级建议 - -| 优先级 | 编号 | 预估工时 | -|--------|------|---------| -| 立即修复 | S-4 (pack 对齐) | 5 分钟 | -| 立即修复 | S-1 (VL53 KF 参数) | 5 分钟 | -| 赛前修复 | M-5 (安全 FSM bypass) | 30 分钟 | -| 赛前修复 | M-6 (ATK 量程校验) | 10 分钟 | -| 赛前修复 | S-2 (快照线程安全) | 15 分钟 | -| 赛前修复 | L-2 (IMU ref 重置) | 5 分钟 | -| 确认配置 | S-3 (CAN 中断优先级) | 10 分钟 | -| 建议改进 | 其余所有 | 按需 | - ---- - -> **审查结论**: 项目整体质量良好，架构设计规范，历史 BUG 修复彻底。上述 S-4 和 S-1 建议在下次烧录前立即修复；M-5 (安全状态机与脚本冲突) 是实车测试中最可能暴露的问题，建议优先验证。 diff --git a/Doc/GLOBAL_NAV_REQUIREMENT.md b/Doc/GLOBAL_NAV_REQUIREMENT.md deleted file mode 100644 index 998e0b9..0000000 --- a/Doc/GLOBAL_NAV_REQUIREMENT.md +++ /dev/null @@ -1,667 +0,0 @@ -# 固定场地条件下的赛道级导航需求说明 - -## 1. 问题背景 - -当前项目已经具备较完整的“单条垄沟内局部导航”能力，包括： - -- 侧向测距支撑的走廊横向定位 -- IMU / EKF 支撑的航向估计 -- 前向距离触发的到端检测 -- 原地转向 -- 走廊内闭环控制 - -但正式比赛要求并不是“在一条走廊里走稳”这么简单，而是： - -- 遍历全部 `6` 条垄沟 -- 在端部完成换沟 -- 最终从唯一出入口驶离场地 -- 再停回启动区 - -因此，真正的问题已经从“局部走廊控制”升级成了“赛道级导航”。 - -本文件针对一个新的核心顾虑做说明： - -- 左右 `VL53L0X` 是近场侧向传感器，实际有效距离有限 -- 当小车走完第一条通道、完成转向、准备去第二条通道口时，下一条通道口可能仍在数米之外 -- 此时无法指望左右侧向 VL53 直接识别远处的下一条通道入口 - -由此引出新的设计问题： - -- 是否需要把当前导航从“局部走廊式”升级为“固定地图下的全局式导航” - -本文件只做需求分析与方案建议，不修改现有代码。 - -## 2. 比赛场地与问题是否成立 - -根据比赛规则文件 `附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md`： - -- 场地尺寸：`390cm x 300cm` -- 共有 `5` 条田垄 -- 每条田垄：长 `220cm`、宽 `30cm` -- 围栏与田垄之间、相邻田垄之间均为 `40cm` 垄沟 -- 实际需要遍历的是 `6` 条垄沟 -- 场地尺寸允许误差：`+-5%` - -结合 `HANDOFF.md` 的赛道理解： - -- 当前赛道本质上是 `6` 段平行窄走廊 -- 其间通过端部动作完成换沟 -- 最终还要完成出场和停回启动区 - -因此，你的顾虑是成立的： - -- 左右侧向 VL53 的作用域主要是在“贴着垄沟/墙体附近”时建立局部几何约束 -- 一旦小车离开当前垄沟、进入端部开阔区或横向换沟区，下一条垄沟入口往往不在 VL53 的可靠观测范围内 -- 所以不能把“下一条通道入口识别”这个任务建立在左右 VL53 的远距离探测能力上 - -这里需要特别澄清一个容易说错的点： - -- 赛道级运动并不一定是“在端部开阔区横移很长一段，再去远距离搜索下一条垄沟入口” -- 更符合当前场地理解的运动方式，是一种 **S 型串行入沟**： - 1. 从出发区直行到第 1 条垄沟入口附近 - 2. 第 1 条垄沟在车体一侧（例如右侧） - 3. 原地右转 `90°` 对准垄沟后入沟 - 4. 沿垄沟通过 - 5. 到端后原地左转 `90°`，进入端部直线连接段 - 6. 再前进一小段到下一条垄沟入口附近 - 7. 再原地左转 `90°` 入下一条垄沟 - 8. 之后重复形成 S 型遍历 - -也就是说： - -- 下一条垄沟并不是完全未知目标 -- 它通常位于“端部连接直线段之后的固定相邻位置” -- 问题核心不是远距离搜索，而是 **按固定几何完成 90° 转向 + 短直线推进 + 再次 90° 入沟** - -换句话说： - -- `VL53` 适合做局部跟墙/居中 -- 不适合独立承担赛道级换沟导航 - -## 3. 当前项目的能力边界 - -`HANDOFF.md` 已经明确指出： - -- 当前项目更像“单垄沟闭环验证系统” -- 还不是完整的 `6` 垄沟遍历赛道导航系统 - -当前已经具备的能力主要是： - -1. 垄沟内定位与控制 -2. 到端检测 -3. 原地转向 -4. 安全停车 - -当前还缺失的关键能力是： - -1. 多垄沟拓扑状态管理 -2. 端部换沟策略 -3. 下一目标垄沟的判定逻辑 -4. 出场与停回启动区的赛道级动作编排 - -所以，这不是某一个传感器量程的小补丁问题，而是系统层次已经需要从“局部控制”提升到“地图引导下的赛道导航”。 - -## 4. 为什么仅靠局部传感器闭环不够 - -### 4.1 局部闭环擅长的任务 - -局部闭环擅长的是： - -- 在 `40cm` 垄沟里保持居中 -- 在局部几何约束下控制横向误差和航向误差 -- 在接近端部时停车或转向 - -这些任务的共同特征是： - -- 机器人周围可观测到明确的近场结构 -- 当前目标由附近环境直接决定 - -### 4.2 换沟问题的本质不同 - -从第一条垄沟换到第二条垄沟，不只是“继续沿墙走”，而是要回答以下更高层的问题： - -1. 我当前已经完成了第几条垄沟 -2. 下一条目标垄沟是哪一条 -3. 这次应该左移还是右移 -4. 端部 `90°` 转向后需要前进多少连接距离 -5. 下一次 `90°` 转向应朝哪一侧入沟 -6. 什么时候说明我已经对准并进入了下一条垄沟，可以重新切回局部走廊跟踪模式 - -这些问题都不是单次近场测距能直接回答的。 - -它们需要： - -- 赛道拓扑信息 -- 段落状态 -- 距离累计 -- 转向后姿态保持 -- 已完成进度记忆 - -所以，换沟本质上是“任务级导航问题”，不是“单纯传感器观测问题”。 - -## 5. 是否需要全局导航 - -### 5.1 需要，但不一定是 SLAM 式全局导航 - -你提出“地图是固定的，是否要改成全局式导航”，我的判断是： - -- 需要“全局导航思想” -- 但不需要上“通用移动机器人 SLAM”那种全局导航系统 - -原因是这个比赛场地有几个重要特征： - -1. 地图是固定结构 -- 场地拓扑不变 -- 垄沟数量固定 -- 宽度和相对排列固定 - -2. 尺寸有误差但不是完全未知 -- 规则允许 `+-5%` -- 说明不能死信纯几何标称值 -- 但也不代表你需要从零建图 - -3. 导航目标是离散段落式的 -- 进入第 1 条垄沟 -- 沿垄沟前进 -- 到端 -- 换到相邻垄沟 -- 重复 -- 最后出场并停回启动区 - -这类问题更适合： - -- 固定地图 -- 拓扑状态机 -- 局部感知闭环 -- 少量里程/姿态积分 - -而不是： - -- 实时全局建图 -- 通用路径规划 -- 自由空间导航 - -### 5.2 更准确的说法：需要“赛道级固定地图导航” - -如果用更工程化的语言描述，真正需要的不是传统 SLAM，而是： - -- 固定地图下的赛道级导航 -- 或者：拓扑-度量混合导航 - -它的含义是： - -1. 上层知道整张赛道的结构 -2. 下层在当前局部段里做高频闭环 -3. 段与段之间的切换靠预定义几何、里程推进和事件触发完成 - -这比“纯局部反应式导航”强很多，但比“完整 SLAM”简单得多，也更符合比赛实际。 - -## 6. 为什么不建议直接走完整 SLAM / 全局自由导航 - -### 6.1 赛场结构太规则，SLAM 的收益不高 - -比赛场地不是开放未知环境，而是高度规则的固定赛道。 - -如果直接上完整 SLAM，会遇到两个问题： - -1. 复杂度远大于实际收益 -2. 还要处理对称结构导致的重定位歧义 - -因为 `6` 条垄沟本身非常相似： - -- 走廊宽度相同 -- 垄长相同 -- 多个局部观测在不同位置上可能长得很像 - -这对通用 SLAM 来说并不天然友好。 - -### 6.2 规则里存在地毯与尺寸误差，纯度量导航也不能硬信 - -比赛规则明确说明： - -- 随机会在 `2` 条垄沟铺设地毯模拟松软路面 -- 场地尺寸允许 `+-5%` 误差 - -这意味着： - -- 纯里程计距离不能被绝对相信 -- 纯固定距离脚本也不能完全相信 - -所以最合理的方案不是“只靠地图”，而是： - -- 地图负责告诉你要去哪一段 -- 传感器负责告诉你你是否已经贴近那一段并进入局部闭环条件 - -## 7. 推荐的导航架构 - -我建议把系统分成三层。 - -### 7.1 第 1 层：局部走廊控制层 - -职责： - -- 在垄沟内保持稳定行驶 -- 利用侧向 VL53 和 IMU 控制 `e_y`、`e_th` -- 处理贴墙、居中、偏置行走等局部任务 - -这层继续使用你现在已经有的能力即可。 - -### 7.2 第 2 层：端部与换沟动作层 - -职责： - -- 识别到端 -- 完成 `90°` 转向 -- 保持连接段朝向直线推进 -- 在下一条垄沟入口处再执行一次 `90°` 转向入沟 -- 在接近下一条垄沟入口时重新捕获局部走廊结构 - -这一层的主要依赖不应再是“直接看到很远处的下一条通道口”，而应是： - -- IMU 航向保持 -- 编码器里程推进 -- 前向/后向长距测距作安全与事件辅助 -- 重新捕获两侧结构时切回走廊模式 - -### 7.3 第 3 层：赛道级拓扑/地图层 - -职责： - -- 记录当前是第几条垄沟 -- 决定下一条目标垄沟编号 -- 决定换沟方向 -- 决定当前所处阶段：入场、走廊、端部、换沟、再入沟、退出、回停 -- 在最终阶段规划如何从最后一条垄沟回到唯一出口并停回启动区 - -这层就是“全局导航思想”的承载层。 - -## 8. 具体到你的顾虑：第二通道口怎么判定 - -这个问题不应该被设计成： - -- “我在端部一转身，然后用 VL53 去远距离搜索第二通道口在哪里” - -更合理的设计应该是： - -### 8.1 已知目标法 - -系统在上层已经知道： - -- 当前完成的是第 `i` 条垄沟 -- 下一个目标是第 `i+1` 条或第 `i-1` 条垄沟 - -也就是说，目标不是“搜索未知入口”，而是“按已知地图去下一个入口”。 - -### 8.2 动作脚本法 - -在端部动作中执行一段有结构的换沟脚本，例如： - -1. 到端停车 -2. 原地转 `90°` 到端部连接段朝向 -3. 用 IMU 保持该朝向直行一小段 -4. 用里程计累计连接段推进距离 -5. 到达下一条垄沟入口附近后再原地转 `90°` -6. 一旦两侧 VL53 重新形成新垄沟的局部几何特征，则确认入沟成功 -7. 切回局部走廊跟踪 - -如果按你描述的实际拓扑，更准确的理解是： - -- 换沟动作不是“横向平移到另一条沟” -- 而是“端部出沟后走一段连接直线，再 90° 入下一沟” -- 整体轨迹更接近规则的 S 型遍历 - -### 8.3 重新捕获法 - -下一条垄沟入口的确认，最可靠的时刻往往不是“远处看见”，而是“进入附近后重新捕获到局部走廊结构”。 - -也就是说： - -- 上层负责把车带到“应该接近下一条垄沟入口”的区域 -- 下层负责在近场把入口真正锁住 - -这比远距离直接识别入口更稳。 - -## 9. 哪些传感器在赛道级导航里应该扮演什么角色 - -### 9.1 左右 VL53L0X - -适合： - -- 垄沟内横向定位 -- 局部几何重捕获 -- 判断自己是否已经重新进入某条垄沟 - -不适合： - -- 远距离寻找下一条通道口 -- 独立承担赛道级换沟导航 - -### 9.2 IMU - -适合： - -- 转向控制 -- 端部横移/换沟时的航向保持 -- 跨局部无墙约束阶段的短时姿态维持 - -### 9.3 编码器 / 里程计 - -适合： - -- 换沟距离推进 -- 已走段长估计 -- 作为脚本动作的度量输入 - -缺点： - -- 地毯和打滑会导致累计误差 - -所以它应作为： - -- 主推进量 -- 但不是唯一最终确认依据 - -### 9.4 前后长距测距 - -当前硬件里前后测距量程远大于 VL53，理论上更适合： - -- 到端检测 -- 判断前方是否接近围栏 -- 在开阔区提供安全保护 -- 对某些段落提供事件辅助 - -它不一定直接告诉你“第二通道口在这”，但可以帮助判断： - -- 是否还在端部开阔区 -- 是否已经逼近另一侧围栏或终点边界 -- 是否应减速/停止/切换动作阶段 - -## 10. 推荐的总体方案 - -### 10.1 推荐方向：固定地图 + 拓扑状态机 + 局部闭环 - -这是我最推荐的方向。 - -其核心思想是： - -- 地图不是在线建出来的，而是事先已知 -- 机器人不需要理解“任意世界坐标” -- 机器人只需要知道自己当前位于哪个赛道段、下一步要切到哪个段 - -可以把整张赛道抽象成若干固定段： - -1. 启动区 -2. 入口对准段 -3. 垄沟 1 -4. 端部连接段 1 -5. 垄沟 2 -6. 端部连接段 2 -7. ... -8. 垄沟 6 -9. 出场段 -10. 启动区停车段 - -如果按运动学动作再细分，每一个“端部连接段”内部实际上可以拆成： - -1. 到端停车 -2. 第一次 `90°` 转向 -3. 连接直线推进 -4. 第二次 `90°` 转向 -5. 入沟重捕获 - -每个段有自己的： - -- 目标朝向 -- 目标推进方向 -- 退出条件 -- 允许使用的观测 -- 安全策略 - -### 10.2 这不是“纯全局”，而是“全局指导下的局部控制” - -这种方案的优势在于： - -- 不会把所有问题都压给近场传感器 -- 不会过度依赖里程计的绝对精度 -- 保留你当前局部走廊控制代码的大部分价值 -- 更贴合固定场地赛事的工程实际 - -## 11. 不推荐的方案 - -### 11.1 不推荐：继续纯局部反应式地扩当前逻辑 - -如果只是继续把当前“单垄沟往返”逻辑往外补，仍然不引入上层赛道状态，那么很容易遇到： - -- 转完向不知道该去第几条沟 -- 明明应该换沟，却又回到原来那条沟附近 -- 因地毯或打滑导致脚本距离错位 -- 无法稳定完成最终出场与回停 - -### 11.2 不推荐：直接上完整 SLAM / 通用全局路径规划 - -这对当前赛题来说过重，收益不一定匹配复杂度。 - -## 12. 对当前项目的直接结论 - -结合当前代码与比赛规则，可以得出以下结论： - -1. 你的顾虑成立 -- 左右 VL53 无法独立解决“数米外下一条垄沟入口识别”问题 - -2. 当前项目确实缺少赛道级导航层 -- 当前更偏向单垄沟局部验证系统 - -3. 后续必须补上固定地图下的全局段落管理 -- 否则无法可靠完成正式比赛要求的 `6` 垄沟遍历 - -4. 但不建议走完整 SLAM 路线 -- 更合适的是固定地图 + 拓扑状态机 + 局部感知闭环 - -## 13. 后续实施建议 - -如果后续开始真正改造导航系统，建议优先级如下： - -1. 先定义完整赛道拓扑和段落状态机 -2. 明确每一段的进入条件、退出条件、目标朝向和目标距离 -3. 让局部走廊控制只负责“在某条已知垄沟里跑稳” -4. 让换沟段由 IMU + 里程计 + 长距测距辅助来完成 -5. 用左右 VL53 做“重新捕获下一条垄沟”确认，而不是远距离搜寻入口 -6. 最后再考虑是否需要引入更强的全局定位增强手段 - -### 13.1 建议分 5 步走 - -结合当前项目现状，建议不要一口气重写整套导航，而是按“先把单段动作语义补完整，再把赛道级层叠上去”的顺序推进。 - -推荐拆成 `5` 步。 - -### 第 1 步：先把现有局部导航链路补到“可复用” - -这一步不是做全局导航，而是先把现有单垄沟能力整理成后续可调用的稳定基础模块。 - -优先要补或改的模块： - -1. `App/nav/segment_fsm.c/.h` -- 增加“动作语义”或“模式感知”输入 -- 区分：走廊前进、原地转向、横向换沟、退出直线 -- 解决当前 `TURN_AT_END` 可能被安全层整段按死的问题 - -2. `App/nav/nav_script.c/.h` -- 不再把它视为最终比赛脚本 -- 先把它收敛成“单段动作编排器”或“局部验证脚本” -- 明确哪些能力未来会上移到赛道级状态机 - -3. `App/preproc/corridor_msgs.h` -- 补充动作模式、段类型、重捕获结果等跨模块消息定义 -- 避免后面继续把语义塞进零散布尔量里 - -4. `App/app_tasks.c` -- 补齐导航启动前的 ready 判定 -- 明确 navTask 每周期里：局部控制输出、赛道级输出、安全层输出的优先级 - -这一阶段的目标不是“遍历 6 条垄沟”，而是： - -- 让“走廊跟踪 / 到端 / 原地转向 / 安全仲裁”这几个局部动作变成可被上层稳定调用的基础能力 - -### 第 2 步：新增赛道级拓扑状态机模块 - -这一步开始真正引入“全局导航思想”，但仍然基于固定地图，不做 SLAM。 - -建议新增模块： - -1. `App/nav/global_nav_fsm.c/.h` -- 负责赛道级阶段推进 -- 记录当前是第几条垄沟 -- 决定下一条目标垄沟编号 -- 决定下一次是左转入沟还是右转入沟 -- 决定当前处于：启动、入沟、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段 - -2. `App/nav/track_map.c/.h` -- 保存固定赛道的拓扑和名义几何 -- 例如：垄沟数量、编号顺序、相邻关系、换沟方向、出口所在侧、启动区相对位置 -- 不追求通用地图系统，只做当前赛题所需的固定地图描述 - -3. `App/preproc/corridor_msgs.h` -- 增加赛道级状态输出结构 -- 例如：当前 corridor_id、target_corridor_id、segment_type、progress_state - -为什么这一步要单独拆出来： - -- 当前项目最大缺口不是局部控制，而是“不知道自己在整张赛道里进行到哪一步” -- 这个职责不能继续堆在 `nav_script.c` 里，否则会越改越像一份超长 if-else 脚本 - -### 第 3 步：新增换沟动作层模块 - -这一层负责跨出当前垄沟、去相邻垄沟入口附近、再把局部控制权交还给走廊跟踪层。 - -建议新增模块： - -1. `App/nav/lane_change_executor.c/.h` -- 输入：当前赛道级目标、目标换沟方向、目标段参数 -- 输出：本周期期望动作 `v/w` -- 内部实现典型动作序列： - - 到端停车 - - 第一次 `90°` 转向到连接段朝向 - - 维持航向直线推进 - - 按里程推进到预计下一沟入口区域 - - 第二次 `90°` 转向入沟 - - 减速搜索并等待局部结构重捕获 - -2. `App/nav/heading_hold.c/.h` 或并入 `lane_change_executor` -- 用 IMU 做连接直线阶段短时航向保持 -- 与走廊控制器解耦，避免把“无侧墙阶段”继续硬塞进 `corridor_ctrl.c` - -3. `App/nav/reacquire_detector.c/.h` -- 负责判断是否已重新进入一条有效垄沟 -- 典型判据：左右 VL53 重新同时形成合理几何、置信度恢复、持续若干拍成立 - -这一阶段的核心目标是： - -- 不依赖“远距离看到下一条垄沟入口” -- 而是“按固定地图推进到预计区域，再由局部传感器完成重捕获确认” - -### 第 4 步：补出场与回停模块 - -前 3 步完成后，系统已经具备： - -- 走一条垄沟 -- 到端 -- 换到相邻垄沟 - -但正式比赛还需要最后的： - -- 从最后一条垄沟驶离 -- 回到唯一出口 -- 停回启动区 - -建议新增模块： - -1. `App/nav/exit_planner.c/.h` -- 定义从最后有效垄沟切到出场段的固定动作逻辑 -- 决定离场时的目标朝向、推进距离、退出条件 - -2. `App/nav/start_zone_dock.c/.h` -- 负责最终回停启动区 -- 可以做成简化版固定脚本，不需要复杂路径规划 - -这一阶段不要追求炫技，重点是： - -- 流程完整 -- 可解释 -- 可调参 -- 能在赛场误差下稳定完成收尾动作 - -### 第 5 步：最后再做统一调参与验证支撑 - -当前项目已经有局部参数，但赛道级导航落地后，还需要把“段参数”和“地图参数”系统化管理。 - -建议补的模块或整理项： - -1. `App/robot_params.h` -- 补充赛道级参数 -- 例如：换沟名义距离、减速搜索距离、重捕获持续拍数、回停距离等 - -2. `App/nav/global_nav_debug.c/.h` 或临时调试结构 -- 给 CubeMonitor / 日志暴露关键内部量 -- 例如：当前 corridor_id、stage、target_heading、reacquire_flag、lane_change_progress - -3. `HANDOFF.md` / `GLOBAL_NAV_REQUIREMENT.md` -- 随代码同步更新 -- 保证后续调试时文档和实现一致 - -这一阶段的目标是： - -- 把“能跑”变成“能调、能解释、能复现” - -### 13.2 各步的交付标准 - -为了避免后续开发一直停留在“看起来写了很多模块”，建议每一步都设一个明确交付标准。 - -第 1 步交付标准： - -- 原地转向不再被安全层错误清零 -- 走廊前进 / 原地转向 / 退出直行三类动作有清晰安全语义 -- 局部导航链路在架空测试中行为稳定可解释 - -第 2 步交付标准： - -- 系统能够明确输出“当前第几条垄沟、下一条目标是哪条、当前赛道阶段是什么” -- 不再依赖单个脚本文件隐式记录全局进度 - -第 3 步交付标准： - -- 能从一条垄沟末端稳定切换到相邻垄沟入口附近 -- 能通过 VL53 重捕获确认重新入沟 - -第 4 步交付标准： - -- 能在完成全部目标垄沟后可靠离场 -- 能完成最终回停启动区 - -第 5 步交付标准： - -- 参数、日志、状态可观测性完整 -- 可以支持正式场地反复调参与问题定位 - -### 13.3 为什么是这个顺序 - -这个顺序的核心原则是： - -1. 先补“动作语义一致性”，再补“赛道级状态记忆” -2. 先解决端部动作可用性，再扩展多垄沟遍历 -3. 先做固定地图和拓扑状态机，再考虑任何更重的全局定位增强 - -如果顺序反过来，例如一开始就写完整 6 垄沟状态机，但底层转向和换沟动作还不稳定，那么上层状态再完整也只会变成“会卡在某一步的复杂脚本”。 - -因此，最合理的开发路径不是“先把全局状态机写满”，而是： - -- 第 1 步：把局部动作做成稳固积木 -- 第 2 步：加赛道级任务管理 -- 第 3 步：补跨段动作 -- 第 4 步：补最终出场与回停 -- 第 5 步：统一调参与调试支撑 - -## 14. 一句话结论 - -你的问题本质上说明：当前系统已经不能只靠“局部走廊导航”来思考了。 - -真正需要补的是“固定地图下的赛道级导航层”，而不是盲目把 VL53 当成远距离入口探测器，也不是直接上复杂 SLAM。 diff --git a/Doc/HYBRID_NAVIGATION_GUIDE.md b/Doc/HYBRID_NAVIGATION_GUIDE.md deleted file mode 100644 index 72321f8..0000000 --- a/Doc/HYBRID_NAVIGATION_GUIDE.md +++ /dev/null @@ -1,458 +0,0 @@ -# 固定地图赛道的混合导航说明 - -## 1. 文档目的 - -本文档面向后续接手项目的人，说明当前比赛场景下为什么推荐采用一种“混合导航”方案，而不是单纯依赖局部传感器闭环，也不是直接上通用 SLAM / 全局路径规划。 - -这里的“混合导航”特指： - -- 上层使用固定地图和拓扑状态机 -- 中层使用段落动作与事件切换 -- 下层使用局部传感器闭环稳定控制 - -这套思路适用于当前这类： - -- 场地结构固定 -- 任务流程固定 -- 局部几何约束很强 -- 但局部传感器看不到全局目标 - -的比赛型机器人系统。 - -## 2. 场地与任务本质 - -根据比赛规则与 `HANDOFF.md`： - -- 场地大小为 `390cm x 300cm` -- 内部有 `5` 条田垄 -- 实际需要遍历的是 `6` 条垄沟 -- 每条垄沟宽 `40cm` -- 每条田垄长 `220cm` -- 只有 `1` 个出入口 -- 出入口外有启动区 - -从导航角度，这不是自由环境中的随机移动问题，而是一个非常明确的固定任务： - -1. 从启动区进入场地 -2. 进入某一条垄沟 -3. 沿垄沟稳定前进并作业 -4. 到达端部后完成转向与换沟 -5. 依次遍历全部 `6` 条垄沟 -6. 最终从唯一出口驶离场地 -7. 自主停回启动区 - -如果把整个任务抽象出来，本质上就是一个 **S 形遍历任务**。 - -示意如下： - -```text -入口 -> 沟1 ↑ - ↓ 沟2 - ↑ 沟3 - ↓ 沟4 - ↑ 沟5 - ↓ 沟6 -> 出口 -``` - -这类问题最重要的不是“任意时刻知道自己在全局坐标中的绝对位置”，而是： - -- 知道自己当前在第几条沟 -- 知道下一条目标沟是哪一条 -- 知道当前处于入沟、走沟、到端、换沟、出场还是回停阶段 -- 在每个阶段里，用合适的传感器和控制策略去完成当前动作 - -## 3. 为什么不能只靠局部导航 - -当前项目已经具备很强的局部导航能力，尤其是在单条垄沟内： - -- 侧向测距可以支撑居中或偏置行驶 -- IMU 可以提供航向与转角信息 -- 前向测距可以做端部触发 -- 里程计可以做短时推进量估计 - -但只靠局部导航会遇到明显边界： - -### 3.1 局部导航能解决什么 - -局部导航适合解决： - -- 在窄沟内居中 -- 在窄沟内保持姿态稳定 -- 接近端部时减速或停车 -- 在局部可观测条件下闭环修正误差 - -### 3.2 局部导航解决不了什么 - -局部导航不擅长解决： - -- 我现在完成了第几条沟 -- 下一条目标沟应该是左边还是右边 -- 转完向后该走多远才能接近下一条沟入口 -- 什么时候该结束整场遍历并朝唯一出口离场 - -这些问题需要： - -- 全局任务记忆 -- 地图拓扑信息 -- 阶段状态机 -- 跨局部观测空窗的动作脚本 - -所以，仅靠“看到什么就跟什么”的局部反应式导航，不足以稳定跑完整场比赛。 - -## 4. 为什么也不建议直接上通用 SLAM - -有些人看到“局部导航不够”，第一反应会是“那就做全局 SLAM”。 - -但当前赛题并不适合直接走这条路线。 - -### 4.1 地图不是未知的 - -赛场结构高度固定： - -- 垄沟数固定 -- 相对排列固定 -- 出入口固定 -- 启动区固定 - -这意味着没有必要像服务机器人那样，在未知环境中一边探索一边建图。 - -### 4.2 场地高度对称 - -`6` 条垄沟在局部上非常相似。 - -这会让很多通用全局定位方法遇到典型问题： - -- 局部观测相似 -- 重定位歧义大 -- 小误差可能让系统把“第 2 沟”认成“第 3 沟”附近 - -### 4.3 比赛更看重稳定完赛，而不是地图美观 - -比赛规则更关心： - -- 能不能完整遍历 -- 能不能不撞边 -- 能不能顺利出场 -- 能不能停回启动区 - -不是在考察一套通用 SLAM 系统的建图效果。 - -### 4.4 通用 SLAM 工程负担大 - -如果直接上完整 SLAM，通常还要处理： - -- 更复杂的状态与数据关联 -- 更高的开发和调参成本 -- 更重的算力与调度开销 -- 更难解释的失败模式 - -这和当前项目追求的工程目标并不匹配。 - -## 5. 什么是“混合导航” - -当前场景下推荐的“混合导航”，可以概括成一句话： - -**用固定地图决定去哪，用状态机决定现在该做什么，用局部传感器闭环决定这一小段怎么稳稳地过去。** - -它不是纯局部，也不是纯全局，而是分层协作。 - -## 6. 混合导航的三层结构 - -## 6.1 上层：固定地图与拓扑状态机 - -上层负责的是“全局任务理解”。 - -它需要维护的信息包括： - -- 当前所在赛道段编号 -- 当前已经完成了第几条垄沟 -- 下一条目标垄沟是哪一条 -- 当前应该左换沟还是右换沟 -- 当前处于：启动、入场、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段 - -这一层不一定关心厘米级位置，而更关心： - -- 拓扑顺序 -- 阶段推进 -- 事件触发 - -可以把整场任务拆成固定段落，例如： - -1. 启动区准备 -2. 入口对准 -3. 垄沟 1 前进 -4. 端部换沟 1 -5. 垄沟 2 返回 -6. 端部换沟 2 -7. 垄沟 3 前进 -8. ... -9. 最后一沟结束 -10. 出场 -11. 回停启动区 - -这就是“固定地图导航”的核心，不是基于任意坐标规划，而是基于已知赛道结构推进任务。 - -## 6.2 中层：段落动作与事件切换 - -中层负责把“上层目标”翻译成可执行动作。 - -例如在某个阶段，机器人可能执行： - -- 入口慢速直行，直到捕获双侧结构 -- 沿当前垄沟闭环跟踪 -- 到端后原地转 `90°` 或 `180°` -- 保持某个航向横向推进一段距离 -- 在接近预计位置后减速，并等待重新捕获新垄沟 -- 切回垄沟跟踪模式 - -这一层的本质是“段脚本 + 事件触发”。 - -它依赖： - -- IMU 姿态 -- 里程计推进量 -- 前向/后向安全距离 -- 局部结构重新捕获结果 - -这层很重要，因为许多时候机器人会暂时处于“看不到完整走廊结构”的状态，比如端部换沟阶段。 - -## 6.3 下层：局部传感器闭环 - -下层负责在小范围内把车稳稳控制住。 - -典型任务： - -- 在垄沟内居中 -- 在垄沟内偏向 1/4 宽度行驶 -- 根据 IMU 保持航向 -- 根据前向测距做减速与停车 - -这一层应当追求： - -- 高频 -- 稳定 -- 可降级 -- 不依赖复杂全局推理 - -你现在已有的大部分控制能力，都属于这一层。 - -## 7. 各类传感器在混合导航中的角色 - -## 7.1 左右 VL53L0X - -推荐职责： - -- 走廊内横向定位 -- 居中/偏置行驶 -- 近场重捕获新垄沟 - -不推荐职责： - -- 远距离识别下一条垄沟入口 -- 独立承担换沟全流程导航 - -原因很简单： - -- 它们是近场侧向传感器 -- 强项是局部几何闭环 -- 弱项是远距离赛道级感知 - -## 7.2 IMU - -推荐职责： - -- 航向估计主来源 -- 转向角度判定 -- 无侧墙约束阶段的短时姿态保持 -- 换沟阶段的朝向控制 - -IMU 在混合导航里非常关键，因为它能帮助机器人跨过“局部结构暂时缺失”的区间。 - -## 7.3 编码器 / 里程计 - -推荐职责： - -- 估算走过了多长距离 -- 在换沟动作中提供推进量 -- 与状态机结合做段落退出条件 - -限制： - -- 遇到地毯、打滑、轮胎差异时会有误差 - -因此它适合作为“推进量参考”，但不适合作为唯一定位真值。 - -## 7.4 前后长距测距 - -推荐职责： - -- 到端检测 -- 防撞保护 -- 开阔区事件辅助 -- 出场或接近边界时的安全约束 - -这类传感器不一定直接告诉你“下一沟入口就在前方”，但能帮助你判断： - -- 是否接近端部 -- 是否接近围栏 -- 是否该切换动作阶段 - -## 8. 为什么这种方案适合当前比赛 - -## 8.1 它符合固定地图的特点 - -比赛地图结构是已知的。 - -这意味着： - -- 目标垄沟不是未知搜索对象 -- 换沟不必靠“发现远处入口” -- 可以由状态机根据当前进度直接推导“下一步该去哪” - -## 8.2 它承认局部传感器的边界 - -混合导航没有让 VL53 去做它不擅长的事情。 - -它承认： - -- VL53 负责局部 -- IMU 负责短时姿态保持 -- 里程计负责推进量 -- 上层状态机负责“全局流程” - -这是符合传感器物理特性的分工。 - -## 8.3 它能兼容尺寸误差与地毯 - -规则里有两个现实问题： - -- 场地尺寸允许 `+-5%` -- 有 `2` 条垄沟会随机铺地毯 - -这意味着： - -- 不能死信地图上的绝对尺寸 -- 不能死信纯里程推进 - -混合导航的好处是： - -- 上层地图只给出大方向和段结构 -- 局部闭环用实时传感器做最终修正 - -这样既利用了先验地图，又不会被固定脚本锁死。 - -## 9. 一个典型的工作流程 - -下面给出一个典型流程，帮助理解这套导航在比赛中的运行方式。 - -### 9.1 启动与入场 - -- 上层状态机进入“入口对准”阶段 -- 机器人从启动区朝入口前进 -- 一旦左右侧传感器稳定捕获到垄沟结构，切换为垄沟跟踪模式 - -### 9.2 垄沟内前进 - -- 下层局部控制根据左右侧测距维持横向位置 -- IMU 维持航向 -- 前向测距用于到端检测 -- 状态机记录当前垄沟编号与方向 - -### 9.3 到端 - -- 前向距离达到阈值 -- 状态机判定当前垄沟已到端 -- 进入转向阶段 - -### 9.4 换沟 - -- IMU 控制转向到目标朝向 -- 编码器推进预定距离 -- 长距测距做安全约束 -- 接近预期位置后减速 -- 直到左右 VL53 再次捕获新垄沟结构 - -### 9.5 再入沟 - -- 一旦检测到新的局部走廊结构 -- 状态机确认已进入下一条垄沟 -- 切换回局部走廊跟踪 - -### 9.6 重复直到遍历完成 - -- 状态机更新“当前第几沟” -- 按 S 形顺序重复以上过程 - -### 9.7 最终离场与回停 - -- 当最后一条垄沟完成后 -- 状态机切换到出场段 -- 利用固定地图与局部感知朝唯一出口离开 -- 出场后再执行停回启动区动作 - -## 10. 与当前项目的关系 - -当前项目已经具备混合导航中的一部分基础： - -- 局部走廊控制 -- IMU 航向处理 -- 到端检测 -- 原地转向 -- 段脚本雏形 - -但当前仍偏向： - -- 单垄沟验证 -- 局部闭环主导 -- 缺少完整的赛道级段落管理 - -所以后续真正需要补的，并不是完全推翻现有控制，而是： - -- 把“局部走廊能力”封装成底层能力 -- 在其上补一个完整的固定地图状态机 -- 把换沟和出场逻辑系统化 - -## 11. 一个容易犯的错误 - -在这类项目里，最容易犯的错误是两种极端： - -### 11.1 极端一：把所有问题都交给局部传感器 - -这会导致： - -- 换沟阶段无从判断全局目标 -- 容易在端部迷失 -- 难以稳定完成多垄沟遍历 - -### 11.2 极端二：把所有问题都交给“全局定位” - -这会导致： - -- 系统复杂度暴涨 -- 对称环境下定位歧义严重 -- 与比赛需求不匹配 - -混合导航的价值就在于避开这两个极端。 - -## 12. 推荐结论 - -对于当前比赛场景，推荐的总体思路是： - -1. 用固定地图描述整条 S 形任务路线 -2. 用拓扑状态机管理“当前在哪一段、下一步去哪一段” -3. 用 IMU 和里程计支撑跨局部观测空窗的动作执行 -4. 用左右 VL53 负责局部垄沟内的高精度横向闭环与重捕获 -5. 用前后长距测距做端部识别与安全保护 - -这就是当前场景最合适的混合导航方案。 - -## 13. 一句话总结 - -这类比赛不是“靠一套万能定位算法解决全部问题”，而是“让合适的层做合适的事”： - -- 地图负责全局流程 -- 状态机负责阶段切换 -- 传感器负责局部闭环 -- 控制器负责把每一小段稳稳跑完 - -这就是混合导航的核心价值。 diff --git a/Doc/IMU_YAW_REQUIREMENT.md b/Doc/IMU_YAW_REQUIREMENT.md deleted file mode 100644 index 8b80974..0000000 --- a/Doc/IMU_YAW_REQUIREMENT.md +++ /dev/null @@ -1,321 +0,0 @@ -# IMU 主导航向需求说明 - -## 1. 背景 - -当前项目运行场景是小车在走廊/垄沟内行驶，依赖以下传感器进行状态估计与控制： - -- HWT101 IMU：提供 `yaw`、`yaw_continuous`、`wz` -- 左右侧 VL53 测距：提供左右前后 4 个侧向距离 -- 编码器里程计：提供线速度 `odom_vx`，并参与运动预测 - -目前系统在 `navTask` 中每 20ms 执行一次状态估计与控制，整体链路见 `HANDOFF.md`。 - -本说明文档用于明确一个新的需求方向： - -- 左右激光测距主要用于横向位置参考 -- 航向角估计希望主要依赖 IMU，而不是依赖左右激光前后差分计算得到的航向角 - -本文档只描述需求、现状和建议，不修改现有代码。 - -## 2. 当前实现现状 - -### 2.1 当前 yaw / 航向相关的数据来源 - -当前系统中的“航向”并不是单一量，而是分成两类： - -1. IMU 原始航向信息 - -- 文件：`App/IMU/hwt101.c` -- IMU 输出： - - `yaw`：原始偏航角，范围 `[-180, 180)` - - `yaw_continuous`：对原始 yaw 做 unwrap 后得到的连续角度 - - `wz`：角速度，单位 `deg/s` - -2. EKF 中的相对航向误差 `e_th` - -- 文件：`App/est/corridor_filter.c` -- 文件：`App/est/corridor_ekf.c` -- `e_th` 表示小车相对当前走廊方向的航向误差，而不是全局绝对航向角 - -### 2.2 当前 EKF 中航向的计算方式 - -当前滤波流程如下： - -1. 预测步 - -- 使用 `odom_vx` 和 `imu_wz` 做预测 -- 对应代码：`CorridorEKF_Predict(odom_vx, imu_wz, dt)` - -2. 侧墙观测更新 - -- 使用左右侧测距更新横向误差 `e_y` -- 同时也使用左右同侧前后测距差来估计航向 `e_th` - -对应观测形式： - -- 左侧航向观测：`z_eth_L = atan2(d_lr - d_lf, Ls)` -- 右侧航向观测：`z_eth_R = atan2(d_rf - d_rr, Ls)` - -相关代码位置：`App/est/corridor_ekf.c` - -3. IMU yaw 观测更新 - -- 在侧墙更新之后，再用 `imu_yaw_continuous` 做一个独立 1DOF 的航向观测更新 -- 该观测形式为： - -`z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad` - -其中 `imu_yaw_ref_rad` 是在侧墙观测可信时锁定的参考值 - -相关代码位置： - -- `App/est/corridor_filter.c` -- `App/est/corridor_ekf.c` - -### 2.3 当前系统对传感器的信任关系 - -从参数和注释来看，当前系统默认策略是： - -- 侧墙激光是走廊内姿态估计的主观测来源 -- IMU yaw 是辅助观测，用于长时约束和侧墙观测缺失时兜底 - -相关参数位于：`App/robot_params.h` - -当前默认值： - -- `PARAM_EKF_R_EY = 0.002f` -- `PARAM_EKF_R_ETH = 0.001f` -- `PARAM_EKF_R_ETH_IMU = 0.01f` - -含义： - -- 侧墙航向观测噪声更小，表示当前更信任侧墙推导出的航向角 -- IMU yaw 观测噪声更大，表示当前 IMU 在 EKF 中主要是弱约束 - -### 2.4 当前导航脚本对 IMU yaw 的使用 - -虽然走廊跟踪阶段的 `e_th` 主要是由 EKF 输出，但 180 度原地转向判定已经直接使用了 IMU 的 `yaw_continuous`。 - -相关代码位置：`App/nav/nav_script.c` - -这说明项目里已经承认一个事实： - -- 在“累计转角判定”这类任务上，IMU 连续 yaw 比 EKF 的 `e_th` 更适合做主依据 - -## 3. 当前方案存在的问题 - -### 3.1 左右激光更适合测位置，不适合主导航向 - -用户当前判断是： - -- 左右激光测距误差大约在 `+-2cm` -- 这个误差水平对于横向位置参考仍然有价值 -- 但对于航向角计算不够稳定，难以直接采纳为主观测 - -这是一个合理判断。 - -原因在于，侧墙航向观测本质上来自“同侧前后两个距离的差分”： - -- 左侧：`d_lr - d_lf` -- 右侧：`d_rf - d_rr` - -差分量本身会放大噪声影响，尤其是在以下条件下： - -- 单个传感器误差较大 -- 前后基线长度有限 -- 墙面不完全平整 -- 传感器安装误差存在偏角或偏移 - -因此，虽然左右激光仍然适合估计： - -- 小车是否居中 -- 小车偏左还是偏右 -- 小车是否位于沟宽的四分之一等目标横向位置 - -但未必适合继续承担“主航向来源”的角色。 - -### 3.2 当前结构下，侧墙观测对 e_th 的影响仍然偏强 - -当前 EKF 的侧墙更新同时包含： - -- `e_y` 观测 -- `e_th_L` / `e_th_R` 观测 - -所以只要侧墙数据有效，系统就会直接利用左右前后差分结果去修正航向。 - -如果侧墙前后差分噪声较大，就可能带来以下问题： - -- `e_th` 抖动 -- 控制输出 `w` 抖动 -- 走廊直行时出现不必要的左右摆动 -- IMU 已经给出较平滑航向，但被激光差分估计不断拉扯 - -## 4. 目标需求 - -### 4.1 总体需求 - -希望重新明确传感器分工： - -- IMU 主要负责航向角估计 -- 左右激光主要负责横向位置参考 - -更具体地说： - -1. 走廊内横向控制 - -- 左右激光用于判断小车在走廊中的横向位置 -- 支持居中行驶 -- 支持偏向左/右四分之一位置行驶等策略 - -2. 走廊内航向控制 - -- 航向估计应主要依赖 IMU -- 侧墙测距不应继续作为航向主观测来源 - -3. 转向阶段 - -- 继续使用 IMU 连续 yaw 作为转角判定主依据 - -### 4.2 需求表达上的准确表述 - -如果用更工程化的语言描述该需求，可以表述为： - -- “侧墙激光参与横向位置估计，不参与或仅弱参与航向角估计。” -- “航向角 `e_th` 的主来源改为 IMU `wz + yaw_continuous`。” -- “侧墙前后差分得到的航向观测仅作为弱约束、校验项，或直接关闭。” - -## 5. 对现有系统的理解结论 - -基于当前代码实现，可以得出以下判断： - -### 5.1 用户的想法与当前实现不一致 - -当前实现里： - -- 左右激光不仅参与横向位置 `e_y` -- 还直接参与航向 `e_th` - -而用户期望的是： - -- 左右激光只负责横向位置参考 -- 航向主要信任 IMU - -因此，这不是简单调一个小参数就完全等价的需求，而是状态估计设计思路上的调整。 - -### 5.2 用户的想法在当前场景下是成立的 - -若侧墙测距误差确实约为 `+-2cm`，则： - -- 用其估计横向偏移仍有意义 -- 用其做前后差分计算航向角则很容易噪声偏大 - -从传感器特性匹配上看，更合理的做法就是： - -- 激光负责位置 -- IMU 负责航向 - -### 5.3 需要区分“横向位置”和“航向角”两个子问题 - -本需求的关键不是“全面抛弃激光”，而是要区分： - -- `e_y`：仍可继续信任侧墙测距 -- `e_th`：应改为主要信任 IMU - -这是本需求最核心的设计点。 - -## 6. 后续可选改造方向 - -本节只记录可能的改造方向，不在本次工作中实施。 - -### 6.1 方向 A：仅通过参数调权，弱化侧墙航向观测 - -思路： - -- 保留现有 EKF 结构不变 -- 仅通过增大 `PARAM_EKF_R_ETH`、减小 `PARAM_EKF_R_ETH_IMU` 来让航向估计更偏向 IMU - -优点： - -- 修改最小 -- 风险相对可控 -- 可以快速实车验证 - -缺点： - -- 侧墙航向观测仍然存在于主更新流程中 -- 只是“变弱”，不是“彻底不参与” - -### 6.2 方向 B：结构性调整，侧墙只更新 e_y - -思路： - -- 修改 EKF 观测模型 -- 侧墙测距只用于更新 `e_y` -- `e_th` 仅由 `imu_wz` 预测和 `imu_yaw` 观测约束 - -优点： - -- 最符合本需求原意 -- 传感器职责边界清晰 - -缺点： - -- 改动比参数调权大 -- 需要重新验证滤波稳定性和控制效果 - -### 6.3 方向 C：侧墙航向只做低频校验或异常检测 - -思路： - -- 不再把 `z_eth_L/z_eth_R` 作为主 EKF 观测 -- 改成仅在长直段、双侧稳定、连续多帧一致时，低频微量校正 IMU 航向 -- 或只用于诊断告警，不直接参与状态更新 - -优点： - -- 兼顾 IMU 主导与环境约束 -- 有助于抑制纯 IMU 长时漂移 - -缺点： - -- 逻辑更复杂 -- 需要额外设计稳定判据 - -## 7. 推荐结论 - -如果以当前用户需求为准，推荐设计原则如下： - -1. 左右激光负责横向位置，不再主导航向 -2. IMU 负责航向主估计 -3. 转弯角度继续使用 IMU 连续 yaw 判定 -4. 如需保留侧墙航向，也应降为弱约束或校验项，而不是主观测 - -换句话说，后续如果要正式调整系统，应优先朝这个方向收敛： - -- `e_y` 由侧墙激光主导 -- `e_th` 由 IMU 主导 - -## 8. 涉及模块清单 - -本需求后续若要实施，主要会影响以下模块： - -- `App/IMU/hwt101.c` - - IMU yaw / yaw_continuous / wz 来源 -- `App/est/corridor_filter.c` - - IMU yaw 参考值与更新调用逻辑 -- `App/est/corridor_ekf.c` - - 侧墙航向观测 `z_eth_L/z_eth_R` 的使用方式 -- `App/robot_params.h` - - 观测噪声参数调权 -- `App/nav/nav_script.c` - - 转弯阶段的 IMU yaw 使用逻辑 - -## 9. 本文档结论摘要 - -本文档确认以下几点： - -- 当前系统现状：侧墙激光不仅用于横向位置，也参与航向角估计 -- 用户需求：侧墙激光只作为位置参考，航向主要信任 IMU -- 该需求与当前实现存在结构性差异 -- 从传感器误差特性看，这一需求是合理的 -- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导 diff --git a/Doc/实施方案.md b/Doc/实施方案.md new file mode 100644 index 0000000..dc0b853 --- /dev/null +++ b/Doc/实施方案.md @@ -0,0 +1,1500 @@ +# ARES 混合导航实施方案 + +> **版本**: v1.0 +> **日期**: 2025 年 +> **依据**: `HANDOFF.md`、`混合导航方案.md`、`map.md`、现有代码分析 +> **本文目标**: 将混合导航方案从概念设计转化为可逐步实施的工程计划，明确每一个文件、每一个结构体、每一个状态转移 + +--- + +## 目录 + +1. [赛道几何精确推演](#1-赛道几何精确推演) +2. [S 型遍历轨迹精确描述](#2-s-型遍历轨迹精确描述) +3. [总体架构设计](#3-总体架构设计) +4. [完整状态机定义](#4-完整状态机定义) +5. [地图数据模型](#5-地图数据模型) +6. [新增模块详细设计](#6-新增模块详细设计) +7. [现有模块改造](#7-现有模块改造) +8. [数据流水线改造](#8-数据流水线改造) +9. [参数清单](#9-参数清单) +10. [文件清单汇总](#10-文件清单汇总) +11. [分步实施计划](#11-分步实施计划) +12. [风险与降级策略](#12-风险与降级策略) + +--- + +## 1. 赛道几何精确推演 + +### 1.1 从 `map.md` 提取的关键尺寸 + +根据地图 (1 字符 = 10cm)，精确测量如下： + +``` +场地净尺寸: X = 300cm (30格) Y = 390cm (39格，不含启动区) + +围栏: 1格 = 10cm 厚 + +田垄: 宽 30cm (3格) 长 220cm (22格) + 左端距左围栏内壁 40cm (4格) + 右端距右围栏内壁 40cm (4格) + +垄沟 (南北向通道): + 每条宽 40cm (4格) + 长度 = 垄背长度 = 220cm (被垄背两侧约束) + +端部开放区 (垄背两端到围栏): + 左端: 40cm (4格) + 右端: 40cm (4格) + +启动区: 宽 40cm × 深 100cm，位于场地最下方 (Y=39)，紧邻左侧通道 +入口: 宽 40cm (4格)，位于左下角，从Y=39到围栏外 +``` + +### 1.2 关键几何参数表 + +| 参数 | 值 | 来源 | +|------|-----|------| +| 垄沟长度 (沟内可行驶) | **220 cm** | 垄背长度 = 22格 | +| 垄沟宽度 | **40 cm** | 围栏/垄背间距 | +| 端部连接通道宽度 (左端) | **40 cm** | 左围栏到垄背左端 | +| 端部连接通道宽度 (右端) | **40 cm** | 右围栏到垄背右端 | +| 连接段推进距离 (沟间距) | **70 cm** | 垄背宽 30cm + 垄沟宽 40cm = 70cm，中心到中心 | +| 入口段长度 (启动区到场内第一个垄沟中心) | **~65 cm** | 从入口边缘到 corridor 1 中心线的距离 | +| 场地总深度 (Y方向) | **390 cm** | 围栏内净尺寸 | +| 场地总宽度 (X方向) | **300 cm** | 围栏内净尺寸 | + +### 1.3 6 条垄沟的 Y 坐标 (中心线，从下往上编号) + +> 坐标系: 原点在左下围栏内壁角，X 向右，Y 向上 + +从地图分析，自下而上（从入口端开始）： + +| 垄沟编号 | 两侧约束 | 中心线 Y 坐标 (约) | +|----------|----------|-------------------| +| Corridor 1 | 下围栏 ↔ 垄背5 | Y ≈ 20 cm | +| Corridor 2 | 垄背5 ↔ 垄背4 | Y ≈ 90 cm | +| Corridor 3 | 垄背4 ↔ 垄背3 | Y ≈ 160 cm | +| Corridor 4 | 垄背3 ↔ 垄背2 | Y ≈ 230 cm | +| Corridor 5 | 垄背2 ↔ 垄背1 | Y ≈ 300 cm | +| Corridor 6 | 垄背1 ↔ 上围栏 | Y ≈ 370 cm | + +> **注**: 具体 Y 坐标值在实施中不重要，因为我们不做全局定位。关键参数是 **沟间距 70cm** 和 **沟长 220cm**。 + +--- + +## 2. S 型遍历轨迹精确描述 + +### 2.1 S 型路径定义 + +``` +启动区 → 入口 → [左端通道向上] → 右转90° → Corridor 1 (→) → 左转90° +→ [右端通道向上 70cm] → 左转90° → Corridor 2 (←) → 右转90° +→ [左端通道向上 70cm] → 右转90° → Corridor 3 (→) → 左转90° +→ [右端通道向上 70cm] → 左转90° → Corridor 4 (←) → 右转90° +→ [左端通道向上 70cm] → 右转90° → Corridor 5 (→) → 左转90° +→ [右端通道向上 70cm] → 左转90° → Corridor 6 (←) → [到左端] +→ 左转90° → [左端通道向下 回到入口] → 出场 → 回停启动区 +``` + +其中 `(→)` 表示从左端到右端行驶，`(←)` 表示从右端到左端行驶。 + +### 2.2 动作序列表 (完整遍历) + +| 步骤 | 动作类型 | 传感器依赖 | 终止条件 | 方向参考 | +|------|---------|-----------|----------|---------| +| 0 | **启动区等待** | 无 | 外部启动信号 | — | +| 1 | **入场直线** | IMU航向 + 前激光 | 到达第一个垄沟入口附近（里程计 + 侧向VL53开始有效） | 向上(+Y) | +| 2 | **右转 90°** | IMU yaw_continuous | 转角达到 90° | 原地转向 | +| 3 | **重捕获入沟** | VL53 双侧有效 + conf | 锁定走廊结构 | — | +| 4 | **Corridor 1 跟踪 (→)** | VL53 + IMU + 前激光 | 前激光到端 | 向右(+X) | +| 5 | **左转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 | +| 6 | **连接段直行 70cm** | IMU航向 + 里程计 | 里程到 70cm 或侧向开始看到下一沟壁 | 向上(+Y) | +| 7 | **左转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 | +| 8 | **重捕获入沟** | VL53 | 锁定 | — | +| 9 | **Corridor 2 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 | 向左(-X) | +| 10 | **右转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 | +| 11 | **连接段直行 70cm** | IMU + 里程计 | 同步骤 6 | 向上(+Y) | +| 12 | **右转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 | +| 13 | **重捕获入沟** | VL53 | 锁定 | — | +| 14 | **Corridor 3 跟踪 (→)** | ... | ... | 向右 | +| ... | *重复 S 型模式* | ... | ... | ... | +| 29 | **Corridor 6 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 (左端围栏) | 向左 | +| 30 | **左转 90° (朝出口)** | IMU | 转角达到 90° | 原地转向 | +| 31 | **出场直线** | IMU + 里程计 + VL53丢失检测 | 侧向VL53全丢 + 里程足够 | 向下(-Y) | +| 32 | **回停启动区** | 里程计 + 超时 | 到达启动区范围内 | 减速停车 | + +### 2.3 转向方向规律 + +关键规律（从垄沟 1 开始，假设 1→2→3→4→5→6 的 S 型）： + +| 从 → 到 | 行驶方向 | 到端后出沟转向 | 连接段朝向 | 入沟转向 | +|---------|---------|-------------|-----------|---------| +| C1(→) → C2 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) | +| C2(←) → C3 | 右→左 | 右转(CW) | 向上 | 右转(CW) | +| C3(→) → C4 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) | +| C4(←) → C5 | 右→左 | 右转(CW) | 向上 | 右转(CW) | +| C5(→) → C6 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) | + +**规律**: 奇数沟(→)到端后两次左转；偶数沟(←)到端后两次右转。 + +可用公式表达: +```c +// 当前沟结束后的转向方向 +if (corridor_id % 2 == 1) // 奇数沟(→)，在右端出 + turn_direction = TURN_LEFT; // CCW, +90° +else // 偶数沟(←)，在左端出 + turn_direction = TURN_RIGHT; // CW, -90° +``` + +--- + +## 3. 总体架构设计 + +### 3.1 三层架构 + +``` +┌───────────────────────────────────────────────────────────────────────┐ +│ 上层: 赛道级导航层 │ +│ global_nav.c/.h + track_map.c/.h │ +│ │ +│ 职责: 知道"当前第几沟、下一沟是谁、该往哪转" │ +│ 输出: ActionRequest_t { action_type, turn_dir, ... } │ +└──────────────────────────────┬────────────────────────────────────────┘ + │ +┌──────────────────────────────▼────────────────────────────────────────┐ +│ 中层: 动作执行层 │ +│ action_executor.c/.h (统一调度器) │ +│ 内部调用: turn_action / straight_action / reacquire │ +│ │ +│ 职责: 把一个"动作请求"变成实际的 v/w 指令序列 │ +│ 输出: RawCmd_t { v, w } │ +└──────────────────────────────┬────────────────────────────────────────┘ + │ +┌──────────────────────────────▼────────────────────────────────────────┐ +│ 下层: 局部闭环层 (已有) │ +│ corridor_ctrl.c + corridor_ekf + corridor_preproc │ +│ │ +│ 职责: 沟内精确居中跟踪 │ +│ 输出: RawCmd_t { v, w } │ +└──────────────────────────────┬────────────────────────────────────────┘ + │ +┌──────────────────────────────▼────────────────────────────────────────┐ +│ 安全仲裁层 (已有，需改造) │ +│ segment_fsm.c │ +│ │ +│ 职责: 根据动作模式做上下文感知的安全裁剪 │ +│ 输出: SegFsmOutput_t { safe_v, safe_w } │ +└──────────────────────────────┬────────────────────────────────────────┘ + │ + CmdSlot → CAN TX +``` + +### 3.2 调用关系 (navTask 内一次循环) + +``` +AppTasks_RunNavTask_Impl() { + // Step 1-3: 不变 (黑板快照 → 预处理 → EKF) + Blackboard_GetSnapshot(&board); + CorridorPreproc_ExtractObs(&board, now, &obs); + CorridorFilter_Update(&obs, wz, vx, dt, yaw, yaw_ok, &state); + + // Step 4: 【改造】赛道级导航 + 动作执行（替代原 NavScript） + GlobalNav_Update(&obs, &state, &board, &nav_output); + // nav_output 包含: + // - action_type: 当前在执行什么动作 + // - cmd: { v, w } (中层输出) + // - request_corridor: bool (是否要求走廊控制器接管) + // - safety_mode: enum (当前动作对安全层的模式要求) + + // Step 5: 控制律（仅在 request_corridor 时运行） + if (nav_output.request_corridor) + CorridorCtrl_Compute(&state, &obs, imu_wz, &raw_cmd); + else + raw_cmd = nav_output.cmd; + + // Step 6: 【改造】安全层（感知当前动作模式） + SegFsm_Update(&raw_cmd, &obs, &state, nav_output.safety_mode, &fsm_out); + + // Step 7: 不变 + CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0); +} +``` + +--- + +## 4. 完整状态机定义 + +### 4.1 赛道级状态机 (GlobalNavStage) + +```c +typedef enum { + GNAV_IDLE, // 启动区等待 + GNAV_ENTRY_STRAIGHT, // 入场直线 (从启动区进入场地，向上行驶到第一条沟入口) + GNAV_TURN_INTO_CORRIDOR, // 转向进入垄沟 (原地转 90°) + GNAV_REACQUIRE, // 重捕获走廊 (低速前进，等VL53锁定) + GNAV_CORRIDOR_TRACK, // 沟内闭环跟踪 (现有 corridor_ctrl) + GNAV_TURN_OUT_OF_CORRIDOR, // 到端后出沟转向 (原地转 90°) + GNAV_LINK_STRAIGHT, // 连接段直行 (IMU航向保持 + 里程计推进) + GNAV_TURN_INTO_NEXT, // 转向进入下一条沟 (原地转 90°) + GNAV_EXIT_STRAIGHT, // 出场直行 (向下回到入口) + GNAV_DOCK, // 回停启动区 + GNAV_FINISHED, // 终态 + GNAV_ERROR // 异常态 (超时兜底) +} GlobalNavStage_t; +``` + +### 4.2 完整状态转移图 + +``` +IDLE + │ Start() + ▼ +ENTRY_STRAIGHT ──────────────────────── [侧向VL53开始有效 或 里程到达] + │ + ▼ +TURN_INTO_CORRIDOR ──────────────────── [IMU 转角 ≥ 90° - tolerance] + │ + ▼ +REACQUIRE ───────────────────────────── [VL53双侧有效 + conf≥阈值 + 持续N拍] + │ + ▼ +CORRIDOR_TRACK ──────────────────────── [前激光 d_front ≤ 到端阈值] + │ + ▼ +TURN_OUT_OF_CORRIDOR ────────────────── [IMU 转角 ≥ 90° - tolerance] + │ + ├─── [还有未遍历的沟? → 是] + │ │ + │ ▼ + │ LINK_STRAIGHT ────────────────── [里程 ≥ 连接段距离 或 侧向VL53探到壁] + │ │ + │ ▼ + │ TURN_INTO_NEXT ───────────────── [IMU 转角 ≥ 90° - tolerance] + │ │ + │ ▼ + │ REACQUIRE → CORRIDOR_TRACK (循环) + │ + ├─── [最后一条沟完成? → 是] + │ │ + │ ▼ + │ EXIT_STRAIGHT ────────────────── [侧向VL53全丢 + 里程足够 或 到达出口] + │ │ + │ ▼ + │ DOCK ─────────────────────────── [里程到 或 超时] + │ │ + │ ▼ + │ FINISHED + │ + └─── [超时保护 → 任何阶段超时] + │ + ▼ + ERROR → FINISHED (安全停车) +``` + +### 4.3 各状态详细行为 + +#### GNAV_IDLE +``` +行为: v=0, w=0 +安全模式: SAFETY_MODE_IDLE +进入时: 系统启动默认态 +退出条件: GlobalNav_Start() 被调用 +退出动作: 记录 IMU yaw_ref，初始化里程计基准 +``` + +#### GNAV_ENTRY_STRAIGHT +``` +行为: v=PARAM_ENTRY_V (0.08 m/s), w=IMU航向保持PD + w = kp_heading * (yaw_ref - yaw_current) // 简单P控制保直 +安全模式: SAFETY_MODE_STRAIGHT (前后激光防撞生效) +进入时: 记录起始里程 +退出条件: 以下任一: + (1) 侧向VL53某侧开始有效 (探到第一条沟的壁) + (2) 里程 ≥ PARAM_ENTRY_DISTANCE + (3) 超时 PARAM_ENTRY_TIMEOUT +退出动作: 记录当前 yaw 为沟口参考 +``` + +#### GNAV_TURN_INTO_CORRIDOR +``` +行为: v=0, w=turn_dir * PARAM_TURN_OMEGA (±1.0 rad/s) + 接近目标角度时减速: 剩余 < 0.5rad 时线性减速至 0.3 rad/s +安全模式: SAFETY_MODE_TURN (允许v=0时保留w, 前后激光不触发全停) +进入时: 记录 turn_start_yaw = IMU yaw_continuous + 从 track_map 查询本次转向方向 +退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE (默认 5°) +退出动作: EKF 重置; 记录新航向参考 +``` + +#### GNAV_REACQUIRE +``` +行为: v=PARAM_REACQUIRE_V (0.05 m/s), w=IMU航向保持PD +安全模式: SAFETY_MODE_STRAIGHT +进入时: EKF 重置; 清零确认计数器; 记录起始里程 +退出条件: 以下全部持续 PARAM_REACQUIRE_CONFIRM_TICKS 拍 (默认5拍=100ms): + (1) VL53 双侧(至少3个)有效 + (2) 左右距离和 ≈ 走廊宽度 (±容差 5cm) + (3) EKF conf ≥ PARAM_REACQUIRE_CONF_THRESH (0.6) +超时保护: PARAM_REACQUIRE_TIMEOUT (5000ms), 超时 → ERROR +退出动作: corridor_ctrl 初始化完成，切换到沟内跟踪 +``` + +#### GNAV_CORRIDOR_TRACK +``` +行为: request_corridor = true (交给现有 corridor_ctrl) +安全模式: SAFETY_MODE_CORRIDOR (现有 SegFsm 逻辑: 前向减速/停车/E-STOP) +进入时: 记录起始里程 +退出条件: 以下任一: + (1) 前激光有效 且 d_front ≤ PARAM_END_DETECT_DIST (0.10m) + (2) 里程 ≥ PARAM_CORRIDOR_LENGTH_MAX (2.5m, 超长保护) +退出动作: 停车; corridor_id 对应的遍历标记置位 +``` + +#### GNAV_TURN_OUT_OF_CORRIDOR +``` +行为: v=0, w=turn_dir * PARAM_TURN_OMEGA + (转向方向由 track_map 决定) +安全模式: SAFETY_MODE_TURN +进入时: 记录 turn_start_yaw +退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE +退出动作: 判断是否为最后一条沟: 是 → EXIT_STRAIGHT; 否 → LINK_STRAIGHT +``` + +#### GNAV_LINK_STRAIGHT +``` +行为: v=PARAM_LINK_V (0.10 m/s), w=IMU航向保持PD +安全模式: SAFETY_MODE_STRAIGHT +进入时: 记录起始里程; 记录航向参考 +退出条件: 以下任一: + (1) 里程 ≥ PARAM_LINK_DISTANCE (0.70m, 即沟间距) + (2) 侧向VL53某侧探到新沟壁 (距离在合理范围内) + (3) 超时 PARAM_LINK_TIMEOUT (8000ms) +退出动作: 准备进入下一条沟的转向 +``` + +#### GNAV_TURN_INTO_NEXT +``` +行为: 同 GNAV_TURN_INTO_CORRIDOR (v=0, w=±turn_omega) +安全模式: SAFETY_MODE_TURN +进入时: 记录 turn_start_yaw; 从 track_map 查询方向 (同出沟方向) +退出条件: |delta_yaw| ≥ 90° - PARAM_TURN_TOLERANCE +退出动作: EKF 重置 → REACQUIRE +``` + +#### GNAV_EXIT_STRAIGHT +``` +行为: v=PARAM_EXIT_V (0.15 m/s), w=IMU航向保持PD + 航向目标: 朝向出口方向（向下） +安全模式: SAFETY_MODE_STRAIGHT (但前向停车距离放宽) +进入时: 记录起始里程 +退出条件: 以下任一: + (1) 侧向VL53全部丢失 + 里程 ≥ PARAM_EXIT_RUNOUT (1.5m) + (2) 里程 ≥ PARAM_EXIT_MAX_DIST (4.0m, 保护) + (3) 超时 PARAM_EXIT_TIMEOUT (20000ms) +退出动作: 进入回停 +``` + +#### GNAV_DOCK +``` +行为: v=PARAM_DOCK_V (0.05 m/s), w=0 + 推进一段短距离后停车 +安全模式: SAFETY_MODE_STRAIGHT +进入时: 记录起始里程 +退出条件: 里程 ≥ PARAM_DOCK_DISTANCE (0.5m) 或超时 +退出动作: 强制零速 → FINISHED +``` + +#### GNAV_FINISHED +``` +行为: v=0, w=0 (永久停车) +安全模式: SAFETY_MODE_IDLE +终态: 不再转移 +``` + +#### GNAV_ERROR +``` +行为: v=0, w=0 (紧急停车) +安全模式: SAFETY_MODE_IDLE +说明: 任何阶段超时保护触发后进入 + 等待 PARAM_ERROR_HOLD_TIME 后 → FINISHED +``` + +--- + +## 5. 地图数据模型 + +### 5.1 设计原则 + +地图不做全局坐标定位。地图只回答三个问题： +1. 从第 N 条沟完成后，下一条是第几条？ +2. 这次该往哪转？（左/右） +3. 当前是不是最后一条沟？ + +### 5.2 数据结构 + +```c +/* ---- App/nav/track_map.h ---- */ + +#define TRACK_MAP_CORRIDOR_COUNT 6 +#define TRACK_MAP_LINK_DISTANCE_M 0.70f /* 沟间距 (中心到中心) */ +#define TRACK_MAP_CORRIDOR_LENGTH_M 2.20f /* 垄沟标称长度 */ + +typedef enum { + TRAVEL_DIR_POSITIVE, /* 从左端到右端 (→) */ + TRAVEL_DIR_NEGATIVE /* 从右端到左端 (←) */ +} TravelDirection_t; + +typedef enum { + TURN_DIR_LEFT = +1, /* 逆时针 (CCW), w > 0 */ + TURN_DIR_RIGHT = -1 /* 顺时针 (CW), w < 0 */ +} TurnDirection_t; + +/* 单条垄沟描述 */ +typedef struct { + uint8_t id; /* 0-5 */ + TravelDirection_t travel_dir; /* 本沟行驶方向 */ + TurnDirection_t exit_turn_dir; /* 出沟时的转向方向 */ + TurnDirection_t entry_turn_dir; /* 入沟时的转向方向 (= exit_turn_dir) */ + bool is_last; /* 是否为最后一条沟 */ +} CorridorDescriptor_t; + +/* 地图查询接口 */ +typedef struct { + CorridorDescriptor_t corridors[TRACK_MAP_CORRIDOR_COUNT]; + uint8_t entry_corridor_id; /* 入场后第一条沟 = 0 */ + float link_distance_m; /* 连接段标称距离 */ + float corridor_length_m; /* 垄沟标称长度 */ +} TrackMap_t; +``` + +### 5.3 地图初始化 (硬编码 S 型遍历表) + +```c +/* ---- App/nav/track_map.c ---- */ + +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: ←, 最后一条 + }, + .entry_corridor_id = 0, + .link_distance_m = 0.70f, + .corridor_length_m = 2.20f, +}; + +/* 查询 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); +``` + +> **为什么 C6 出沟后是左转？** +> C6(←) 行驶到左端完成后，需要朝向出口（向下）方向离场。从向左(-X)行驶的状态左转90°后，正好朝向下方(-Y)，对准出口。 + +--- + +## 6. 新增模块详细设计 + +### 6.1 `App/nav/global_nav.c/.h` — 赛道级总控 + +这是整个混合导航的核心模块，替代现有 `nav_script.c` 的赛道编排职能。 + +#### 头文件接口 + +```c +/* ---- App/nav/global_nav.h ---- */ + +#include "corridor_msgs.h" +#include "track_map.h" + +/* 安全模式 (传递给 segment_fsm) */ +typedef enum { + SAFETY_MODE_IDLE, /* 零速，不做任何裁剪 */ + SAFETY_MODE_CORRIDOR, /* 沟内: 前向减速/停车/E-STOP 全开 */ + SAFETY_MODE_TURN, /* 转向: 允许v=0+w≠0, 前向不全停 */ + SAFETY_MODE_STRAIGHT /* 直行段: 前后激光防撞, 无侧向约束 */ +} SafetyMode_t; + +/* 赛道级导航配置 */ +typedef struct { + float entry_v; /* 入场速度 m/s */ + float entry_distance; /* 入场里程上限 m */ + uint32_t entry_timeout_ms; /* 入场超时 ms */ + + float turn_omega; /* 转向角速度 rad/s */ + float turn_tolerance_rad; /* 转向完成容差 rad */ + float turn_decel_zone_rad; /* 接近目标角时的减速区 rad */ + float turn_min_omega; /* 减速区最低角速度 rad/s */ + uint32_t turn_timeout_ms; /* 单次转向超时 ms */ + + float reacquire_v; /* 重捕获速度 m/s */ + float reacquire_conf_thresh; /* 重捕获置信度阈值 */ + float reacquire_width_tol; /* 走廊宽度容差 m */ + uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */ + uint32_t reacquire_timeout_ms; + + float corridor_end_detect_dist; /* 到端检测距离 m */ + float corridor_length_max; /* 沟内里程保护上限 m */ + + float link_v; /* 连接段速度 m/s */ + float link_distance; /* 连接段标称距离 m */ + uint32_t link_timeout_ms; + + float exit_v; /* 出场速度 m/s */ + float exit_runout; /* 出场冲刺距离 m */ + float exit_max_dist; /* 出场里程保护 m */ + uint32_t exit_timeout_ms; + + float dock_v; /* 回停速度 m/s */ + float dock_distance; /* 回停距离 m */ + + float heading_kp; /* 航向保持P增益 */ +} GlobalNavConfig_t; + +/* 赛道级导航输出 */ +typedef struct { + GlobalNavStage_t stage; /* 当前阶段 */ + const char* stage_name; /* 阶段名字符串 (调试) */ + uint8_t corridor_id; /* 当前/目标垄沟 ID */ + uint8_t corridors_done; /* 已完成垄沟数 */ + + bool request_corridor; /* 是否请求沟内闭环 */ + bool use_override; /* 是否用 override 指令 */ + float override_v; + float override_w; + + SafetyMode_t safety_mode; /* 当前安全模式 */ + bool active; /* 导航是否在运行 */ +} GlobalNavOutput_t; + +/* API */ +void GlobalNav_Init(const GlobalNavConfig_t* cfg); +void GlobalNav_Start(void); +void GlobalNav_Stop(void); +void GlobalNav_Reset(void); +void GlobalNav_Update( + const CorridorObs_t* obs, + const CorridorState_t* state, + const RobotBlackboard_t* board, /* 需要读 IMU yaw_continuous, odom */ + GlobalNavOutput_t* out +); +GlobalNavStage_t GlobalNav_GetStage(void); +``` + +#### 内部状态 + +```c +/* ---- global_nav.c 内部 ---- */ + +static struct { + GlobalNavStage_t stage; + bool running; + + /* 赛道级 */ + uint8_t current_corridor_id; + uint8_t corridors_completed; + + /* 转向 */ + float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */ + float turn_target_delta_deg; /* 90° = ±90° */ + int8_t turn_sign; /* +1 or -1 */ + + /* 里程 */ + float stage_start_odom_s; /* EKF s 或 里程计 at stage entry */ + float stage_start_vx_accum; /* 里程计累积距离 */ + + /* 超时 */ + uint32_t stage_start_ms; + + /* 重捕获 */ + uint8_t reacquire_ok_count; + + /* 出场 */ + bool exit_vl53_lost; + float exit_lost_odom_s; + + /* 航向保持 */ + float heading_ref_deg; /* 直行段航向参考 */ + + /* 配置引用 */ + GlobalNavConfig_t cfg; +} s_nav; +``` + +#### 核心 Update 逻辑 (伪代码) + +```c +void GlobalNav_Update(..., GlobalNavOutput_t* out) { + uint32_t now = HAL_GetTick(); + float imu_yaw_deg = board->imu_yaw_continuous.value; + float odom_vx = board->odom_vx.value; + float elapsed_ms = now - s_nav.stage_start_ms; + + // 默认输出 + out->use_override = true; + out->request_corridor = false; + out->override_v = 0; + out->override_w = 0; + out->safety_mode = SAFETY_MODE_IDLE; + out->stage = s_nav.stage; + out->corridor_id = s_nav.current_corridor_id; + out->corridors_done = s_nav.corridors_completed; + out->active = s_nav.running; + + if (!s_nav.running) return; + + switch (s_nav.stage) { + + case GNAV_ENTRY_STRAIGHT: + out->override_v = s_nav.cfg.entry_v; + out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, s_nav.cfg.heading_kp); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + // 退出: 侧向探到壁 或 里程到 或 超时 + if (side_walls_detected(obs) || odom_advanced_enough() || elapsed_ms > s_nav.cfg.entry_timeout_ms) + transition_to(GNAV_TURN_INTO_CORRIDOR); + break; + + case GNAV_TURN_INTO_CORRIDOR: + case GNAV_TURN_OUT_OF_CORRIDOR: + case GNAV_TURN_INTO_NEXT: + execute_turn(obs, state, board, out); + break; + + case GNAV_REACQUIRE: + out->override_v = s_nav.cfg.reacquire_v; + out->override_w = heading_hold_pd(...); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (check_reacquire_conditions(obs, state)) + s_nav.reacquire_ok_count++; + else + s_nav.reacquire_ok_count = 0; + + if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) { + // 重捕获成功，进入沟内跟踪 + transition_to(GNAV_CORRIDOR_TRACK); + } + if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) + transition_to(GNAV_ERROR); + break; + + case GNAV_CORRIDOR_TRACK: + out->use_override = false; + out->request_corridor = true; + out->safety_mode = SAFETY_MODE_CORRIDOR; + + // 到端检测 + bool front_valid = obs->valid_mask & CORRIDOR_OBS_MASK_FRONT; + if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) { + s_nav.corridors_completed++; + transition_to(GNAV_TURN_OUT_OF_CORRIDOR); + } + // 里程保护 + if (odom_since_entry() > s_nav.cfg.corridor_length_max) + transition_to(GNAV_TURN_OUT_OF_CORRIDOR); + break; + + case GNAV_LINK_STRAIGHT: + out->override_v = s_nav.cfg.link_v; + out->override_w = heading_hold_pd(...); + 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); + if (elapsed_ms > s_nav.cfg.link_timeout_ms) + transition_to(GNAV_ERROR); + break; + + case GNAV_EXIT_STRAIGHT: + out->override_v = s_nav.cfg.exit_v; + out->override_w = heading_hold_pd(...); + out->safety_mode = SAFETY_MODE_STRAIGHT; + + // 检测侧向全丢 + if (!s_nav.exit_vl53_lost && all_side_lost(obs)) { + s_nav.exit_vl53_lost = true; + s_nav.exit_lost_odom_s = current_odom(); + } + if (s_nav.exit_vl53_lost && (current_odom() - s_nav.exit_lost_odom_s) >= s_nav.cfg.exit_runout) + transition_to(GNAV_DOCK); + if (elapsed_ms > s_nav.cfg.exit_timeout_ms) + transition_to(GNAV_DOCK); + break; + + case GNAV_DOCK: + out->override_v = s_nav.cfg.dock_v; + out->override_w = 0; + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (odom_since_entry() >= s_nav.cfg.dock_distance || elapsed_ms > 5000) + transition_to(GNAV_FINISHED); + break; + + case GNAV_FINISHED: + out->override_v = 0; + out->override_w = 0; + out->active = false; + break; + + case GNAV_ERROR: + out->override_v = 0; + out->override_w = 0; + out->safety_mode = SAFETY_MODE_IDLE; + if (elapsed_ms > 2000) + transition_to(GNAV_FINISHED); + break; + } +} +``` + +#### transition_to 函数 + +```c +static void transition_to(GlobalNavStage_t next) { + // 通用: 记录进入时间和里程 + s_nav.stage_start_ms = HAL_GetTick(); + s_nav.stage_start_odom_s = /* 当前里程 */; + s_nav.reacquire_ok_count = 0; + + switch (next) { + case GNAV_TURN_INTO_CORRIDOR: { + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id); + s_nav.turn_sign = (int8_t)cd->entry_turn_dir; + s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */; + s_nav.turn_target_delta_deg = 90.0f; + break; + } + case GNAV_TURN_OUT_OF_CORRIDOR: { + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id); + s_nav.turn_sign = (int8_t)cd->exit_turn_dir; + s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */; + s_nav.turn_target_delta_deg = 90.0f; + break; + } + case GNAV_TURN_INTO_NEXT: { + uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id); + const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id); + s_nav.turn_sign = (int8_t)cd->entry_turn_dir; + s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */; + s_nav.turn_target_delta_deg = 90.0f; + s_nav.current_corridor_id = next_id; + break; + } + case GNAV_REACQUIRE: + CorridorFilter_Reset(); // EKF 重置 + s_nav.heading_ref_deg = /* 当前 IMU yaw */; + break; + + case GNAV_CORRIDOR_TRACK: + break; + + case GNAV_LINK_STRAIGHT: { + s_nav.heading_ref_deg = /* 当前 IMU yaw */; + break; + } + case GNAV_EXIT_STRAIGHT: { + // 最后一条沟出来后，转向应该已经朝向出口 + s_nav.heading_ref_deg = /* 当前 IMU yaw */; + s_nav.exit_vl53_lost = false; + break; + } + default: + break; + } + + s_nav.stage = next; +} +``` + +#### 转向执行 (统一的 90° 转向逻辑) + +```c +static void execute_turn(const CorridorObs_t* obs, const CorridorState_t* state, + const RobotBlackboard_t* board, GlobalNavOutput_t* out) { + float imu_yaw = board->imu_yaw_continuous.value; + float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign; + float target = s_nav.turn_target_delta_deg; + float remaining_deg = target - delta; + float remaining_rad = remaining_deg * PARAM_DEG2RAD_FACTOR; + + float omega = s_nav.cfg.turn_omega; + + // 接近目标时减速 + if (remaining_rad < s_nav.cfg.turn_decel_zone_rad) { + float ratio = remaining_rad / s_nav.cfg.turn_decel_zone_rad; + omega = s_nav.cfg.turn_min_omega + ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega); + } + + out->override_v = 0; + out->override_w = s_nav.turn_sign * omega; + out->use_override = true; + out->safety_mode = SAFETY_MODE_TURN; + + float tolerance_deg = s_nav.cfg.turn_tolerance_rad * PARAM_RAD2DEG_FACTOR; + + if (delta >= target - tolerance_deg) { + // 转向完成 + switch (s_nav.stage) { + case GNAV_TURN_INTO_CORRIDOR: + case GNAV_TURN_INTO_NEXT: + transition_to(GNAV_REACQUIRE); + break; + case GNAV_TURN_OUT_OF_CORRIDOR: + if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) + transition_to(GNAV_EXIT_STRAIGHT); + else + transition_to(GNAV_LINK_STRAIGHT); + break; + default: + break; + } + } + + // 超时保护 + if (HAL_GetTick() - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) + transition_to(GNAV_ERROR); +} +``` + +### 6.2 `App/nav/track_map.c/.h` — 赛道地图 + +见第 5 节数据结构。实现很简单，就是一个 const 表 + 查询函数。 + +```c +/* track_map.c */ +#include "track_map.h" + +static const TrackMap_t s_map = { /* 见 5.3 */ }; +static bool s_initialized = false; + +void TrackMap_Init(void) { + s_initialized = true; +} + +const TrackMap_t* TrackMap_Get(void) { + return &s_map; +} + +const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id) { + if (id >= TRACK_MAP_CORRIDOR_COUNT) return &s_map.corridors[0]; + return &s_map.corridors[id]; +} + +uint8_t TrackMap_GetNextCorridorId(uint8_t current_id) { + if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) return current_id; + return current_id + 1; +} + +bool TrackMap_IsLastCorridor(uint8_t id) { + return s_map.corridors[id].is_last; +} + +TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id) { + return s_map.corridors[id].exit_turn_dir; +} + +TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id) { + return s_map.corridors[id].entry_turn_dir; +} +``` + +--- + +## 7. 现有模块改造 + +### 7.1 `segment_fsm.c/.h` — 安全层增加动作模式感知 + +#### 改动要点 + +1. 新增 `SafetyMode_t` 参数输入 +2. `SegFsm_Update()` 签名变更: 增加 `SafetyMode_t mode` 参数 +3. 根据 mode 决定安全策略 + +#### 改动后的行为表 + +| SafetyMode | 前向减速/停车 | E-STOP (conf低) | w 通过 | +|------------|-------------|-----------------|--------| +| IDLE | 不触发 | 不触发 | 清零 | +| CORRIDOR | ✅ 正常触发 | ✅ 正常触发 | 正常 | +| TURN | ❌ 不触发 (v=0时不检查前距) | ❌ 不触发 (转向时conf低是正常的) | ✅ 直接通过 | +| STRAIGHT | ✅ 正常触发 | ❌ 不触发 | 正常 | + +#### 改动后的函数签名 + +```c +/* segment_fsm.h 改动 */ + +// 删除旧的: +// void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs, +// const CorridorState_t* state, SegFsmOutput_t* out); + +// 新增: +void SegFsm_Update(const RawCmd_t* raw_cmd, + const CorridorObs_t* obs, + const CorridorState_t* state, + SafetyMode_t mode, /* 新增 */ + SegFsmOutput_t* out); +``` + +#### 关键代码改动 (`segment_fsm.c`) + +```c +void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs, + const CorridorState_t* state, SafetyMode_t mode, + SegFsmOutput_t* out) { + + if (mode == SAFETY_MODE_IDLE) { + out->state = SEG_STATE_IDLE; + out->safe_v = 0; + out->safe_w = 0; + return; + } + + if (mode == SAFETY_MODE_TURN) { + // 转向模式: 直接放行，不做前向检查 + out->state = SEG_STATE_CORRIDOR; // 复用 CORRIDOR 表示放行 + out->safe_v = raw_cmd->v; + out->safe_w = raw_cmd->w; + // 可选: w 的绝对值做上限裁剪 + return; + } + + // CORRIDOR / STRAIGHT 模式: 执行原有逻辑 + // CORRIDOR 额外检查 conf → E-STOP + // STRAIGHT 不检查 conf + + if (mode == SAFETY_MODE_CORRIDOR) { + // 原有 E-STOP 逻辑 (conf 检查) + if (state->conf < s_cfg.conf_estop_thresh) { + s_state = SEG_STATE_ESTOP; + } + if (s_state == SEG_STATE_ESTOP && state->conf >= 0.5f) { + s_state = SEG_STATE_CORRIDOR; + } + } + + // 前向距离检查 (CORRIDOR 和 STRAIGHT 都做) + // ... 现有逻辑不变 ... +} +``` + +### 7.2 `nav_script.c/.h` — 降级为测试脚本 + +保留现有代码不删除，但不再在正式比赛流程中使用。 + +改动： +- 在 `app_tasks.c` 中通过编译开关选择使用 `NavScript` 还是 `GlobalNav` +- `nav_script.c` 保持现状，用于单垄沟测试验证 + +```c +/* app_tasks.c 中增加编译开关 */ +#define USE_GLOBAL_NAV 1 /* 1=正式比赛模式 0=单沟测试模式 */ +``` + +### 7.3 `corridor_msgs.h` — 补充枚举和结构 + +新增以下定义： + +```c +/* corridor_msgs.h 新增 */ + +/* 赛道级阶段枚举 (已在 global_nav.h 定义，这里仅前向声明) */ +/* #include "global_nav.h" 或直接在 corridor_msgs.h 中定义 */ + +/* 安全模式枚举 (可在此处定义，供 segment_fsm 和 global_nav 共享) */ +typedef enum { + SAFETY_MODE_IDLE, + SAFETY_MODE_CORRIDOR, + SAFETY_MODE_TURN, + SAFETY_MODE_STRAIGHT +} SafetyMode_t; +``` + +> 注意: `SafetyMode_t` 定义应放在 `corridor_msgs.h` 中（数据契约），而非 `global_nav.h` 中，以便 `segment_fsm` 不需要反向依赖 `global_nav`。 + +### 7.4 `corridor_filter.c/.h` — 增加外部 Reset 接口 + +当前 `CorridorFilter_Reset()` 可能不存在或功能不完整。需要确保： + +```c +/* corridor_filter.h 确认/新增 */ +void CorridorFilter_Reset(void); +``` + +该函数应： +1. 调用 `CorridorEKF_Reset()` +2. 重置 yaw_ref 状态 +3. 重置 yaw_ref_locked 标志 + +这在每次进入新垄沟时必须调用，否则 EKF 会用旧沟的参考值去更新新沟的状态。 + +### 7.5 `app_tasks.c` — navTask 流水线改造 + +详见第 8 节。 + +--- + +## 8. 数据流水线改造 + +### 8.1 改造后的 navTask 流水线 + +```c +/* app_tasks.c - AppTasks_RunNavTask_Impl() */ + +void AppTasks_RunNavTask_Impl(void) { + static uint32_t last_ms = 0; + uint32_t now = HAL_GetTick(); + float dt = (now - last_ms) * 0.001f; + if (dt <= 0.0f || dt > 0.1f) dt = 0.02f; + last_ms = now; + + /* ========== Step 1: 传感器快照 (不变) ========== */ + RobotBlackboard_t board; + Blackboard_GetSnapshot(&board); + + /* ========== Step 2: 预处理 (不变) ========== */ + CorridorObs_t obs; + CorridorPreproc_ExtractObs(&board, now, &obs); + + /* ========== Step 3: EKF (不变) ========== */ + float imu_wz_rad = PARAM_DEG2RAD(board.imu_wz.value); + float odom_vx = board.odom_vx.value; + float imu_yaw_rad = PARAM_DEG2RAD(board.imu_yaw_continuous.value); + bool imu_yaw_ok = board.imu_yaw_continuous.is_valid; + + CorridorState_t corridor_state; + CorridorFilter_Update(&obs, imu_wz_rad, odom_vx, dt, + imu_yaw_rad, imu_yaw_ok, &corridor_state); + +#if USE_GLOBAL_NAV + /* ========== Step 4: 赛道级导航 (新) ========== */ + GlobalNavOutput_t nav_out; + GlobalNav_Update(&obs, &corridor_state, &board, &nav_out); + + /* ========== Step 5: 控制律 ========== */ + RawCmd_t raw_cmd = {0}; + if (nav_out.request_corridor) { + CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd); + } else if (nav_out.use_override) { + raw_cmd.v = nav_out.override_v; + raw_cmd.w = nav_out.override_w; + } + + /* ========== Step 6: 安全仲裁 (改造) ========== */ + SegFsmOutput_t fsm_out; + SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out); + +#else + /* ---------- 旧版: 单沟测试模式 ---------- */ + NavScriptOutput_t script_out; + float imu_yaw_deg = board.imu_yaw_continuous.value; + NavScript_Update(&obs, &corridor_state, imu_yaw_deg, &script_out); + + RawCmd_t raw_cmd = {0}; + if (script_out.use_override) { + raw_cmd.v = script_out.override_v; + raw_cmd.w = script_out.override_w; + } else if (script_out.request_corridor) { + CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd); + } + + SegFsmOutput_t fsm_out; + SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out); +#endif + + /* ========== Step 7: 推送指令 (不变) ========== */ + CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0); +} +``` + +### 8.2 初始化改造 + +```c +void AppTasks_Init(void) { + // ... 现有初始化不变 ... + + // 新增 + TrackMap_Init(); + + GlobalNavConfig_t gnav_cfg = { + .entry_v = PARAM_GNAV_ENTRY_V, + .entry_distance = PARAM_GNAV_ENTRY_DISTANCE, + .entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT, + .turn_omega = PARAM_GNAV_TURN_OMEGA, + .turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE, + .turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE, + .turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA, + .turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT, + .reacquire_v = PARAM_GNAV_REACQUIRE_V, + .reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF, + .reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL, + .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS, + .reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT, + .corridor_end_detect_dist = PARAM_SAFE_D_FRONT_STOP, + .corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN, + .link_v = PARAM_GNAV_LINK_V, + .link_distance = PARAM_GNAV_LINK_DISTANCE, + .link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT, + .exit_v = PARAM_GNAV_EXIT_V, + .exit_runout = PARAM_GNAV_EXIT_RUNOUT, + .exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST, + .exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT, + .dock_v = PARAM_GNAV_DOCK_V, + .dock_distance = PARAM_GNAV_DOCK_DISTANCE, + .heading_kp = PARAM_GNAV_HEADING_KP, + }; + GlobalNav_Init(&gnav_cfg); + + // ... 延迟启动 ... + // 在 navTask 中延迟后调用 GlobalNav_Start() +} +``` + +--- + +## 9. 参数清单 + +### 9.1 新增参数 (`robot_params.h`) + +```c +/* ======== P6 — 赛道级导航参数 ======== */ + +/* 入场段 */ +#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 */ +#define PARAM_GNAV_ENTRY_DISTANCE 0.65f /* m — 入场里程上限 */ +#define PARAM_GNAV_ENTRY_TIMEOUT 10000 /* ms — 入场超时 */ + +/* 转向 */ +#define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */ +#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */ +#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */ +#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */ +#define PARAM_GNAV_TURN_TIMEOUT 8000 /* ms — 单次转向超时 */ + +/* 重捕获 */ +#define PARAM_GNAV_REACQUIRE_V 0.05f /* m/s — 重捕获入沟速度 */ +#define PARAM_GNAV_REACQUIRE_CONF 0.6f /* — — 重捕获置信度阈值 */ +#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f /* m — 走廊宽度容差 */ +#define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数 (5×20ms=100ms) */ +#define PARAM_GNAV_REACQUIRE_TIMEOUT 5000 /* ms — 重捕获超时 */ + +/* 沟内 */ +#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.50f /* m — 沟内里程保护上限 */ + +/* 连接段 */ +#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 */ +#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距) */ +#define PARAM_GNAV_LINK_TIMEOUT 8000 /* ms — 连接段超时 */ + +/* 出场 */ +#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 20000 /* ms — 出场超时 */ + +/* 回停 */ +#define PARAM_GNAV_DOCK_V 0.05f /* m/s — 回停速度 */ +#define PARAM_GNAV_DOCK_DISTANCE 0.50f /* m — 回停推进距离 */ + +/* 航向保持 */ +#define PARAM_GNAV_HEADING_KP 0.03f /* — — 航向保持P增益 (输入°，输出rad/s) */ + +/* 编译开关 */ +#define USE_GLOBAL_NAV 1 /* 1=赛道模式 0=单沟测试 */ +``` + +### 9.2 现有参数复用 + +以下现有参数直接复用，无需修改： + +| 参数 | 用途 | +|------|------| +| `PARAM_SAFE_D_FRONT_STOP` | 到端检测距离 (沟内) | +| `PARAM_SAFE_D_FRONT_APPROACH` | 前向减速预警 | +| `PARAM_SAFE_CONF_ESTOP` | 沟内 E-STOP 阈值 | +| `PARAM_CTRL_*` | 沟内控制器增益 | +| `PARAM_EKF_*` | EKF 滤波参数 | +| `PARAM_CORRIDOR_WIDTH` | 走廊宽度 | + +--- + +## 10. 文件清单汇总 + +### 10.1 新增文件 + +| 文件 | 位置 | 职责 | 代码量估计 | +|------|------|------|-----------| +| `global_nav.h` | `App/nav/` | 赛道级导航头文件 | ~120 行 | +| `global_nav.c` | `App/nav/` | 赛道级导航实现 | ~500 行 | +| `track_map.h` | `App/nav/` | 地图数据结构 | ~60 行 | +| `track_map.c` | `App/nav/` | 地图查询实现 | ~80 行 | + +**总计新增: 4 个文件，约 760 行代码** + +### 10.2 需要修改的文件 + +| 文件 | 改动内容 | 改动量 | +|------|---------|--------| +| `App/nav/segment_fsm.h` | `SegFsm_Update()` 签名增加 `SafetyMode_t` 参数 | ~10 行 | +| `App/nav/segment_fsm.c` | 根据 SafetyMode 分支安全策略 | ~50 行 | +| `App/preproc/corridor_msgs.h` | 新增 `SafetyMode_t` 枚举定义 | ~10 行 | +| `App/est/corridor_filter.h` | 确认 `CorridorFilter_Reset()` 接口 | ~5 行 | +| `App/est/corridor_filter.c` | 实现完整 Reset (含 yaw_ref 重置) | ~15 行 | +| `App/app_tasks.c` | navTask 流水线替换 + 初始化改造 | ~80 行 | +| `App/robot_params.h` | 新增 P6 参数组 | ~30 行 | +| `CMakeLists.txt` | 新增源文件编译 | ~3 行 | + +**总计修改: 8 个文件，约 200 行改动** + +### 10.3 不需要改动的文件 + +| 文件 | 说明 | +|------|------| +| `App/Can/snc_can_app.c/.h` | 已冻结，勿动 | +| `App/Contract/robot_blackboard.c/.h` | 接口已足够 | +| `App/Contract/robot_odom.c/.h` | 接口已足够 | +| `App/Contract/robot_cmd_slot.c/.h` | 接口已足够 | +| `App/est/corridor_ekf.c/.h` | 数学核心不变 | +| `App/preproc/corridor_preproc.c/.h` | 预处理逻辑不变 | +| `App/nav/corridor_ctrl.c/.h` | 控制律不变 | +| `App/nav/nav_script.c/.h` | 保留不删，通过编译开关切换 | +| `App/IMU/hwt101.c/.h` | 驱动不变 | +| `App/laser/laser_manager.c/.h` | 驱动不变 | +| `App/VL53L0X_API/**` | 驱动不变 | + +--- + +## 11. 分步实施计划 + +### Phase 0: 准备工作 (半天) + +**目标**: 为新模块搭建脚手架，不影响现有功能 + +1. 在 `robot_params.h` 中添加 P6 参数组和 `USE_GLOBAL_NAV` 开关（先设为 0） +2. 在 `corridor_msgs.h` 中添加 `SafetyMode_t` 枚举 +3. 创建 `track_map.h/.c` 空壳文件 +4. 创建 `global_nav.h/.c` 空壳文件（Init/Start/Stop/Update 都是空实现） +5. 修改 `CMakeLists.txt` 添加新源文件 +6. 确认编译通过 + +**验证**: `cmake --build build/Debug` 无错误 + +--- + +### Phase 1: 安全层改造 (1天) + +**目标**: 让安全层支持动作模式，解决 RISK-1 + +1. 修改 `segment_fsm.h`: `SegFsm_Update()` 增加 `SafetyMode_t mode` 参数 +2. 修改 `segment_fsm.c`: 根据 mode 分支安全策略 +3. 修改 `app_tasks.c` 中调用 `SegFsm_Update()` 的地方，旧模式传 `SAFETY_MODE_CORRIDOR` + +**验证**: +- `USE_GLOBAL_NAV=0` 编译通过 +- 现有单沟测试行为不变 (回归测试) +- 手动测试 `SAFETY_MODE_TURN` 模式下 v=0+w≠0 能通过安全层 + +--- + +### Phase 2: 地图模块 (半天) + +**目标**: 完成 `track_map` 并验证查询逻辑 + +1. 实现 `track_map.c` 的全部查询函数 +2. 在 `global_nav.c` 的 Init 中调用 `TrackMap_Init()` + +**验证**: 在初始化日志中打印 6 条沟的遍历顺序和转向方向，确认 S 型正确 + +--- + +### Phase 3: 赛道级状态机核心 (2-3天) + +**目标**: 实现 `global_nav.c` 的完整状态机 + +分步： +1. 先实现 IDLE → ENTRY_STRAIGHT → TURN_INTO_CORRIDOR → REACQUIRE → CORRIDOR_TRACK 链路 +2. 测试单沟入沟流程 +3. 再实现 CORRIDOR_TRACK → TURN_OUT → LINK_STRAIGHT → TURN_INTO_NEXT → REACQUIRE 链路 +4. 测试双沟 S 型过渡 +5. 最后实现 EXIT_STRAIGHT → DOCK → FINISHED 链路 +6. 测试完整流程 + +**验证**: +- 用 printf 打印状态转移日志 +- 先在桌面空转 (无传感器) 验证状态机逻辑 +- 再实车单沟测试 +- 再实车双沟测试 +- 最后实车全 6 沟测试 + +--- + +### Phase 4: EKF 重置流程 (半天) + +**目标**: 确保每次进入新垄沟时 EKF 状态干净 + +1. 确认 `CorridorFilter_Reset()` 完整重置 EKF + yaw_ref +2. 在 REACQUIRE 进入时调用 Reset +3. 验证重捕获判据在各种入沟角度下的鲁棒性 + +**验证**: 连续跑 2-3 条沟，观察每条沟入沟后 EKF conf 的收敛曲线 + +--- + +### Phase 5: 流水线集成 (1天) + +**目标**: `USE_GLOBAL_NAV=1` 时完整流水线打通 + +1. 修改 `app_tasks.c` 的 `AppTasks_RunNavTask_Impl()` +2. 修改 `AppTasks_Init()` 的初始化流程 +3. 用 `#if USE_GLOBAL_NAV` 保留旧路径 + +**验证**: 全编译 + 实车全流程测试 + +--- + +### Phase 6: 参数调优与鲁棒化 (2-3天) + +**目标**: 在实车上反复测试并调优 + +重点调优项： +1. 入场段里程和速度 +2. 转向角速度和容差 +3. 重捕获判据阈值和确认拍数 +4. 连接段里程和超时 +5. 出场检测逻辑 +6. 各阶段超时值 + +--- + +### 总工期估计 + +| 阶段 | 工期 | 累计 | +|------|------|------| +| Phase 0: 准备 | 0.5 天 | 0.5 天 | +| Phase 1: 安全层 | 1 天 | 1.5 天 | +| Phase 2: 地图 | 0.5 天 | 2 天 | +| Phase 3: 状态机 | 2-3 天 | 4-5 天 | +| Phase 4: EKF 重置 | 0.5 天 | 4.5-5.5 天 | +| Phase 5: 集成 | 1 天 | 5.5-6.5 天 | +| Phase 6: 调优 | 2-3 天 | 7.5-9.5 天 | +| **总计** | **~8-10 天** | | + +--- + +## 12. 风险与降级策略 + +### 12.1 已知风险 + +| 风险 | 严重度 | 说明 | 缓解措施 | +|------|--------|------|---------| +| **转向精度不足** | 高 | IMU 漂移导致 90° 转角不准 | 转向后用 VL53 重捕获作为闭环验证；容差设 5° | +| **连接段偏航** | 高 | IMU 短时漂移 + 轮子打滑导致连接段走歪 | 里程上限保护 + 超时保护 + 侧向VL53早期探测 | +| **重捕获失败** | 中 | 入沟角度过大导致 VL53 无法同时看到两侧壁 | 重捕获阶段允许单侧先有效，慢速蠕行直到双侧 | +| **到端检测假阳性** | 中 | 沟内障碍物被误判为端部围栏 | 可加里程下限保护 (已走 > 1.5m 才允许到端) | +| **出场方向错误** | 低 | 最后一条沟出来后航向偏差过大 | 出场段用 IMU 保持航向 + 里程超限保护 | + +### 12.2 降级策略 + +| 场景 | 降级方案 | +|------|---------| +| 重捕获超时 | → ERROR → 停车 (保得分，不撞) | +| 连接段超时 | → ERROR → 停车 | +| 任何阶段 IMU 数据异常 | → ERROR → 停车 | +| 只能跑 3 条沟 | 修改 `track_map` 的 `is_last` 标志，提前结束 | +| 来不及实现完整出场 | 最后一条沟结束后直接停车 (放弃回停得分) | + +### 12.3 不做什么 (明确的非目标) + +1. **不做 SLAM / 全局定位**: 不需要知道 (x, y) 绝对坐标 +2. **不做路径规划**: S 型路径是硬编码的，不需要搜索 +3. **不做目标物检测/抓取**: 本方案只管导航，不管捡拾 +4. **不重写 EKF**: 现有 EKF 对沟内跟踪已经够用 +5. **不重写驱动层**: 传感器驱动、CAN 协议、里程计全部保留 +6. **不做动态避障**: 沟内假设无动态障碍物 + +--- + +## 附录 A: 完整 S 型遍历动作序列表 + +| # | 状态 | 行为 | v (m/s) | w (rad/s) | 主传感器 | 退出条件 | +|---|------|------|---------|-----------|---------|---------| +| 0 | IDLE | 等待 | 0 | 0 | — | Start() | +| 1 | ENTRY_STRAIGHT | 慢速前进 | 0.08 | PD保直 | IMU | 侧向VL53有效/里程/超时 | +| 2 | TURN_INTO_CORRIDOR (右转) | 原地转90° | 0 | -1.0 | IMU yaw | 转角≥85° | +| 3 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效+conf≥0.6 | +| 4 | CORRIDOR_TRACK (C1→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 5 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 6 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m | +| 7 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 8 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 | +| 9 | CORRIDOR_TRACK (C2←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 10 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° | +| 11 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m | +| 12 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° | +| 13 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 | +| 14 | CORRIDOR_TRACK (C3→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 15 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 16 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m | +| 17 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 18 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 | +| 19 | CORRIDOR_TRACK (C4←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 20 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° | +| 21 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m | +| 22 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° | +| 23 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 | +| 24 | CORRIDOR_TRACK (C5→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 25 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 26 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m | +| 27 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 28 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 | +| 29 | CORRIDOR_TRACK (C6←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m | +| 30 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° | +| 31 | EXIT_STRAIGHT | 朝出口直行 | 0.15 | PD保直 | IMU+odom | VL53全丢+里程 | +| 32 | DOCK | 回停 | 0.05 | 0 | odom | 里程≥0.5m | +| 33 | FINISHED | 停车 | 0 | 0 | — | 终态 | + +> **全程共**: 6 次沟内跟踪 + 11 次 90° 转向 + 5 次连接段 + 6 次重捕获 + 1 次入场 + 1 次出场 + 1 次回停 = **33 步动作** + +--- + +## 附录 B: 关键设计决策记录 + +| 决策 | 选择 | 理由 | +|------|------|------| +| 地图用静态表还是动态生成 | **静态表** | 场地固定，运行时不变 | +| 转向用 90° 还是 180° | **90°** | S 型遍历天然是 90° 转向 | +| 是否需要全局坐标 (x, y) | **不需要** | 拓扑+里程足够，全局定位反而引入累积误差 | +| 重捕获判据用 conf 还是 raw VL53 | **两者都用** | conf 依赖 EKF 收敛时间，raw VL53 提供即时几何校验 | +| EKF 每次入沟是否 Reset | **Reset** | 不同沟的 e_y 参考不同，必须重建 | +| 航向保持用 PD 还是纯 P | **纯 P** | 连接段短 (<1s)，微分项贡献小，简化设计 | +| 出场方向如何确定 | **IMU 保持最后转出方向** | 最后一条沟出来后已经朝向出口 | +| 混合导航模块合几个文件 | **global_nav 1个 + track_map 1个** | 原方案拆 6 个文件过碎，实际耦合度高，合并更清晰 | +| 是否保留 nav_script | **保留，编译开关切换** | 作为单沟验证脚本仍有价值 | + +--- + +## 附录 C: 与混合导航方案.md 的差异说明 + +| 混合导航方案.md 建议 | 本方案处理 | 原因 | +|---------------------|----------|------| +| 新增 6 个文件 (global_nav_fsm, track_map, lane_transition, reacquire_detector, heading_hold, exit_dock) | **合并为 2 个文件** (global_nav, track_map) | lane_transition / reacquire / heading_hold / exit_dock 的逻辑都嵌入 global_nav 的状态处理中，单独成文件会增加模块间调用复杂度，且每个文件代码量不足 100 行 | +| 航向不再使用 VL53 差分 | **保留 EKF 中的 VL53 航向观测，但降权** | 完全移除 VL53 航向观测需要改 EKF 数学，风险大；EKF 已有 chi2 拒绝机制，让它自然退化即可 | +| 赛道级核心状态量列了 10 个变量 | **精简为必要字段** | `stage_progress` 等概念已被 odom_s + elapsed_ms 覆盖 | +| REACQUIRE_CORRIDOR 作为独立阶段 | **确认保留** | 这是连接沟内闭环与段间动作的关键过渡，必须显式建模 | +| segment_fsm 区分 4 种动作模式 | **确认采纳，但简化为 SafetyMode_t 枚举** | 不需要传复杂结构体，一个枚举足够 | + +--- + +> **文档结束** +> 本方案设计完成后，实施第一步应是 Phase 0（脚手架搭建），确保不破坏现有功能的前提下开始增量开发。 diff --git a/Doc/混合导航方案.md b/Doc/混合导航方案.md new file mode 100644 index 0000000..ab358bb --- /dev/null +++ b/Doc/混合导航方案.md @@ -0,0 +1,549 @@ +# 混合导航方案 + +## 1. 文档目的 + +本文档用于明确本项目后续正式比赛版导航应采用的总体方案。 + +目标不是重写当前全部导航代码，而是： + +1. 保留现有“垄沟内局部闭环控制”能力 +2. 在其上补齐赛道级状态机与段间动作编排 +3. 让机器人能够按照固定地图完成 6 条垄沟的 S 型遍历 +4. 最终从唯一出口驶离并停回启动区 + +本文档强调的是“混合导航”： + +- 上层使用固定地图和拓扑状态机决定现在该去哪 +- 中层使用动作执行器完成转向、连接段推进、再入沟 +- 下层使用现有局部传感器闭环完成沟内稳定行驶 + +它不是纯局部反应式导航，也不是通用 SLAM。 + +## 2. 已知场地理解 + +根据 `Doc/map.md`： + +- 场地净尺寸约为 `300cm x 390cm` +- 内部有 `5` 条田垄 +- 因围栏与田垄、田垄与田垄之间均有通道，所以可通行垄沟实际为 `6` 条 +- 启动区位于场地下侧靠左，外接唯一入口 +- 各条垄沟是横向分布的长通道 +- 垄沟间通过左右两端的短连接段串起来 + +因此，比赛中的真实轨迹不是“在端部横移搜索下一条沟”，而是： + +1. 从启动区进入场地 +2. 沿入口直线段前进 +3. 到第 1 条垄沟入口附近 +4. 原地转 `90°` 入沟 +5. 沿垄沟通过 +6. 到端后原地转 `90°` +7. 走一小段连接直线 +8. 再原地转 `90°` 入下一条垄沟 +9. 重复以上动作，形成 **S 型遍历** +10. 全部垄沟完成后离场并回停启动区 + +## 3. 为什么要做混合导航 + +当前项目已经具备较强的局部能力： + +- 4 路侧向 VL53 做走廊观测 +- IMU 提供 `wz` 和 `yaw_continuous` +- EKF / Filter 输出 `e_y`、`e_th`、`conf` +- `corridor_ctrl` 输出沟内控制指令 +- `segment_fsm` 负责安全裁剪 +- `nav_script` 能做单段脚本验证 + +但这套能力本质上仍然偏向： + +- “单条垄沟怎么跑稳” +- 而不是 +- “整张赛道下一步该去哪里” + +正式比赛需要解决的核心问题有： + +1. 当前正在第几条垄沟 +2. 下一条应该进入哪条垄沟 +3. 当前应该左转还是右转 +4. 什么时候从沟内控制切到端部动作 +5. 什么时候从端部动作切回沟内控制 +6. 什么时候结束全部遍历并离场 +7. 离场后如何回停到启动区 + +这些问题无法只靠局部测距瞬时值回答，必须引入上层任务状态。 + +## 4. 混合导航的核心思想 + +本项目推荐采用： + +**固定地图 + 赛道级状态机 + 局部闭环控制** + +其中： + +- 固定地图负责描述赛道结构 +- 状态机负责描述任务推进 +- 局部闭环负责把当前这一小段走稳 + +整体思路是： + +- 用地图回答“接下来去哪” +- 用状态机回答“现在该做什么动作” +- 用传感器闭环回答“这一段怎么安全稳定地过去” + +## 5. 三层架构 + +### 5.1 上层：赛道级导航层 + +职责： + +- 记录当前 `corridor_id` +- 决定下一个目标 `target_corridor_id` +- 决定当前阶段 +- 决定下一步是左转还是右转 +- 在所有阶段之间推进任务 + +这层不直接控制车轮，只输出“当前应该执行哪种段动作”。 + +### 5.2 中层：段动作执行层 + +职责： + +- 入场直线推进 +- `90°` 原地转向 +- 连接段直线推进 +- 再次 `90°` 入沟 +- 出场段动作 +- 回停启动区动作 + +这层输出当前周期的期望 `v/w`，但仍需经过安全层裁剪。 + +### 5.3 下层：局部闭环控制层 + +职责： + +- 在垄沟内保持居中 +- 控制 `e_y` 和 `e_th` +- 提供局部重捕获判据 +- 在当前段可观测时给出稳定闭环 + +这一层尽量复用现有实现，不重复发明轮子。 + +## 6. 当前代码与未来架构的对应关系 + +现有代码可保留并复用的部分： + +1. `App/preproc/` +- 继续负责传感器清洗与观测构造 + +2. `App/est/` +- 继续负责 `e_y / e_th / conf` 估计 + +3. `App/nav/corridor_ctrl.c` +- 继续负责沟内局部控制 + +4. `App/Contract/robot_blackboard.*` +- 继续作为全局传感器快照中心 + +5. `App/Contract/robot_cmd_slot.*` +- 继续作为导航输出到 CAN 的命令槽 + +6. `App/nav/segment_fsm.*` +- 保留为安全层，但后续必须增加“动作语义感知” + +当前不应再承担最终比赛全局职责的部分： + +1. `App/nav/nav_script.c` +- 当前更像“单垄沟验证脚本” +- 不适合继续膨胀成完整赛道导航总控 + +因此后续应新增赛道级模块，而不是把全部逻辑继续堆进 `nav_script.c`。 + +## 7. 推荐状态机建模 + +建议把赛道任务拆成以下大阶段： + +1. `START_ZONE` +- 启动区待发 + +2. `ENTRY_STRAIGHT` +- 从启动区经唯一入口进入场地 +- 沿左侧入口直线段前进 + +3. `TURN_INTO_CORRIDOR` +- 到目标垄沟入口后原地转 `90°` +- 对准目标垄沟 + +4. `CORRIDOR_TRACK` +- 沟内闭环跟踪 +- 使用现有 `corridor_ctrl` + +5. `TURN_OUT_AT_END` +- 到达当前垄沟末端 +- 原地转 `90°` 转向连接段 + +6. `LINK_STRAIGHT` +- 沿端部连接段直行一小段 +- 用 IMU 保持航向 +- 用里程计或事件触发控制推进 + +7. `TURN_INTO_NEXT_CORRIDOR` +- 原地转 `90°` +- 对准下一条垄沟 + +8. `REACQUIRE_CORRIDOR` +- 低速确认两侧 VL53 是否重新形成合理走廊结构 +- 成功后切回 `CORRIDOR_TRACK` + +9. `EXIT_FIELD` +- 全部垄沟完成后，朝唯一出口离场 + +10. `DOCK_START_ZONE` +- 回到启动区并停车 + +11. `FINISHED` +- 比赛结束 + +## 8. 赛道级核心状态量 + +建议赛道级层显式维护以下变量： + +- `current_corridor_id` +- `target_corridor_id` +- `total_corridor_count = 6` +- `travel_direction` +- `turn_side` +- `stage` +- `stage_progress` +- `next_turn_is_left` +- `is_final_exit_phase` +- `reacquire_confirm_count` + +其中最关键的是： + +- 当前在第几条沟 +- 下一条是哪条沟 +- 这次入沟应该左转还是右转 +- 当前处于哪个动作阶段 + +## 9. 传感器参数与角色分工 + +### 9.1 左右 VL53L0X + +已知参数： + +- 每侧 2 个，共 4 个 +- 主要用于侧向测距 +- 精确测量距离按当前工程经验取 **1.2m 以内** +- 当前已由人工完成标定，但单点测距仍存在约 **±1cm** 的偏差 + +适合： + +- 沟内居中 +- 入沟重捕获确认 + +不适合： + +- 作为 `yaw / e_th` 的主观测来源 +- 远距离搜索下一条沟入口 +- 独立完成赛道级导航 + +设计含义： + +- `VL53` 是近场几何约束传感器 +- 只能在“已经接近某条沟”时帮你锁住这条沟 +- 不能把“下一条沟在哪里”这个问题压给它 +- 由于单点误差量级约为 `±1cm`，同侧前后差分法对噪声非常敏感 +- 因此不推荐继续用 `VL53` 前后差分直接计算 `yaw`，航向应主要依赖 `IMU` + +### 9.2 前后 STP-23L + +已知参数： + +- 前后各 1 个 +- 有效测距范围 **7cm ~ 7.5m** + +适合： + +- 到端检测 +- 前后安全边界监测 +- 开阔区边界辅助判定 +- 某些段落的事件触发 + +不适合： + +- 独立判断当前位于哪条垄沟 + +设计含义： + +- `STP` 是远距离边界感知传感器 +- 它适合回答“前面/后面还有多远”“是否接近端部或围栏” +- 不适合承担精细入沟定位 + +### 9.3 前后 ATK-MS53L1M + +已知参数： + +- 前后各 1 个 +- 有效测距范围 **4cm ~ 3.9m** + +适合： + +- 近距离补盲 +- 填补 STP 在近端盲区的不足 +- 近场防撞保护 + +设计含义： + +- `ATK` 不是主导航传感器 +- 它的核心价值是让前后边界感知在近距离不断层 +- 在转向、再入沟、靠近围栏时很重要 + +### 9.4 IMU + +适合： + +- 原地转 `90°` +- 连接段航向保持 +- 无侧墙阶段的短时姿态约束 + +### 9.5 编码器 / 里程计 + +适合： + +- 连接段推进量估计 +- 段落推进计量 +- 动作超时和距离上限保护 + +注意： + +- 地毯和打滑会影响绝对精度 +- 不能单独作为最终入沟确认依据 + +## 10. 各传感器在混合导航中的分工原则 + +建议按下面的分工使用传感器： + +1. 沟内阶段 +- 主用：左右 `VL53` 做横向约束，`IMU` 做航向约束 +- 辅助：前后激光仅做安全和到端检测 + +2. 转向阶段 +- 主用：`IMU yaw_continuous` +- 辅助：前后激光做安全保护 + +3. 连接段阶段 +- 主用：`IMU + 里程计` +- 辅助：前后 `STP/ATK` 做边界与防撞 + +4. 再入沟阶段 +- 主用：左右 `VL53` +- 辅助：`IMU` 做姿态稳定，前后激光做安全兜底 + +一句话总结： + +- `VL53` 负责“锁住局部走廊” +- `IMU` 负责“航向约束和跨过无墙约束阶段” +- `里程计` 负责“推进量” +- `STP/ATK` 负责“边界和安全” + +## 10.1 关于航向观测的专项说明 + +当前侧向 `VL53L0X` 虽然已经完成标定，但单点测距仍有约 `±1cm` 偏差。 + +这个精度对于： + +- 居中控制 +- 左右偏移判断 +- 重新捕获一条沟 + +通常是够用的。 + +但如果把它直接用于航向估计，例如用同侧前后距离差去推导 `yaw / e_th`，会遇到两个问题： + +1. 同侧前后差分属于“小量减小量”，对噪声天然敏感 +2. 当前 `±1cm` 的单点误差已经足以让差分航向观测明显抖动 + +因此推荐原则是： + +- `VL53` 负责横向约束和重捕获 +- `IMU wz + yaw_continuous` 负责航向估计与转向控制 +- 不再把 `VL53` 作为 `yaw` 主观测 + +## 11. 动作执行原则 + +### 11.1 沟内阶段 + +- 主要依赖侧向 VL53 做横向闭环，IMU 做航向闭环 +- 使用 `corridor_ctrl` +- 安全层负责限速和急停 + +### 11.2 转向阶段 + +- 主要依赖 IMU `yaw_continuous` +- 目标是稳定完成 `90°` +- 安全层不能再沿用普通“前方太近则整段全停”的逻辑 +- 必须允许 `v=0, w!=0` 的受限原地转向 + +### 11.3 连接段阶段 + +- 主要依赖 IMU 保持连接段朝向 +- 使用里程计推进 +- 接近预计入口后降速 +- 前后 `STP/ATK` 负责边界辅助与防撞 +- 进入重捕获阶段等待局部结构恢复 + +### 11.4 重捕获阶段 + +判据建议包括： + +- 左右两侧 VL53 同时有效 +- 左右几何关系符合 40cm 垄沟模型 +- `conf` 高于阈值 +- 持续若干拍成立 + +只有重捕获成功后，才允许切回沟内闭环。 + +## 12. 推荐新增模块 + +建议新增以下模块。 + +### 12.1 `App/nav/global_nav_fsm.c/.h` + +职责： + +- 维护整场比赛任务阶段 +- 管理 `corridor_id` +- 决定下一步目标段 +- 向下游发布当前动作类型 + +### 12.2 `App/nav/track_map.c/.h` + +职责： + +- 固化比赛地图拓扑 +- 保存各条垄沟、连接段、入口、出口的相对关系 +- 提供“当前完成哪条后下一条是谁”的规则查询 + +### 12.3 `App/nav/lane_transition.c/.h` + +职责： + +- 执行端部出沟、连接段推进、再入沟 +- 内部管理两个 `90°` 转向和一段连接直线 + +### 12.4 `App/nav/reacquire_detector.c/.h` + +职责： + +- 负责判断是否已重新进入目标垄沟 +- 对 VL53 几何结构和 `conf` 做持续判定 + +### 12.5 `App/nav/heading_hold.c/.h` + +职责： + +- 在无侧墙阶段提供短时航向保持 +- 可独立实现，也可并入 `lane_transition` + +### 12.6 `App/nav/exit_dock.c/.h` + +职责： + +- 负责最终离场与启动区停车 + +## 13. 推荐修改的现有模块 + +### 13.1 `segment_fsm` + +必须补： + +- 动作模式输入 +- 区分： + - 沟内前进 + - 原地转向 + - 连接段推进 + - 出场段直线 +- 否则正式比赛阶段会在端部动作上卡死 + +### 13.2 `nav_script` + +建议定位调整为： + +- 临时验证脚本 +- 单段测试脚本 +- 或过渡期动作编排器 + +不建议继续作为最终赛道总控。 + +### 13.3 `corridor_msgs` + +应补充： + +- 赛道级阶段枚举 +- 动作模式枚举 +- 重捕获结果结构 +- 赛道级状态输出结构 + +## 14. 推荐实施顺序 + +### 第 1 步：补底层动作语义 + +先修好： + +- 原地转向安全逻辑 +- 局部控制与安全层语义一致性 +- 局部测试模式与可观测性 + +目标： + +- 让“走沟、转向、连接直行”都能单独稳定测试 + +### 第 2 步：加入赛道级状态机 + +新增： + +- `global_nav_fsm` +- `track_map` + +目标： + +- 系统明确知道“当前第几沟、下一沟是谁、这次该左转还是右转” + +### 第 3 步：加入段间动作执行器 + +新增： + +- `lane_transition` +- `heading_hold` +- `reacquire_detector` + +目标： + +- 从一条沟末端稳定过渡到下一条沟入口并重新入沟 + +### 第 4 步：补最终出场与回停 + +新增： + +- `exit_dock` + +目标： + +- 让整场流程闭环，不只是在 6 条沟之间来回 + +### 第 5 步：统一参数、日志和调试接口 + +目标： + +- 可调 +- 可观测 +- 可复现 +- 可在实地快速定位问题 + +## 15. 一句话结论 + +本项目后续不应继续按“单沟脚本补丁”方式扩展。 + +正确方向应是： + +**用固定地图描述赛道，用赛道级状态机管理 S 型遍历，用动作执行器完成两次 90° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。**