diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index afbcb62..562bf49 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -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], ¤t_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], ¤t_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); // 左腿控制 { diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index ecabacc..2769e72 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -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; diff --git a/User/module/config.c b/User/module/config.c index 586df6c..af5bd5e 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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, diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 3f53e2a..e719a74 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -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);