尝试改为力控

LJW0401 · LJW0401 · commit ff40886f54ed · 2024-06-13T18:06:58.000+08:00
diff --git a/application/chassis/chassis_balance.c b/application/chassis/chassis_balance.c
@@ -56,8 +56,8 @@ static Chassis_s CHASSIS = {
             .k = {{1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, 
                   {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}},
             // clang-format on
-            .Tp = 0.3f,
-            .T = 0.5f,
+            .Tp = TP_RATIO,
+            .T = T_RATIO,
             .length = 1.0f,
         },
     .dyaw = 0.0f,
@@ -175,7 +175,7 @@ void ChassisHandleException(void)
     }
 
     for (uint8_t i = 0; i < 4; i++) {
-        if (fabs(CHASSIS.joint_motor[i].fdb.tor) > MAX_JOINT_TORQUE) {
+        if (fabs(CHASSIS.joint_motor[i].fdb.tor) > MAX_TORQUE_PROTECT) {
             CHASSIS.error_code |= JOINT_ERROR_OFFSET;
             break;
         }
@@ -550,11 +550,12 @@ void ChassisReference(void)
 /*-------------------- Console --------------------*/
 
 static void LocomotionController(float Tp[2], float T_w[2]);
-#ifdef LOCATION_CONTROL
+#if LOCATION_CONTROL
 static void LegController(double joint_pos_l[2], double joint_pos_r[2]);
 #else
 static void LegController(float F[2]);
 #endif
+static float LegFeedForward(float theta);
 static void SetK(float leg_length, float k[2][6]);
 static void LQRFeedbackCalc(float k[2][6], float x[6], float t[2]);
 
@@ -640,6 +641,13 @@ static void LocomotionController(float Tp[2], float T_w[2])
     Tp[1] = tp - CHASSIS.pid.leg_angle_angle.out;
 }
 
+/**
+ * @brief        前馈控制
+ * @param[in]    theta 当前腿与竖直方向夹角
+ * @return       前馈量
+ */
+static float LegFeedForward(float theta) { return BODY_MASS * GRAVITY * cosf(theta) / 2; }
+
 /**
  * @brief         设置LQR的反馈矩阵K
  * @param[in]     leg_length 当前腿长
@@ -681,15 +689,15 @@ static void LQRFeedbackCalc(float k[2][6], float x[6], float t[2])
  */
 static void LegController(double joint_pos_l[2], double joint_pos_r[2])
 {
-    // static float delta_Angle = 0;
-    // float dAngle = CHASSIS.fdb.phi_dot * PITCH_VEL_RATIO;
-    // float dAngle_1 = PID_calc(&CHASSIS.pid.pitch_angle, CHASSIS.fdb.phi, CHASSIS.ref.phi);
+    static float delta_Angle = 0;
+    float dAngle = CHASSIS.fdb.phi_dot * PITCH_VEL_RATIO;
+    float dAngle_1 = PID_calc(&CHASSIS.pid.pitch_angle, CHASSIS.fdb.phi, CHASSIS.ref.phi);
     // float dAngle_2 = PID_calc(&CHASSIS.pid.pitch_vel, CHASSIS.fdb.phi_dot, CHASSIS.ref.phi_dot);
 
-    // delta_Angle += (dAngle + dAngle_1) * CHASSIS_CONTROL_TIME_S;
-    // delta_Angle = fp32_constrain(delta_Angle, MIN_DELTA_ROD_ANGLE, MAX_DELTA_ROD_ANGLE);
+    delta_Angle += (dAngle + dAngle_1) * CHASSIS_CONTROL_TIME_S;
+    delta_Angle = fp32_constrain(delta_Angle, MIN_DELTA_ROD_ANGLE, MAX_DELTA_ROD_ANGLE);
 
-    float delta_Angle = PID_calc(&CHASSIS.pid.pitch_angle, CHASSIS.fdb.phi, CHASSIS.ref.phi);
+    // float delta_Angle = PID_calc(&CHASSIS.pid.pitch_angle, CHASSIS.fdb.phi, CHASSIS.ref.phi);
 
     CHASSIS.ref.leg[0].rod.Angle = M_PI_2 + delta_Angle;
     CHASSIS.ref.leg[1].rod.Angle = M_PI_2 + delta_Angle;
@@ -724,19 +732,19 @@ static void LegController(float F[2])
         &CHASSIS.pid.leg_length_length[0], CHASSIS.fdb.leg[0].rod.Length,
         CHASSIS.ref.leg[0].rod.Length);
     float theta_l = CHASSIS.fdb.leg[0].rod.Angle - M_PI_2 - CHASSIS.imu->pitch;
-    float fdf_left = 0;  // LegFeedforward(theta_l);
+    float fdf_left = LegFeedForward(theta_l) * FF_RATIO;
 
     PID_calc(
         &CHASSIS.pid.leg_length_length[1], CHASSIS.fdb.leg[1].rod.Length,
         CHASSIS.ref.leg[1].rod.Length);
     float theta_r = CHASSIS.fdb.leg[1].rod.Angle - M_PI_2 - CHASSIS.imu->pitch;
-    float fdf_right = 0;  // LegFeedforward(theta_r);
+    float fdf_right = LegFeedForward(theta_r) * FF_RATIO;
 
     PID_calc(&CHASSIS.pid.roll_angle, CHASSIS.fdb.roll, CHASSIS.ref.roll);
-    PID_calc(&CHASSIS.pid.roll_velocity, CHASSIS.fdb.roll_velocity, CHASSIS.pid.roll_angle.out);
+    // PID_calc(&CHASSIS.pid.roll_velocity, CHASSIS.fdb.roll_velocity, CHASSIS.pid.roll_angle.out);
 
-    F[0] = CHASSIS.pid.leg_length_length[0].out + fdf_left + CHASSIS.pid.roll_velocity.out;
-    F[1] = CHASSIS.pid.leg_length_length[1].out + fdf_right - CHASSIS.pid.roll_velocity.out;
+    F[0] = CHASSIS.pid.leg_length_length[0].out + fdf_left + CHASSIS.pid.roll_angle.out;
+    F[1] = CHASSIS.pid.leg_length_length[1].out + fdf_right - CHASSIS.pid.roll_angle.out;
 }
 #endif
 
