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

牛逼

Browse Source
This commit is contained in:
Robofish 2025-10-07 03:45:56 +08:00
parent 1184b2d532
commit 51fe29f605
1 changed files with 76 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
User/module/balance_chassis.c
View File

@ -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 <math.h>
#include <stddef.h>
#include <string.h>
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], &current_state);
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;
LQR_UpdateState(&c->lqr[1], &current_state);
current_state.theta = c->vmc_[1].leg.theta;
current_state.d_theta = c->vmc_[1].leg.d_theta;
LQR_UpdateState(&c->lqr[1], &current_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;
}
}