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

抽象参数

Browse Source
This commit is contained in:
Robofish 2025-11-03 01:51:01 +08:00
parent a0908af220
commit b1300f0d7d
4 changed files with 62 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
User/module/balance_chassis.c
View File

@ -88,6 +88,10 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);
LowPassFilter2p_Reset(&c->filter.velocity_est, 0.0f);
LowPassFilter2p_Reset(&c->filter.phi, 0.0f);
LowPassFilter2p_Reset(&c->filter.d_phi, 0.0f);
LowPassFilter2p_Reset(&c->filter.d_theta[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.d_theta[1], 0.0f);
c->mode = mode;
c->wz_multi=0.0f;
@ -221,6 +225,18 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
/* 初始化速度估计滤波器 */
LowPassFilter2p_Init(&c->filter.velocity_est, target_freq,
param->low_pass_cutoff_freq.velocity_est);
/* 初始化俯仰角和俯仰角速度滤波器 */
LowPassFilter2p_Init(&c->filter.phi, target_freq,
param->low_pass_cutoff_freq.phi);
LowPassFilter2p_Init(&c->filter.d_phi, target_freq,
param->low_pass_cutoff_freq.d_phi);
/* 初始化摆杆角速度滤波器 */
LowPassFilter2p_Init(&c->filter.d_theta[0], target_freq,
param->low_pass_cutoff_freq.d_theta);
LowPassFilter2p_Init(&c->filter.d_theta[1], target_freq,
param->low_pass_cutoff_freq.d_theta);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
@ -406,16 +422,24 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 构建当前状态 */
LQR_State_t current_state = {0};
current_state.theta = c->vmc_[0].leg.theta;
current_state.d_theta = c->vmc_[0].leg.d_theta;
current_state.d_theta = LowPassFilter2p_Apply(&c->filter.d_theta[0], c->vmc_[0].leg.d_theta);
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = -c->feedback.imu.euler.pit;
current_state.d_phi = -c->feedback.imu.gyro.y;
/* 使用滤波后的俯仰角和俯仰角速度 */
current_state.phi = LowPassFilter2p_Apply(&c->filter.phi, -c->feedback.imu.euler.pit);
current_state.d_phi = LowPassFilter2p_Apply(&c->filter.d_phi, -c->feedback.imu.gyro.y);
/* 保存滤波后的摆杆角速度，用于后续计算 */
float filtered_d_theta[2];
filtered_d_theta[0] = current_state.d_theta; // 左腿已经滤波过的d_theta
LQR_UpdateState(&c->lqr[0], &current_state);
current_state.theta = c->vmc_[1].leg.theta;
current_state.d_theta = c->vmc_[1].leg.d_theta;
current_state.d_theta = LowPassFilter2p_Apply(&c->filter.d_theta[1], c->vmc_[1].leg.d_theta);
filtered_d_theta[1] = current_state.d_theta; // 右腿已经滤波过的d_theta
LQR_UpdateState(&c->lqr[1], &current_state);
/* 根据当前腿长更新增益矩阵 */
@ -492,7 +516,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.16f) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->lqr[0].K_matrix[1][1] * (-filtered_d_theta[0] + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
if (c->vmc_[1].leg.is_ground_contact){
@ -504,7 +528,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.16f) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->lqr[1].K_matrix[1][1] * (-filtered_d_theta[1] + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
@ -527,15 +551,15 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 限制补偿长度范围，防止过度补偿
// 如果任一腿离地，限制补偿长度
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_length > 0.02f)
roll_compensation_length = 0.02f;
if (roll_compensation_length < -0.02f)
roll_compensation_length = -0.02f;
if (roll_compensation_length > 0.10f)
roll_compensation_length = 0.10f;
if (roll_compensation_length < -0.10f)
roll_compensation_length = -0.10f;
} else {
if (roll_compensation_length > 0.05f)
roll_compensation_length = 0.05f;
if (roll_compensation_length < -0.05f)
roll_compensation_length = -0.05f;
if (roll_compensation_length > 0.15f)
roll_compensation_length = 0.15f;
if (roll_compensation_length < -0.15f)
roll_compensation_length = -0.15f;
}
// 目标腿长设定（包含横滚角补偿）
@ -558,11 +582,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
c->vmc_[0].leg.theta, filtered_d_theta[0], c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
c->vmc_[1].leg.theta, filtered_d_theta[1], c->dt);
// 左腿控制
{

6
User/module/balance_chassis.h
View File

@ -94,6 +94,9 @@ typedef struct {
float joint; /* 关节电机输出 */
float wheel; /* 轮子电机输出 */
float velocity_est; /* 速度估计滤波器 */
float phi; /* 俯仰角滤波器 */
float d_phi; /* 俯仰角速度滤波器 */
float d_theta; /* 摆杆角速度滤波器 */
} low_pass_cutoff_freq;
} Chassis_Params_t;
@ -162,6 +165,9 @@ typedef struct {
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */
LowPassFilter2p_t phi; /* 俯仰角滤波器 */
LowPassFilter2p_t d_phi; /* 俯仰角速度滤波器 */
LowPassFilter2p_t d_theta[2]; /* 摆杆角速度滤波器，左右腿各一个 */
} filter;
} Chassis_t;

