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-14 17:17:17 +08:00
parent 838c121a18
commit e9e2f666fb
11 changed files with 1609 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
Core/Src/ladrc.c
View File

@@ -1,76 +1,155 @@
#include "ladrc.h"
/**
* @brief 初始化单体 LADRC 参数
/*
* 这个文件实现两层内容：
* 1) 单个电机的 LADRC 控制器。
* 2) 四轮底盘的调度层（读取编码器 -> 滤波 -> LADRC -> PWM 输出）。
*
* 额外升级点：
* - 提供目标 RPM / 控制输出 getter，供上层故障诊断与状态上报使用。
*/
void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max) {
// 状态初始化
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
// 参数赋值
#define LADRC_CONTROL_DT_S 0.01f
#define LADRC_RPM_FILTER_ALPHA 0.30f
#define LADRC_DEFAULT_WC 25.0f
#define LADRC_DEFAULT_WO 60.0f
#define LADRC_DEFAULT_B0 0.20f
#define LADRC_DEFAULT_OUT_MAX 1000.0f
/* ================== 内部辅助函数 ================== */
static float LADRC_Abs(float x);
static float LADRC_Clamp(float x, float min_value, float max_value);
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;
}
/**
* @brief 初始化单个 LADRC 控制器。
* @note 顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
*/
void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max)
{
if (ladrc == NULL)
{
return;
}
if (h <= 0.0f)
{
h = LADRC_CONTROL_DT_S;
}
if (LADRC_Abs(b0) < 1e-6f)
{
b0 = LADRC_DEFAULT_B0;
}
if (wc < 0.0f)
{
wc = -wc;
}
if (wo < 0.0f)
{
wo = -wo;
}
if (max <= 0.0f)
{
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->wc = wc;
ladrc->wo = wo;
ladrc->b0 = b0;
ladrc->h = h;
ladrc->out_max = max;
// 计算观测器增益
ladrc->beta1 = 2.0f * wo;
ladrc->beta2 = wo * wo;
}
/**
* @brief 单体 LADRC 核心运算
* @brief 单个 LADRC 周期运算。
* @param ladrc 控制器实例
* @param actual_val 当前测量速度（RPM）
* @retval 限幅后的控制输出（通常可直接映射到 PWM）
*/
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val) {
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
{
float e;
float u0;
float out;
if (ladrc == NULL)
{
return 0.0f;
}
ladrc->y = actual_val;
// 第一部分：LESO
float e = ladrc->y - ladrc->z1;
/* 第 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;
// 第二部分：LSEF & 扰动补偿
float u0 = ladrc->wc * (ladrc->v - ladrc->z1);
float out = (u0 - ladrc->z2) / ladrc->b0;
// 第三部分：输出限幅保护
if (out > ladrc->out_max) out = ladrc->out_max;
if (out < -ladrc->out_max) out = -ladrc->out_max;
ladrc->u = out;
/* 第 2 部分：LSEF + 扰动补偿 */
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
out = (u0 - ladrc->z2) / ladrc->b0;
/* 第 3 部分：输出限幅，保护电机和驱动 */
out = LADRC_Clamp(out, -ladrc->out_max, ladrc->out_max);
ladrc->u = out;
return out;
}
/* =====================================================================
* 针对四轮底盘的 LADRC 扩展调度层
* 针对四轮底盘的 LADRC 调度层
* ===================================================================== */
// 实例化 4 个控制器的实体
LADRC_TypeDef ladrc_motors[4];
/**
* @brief 一键初始化底层硬件与四个 LADRC
* @brief 一键初始化底层硬件和四个 LADRC 控制器。
*/
void FourWheel_LADRC_Init(void)
{
// 1. 唤醒底层电机与编码器硬件
int i;
Motor_Init();
// 2. 初始化 4 个控制器的参数 (注意这里的限幅是 1000，匹配底层的 PWM_LIMIT)
for(int i = 0; i < 4; i++) {
LADRC_Init(&ladrc_motors[i], 25.0f, 60.0f, 0.2f, 0.01f, 1000.0f);
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);
}
}
/**
* @brief 一键下发四个轮子的目标转速
* @brief 设置四个轮子的目标 RPM。
*/
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
{
@@ -81,29 +160,57 @@ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr
}
/**
* @brief 四轮闭环大循环 (强烈建议放在 10ms 的定时器中断内执行)
* @brief 四轮 LADRC 大循环，建议固定 10ms 调用一次。
* @note 调度顺序：
* 1) 更新编码器测速
* 2) 一阶低通滤波，减小测速毛刺
* 3) 每个轮子独立做 LADRC
* 4) 输出到 H 桥
*/
void FourWheel_LADRC_Control_Loop(void)
{
Motor_Update_RPM(0.01f);
// 静态数组，用来记住上一次的平滑速度
static float filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
// 滤波系数 alpha (0.0 ~ 1.0)。越小越平滑，越大越敏捷。
// 0.3 的意思是：相信 30% 的最新数据，保留 70% 的历史数据
const float alpha = 0.3f;
int i;
for(int i = 0; i < 4; i++)
Motor_Update_RPM(LADRC_CONTROL_DT_S);
for (i = 0; i < 4; ++i)
{
// 1. 拿到底层的原始狂野转速
float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
float pwm_out;
// 2. 软件减震器：一阶低通滤波！
filtered_rpm[i] = (1.0f - alpha) * filtered_rpm[i] + alpha * raw_rpm;
// 3. 把过滤掉毛刺、极其顺滑的速度扔给 LADRC
float pwm_out = LADRC_Calc(&ladrc_motors[i], filtered_rpm[i]);
filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * filtered_rpm[i]
+ LADRC_RPM_FILTER_ALPHA * raw_rpm;
pwm_out = LADRC_Calc(&ladrc_motors[i], 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))
{
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))
{
return 0.0f;
}
return ladrc_motors[id].u;
}