Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-12 14:11:03 +08:00
parent 37a1788543
commit 521b70c29b
4 changed files with 11 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
App/nav/corridor_ctrl.c
View File

@@ -1,13 +1,10 @@
#include "corridor_ctrl.h"
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
/* ====================== 内部状态 ====================== */
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) {