导航部分效果最好的版本，主分支
This commit is contained in:
169
App/nav/corridor_ctrl.c
Normal file
169
App/nav/corridor_ctrl.c
Normal file
@@ -0,0 +1,169 @@
|
||||
#include "corridor_ctrl.h"
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* ====================== 内部状态 ====================== */
|
||||
static CorridorCtrlConfig_t s_cfg;
|
||||
static bool s_initialized = false;
|
||||
|
||||
/* 辅助：浮点数限幅 */
|
||||
static inline float clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
s_initialized = true;
|
||||
}
|
||||
|
||||
void CorridorCtrl_Compute(const CorridorState_t *state,
|
||||
const CorridorObs_t *obs,
|
||||
float imu_wz,
|
||||
RawCmd_t *out_cmd)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
out_cmd->v = 0.0f;
|
||||
out_cmd->w = 0.0f;
|
||||
out_cmd->flags = 0U;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 出沟保护: 当前激光检测到接近出口时，停止使用左右激光控制
|
||||
* 避免出沟时左右激光数据突变导致车身大幅度转向
|
||||
* ======================================================== */
|
||||
bool near_exit = false;
|
||||
if ((obs->valid_mask & (1U << 4)) != 0U) { /* 前激光有效 (bit 4) */
|
||||
if (obs->d_front <= s_cfg.exit_front_dist) {
|
||||
near_exit = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 核心控制律:
|
||||
* w_cmd = kp_theta * e_th + kd_theta * (-imu_wz) + kp_y * e_y
|
||||
*
|
||||
* - kp_theta * e_th : 比例项，车头偏了就回转
|
||||
* - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
|
||||
* 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
|
||||
* - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
|
||||
*
|
||||
* 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏
|
||||
* ======================================================== */
|
||||
|
||||
float w_cmd;
|
||||
bool escape_active = false;
|
||||
if (near_exit) {
|
||||
/* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
|
||||
w_cmd = -(s_cfg.kd_theta * imu_wz);
|
||||
} else {
|
||||
/*
|
||||
* 简化原则：
|
||||
* - IMU 只管航向 (e_th + wz)
|
||||
* - 左右激光只管居中
|
||||
*
|
||||
* 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
|
||||
* e_y_direct = (d_right - d_left)/2
|
||||
* 车在正中时 d_left ~= d_right，因此误差应接近 0。
|
||||
*
|
||||
* 若当前帧双侧不完整，再退回滤波器给出的 e_y。
|
||||
*/
|
||||
float e_y_ctrl = state->e_y;
|
||||
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 1)) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 3)) != 0U);
|
||||
|
||||
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;
|
||||
e_y_ctrl = 0.5f * (d_right - d_left);
|
||||
}
|
||||
|
||||
/* 正常控制: IMU 管航向, 左右激光管居中 */
|
||||
w_cmd = -(s_cfg.kp_theta * state->e_th
|
||||
+ s_cfg.kd_theta * imu_wz
|
||||
+ s_cfg.kp_y * e_y_ctrl);
|
||||
|
||||
/* ========================================================
|
||||
* 近墙脱离保护:
|
||||
* 当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。
|
||||
* 此时不能只等 EKF 慢慢回中，直接叠加一个远离墙面的转向保护项。
|
||||
* ======================================================== */
|
||||
{
|
||||
bool left_front_ok = ((obs->valid_mask & (1U << 0)) != 0U);
|
||||
bool left_rear_ok = ((obs->valid_mask & (1U << 1)) != 0U);
|
||||
bool right_front_ok = ((obs->valid_mask & (1U << 2)) != 0U);
|
||||
bool right_rear_ok = ((obs->valid_mask & (1U << 3)) != 0U);
|
||||
float w_escape = 0.0f;
|
||||
float left_min = 10.0f;
|
||||
float right_min = 10.0f;
|
||||
|
||||
if (left_front_ok && obs->d_lf < left_min) left_min = obs->d_lf;
|
||||
if (left_rear_ok && obs->d_lr < left_min) left_min = obs->d_lr;
|
||||
if (right_front_ok && obs->d_rf < right_min) right_min = obs->d_rf;
|
||||
if (right_rear_ok && obs->d_rr < right_min) right_min = obs->d_rr;
|
||||
|
||||
if (left_min < s_cfg.wall_escape_dist) {
|
||||
float err = s_cfg.wall_escape_dist - left_min;
|
||||
w_escape -= s_cfg.wall_escape_kp * err; /* 左侧很近 -> 轻微右转 */
|
||||
escape_active = true;
|
||||
}
|
||||
|
||||
if (right_min < s_cfg.wall_escape_dist) {
|
||||
float err = s_cfg.wall_escape_dist - right_min;
|
||||
w_escape += s_cfg.wall_escape_kp * err; /* 右侧很近 -> 轻微左转 */
|
||||
escape_active = true;
|
||||
}
|
||||
|
||||
w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max);
|
||||
w_cmd += w_escape;
|
||||
}
|
||||
}
|
||||
|
||||
/* 角速度限幅：防止 PD 溢出导致原地打转 */
|
||||
w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
|
||||
|
||||
/* ========================================================
|
||||
* 线速度策略:
|
||||
* 基础巡航速度 v_cruise，
|
||||
* 当角速度偏大时适当降速（弯道减速），保持运动学协调
|
||||
* 公式: v = v_cruise * (1 - k * |w/w_max|)
|
||||
* k 取 0.3~0.5 较保守
|
||||
* ======================================================== */
|
||||
float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
|
||||
float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
|
||||
|
||||
/* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */
|
||||
if (escape_active) {
|
||||
v_cmd *= 0.80f;
|
||||
}
|
||||
|
||||
/* 线速度限幅：不允许倒车，不允许超速 */
|
||||
v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
|
||||
|
||||
/* ========================================================
|
||||
* 置信度降级保护:
|
||||
* 当滤波器健康度 conf 过低（两边雷达全瞎），
|
||||
* 说明走廊参照完全丢失，降低线速度防止盲飞
|
||||
*
|
||||
* 注意：阈值不宜过高，否则会过度降级导致控制器失效
|
||||
* ======================================================== */
|
||||
if (state->conf < 0.2f) {
|
||||
/* 健康度极低：速度打三折，保持航向惯性滑行 */
|
||||
v_cmd *= 0.3f;
|
||||
} else if (state->conf < 0.4f) {
|
||||
/* 健康度较低（单侧退化）：速度打七折 */
|
||||
v_cmd *= 0.7f;
|
||||
}
|
||||
|
||||
/* 输出赋值 */
|
||||
out_cmd->t_ms = state->t_ms;
|
||||
out_cmd->v = v_cmd;
|
||||
out_cmd->w = w_cmd;
|
||||
out_cmd->flags = 0U;
|
||||
}
|
||||
52
App/nav/corridor_ctrl.h
Normal file
52
App/nav/corridor_ctrl.h
Normal file
@@ -0,0 +1,52 @@
|
||||
#ifndef CORRIDOR_CTRL_H
|
||||
#define CORRIDOR_CTRL_H
|
||||
|
||||
#include "preproc/corridor_msgs.h"
|
||||
|
||||
/**
|
||||
* @brief 走廊控制器调参配置
|
||||
* @note 所有增益参数都应在调试阶段"从小往大调"，防止震荡
|
||||
*/
|
||||
typedef struct {
|
||||
float kp_theta; // 航向偏差比例增益 (rad/s per rad)
|
||||
float kd_theta; // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
|
||||
float kp_y; // 横向偏差比例增益 (rad/s per m)
|
||||
|
||||
float v_cruise; // 走廊内巡航线速度 (m/s)
|
||||
float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
|
||||
float v_max; // 线速度输出硬限幅 (m/s)
|
||||
float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
|
||||
|
||||
float exit_front_dist; // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制
|
||||
float wall_escape_dist; // 近墙脱离阈值 (m)，小于此值触发直接远离墙面
|
||||
float wall_escape_kp; // 近墙脱离增益 (rad/s per m)
|
||||
float wall_escape_w_max; // 近墙脱离角速度限幅 (rad/s)
|
||||
} CorridorCtrlConfig_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 初始化走廊控制器
|
||||
* @param config 控制器增益与限幅配置
|
||||
*/
|
||||
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 核心函数：根据滤波器输出的走廊状态计算控制量
|
||||
* @param state 滤波器输出的走廊状态 (e_y, e_th, conf)
|
||||
* @param obs 预处理层的观测快照 (用于获取 d_front 等辅助信息)
|
||||
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
|
||||
* @param out_cmd 输出的控制指令 (v, w)
|
||||
*/
|
||||
void CorridorCtrl_Compute(const CorridorState_t *state,
|
||||
const CorridorObs_t *obs,
|
||||
float imu_wz,
|
||||
RawCmd_t *out_cmd);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // CORRIDOR_CTRL_H
|
||||
1051
App/nav/global_nav.c
Normal file
1051
App/nav/global_nav.c
Normal file
File diff suppressed because it is too large
Load Diff
156
App/nav/global_nav.h
Normal file
156
App/nav/global_nav.h
Normal file
@@ -0,0 +1,156 @@
|
||||
/**
|
||||
* @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_ALIGN, /* 捕获成功后停车对齐航线 */
|
||||
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 */
|
||||
float reacquire_min_odom; /* 重捕获最小入沟里程 m */
|
||||
uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
|
||||
uint32_t reacquire_timeout_ms;
|
||||
|
||||
/* 对齐 */
|
||||
float align_kp_th; /* 对齐航向P增益 (rad/s per rad) */
|
||||
float align_kp_y; /* 对齐横向P增益 (rad/s per m) */
|
||||
float align_th_tol_rad; /* 对齐航向容差 (rad) */
|
||||
float align_y_tol_m; /* 对齐横向容差 (m) */
|
||||
uint8_t align_confirm_ticks; /* 对齐确认拍数 */
|
||||
uint32_t align_timeout_ms; /* 对齐超时 ms */
|
||||
float reacquire_min_back_dist; /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */
|
||||
|
||||
/* 沟内 */
|
||||
float corridor_end_detect_dist; /* 到端检测距离 m */
|
||||
float corridor_length_max; /* 沟内里程保护上限 m */
|
||||
|
||||
/* 连接段 */
|
||||
float link_v;
|
||||
float link_distance;
|
||||
uint32_t link_timeout_ms;
|
||||
float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */
|
||||
float link_wall_target; /* 墙壁跟随目标距离 (m) */
|
||||
float link_wall_kp; /* 墙壁跟随P增益 */
|
||||
float link_wall_heading_kp; /* 单边墙平行修正增益 */
|
||||
float link_wall_blend; /* 墙壁跟随权重 (0~1) */
|
||||
|
||||
/* 出场 */
|
||||
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 now_ms 当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
|
||||
* @param out 导航输出
|
||||
*/
|
||||
void GlobalNav_Update(const CorridorObs_t* obs,
|
||||
const CorridorState_t* state,
|
||||
const RobotBlackboard_t* board,
|
||||
uint32_t now_ms,
|
||||
GlobalNavOutput_t* out);
|
||||
|
||||
GlobalNavStage_t GlobalNav_GetStage(void);
|
||||
const char* GlobalNav_GetStageName(GlobalNavStage_t stage);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* GLOBAL_NAV_H */
|
||||
424
App/nav/nav_script.c
Normal file
424
App/nav/nav_script.c
Normal file
@@ -0,0 +1,424 @@
|
||||
/**
|
||||
* @file nav_script.c
|
||||
* @brief 段脚本执行器实现：编排完整比赛流程
|
||||
*/
|
||||
|
||||
#include "nav_script.h"
|
||||
#include "est/corridor_filter.h"
|
||||
#include "robot_params.h"
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/* =========================================================
|
||||
* 内部状态
|
||||
* ========================================================= */
|
||||
static NavScriptConfig_t s_cfg;
|
||||
static NavScriptStage_t s_stage = SCRIPT_STAGE_IDLE;
|
||||
static bool s_initialized = false;
|
||||
static bool s_running = false;
|
||||
|
||||
/* 阶段内部状态变量 */
|
||||
static struct {
|
||||
uint32_t entry_align_start_ms; // 入口对准开始时间
|
||||
float turn_start_e_th; // 转向开始时的 EKF 航向 (保留作后备)
|
||||
float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg)
|
||||
bool turn_started; // 转向是否已开始
|
||||
bool turn_coarse_done; // 180° IMU 粗转是否已完成
|
||||
uint32_t turn_settle_start_ms; // 进入侧墙精调阶段的时刻
|
||||
float corridor_s_entry; // 进入垄沟时的 s 里程
|
||||
float end_rearm_s; // 掉头后到端检测重新使能的起始里程
|
||||
bool end_armed; // 到端检测是否已重新使能
|
||||
NavScriptStage_t post_turn_stage; // 本次转向完成后要进入的走廊阶段
|
||||
int pass_count; // 已走过的垄沟数
|
||||
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
|
||||
} s_internal;
|
||||
|
||||
#define SCRIPT_END_REARM_DIST_M 0.12f
|
||||
#define SCRIPT_TURN_SETTLE_KP 1.4f
|
||||
#define SCRIPT_TURN_SETTLE_W_MAX 0.6f
|
||||
#define SCRIPT_TURN_SETTLE_TOL_RAD 0.05f
|
||||
#define SCRIPT_TURN_SETTLE_TIMEOUT_MS 1200u
|
||||
|
||||
/* =========================================================
|
||||
* 内部辅助函数
|
||||
* ========================================================= */
|
||||
static inline float clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
static bool compute_wall_heading_error(const CorridorObs_t *obs, float *out_heading_rad)
|
||||
{
|
||||
float heading_sum = 0.0f;
|
||||
int valid_count = 0;
|
||||
const float sensor_base = PARAM_SENSOR_BASE_LENGTH;
|
||||
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (left_ok) {
|
||||
heading_sum += atan2f(obs->d_lr - obs->d_lf, sensor_base);
|
||||
valid_count++;
|
||||
}
|
||||
|
||||
if (right_ok) {
|
||||
heading_sum += atan2f(obs->d_rf - obs->d_rr, sensor_base);
|
||||
valid_count++;
|
||||
}
|
||||
|
||||
if (valid_count == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*out_heading_rad = heading_sum / (float)valid_count;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void NavScript_Init(const NavScriptConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
s_stage = SCRIPT_STAGE_IDLE;
|
||||
s_running = false;
|
||||
s_initialized = true;
|
||||
|
||||
/* 初始化内部状态 */
|
||||
memset(&s_internal, 0, sizeof(s_internal));
|
||||
}
|
||||
|
||||
void NavScript_Start(void)
|
||||
{
|
||||
if (s_initialized) {
|
||||
s_stage = SCRIPT_STAGE_ENTRY_ALIGN;
|
||||
s_running = true;
|
||||
s_internal.entry_align_start_ms = 0;
|
||||
s_internal.pass_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
NavScriptStage_t NavScript_GetStage(void)
|
||||
{
|
||||
return s_stage;
|
||||
}
|
||||
|
||||
const char* NavScript_GetStageName(NavScriptStage_t stage)
|
||||
{
|
||||
switch (stage) {
|
||||
case SCRIPT_STAGE_IDLE: return "IDLE";
|
||||
case SCRIPT_STAGE_ENTRY_ALIGN: return "ENTRY_ALIGN";
|
||||
case SCRIPT_STAGE_CORRIDOR_FORWARD: return "CORRIDOR_FWD";
|
||||
case SCRIPT_STAGE_TURN_AT_END: return "TURN_AT_END";
|
||||
case SCRIPT_STAGE_CORRIDOR_BACKWARD: return "CORRIDOR_BWD";
|
||||
case SCRIPT_STAGE_EXIT: return "EXIT";
|
||||
case SCRIPT_STAGE_FINISHED: return "FINISHED";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
void NavScript_Reset(void)
|
||||
{
|
||||
s_stage = SCRIPT_STAGE_IDLE;
|
||||
s_running = false;
|
||||
memset(&s_internal, 0, sizeof(s_internal));
|
||||
}
|
||||
|
||||
void NavScript_Update(const CorridorObs_t *obs,
|
||||
const CorridorState_t *state,
|
||||
float imu_yaw_continuous_deg,
|
||||
NavScriptOutput_t *out)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
out->stage = SCRIPT_STAGE_IDLE;
|
||||
out->stage_name = "IDLE";
|
||||
out->active = false;
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
return;
|
||||
}
|
||||
|
||||
/* 默认输出 */
|
||||
out->stage = s_stage;
|
||||
out->stage_name = NavScript_GetStageName(s_stage);
|
||||
out->active = s_running;
|
||||
out->request_corridor = true; // 默认请求走廊控制器
|
||||
out->use_override = false; // 默认不覆盖
|
||||
|
||||
if (!s_running) {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 核心状态机：按比赛流程执行
|
||||
* ======================================================== */
|
||||
switch (s_stage) {
|
||||
/* ==============================================
|
||||
* STAGE 1: 入口对准
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_ENTRY_ALIGN: {
|
||||
if (s_internal.entry_align_start_ms == 0) {
|
||||
s_internal.entry_align_start_ms = state->t_ms;
|
||||
}
|
||||
|
||||
/* 检查是否已进入垄沟：两侧都有距离数据 */
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (left_ok && right_ok && state->conf >= 0.8f) {
|
||||
/* 两侧雷达都有数据，且置信度高 -> 进入垄沟，开始跟踪 */
|
||||
s_internal.corridor_s_entry = state->s;
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = true;
|
||||
s_internal.pass_count = 1;
|
||||
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
out->request_corridor = true;
|
||||
} else {
|
||||
/* 入口对准策略：慢速前进，让侧向雷达找墙 */
|
||||
out->override_v = s_cfg.entry_align_v; // 慢速前进 (由 PARAM_SCRIPT_ENTRY_V 配置)
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
out->request_corridor = false;
|
||||
|
||||
/* P1 修复: 超时保护使用配置参数 s_cfg.entry_align_timeout，
|
||||
* 而非硬编码 30000 ms */
|
||||
uint32_t elapsed = state->t_ms - s_internal.entry_align_start_ms;
|
||||
if (elapsed > (uint32_t)s_cfg.entry_align_timeout) {
|
||||
/* 超时强制进入走廊跟踪：必须补齐与正常入沟一致的内部状态，
|
||||
* 否则 pass_count 停留在 0，导致后续 TURN_AT_END 判定时
|
||||
* 多跑一趟走廊（三趟而非文档描述的两趟）。 */
|
||||
s_internal.corridor_s_entry = state->s;
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = true;
|
||||
s_internal.pass_count = 1;
|
||||
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 2: 向前走垄沟
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
|
||||
/* 使用走廊控制器 */
|
||||
out->request_corridor = true;
|
||||
|
||||
if (!s_internal.end_armed) {
|
||||
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
|
||||
s_internal.end_armed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* 检查是否到端 */
|
||||
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||||
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
|
||||
/* 前向距离足够近 -> 到达垄沟末端，递增趟数并准备转向 */
|
||||
s_internal.pass_count++;
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = false;
|
||||
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
|
||||
s_stage = SCRIPT_STAGE_TURN_AT_END;
|
||||
out->request_corridor = false;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 3: 到端原地转向
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_TURN_AT_END: {
|
||||
out->request_corridor = false;
|
||||
|
||||
if (!s_internal.turn_started) {
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = true;
|
||||
s_internal.turn_coarse_done = false;
|
||||
s_internal.turn_settle_start_ms = 0U;
|
||||
}
|
||||
|
||||
/* 原地 180 度转向策略 */
|
||||
float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度)
|
||||
|
||||
/* ---- 使用 IMU 连续 yaw 判定转角 (deg → rad) ---- *
|
||||
* 优点：IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定，
|
||||
* 且 unwrap 后不存在 ±180° 跳变问题。
|
||||
* 后备：如果 IMU 离线，退化回 EKF e_th 差值判定。 */
|
||||
float delta_turned;
|
||||
float imu_delta_deg = imu_yaw_continuous_deg - s_internal.turn_start_imu_yaw_deg;
|
||||
delta_turned = imu_delta_deg * 0.01745329252f; // deg → rad
|
||||
|
||||
float remaining = target_delta - fabsf(delta_turned);
|
||||
|
||||
if (!s_internal.turn_coarse_done && remaining <= 0.1f) {
|
||||
s_internal.turn_coarse_done = true;
|
||||
s_internal.turn_settle_start_ms = obs->t_ms;
|
||||
}
|
||||
|
||||
if (s_internal.turn_coarse_done) {
|
||||
float wall_heading = 0.0f;
|
||||
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading);
|
||||
bool settle_timeout = (obs->t_ms - s_internal.turn_settle_start_ms) >= SCRIPT_TURN_SETTLE_TIMEOUT_MS;
|
||||
|
||||
if (wall_heading_ok) {
|
||||
if (fabsf(wall_heading) <= SCRIPT_TURN_SETTLE_TOL_RAD) {
|
||||
CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f);
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = false;
|
||||
s_stage = s_internal.post_turn_stage;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
} else {
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = clampf(-SCRIPT_TURN_SETTLE_KP * wall_heading,
|
||||
-SCRIPT_TURN_SETTLE_W_MAX,
|
||||
SCRIPT_TURN_SETTLE_W_MAX);
|
||||
out->use_override = true;
|
||||
}
|
||||
} else if (settle_timeout) {
|
||||
/* 极端情况下侧墙暂时不可用，超时后接受 IMU 粗转结果，避免卡死。 */
|
||||
CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f);
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = false;
|
||||
s_stage = s_internal.post_turn_stage;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
} else {
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
}
|
||||
} else {
|
||||
/* 继续转向：根据目标方向选择角速度 */
|
||||
float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f;
|
||||
|
||||
/* 接近目标时减速 */
|
||||
float turn_speed = s_cfg.turn_omega;
|
||||
if (remaining < 0.5f) {
|
||||
turn_speed *= (remaining / 0.5f); // 线性减速
|
||||
turn_speed = clampf(turn_speed, 0.3f, s_cfg.turn_omega);
|
||||
}
|
||||
|
||||
out->override_v = 0.0f; // 线速度为 0，原地转
|
||||
out->override_w = turn_dir * turn_speed;
|
||||
out->use_override = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 4: 返程走垄沟 (原地转 180° 后继续正向行驶)
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_CORRIDOR_BACKWARD: {
|
||||
/* 使用走廊控制器，正向行驶 */
|
||||
out->request_corridor = true;
|
||||
|
||||
/* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"，
|
||||
* 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */
|
||||
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||||
|
||||
/* 掉头回来时，前向雷达可能还残留近端读数。
|
||||
* 必须先离开端墙一小段距离，再允许重新触发到端检测。 */
|
||||
if (!s_internal.end_armed) {
|
||||
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
|
||||
s_internal.end_armed = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
|
||||
/* [改进H] 到达另一端: 递增趟数并判断是否退出
|
||||
* pass_count=1(入沟首趟), =2(返回), =3(第三趟) → EXIT */
|
||||
s_internal.pass_count++;
|
||||
if (s_internal.pass_count >= 3) {
|
||||
/* 走了 3 趟 (去-回-去)，进入退出阶段 */
|
||||
s_stage = SCRIPT_STAGE_EXIT;
|
||||
s_internal.exit_start_s = 0.0f;
|
||||
} else {
|
||||
/* 继续往返: 转向后进入 FORWARD */
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = false;
|
||||
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
s_stage = SCRIPT_STAGE_TURN_AT_END;
|
||||
}
|
||||
out->request_corridor = false;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 5: 退出场地
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_EXIT: {
|
||||
/* P1 修复: 退出速度使用独立的 exit_v 参数，
|
||||
* 而非误用角速度参数 turn_omega * 0.5f */
|
||||
out->request_corridor = false;
|
||||
out->override_v = s_cfg.exit_v; // 直线冲出速度 (由 PARAM_SCRIPT_EXIT_V 配置)
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
|
||||
/* 检测是否已离开：两侧雷达都丢失 */
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (!left_ok && !right_ok) {
|
||||
/* 两侧都丢了 -> 已离开垄沟，再往前冲 exit_runout_m 后停车 */
|
||||
if (s_internal.exit_start_s == 0.0f) {
|
||||
s_internal.exit_start_s = state->s;
|
||||
}
|
||||
|
||||
if (state->s - s_internal.exit_start_s >= s_cfg.exit_runout_m) {
|
||||
/* 冲够了，停车 */
|
||||
s_stage = SCRIPT_STAGE_FINISHED;
|
||||
s_internal.exit_start_s = 0.0f;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 6: 比赛完成，安全停车
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_FINISHED: {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
s_running = false;
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* DEFAULT/IDLE: 安全停车
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_IDLE:
|
||||
default: {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
133
App/nav/nav_script.h
Normal file
133
App/nav/nav_script.h
Normal file
@@ -0,0 +1,133 @@
|
||||
/**
|
||||
* @file nav_script.h
|
||||
* @brief 段脚本执行器：编排完整比赛流程
|
||||
*
|
||||
* 比赛流程模型：
|
||||
* ┌─────────────┐
|
||||
* │ ENTRY_ALIGN │ 入口对准：启动区 -> 第一垄沟入口
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ CORRIDOR_A │ 第一垄沟跟踪
|
||||
* └──────┬──────┘
|
||||
* │ (到端)
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ TURN_180 │ 原地 180 度转向
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ CORRIDOR_B │ 返回垄沟跟踪
|
||||
* └──────┬──────┘
|
||||
* │ (到端)
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ EXIT │ 退出比赛场地
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ FINISH │ 停在启动区
|
||||
* └─────────────┘
|
||||
*/
|
||||
|
||||
#ifndef NAV_SCRIPT_H
|
||||
#define NAV_SCRIPT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "preproc/corridor_msgs.h"
|
||||
#include "preproc/corridor_preproc.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本阶段枚举
|
||||
* ========================================================= */
|
||||
typedef enum {
|
||||
SCRIPT_STAGE_IDLE = 0, // 待命阶段，不输出任何指令
|
||||
SCRIPT_STAGE_ENTRY_ALIGN, // 入口对准：从启动区到第一垄沟
|
||||
SCRIPT_STAGE_CORRIDOR_FORWARD, // 向前走垄沟
|
||||
SCRIPT_STAGE_TURN_AT_END, // 到端原地转向
|
||||
SCRIPT_STAGE_CORRIDOR_BACKWARD,// 向后走垄沟
|
||||
SCRIPT_STAGE_EXIT, // 退出场地
|
||||
SCRIPT_STAGE_FINISHED, // 比赛完成，安全停车
|
||||
} NavScriptStage_t;
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本配置参数
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
float turn_target_angle; // 原地转向目标角度 (rad)，默认 PI (180度)
|
||||
float turn_omega; // 原地转向角速度 (rad/s)
|
||||
float corridor_length; // 垄沟长度估计 (m)，用于触发到端（备用）
|
||||
float entry_align_timeout; // 入口对准超时 (ms)
|
||||
float d_entry_exit_front; // 出入口前向距离阈值 (m)
|
||||
float entry_align_v; // 入口对准慢速前进速度 (m/s)
|
||||
float exit_runout_m; // 离开走廊后继续冲出的距离 (m)
|
||||
float exit_v; // 退出场地直线冲出速度 (m/s)
|
||||
} NavScriptConfig_t;
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本输出
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
NavScriptStage_t stage; // 当前执行阶段
|
||||
const char *stage_name; // 阶段名称字符串
|
||||
bool active; // 脚本是否正在运行
|
||||
bool request_corridor; // 是否请求走廊控制器
|
||||
float override_v; // 覆盖输出的 v (m/s)
|
||||
float override_w; // 覆盖输出的 w (rad/s)
|
||||
bool use_override; // 是否使用覆盖值
|
||||
} NavScriptOutput_t;
|
||||
|
||||
/* =========================================================
|
||||
* API 接口
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 初始化段脚本执行器
|
||||
*/
|
||||
void NavScript_Init(const NavScriptConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 开始运行比赛脚本
|
||||
*/
|
||||
void NavScript_Start(void);
|
||||
|
||||
/**
|
||||
* @brief 核心执行函数：每个导航周期调用一次
|
||||
* @param obs 预处理层的观测快照
|
||||
* @param state 滤波器的走廊状态
|
||||
* @param imu_yaw_continuous_deg IMU unwrap 后的连续偏航角 (度)，用于转弯判定
|
||||
* @param out 段脚本输出
|
||||
*/
|
||||
void NavScript_Update(const CorridorObs_t *obs,
|
||||
const CorridorState_t *state,
|
||||
float imu_yaw_continuous_deg,
|
||||
NavScriptOutput_t *out);
|
||||
|
||||
/**
|
||||
* @brief 获取当前执行阶段
|
||||
*/
|
||||
NavScriptStage_t NavScript_GetStage(void);
|
||||
|
||||
/**
|
||||
* @brief 获取阶段名称
|
||||
*/
|
||||
const char* NavScript_GetStageName(NavScriptStage_t stage);
|
||||
|
||||
/**
|
||||
* @brief 强制重置脚本到 IDLE
|
||||
*/
|
||||
void NavScript_Reset(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV_SCRIPT_H
|
||||
165
App/nav/segment_fsm.c
Normal file
165
App/nav/segment_fsm.c
Normal file
@@ -0,0 +1,165 @@
|
||||
#include "segment_fsm.h"
|
||||
#include <string.h>
|
||||
|
||||
/* ====================== 内部状态 ====================== */
|
||||
static SegFsmConfig_t s_cfg;
|
||||
static SegFsmState_t s_state = SEG_STATE_IDLE;
|
||||
static bool s_initialized = false;
|
||||
|
||||
/* 辅助：浮点数限幅 */
|
||||
static inline float seg_clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
void SegFsm_Init(const SegFsmConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
s_state = SEG_STATE_IDLE;
|
||||
s_initialized = true;
|
||||
}
|
||||
|
||||
void SegFsm_Start(void)
|
||||
{
|
||||
if (s_initialized) {
|
||||
s_state = SEG_STATE_CORRIDOR;
|
||||
}
|
||||
}
|
||||
|
||||
SegFsmState_t SegFsm_GetState(void)
|
||||
{
|
||||
return s_state;
|
||||
}
|
||||
|
||||
const char* SegFsm_GetStateName(SegFsmState_t s)
|
||||
{
|
||||
switch (s) {
|
||||
case SEG_STATE_IDLE: return "IDLE";
|
||||
case SEG_STATE_CORRIDOR: return "CORRIDOR";
|
||||
case SEG_STATE_APPROACH: return "APPROACH";
|
||||
case SEG_STATE_STOP: return "STOP";
|
||||
case SEG_STATE_ESTOP: return "E-STOP";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
out->state = SEG_STATE_IDLE;
|
||||
out->safe_v = 0.0f;
|
||||
out->safe_w = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 0. IDLE 模式：零速，不做任何裁剪
|
||||
* ======================================================== */
|
||||
if (mode == SAFETY_MODE_IDLE) {
|
||||
out->state = SEG_STATE_IDLE;
|
||||
out->safe_v = 0.0f;
|
||||
out->safe_w = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 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;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 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: {
|
||||
float range = s_cfg.d_front_approach - s_cfg.d_front_stop;
|
||||
float ratio = 1.0f;
|
||||
if (range > 0.001f) {
|
||||
ratio = (d_front - s_cfg.d_front_stop) / range;
|
||||
ratio = seg_clampf(ratio, 0.0f, 1.0f);
|
||||
}
|
||||
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;
|
||||
break;
|
||||
}
|
||||
|
||||
case SEG_STATE_STOP:
|
||||
default: {
|
||||
out->safe_v = 0.0f;
|
||||
out->safe_w = 0.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
out->state = s_state;
|
||||
}
|
||||
80
App/nav/segment_fsm.h
Normal file
80
App/nav/segment_fsm.h
Normal file
@@ -0,0 +1,80 @@
|
||||
#ifndef SEGMENT_FSM_H
|
||||
#define SEGMENT_FSM_H
|
||||
|
||||
#include "preproc/corridor_msgs.h"
|
||||
#include "preproc/corridor_preproc.h"
|
||||
|
||||
/**
|
||||
* @brief 段状态机的运行阶段枚举
|
||||
*/
|
||||
typedef enum {
|
||||
SEG_STATE_IDLE = 0, // 待命：不输出任何指令
|
||||
SEG_STATE_CORRIDOR, // 走廊跟踪中：放行控制器输出
|
||||
SEG_STATE_APPROACH, // 接近端墙：降速保护
|
||||
SEG_STATE_STOP, // 到端停车：强制零速
|
||||
SEG_STATE_ESTOP, // 紧急停车：传感器全部失效或异常
|
||||
} SegFsmState_t;
|
||||
|
||||
/**
|
||||
* @brief 段状态机配置参数
|
||||
*/
|
||||
typedef struct {
|
||||
float d_front_stop; // 前向停车距离阈值 (m)，低于此值直接停车
|
||||
float d_front_approach; // 前向减速预警距离 (m)，低于此值开始线性减速
|
||||
float approach_min_v; // 减速区内最低速度 (m/s)，防止爬行太慢永远到不了
|
||||
float conf_estop_thresh; // 置信度紧急停车阈值，低于此值触发 E-Stop
|
||||
} SegFsmConfig_t;
|
||||
|
||||
/**
|
||||
* @brief 段状态机对外暴露的完整快照 (供日志或上位机读取)
|
||||
*/
|
||||
typedef struct {
|
||||
SegFsmState_t state; // 当前状态
|
||||
float safe_v; // 经安全仲裁后的最终线速度 (m/s)
|
||||
float safe_w; // 经安全仲裁后的最终角速度 (rad/s)
|
||||
} SegFsmOutput_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 初始化段状态机
|
||||
* @param config 距离阈值与降级参数
|
||||
*/
|
||||
void SegFsm_Init(const SegFsmConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 启动走廊跟踪（从 IDLE 切换到 CORRIDOR）
|
||||
*/
|
||||
void SegFsm_Start(void);
|
||||
|
||||
/**
|
||||
* @brief 核心函数：输入控制器的期望指令，输出安全仲裁后的最终指令
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* @brief 获取当前状态机状态 (用于日志打印)
|
||||
*/
|
||||
SegFsmState_t SegFsm_GetState(void);
|
||||
|
||||
/**
|
||||
* @brief 获取状态名称字符串 (用于日志打印)
|
||||
*/
|
||||
const char* SegFsm_GetStateName(SegFsmState_t s);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // SEGMENT_FSM_H
|
||||
96
App/nav/track_map.c
Normal file
96
App/nav/track_map.c
Normal file
@@ -0,0 +1,96 @@
|
||||
/**
|
||||
* @file track_map.c
|
||||
* @brief 赛道地图 — S 型遍历拓扑表
|
||||
*
|
||||
* 正确的场地结构 (参见 Doc/map.md):
|
||||
* - 垄沟沿 X 轴（横向）分布，长 220cm
|
||||
* - 左右两端各有纵向端部通道，宽 40cm
|
||||
* - 启动区在左下角，入口对齐左端通道
|
||||
* - C1 最靠近入口(南侧)，C6 最远(北侧)
|
||||
*
|
||||
* S 型路径 (以"北=Y递减=地图上方"为参考):
|
||||
* 入场→左端通道→右转入C1(→东)→右端到端→左转(朝北)
|
||||
* →北行70cm→左转入C2(←西)→左端到端→右转(朝北)
|
||||
* →北行70cm→右转入C3(→东)→...
|
||||
* →C6(←西)→左端到端→左转(朝南)→南行出场
|
||||
*
|
||||
* entry_turn_dir: 从纵向通道(朝北)转入垄沟的方向
|
||||
* exit_turn_dir: 从垄沟到端后转入纵向通道的方向
|
||||
*/
|
||||
|
||||
#include "track_map.h"
|
||||
|
||||
/* =========================================================
|
||||
* 硬编码 S 型遍历表
|
||||
*
|
||||
* 转向方向推导 (北=机器人沿纵向通道前进方向):
|
||||
* C1: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
|
||||
* C2: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
|
||||
* C3: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
|
||||
* C4: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
|
||||
* C5: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
|
||||
* C6: 右端通道朝北→左转→朝西入沟; 到左端→左转→朝南(出场)
|
||||
* ========================================================= */
|
||||
static const TrackMap_t s_map = {
|
||||
.corridors = {
|
||||
/* id travel_dir exit_turn entry_turn is_last
|
||||
* -- ---------- --------- ---------- ------- */
|
||||
{ 0, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C1: →东, 从左端右转入, 到右端左转出 */
|
||||
{ 1, TRAVEL_DIR_WEST, TURN_DIR_RIGHT, TURN_DIR_LEFT, false }, /* C2: ←西, 从右端左转入, 到左端右转出 */
|
||||
{ 2, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C3: →东 */
|
||||
{ 3, TRAVEL_DIR_WEST, TURN_DIR_RIGHT, TURN_DIR_LEFT, false }, /* C4: ←西 */
|
||||
{ 4, TRAVEL_DIR_EAST, TURN_DIR_LEFT, TURN_DIR_RIGHT, false }, /* C5: →东 */
|
||||
{ 5, TRAVEL_DIR_WEST, TURN_DIR_LEFT, TURN_DIR_LEFT, true }, /* C6: ←西, 从右端左转入, 到左端左转出(朝南出场) */
|
||||
},
|
||||
.entry_corridor_id = 0,
|
||||
.link_distance_m = TRACK_MAP_LINK_DISTANCE_M,
|
||||
.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;
|
||||
}
|
||||
91
App/nav/track_map.h
Normal file
91
App/nav/track_map.h
Normal file
@@ -0,0 +1,91 @@
|
||||
/**
|
||||
* @file track_map.h
|
||||
* @brief 赛道地图 — 固化 S 型遍历拓扑
|
||||
*
|
||||
* 赛道几何 (参见 Doc/map.md):
|
||||
* - 场地 300cm(X) × 390cm(Y)，5 条横向垄背把场地切成 6 条横向垄沟
|
||||
* - 垄沟沿 X 轴方向，长 220cm，宽 40cm
|
||||
* - 左右两端各有一条纵向端部通道 (宽 40cm，长 390cm)
|
||||
* - 启动区在左下角，入口对齐左端通道
|
||||
* - 垄沟1(最靠近入口) 到 垄沟6(最远离入口) 自下而上排列
|
||||
*
|
||||
* S 型遍历:
|
||||
* 入场→左端通道→右转入C1(→)→右端到端→左转→北行→左转入C2(←)
|
||||
* →左端到端→右转→北行→右转入C3(→)→...→C6(←)→左端→左转→南行出场
|
||||
*
|
||||
* 地图不做全局坐标定位，只回答三个问题：
|
||||
* 1. 从第 N 条沟完成后，下一条是第几条？
|
||||
* 2. 这次该往哪转？(左/右)
|
||||
* 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_EAST = 0, /* 从左端到右端 (→, +X) 奇数沟 */
|
||||
TRAVEL_DIR_WEST = 1 /* 从右端到左端 (←, -X) 偶数沟 */
|
||||
} 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 */
|
||||
Reference in New Issue
Block a user