diff --git a/App/app_tasks.c b/App/app_tasks.c index 2daa147..6c5b501 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -269,14 +269,14 @@ void AppTasks_RunVl53Task_Impl(void *argument) /* ========================================================= * 4. 走廊导航控制任务 (Nav Pipeline) - * 周期：由 PARAM_NAV_TASK_PERIOD_MS 配置 + * 周期：20ms (50Hz)，与 CAN 发送同频 * 流水线：Obs → Filter → Ctrl → Script → FSM → CmdSlot_Push * ========================================================= */ void AppTasks_RunNavTask_Impl(void *argument) { (void)argument; uint32_t wake_tick = osKernelGetTickCount(); - const uint32_t period_ticks = AppTasks_MsToTicks(PARAM_NAV_TASK_PERIOD_MS); + const uint32_t period_ticks = AppTasks_MsToTicks(20U); uint32_t last_ms = HAL_GetTick(); /* 等传感器全部就绪再启动 (避免刚上电全是脏数据)。 @@ -293,7 +293,7 @@ void AppTasks_RunNavTask_Impl(void *argument) uint32_t now_ms = HAL_GetTick(); float dt_s = (float)(now_ms - last_ms) / 1000.0f; if (dt_s <= 0.0f || dt_s > 0.5f) { - dt_s = (float)PARAM_NAV_TASK_PERIOD_MS / 1000.0f; /* 容错：防止首拍或溢出 */ + dt_s = 0.02f; /* 容错：防止首拍或溢出 */ } last_ms = now_ms; @@ -440,11 +440,6 @@ void AppTasks_Init(void) .w_max = PARAM_CTRL_W_MAX, /* 角速度限幅 */ .v_max = PARAM_CTRL_V_MAX, /* 线速度限幅 */ .speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */ - .speed_reduction_deadband = PARAM_CTRL_SPEED_RED_DB, - .w_slew_rate = PARAM_CTRL_W_SLEW_RATE, - .startup_dist = PARAM_CTRL_STARTUP_DIST, - .startup_kp_y_scale = PARAM_CTRL_STARTUP_KPY_SCALE, - .startup_w_scale = PARAM_CTRL_STARTUP_W_SCALE, .exit_front_dist = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */ .wall_escape_dist = PARAM_CTRL_WALL_ESCAPE_DIST, .wall_escape_kp = PARAM_CTRL_WALL_ESCAPE_KP, diff --git a/App/nav/corridor_ctrl.c b/App/nav/corridor_ctrl.c index d037506..e0112ce 100644 --- a/App/nav/corridor_ctrl.c +++ b/App/nav/corridor_ctrl.c @@ -1,13 +1,10 @@ #include "corridor_ctrl.h" #include #include -#include /* ====================== 内部状态 ====================== */ static CorridorCtrlConfig_t s_cfg; static bool s_initialized = false; -static float s_last_w_cmd = 0.0f; -static uint32_t s_last_t_ms = 0U; /* 辅助：浮点数限幅 */ static inline float clampf(float val, float lo, float hi) @@ -21,8 +18,6 @@ void CorridorCtrl_Init(const CorridorCtrlConfig_t *config) { s_cfg = *config; s_initialized = true; - s_last_w_cmd = 0.0f; - s_last_t_ms = 0U; } void CorridorCtrl_Compute(const CorridorState_t *state, @@ -62,31 +57,14 @@ void CorridorCtrl_Compute(const CorridorState_t *state, float w_cmd; bool escape_active = false; - bool front_guard_active = false; - float kp_theta_eff = s_cfg.kp_theta; - float kp_y_eff = s_cfg.kp_y; - float w_max_eff = s_cfg.w_max; - - if (!near_exit && s_cfg.startup_dist > 0.01f && state->s < s_cfg.startup_dist) { - float ratio = state->s / s_cfg.startup_dist; - if (ratio < 0.0f) ratio = 0.0f; - if (ratio > 1.0f) ratio = 1.0f; - - kp_y_eff = s_cfg.kp_y * (s_cfg.startup_kp_y_scale + (1.0f - s_cfg.startup_kp_y_scale) * ratio); - w_max_eff = s_cfg.w_max * (s_cfg.startup_w_scale + (1.0f - s_cfg.startup_w_scale) * ratio); - if (w_max_eff < 0.25f) { - w_max_eff = 0.25f; - } - } - if (near_exit) { /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */ w_cmd = -(s_cfg.kd_theta * imu_wz); } else { /* 正常控制: 完整PD控制律 */ - w_cmd = -(kp_theta_eff * state->e_th + w_cmd = -(s_cfg.kp_theta * state->e_th + s_cfg.kd_theta * imu_wz - + kp_y_eff * state->e_y); + + s_cfg.kp_y * state->e_y); /* ======================================================== * 近墙脱离保护: @@ -121,67 +99,11 @@ void CorridorCtrl_Compute(const CorridorState_t *state, w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max); w_cmd += w_escape; - - /* 前角防呆： - * 如果某一侧前角已经明显很近，就绝不允许继续朝这一侧打方向。 - * 这专门处理“已贴左墙却继续左打 / 已贴右墙却继续右打”的情况。 */ - { - float front_guard_dist = s_cfg.wall_escape_dist + 0.01f; - - if (left_front_ok && obs->d_lf < front_guard_dist) { - float err = front_guard_dist - obs->d_lf; - float w_guard = 0.15f + 0.20f * (err / front_guard_dist); - if (w_cmd > -w_guard) { - w_cmd = -w_guard; - } - front_guard_active = true; - } - - if (right_front_ok && obs->d_rf < front_guard_dist) { - float err = front_guard_dist - obs->d_rf; - float w_guard = 0.15f + 0.20f * (err / front_guard_dist); - if (w_cmd < w_guard) { - w_cmd = w_guard; - } - front_guard_active = true; - } - } } } /* 角速度限幅：防止 PD 溢出导致原地打转 */ - w_cmd = clampf(w_cmd, -w_max_eff, w_max_eff); - - /* 近墙保护触发时，再对总角速度做一次更保守的限幅， - * 防止“贴墙 -> 一把猛打 -> 前向安全仲裁介入”的连锁反应。 */ - if (escape_active) { - w_cmd = clampf(w_cmd, -0.60f, 0.60f); - } - - if (front_guard_active) { - w_cmd = clampf(w_cmd, -0.35f, 0.35f); - } - - /* 角速度变化率限幅：抑制单拍突变，避免看起来像“突然失控猛打方向”。 */ - { - float dt = 0.01f; - if (s_last_t_ms != 0U && state->t_ms > s_last_t_ms) { - dt = (float)(state->t_ms - s_last_t_ms) * 0.001f; - if (dt <= 0.0f || dt > 0.1f) { - dt = 0.01f; - } - } - - { - float max_step = s_cfg.w_slew_rate * dt; - float delta = w_cmd - s_last_w_cmd; - delta = clampf(delta, -max_step, max_step); - w_cmd = s_last_w_cmd + delta; - } - - s_last_w_cmd = w_cmd; - s_last_t_ms = state->t_ms; - } + w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max); /* ======================================================== * 线速度策略: @@ -190,18 +112,7 @@ void CorridorCtrl_Compute(const CorridorState_t *state, * 公式: v = v_cruise * (1 - k * |w/w_max|) * k 取 0.3~0.5 较保守 * ======================================================== */ - float speed_reduction = 0.0f; - { - float w_ratio = fabsf(w_cmd) / w_max_eff; - float db = s_cfg.speed_reduction_deadband; - if (db < 0.0f) db = 0.0f; - if (db > 0.95f) db = 0.95f; - - if (w_ratio > db) { - float active_ratio = (w_ratio - db) / (1.0f - db); - speed_reduction = s_cfg.speed_reduction_k * active_ratio; - } - } + 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); /* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */ @@ -209,10 +120,6 @@ void CorridorCtrl_Compute(const CorridorState_t *state, v_cmd *= 0.80f; } - if (front_guard_active) { - v_cmd *= 0.65f; - } - /* 线速度限幅：不允许倒车，不允许超速 */ v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max); @@ -220,7 +127,7 @@ void CorridorCtrl_Compute(const CorridorState_t *state, * 置信度降级保护: * 当滤波器健康度 conf 过低（两边雷达全瞎）， * 说明走廊参照完全丢失，降低线速度防止盲飞 - * + * * 注意：阈值不宜过高，否则会过度降级导致控制器失效 * ======================================================== */ if (state->conf < 0.2f) { diff --git a/App/nav/corridor_ctrl.h b/App/nav/corridor_ctrl.h index 2a5a206..b789152 100644 --- a/App/nav/corridor_ctrl.h +++ b/App/nav/corridor_ctrl.h @@ -16,11 +16,6 @@ typedef struct { 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 speed_reduction_deadband; // 小角速度死区：低于该比例时不减速 - float w_slew_rate; // 角速度变化率限幅 (rad/s^2)，防止一帧猛打方向 - float startup_dist; // 入沟软启动距离 (m)，前一小段降低回中力度 - float startup_kp_y_scale; // 入沟起始横向增益缩放 (0~1) - float startup_w_scale; // 入沟起始角速度限幅缩放 (0~1) float exit_front_dist; // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制 float wall_escape_dist; // 近墙脱离阈值 (m)，小于此值触发直接远离墙面 diff --git a/App/robot_params.h b/App/robot_params.h index bdca6ce..af58cfc 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -453,7 +453,7 @@ extern "C" { * 1: 使用滤波后的 range_mm_filtered * 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比 */ -#define PARAM_VL53_USE_EMA_FILTER 1 +#define PARAM_VL53_USE_EMA_FILTER 0 /** @brief [调优] VL53L0X EMA滤波平滑系数 alpha * 含义：新测量值的权重 (0.0~1.0) @@ -489,10 +489,9 @@ extern "C" { #define PARAM_NAV_STARTUP_DELAY_MS 5000U /** @brief 导航主循环周期 [ms] - * 包含：预处理 -> EKF -> GlobalNav -> CorridorCtrl -> Safety - * 从 20ms 提高到 10ms，用更高闭环频率提升预测与回中响应。 + * 保留为参数形式，当前使用 20ms，与 411 稳定版本一致。 */ -#define PARAM_NAV_TASK_PERIOD_MS 10U +#define PARAM_NAV_TASK_PERIOD_MS 20U /* --- 入场段 --- */ /* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */