起来自旋

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, &param->leglength);
     PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, &param->leglength);
     PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
+    PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->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[0] = 0.12f + c_cmd->height * 0.1f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
-    target_L0[1] = 0.12f + c_cmd->height * 0.1f;
+    target_L0[1] = 0.12f + c_cmd->height * 0.1f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
     
     // 获取腿长变化率
     VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);

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 */
 

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 = {