@@ -807,8 +815,8 @@ static void ConsoleNormal(void)
 {
     float tp[2], t[2];
     LocomotionController(tp, t);
-    CHASSIS.ref.leg[0].rod.Tp = tp[0];
-    CHASSIS.ref.leg[1].rod.Tp = tp[1];
+    CHASSIS.ref.leg[0].rod.Tp = -tp[0];
+    CHASSIS.ref.leg[1].rod.Tp = -tp[1];
 
 #if LOCATION_CONTROL
     double joint_pos_l[2], joint_pos_r[2];
@@ -855,6 +863,11 @@ static void ConsoleNormal(void)
 
     CHASSIS.joint_motor[2].set.tor = -joint_torque[0] * (J2_DIRECTION);
     CHASSIS.joint_motor[3].set.tor = -joint_torque[1] * (J3_DIRECTION);
+
+    CHASSIS.joint_motor[0].set.tor = fp32_constrain(CHASSIS.joint_motor[0].set.tor, MIN_JOINT_TORQUE, MAX_JOINT_TORQUE);
+    CHASSIS.joint_motor[1].set.tor = fp32_constrain(CHASSIS.joint_motor[1].set.tor, MIN_JOINT_TORQUE, MAX_JOINT_TORQUE);
+    CHASSIS.joint_motor[2].set.tor = fp32_constrain(CHASSIS.joint_motor[2].set.tor, MIN_JOINT_TORQUE, MAX_JOINT_TORQUE);
+    CHASSIS.joint_motor[3].set.tor = fp32_constrain(CHASSIS.joint_motor[3].set.tor, MIN_JOINT_TORQUE, MAX_JOINT_TORQUE);
 #endif
 
     // QUESTION: 排查电机发送的力矩要反向的问题，这种情况下控制正常
diff --git a/application/robot_param_balanced_infantry.h b/application/robot_param_balanced_infantry.h
@@ -15,7 +15,7 @@
 
 // clang-format off
 /*-------------------- Chassis --------------------*/
-#define LOCATION_CONTROL 1 // 位置控制
+#define LOCATION_CONTROL 0 // 位置控制
 // 底盘任务相关宏定义
 #define CHASSIS_TASK_INIT_TIME 357   // 任务开始空闲一段时间
 #define CHASSIS_CONTROL_TIME_MS 2    // 底盘任务控制间隔 2ms
@@ -35,8 +35,12 @@
 #define WHEEL_DEADZONE (0.01f)  // (m/s)轮子速度死区
 
 // ratio parameters ---------------------
-#define VEL_ADD_RATIO (0.008f)  // 速度增量比例系数
-#define PITCH_VEL_RATIO (0.9f)  // pitch轴速度比例系数
+#define VEL_ADD_RATIO    (0.008f)  // 速度增量比例系数
+#define PITCH_VEL_RATIO  (0.9f)    // pitch轴速度比例系数
+#define FF_RATIO         (0.25f)   // 前馈比例系数
+
+#define TP_RATIO (0.08f)
+#define T_RATIO  (0.5f)
 
 // motor parameters ---------------------
 #define JOINT_CAN (1)
@@ -51,6 +55,7 @@
 #define W1_DIRECTION (-1)
 
 //physical parameters ---------------------
+#define BODY_MASS            (12.65813f)    // (kg)机身重量
 #define LEG_MASS             (0.4f)    // (kg)腿重量
 #define WHEEL_MASS           (1.74f)   // (kg)轮子重量
 #define WHEEL_RADIUS         (0.106f)  // (m)轮子半径
@@ -66,6 +71,7 @@
 
 //upper_limit parameters ---------------------
 #define MAX_DELTA_ROD_ANGLE (0.25f) // (rad)腿摆角最大变化量
+#define MAX_TORQUE_PROTECT  (10.0f)  // (Nm)最大扭矩保护
 
 #define MAX_THETA      (1.0f)
 #define MAX_THETA_DOT  (2.0f)
@@ -92,11 +98,11 @@
 #define MAX_SPEED_VECTOR_VY  (1.5f)
 #define MAX_SPEED_VECTOR_WZ  (3.0f)
 
-#define MAX_VEL_ADD   (1.0f)  // (m/s)速度增量上限
-#define MAX_PITCH_VEL (0.1f)  // (rad/s)pitch轴速度上限
+#define MAX_JOINT_TORQUE   (5.0f)  // (Nm)关节最大扭矩
+#define MAX_VEL_ADD        (1.0f)  // (m/s)速度增量上限
+#define MAX_PITCH_VEL      (0.1f)  // (rad/s)pitch轴速度上限
 
 #define MAX_TOUCH_INTERVAL (200)   // (ms)最大离地时间，超过这个时间认为离地
-#define MAX_JOINT_TORQUE   (6.0f)  // (Nm)关节最大扭矩
 //lower_limit parameters ---------------------
 #define MIN_DELTA_ROD_ANGLE (-MAX_DELTA_ROD_ANGLE) // (rad)腿摆角最小变化量
 
@@ -125,8 +131,9 @@
 #define MIN_SPEED_VECTOR_VY  (-MAX_SPEED_VECTOR_VY)
 #define MIN_SPEED_VECTOR_WZ  (-MAX_SPEED_VECTOR_WZ)
 
-#define MIN_VEL_ADD   (-MAX_VEL_ADD)    // (m/s)速度增量下限
-#define MIN_PITCH_VEL (-MAX_PITCH_VEL)  // (rad/s)pitch轴速度下限
+#define MIN_JOINT_TORQUE   (-MAX_JOINT_TORQUE)  // 
+#define MIN_VEL_ADD        (-MAX_VEL_ADD)    // (m/s)速度增量下限
+#define MIN_PITCH_VEL      (-MAX_PITCH_VEL)  // (rad/s)pitch轴速度下限
 
 
 //PID parameters ---------------------
@@ -145,11 +152,11 @@
 #define MAX_OUT_CHASSIS_YAW_VELOCITY   (1.0f)
 
 //roll轴跟踪角度环PID参数
-#define KP_CHASSIS_ROLL_ANGLE        (0.6f)
+#define KP_CHASSIS_ROLL_ANGLE        (0.0f)
 #define KI_CHASSIS_ROLL_ANGLE        (0.0f)
-#define KD_CHASSIS_ROLL_ANGLE        (0.1f)
+#define KD_CHASSIS_ROLL_ANGLE        (0.0f)
 #define MAX_IOUT_CHASSIS_ROLL_ANGLE  (0.0f)
-#define MAX_OUT_CHASSIS_ROLL_ANGLE   (0.12f)
+#define MAX_OUT_CHASSIS_ROLL_ANGLE   (0.0f)
 
 // //roll轴跟踪速度环PID参数
 // #define KP_CHASSIS_ROLL_VELOCITY 0.1f
@@ -159,30 +166,25 @@
 // #define MAX_OUT_CHASSIS_ROLL_VELOCITY 0.12f
 
 //pitch轴跟踪角度环PID参数
-#define KP_CHASSIS_PITCH_ANGLE        (0.8f)
+#define KP_CHASSIS_PITCH_ANGLE        (0.0f)
 #define KI_CHASSIS_PITCH_ANGLE        (0.0f)
-#define KD_CHASSIS_PITCH_ANGLE        (0.3f)
-#define MAX_IOUT_CHASSIS_PITCH_ANGLE  (0.05f)
-#define MAX_OUT_CHASSIS_PITCH_ANGLE   (0.2f)
-// #define KP_CHASSIS_PITCH_ANGLE        (2.0f)
-// #define KI_CHASSIS_PITCH_ANGLE        (0.0f)
-// #define KD_CHASSIS_PITCH_ANGLE        (0.2f)
-// #define MAX_IOUT_CHASSIS_PITCH_ANGLE  (0.0f)
-// #define MAX_OUT_CHASSIS_PITCH_ANGLE   (1.0f)
+#define KD_CHASSIS_PITCH_ANGLE        (0.0f)
+#define MAX_IOUT_CHASSIS_PITCH_ANGLE  (0.0f)
+#define MAX_OUT_CHASSIS_PITCH_ANGLE   (0.0f)
 
 //pitch轴跟踪速度环PID参数
-#define KP_CHASSIS_PITCH_VELOCITY        (1.5f)
-#define KI_CHASSIS_PITCH_VELOCITY        (0.0f)
-#define KD_CHASSIS_PITCH_VELOCITY        (0.0f)
-#define MAX_IOUT_CHASSIS_PITCH_VELOCITY  (0.0f)
-#define MAX_OUT_CHASSIS_PITCH_VELOCITY   (0.0f)
+// #define KP_CHASSIS_PITCH_VELOCITY        (1.5f)
+// #define KI_CHASSIS_PITCH_VELOCITY        (0.0f)
+// #define KD_CHASSIS_PITCH_VELOCITY        (0.0f)
+// #define MAX_IOUT_CHASSIS_PITCH_VELOCITY  (0.0f)
+// #define MAX_OUT_CHASSIS_PITCH_VELOCITY   (0.0f)
 
 // 腿长跟踪长度环PID参数
-#define KP_CHASSIS_LEG_LENGTH_LENGTH        (90.0f)
-#define KI_CHASSIS_LEG_LENGTH_LENGTH        (0.1f)
+#define KP_CHASSIS_LEG_LENGTH_LENGTH        (1.0f)
+#define KI_CHASSIS_LEG_LENGTH_LENGTH        (0.0f)
 #define KD_CHASSIS_LEG_LENGTH_LENGTH        (1.0f)
 #define MAX_IOUT_CHASSIS_LEG_LENGTH_LENGTH  (0.5f)
-#define MAX_OUT_CHASSIS_LEG_LENGTH_LENGTH   (30.0f)
+#define MAX_OUT_CHASSIS_LEG_LENGTH_LENGTH   (10.0f)
 
 // 腿长跟踪速度环PID参数
 // #define KP_CHASSIS_LEG_LENGTH_SPEED 0.0f
diff --git a/components/controller/leg_model.c b/components/controller/leg_model.c
@@ -289,40 +289,40 @@ void L2K(double L0, double K[12])
   /*     2024-03-18 11:58:20 */
   t2 = L0 * L0;
   t3 = L0 * L0 * L0;
-  K[0] = ((L0 * -471.26895290849069 + t2 * 655.82771179456677) -
-          t3 * 588.3102597819942) -
-         33.946942389013763;
-  K[1] = ((L0 * 67.93355856622631 - t2 * 299.68896582918688) +
-          t3 * 352.052850180012) +
-         24.146302504485291;
-  K[2] = ((L0 * -46.558110626768539 - t2 * 59.647562966390211) +
-          t3 * 41.70979643117) -
-         11.33548614145108;
-  K[3] = ((L0 * -6.3405825085466407 + t2 * 15.672601321835741) -
-          t3 * 14.161872016176581) +
-         7.4540007322748174;
-  K[4] = ((L0 * -25.943755021409551 + t2 * 60.27738396726874) -
-          t3 * 53.6960171711332) -
-         17.613872554801819;
-  K[5] = ((L0 * -40.635458780584571 + t2 * 60.150208570349967) -
-          t3 * 36.579993054252192) +
-         14.169989391295781;
-  K[6] = ((L0 * -10.9848327194688 - t2 * 24.559200883204142) +
-          t3 * 47.839287745993943) -
-         37.386515524085418;
-  K[7] = ((L0 * -91.437185828499182 + t2 * 176.47795931259941) -
-          t3 * 146.67425720587451) +
-         27.167894593487269;
-  K[8] = ((L0 * -128.50060351277159 + t2 * 190.21166081747671) -
-          t3 * 115.67609484590849) +
-         44.809440896934881;
-  K[9] = ((L0 * 82.041356925319178 - t2 * 190.61382473419741) +
-          t3 * 169.80171554192819) +
-         55.69995568908702;
-  K[10] = ((L0 * -30.910579695490629 + t2 * 62.987841473601947) -
-           t3 * 55.628594826648992) +
-          9.3895495473674959;
-  K[11] = ((L0 * 17.668378988995538 - t2 * 43.043680966815828) +
-           t3 * 40.559039858925047) +
-          3.506432700187557;
+  K[0] = ((L0 * -255.00411409889119 + t2 * 277.20791430131959) -
+          t3 * 193.628649543518) -
+         10.663464768105079;
+  K[1] = ((L0 * 218.04509345933951 - t2 * 698.77836229904381) +
+          t3 * 729.49732715384755) +
+         13.492713153034879;
+  K[2] = ((L0 * -25.6847795144051 - t2 * 40.984568601388958) +
+          t3 * 37.761484637897979) -
+         2.9429417903170951;
+  K[3] = ((L0 * 30.251977741287678 - t2 * 66.353686976052515) +
+          t3 * 58.077092793225951) +
+         3.1379053718892989;
+  K[4] = ((L0 * -37.81755156273605 + t2 * 62.763299712877007) -
+          t3 * 35.04231722810475) -
+         13.576411971474741;
+  K[5] = ((L0 * -61.31974658662913 - t2 * 9.5741774438885017) +
+          t3 * 97.099511950688282) +
+         35.7361814333234;
+  K[6] = ((L0 * -14.1841566253297 - t2 * 29.039707775014559) +
+          t3 * 61.309562891650579) -
+         15.54961092745339;
+  K[7] = ((L0 * -59.15085840928522 + t2 * 41.782414943458193) +
+          t3 * 12.04836935795807) +
+         30.39198488175602;
+  K[8] = ((L0 * -30.659873293307569 - t2 * 4.7870887219700373) +
+          t3 * 48.549755975375582) +
+         17.868090716661069;
+  K[9] = ((L0 * 75.6351031255065 - t2 * 125.5265994258763) +
+          t3 * 70.084634456346024) +
+         27.15282394294643;
+  K[10] = ((L0 * -13.671180603046761 + t2 * 23.9054797259361) -
+           t3 * 18.8980267722451) +
+          4.850381218873169;
+  K[11] = ((L0 * 22.98892399175223 - t2 * 48.223384207980708) +
+           t3 * 39.184535619891847) +
+          0.38551400444303008;
 }
