diff --git a/App/app_tasks.c b/App/app_tasks.c index 6c5b501..2daa147 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -269,14 +269,14 @@ void AppTasks_RunVl53Task_Impl(void *argument) /* ========================================================= * 4. 走廊导航控制任务 (Nav Pipeline) - * 周期：20ms (50Hz)，与 CAN 发送同频 + * 周期：由 PARAM_NAV_TASK_PERIOD_MS 配置 * 流水线：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(20U); + const uint32_t period_ticks = AppTasks_MsToTicks(PARAM_NAV_TASK_PERIOD_MS); 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 = 0.02f; /* 容错：防止首拍或溢出 */ + dt_s = (float)PARAM_NAV_TASK_PERIOD_MS / 1000.0f; /* 容错：防止首拍或溢出 */ } last_ms = now_ms; @@ -440,6 +440,11 @@ 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 fec0cd7..d037506 100644 --- a/App/nav/corridor_ctrl.c +++ b/App/nav/corridor_ctrl.c @@ -1,10 +1,13 @@ #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) @@ -18,6 +21,8 @@ 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, @@ -57,14 +62,31 @@ 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 = -(s_cfg.kp_theta * state->e_th + w_cmd = -(kp_theta_eff * state->e_th + s_cfg.kd_theta * imu_wz - + s_cfg.kp_y * state->e_y); + + kp_y_eff * state->e_y); /* ======================================================== * 近墙脱离保护: @@ -99,11 +121,67 @@ 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, -s_cfg.w_max, s_cfg.w_max); + 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; + } /* ======================================================== * 线速度策略: @@ -112,7 +190,18 @@ void CorridorCtrl_Compute(const CorridorState_t *state, * 公式: 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 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 v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction); /* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */ @@ -120,6 +209,10 @@ 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); diff --git a/App/nav/corridor_ctrl.h b/App/nav/corridor_ctrl.h index b789152..2a5a206 100644 --- a/App/nav/corridor_ctrl.h +++ b/App/nav/corridor_ctrl.h @@ -16,6 +16,11 @@ 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/nav/global_nav.c b/App/nav/global_nav.c index 90a3e5b..6b6c3b1 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -95,6 +95,21 @@ static inline float gnav_fabsf(float x) return x < 0.0f ? -x : x; } +typedef struct { + bool sides_complete; + bool near_sat; + bool width_ok; + bool heading_valid; + bool diagonal_conflict; + bool low_yaw_rate; + bool safe_for_align; + bool need_align; + float d_left_avg; + float d_right_avg; + float e_y_m; + float heading_rad; +} CorridorPoseEval_t; + /** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */ static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp) { @@ -187,6 +202,83 @@ static bool compute_wall_heading_error(const CorridorObs_t* obs, float* out_head return true; } +static void evaluate_corridor_pose(const CorridorObs_t* obs, + const RobotBlackboard_t* board, + CorridorPoseEval_t* out_eval) +{ + memset(out_eval, 0, sizeof(*out_eval)); + + 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); + + out_eval->sides_complete = left_ok && right_ok; + if (!out_eval->sides_complete) { + return; + } + + out_eval->d_left_avg = (obs->d_lf + obs->d_lr) * 0.5f; + out_eval->d_right_avg = (obs->d_rf + obs->d_rr) * 0.5f; + out_eval->e_y_m = 0.5f * (out_eval->d_right_avg - out_eval->d_left_avg); + + { + float total_width = out_eval->d_left_avg + out_eval->d_right_avg + PARAM_ROBOT_WIDTH; + float width_err = gnav_fabsf(total_width - s_nav.cfg.corridor_width); + out_eval->width_ok = (width_err <= s_nav.cfg.reacquire_width_tol); + } + + { + float sat_eps = 0.002f; + out_eval->near_sat = (obs->d_lf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) || + (obs->d_lr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) || + (obs->d_rf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) || + (obs->d_rr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)); + } + + { + float near_thresh = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH) - 0.015f; + bool lf_near = obs->d_lf < near_thresh; + bool lr_near = obs->d_lr < near_thresh; + bool rf_near = obs->d_rf < near_thresh; + bool rr_near = obs->d_rr < near_thresh; + + /* 对角贴墙模式：大角度进沟时常见，此时 wall heading 几何容易失真。 */ + out_eval->diagonal_conflict = (lf_near && rr_near) || (rf_near && lr_near); + } + + { + float sensor_base = PARAM_SENSOR_BASE_LENGTH; + float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base); + float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base); + + if (gnav_fabsf(left_heading - right_heading) <= PARAM_DEG2RAD(8.0f)) { + out_eval->heading_rad = 0.5f * (left_heading + right_heading); + out_eval->heading_valid = out_eval->width_ok && !out_eval->near_sat; + } + } + + if (board != NULL && board->imu_wz.is_valid) { + out_eval->low_yaw_rate = gnav_fabsf(board->imu_wz.value) < 25.0f; + } + + { + float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH); + float min_side = (out_eval->d_left_avg < out_eval->d_right_avg) + ? out_eval->d_left_avg : out_eval->d_right_avg; + bool heading_bad = out_eval->heading_valid && + (gnav_fabsf(out_eval->heading_rad) > s_nav.cfg.align_th_tol_rad); + bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m); + bool off_center = gnav_fabsf(out_eval->e_y_m) > s_nav.cfg.align_y_tol_m; + + out_eval->need_align = heading_bad || near_wall || off_center || out_eval->diagonal_conflict; + } + + out_eval->safe_for_align = out_eval->heading_valid && + !out_eval->diagonal_conflict && + out_eval->low_yaw_rate; +} + static void update_wall_heading_stability(bool valid, float heading_rad) { if (!valid) { @@ -208,33 +300,6 @@ static void update_wall_heading_stability(bool valid, float heading_rad) s_nav.wall_heading_prev_valid = true; } -/* 重捕获已确认后，判断是否需要短暂停车摆正。 - * 除了航向误差本身，还把“明显贴墙/明显偏中心”作为触发条件。 - * 这样即使车头看起来近似平行，但整车已经贴到一侧，也会先停一下再进跟踪。 */ -static bool need_align_after_reacquire(const CorridorObs_t* obs, float wall_heading_error) -{ - 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) { - return gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad; - } - - { - float d_left = (obs->d_lf + obs->d_lr) * 0.5f; - float d_right = (obs->d_rf + obs->d_rr) * 0.5f; - float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH); - float min_side = (d_left < d_right) ? d_left : d_right; - bool heading_bad = gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad; - bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m); - bool off_center = gnav_fabsf(d_left - d_right) > (2.0f * s_nav.cfg.align_y_tol_m); - - return heading_bad || near_wall || off_center; - } -} - /** 检查重捕获条件 */ static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state) { @@ -635,22 +700,24 @@ void GlobalNav_Update(const CorridorObs_t* obs, } if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) { - float wall_heading_error = 0.0f; - bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error); - update_wall_heading_stability(wall_heading_ok, wall_heading_error); + CorridorPoseEval_t pose_eval; + evaluate_corridor_pose(obs, board, &pose_eval); + update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad); - /* 重捕获成功后，优先用 side VL53 短暂停车摆正车头。 - * 只有 wall heading 已稳定时才允许进入 ALIGN 或直接放行。 */ - if (wall_heading_ok && s_nav.wall_heading_stable_count >= 4U) { - if (need_align_after_reacquire(obs, wall_heading_error)) { - transition_to(GNAV_ALIGN, board); - } else { - if (board->imu_yaw_continuous.is_valid) { - CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); - s_nav.heading_ref_deg = board->imu_yaw_continuous.value; - } - transition_to(GNAV_CORRIDOR_TRACK, board); + /* 评估结果说明： + * - need_align: 车头歪/偏心/贴墙/对角近墙，需要先停一下 + * - safe_for_align: 当前几何足够健康，允许按姿态估计输出转向 + * + * 即使几何暂时不安全，只要 need_align=true 也先进入 ALIGN 停住， + * 避免一边前进一边突然大角度纠正。 */ + if (pose_eval.need_align) { + transition_to(GNAV_ALIGN, board); + } else { + if (board->imu_yaw_continuous.is_valid) { + CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); + s_nav.heading_ref_deg = board->imu_yaw_continuous.value; } + transition_to(GNAV_CORRIDOR_TRACK, board); } } if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) { @@ -668,23 +735,26 @@ void GlobalNav_Update(const CorridorObs_t* obs, * 控制器还继续朝墙边打”的情况。 * ============================================================ */ case GNAV_ALIGN: { - float wall_heading_error = 0.0f; - bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error); - update_wall_heading_stability(wall_heading_ok, wall_heading_error); + CorridorPoseEval_t pose_eval; + evaluate_corridor_pose(obs, board, &pose_eval); + update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad); out->override_v = 0.0f; out->use_override = true; out->request_corridor = false; out->safety_mode = SAFETY_MODE_STRAIGHT; - if (wall_heading_ok && s_nav.wall_heading_stable_count >= 2U) { - out->override_w = gnav_clampf(-s_nav.cfg.align_kp_th * wall_heading_error, - -0.25f, 0.25f); + if (pose_eval.safe_for_align && s_nav.wall_heading_stable_count >= 2U) { + float w_align = -(s_nav.cfg.align_kp_th * pose_eval.heading_rad + + s_nav.cfg.align_kp_y * pose_eval.e_y_m); + out->override_w = gnav_clampf(w_align, -0.20f, 0.20f); } else { out->override_w = 0.0f; } - if (wall_heading_ok && gnav_fabsf(wall_heading_error) < s_nav.cfg.align_th_tol_rad) { + if (pose_eval.safe_for_align && + gnav_fabsf(pose_eval.heading_rad) < s_nav.cfg.align_th_tol_rad && + gnav_fabsf(pose_eval.e_y_m) < s_nav.cfg.align_y_tol_m) { s_nav.align_ok_count++; } else { s_nav.align_ok_count = 0; @@ -697,6 +767,13 @@ void GlobalNav_Update(const CorridorObs_t* obs, } transition_to(GNAV_CORRIDOR_TRACK, board); } + if (!pose_eval.safe_for_align && elapsed_ms > 400U) { + if (board->imu_yaw_continuous.is_valid) { + CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); + s_nav.heading_ref_deg = board->imu_yaw_continuous.value; + } + transition_to(GNAV_CORRIDOR_TRACK, board); + } if (elapsed_ms > s_nav.cfg.align_timeout_ms) { if (board->imu_yaw_continuous.is_valid) { CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); diff --git a/App/preproc/corridor_preproc.c b/App/preproc/corridor_preproc.c index 804c3cb..c3187de 100644 --- a/App/preproc/corridor_preproc.c +++ b/App/preproc/corridor_preproc.c @@ -18,6 +18,14 @@ static bool process_side_laser(const SensorItem_t *item, float *out_m) if (dist_m > PREPROC_MAX_SIDE_RANGE_M || dist_m < PREPROC_MIN_SIDE_RANGE_M) { return false; } + + /* 靠墙过近退化： + * 侧向 VL53 在最小量程附近（例如实际 3cm、模组最小可信 4cm）时， + * 直接判无效会让系统退化成单边观测，反而更容易贴墙。 + * 这里改成“钳位到最小可信距离”，既保留该侧存在感，又避免把不可信的超近值直接送上去。 */ + if (dist_m < PREPROC_SAT_NEAR_SIDE_RANGE_M) { + dist_m = PREPROC_SAT_NEAR_SIDE_RANGE_M; + } *out_m = dist_m; return true; @@ -116,4 +124,4 @@ void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, if (out_obs->d_back < 0.0f) out_obs->d_back = 0.0f; out_obs->valid_mask |= CORRIDOR_OBS_MASK_BACK; } -} \ No newline at end of file +} diff --git a/App/preproc/corridor_preproc.h b/App/preproc/corridor_preproc.h index 99a773d..8574bff 100644 --- a/App/preproc/corridor_preproc.h +++ b/App/preproc/corridor_preproc.h @@ -17,6 +17,9 @@ /* VL53L0X 侧向雷达的物理有效探测区间 (m) */ #define PREPROC_MAX_SIDE_RANGE_M 2.0f #define PREPROC_MIN_SIDE_RANGE_M 0.02f +/* 侧向 VL53 靠墙过近时的退化阈值 (m) + * 低于此值虽然可能还有数字，但已接近最小量程区，不再作为可信几何量使用。 */ +#define PREPROC_SAT_NEAR_SIDE_RANGE_M PARAM_VL53_SIDE_SAT_NEAR_M /* 前后向雷达近战盲区阈值 (m) (STP 7cm盲区 + 1cm工程裕量) */ #define PREPROC_BLIND_ZONE_M 0.08f @@ -40,4 +43,4 @@ extern "C" { } #endif -#endif // CORRIDOR_PREPROC_H \ No newline at end of file +#endif // CORRIDOR_PREPROC_H diff --git a/App/robot_params.h b/App/robot_params.h index 46569fd..bdca6ce 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -112,6 +112,13 @@ extern "C" { */ #define PARAM_VL53_RIGHT_INSET 0.0f +/** @brief [调优] 侧向 VL53 近墙退化阈值 [m] + * 含义：当侧向 VL53 读数小于该值时，认为已进入最小量程附近，数据不再可信。 + * 处理策略：预处理层直接将该侧观测判为无效，让上层退化为主要信另一侧。 + * 典型值：0.05~0.06 + */ +#define PARAM_VL53_SIDE_SAT_NEAR_M 0.03f + /* ------------------- 编码器参数 ------------------- */ /** @brief [实测] 编码器每转脉冲数 (CPR) * 测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲 @@ -305,6 +312,32 @@ extern "C" { */ #define PARAM_CTRL_SPEED_REDUCTION 0.4f +/** @brief [调优] 弯道减速死区 [0~1] + * 含义：当 |w|/w_max 小于该比例时，不做减速。 + * 目的：小幅修正时保持巡航，不要长期因为轻微纠偏而慢速运行。 + */ +#define PARAM_CTRL_SPEED_RED_DB 0.35f + +/** @brief [调优] 角速度变化率限幅 [rad/s^2] + * 含义：限制相邻控制周期之间角速度变化，防止一帧突然猛打方向。 + */ +#define PARAM_CTRL_W_SLEW_RATE 6.0f + +/** @brief [调优] 入沟软启动距离 [m] + * 含义：进入沟内后的前一小段距离，降低回中力度，避免一入沟就猛打一把。 + */ +#define PARAM_CTRL_STARTUP_DIST 0.15f + +/** @brief [调优] 入沟起始横向增益缩放 [0~1] + * 含义：state->s=0 时 kp_y 的缩放比例，随后随距离线性恢复到 1。 + */ +#define PARAM_CTRL_STARTUP_KPY_SCALE 0.45f + +/** @brief [调优] 入沟起始角速度限幅缩放 [0~1] + * 含义：state->s=0 时 w_max 的缩放比例，随后随距离线性恢复到 1。 + */ +#define PARAM_CTRL_STARTUP_W_SCALE 0.45f + /** @brief [调优] 出沟检测距离 [m] * 含义：前激光小于此值时禁用左右激光控制，避免出沟时数据突变导致大幅转向 * 典型值：0.40 (40cm) @@ -420,7 +453,7 @@ extern "C" { * 1: 使用滤波后的 range_mm_filtered * 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比 */ -#define PARAM_VL53_USE_EMA_FILTER 0 +#define PARAM_VL53_USE_EMA_FILTER 1 /** @brief [调优] VL53L0X EMA滤波平滑系数 alpha * 含义：新测量值的权重 (0.0~1.0) @@ -455,6 +488,12 @@ extern "C" { */ #define PARAM_NAV_STARTUP_DELAY_MS 5000U +/** @brief 导航主循环周期 [ms] + * 包含：预处理 -> EKF -> GlobalNav -> CorridorCtrl -> Safety + * 从 20ms 提高到 10ms，用更高闭环频率提升预测与回中响应。 + */ +#define PARAM_NAV_TASK_PERIOD_MS 10U + /* --- 入场段 --- */ /* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */ #define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 (沿左端纵向通道向北) */ @@ -472,7 +511,7 @@ extern "C" { #define PARAM_GNAV_REACQUIRE_V 0.1f /* m/s — 重捕获入沟速度 */ #define PARAM_GNAV_REACQUIRE_CONF 0.4f /* — 重捕获置信度阈值（从0.6降到0.4，更容易成功） */ #define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.08f /* m — 走廊宽度容差（从5cm放宽到8cm） */ -#define PARAM_GNAV_REACQUIRE_MIN_ODOM 0.12f /* m — 最小入沟里程，避免沟口过早假成功 */ +#define PARAM_GNAV_REACQUIRE_MIN_ODOM 0.06f /* m — 最小入沟里程，快捕获后尽快停车 */ #define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数，取更稳的 100ms */ #define PARAM_GNAV_REACQUIRE_TIMEOUT 5000U /* ms — 重捕获超时 */ @@ -483,7 +522,7 @@ extern "C" { #define PARAM_GNAV_ALIGN_Y_TOL 0.02f /* m — 对齐横向容差 (2cm) */ #define PARAM_GNAV_ALIGN_TICKS 5 /* 拍 — 对齐确认次数 (5×20ms=100ms) */ #define PARAM_GNAV_ALIGN_TIMEOUT 3000U /* ms — 对齐超时 */ -#define PARAM_GNAV_REACQUIRE_MIN_BACK 0.40f /* m — 重捕获最小后激光距离，判断是否真正进沟 */ +#define PARAM_GNAV_REACQUIRE_MIN_BACK 0.38f /* m — 重捕获最小后激光距离，判断是否真正进沟 */ /* --- 沟内 --- */ #define PARAM_GNAV_CORRIDOR_MAX_LEN 2.70f /* m — 沟内里程保护上限 */