改参数

User/module/balance_chassis.c

@@ -19,7 +19,7 @@
 
 #define WHEEL_RADIUS        0.068f   /* 轮子半径 (m) */
 #define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
-#define WHEEL_GEAR_RATIO    8.0f     /* 轮毂电机扭矩 */
+#define WHEEL_GEAR_RATIO    4.5f     /* 轮毂电机扭矩 */
 #define BASE_LEG_LENGTH     0.16f    /* 基础腿长 (m) */
 #define LEG_LENGTH_RANGE    0.16f    /* 腿长调节范围 (m) */
 #define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */

User/module/config.c

@@ -317,19 +317,20 @@ Config_RobotParam_t robot_config = {
         },
 
     .lqr_gains = {
-    .k11_coeff = { -2.210764178888050e+02f, 2.426363711747753e+02f, -1.195592871161101e+02f, -3.943946668753056e+00f }, // theta
-    .k12_coeff = { -2.137164311515632e+00f, 1.217737477437830e+00f, -9.525046061205586e+00f, -3.204960462460905e-01f },  // d_theta
-    .k13_coeff = { -5.403421185903361e+01f, 5.400834583224349e+01f, -1.871421701200579e+01f, -2.187399624044644e+00f },  // x
-    .k14_coeff = { -4.592744044784984e+01f, 4.660766512779469e+01f, -1.805265440365850e+01f, -2.699994690713614e+00f },  // d_x
-    .k15_coeff = { -9.685784551581214e+01f, 1.159388980795003e+02f, -5.296171221139453e+01f, 1.123906253873898e+01f }, // phi
-    .k16_coeff = { -2.294801139621741e+01f, 2.749641396600526e+01f, -1.259823164155369e+01f, 2.761284676172917e+00f },  // d_phi
-    .k21_coeff = { 7.727438740554770e+01f, -3.291782819657940e+01f, -1.757884718209812e+01f, 1.497962178015115e+01f }, // theta
-    .k22_coeff = { -4.313876591098404e-01f, 3.014310593976116e+00f, -2.775571423570345e+00f, 1.949396772465259e+00f },  // d_theta
-    .k23_coeff = { -8.163301739920055e+01f, 9.889264843312036e+01f, -4.547892032101780e+01f, 9.369462147227104e+00f },  // x
-    .k24_coeff = { -1.025410522837800e+02f, 1.187387974704523e+02f, -5.202150278239223e+01f, 1.038835798060997e+01f },  // d_x
-    .k25_coeff = { 2.742584002836992e+02f, -2.735816033702905e+02f, 9.479793291043821e+01f, 7.314831370012082e+00f }, // phi
-    .k26_coeff = { 6.734357804702088e+01f, -6.735254573603545e+01f, 2.346143287673895e+01f, 1.501777400084277e+00f },  // d_phi
+        .k11_coeff = { -2.110325959941390e+02f, 2.474876302869424e+02f, -1.259312069344584e+02f, -3.555067587081389e+00f }, // theta
+        .k12_coeff = { 1.729035713826111e+00f, -1.094205267383853e+00f, -8.235618497594484e+00f, -2.949091603597677e-01f },  // d_theta
+        .k13_coeff = { -3.661055595462458e+01f, 4.066046385752247e+01f, -1.618786894025212e+01f, -1.618742454043447e+00f },  // x
+        .k14_coeff = { -2.988926501197955e+01f, 3.445642428071118e+01f, -1.547117906530474e+01f, -2.039186188549067e+00f },  // d_x
+        .k15_coeff = { -4.094585161782607e+01f, 6.119228618474273e+01f, -3.603009625243965e+01f, 1.037078067693317e+01f }, // phi
+        .k16_coeff = { -7.625020535837323e+00f, 1.097226354374976e+01f, -6.302244199082423e+00f, 1.920720114282383e+00f },  // d_phi
+        .k21_coeff = { 3.113860878856309e+02f, -2.577426343951217e+02f, 4.982404130107013e+01f, 1.415180465415879e+01f }, // theta
+        .k22_coeff = { 1.126082466887103e+01f, -9.812804977178935e+00f, 1.814407629267238e+00f, 1.941377111901043e+00f },  // d_theta
+        .k23_coeff = { -1.771607516002724e+01f, 4.047388400543465e+01f, -2.962942748901807e+01f, 8.971209797230671e+00f },  // x
+        .k24_coeff = { -3.391460592939657e+01f, 5.575858169617614e+01f, -3.466996436706782e+01f, 9.833893476570065e+00f },  // d_x
+        .k25_coeff = { 2.392778926806863e+02f, -2.574664846375947e+02f, 9.953632837845201e+01f, 2.579297039170335e+00f }, // phi
+        .k26_coeff = { 4.297556375859175e+01f, -4.657509218314731e+01f, 1.826212630358702e+01f, 1.257410509642398e-01f },  // d_phi
     },
