From 51fe29f605879981902b7dca483f6cdcff58c902 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 03:45:56 +0800 Subject: [PATCH] =?UTF-8?q?=E7=89=9B=E9=80=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 149 +++++++++++++++++----------------- 1 file changed, 76 insertions(+), 73 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index a6375f3..cd1d833 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1,16 +1,15 @@ -#include "device/motor_lz.h" +#include "module/balance_chassis.h" #include "bsp/can.h" #include "bsp/time.h" #include "component/kinematics.h" #include "component/limiter.h" #include "component/user_math.h" +#include "device/motor_lz.h" #include "device/virtual_chassis.h" -#include "module/balance_chassis.h" #include #include #include - static Virtual_Chassis_t virtual_chassis; /** @@ -149,7 +148,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) { return -1; // 参数错误 } Virtual_Chassis_Update(&virtual_chassis); - + for (int i = 0; i < 4; i++) { c->feedback.joint[i] = virtual_chassis.data.joint[i]; if (c->feedback.joint[i].rotor_abs_angle > M_PI) { @@ -194,10 +193,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, - -c->feedback.imu.euler.rol, -c->feedback.imu.gyro.y); + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, - -c->feedback.imu.euler.rol, -c->feedback.imu.gyro.y); + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); /*根据底盘模式执行不同的控制逻辑*/ switch (c->mode) { @@ -217,20 +216,17 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_LQRControl(c, c_cmd); VMC_InverseSolve(&c->vmc_[0], fn, tp); - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], - &c->output.joint[1]); + VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); VMC_InverseSolve(&c->vmc_[1], fn, tp); - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], - &c->output.joint[2]); + VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); // Chassis_MotorEnable(c); - c->output.wheel[0] = c_cmd->move_vec.vx *0.2f; + c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f; - c->output.wheel[1] = c_cmd->move_vec.vx *0.2f; + c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f; Chassis_Output(c); // 统一输出 } break; - case CHASSIS_MODE_WHELL_LEG_BALANCE: // 执行LQR控制（包含PID腿长控制） Chassis_LQRControl(c, c_cmd); @@ -258,14 +254,15 @@ void Chassis_Output(Chassis_t *c) { // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); // // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); // } - float out[4] = { - c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]}; - // out[0] = 0.0f; - // out[1] = 0.0f; - // out[2] = 0.0f; - // out[3] = 0.0f; + float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2], + c->output.joint[3]}; + // out[0] = 0.0f; + // out[1] = 0.0f; + // out[2] = 0.0f; + // out[3] = 0.0f; Virtual_Chassis_SendJointTorque(&virtual_chassis, out); - Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]); + Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], + c->output.wheel[1]); // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f); // for (int i = 0; i < 2; i++) { } @@ -275,7 +272,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { return CHASSIS_ERR_NULL; } - /* 运动参数（参考C++版本的状态估计） */ static float xhat = 0.0f, x_dot_hat = 0.0f; float target_dot_x = 0.0f; @@ -291,51 +287,52 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->chassis_state.target_x += target_dot_x * c->dt; /* 分别计算左右腿的LQR控制 */ - /* 构建当前状态 */ - 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.x = xhat; - current_state.d_x = x_dot_hat; - current_state.phi = c->feedback.imu.euler.rol; - current_state.d_phi = c->feedback.imu.gyro.y; + /* 构建当前状态 */ + 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.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; - LQR_UpdateState(&c->lqr[0], ¤t_state); + 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; - LQR_UpdateState(&c->lqr[1], ¤t_state); + current_state.theta = c->vmc_[1].leg.theta; + current_state.d_theta = c->vmc_[1].leg.d_theta; + LQR_UpdateState(&c->lqr[1], ¤t_state); - /* 根据当前腿长更新增益矩阵 */ - LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); - LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); + /* 根据当前腿长更新增益矩阵 */ + LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); + LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); - /* 构建目标状态 */ - LQR_State_t target_state = {0}; - target_state.theta = 0.0f; // 目标摆杆角度 - target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = 0.0f; // 目标俯仰角 - target_state.d_phi = 0.0f; // 目标俯仰角速度 - target_state.x = c->chassis_state.target_x; // 目标位置 - target_state.d_x = target_dot_x; // 目标速度 - - /* 更新LQR状态 */ - LQR_SetTargetState(&c->lqr[0], &target_state); - LQR_SetTargetState(&c->lqr[1], &target_state); - - LQR_Control(&c->lqr[0]); - LQR_Control(&c->lqr[1]); + /* 构建目标状态 */ + LQR_State_t target_state = {0}; + target_state.theta = 0.0f; // 目标摆杆角度 + target_state.d_theta = 0.0f; // 目标摆杆角速度 + target_state.phi = -0.1f; // 目标俯仰角 + target_state.d_phi = 0.0f; // 目标俯仰角速度 + target_state.x = c->chassis_state.target_x; // 目标位置 + target_state.d_x = target_dot_x; // 目标速度 + /* 更新LQR状态 */ + LQR_SetTargetState(&c->lqr[0], &target_state); + LQR_SetTargetState(&c->lqr[1], &target_state); + LQR_Control(&c->lqr[0]); + LQR_Control(&c->lqr[1]); c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.target_yaw = c->param->mech_zero_yaw; - c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, + c->yaw_control.yaw_force = + PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); /* 轮毂力矩输出（参考C++版本的减速比） */ - c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; - c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; + c->output.wheel[0] = + c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; + c->output.wheel[1] = + c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f; // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f; /* 腿长控制和VMC逆解算（使用PID控制） */ @@ -345,15 +342,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 横滚角PID补偿计算 */ // 使用PID控制器计算横滚角补偿力，目标横滚角为0 - float roll_compensation_force = PID_Calc(&c->pid.roll, 0.0f, - c->feedback.imu.euler.rol, - c->feedback.imu.gyro.x, - c->dt); - + float roll_compensation_force = + PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, + c->feedback.imu.gyro.x, c->dt); + // 限制补偿力范围，防止过度补偿 - if (roll_compensation_force > 20.0f) roll_compensation_force = 20.0f; - if (roll_compensation_force < -20.0f) roll_compensation_force = -20.0f; - + if (roll_compensation_force > 20.0f) + roll_compensation_force = 20.0f; + if (roll_compensation_force < -20.0f) + roll_compensation_force = -20.0f; + // 目标腿长设定（移除横滚角补偿） target_L0[0] = 0.12f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节 target_L0[1] = 0.12f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节 @@ -367,19 +365,22 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 使用tp_pid进行左右腿摆角同步控制 // 左腿补偿力矩：使左腿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); + PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, + c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） Delta_Tp[1] = -Delta_Tp[0]; - + // 左腿控制 { // 使用PID进行腿长控制 float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0], c->vmc_[0].leg.L0, leg_d_length[0], c->dt); - // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力） - virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 40.0f ; + // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - + // 横滚角补偿力（左腿减少支撑力） + virtual_force[0] = + (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + + pid_output + 40.0f; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], @@ -388,8 +389,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { &c->output.joint[1]); } else { // VMC失败，设为0力矩 - c->output.joint[0]= 0.0f; - c->output.joint[1]= 0.0f; + c->output.joint[0] = 0.0f; + c->output.joint[1] = 0.0f; } } @@ -399,8 +400,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1], c->vmc_[1].leg.L0, leg_d_length[1], c->dt); - // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力） - virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 40.0f ; + // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + + // 横滚角补偿力（右腿增加支撑力） + virtual_force[1] = + (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + + pid_output + 40.0f; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], @@ -423,8 +427,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { for (int i = 0; i < 4; i++) { if (fabsf(c->output.joint[i]) > 20.0f) { - c->output.joint[i] = - (c->output.joint[i] > 0) ? 20.0f : -20.0f; + c->output.joint[i] = (c->output.joint[i] > 0) ? 20.0f : -20.0f; } }