280 lines
6.4 KiB
C
280 lines
6.4 KiB
C
#include "ladrc.h"
|
||
#include "f4_can_app.h"
|
||
|
||
#define LADRC_CONTROL_DT_S 0.005f
|
||
#define LADRC_RPM_FILTER_ALPHA 0.20f
|
||
|
||
/* 这组只是保守起始值，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_TARGET_SLEW_RPM_PER_S 100.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;
|
||
}
|
||
|
||
static float LADRC_Clamp(float x, float min_value, float max_value)
|
||
{
|
||
if (x > max_value)
|
||
{
|
||
return max_value;
|
||
}
|
||
if (x < min_value)
|
||
{
|
||
return min_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)
|
||
{
|
||
return;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
if (h <= 0.0f)
|
||
{
|
||
h = LADRC_CONTROL_DT_S;
|
||
}
|
||
if (LADRC_Abs(b0) < 1e-6f)
|
||
{
|
||
b0 = LADRC_DEFAULT_B0;
|
||
}
|
||
if (b0 < 0.0f)
|
||
{
|
||
b0 = -b0;
|
||
}
|
||
if (wc < 0.0f)
|
||
{
|
||
wc = -wc;
|
||
}
|
||
if (wo < 0.0f)
|
||
{
|
||
wo = -wo;
|
||
}
|
||
if (max <= 0.0f)
|
||
{
|
||
max = LADRC_DEFAULT_OUT_MAX;
|
||
}
|
||
|
||
LADRC_ResetStates(ladrc);
|
||
|
||
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;
|
||
}
|
||
|
||
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
|
||
{
|
||
float z1_new;
|
||
float z2_new;
|
||
float u0;
|
||
float u_raw;
|
||
float u_sat;
|
||
|
||
if (ladrc == NULL)
|
||
{
|
||
return 0.0f;
|
||
}
|
||
|
||
ladrc->y = actual_val;
|
||
|
||
/* 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;
|
||
|
||
z2_new = ladrc->Ad21 * ladrc->z1 + ladrc->Ad22 * ladrc->z2
|
||
+ ladrc->Bdy2 * ladrc->y + ladrc->Bdu2 * ladrc->u
|
||
+ ladrc->Bdaw2 * ladrc->eu_prev;
|
||
|
||
ladrc->z1 = z1_new;
|
||
ladrc->z2 = z2_new;
|
||
|
||
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;
|
||
}
|
||
|
||
void FourWheel_LADRC_Init(void)
|
||
{
|
||
int i;
|
||
|
||
Motor_Init();
|
||
|
||
for (i = 0; i < 4; ++i)
|
||
{
|
||
LADRC_Init(&ladrc_motors[i],
|
||
LADRC_DEFAULT_WC,
|
||
LADRC_DEFAULT_WO,
|
||
LADRC_DEFAULT_B0,
|
||
LADRC_CONTROL_DT_S,
|
||
LADRC_DEFAULT_OUT_MAX);
|
||
s_filtered_rpm[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
void FourWheel_LADRC_ResetAll(void)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < 4; ++i)
|
||
{
|
||
LADRC_ResetStates(&ladrc_motors[i]);
|
||
s_filtered_rpm[i] = 0.0f;
|
||
}
|
||
|
||
Motor_Brake_All();
|
||
}
|
||
|
||
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
|
||
{
|
||
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;
|
||
}
|
||
|
||
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)
|
||
{
|
||
FourWheel_LADRC_ResetAll();
|
||
return;
|
||
}
|
||
|
||
for (i = 0; i < 4; ++i)
|
||
{
|
||
float raw_rpm;
|
||
float pwm_out;
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
float FourWheel_Get_Target_RPM(Motor_ID_t id)
|
||
{
|
||
if ((id < MOTOR_FL) || (id > MOTOR_RR))
|
||
{
|
||
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;
|
||
}
|
||
|
||
float FourWheel_Get_Control_Output(Motor_ID_t id)
|
||
{
|
||
if ((id < MOTOR_FL) || (id > MOTOR_RR))
|
||
{
|
||
return 0.0f;
|
||
}
|
||
|
||
return ladrc_motors[id].u;
|
||
}
|