170 lines
6.6 KiB
C
170 lines
6.6 KiB
C
#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;
|
||
}
|