离地时进行速度闭环，基本实现稳定飞坡

application/chassis/balance.c

@@ -113,7 +113,7 @@ void BalanceInit()
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
         .Kp = 1200,
-        .Kd = 150,
+        .Kd = 200,
         .Ki = 0,
         .MaxOut = 60,
         .DeadBand = 0.0001f,
@@ -173,6 +173,7 @@ void BalanceInit()
 
     // 状态初始化
     l_side.target_len = r_side.target_len = 0.12;
+    l_side.gravity_ff = r_side.gravity_ff = 60.0f;
     chassis.vel_cov = 100;      // 速度协方差初始化
     chassis_status = ROBOT_READY;
     DWT_GetDeltaT(&balance_dwt_cnt);
@@ -231,8 +232,6 @@ static void ResetChassis()
     l_side.target_len = r_side.target_len = 0.12;
     // 角度输入为当前角度
     chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
-    // 驱动轮支持力为定值
-    l_side.normal_force = r_side.normal_force = 100.0f;
 
     // 撞墙时前后移动保证能重新站立,执行速度输入
     LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
@@ -285,8 +284,6 @@ static void WokingStateSet()
         chassis.dist = chassis.target_dist = 0;
         // 角度输入为当前角度
         chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
-        // 驱动轮支持力为定值
-        l_side.normal_force = r_side.normal_force = 100.0f;
 
         for (uint8_t i = 0; i < JOINT_CNT; i++)
             HTMotorStop(joint[i]);
@@ -383,11 +380,10 @@ static void LegControl() /* 腿长控制和Roll补偿 */
     l_side.target_len += roll_compensate_pid.Output;
     r_side.target_len -= roll_compensate_pid.Output;
 
-    static float gravity_comp = 60;
     static float roll_extra_comp_p = 400;
     float roll_comp = roll_extra_comp_p * chassis.roll;
-    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;
+    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + l_side.gravity_ff - roll_comp;
-    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp;
+    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_side.gravity_ff + roll_comp;
 }
 
 static void WattLimitSet() /* 设定运动模态的输出 */
@@ -425,18 +421,15 @@ void BalanceTask()
     // VMC映射成关节输出
     VMCProject(&l_side);
     VMCProject(&r_side);
+    // 驱动轮支持力解算
+    NormalForceSolve(&l_side, Chassis_IMU_data);
+    NormalForceSolve(&r_side, Chassis_IMU_data);
 
     // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
     if (chassis_status == ROBOT_STOP ||
         chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
         chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
         return; // 复位模态或急停,直接退出
-    else
-    {
-        // 正常模式下再进行驱动轮支持力解算
-        NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
-        NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
-    }
 
     // 运动模态,电机输出映射和限幅
     WattLimitSet();

application/chassis/balance.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "stdint.h"
+
 // 底盘参数
 #define CALF_LEN 0.24f              // 小腿
 #define THIGH_LEN 0.14f             // 大腿
@@ -55,6 +57,8 @@ typedef struct
     float T_wheel;
     float zw_ddot;    // 驱动轮竖直方向加速度
     float normal_force;  // 支持力
+    float gravity_ff;    // 重力前馈
+    uint8_t fly_flag;    // 离地标志位
 
     // pod
     float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一

application/chassis/fly_detection.h

@@ -4,7 +4,7 @@
 #include "user_lib.h"
 
 // 驱动轮支持力解算
-void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
+void NormalForceSolve(LinkNPodParam *p, INS_t *imu)
 {
     static float accx, accy, accz;
     accx = imu->MotionAccel_b[X];
@@ -15,11 +15,17 @@ void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
     pitch = imu->Pitch;
     roll = imu->Roll;
 
-    // 驱动轮竖直方向加速度
+    // 机体竖直方向加速度
     p->zw_ddot = -msin(roll) * accx + mcos(roll) * msin(pitch) * accy + mcos(pitch) * mcos(roll) * accz;
 
     // 驱动轮支持力解算
     static float P;
     P = p->F_leg * mcos(p->theta) + p->T_hip * msin(p->theta) / p->leg_len;
     p->normal_force = P + WHEEL_MASS * (p->zw_ddot + 9.81f); 
+
+    // 离地检测
+    if(p->normal_force < 20.0f)
+        p->fly_flag = 1;
+    else
+        p->fly_flag = 0;
 }

application/chassis/lqr_calc.h

@@ -9,48 +9,44 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    float k[12][3] = {62.680622,-74.772126,-13.135672,
+    static float k[12][3] = {62.680622,-74.772126,-13.135672,
-                        1.620796,-4.331826,-0.454705,
+                            1.620796,-4.331826,-0.454705,
-                        32.093563,-25.558681,-16.605856,
+                            32.093563,-25.558681,-16.605856,
-                        19.128242,-17.562747,-10.815380,
+                            19.128242,-17.562747,-10.815380,
-                        225.373594,-201.324771,53.236052,
+                            225.373594,-201.324771,53.236052,
-                        13.575849,-13.013546,3.845604,
+                            13.575849,-13.013546,3.845604,
-                        76.704297,-72.201666,20.359891,
+                            76.704297,-72.201666,20.359891,
-                        4.520141,-4.051799,1.189345,
+                            4.520141,-4.051799,1.189345,
-                        139.418565,-125.750074,34.110069,
+                            139.418565,-125.750074,34.110069,
-                        88.295852,-79.170124,21.430975,
+                            88.295852,-79.170124,21.430975,
-                        -163.725633,127.852860,110.004619,
+                            -163.725633,127.852860,110.004619,
-                        -9.557678,7.476790,4.655220};
+                            -9.557678,7.476790,4.655220};
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;
 
-    // 离地检测
-    if (p->normal_force < 20.0f)
-    {
-        for (size_t i = 0; i < 12; i++)
-        {
-            // 除 theta 和 theta_dot 的关节输出外，其余增益全部置0
-            if(i != 6 && i != 7)
-            {
-                for (size_t j = 0; j < 3; j++)
-                {
-                    k[i][j] = 0;
-                }
-            }
-        }
-    }
-    
-    // 计算增益
     for (uint8_t i = 0; i < 2; ++i)
     {
         uint8_t j = i * 6;
-        T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
+
-               (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
+        if(i == 0)      // 离地时仅对速度闭环，保证落地时轮速与机体速度一致
-               (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+        {
-               (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
+            T[i] = (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + (p->fly_flag ? 0 :
-               (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+                   ( (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
-               (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w;
+                     (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + 
+                     (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+                     (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+                     (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
+        }
+        else if(i == 1)     // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
+        {
+            T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
+                   (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 :
+                   ( (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+                     (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
+                     (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+                     (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
+        }
     }
     p->T_wheel = T[0];
     p->T_hip = T[1];