diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index d6ae7cc..1a34274 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -28,7 +28,9 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { PID_Reset(&c->pid.leglength[0]); PID_Reset(&c->pid.leglength[1]); PID_Reset(&c->pid.yaw); - + PID_Reset(&c->pid.roll); + LQR_Reset(&c->lqr[0]); + LQR_Reset(&c->lqr[1]); c->mode = mode; c->state = 0; // 重置状态,确保每次切换模式时都重新初始化 @@ -88,6 +90,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, ¶m->leglength); PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, ¶m->leglength); PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); + PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); /*初始化LQR控制器*/ LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); @@ -417,9 +420,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float target_L0[2]; float leg_d_length[2]; // 腿长变化率 + /* 横滚角PID补偿计算 */ + float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt); + // 目标腿长设定 - target_L0[0] = 0.12f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节 - target_L0[1] = 0.12f + c_cmd->height * 0.1f; + target_L0[0] = 0.12f + c_cmd->height * 0.1f + roll_compensation; // 左腿:基础腿长 + 高度调节 + 横滚角补偿 + target_L0[1] = 0.12f + c_cmd->height * 0.1f - roll_compensation; // 右腿:基础腿长 + 高度调节 - 横滚角补偿 // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index b525f70..0dbffe9 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -72,6 +72,7 @@ typedef struct { VMC_Param_t vmc_param[2]; /* VMC参数 */ KPID_Params_t leglength; /* 腿长PID控制参数,用于控制虚拟腿长度 */ KPID_Params_t yaw; /* yaw轴PID控制参数,用于控制底盘朝向 */ + KPID_Params_t roll; /* roll轴PID控制参数,用于横滚角补偿 */ MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */ @@ -149,6 +150,7 @@ typedef struct { KPID_t follow; /* 跟随云台用的PID */ KPID_t balance; /* 平衡用的PID */ KPID_t yaw; /* yaw轴控制PID */ + KPID_t roll; /* 横滚角控制PID */ KPID_t leglength[2]; /* 两条腿的腿长PID */ diff --git a/User/module/config.c b/User/module/config.c index d70155a..4040620 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -88,6 +88,16 @@ Config_RobotParam_t robot_config = { .d_cutoff_freq=30.0f, .range=M_2PI, /* 2*PI,用于处理角度循环误差 */ }, + .roll={ + .k=1.0f, + .p=5.0f, /* 横滚角比例系数 */ + .i=0.0f, /* 横滚角积分系数 */ + .d=0.2f, /* 横滚角微分系数 */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=0.05f, /* 输出限幅,腿长差值限制 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, .low_pass_cutoff_freq = {