robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

第一次改lqr

Browse Source
This commit is contained in:
Robofish 2025-10-02 21:54:50 +08:00
parent 0b74f21104
commit fe6a028033
8 changed files with 68 additions and 257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/FreeRTOSConfig.h
View File

@ -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 */

175
User/component/lqr.c
View File

@ -1,70 +1,32 @@
/*
* LQR控制器实现
* LQR控制器实现：单腿建模
*/
#include "lqr.h"
#include <string.h>
#include <stddef.h>
/* 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;
}

97
User/component/lqr.h
View File

@ -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
}

4
User/component/user_math.c
View File

@ -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 函数运行结果

35
User/module/balance_chassis.c
View File

@ -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 <math.h>
#include <string.h>
#include <stddef.h>
/**
* @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, &param->tp);
/*初始化LQR控制器*/
LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[1], &param->lqr_gains);
LQR_Init(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->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], &param);
}
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;

8
User/module/balance_chassis.h
View File

@ -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;

2
User/module/config.c
View File

@ -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 = {

2
User/task/rc.c
View File

@ -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;