+
     .jump_params = {
         .crouch_time_ms = 300,
         .launch_time_ms = 120,

utils/6. 综合运动控制验证/K-L0/K.m

@@ -0,0 +1,28 @@
+close all;
+clear;clc;
+L0=[0.12 0.13 0.14 0.15 0.16 0.17 0.18 0.19 0.20 0.21 0.22 0.23 0.24 0.25 0.26 0.27 0.28 0.29 0.30 0.31 0.32 0.33 0.34 0.35 0.36];
+K11=[-13.0207 -13.6522 -14.2545 -14.8292 -15.3780 -15.9028 -16.4052 -16.8868 -17.3490 -17.7934 -18.2212 -18.6337 -19.0320 -19.4172 -19.7902 -20.1519 -20.5031 -20.8445 -21.1769 -21.5008 -21.8169 -22.1256 -22.4274 -22.7227 -23.0120];
+K12=[-1.3999 -1.4844 -1.5691 -1.6540 -1.7390 -1.8240 -1.9090 -1.9940 -2.0790 -2.1640 -2.2490 -2.3340 -2.4191 -2.5042 -2.5893 -2.6745 -2.7597 -2.8451 -2.9305 -3.0160 -3.1016 -3.1873 -3.2732 -3.3592 -3.4453];
+K13=[-3.6148 -3.6840 -3.7481 -3.8071 -3.8611 -3.9104 -3.9552 -3.9960 -4.0330 -4.0666 -4.0971 -4.1249 -4.1501 -4.1730 -4.1940 -4.2130 -4.2304 -4.2464 -4.2609 -4.2743 -4.2866 -4.2979 -4.3083 -4.3179 -4.3267];
+K14=[-3.8656 -3.9320 -3.9955 -4.0559 -4.1129 -4.1667 -4.2173 -4.2648 -4.3096 -4.3518 -4.3915 -4.4291 -4.4647 -4.4985 -4.5307 -4.5613 -4.5906 -4.6187 -4.6457 -4.6717 -4.6967 -4.7209 -4.7444 -4.7671 -4.7892];
+K15=[6.2710 6.0483 5.8343 5.6293 5.4333 5.2462 5.0681 4.8986 4.7375 4.5845 4.4393 4.3014 4.1706 4.0464 3.9284 3.8164 3.7100 3.6088 3.5125 3.4209 3.3336 3.2505 3.1712 3.0955 3.0232];
+K16=[1.1563 1.1201 1.0854 1.0522 1.0206 0.9904 0.9617 0.9343 0.9083 0.8836 0.8602 0.8379 0.8167 0.7966 0.7775 0.7593 0.7421 0.7256 0.7100 0.6951 0.6809 0.6674 0.6545 0.6422 0.6304];
+K21=[11.9463 11.9148 11.8490 11.7565 11.6436 11.5153 11.3760 11.2290 11.0769 10.9220 10.7660 10.6102 10.4556 10.3030 10.1531 10.0063 9.8629 9.7231 9.5871 9.4549 9.3266 9.2021 9.0814 8.9645 8.8513];
+K22=[1.8154 1.7992 1.7822 1.7645 1.7465 1.7284 1.7103 1.6924 1.6747 1.6574 1.6405 1.6241 1.6081 1.5926 1.5776 1.5631 1.5491 1.5356 1.5225 1.5099 1.4978 1.4860 1.4747 1.4638 1.4532];
+K23=[5.2662 5.0707 4.8792 4.6930 4.5131 4.3401 4.1744 4.0161 3.8652 3.7216 3.5852 3.4556 3.3327 3.2160 3.1053 3.0003 2.9006 2.8060 2.7162 2.6308 2.5496 2.4724 2.3989 2.3289 2.2622];
+K24=[5.3712 5.1585 4.9545 4.7593 4.5731 4.3960 4.2278 4.0683 3.9172 3.7741 3.6388 3.5107 3.3895 3.2748 3.1663 3.0635 2.9662 2.8739 2.7863 2.7032 2.6243 2.5493 2.4779 2.4100 2.3453];
+K25=[11.6084 11.9794 12.3174 12.6247 12.9036 13.1566 13.3861 13.5942 13.7831 13.9546 14.1105 14.2524 14.3817 14.4997 14.6076 14.7063 14.7968 14.8800 14.9564 15.0268 15.0918 15.1518 15.2074 15.2589 15.3067];
+K26=[1.7058 1.7715 1.8315 1.8863 1.9362 1.9817 2.0231 2.0609 2.0953 2.1267 2.1553 2.1815 2.2055 2.2276 2.2478 2.2664 2.2835 2.2993 2.3139 2.3274 2.3399 2.3515 2.3623 2.3724 2.3817];
+
+% K11=-172.5944*L_0^3+194.4583*L_0^2-102.6573*L_0-3.2160
+% K12=-0.9840*L_0^3+0.4770*L_0^2-8.5684*L_0-0.3764
+% K13=-38.8812*L_0^3+40.3986*L_0^2-15.0820*L_0-2.3214
+% K14=-25.9829*L_0^3+28.2485*L_0^2-12.5490*L_0-2.7215
+% K15=-53.1626*L_0^3+71.4193*L_0^2-37.8708*L_0+9.8810
+% K16=-8.7478*L_0^3+11.6234*L_0^2-6.1338*L_0+1.7402
+% K21=182.7742*L_0^3-134.1998*L_0^2+17.2318*L_0+11.5442
+% K22=5.0007*L_0^3-1.7221*L_0^2-1.6219*L_0+2.0283
+% K23=-34.7106*L_0^3+55.1695*L_0^2-32.5200*L_0+8.4420
+% K24=-55.6192*L_0^3+72.6502*L_0^2-37.0784*L_0+8.8727
+% K25=214.7065*L_0^3-218.2300*L_0^2+79.9589*L_0+4.8045
+% K26=36.3354*L_0^3-37.3084*L_0^2+13.9211*L_0+0.5130

utils/6. 综合运动控制验证/LQR_K.m

@@ -0,0 +1,84 @@
+close all;
+clear;clc;
+%% 参数
+global g R L L_M l M m_p m_w I_p I_M I_w;
+L0=0.16;
+g=9.80665;
+R=0.155/2;
+L=L0/2;
+L_M=L0/2;
+l=0.02;
+M=10;
+m_p=1;
+m_w=1;
+I_p=1/12*m_p*(L+L_M)^2;
+I_M=1/12*M*(0.1^2+0.3^2);
+I_w=1/2*m_w*R^2;
+
+A21=(I_M*I_w*L*g*m_p+I_M*L*M^2*R^2*g+I_M*L_M*M^2*R^2*g+I_w*L*M^2*g*l^2+I_w*L_M*M^2*g*l^2+I_M*L*R^2*g*m_p^2+I_M*I_w*L*M*g+I_M*I_w*L_M*M*g+L*M*R^2*g*l^2*m_p^2+L*M^2*R^2*g*l^2*m_p+L*M^2*R^2*g*l^2*m_w+L_M*M^2*R^2*g*l^2*m_p+L_M*M^2*R^2*g*l^2*m_w+2*I_M*L*M*R^2*g*m_p+I_M*L*M*R^2*g*m_w+I_M*L_M*M*R^2*g*m_p+I_M*L_M*M*R^2*g*m_w+I_w*L*M*g*l^2*m_p+I_M*L*R^2*g*m_p*m_w+L*M*R^2*g*l^2*m_p*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A25=(I_w*L*M^2*g*l^2+I_w*L_M*M^2*g*l^2+L*M^2*R^2*g*l^2*m_w+L_M*M^2*R^2*g*l^2*m_p+L_M*M^2*R^2*g*l^2*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A41=-(R*(R*g*L^2*M^2*l^2*m_p+I_M*R*g*L^2*M^2+R*g*L^2*M*l^2*m_p^2+2*I_M*R*g*L^2*M*m_p+I_M*R*g*L^2*m_p^2+R*g*L*L_M*M^2*l^2*m_p+2*I_M*R*g*L*L_M*M^2+2*I_M*R*g*L*L_M*M*m_p+I_M*R*g*L_M^2*M^2))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A45=(R*(I_p*M^2*R*g*l^2-L*L_M*M^2*R*g*l^2*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A61=(g*l*m_w*L^2*M^2*R^2+I_w*g*l*L^2*M^2+g*l*m_w*L^2*M*R^2*m_p+I_w*g*l*L^2*M*m_p+g*l*L*L_M*M^2*R^2*m_p+2*g*l*m_w*L*L_M*M^2*R^2+2*I_w*g*l*L*L_M*M^2+g*l*L*L_M*M*R^2*m_p^2+g*l*m_w*L*L_M*M*R^2*m_p+I_w*g*l*L*L_M*M*m_p+g*l*L_M^2*M^2*R^2*m_p+g*l*m_w*L_M^2*M^2*R^2+I_w*g*l*L_M^2*M^2)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A65=(I_p*I_w*M*g*l+I_w*L^2*M^2*g*l+I_w*L_M^2*M^2*g*l+I_p*M^2*R^2*g*l+L^2*M^2*R^2*g*l*m_w+L_M^2*M^2*R^2*g*l*m_p+L_M^2*M^2*R^2*g*l*m_w+2*I_w*L*L_M*M^2*g*l+I_w*L^2*M*g*l*m_p+I_p*M*R^2*g*l*m_p+I_p*M*R^2*g*l*m_w+2*L*L_M*M^2*R^2*g*l*m_w+L^2*M*R^2*g*l*m_p*m_w)/(I_M*I_p*I_w + I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A=[ 0   1   0   0   0   0;
+    A21 0   0   0   A25 0;
+    0   0   0   1   0   0;
+    A41 0   0   0   A45 0;
+    0   0   0   0   0   1;
+    A61 0   0   0   A65 0];
+
+B21=-(I_M*I_w+I_M*M*R^2+I_w*M*l^2+I_M*R^2*m_p+I_M*R^2*m_w+I_M*L*M*R+I_M*L_M*M*R+M*R^2*l^2*m_p+M*R^2*l^2*m_w+I_M*L*R*m_p+L*M*R*l^2*m_p)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B22=(I_M*I_w+I_M*M*R^2+I_w*M*l^2+I_M*R^2*m_p+I_M*R^2*m_w+M*R^2*l^2*m_p+M*R^2*l^2*m_w+I_w*L*M*l+I_w*L_M*M*l+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B41=(R*(I_M*I_p+I_M*L^2*M+I_M*L_M^2*M+I_p*M*l^2+I_M*L^2*m_p+2*I_M*L*L_M*M+L^2*M*l^2*m_p+I_M*L*M*R+I_M*L_M*M*R+I_M*L*R*m_p+L*M*R*l^2*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B42=-(R*(I_M*L*M*R+I_M*L_M*M*R-I_p*M*R*l+I_M*L*R*m_p+L*M*R*l^2*m_p+L*L_M*M*R*l*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B61=-(I_w*L*M*l+I_w*L_M*M*l-I_p*M*R*l+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w+L*L_M*M*R*l*m_p)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B62=(I_p*I_w+I_w*L^2*M+I_w*L_M^2*M+I_p*M*R^2+I_w*L^2*m_p+I_p*R^2*m_p+I_p*R^2*m_w+L^2*M*R^2*m_w+L_M^2*M*R^2*m_p+L_M^2*M*R^2*m_w+2*I_w*L*L_M*M+L^2*R^2*m_p*m_w+I_w*L*M*l+I_w*L_M*M*l+2*L*L_M*M*R^2*m_w+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B=[ 0   0;
+    B21 B22;
+    0   0;
+    B41 B42;
+    0   0;
+    B61 B62];
+
+C=[ 1   0   0   0   0   0;
+    0   1   0   0   0   0;
+    0   0   1   0   0   0;
+    0   0   0   1   0   0;
+    0   0   0   0   1   0;
+    0   0   0   0   0   1];
+D=[ 0   0;
+    0   0;
+    0   0;
+    0   0;
+    0   0;
+    0   0];
+
+Q=[ 1   0   0   0   0   0;
+    0   1   0   0   0   0;
+    0   0   20   0   0   0;
+    0   0   0   5   0   0;
+    0   0   0   0   50   0;
+    0   0   0   0   0   1];
+R_=[1   0;
+    0   0.25];
+%% 
+Qc=ctrb(A,B);
+rank(Qc);
+%% 计算增益矩阵
+[K,S,P]=lqr(A,B,Q,R_);
+K
+%% 模拟系统响应
+% figure(1);
+% x0=[10/180*pi;
+%     0;
+%     0;
+%     0;
+%     10/180*pi;
+%     0];%初始状态
+% t=0:0.05:10;
+% u=[ zeros(size(t));
+%     zeros(size(t));];
+% [y,x]=lsim(A-B*K,B,C,D,u,t,x0);%模拟系统响应
+% plot(t,y);
+% legend('θ(t)','dθ(t)','x(t)','dx(t)','ɸ','dɸ')%添加图例

utils/Simulation-master/balance/series_legs/get_k_length.m

@@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
     R1=0.068;                         %驱动轮半径
     L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
     LM1=leg_length/2;                 %摆杆重心到其转轴距离
-    l1=0.01;                          %机体质心距离转轴距离
+    l1=0.02;                          %机体质心距离转轴距离
     mw1=0.60;                         %驱动轮质量
     mp1=1.8;                         %杆质量
-    M1=12.0;                          %机体质量
+    M1=16.0;                          %机体质量
     Iw1=mw1*R1^2;                     %驱动轮转动惯量
     Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
     IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
@@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
     B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
     B=double(B);
     
-    Q=diag([1500 200 5000 2000 20000 1000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[240 0;0 60];                %T Tp
+    Q=diag([1 1 20 5 50 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[1.2 0;0 0.25];                %T Tp
     
     K=lqr(A,B,Q,R);