Falling_jasmine/FDR-Core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-03-28 13:48:31 +08:00
parent ca76b115ea
commit 0e9f24d3f6
10 changed files with 329 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
Core/Src/ladrc.c
View File

@@ -1,30 +1,27 @@
#include "ladrc.h"
#include "f4_can_app.h"
/*
* 这个文件实现两层内容：
* 1) 单个电机的 LADRC 控制器。
* 2) 四轮底盘的调度层（读取编码器 -> 滤波 -> LADRC -> PWM 输出）。
*
* 额外升级点：
* - 提供目标 RPM / 控制输出 getter，供上层故障诊断与状态上报使用。
*/
#define LADRC_CONTROL_DT_S 0.005f
#define LADRC_RPM_FILTER_ALPHA 0.20f
#define LADRC_CONTROL_DT_S 0.01f
#define LADRC_RPM_FILTER_ALPHA 0.30f
/* 这组只是保守起始值，b0 仍建议后续辨识 */
#define LADRC_DEFAULT_WC 12.0f
#define LADRC_DEFAULT_WO 25.0f
#define LADRC_DEFAULT_B0 0.80f
#define LADRC_DEFAULT_OUT_MAX 1000.0f
#define LADRC_DEFAULT_WC 20.0f
#define LADRC_DEFAULT_WO 25.0f
#define LADRC_DEFAULT_B0 0.40f
#define LADRC_DEFAULT_OUT_MAX 1000.0f
/* 把目标爬坡放到控制循环内部，避免依赖外部调用频率 */
#define LADRC_TARGET_SLEW_RPM_PER_S 40.0f
/* ================== 内部辅助函数 ================== */
static float LADRC_Abs(float x);
static float LADRC_Clamp(float x, float min_value, float max_value);
static float Ramp_To_Target(float curr, float target, float step);
static void LADRC_ResetStates(LADRC_TypeDef *ladrc);
static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
LADRC_TypeDef ladrc_motors[4];
static float LADRC_Abs(float x)
{
return (x >= 0.0f) ? x : -x;
@@ -43,6 +40,19 @@ static float LADRC_Clamp(float x, float min_value, float max_value)
return x;
}
static float Ramp_To_Target(float curr, float target, float step)
{
if (target > curr + step)
{
return curr + step;
}
if (target < curr - step)
{
return curr - step;
}
return target;
}
static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
{
if (ladrc == NULL)
@@ -50,19 +60,21 @@ static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
return;
}
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
ladrc->v_cmd = 0.0f;
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
ladrc->eu_prev = 0.0f;
}
/**
* @brief 初始化单个 LADRC 控制器。
* @note 顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
*/
void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max)
{
float h_beta1_2;
float h2_beta2_4;
float delta;
if (ladrc == NULL)
{
return;
@@ -76,6 +88,10 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
{
b0 = LADRC_DEFAULT_B0;
}
if (b0 < 0.0f)
{
b0 = -b0;
}
if (wc < 0.0f)
{
wc = -wc;
@@ -89,33 +105,46 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
max = LADRC_DEFAULT_OUT_MAX;
}
ladrc->v = 0.0f; /* 目标值 */
ladrc->y = 0.0f; /* 测量值 */
ladrc->z1 = 0.0f; /* 状态观测量 */
ladrc->z2 = 0.0f; /* 总扰动观测量 */
ladrc->u = 0.0f; /* 上一拍控制输出 */
LADRC_ResetStates(ladrc);
ladrc->wc = wc;
ladrc->wo = wo;
ladrc->b0 = b0;
ladrc->h = h;
ladrc->wc = wc;
ladrc->wo = wo;
ladrc->b0 = b0;
ladrc->h = h;
ladrc->out_max = max;
ladrc->beta1 = 2.0f * wo;
ladrc->beta2 = wo * wo;
h_beta1_2 = h * ladrc->beta1 * 0.5f;
h2_beta2_4 = h * h * ladrc->beta2 * 0.25f;
delta = 1.0f + h_beta1_2 + h2_beta2_4;
/* x(k+1) = Ad*x(k) + Bdu*u(k) + Bdy*y(k) + Bdaw*eu(k-1) */
ladrc->Ad11 = (1.0f - h_beta1_2 - h2_beta2_4) / delta;
ladrc->Ad12 = h / delta;
ladrc->Ad21 = (-h * ladrc->beta2) / delta;
ladrc->Ad22 = (1.0f + h_beta1_2 - h2_beta2_4) / delta;
ladrc->Bdu1 = (h * ladrc->b0) / delta;
ladrc->Bdu2 = (-h * h * ladrc->beta2 * ladrc->b0 * 0.5f) / delta;
ladrc->Bdy1 = (h * (ladrc->beta1 + h * ladrc->beta2 * 0.5f)) / delta;
ladrc->Bdy2 = (h * ladrc->beta2) / delta;
/* e_u = u_sat - u_raw; 取负增益让饱和时对 z2 形成恢复作用 */
ladrc->kaw = -(ladrc->wo * ladrc->b0);
ladrc->Bdaw1 = (0.5f * h * h * ladrc->kaw) / delta;
ladrc->Bdaw2 = (h * (1.0f + h_beta1_2) * ladrc->kaw) / delta;
}
/**
* @brief 单个 LADRC 周期运算。
* @param ladrc 控制器实例
* @param actual_val 当前测量速度（RPM）
* @retval 限幅后的控制输出（通常可直接映射到 PWM）
*/
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
{
float e;
float z1_new;
float z2_new;
float u0;
float out;
float u_raw;
float u_sat;
if (ladrc == NULL)
{
@@ -124,31 +153,29 @@ float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
ladrc->y = actual_val;
/* 第 1 部分：LESO（线性扩张状态观测器） */
e = ladrc->y - ladrc->z1;
ladrc->z1 += (ladrc->z2 + ladrc->b0 * ladrc->u + ladrc->beta1 * e) * ladrc->h;
ladrc->z2 += (ladrc->beta2 * e) * ladrc->h;
/* LESO + anti-windup 输入统一在同一套 Tustin 离散模型里 */
z1_new = ladrc->Ad11 * ladrc->z1 + ladrc->Ad12 * ladrc->z2
+ ladrc->Bdy1 * ladrc->y + ladrc->Bdu1 * ladrc->u
+ ladrc->Bdaw1 * ladrc->eu_prev;
/* 第 2 部分：LSEF + 扰动补偿 */
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
out = (u0 - ladrc->z2) / ladrc->b0;
z2_new = ladrc->Ad21 * ladrc->z1 + ladrc->Ad22 * ladrc->z2
+ ladrc->Bdy2 * ladrc->y + ladrc->Bdu2 * ladrc->u
+ ladrc->Bdaw2 * ladrc->eu_prev;
/* 第 3 部分：输出限幅，保护电机和驱动 */
out = LADRC_Clamp(out, -ladrc->out_max, ladrc->out_max);
ladrc->z1 = z1_new;
ladrc->z2 = z2_new;
ladrc->u = out;
return out;
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
u_raw = (u0 - ladrc->z2) / ladrc->b0;
u_sat = LADRC_Clamp(u_raw, -ladrc->out_max, ladrc->out_max);
/* 当前拍算出的饱和误差，下一拍再经 Bdaw 进入观测器 */
ladrc->eu_prev = u_sat - u_raw;
ladrc->u = u_sat;
return u_sat;
}
/* =====================================================================
* 针对四轮底盘的 LADRC 调度层
* ===================================================================== */
LADRC_TypeDef ladrc_motors[4];
/**
* @brief 一键初始化底层硬件和四个 LADRC 控制器。
*/
void FourWheel_LADRC_Init(void)
{
int i;
@@ -180,40 +207,20 @@ void FourWheel_LADRC_ResetAll(void)
Motor_Brake_All();
}
/**
* @brief 设置四个轮子的目标 RPM。
*/
#define LADRC_TARGET_SLEW_RPM_PER_S 80.0f // 目标斜率，先试 80 rpm/s
static float Ramp_To_Target(float curr, float target, float step)
{
if (target > curr + step) return curr + step;
if (target < curr - step) return curr - step;
return target;
}
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
{
const float step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
ladrc_motors[MOTOR_FL].v = Ramp_To_Target(ladrc_motors[MOTOR_FL].v, fl_rpm, step);
ladrc_motors[MOTOR_RL].v = Ramp_To_Target(ladrc_motors[MOTOR_RL].v, rl_rpm, step);
ladrc_motors[MOTOR_FR].v = Ramp_To_Target(ladrc_motors[MOTOR_FR].v, fr_rpm, step);
ladrc_motors[MOTOR_RR].v = Ramp_To_Target(ladrc_motors[MOTOR_RR].v, rr_rpm, step);
ladrc_motors[MOTOR_FL].v_cmd = fl_rpm;
ladrc_motors[MOTOR_RL].v_cmd = rl_rpm;
ladrc_motors[MOTOR_FR].v_cmd = fr_rpm;
ladrc_motors[MOTOR_RR].v_cmd = rr_rpm;
}
/**
* @brief 四轮 LADRC 大循环，建议固定 10ms 调用一次。
* @note 调度顺序：
* 1) 更新编码器测速
* 2) 一阶低通滤波，减小测速毛刺
* 3) 每个轮子独立做 LADRC
* 4) 输出到 H 桥
*/
void FourWheel_LADRC_Control_Loop(void)
{
int i;
const float ref_step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
/* 5ms 调度；内部速度估计窗口在 motor.c 里做成 10ms */
Motor_Update_RPM(LADRC_CONTROL_DT_S);
if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
@@ -224,21 +231,23 @@ void FourWheel_LADRC_Control_Loop(void)
for (i = 0; i < 4; ++i)
{
float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
float raw_rpm;
float pwm_out;
s_filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * s_filtered_rpm[i]
+ LADRC_RPM_FILTER_ALPHA * raw_rpm;
ladrc_motors[i].v = Ramp_To_Target(ladrc_motors[i].v,
ladrc_motors[i].v_cmd,
ref_step);
raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
/* 外部低通保留，但减为“轻滤波” */
s_filtered_rpm[i] += LADRC_RPM_FILTER_ALPHA * (raw_rpm - s_filtered_rpm[i]);
pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
}
}
/**
* @brief 读取指定轮子的“当前目标 RPM”。
* @note 该接口主要供上层做状态上报与故障诊断使用。
*/
float FourWheel_Get_Target_RPM(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))
@@ -246,13 +255,19 @@ float FourWheel_Get_Target_RPM(Motor_ID_t id)
return 0.0f;
}
return ladrc_motors[id].v_cmd;
}
float FourWheel_Get_Ref_RPM(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))
{
return 0.0f;
}
return ladrc_motors[id].v;
}
/**
* @brief 读取指定轮子的“当前控制输出”。
* @note 该值尚未经过电机左右镜像翻转，但足够用于判断是否长期顶满。
*/
float FourWheel_Get_Control_Output(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))