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 11:57:14 +08:00
parent cedcd4738e
commit 9ffb750072
10 changed files with 521 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
App/nav/corridor_ctrl.c
View File

@@ -32,6 +32,17 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
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
@@ -40,11 +51,56 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
* - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
* 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
* - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
*
* 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏
* ======================================================== */
float w_cmd = -(s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * imu_wz
+ s_cfg.kp_y * state->e_y);
float w_cmd;
bool escape_active = false;
if (near_exit) {
/* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
w_cmd = -(s_cfg.kd_theta * imu_wz);
} else {
/* 正常控制: 完整PD控制律 */
w_cmd = -(s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * imu_wz
+ s_cfg.kp_y * state->e_y);
/* ========================================================
* 近墙脱离保护:
* 当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。
* 此时不能只等 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);
@@ -59,6 +115,11 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
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);
@@ -66,11 +127,13 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
* 置信度降级保护:
* 当滤波器健康度 conf 过低（两边雷达全瞎），
* 说明走廊参照完全丢失，降低线速度防止盲飞
*
* 注意：阈值不宜过高，否则会过度降级导致控制器失效
* ======================================================== */
if (state->conf < 0.3f) {
if (state->conf < 0.2f) {
/* 健康度极低：速度打三折，保持航向惯性滑行 */
v_cmd *= 0.3f;
} else if (state->conf < 0.6f) {
} else if (state->conf < 0.4f) {
/* 健康度较低（单侧退化）：速度打七折 */
v_cmd *= 0.7f;
}