1.0
This commit is contained in:
@@ -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
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
596
App/nav/global_nav.c
Normal file
596
App/nav/global_nav.c
Normal file
@@ -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 <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/* =========================================================
|
||||
* 内部状态
|
||||
* ========================================================= */
|
||||
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;
|
||||
}
|
||||
138
App/nav/global_nav.h
Normal file
138
App/nav/global_nav.h
Normal file
@@ -0,0 +1,138 @@
|
||||
/**
|
||||
* @file global_nav.h
|
||||
* @brief 赛道级总控 — 混合导航状态机
|
||||
*
|
||||
* 三层架构中的最上层：
|
||||
* 上层 (本模块): 知道"当前第几沟、下一沟是谁、该往哪转"
|
||||
* 中层 (内嵌): 转向/连接段/重捕获动作执行
|
||||
* 下层 (已有): 沟内 corridor_ctrl 闭环
|
||||
*
|
||||
* 替代原 nav_script.c 的赛道编排职能。
|
||||
*/
|
||||
|
||||
#ifndef GLOBAL_NAV_H
|
||||
#define GLOBAL_NAV_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#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 */
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
/**
|
||||
|
||||
78
App/nav/track_map.c
Normal file
78
App/nav/track_map.c
Normal file
@@ -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;
|
||||
}
|
||||
80
App/nav/track_map.h
Normal file
80
App/nav/track_map.h
Normal file
@@ -0,0 +1,80 @@
|
||||
/**
|
||||
* @file track_map.h
|
||||
* @brief 赛道地图 — 固化 S 型遍历拓扑
|
||||
*
|
||||
* 地图不做全局坐标定位，只回答三个问题：
|
||||
* 1. 从第 N 条沟完成后，下一条是第几条？
|
||||
* 2. 这次该往哪转？(左/右)
|
||||
* 3. 当前是不是最后一条沟？
|
||||
*/
|
||||
|
||||
#ifndef TRACK_MAP_H
|
||||
#define TRACK_MAP_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#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 */
|
||||
@@ -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
|
||||
@@ -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) */
|
||||
|
||||
/* =========================================================
|
||||
* 宏工具函数
|
||||
* ========================================================= */
|
||||
|
||||
@@ -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 (安全状态机与脚本冲突) 是实车测试中最可能暴露的问题，建议优先验证。
|
||||
@@ -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。
|
||||
@@ -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. 一句话总结
|
||||
|
||||
这类比赛不是“靠一套万能定位算法解决全部问题”，而是“让合适的层做合适的事”：
|
||||
|
||||
- 地图负责全局流程
|
||||
- 状态机负责阶段切换
|
||||
- 传感器负责局部闭环
|
||||
- 控制器负责把每一小段稳稳跑完
|
||||
|
||||
这就是混合导航的核心价值。
|
||||
@@ -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
|
||||
- 该需求与当前实现存在结构性差异
|
||||
- 从传感器误差特性看，这一需求是合理的
|
||||
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导
|
||||
1500
Doc/实施方案.md
Normal file
1500
Doc/实施方案.md
Normal file
File diff suppressed because it is too large
Load Diff
549
Doc/混合导航方案.md
Normal file
549
Doc/混合导航方案.md
Normal file
@@ -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° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。**
|
||||
Reference in New Issue
Block a user