2026-03-31 23:30:33 +08:00
|
|
|
|
#ifndef CORRIDOR_CTRL_H
|
|
|
|
|
|
#define CORRIDOR_CTRL_H
|
|
|
|
|
|
|
|
|
|
|
|
#include "preproc/corridor_msgs.h"
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
|
* @brief 走廊控制器调参配置
|
|
|
|
|
|
* @note 所有增益参数都应在调试阶段"从小往大调"，防止震荡
|
|
|
|
|
|
*/
|
|
|
|
|
|
typedef struct {
|
|
|
|
|
|
float kp_theta; // 航向偏差比例增益 (rad/s per rad)
|
|
|
|
|
|
float kd_theta; // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
|
|
|
|
|
|
float kp_y; // 横向偏差比例增益 (rad/s per m)
|
|
|
|
|
|
|
|
|
|
|
|
float v_cruise; // 走廊内巡航线速度 (m/s)
|
|
|
|
|
|
float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
|
|
|
|
|
|
float v_max; // 线速度输出硬限幅 (m/s)
|
|
|
|
|
|
float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
|
2026-04-12 11:57:14 +08:00
|
|
|
|
|
|
|
|
|
|
float exit_front_dist; // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制
|
|
|
|
|
|
float wall_escape_dist; // 近墙脱离阈值 (m)，小于此值触发直接远离墙面
|
|
|
|
|
|
float wall_escape_kp; // 近墙脱离增益 (rad/s per m)
|
|
|
|
|
|
float wall_escape_w_max; // 近墙脱离角速度限幅 (rad/s)
|
2026-03-31 23:30:33 +08:00
|
|
|
|
} CorridorCtrlConfig_t;
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef __cplusplus
|
|
|
|
|
|
extern "C" {
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
|
* @brief 初始化走廊控制器
|
|
|
|
|
|
* @param config 控制器增益与限幅配置
|
|
|
|
|
|
*/
|
|
|
|
|
|
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
|
* @brief 核心函数：根据滤波器输出的走廊状态计算控制量
|
|
|
|
|
|
* @param state 滤波器输出的走廊状态 (e_y, e_th, conf)
|
|
|
|
|
|
* @param obs 预处理层的观测快照 (用于获取 d_front 等辅助信息)
|
|
|
|
|
|
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
|
|
|
|
|
|
* @param out_cmd 输出的控制指令 (v, w)
|
|
|
|
|
|
*/
|
|
|
|
|
|
void CorridorCtrl_Compute(const CorridorState_t *state,
|
|
|
|
|
|
const CorridorObs_t *obs,
|
|
|
|
|
|
float imu_wz,
|
|
|
|
|
|
RawCmd_t *out_cmd);
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef __cplusplus
|
|
|
|
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#endif // CORRIDOR_CTRL_H
|
