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-09-17 07:10:10 +08:00
parent e7cccbf706
commit de8bc5ac8c
3 changed files with 36 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
User/module/balance_chassis.c
View File

@ -229,7 +229,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮腿平衡模式，使用LQR控制
// 轮腿平衡模式，使用LQR控制和PID腿长控制
// 更新VMC正解算
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
@ -237,15 +237,13 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// 执行LQR控制（包含PID腿长控制）
if (Chassis_LQRControl(c, c_cmd) != 0) {
// LQR控制失败，切换到安全模式
return CHASSIS_ERR;
}
// 执行LQR控制
// if (Chassis_LQRControl(c, c_cmd) != 0) {
// // LQR控制失败，切换到安全模式
// return CHASSIS_ERR;
// }
// Chassis_Output(c); // 统一输出
Chassis_Output(c); // 统一输出
break;
default:
@ -282,11 +280,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float leg_fn[2];
bool onground_flag[2];
// 简化的地面接触检测，可以后续改进
// 暂时关闭离地监测，强制设置为着地状态
leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]);
leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]);
onground_flag[0] = (leg_fn[0] > 30.0f);
onground_flag[1] = (leg_fn[1] > 30.0f);
onground_flag[0] = true; // 强制设置左腿着地
onground_flag[1] = true; // 强制设置右腿着地
/* 获取VMC状态（等效摆杆参数） */
float leg_L0[2], leg_theta[2], leg_d_theta[2];
@ -383,8 +381,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float leg_d_length[2]; // 腿长变化率
// 目标腿长设定
target_L0[0] = 0.13f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节
target_L0[1] = 0.13f + c_cmd->height * 0.1f;
target_L0[0] = 0.12f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节
target_L0[1] = 0.12f + c_cmd->height * 0.1f;
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@ -397,7 +395,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[leg] = Tp[leg] * sinf(leg_theta[leg]) +
pid_output + // PID腿长控制输出
50.0f; // 基础支撑力
35.0f; // 基础支撑力
// VMC逆解算
if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) {

11
User/module/balance_chassis.h
View File

@ -70,7 +70,8 @@ typedef struct {
typedef struct {
VMC_Param_t vmc_param[2]; /* VMC参数 */
KPID_Params_t leglength;
KPID_Params_t leglength; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
@ -127,6 +128,13 @@ typedef struct {
float last_velocity_x; /* 上一次速度用于数值微分 */
} chassis_state;
/* yaw控制相关 */
struct {
float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */
} yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
@ -140,6 +148,7 @@ typedef struct {
KPID_t right_wheel; /* 右轮PID */
KPID_t follow; /* 跟随云台用的PID */
KPID_t balance; /* 平衡用的PID */
KPID_t yaw; /* yaw轴控制PID */
KPID_t leglength[2]; /* 两条腿的腿长PID */

16
User/module/config.c
View File

@ -70,14 +70,24 @@ Config_RobotParam_t robot_config = {
.chassis_param = {
.leglength={
.k=1.0f,
.p=0.10f,
.p=10.0f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=100.0f,
.out_limit=10.0f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
}
},
.yaw={
.k=0.0f,
.p=0.1f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=0.2f,
.d_cutoff_freq=30.0f,
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
},
.low_pass_cutoff_freq = {