From fe6a0280334d10ecaa561b5791c85b2597f23d5a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 2 Oct 2025 21:54:50 +0800 Subject: [PATCH] =?UTF-8?q?=E7=AC=AC=E4=B8=80=E6=AC=A1=E6=94=B9lqr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/FreeRTOSConfig.h | 2 +- User/component/lqr.c | 175 ++++++++-------------------------- User/component/lqr.h | 97 +++---------------- User/component/user_math.c | 4 +- User/module/balance_chassis.c | 35 +++---- User/module/balance_chassis.h | 8 +- User/module/config.c | 2 - User/task/rc.c | 2 +- 8 files changed, 68 insertions(+), 257 deletions(-) diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h index 16c89ac..fee994f 100644 --- a/Core/Inc/FreeRTOSConfig.h +++ b/Core/Inc/FreeRTOSConfig.h @@ -187,7 +187,7 @@ standard names. */ /* USER CODE BEGIN Defines */ /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */ -#define configAPPLICATION_ALLOCATED_HEAP 1 +// #define configAPPLICATION_ALLOCATED_HEAP 1 /* USER CODE END Defines */ #endif /* FREERTOS_CONFIG_H */ diff --git a/User/component/lqr.c b/User/component/lqr.c index 827f262..ae3c309 100644 --- a/User/component/lqr.c +++ b/User/component/lqr.c @@ -1,70 +1,32 @@ /* - * LQR控制器实现 + * LQR控制器实现：单腿建模 */ #include "lqr.h" #include +#include /* Private macros ----------------------------------------------------------- */ #define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x))) /* Private variables -------------------------------------------------------- */ -/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */ -/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */ -static LQR_GainMatrix_t default_gain_matrix = { - /* K矩阵第一行 - 轮毂力矩T的增益系数 */ - .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* theta */ - .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_theta */ - .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* x */ - .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* d_x */ - .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* phi */ - .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_phi */ - - /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */ - .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* theta */ - .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_theta */ - .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* x */ - .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* d_x */ - .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* phi */ - .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_phi */ -}; - /* Private function prototypes ---------------------------------------------- */ -static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr); -static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr); +static int8_t LQR_CalculateErrorState(LQR_t *lqr); +static float LQR_PolynomialCalc(const float *coeff, float leg_length); /* Public functions --------------------------------------------------------- */ -int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) { - if (lqr == NULL) { +int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) { + if (lqr == NULL || gain_matrix == NULL) { return -1; } /* 清零结构体 */ - memset(lqr, 0, sizeof(LQR_Controller_t)); + memset(lqr, 0, sizeof(LQR_t)); - /* 设置控制限制 */ - lqr->param.max_wheel_torque = max_wheel_torque; - lqr->param.max_joint_torque = max_joint_torque; - - /* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */ - lqr->param.R = 0.072f; /* 驱动轮半径 */ - lqr->param.l = 0.03f; /* 机体重心到转轴距离 */ - lqr->param.mw = 0.60f; /* 驱动轮质量 */ - lqr->param.mp = 1.8f; /* 摆杆质量 */ - lqr->param.M = 1.8f; /* 机体质量 */ - lqr->param.g = 9.8f; /* 重力加速度 */ - - /* 计算转动惯量 */ - lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R; - lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f; - - /* 设置默认增益矩阵 */ - lqr->gain_matrix = &default_gain_matrix; - - /* 设置默认目标状态（平衡状态） */ - memset(&lqr->param.target_state, 0, sizeof(LQR_State_t)); + /* 设置增益矩阵 */ + lqr->gain_matrix = gain_matrix; /* 初始化当前腿长为中等值 */ lqr->current_leg_length = 0.25f; @@ -77,41 +39,30 @@ int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_t return 0; } -int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) { - if (lqr == NULL || gain_matrix == NULL) { - return -1; - } - - lqr->gain_matrix = gain_matrix; - - /* 重新计算增益矩阵 */ - return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length); -} - -int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) { - if (lqr == NULL || state == NULL) { +int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state) { + if (lqr == NULL || current_state == NULL) { return -1; } /* 更新当前状态 */ - lqr->current_state = *state; + lqr->current_state = *current_state; /* 计算状态误差 */ return LQR_CalculateErrorState(lqr); } -int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) { +int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) { if (lqr == NULL || target_state == NULL) { return -1; } - lqr->param.target_state = *target_state; + lqr->target_state = *target_state; /* 重新计算状态误差 */ return LQR_CalculateErrorState(lqr); } -int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) { +int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) { if (lqr == NULL || lqr->gain_matrix == NULL) { return -1; } @@ -120,14 +71,6 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) { leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f); lqr->current_leg_length = leg_length; - /* 更新与腿长相关的物理参数 */ - lqr->param.L = leg_length / 2.0f; /* 摆杆重心到驱动轮轴距离 */ - lqr->param.LM = leg_length / 2.0f; /* 摆杆重心到其转轴距离 */ - - /* 计算摆杆转动惯量 */ - float leg_total_length = lqr->param.L + lqr->param.LM; - lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f; - /* 使用多项式拟合计算当前增益矩阵 */ lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */ lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */ @@ -146,7 +89,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) { return 0; } -int8_t LQR_Control(LQR_Controller_t *lqr) { +int8_t LQR_Control(LQR_t *lqr) { if (lqr == NULL || !lqr->initialized) { return -1; } @@ -175,38 +118,42 @@ int8_t LQR_Control(LQR_Controller_t *lqr) { lqr->K_matrix[1][5] * lqr->error_state.d_phi ); - /* 应用控制限制 */ - return LQR_ApplyControlLimits(lqr); -} - -int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) { - if (lqr == NULL || output == NULL) { - return -1; - } - - *output = lqr->control_output; return 0; } -int8_t LQR_Reset(LQR_Controller_t *lqr) { +int8_t LQR_Reset(LQR_t *lqr) { if (lqr == NULL) { return -1; } /* 清零状态和输出 */ memset(&lqr->current_state, 0, sizeof(LQR_State_t)); + memset(&lqr->target_state, 0, sizeof(LQR_State_t)); memset(&lqr->error_state, 0, sizeof(LQR_State_t)); - memset(&lqr->control_output, 0, sizeof(LQR_Input_t)); - memset(&lqr->param.target_state, 0, sizeof(LQR_State_t)); - - /* 重置限幅标志 */ - lqr->wheel_torque_limited = false; - lqr->joint_torque_limited = false; + memset(&lqr->control_output, 0, sizeof(LQR_Output_t)); return 0; } -float LQR_PolynomialCalc(const float *coeff, float leg_length) { +/* Private functions -------------------------------------------------------- */ + +static int8_t LQR_CalculateErrorState(LQR_t *lqr) { + if (lqr == NULL) { + return -1; + } + + /* 计算状态误差 */ + lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta; + lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta; + lqr->error_state.x = lqr->current_state.x - lqr->target_state.x; + lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x; + lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi; + lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi; + + return 0; +} + +static float LQR_PolynomialCalc(const float *coeff, float leg_length) { if (coeff == NULL) { return 0.0f; } @@ -218,49 +165,3 @@ float LQR_PolynomialCalc(const float *coeff, float leg_length) { return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3]; } - -/* Private functions -------------------------------------------------------- */ - -static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) { - if (lqr == NULL) { - return -1; - } - - /* 计算状态误差 */ - lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta; - lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta; - lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x; - lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x; - lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi; - lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi; - - return 0; -} - -static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) { - if (lqr == NULL) { - return -1; - } - - /* 重置限幅标志 */ - lqr->wheel_torque_limited = false; - lqr->joint_torque_limited = false; - - /* 限制轮毂力矩 */ - if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) { - lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, - -lqr->param.max_wheel_torque, - lqr->param.max_wheel_torque); - lqr->wheel_torque_limited = true; - } - - /* 限制髋关节力矩 */ - if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) { - lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp, - -lqr->param.max_joint_torque, - lqr->param.max_joint_torque); - lqr->joint_torque_limited = true; - } - - return 0; -} diff --git a/User/component/lqr.h b/User/component/lqr.h index 59506e2..e05f224 100644 --- a/User/component/lqr.h +++ b/User/component/lqr.h @@ -1,6 +1,6 @@ /* * LQR控制器 - * 用于轮腿平衡机器人的线性二次调节器控制 + * 用于轮腿平衡机器人的线性二次调节器控制：单腿建模 */ #pragma once @@ -21,16 +21,6 @@ extern "C" { /* Exported types ----------------------------------------------------------- */ -/** - * @brief LQR状态向量 - * 状态定义： - * theta: 摆杆与竖直方向夹角 (rad) - * d_theta: 摆杆角速度 (rad/s) - * x: 驱动轮位移 (m) - * d_x: 驱动轮速度 (m/s) - * phi: 机体与水平夹角 (rad) - * d_phi: 机体角速度 (rad/s) - */ typedef struct { float theta; /* 摆杆角度 */ float d_theta; /* 摆杆角速度 */ @@ -40,19 +30,11 @@ typedef struct { float d_phi; /* 机体俯仰角速度 */ } LQR_State_t; -/** - * @brief LQR控制输入向量 - */ typedef struct { float T; /* 轮毂力矩 (N·m) */ float Tp; /* 髋关节力矩 (N·m) */ -} LQR_Input_t; +} LQR_Output_t; -/** - * @brief LQR增益矩阵参数 - * K矩阵的每个元素的多项式拟合系数 - * K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3] - */ typedef struct { /* K矩阵第一行(轮毂力矩T对应的增益) */ float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */ @@ -71,41 +53,18 @@ typedef struct { float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */ } LQR_GainMatrix_t; -/** - * @brief LQR控制器参数 - */ -typedef struct { - /* 系统物理参数 */ - float R; /* 驱动轮半径 (m) */ - float L; /* 摆杆重心到驱动轮轴距离 (m) */ - float LM; /* 摆杆重心到其转轴距离 (m) */ - float l; /* 机体重心到其转轴距离 (m) */ - float mw; /* 驱动轮质量 (kg) */ - float mp; /* 摆杆质量 (kg) */ - float M; /* 机体质量 (kg) */ - float Iw; /* 驱动轮转动惯量 (kg·m²) */ - float Ip; /* 摆杆绕质心转动惯量 (kg·m²) */ - float IM; /* 机体绕质心转动惯量 (kg·m²) */ - float g; /* 重力加速度 (m/s²) */ - - /* 控制限制 */ - float max_wheel_torque; /* 轮毂电机最大力矩 (N·m) */ - float max_joint_torque; /* 关节电机最大力矩 (N·m) */ - - /* 目标状态 */ - LQR_State_t target_state; /* 目标状态 */ -} LQR_Param_t; /** * @brief LQR控制器主结构体 */ typedef struct { - LQR_Param_t param; /* 控制器参数 */ LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */ LQR_State_t current_state; /* 当前状态 */ + LQR_State_t target_state; /* 目标状态 */ LQR_State_t error_state; /* 状态误差 */ - LQR_Input_t control_output; /* 控制输出 */ + + LQR_Output_t control_output; /* 控制输出 */ /* 运行时变量 */ float current_leg_length; /* 当前腿长 */ @@ -113,10 +72,7 @@ typedef struct { bool initialized; /* 初始化标志 */ - /* 限幅标志 */ - bool wheel_torque_limited; /* 轮毂力矩是否被限幅 */ - bool joint_torque_limited; /* 关节力矩是否被限幅 */ -} LQR_Controller_t; +} LQR_t; /* Exported functions prototypes -------------------------------------------- */ @@ -124,20 +80,11 @@ typedef struct { * @brief 初始化LQR控制器 * * @param lqr LQR控制器结构体指针 - * @param max_wheel_torque 轮毂电机最大力矩 - * @param max_joint_torque 关节电机最大力矩 - * @return int8_t 0-成功, 其他-失败 - */ -int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque); - -/** - * @brief 设置LQR增益矩阵参数 - * - * @param lqr LQR控制器结构体指针 * @param gain_matrix 增益矩阵参数指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix); +int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix); + /** * @brief 更新当前状态 @@ -146,7 +93,7 @@ int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix); * @param state 当前状态 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state); +int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state); /** * @brief 设置目标状态 @@ -155,7 +102,7 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state); * @param target_state 目标状态 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state); +int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state); /** * @brief 根据当前腿长计算增益矩阵 @@ -164,7 +111,7 @@ int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state * @param leg_length 当前腿长 (m) * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length); +int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length); /** * @brief 执行LQR控制计算 @@ -172,16 +119,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length); * @param lqr LQR控制器结构体指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_Control(LQR_Controller_t *lqr); - -/** - * @brief 获取控制输出 - * - * @param lqr LQR控制器结构体指针 - * @param output 输出控制指令指针 - * @return int8_t 0-成功, 其他-失败 - */ -int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output); +int8_t LQR_Control(LQR_t *lqr); /** * @brief 重置LQR控制器 @@ -189,16 +127,7 @@ int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output); * @param lqr LQR控制器结构体指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_Reset(LQR_Controller_t *lqr); - -/** - * @brief 根据多项式系数计算增益值 - * - * @param coeff 多项式系数数组 - * @param leg_length 腿长 - * @return float 增益值 - */ -float LQR_PolynomialCalc(const float *coeff, float leg_length); +int8_t LQR_Reset(LQR_t *lqr); #ifdef __cplusplus } diff --git a/User/component/user_math.c b/User/component/user_math.c index 49a4723..084bef3 100644 --- a/User/component/user_math.c +++ b/User/component/user_math.c @@ -51,8 +51,8 @@ inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 * 例如编码器：相差1.5PI其实等于相差-0.5PI * - * \param sp 被操作的值 - * \param fb 变化量 + * \param sp 设定值 + * \param fb 反馈值 * \param range 被操作的值变化范围，正数时起效 * * \return 函数运行结果 diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index ea22b7f..3bffb46 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1,3 +1,4 @@ +#define _USE_MATH_DEFINES #include "module/balance_chassis.h" #include "bsp/time.h" #include "bsp/can.h" @@ -6,6 +7,7 @@ #include "component/limiter.h" #include #include +#include /** * @brief 使能所有电机 @@ -139,10 +141,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp); /*初始化LQR控制器*/ - LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); - LQR_SetGainMatrix(&c->lqr[0], ¶m->lqr_gains); - LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); - LQR_SetGainMatrix(&c->lqr[1], ¶m->lqr_gains); + LQR_Init(&c->lqr[0], ¶m->lqr_gains); + LQR_Init(&c->lqr[1], ¶m->lqr_gains); /*初始化机体状态*/ c->chassis_state.position_x = 0.0f; @@ -262,13 +262,8 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); float fn; - float tp1, tp2; fn = -25.0f; - - // tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); - // tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt); - VMC_InverseSolve(&c->vmc_[0], fn, 0.0f); VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); VMC_InverseSolve(&c->vmc_[1], fn, 0.0f); @@ -336,13 +331,13 @@ void Chassis_Output(Chassis_t *c) { if (c == NULL) return; for (int i = 0; i < 4; i++) { MOTOR_LZ_MotionParam_t param = {0}; - // param.torque = c->output.joint[i].torque; + param.torque = c->output.joint[i].torque; MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); } BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成 for (int i = 0; i < 2; i++) { - // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); - MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); + MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); + // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); } } @@ -402,7 +397,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { target_state.d_theta = 0.0f; // 目标摆杆角速度 // target_state.x = 0; // 目标位置 // target_state.d_x = 0.0f; // 目标速度 - target_state.phi = -0.2f; // 目标俯仰角 + target_state.phi = -0.1f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 // target_state.d_theta = 0.0f; // 目标摆杆角速度 @@ -421,16 +416,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { if (onground_flag[leg]) { /* 接地状态：使用LQR控制器计算输出 */ if (LQR_Control(&c->lqr[leg]) == 0) { - LQR_Input_t lqr_output = {0}; - if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) { - Tw[leg] = lqr_output.T; - Tp[leg] = lqr_output.Tp; - // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出 - // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出 - } else { - Tw[leg] = 0.0f; - Tp[leg] = 0.0f; - } + Tw[leg] = c->lqr[leg].control_output.T; + Tp[leg] = c->lqr[leg].control_output.Tp; + // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出 + // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出 } else { Tw[leg] = 0.0f; Tp[leg] = 0.0f; diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index d102d34..64669fa 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -75,12 +75,6 @@ typedef struct { MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */ VMC_Param_t vmc_param[2]; /* VMC参数 */ LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */ - - /* LQR控制器参数 */ - struct { - float max_wheel_torque; /* 轮毂电机最大力矩限制 */ - float max_joint_torque; /* 关节电机最大力矩限制 */ - } lqr_param; KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */ KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */ @@ -117,7 +111,7 @@ typedef struct { Chassis_Output_t output; VMC_t vmc_[2]; /* 两条腿的VMC */ - LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */ + LQR_t lqr[2]; /* 两条腿的LQR控制器 */ int8_t state; diff --git a/User/module/config.c b/User/module/config.c index 082cff4..25b2e0b 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -173,8 +173,6 @@ Config_RobotParam_t robot_config = { .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi }, - .lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm - .lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm }, .rc_can_param = { diff --git a/User/task/rc.c b/User/task/rc.c index c6a67c9..5a13b90 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -52,7 +52,7 @@ void Task_rc(void *argument) { cmd_to_chassis.mode = CHASSIS_MODE_RECOVER; break; case RC_CAN_SW_DOWN: // 下位 - cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE; + cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; break; default: cmd_to_chassis.mode = CHASSIS_MODE_RELAX;