26
User/module/config.c
View File

@ -237,6 +237,8 @@ Config_RobotParam_t robot_config = {
.joint = 30.0f, // 关节电机滤波器截止频率
.wheel = 50.0f, // 轮子电机滤波器截止频率
.velocity_est = 10.0f, // 速度估计滤波器截止频率
.phi = 30.0f, // 俯仰角滤波器截止频率
.d_phi = 30.0f, // 俯仰角
},
.yaw_motor = { // 云台Yaw轴电机
@ -267,18 +269,18 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -2.521608956196071e+02f, 2.726920584021966e+02f, -1.226029917752676e+02f, -3.658425589113113e+00f }, // theta
.k12_coeff = { -2.791745431179899e+00f, 2.977171588171382e+00f, -8.482847921716969e+00f, -2.549251989061084e-01f }, // d_theta
.k13_coeff = { -7.117984035711224e+01f, 7.136358210118603e+01f, -2.482097503246936e+01f, -1.465382594689337e+00f }, // x
.k14_coeff = { -5.096413946871127e+01f, 5.158481633857484e+01f, -1.949187011275690e+01f, -1.578480316034549e+00f }, // d_x
.k15_coeff = { -5.143495490166011e+01f, 6.689655927757387e+01f, -3.271512900728912e+01f, 6.773814929578457e+00f }, // phi
.k16_coeff = { -1.834269104867269e+01f, 2.340972691877904e+01f, -1.144818100011914e+01f, 2.495597855798016e+00f }, // d_phi
.k21_coeff = { 7.774379313049623e+01f, -2.541134089584162e+01f, -2.498975065689273e+01f, 1.426556516484627e+01f }, // theta
.k22_coeff = { 2.767476224142276e+00f, 2.139939778695216e-02f, -1.895904297557682e+00f, 1.658428058979244e+00f }, // d_theta
.k23_coeff = { -7.402087120213977e+01f, 9.414895415370154e+01f, -4.552133085081052e+01f, 9.802187964966528e+00f }, // x
.k24_coeff = { -7.810684593036375e+01f, 9.325962600072852e+01f, -4.231509773607639e+01f, 8.736511218580238e+00f }, // d_x
.k25_coeff = { 1.628215249943350e+02f, -1.638967569586973e+02f, 5.714874400700918e+01f, 5.761974669276071e+00f }, // phi
.k26_coeff = { 5.976470654409622e+01f, -6.012998591768613e+01f, 2.100604855465028e+01f, 1.219608179868111e+00f }, // d_phi
.k11_coeff = { -2.569072693898239e+02f, 2.779555755091039e+02f, -1.222426847901187e+02f, -3.002573318155172e+00f }, // theta
.k12_coeff = { -3.247618420086355e+00f, 3.872911234165093e+00f, -8.119632968110409e+00f, -1.957273727935414e-01f }, // d_theta
.k13_coeff = { -5.954584477716254e+01f, 5.959511956695892e+01f, -2.066442637256297e+01f, -9.161871734864370e-01f }, // x
.k14_coeff = { -5.009246130273538e+01f, 5.050784505092143e+01f, -1.866307280192490e+01f, -1.111173197848232e+00f }, // d_x
.k15_coeff = { -3.764642131668238e+01f, 4.923720544894766e+01f, -2.404635094807352e+01f, 4.874788569972004e+00f }, // phi
.k16_coeff = { -1.794650886553529e+01f, 2.322861807673308e+01f, -1.145809045948791e+01f, 2.495007469834424e+00f }, // d_phi
.k21_coeff = { 9.642944467074803e+01f, -3.922704773721851e+01f, -2.296214867229078e+01f, 1.456585213532125e+01f }, // theta
.k22_coeff = { 2.539127229568287e+00f, 4.072407366772993e-01f, -2.149996585958249e+00f, 1.685944897687379e+00f }, // d_theta
.k23_coeff = { -6.099246751150465e+01f, 7.843903088544945e+01f, -3.823324746035261e+01f, 8.252965396092685e+00f }, // x
.k24_coeff = { -7.020010754832062e+01f, 8.529669239744973e+01f, -3.934602729745466e+01f, 8.196715048739026e+00f }, // d_x
.k25_coeff = { 1.330140568816526e+02f, -1.334911798369293e+02f, 4.629003116303056e+01f, 4.254186686006904e+00f }, // phi
.k26_coeff = { 6.836307472445371e+01f, -6.864899432084550e+01f, 2.390040226199122e+01f, 1.101236673854717e+00f }, // d_phi
},
.theta = 0.0f,
.x = 0.3f,

4
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([600 100 3000 200 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[140 0;0 40]; %T Tp
Q=diag([500 100 2000 200 5000 600]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[160 0;0 40]; %T Tp
K=lqr(A,B,Q,R);