改极性

2026-01-11 16:52:19 +08:00 · 2026-01-11 16:52:19 +08:00 · 9652f6c5cc
commit 9652f6c5cc
parent 4aab1a909d
52 changed files with 4582 additions and 577 deletions
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
--- a/User/component/vmc.c
+++ b/User/component/vmc.c
@ -80,7 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
    if (vmc == NULL || !vmc->initialized) {
        return -1;
    }
-    // body_pitch = body_pitch; // 机体俯仰角取反
+    body_pitch = -body_pitch; // 机体俯仰角取反
    
    VMC_Leg_t *leg = &vmc->leg;
    
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@ -129,7 +129,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
  float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f;   // rad/s
  float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s

-  float wheel_radius = 0.072f;
+  float wheel_radius = 0.068f;

  float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
  float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
--- a/User/module/config.c
+++ b/User/module/config.c
@ -235,18 +235,18 @@ Config_RobotParam_t robot_config = {
        },

 .lqr_gains = {
-        .k11_coeff = { -2.213202553185133e+02f, 2.353939463356143e+02f, -1.057072351438971e+02f, -1.581085937677281e+00f }, // theta
-        .k12_coeff = { -9.181864404672975e+00f, 8.531964722737065e+00f, -9.696625432480346e+00f, -2.388898921230960e-01f },  // d_theta
-        .k13_coeff = { -6.328339397527442e+01f, 6.270159865929592e+01f, -2.133356351416681e+01f, -2.795774497769496e-01f },  // x
-        .k14_coeff = { -7.428160824353201e+01f, 7.371925049068537e+01f, -2.613745545093503e+01f, -5.994101373770330e-01f },  // d_x
-        .k15_coeff = { -6.968934105907989e+01f, 9.229969229361623e+01f, -4.424018428098277e+01f, 8.098181536555296e+00f }, // phi
-        .k16_coeff = { -1.527045508038401e+01f, 2.030548630730375e+01f, -1.009526207086012e+01f, 2.014358176738665e+00f },  // d_phi
-        .k21_coeff = { 6.254476937997669e+01f, 9.037146968574660e+00f, -4.492072460618583e+01f, 1.770766202994207e+01f }, // theta
-        .k22_coeff = { 3.165057029795604e-02f, 7.350960766534424e+00f, -6.597366624137901e+00f, 2.798506180182324e+00f },  // d_theta
-        .k23_coeff = { -5.827814614802593e+01f, 7.789995488757775e+01f, -3.841148024725668e+01f, 8.034534049078013e+00f },  // x
-        .k24_coeff = { -8.937952443465080e+01f, 1.128943502182752e+02f, -5.293642666103645e+01f, 1.073722383888271e+01f },  // d_x
-        .k25_coeff = { 2.478483065877546e+02f, -2.463640234149189e+02f, 8.359617215530402e+01f, 8.324247402653134e+00f }, // phi
-        .k26_coeff = { 6.307211927250707e+01f, -6.266313408748906e+01f, 2.129449351279647e+01f, 9.249265186231070e-01f },  // d_phi
+    .k11_coeff = { -2.844277590546524e+02f, 3.014647915495374e+02f, -1.268350617667978e+02f, -3.081795747371510e+00f }, // theta
+    .k12_coeff = { -3.549555833965762e+00f, 4.386109489290642e+00f, -8.231027185636849e+00f, -1.385726427113662e-01f },  // d_theta
+    .k13_coeff = { -7.526831317875141e+01f, 7.503437037461828e+01f, -2.586278102813240e+01f, -2.882206207079063e-01f },  // x
+    .k14_coeff = { -6.463718727551078e+01f, 6.474653251665229e+01f, -2.345655849302561e+01f, -4.576568180320017e-01f },  // d_x
+    .k15_coeff = { -1.724417956662031e+01f, 2.623364741718471e+01f, -1.364297468210267e+01f, 2.161483799244492e+00f }, // phi
+    .k16_coeff = { -1.670817080995841e+01f, 2.265762117831611e+01f, -1.161008984543497e+01f, 2.496364045937191e+00f },  // d_phi
+    .k21_coeff = { 7.413511085596214e+01f, -6.090178313593463e+00f, -4.039446413615985e+01f, 1.653501139374634e+01f }, // theta
+    .k22_coeff = { 6.411680140494978e+00f, -3.006095633385133e+00f, -1.177334020684990e+00f, 1.672016772859867e+00f },  // d_theta
+    .k23_coeff = { -5.915654230562277e+01f, 7.939714736323532e+01f, -4.009665367872189e+01f, 8.868997496924505e+00f },  // x
+    .k24_coeff = { -6.361086354133386e+01f, 8.119104026481590e+01f, -3.922514760493152e+01f, 8.469126805164274e+00f },  // d_x
+    .k25_coeff = { 5.672537764809064e+01f, -5.809781196995141e+01f, 2.039529228765693e+01f, 5.636457827688945e+00f }, // phi
+    .k26_coeff = { 7.417061748885547e+01f, -7.440121006735944e+01f, 2.577732090617134e+01f, 8.817723457684845e-01f },  // d_phi
 },
        .theta = 0.0f,
        .x = 0.0f,
--- a/User/task/atti_esit.c
+++ b/User/task/atti_esit.c
@ -92,7 +92,7 @@ void Task_atti_esit(void *argument) {
    imu_for_chassis.gyro.z = bmi088.gyro.z;
    /* 欧拉角绕z轴旋转270度 */
    imu_for_chassis.euler.rol = eulr_to_send.pit;
-    imu_for_chassis.euler.pit = -eulr_to_send.rol;
+    imu_for_chassis.euler.pit = eulr_to_send.rol;
    imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度
    osMessageQueueReset(
        task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞
--- a/utils/.DS_Store
+++ b/utils/.DS_Store
--- a/utils/Simulation-master/.DS_Store
+++ b/utils/Simulation-master/.DS_Store
--- a/utils/Simulation-master/.gitignore
+++ b/utils/Simulation-master/.gitignore
@ -0,0 +1,13 @@
+# Duplicate files
+*(*)*
+
+# MATLAB files
+balance/parallel_legs/slprj
+balance/series_legs/slprj
+balance/series_parallel_legs/slprj
+*/slprj/
+*/codegen/
+*.prj
+*.slxc
+*.autosave
+*.asv
--- a/utils/Simulation-master/LICENSE
+++ b/utils/Simulation-master/LICENSE
@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 是小企鹅呀
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
--- a/utils/Simulation-master/balance/.DS_Store
+++ b/utils/Simulation-master/balance/.DS_Store
--- a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx
+++ b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx
--- a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original
+++ b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original
--- a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b
+++ b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b
--- a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a
+++ b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a
--- a/utils/Simulation-master/balance/parallel_legs/d_phi0.m
+++ b/utils/Simulation-master/balance/parallel_legs/d_phi0.m
@ -0,0 +1,20 @@
+
+syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
+
+XD=l5+l4*cos(phi_4);
+YD=l4*sin(phi_4);
+XB=l1*cos(phi_1);
+YB=l1*sin(phi_1);
+A0=2*l2*(XD-XB);
+B0=2*l2*(YD-YB);
+lBD=sqrt((XD-XB)^2+(YD-YB)^2);
+C0=l2^2+lBD^2-l3^2;
+phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
+XC=XB+l2*cos(phi_2);
+YC=YB+l2*sin(phi_2);
+L0=sqrt((XC-l5/2)^2+YC^2);
+phi_0=atan(YC/(XC-l5/2));
+
+J=jacobian([L0;phi_0],[phi_1;phi_4]);
+phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4
+
--- a/utils/Simulation-master/balance/parallel_legs/display_polynomial.m
+++ b/utils/Simulation-master/balance/parallel_legs/display_polynomial.m
@ -0,0 +1,19 @@
+% 定义一个函数来显示多项式方程
+function display_polynomial(coefficients, name)
+    equation = sprintf('%s = ', name);
+    n = length(coefficients);
+    for i = 1:n
+        if coefficients(i) ~= 0
+            if i == n
+                term = sprintf('%.4f', coefficients(i));
+            else
+                term = sprintf('%.4f*t%d', coefficients(i), n-i);
+            end
+            if i > 1 && coefficients(i) > 0
+                term = [' + ', term];
+            end
+            equation = [equation, term];
+        end
+    end
+    fprintf('%s;\n', equation);
+end
--- a/utils/Simulation-master/balance/parallel_legs/get_k.m
+++ b/utils/Simulation-master/balance/parallel_legs/get_k.m
@ -0,0 +1,82 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
+% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
+% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
+% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
+% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
+% 
+% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
+% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
+% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
+% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
+% grid on;                   %添加网格线
+% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);  %将网格线变成虚线
+% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
+% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
+% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
+% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
+% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
+% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
+% 
+% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
+% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
+% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
+% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
+% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
+% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
+toc
--- a/utils/Simulation-master/balance/parallel_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/parallel_legs/get_k_length.m
@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.072;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=-0.01;                          %机体质心距离转轴距离
+    mw1=0.80;                         %驱动轮质量
+    mp1=1.11;                         %杆质量
+    M1=12.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;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    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(1000 1 200 200 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[150 0;0 25];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
--- a/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx
+++ b/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx
--- a/utils/Simulation-master/balance/series_legs/blance_leg.slx
+++ b/utils/Simulation-master/balance/series_legs/blance_leg.slx
--- a/utils/Simulation-master/balance/series_legs/calc.mlx
+++ b/utils/Simulation-master/balance/series_legs/calc.mlx
--- a/utils/Simulation-master/balance/series_legs/doc/2_link.png
+++ b/utils/Simulation-master/balance/series_legs/doc/2_link.png
--- a/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md
+++ b/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md
@ -0,0 +1,154 @@
+# 2连杆分析
+
+参考：[五连杆运动学解算与VMC](https://zhuanlan.zhihu.com/p/613007726)
+
+![pic](./2_link.png)
+
+## 1 正运动学解算
+
+$\phi_1$和$\phi_2$可由电机编码器测量得到。
+
+$C$点坐标：
+
+$$
+\left \{
+    \begin{array}{l}
+        x_C = l_1\cos\phi_1 + l_2\cos\phi_2\\
+        y_C = l_1\sin\phi_1 + l_2\sin\phi_2
+    \end{array}
+\right .
+$$
+
+得：
+
+$$
+\left \{
+    \begin{array}{l}
+        L0 = \sqrt{x_C^2 + y_C^2} \\
+        \phi_0 = \arctan{\frac{y_C}{x_C}}
+    \end{array}
+\right .
+$$
+
+## 2 逆运动学解算
+
+由余弦定理易得：
+
+$$
+\phi_2+\phi_3 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}
+$$
+
+又：
+
+$$
+\phi_3 = \pi - \phi_1
+$$
+
+得：
+
+$$
+\phi_2 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}+\phi_1-\pi
+$$
+
+## 3 雅可比矩阵
+
+<!-- $$
+\left [
+    \begin{matrix}
+        \dot L0 \\
+        \dot \phi_0
+    \end{matrix}
+\right ]
+
+ =
+
+J
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$ -->
+
+基于文中描述可得：
+
+$$
+\left \{
+    \begin{array}{l}
+        \dot x_C = -l_1\dot\phi_1\sin\phi_1 - l_2\dot\phi_2\sin\phi_2\\
+        \dot y_C = l_1\dot\phi_1\cos\phi_1 + l_2\dot\phi_2\cos\phi_2
+    \end{array}
+\right .
+$$
+
+即：
+
+$$
+\left [
+    \begin{matrix}
+        \dot x_C \\
+        \dot y_C
+    \end{matrix}
+\right ]
+
+ =
+\left [
+    \begin{matrix}
+        -l_1\sin\phi_1 & -l_2\sin\phi_2 \\
+         l_1\cos\phi_1 &  l_2\cos\phi_2
+    \end{matrix}
+\right ]
+
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$
+
+记作：
+
+$$
+\left [
+    \begin{matrix}
+        \dot x_C \\
+        \dot y_C
+    \end{matrix}
+\right ]
+
+ =
+J_0
+
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$
+
+下面操作与文中相同，可得：
+
+$$
+J^T = J_0^TRM =
+\left[
+    \begin{matrix}
+        l_1 \,\sin \left(\phi_0 -\phi_1 \right) & \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 }\\
+        l_2 \,\sin \left(\phi_0 -\phi_2 \right) & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
+    \end{matrix}
+\right]
+$$
+
+即：
+
+$$
+J =
+\left[
+    \begin{matrix}
+        l_1 \,\sin \left(\phi_0 -\phi_1 \right) & l_2 \,\sin \left(\phi_0 -\phi_2 \right)\\
+        \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 } & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
+    \end{matrix}
+\right]
+$$
--- a/utils/Simulation-master/balance/series_legs/get_k.m
+++ b/utils/Simulation-master/balance/series_legs/get_k.m
@ -0,0 +1,96 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% 打印格式化的C代码结构，可以直接复制到config.c中
+fprintf('\n=========== 可直接复制的C代码 ===========\n');
+fprintf('.lqr_gains = {\n');
+fprintf('    .k11_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a11(1), a11(2), a11(3), a11(4));
+fprintf('    .k12_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_theta\n', a12(1), a12(2), a12(3), a12(4));
+fprintf('    .k13_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // x\n', a13(1), a13(2), a13(3), a13(4));
+fprintf('    .k14_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_x\n', a14(1), a14(2), a14(3), a14(4));
+fprintf('    .k15_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a15(1), a15(2), a15(3), a15(4));
+fprintf('    .k16_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_phi\n', a16(1), a16(2), a16(3), a16(4));
+fprintf('    .k21_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a21(1), a21(2), a21(3), a21(4));
+fprintf('    .k22_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_theta\n', a22(1), a22(2), a22(3), a22(4));
+fprintf('    .k23_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // x\n', a23(1), a23(2), a23(3), a23(4));
+fprintf('    .k24_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_x\n', a24(1), a24(2), a24(3), a24(4));
+fprintf('    .k25_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a25(1), a25(2), a25(3), a25(4));
+fprintf('    .k26_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_phi\n', a26(1), a26(2), a26(3), a26(4));
+fprintf('},\n');
+fprintf('========================================\n\n');
+
+% 可选：显示拟合曲线图
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% 
+% figure;
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合');
+% subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合');
+% subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合');
+% subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合');
+% subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合');
+% 
+% subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合');
+% subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合');
+% subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合');
+% subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合');
+% 
+% for i = 1:12
+%     subplot(3,4,i);
+%     grid on;                   %添加网格线
+%     set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5);  %将网格线变成虚线
+%     legend('原始数据','拟合曲线','Location','best');
+% end
+
+toc
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.068;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=-0.03;                          %机体质心距离转轴距离
+    mw1=0.60;                         %驱动轮质量
+    mp1=1.8;                         %杆质量
+    M1=12.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;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    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([100 100 2000 200 5000 600]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[240 0;0 40];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
--- a/utils/Simulation-master/balance/series_legs/leg_sim.slx
+++ b/utils/Simulation-master/balance/series_legs/leg_sim.slx
--- a/utils/Simulation-master/balance/series_parallel_legs/README.md
+++ b/utils/Simulation-master/balance/series_parallel_legs/README.md
@ -0,0 +1,3 @@
+# 串并联腿仿真总结
+
+这次仿真对小企鹅来说最重要的一点就是确定了替换为串联腿时不需要替换模型，直接移植现有并联腿模型即可。
--- a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx
+++ b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx
--- a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original
+++ b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original
--- a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b
+++ b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b
--- a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a
+++ b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a
--- a/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m
+++ b/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m
@ -0,0 +1,20 @@
+
+syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
+
+XD=l5+l4*cos(phi_4);
+YD=l4*sin(phi_4);
+XB=l1*cos(phi_1);
+YB=l1*sin(phi_1);
+A0=2*l2*(XD-XB);
+B0=2*l2*(YD-YB);
+lBD=sqrt((XD-XB)^2+(YD-YB)^2);
+C0=l2^2+lBD^2-l3^2;
+phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
+XC=XB+l2*cos(phi_2);
+YC=YB+l2*sin(phi_2);
+L0=sqrt((XC-l5/2)^2+YC^2);
+phi_0=atan(YC/(XC-l5/2));
+
+J=jacobian([L0;phi_0],[phi_1;phi_4]);
+phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4
+
--- a/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m
+++ b/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m
@ -0,0 +1,19 @@
+% 定义一个函数来显示多项式方程
+function display_polynomial(coefficients, name)
+    equation = sprintf('%s = ', name);
+    n = length(coefficients);
+    for i = 1:n
+        if coefficients(i) ~= 0
+            if i == n
+                term = sprintf('%.4f', coefficients(i));
+            else
+                term = sprintf('%.4f*t%d', coefficients(i), n-i);
+            end
+            if i > 1 && coefficients(i) > 0
+                term = [' + ', term];
+            end
+            equation = [equation, term];
+        end
+    end
+    fprintf('%s;\n', equation);
+end
--- a/utils/Simulation-master/balance/series_parallel_legs/get_k.m
+++ b/utils/Simulation-master/balance/series_parallel_legs/get_k.m
@ -0,0 +1,82 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
+% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
+% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
+% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
+% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
+% 
+% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
+% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
+% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
+% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
+% grid on;                   %添加网格线
+% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);  %将网格线变成虚线
+% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
+% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
+% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
+% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
+% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
+% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
+% 
+% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
+% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
+% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
+% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
+% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
+% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
+toc
--- a/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m
@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.086;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=0.03;                          %机体质心距离转轴距离
+    mw1=1.18;                         %驱动轮质量
+    mp1=1.11;                         %杆质量
+    M1=10.3;                          %机体质量
+    Iw1=mw1*R1^2;                     %驱动轮转动惯量
+    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
+    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    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([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[240 0;0 25];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
--- a/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx
+++ b/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx
--- a/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx
+++ b/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx
--- a/utils/Simulation-master/dog_simple/dog.slx
+++ b/utils/Simulation-master/dog_simple/dog.slx
--- a/utils/Simulation-master/dog_simple/dog.slx.r2023a
+++ b/utils/Simulation-master/dog_simple/dog.slx.r2023a
--- a/utils/k_calc/chassis_calc_heu.m
+++ b/utils/k_calc/chassis_calc_heu.m
@ -0,0 +1,78 @@
+%LQR求解
+clear;
+
+syms theta dot_theta ddot_theta;
+syms x dot_x ddot_x;
+syms x_body dot_x_body ddot_x_body;
+syms phi dot_phi ddot_phi;
+syms T T_p;
+syms R L L_M l m_w m_p M I_w I_p I_M g;
+body_fusion = 1;    %机体速度
+g = 9.8;            %重力加速度
+R = 0.076;          %轮半径
+m_w = 0.47; %轮质量
+m_p = 1.874;        %摆杆质量
+M = 6.975;           %机体质量
+I_w = 0.001974; %轮转动惯量
+I_M = 0.143;                 %机体转动惯量
+l = 0.0353;                  %机体质心离转轴距离
+
+Q_cost=diag([160 160 200 120 2000 50]);
+R_cost=diag([1, 0.25]);
+
+if body_fusion
+    ddot_x = ddot_x_body - (L+L_M)*cos(theta)*ddot_theta + (L+L_M)*sin(theta)*dot_theta^2;
+end
+%对机体受力分析
+N_M = M * (ddot_x + (L + L_M) * (-dot_theta^2*sin(theta) + ddot_theta*cos(theta)) - l*(-dot_phi^2*sin(phi) + ddot_phi*cos(phi)));
+P_M = M*g + M*((L+L_M)*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta)) + l*(-dot_phi^2*cos(phi) - ddot_phi*sin(phi)));
+N = N_M + m_p*(ddot_x + L*(-dot_theta^2*sin(theta) + ddot_theta*cos(theta)));
+P = P_M + m_p*g + m_p*L*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta));
+
+%方程求解
+equ1 = ddot_x - (T - N*R)/(I_w/R + m_w*R);
+equ2 = (P*L + P_M*L_M)*sin(theta) - (N*L+N_M*L_M)*cos(theta) - T + T_p - I_p*ddot_theta;
+equ3 = T_p + N_M * l * cos(phi) + P_M * l * sin(phi) - I_M * ddot_phi;
+if body_fusion
+    [ddot_theta, ddot_x_body, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x_body, ddot_phi]);
+    Ja = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [theta, dot_theta, x_body, dot_x_body, phi, dot_phi]);
+    Jb = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [T, T_p]);
+    
+    A = simplify(vpa(subs(Ja, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
+    B = simplify(vpa(subs(Jb, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
+    
+else
+    [ddot_theta, ddot_x, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x, ddot_phi]);
+    Ja = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [theta, dot_theta, x, dot_x, phi, dot_phi]);
+    Jb = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [T, T_p]);
+    
+    A = simplify(vpa(subs(Ja, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
+    B = simplify(vpa(subs(Jb, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
+end
+
+%% LQR计算
+
+leg_var = 0.096;
+K=zeros(30,12);
+leglen=zeros(30,1);
+for i=1:30
+    leg_var=leg_var+0.01; % 10mm线性化一次
+    llm= 0.8424 * leg_var - 0.0272;
+    ll = 0.1576 * leg_var + 0.0272;
+    leglen(i)=leg_var;
+    i_p = 0.0328 * leg_var + 0.0216;
+    trans_A=double(subs(A,[L L_M I_p],[ll llm i_p]));
+    trans_B=double(subs(B,[L L_M I_p],[ll llm i_p]));
+    KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 系数拟合
+K_cons=zeros(12,3);  %排列顺序是
+
+for i=1:12
+    res=fit(leglen,K(:,i),'poly2');
+    K_cons(i,:)=[res.p1,res.p2,res.p3];
+end
+disp(K_cons)
--- a/utils/matlab/LQR_calc.m
+++ b/utils/matlab/LQR_calc.m
@ -0,0 +1,86 @@
+clear;
+clc;
+
+syms x xd xdd     T Tp    thetadd thetad theta    phidd phid phi    P N PM NM    L LM;
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;  
+
+% 驱动轮
+R = 0.075;          %轮子半径
+mw = 0.58;          %轮子质量
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;  % 需要加上定子质量
+Ip = mp*L^2/3;  %摆杆转动惯量
+
+% 机体
+M = 12;        %机体重量
+l = -0.014;    %机体质心到关节电机转轴的距离
+IM = 0.124;
+
+%% 经典力学方程
+fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
+fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
+fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
+fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
+
+%% 不同部件之间的力求解
+[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
+f1=xdd==(T-N*R)/(Iw/R+mw*R);
+f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
+[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
+
+%% 计算雅可比矩阵A and B
+func=[thetad,thetadd,xd,xdd,phid,phidd];
+A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
+temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_A=simplify(temp_A);
+
+B_lin_model=jacobian(func,[T Tp]);
+temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_B=simplify(temp_B);
+
+%% 计算不同腿长下LQR增益K
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+Q_mat = diag([1,1,500,100,5000,1]);
+R_mat = diag([1,0.25]);
+K = zeros(20,12);
+leg_len = zeros(20,1);
+
+for i=1:20
+    L_var = L_var + 0.005;
+    leg_len(i) = L_var*2;
+    A = double(subs(final_A, [L LM], [L_var L_var]));
+    B = double(subs(final_B, [L LM], [L_var L_var]));
+    KK = lqrd(A, B, Q_mat, R_mat, 0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 不同腿长下二项式拟合K
+K_cons=zeros(12,3);  
+
+for i=1:12
+    res=fit(leg_len,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1, res.p2, res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end
--- a/utils/matlab/UseBodyVel.m
+++ b/utils/matlab/UseBodyVel.m
@ -0,0 +1,103 @@
+clear;
+clc;
+syms theta phi L x x_b N N_f T T_p  M N_M P_M L_M
+syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
+syms x_b_dot x_b_ddot
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;           %重力加速度
+
+% 驱动轮
+mw = 0.58;          %轮子质量
+R = 0.075;          %轮子半径
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;
+Ip = mp*L^2/3;   % 摆杆转动惯量
+
+% 机体
+M = 12;         %机体重量
+IM = 0.124;     %机体惯量,绕质心
+l = -0.014;     %机体质心到电机转轴的距离
+
+% QR设置为相同的权重
+Q_cost = diag([1,1,500,100,5000,1]);
+R_cost = diag([1,0.25]);
+useBodyVelocity = 1;
+
+%% 经典力学方程
+if useBodyVelocity
+    x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
+end
+N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
+P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
+N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
+P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
+
+eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
+eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
+eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
+
+%% 计算雅可比矩阵
+U = [T T_p].';
+
+if useBodyVelocity
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
+    X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_b_dot,simplify(model_sol.x_b_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+else
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
+    X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_dot,simplify(model_sol.x_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+end
+
+%% 计算变长度LQR
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+K=zeros(20,12);
+leglen=zeros(20,1);
+
+for i=1:20
+    L_var=L_var+0.005; % 10mm线性化一次
+    leglen(i)=L_var*2;
+    trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
+    trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
+    KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 用二项式拟合K,一共12个参数
+K_cons=zeros(12,3);
+
+for i=1:12
+    res=fit(leglen,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1,res.p2,res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end
+
+
--- a/utils/matlab/vmc.m
+++ b/utils/matlab/vmc.m
@ -0,0 +1,39 @@
+clear;
+clc;
+
+syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
+syms phi_dot_1 phi_dot_4
+syms l1 l2 l3 l4
+
+%% 腿长解算
+x_B = l1*cos(phi1);
+y_B = l1*sin(phi1);
+x_C = x_B+l2*cos(phi2);
+y_C = y_B+l2*sin(phi2);
+x_D = l5+l4*cos(phi4);
+y_D = l4*sin(phi4);
+x_dot_B = diff(x_B,t);
+y_dot_B = diff(y_B,t);
+x_dot_C = diff(x_C,t);
+y_dot_C = diff(y_C,t);
+x_dot_D = diff(x_D,t);
+y_dot_D = diff(y_D,t);
+
+%% 速度导数求解
+phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
+
+x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
+x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
+y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+
+%% 运动映射(虚功原理)+极坐标转换
+x_dot = [x_dot_C; y_dot_C];
+q_dot = [phi_dot_1; phi_dot_4];
+x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
+J = simplify(jacobian(x_dot,q_dot));
+rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
+          sin(phi0-pi/2)  cos(phi0-pi/2)];
+map = [0 -1/L0;
+       1 0];
+Trans_Jacobian = simplify(J.'*rotate*map);
--- a/utils/matlab/wheel_leg.slx
+++ b/utils/matlab/wheel_leg.slx
--- a/utils/matlab/wheel_leg.slxc
+++ b/utils/matlab/wheel_leg.slxc
--- a/utils/mod_wheelleg_chassis.cpp
+++ b/utils/mod_wheelleg_chassis.cpp
@ -0,0 +1,731 @@
+#include "mod_wheelleg_chassis.hpp"
+
+#include <cmath>
+#include <tuple>
+
+using namespace Module;
+Wheelleg::Wheelleg(Param &param)
+    : param_(param),
+      roll_pid_(param.roll_pid_param, 333.0f),
+      yaw_pid_(param.yaw_pid_param, 333.0f),
+      yaccl_pid_(param.yaccl_pid_param, 333.0f),
+      lowfilter_(333.0f, 50.0f),
+      acclfilter_(333.0f, 30.0f),
+      manfilter_(param.manfilter_param),
+
+      ctrl_lock_(true)
+{
+  memset(&(this->cmd_), 0, sizeof(this->cmd_));
+
+  this->hip_motor_.at(0) =
+      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
+  this->hip_motor_.at(1) =
+      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
+  this->hip_motor_.at(2) =
+      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
+  this->hip_motor_.at(3) =
+      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
+
+  this->wheel_motor_.at(0) =
+      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
+  this->wheel_motor_.at(1) =
+      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
+
+  this->leglength_pid_.at(0) =
+      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
+  this->leglength_pid_.at(1) =
+      new Component::PID(param.leglength_pid_param.at(1), 333.0f);
+
+  this->theta_pid_.at(0) =
+      new Component::PID(param.theta_pid_param.at(0), 333.0f);
+  this->theta_pid_.at(1) =
+      new Component::PID(param.theta_pid_param.at(1), 333.0f);
+
+  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
+  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
+
+  this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
+  this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
+
+  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg *chassis)
+  {
+    chassis->ctrl_lock_.Wait(UINT32_MAX);
+    switch (event)
+    {
+    case SET_MODE_RELAX:
+      chassis->SetMode(RELAX);
+      break;
+    case SET_MODE_STAND:
+      chassis->SetMode(STAND);
+      break;
+    case SET_MODE_ROTOR:
+      chassis->SetMode(ROTOR);
+      break;
+    case SET_MODE_RESET:
+      chassis->SetMode(RESET);
+      break;
+    default:
+      break;
+    }
+    chassis->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Wheelleg *, ChassisEvent>(
+      event_callback, this, this->param_.EVENT_MAP);
+
+  auto chassis_thread = [](Wheelleg *chassis)
+  {
+    auto cmd_sub =
+        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
+    auto eulr_sub =
+        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
+    auto gyro_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
+
+    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+
+    auto accl_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
+    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1)
+    {
+      eulr_sub.DumpData(chassis->eulr_); /* imu */
+      gyro_sub.DumpData(chassis->gyro_); /* imu */
+      accl_sub.DumpData(chassis->accl_);
+
+      yaw_sub.DumpData(chassis->yaw_);
+      cmd_sub.DumpData(chassis->cmd_);
+      // yaw_sub.DumpData(chassis->yaw_);
+
+      chassis->ctrl_lock_.Wait(UINT32_MAX);
+      chassis->MotorSetAble();
+      chassis->UpdateFeedback();
+      chassis->Calculate();
+      chassis->Control();
+      chassis->ctrl_lock_.Post();
+
+      /* 运行结束，等待下一次唤醒 */
+      chassis->thread_.SleepUntil(3, last_online_time);
+    }
+  };
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
+                       System::Thread::MEDIUM);
+}
+
+void Wheelleg::MotorSetAble()
+{
+  if (this->hip_motor_flag_ == 0)
+  {
+    this->hip_motor_[0]->Relax();
+    this->hip_motor_[1]->Relax();
+    this->hip_motor_[2]->Relax();
+    this->hip_motor_[3]->Relax();
+    this->dm_motor_flag_ = 0;
+  }
+
+  else
+  {
+    if (this->dm_motor_flag_ == 0)
+    {
+      for (int i = 0; i < 5; i++)
+      {
+        this->hip_motor_[0]->Enable();
+      }
+      for (int i = 0; i < 5; i++)
+      {
+        this->hip_motor_[1]->Enable();
+      }
+      for (int i = 0; i < 5; i++)
+      {
+        this->hip_motor_[2]->Enable();
+      }
+      for (int i = 0; i < 5; i++)
+      {
+        this->hip_motor_[3]->Enable();
+      }
+
+      this->dm_motor_flag_ = 1;
+    }
+  };
+}
+
+void Wheelleg::UpdateFeedback()
+{
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+  this->last_wakeup_ = this->now_;
+
+  this->wheel_motor_[0]->Update();
+  this->wheel_motor_[1]->Update();
+
+  this->leg_argu_[0].phi4_ =
+      this->hip_motor_[0]->GetAngle() -
+      this->param_.wheel_param.mech_zero[0]; // 前关节角度
+  this->leg_argu_[0].phi1_ =
+      this->hip_motor_[1]->GetAngle() -
+      this->param_.wheel_param.mech_zero[1]; // 后关节角度
+
+  this->leg_argu_[1].phi4_ =
+      (-this->hip_motor_[2]->GetAngle() +
+       this->param_.wheel_param.mech_zero[3]); // 前关节角度
+  if (leg_argu_[1].phi4_ > M_PI)
+  {
+    this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
+  }
+
+  this->leg_argu_[1].phi1_ =
+      (-this->hip_motor_[3]->GetAngle() +
+       this->param_.wheel_param.mech_zero[2]); // 后关节角度
+  if (leg_argu_[1].phi1_ > M_PI)
+  {
+    this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
+  }
+
+  this->pit_ = this->eulr_.pit;
+  if (this->pit_ > M_PI)
+  {
+    this->pit_ = this->eulr_.pit - 2 * M_PI;
+  }
+
+  /*  注意极性 */
+  std::tuple<float, float, float, float> result0 =
+      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[0].L0 = std::get<0>(result0);
+  this->leg_argu_[0].d_L0 = std::get<1>(result0);
+  this->leg_argu_[0].theta = -std::get<2>(result0);
+  this->leg_argu_[0].d_theta = -std::get<3>(result0);
+
+  if (leg_argu_[0].theta > M_PI)
+  {
+    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
+  }
+  if (leg_argu_[0].theta < -M_PI)
+  {
+    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
+  }
+
+  std::tuple<float, float, float, float> result1 =
+      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[1].L0 = std::get<0>(result1);
+  this->leg_argu_[1].d_L0 = std::get<1>(result1);
+  this->leg_argu_[1].theta = -std::get<2>(result1);
+  this->leg_argu_[1].d_theta = -std::get<3>(result1);
+
+  if (leg_argu_[1].theta > M_PI)
+  {
+    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
+  }
+  if (leg_argu_[1].theta < -M_PI)
+  {
+    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
+  }
+
+  /* 轮子转速近似于编码器速度 由此推测机体速度 */
+  this->leg_argu_[0].single_x_dot =
+      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
+
+  this->leg_argu_[1].single_x_dot =
+      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
+
+  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
+  this->move_argu_.x_dot =
+
+      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
+  this->move_argu_.x_dot =
+      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
+      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
+  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
+
+  this->move_argu_.delta_speed =
+      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
+
+  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
+
+  if (now_ > 1000000)
+  {
+    this->move_argu_.x_dot_hat =
+        manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
+                               this->move_argu_.last_x_dot, accl_.y * 9.8f,
+                               dt_) -
+        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
+         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
+            2;
+    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
+  }
+}
+
+void Wheelleg::Calculate()
+{
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
+    // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
+    //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
+    //     move_argu_.target_dot_x -= 0.004;
+    //   }
+    //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
+    //     move_argu_.target_dot_x += 0.004;
+    //   }
+    // } else {
+    //   move_argu_.target_dot_x = cmd_.y * 1.5f;
+    // }
+    // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
+    // move_argu_.target_x;
+    move_argu_.target_x = 0;
+    move_argu_.target_dot_x = 0;
+    break;
+  case Wheelleg::STAND:
+
+    /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
+    if (fabs(move_argu_.target_dot_x -
+             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
+    {
+      if (move_argu_.target_dot_x >
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x -= 0.004;
+      }
+      if (move_argu_.target_dot_x <
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x += 0.004;
+      }
+    }
+    else
+    {
+      move_argu_.target_dot_x =
+          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+    }
+    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+
+    this->move_argu_.target_yaw = 0.0f;
+
+    /*双零点*/
+    if (this->yaw_ > M_PI_2)
+    {
+      this->move_argu_.target_yaw = 3.14158f;
+    }
+    if (this->yaw_ < -M_PI_2)
+    {
+      this->move_argu_.target_yaw = 3.14158f;
+    }
+    break;
+
+  case Wheelleg::ROTOR:
+    if (fabs(move_argu_.target_dot_x -
+             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
+    {
+      if (move_argu_.target_dot_x >
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x -= 0.004;
+      }
+      if (move_argu_.target_dot_x <
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x += 0.004;
+      }
+    }
+    else
+    {
+      move_argu_.target_dot_x =
+          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+    }
+    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+    this->move_argu_.target_yaw = this->yaw_ + 0.01;
+
+    break;
+    // move_argu_.target_dot_x = cmd_.x;
+    // move_argu_.target_x =
+    //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+    // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
+    // cmd_.y);
+    // // move_argu_.x_dot =
+    // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+    // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
+    // break;
+
+  case Wheelleg::RESET:
+    this->move_argu_.target_dot_x = 0;
+    move_argu_.target_x = 0;
+    this->move_argu_.target_yaw = this->eulr_.yaw;
+    this->move_argu_.xhat = 0;
+
+    //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
+    // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
+    //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
+    // }
+    break;
+
+  default:
+    XB_ASSERT(false);
+    return;
+  }
+
+  leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
+  onground0_flag_ = false;
+  if (leg_argu_[0].Fn > 30)
+  {
+    onground0_flag_ = true;
+  }
+  leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
+  onground1_flag_ = false;
+  if (leg_argu_[1].Fn > 30)
+  {
+    onground1_flag_ = true;
+  }
+  std::tuple<float, float> result3;
+  std::tuple<float, float> result4;
+
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
+  case Wheelleg::ROTOR:
+  case Wheelleg::STAND:
+
+    for (int i = 0; i < 12; i++)
+    {
+      leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
+          &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
+          leg_argu_[0].L0);
+    }
+
+    for (int i = 0; i < 12; i++)
+    {
+      leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
+          &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
+          leg_argu_[1].L0);
+    }
+    if (now_ > 1000000)
+      if (fabs(move_argu_.target_dot_x) > 0.5 ||
+          fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
+          ((!onground0_flag_) & (!onground1_flag_)))
+      {
+        leg_argu_[0].LQR_K[2] = 0;
+        leg_argu_[1].LQR_K[2] = 0;
+        this->move_argu_.xhat = 0;
+        move_argu_.target_x = 0;
+      }
+
+    if (onground0_flag_)
+    {
+      leg_argu_[0].Tw =
+          (leg_argu_[0].LQR_K[0] *
+               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+           leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
+           leg_argu_[0].LQR_K[2] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[0].LQR_K[3] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
+           leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
+      leg_argu_[0].Tp =
+          (leg_argu_[0].LQR_K[6] *
+               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
+           leg_argu_[0].LQR_K[8] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[0].LQR_K[9] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
+           leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
+    }
+    else
+    {
+      leg_argu_[0].Tw = 0;
+
+      leg_argu_[0].Tp =
+          (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
+           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+    }
+    if (onground1_flag_)
+    {
+      leg_argu_[1].Tw =
+          (leg_argu_[1].LQR_K[0] *
+               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+           leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
+           leg_argu_[1].LQR_K[2] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[1].LQR_K[3] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
+           leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
+      leg_argu_[1].Tp =
+          (leg_argu_[1].LQR_K[6] *
+               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
+           leg_argu_[1].LQR_K[8] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[1].LQR_K[9] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
+           leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
+    }
+    else
+    {
+      leg_argu_[1].Tw = 0;
+      leg_argu_[1].Tp =
+          (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
+           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+    }
+
+    this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                  this->param_.wheel_param.static_L0[0] +
+                                  +roll_pid_.Calculate(0, eulr_.rol, dt_);
+    this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                  this->param_.wheel_param.static_L0[1] -
+                                  roll_pid_.Calculate(0, eulr_.rol, dt_);
+
+    leg_argu_[0].F0 =
+        leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
+        this->param_.wheel_param.static_F0[0] +
+        leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
+                                        this->leg_argu_[0].L0, this->dt_);
+    leg_argu_[1].F0 =
+        leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
+        this->param_.wheel_param.static_F0[1] +
+        leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
+                                        this->leg_argu_[1].L0, this->dt_);
+
+    this->leg_argu_[0].Delta_Tp =
+        leg_argu_[0].L0 * 10.0f *
+        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                 this->leg_argu_[0].theta, this->dt_);
+    this->leg_argu_[1].Delta_Tp =
+        -leg_argu_[1].L0 * 10.0f *
+        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                 this->leg_argu_[0].theta, this->dt_);
+
+    result3 = leg_[0]->VMCinserve(
+        -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+        -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
+    this->leg_argu_[0].T1 = std::get<0>(result3);
+    this->leg_argu_[0].T2 = std::get<1>(result3);
+
+    result4 = leg_[1]->VMCinserve(
+        -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+        -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
+    this->leg_argu_[1].T1 = -std::get<0>(result4);
+    this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+    if (onground0_flag_ & onground1_flag_)
+    {
+      move_argu_.yaw_force =
+          -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
+    }
+    else
+    {
+      move_argu_.yaw_force = 0;
+    }
+    /*3508不带减速箱是Tw*3.2f*/
+    /*2006带减速是Tw/1.8f*/
+    /* 3508带15.7减速箱是Tw/4.9256 */
+
+    this->wheel_motor_out_[0] =
+        this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
+
+    this->wheel_motor_out_[1] =
+        this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
+
+    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+    this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+    this->hip_motor_flag_ = 1;
+    break;
+
+  case Wheelleg::RESET:
+    if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
+        fabs(leg_argu_[1].theta) > 1.57)
+    {
+      leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+
+      if (fabs(pit_) > M_PI / 2)
+      {
+        if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03)
+        {
+          leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
+        }
+        if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03)
+        {
+          leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
+        }
+        leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
+                                   0.65f, this->leg_argu_[0].L0, this->dt_);
+        leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
+                                   0.65f, this->leg_argu_[1].L0, this->dt_);
+      }
+      if (fabs(leg_argu_[0].theta) < 1.57)
+      {
+        leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
+        leg_argu_[0].target_theta = leg_argu_[0].theta;
+      }
+
+      if (fabs(leg_argu_[1].theta) < 1.57)
+      {
+        leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+        leg_argu_[1].target_theta = leg_argu_[1].theta;
+      }
+
+      if (leg_argu_[0].target_theta > M_PI)
+      {
+        leg_argu_[0].target_theta -= 2 * M_PI;
+      }
+      if (leg_argu_[0].target_theta < -M_PI)
+      {
+        leg_argu_[0].target_theta += 2 * M_PI;
+      }
+      if (leg_argu_[1].target_theta > M_PI)
+      {
+        leg_argu_[1].target_theta -= 2 * M_PI;
+      }
+      if (leg_argu_[1].target_theta < -M_PI)
+      {
+        leg_argu_[1].target_theta += 2 * M_PI;
+      }
+      leg_argu_[0].Tp =
+          500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
+                                               leg_argu_[0].theta, dt_);
+      leg_argu_[1].Tp =
+          500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
+                                               leg_argu_[1].theta, dt_);
+    }
+    else
+    {
+      leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
+                                0.10f, this->leg_argu_[0].L0, this->dt_);
+      leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
+                                0.10f, this->leg_argu_[1].L0, this->dt_);
+
+      if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20))
+      {
+        leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
+                                    0.1, leg_argu_[0].theta, dt_);
+        leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
+                                    0.1, leg_argu_[1].theta, dt_);
+        clampf(&leg_argu_[0].Tp, -10, 10);
+        clampf(&leg_argu_[1].Tp, -10, 10);
+      }
+      else
+      {
+        leg_argu_[0].Tp = 0;
+        leg_argu_[1].Tp = 0;
+      }
+    }
+
+    result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                                  -leg_argu_[0].Tp, leg_argu_[0].F0);
+    this->leg_argu_[0].T1 = std::get<0>(result3);
+    this->leg_argu_[0].T2 = std::get<1>(result3);
+
+    result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                                  -leg_argu_[1].Tp, leg_argu_[1].F0);
+    this->leg_argu_[1].T1 = -std::get<0>(result4);
+    this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+    this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+    this->hip_motor_flag_ = 1;
+    break;
+  }
+}
+
+void Wheelleg::Control()
+{
+  clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
+  clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
+  clampf(&hip_motor_out_[0], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[3], -20.0f, 20.0f);
+
+  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
+  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
+  //   wheel_motor_out_[0] = 0;
+  //   wheel_motor_out_[1] = 0;
+
+  //   if (fabs(this->pit_) > 0.5f) {
+  //     this->hip_motor_flag_ = 0;
+  //   }
+  // }
+
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
+
+    this->wheel_motor_[0]->Relax();
+    this->wheel_motor_[1]->Relax();
+    hip_motor_flag_ = 0;
+    hip_motor_[0]->SetMit(0.0f);
+    hip_motor_[1]->SetMit(0.0f);
+    hip_motor_[2]->SetMit(0.0f);
+    hip_motor_[3]->SetMit(0.0f);
+    break;
+
+  case Wheelleg::STAND:
+  case Wheelleg::ROTOR:
+    // this->wheel_motor_[0]->Relax();
+    // this->wheel_motor_[1]->Relax();
+    this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
+    this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
+    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+    // hip_motor_[0]->SetMit(0.0f);
+    // hip_motor_[1]->SetMit(0.0f);
+    // hip_motor_[2]->SetMit(0.0f);
+    // hip_motor_[3]->SetMit(0.0f);
+    break;
+
+  case Wheelleg::RESET:
+
+    this->wheel_motor_[0]->Relax();
+    this->wheel_motor_[1]->Relax();
+
+    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+
+    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+
+    break;
+  }
+}
+
+void Wheelleg::SetMode(Wheelleg::Mode mode)
+{
+  if (mode == this->mode_)
+  {
+    return;
+  }
+
+  this->leg_[0]->Reset();
+  this->leg_[1]->Reset();
+  move_argu_.x = 0;
+  move_argu_.x_dot = 0;
+  move_argu_.last_x_dot = 0;
+  move_argu_.target_x = move_argu_.xhat;
+  move_argu_.target_yaw = this->eulr_.yaw;
+  move_argu_.target_dot_x = 0;
+  move_argu_.xhat = 0;
+  move_argu_.x_dot_hat = 0;
+  this->manfilter_.reset_comp();
+  this->mode_ = mode;
+}
--- a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c
+++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c
--- a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h
+++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h
@ -0,0 +1,299 @@
+#ifndef _CHASSIS_TASK
+#define _CHASSIS_TASK
+
+#include "main.h"
+#include "struct_typedef.h"
+#include "pid.h"
+#include "bsp_can.h"
+// ------------- Limit info ------------- 
+#define MAX_ACCL 13000.0f
+#define MAX_ACCL_JOINT 15.0f
+#define MAX_FOOT_OUTPUT 2048
+
+//  ------------- Target value info ------------- 
+#define SIT_MODE_HEIGHT_SET            0.18f
+#define NORMAL_MODE_HEIGHT_SET         0.10f
+#define HIGH_MODE_HEIGHT_SET           0.27f
+#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
+
+// ------------- Mech info ------------- 
+#define L1 0.15f
+#define L2 0.25f
+#define L3 0.25f
+#define L4 0.15f
+#define L5 0.1f
+
+#define WHEEL_PERIMETER 0.56547
+#define WHEEL_RADIUS 0.09f
+#define LEG_OFFSET       0.3790855135f // 水平位置到上限位的夹角
+#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
+#define LOWER_SUPPORT_FORCE 0.0f
+#define MOVE_LOWER_BOUND 0.3f
+#define EXIT_PITCH_ANGLE 0.1f
+#define DANGER_PITCH_ANGLE 0.25f
+
+#define FEED_f 7.5f
+#define FEED_f_1 3.5f
+
+#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
+
+#define MOTOR_POS_UPPER_BOUND 0.05f
+#define MOTOR_POS_LOWER_BOUND 1.4f
+#define LIMITED_TORQUE 0.5f
+#define UNLIMITED_TORQUE 200.0f
+
+// ------------- Time info ------------- 
+#define CHASSIS_TASK_INIT_TIME  500
+#define TASK_RUN_TIME           0.002f
+
+// ------------- Transfer info ------------- 
+#define MOTOR_ECD_TO_RAD     0.00019174779      // 2*PI / 32767
+#define HALF_ECD_RANGE                14383
+#define HALF_POSITION_RANGE 3.0f
+// #define CC                          0.00512f 
+// #define CC                          1/494.0f
+#define TORQ_K                      494.483818182
+// ------------- Math info ------------- 
+#define PI2					  6.28318530717959f
+#define PI					  3.14159265358979f
+#define PI_2				  1.57079632679489f
+#define PI_4				  0.78539816339744f
+
+
+
+typedef enum
+{
+    ENABLE_CHASSIS = 0,
+    DISABLE_CHASSIS,
+} chassis_mode_e;
+
+typedef enum
+{
+    NO_FORCE,
+    FOOT_LAUNCHING,
+    JOINT_LAUNCHING,
+    BALANCING_READY,
+    JOINT_REDUCING,
+} chassis_balancing_mode_e;
+
+typedef enum
+{
+    NONE,
+    NORMAL_MOVING_MODE,
+    ABNORMAL_MOVING_MODE,
+    JUMPING_MODE,
+    CAP_MODE,
+    FLY_MODE,
+    TK_MODE,
+} sport_mode_e;
+
+typedef enum
+{
+    READY_TO_JUMP,
+    PREPARING_LANDING,
+    PREPARING_STAND_JUMPING,
+    CONSTACTING_LEGS,
+    EXTENDING_LEGS,
+    CONSTACTING_LEGS_2,
+    FINISHED,
+} jumping_stage_e;
+
+typedef enum
+{
+    NOT_DEFINE,
+    STANDING_JUMP,
+    MOVING_JUMP,
+} jumping_mode_e;
+
+typedef enum
+{
+    SIT_MODE = 0,
+    NORMAL_MODE,
+    HIGH_MODE,
+    EXTREMELY_HIGH_MODE,
+    CHANGING_HIGH,
+} chassis_high_mode_e;
+
+typedef enum
+{
+    MOTOR_NO_FORCE = 0,
+    MOTOR_FORCE,
+} chassis_motor_mode_e;
+
+typedef enum
+{
+	ON_GROUND = 0,
+	OFF_GROUND = 1,
+} suspend_flag_e;
+
+typedef struct
+{
+    chassis_mode_e           chassis_mode, last_chassis_mode;
+    chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
+    sport_mode_e             sport_mode, last_sport_mode; 
+    
+    jumping_mode_e           jumping_mode, last_jumping_mode;
+    jumping_stage_e          jumping_stage, last_jumping_stage;
+
+    chassis_high_mode_e       chassis_high_mode, last_chassis_high_mode;
+
+} mode_t;
+
+typedef struct
+{
+    const fp32 *chassis_INS_angle_point;
+  	const fp32 *chassis_INS_gyro_point;
+    const fp32 *chassis_INS_accel_point;
+    fp32 yaw_angle, pitch_angle, roll_angle;
+    fp32 yaw_gyro, pitch_gyro, roll_gyro;
+    fp32 yaw_accel, pitch_accel, roll_accel;
+
+    fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
+    fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
+
+    fp32 ideal_high;
+    fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
+    fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
+    fp32 leg_dlength_L;
+    fp32 leg_dlength_R;
+
+    fp32 foot_roll_angle;
+    fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
+    fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
+    fp32 leg_gyro_L, leg_gyro_R;
+
+    fp32 foot_distance, foot_distance_K, foot_distance_set;
+    fp32 foot_speed, foot_speed_K, foot_speed_set;
+
+    fp32 supportive_force_L, supportive_force_R;
+
+} chassis_posture_info_t;
+
+typedef struct
+{
+    // -------- horizontal force -------- 
+    fp32 joint_balancing_torque_L, joint_balancing_torque_R;
+    fp32 foot_balancing_torque_L,  foot_balancing_torque_R;
+	
+	fp32 foot_moving_torque_L,  foot_moving_torque_R;
+    fp32 joint_moving_torque_L, joint_moving_torque_R;
+
+    fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
+
+    fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
+    fp32 foot_horizontal_torque_L,  foot_horizontal_torque_R;
+
+    fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
+    fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
+
+    fp32 yaw_torque;
+
+    // -------- vertical force -------- 
+    fp32 joint_roll_torque_L,  joint_roll_torque_R;
+    fp32 joint_stand_torque_L, joint_stand_torque_R;
+
+    fp32 joint_vertical_torque_L,      joint_vertical_torque_R;
+    fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
+
+    fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
+    fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
+
+} torque_info_t;
+
+typedef struct 
+{
+    fp32 J1_L,J2_L;
+	fp32 J3_L,J4_L;
+	fp32 J1_R,J2_R;
+	fp32 J3_R,J4_R;
+	fp32 invJ1_L,invJ2_L;
+	fp32 invJ3_L,invJ4_L;
+	fp32 invJ1_R,invJ2_R;
+	fp32 invJ3_R,invJ4_R;
+
+} mapping_info_t;
+
+typedef struct
+{
+    const HT_motor_measure_t *motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 position;
+    fp32 init_position;
+    fp32 position_offset;
+
+    fp32 velocity;
+    fp32 torque_out, torque_get;
+    fp32 max_torque, min_torque;
+} joint_motor_t;
+
+typedef struct
+{
+    motor_measure_t motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 distance, distance_offset, last_position, position, turns;
+    fp32 speed;
+    fp32 torque_out, torque_get;
+
+} foot_motor_t;
+
+typedef struct
+{
+    bool_t init_flag;
+	suspend_flag_e suspend_flag_L, last_suspend_flag_L;
+    suspend_flag_e suspend_flag_R, last_suspend_flag_R;
+    bool_t Ignore_Off_Ground;
+    bool_t abnormal_flag;
+    bool_t static_flag, last_static_flag;
+    bool_t moving_flag, last_moving_flag;
+    bool_t rotation_flag;
+    bool_t controlling_flag;
+    bool_t set_pos_after_moving;
+    bool_t overpower_warning_flag;
+    bool_t last_overpower_warning_flag;
+    bool_t stablize_high_flag;
+    bool_t last_stablize_high_flag;
+} flag_info_t;
+
+typedef struct 
+{
+	pid_type_def buffer_control_pid;
+	pid_type_def cap_pid;
+	pid_type_def scale_down_pid;
+} pid_info_t;
+
+typedef struct
+{
+    mode_t                  mode;
+    chassis_posture_info_t  chassis_posture_info;
+    torque_info_t           torque_info;
+    mapping_info_t          mapping_info;
+    flag_info_t             flag_info;
+	pid_info_t              pid_info;
+    const Gimbal_ctrl_t     *chassis_rc_ctrl;
+
+    joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
+    foot_motor_t foot_motor_L, foot_motor_R;
+
+}chassis_control_t;
+
+enum Chassis_Mode
+{
+  Chassis_No_Force = 0,
+  Follow_Gimbal,
+  Follow_Gimbal_90Deg,
+  No_Follow,
+  Rotate,
+//   TK_MODE,
+};
+
+extern enum Chassis_Mode chassis_mode;
+extern chassis_control_t chassis_control;
+extern fp32 roll_PD[2];
+
+#endif
--- a/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m
+++ b/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m
@ -0,0 +1,321 @@
+% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
+% v2：添加了可以专门调试定腿长的功能（2024/05/08）
+% v3：优化部分注释，添加单位说明（2024/05/15）
+% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
+
+% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
+
+%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
+tic
+clear all
+clc
+
+% 定义机器人机体参数
+syms R_w                % 驱动轮半径
+syms R_l                % 驱动轮轮距/2
+syms l_l l_r            % 左右腿长
+syms l_wl l_wr          % 驱动轮质心到左右腿部质心距离
+syms l_bl l_br          % 机体质心到左右腿部质心距离
+syms l_c                % 机体质心到腿部关节中心点距离
+syms m_w m_l m_b        % 驱动轮质量 腿部质量 机体质量
+syms I_w                % 驱动轮转动惯量           (自然坐标系法向)
+syms I_ll I_lr          % 驱动轮左右腿部转动惯量    (自然坐标系法向，实际上会变化)
+syms I_b                % 机体转动惯量             (自然坐标系法向)
+syms I_z                % 机器人z轴转动惯量        (简化为常量)
+
+% 定义其他独立变量并补充其导数
+syms theta_wl   theta_wr   % 左右驱动轮转角
+syms dtheta_wl  dtheta_wr 
+syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
+
+% 定义状态向量
+syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+
+% 定义控制向量
+syms T_wl T_wr T_bl T_br
+
+% 输入物理参数：重力加速度
+syms g
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
+
+% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
+eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
+eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
+eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
+eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
+eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
+[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
+
+
+% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
+J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
+J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
+
+% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
+A = sym('A',[10 10]);
+B = sym('B',[10 4]);
+
+% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
+for p = 5:2:9
+    A_index = (p - 3)/2;
+    A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
+    A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
+    for q = 6:2:10
+        A(q,p) = J_A(q/2,A_index);
+    end
+end
+
+% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
+for r = 1:10
+    if rem(r,2) == 0
+        A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
+    else
+        A(r,:) = zeros(1,10);
+        A(r,r+1) = 1;
+    end
+end
+
+% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
+for h = 1:4
+    B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
+    B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
+    for f = 6:2:10
+        B(f,h) = J_B(f/2,h);
+    end
+end
+
+% B的其余数值为0
+for e = 1:2:9
+    B(e,:) = zeros(1,4);
+end
+
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
+
+% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
+g_ac = 9.81;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                      此处可以输入机器人机体基本参数                      %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+R_w_ac = 0.9;                           % 驱动轮半径                  （单位：m）
+R_l_ac = 0.25;                            % 两个驱动轮之间距离/2         （单位：m）
+l_c_ac = 0.037;                            % 机体质心到腿部关节中心点距离  （单位：m）
+m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
+I_w_ac = (3510000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
+I_b_ac = 0.260;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
+I_z_ac = 0.226;                            % 机器人z轴转动惯量            （单位：kg m^2）
+
+%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+
+l_l_ac = 0.18;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
+l_wl_ac = 0.05;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
+l_bl_ac = 0.13;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
+I_ll_ac = 0.02054500;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
+l_r_ac = 0.18;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
+l_wr_ac = 0.05;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
+l_br_ac = 0.13;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
+I_lr_ac = 0.02054500;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
+
+% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
+% 两侧两个关节电机之间的中间点相连所形成的轴
+% （如果目的是小板凳，考虑使左右腿相关数据一致）
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
+% 通过以下方式记录数据: 矩阵分4列，
+% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
+% 第二列为l_wl，单位：m
+% 第三列为l_bl，单位：m
+% 第四列为I_ll，单位：kg m^2
+% （注意单位别搞错！）
+% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
+
+Leg_data_l = [0.11,  0.0480,  0.0620,  0.01819599;
+              0.12,  0.0470,  0.0730,  0.01862845;
+              0.13,  0.0476,  0.0824,  0.01898641;
+              0.14,  0.0480,  0.0920,  0.01931342;
+              0.15,  0.0490,  0.1010,  0.01962521;
+              0.16,  0.0500,  0.1100,  0.01993092;
+              0.17,  0.0510,  0.1190,  0.02023626;
+              0.18,  0.0525,  0.1275,  0.02054500;
+              0.19,  0.0539,  0.1361,  0.02085969;
+              0.20,  0.0554,  0.1446,  0.02118212;
+              0.21,  0.0570,  0.1530,  0.02151357;
+              0.22,  0.0586,  0.1614,  0.02185496;
+              0.23,  0.0600,  0.1700,  0.02220695;
+              0.24,  0.0621,  0.1779,  0.02256999;
+              0.25,  0.0639,  0.1861,  0.02294442;
+              0.26,  0.0657,  0.1943,  0.02333041;
+              0.27,  0.0676,  0.2024,  0.02372806;
+              0.28,  0.0700,  0.2100,  0.02413735;
+              0.29,  0.0713,  0.2187,  0.02455817;
+              0.30,  0.0733,  0.2267,  0.02499030];
+% 以上数据应通过实际测量或sw图纸获得
+
+% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
+% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
+Leg_data_r = Leg_data_l;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           此处可以输入QR矩阵                           %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 矩阵Q中，以下列分别对应：
+%        s     ds     phi     dphi     theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+lqr_Q = [1,    0,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    2,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     12000,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      200,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       1000,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       1,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        1000,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       1,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       0,        20000,   0;
+         0,    0,     0,      0,       0,       0,        0,       0,        0,      1];
+% 其中：
+% s       : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
+% phi     ：机器人水平方向移动时yaw偏航角度，dphi为其导数
+% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
+% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
+% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
+
+% 矩阵中，以下列分别对应：
+%        T_wl    T_wr     T_bl     T_br
+lqr_R = [0.25,      0,       0,       0;
+         0,      0.25,       0,       0;
+         0,      0,       1.5,       0;
+         0,      0,       0,       1.5];
+% 其中：
+% T_wl: 左侧驱动轮输出力矩
+% T_wr：右侧驱动轮输出力矩
+% T_bl：左侧髋关节输出力矩
+% T_br：右腿髋关节输出力矩
+% 单位皆为Nm
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),',  ') '\n'], K.')
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
+
+length = size(Leg_data_l,1); % 测量腿部数据集的行数
+
+% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
+% 是l_r，第三列是对应的K_ij的数值
+K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
+
+for i = 1:length
+    for j = 1:length
+        index = (i - 1)*length + j;
+        l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
+        l_wl_ac = Leg_data_l(i,2);
+        l_bl_ac = Leg_data_l(i,3);
+        I_ll_ac = Leg_data_l(i,4);
+        l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
+        l_wr_ac = Leg_data_r(j,2);
+        l_br_ac = Leg_data_r(j,3);
+        I_lr_ac = Leg_data_r(j,4);
+        for k = 1:40
+            K_sample(index,1,k) = l_l_ac;
+            K_sample(index,2,k) = l_r_ac;
+        end
+        K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R);
+        % 根据指定的l_l,l_r输入对应的K_ij的数值
+        for l = 1:4
+            for m = 1:10
+                K_sample(index,3,(l - 1)*10 + m) = K(l,m);
+            end
+        end
+    end
+end
+
+% 创建收集全部K_ij的多项式拟合的全部系数的集合
+K_Fit_Coefficients = zeros(40,6);
+for n = 1:40
+    K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
+    K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
+end
+Polynomial_expression = formula(K_Surface_Fit)
+
+% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
+% 每一行和K_ij的对应关系如下：
+% - 第1行对应K_1,1
+% - 第14行对应K_2,4
+% - 第22行对应K_3,2
+% - 第37行对应K_4,7
+% ... 其他行对应关系类似
+% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
+% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
+% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
+% 每一列和系数pij的对应关系如下：
+% - 第1列对应p00
+% - 第2列对应p10
+% - 第3列对应p01
+% - 第4列对应p20
+% - 第5列对应p11
+% - 第6列对应p02
+K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),',  ') '\n'], K_Fit_Coefficients.')
+
+% 正确食用方法：
+% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
+% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
+% 2.并填入对应系数即可
+
+toc
+
+%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
+% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
+% （前面的蛆，以后再来探索吧（bushi
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+    % 基于机体以及物理参数，获得控制矩阵A，B的全部数值
+    A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+    B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+
+    % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
+    % P为Riccati方程的解，矩阵L可以无视
+    [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
+end
+
--- a/utils/香港大学轮腿电控及建模开源-3/some_functions.c
+++ b/utils/香港大学轮腿电控及建模开源-3/some_functions.c
@ -0,0 +1,304 @@
+//             Front
+//         | 1       4 |
+//  (1)Left|           |Right(2)
+//         | 2       3 |
+
+#define HT_SLAVE_ID1        0x01
+#define HT_SLAVE_ID2        0x02
+#define HT_SLAVE_ID3        0x03
+#define HT_SLAVE_ID4        0x04
+
+#define CAN_BM_M1_ID 0x97
+#define CAN_BM_M2_ID 0x98
+#define BM_CAN hcan2
+
+#define get_HT_motor_measure(ptr, data)                                    \
+{                        \
+	(ptr)->last_ecd = (ptr)->ecd;                                   \
+	(ptr)->ecd =       uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16);      \
+	(ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\
+	(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
+}	
+
+#define get_BM_motor_measure(ptr, data)                           	\
+{                                                                   \
+	(ptr)->last_ecd = (ptr)->ecd;                                   \
+	(ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]);            \
+	(ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]);      \
+	(ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]);  \
+	(ptr)->error = (data)[6]; \
+	(ptr)->mode =  (data)[7];                                   \
+}
+
+void CAN_BM_ENABLE_CMD(void)
+{
+	CAN_TxHeaderTypeDef	bm_tx_measure;
+	uint8_t				bm_can_send_data[8];
+
+	uint32_t send_mail_box;
+	bm_tx_measure.StdId = 0x105; // 120
+	bm_tx_measure.IDE = CAN_ID_STD;
+	bm_tx_measure.RTR = CAN_RTR_DATA;
+	bm_tx_measure.DLC = 0x08;
+	bm_can_send_data[0] = 0x0A;
+	bm_can_send_data[1] = 0x0A;
+	bm_can_send_data[2] = 0x00;
+	bm_can_send_data[3] = 0x00;
+	bm_can_send_data[4] = 0x00;
+	bm_can_send_data[5] = 0x00;
+	bm_can_send_data[6] = 0x00;
+	bm_can_send_data[7] = 0x00;
+
+	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
+}
+
+void CAN_BM_MODE_SET_CMD(void)
+{
+	CAN_TxHeaderTypeDef	bm_tx_measure;
+	uint8_t				bm_can_send_data[8];
+
+	uint32_t send_mail_box;
+	bm_tx_measure.StdId = 0x106; // 120
+	bm_tx_measure.IDE = CAN_ID_STD;
+	bm_tx_measure.RTR = CAN_RTR_DATA;
+	bm_tx_measure.DLC = 0x08;
+	bm_can_send_data[0] = 0x01;
+	bm_can_send_data[1] = 0x01;
+	bm_can_send_data[2] = 0x00;
+	bm_can_send_data[3] = 0x00;
+	bm_can_send_data[4] = 0x00;
+	bm_can_send_data[5] = 0x00;
+	bm_can_send_data[6] = 0x00;
+	bm_can_send_data[7] = 0x00;
+
+	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
+}
+
+void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
+{
+	CAN_TxHeaderTypeDef	bm_tx_measure;
+	uint8_t				bm_can_send_data[8];
+
+	uint32_t send_mail_box;
+	bm_tx_measure.StdId = 0x32; // 120
+	bm_tx_measure.IDE = CAN_ID_STD;
+	bm_tx_measure.RTR = CAN_RTR_DATA;
+	bm_tx_measure.DLC = 0x08;
+	bm_can_send_data[0] = motor1 >> 8;
+	bm_can_send_data[1] = motor1;
+	bm_can_send_data[2] = motor2 >> 8;
+	bm_can_send_data[3] = motor2;
+	bm_can_send_data[4] = 0x00;
+	bm_can_send_data[5] = 0x00;
+	bm_can_send_data[6] = 0x00;
+	bm_can_send_data[7] = 0x00;
+
+	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
+}
+
+#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
+#define P_MIN -95.5f// Radians
+#define P_MAX 95.5f
+#define V_MIN -45.0f// Rad/s
+#define V_MAX 45.0f
+#define KP_MIN 0.0f// N-m/rad
+#define KP_MAX 500.0f
+#define KD_MIN 0.0f// N-m/rad/s
+#define KD_MAX 5.0f
+#define T_MIN -18.0f
+#define T_MAX 18.0f
+
+void CAN_HT_CMD( uint8_t id, fp32 f_t )
+{
+    uint32_t canTxMailbox = CAN_TX_MAILBOX0;
+
+    fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f;
+    uint16_t p, v, kp, kd, t;
+    uint8_t buf[8];
+    LIMIT_MIN_MAX(f_p,  P_MIN,  P_MAX);
+    LIMIT_MIN_MAX(f_v,  V_MIN,  V_MAX);
+    LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX);
+    LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX);
+    LIMIT_MIN_MAX(f_t,  T_MIN,  T_MAX);
+    
+    p = float_to_uint(f_p,      P_MIN,  P_MAX,  16);            
+    v = float_to_uint(f_v,      V_MIN,  V_MAX,  12);
+    kp = float_to_uint(f_kp,    KP_MIN, KP_MAX, 12);
+    kd = float_to_uint(f_kd,    KD_MIN, KD_MAX, 12);
+    t = float_to_uint(f_t,      T_MIN,  T_MAX,  12);
+    
+    buf[0] = p>>8;
+    buf[1] = p&0xFF;
+    buf[2] = v>>4;
+    buf[3] = ((v&0xF)<<4)|(kp>>8);
+    buf[4] = kp&0xFF;
+    buf[5] = kd>>4;
+    buf[6] = ((kd&0xF)<<4)|(t>>8);
+    buf[7] = t&0xff;
+
+    chassis_tx_message.StdId = id;
+    chassis_tx_message.IDE = CAN_ID_STD;
+    chassis_tx_message.RTR = CAN_RTR_DATA;
+    chassis_tx_message.DLC = 0x08;
+
+		// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
+		if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX0;} 
+		else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX1;} 
+		else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX2;}
+		
+	if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){};
+}
+void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] )
+{
+    uint32_t canTxMailbox= CAN_TX_MAILBOX0;
+
+    chassis_tx_message.StdId = id;
+    chassis_tx_message.IDE = CAN_ID_STD;
+    chassis_tx_message.RTR = CAN_RTR_DATA;
+    chassis_tx_message.DLC = 0x08;
+
+	// 	while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
+		if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX0;} 
+		else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX1;} 
+		else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) 
+		{canTxMailbox = CAN_TX_MAILBOX2;}
+		
+	if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){};
+}
+
+void Forward_kinematic_solution(chassis_control_t *feedback_update,
+											fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)
+{
+	fp32 dL0=0,L0=0,Q0=0,S0=0;
+	fp32 xb,xd,yb,yd,Lbd,xc,yc;
+	fp32 A0,B0,C0,Q2,Q3,S2,S3;
+	fp32 vxb,vxd,vyb,vyd,vxc,vyc;
+	fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
+	fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
+	fp32 axb,ayb,axd,ayd,a2,axc;
+	/******************************/
+	Q1 = PI + Q1;
+	cos_Q1 = arm_cos_f32(Q1);
+	sin_Q1 = arm_sin_f32(Q1);
+	cos_Q4 = arm_cos_f32(Q4);
+	sin_Q4 = arm_sin_f32(Q4);
+	xb = -L5/2.0f + L1*cos_Q1;
+	xd =  L5/2.0f + L4*cos_Q4;
+	yb = L1*sin_Q1;
+	yd = L4*sin_Q4;
+
+	Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
+	A0 = 2.0f*L2*(xd-xb);
+	B0 = 2.0f*L2*(yd-yb);
+	C0 = L2*L2+Lbd-L3*L3;
+	Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
+	
+	xc = xb + arm_cos_f32(Q2)*L2;
+	yc = yb + arm_sin_f32(Q2)*L2;
+
+	L0=Sqrt( xc*xc + yc*yc );
+	Q0 = atan(yc/xc);
+	
+	vxb = -S1*L1*sin_Q1;
+	vyb = S1*L1*cos_Q1;
+	vxd = -S4*L4*sin_Q4;
+	vyd = S4*L4*cos_Q4;
+	Q3 = atan_tl((yc-yd)/(xc-xd));
+	S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2)); 
+	S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2)); 
+	vxc = vxb - S2*L2*arm_sin_f32(Q2);
+  	vyc = vyb + S2*L2*arm_cos_f32(Q2);
+	S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc);
+
+	if( Q0 < 0 )
+		Q0 += PI;
+	/*******************************/
+	if (ce)
+	{
+		feedback_update->chassis_posture_info .leg_length_L = L0;
+		feedback_update->chassis_posture_info .leg_angle_L  = Q0;
+		// feedback_update->chassis_posture_info .leg_gyro_L   		= S0;
+	}
+	else 
+	{
+		feedback_update->chassis_posture_info .leg_length_R = L0;
+		feedback_update->chassis_posture_info .leg_angle_R  = Q0;
+		// feedback_update->chassis_posture_info .leg_gyro_R   		= S0;
+	}
+}
+
+void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce )
+{
+	fp32 dL0=0,L0=0,Q0=0,S0=0;
+	fp32 xb,xd,yb,yd,Lbd,xc,yc;
+	fp32 A0,B0,C0,Q2,Q3,S2,S3;
+	fp32 vxb,vxd,vyb,vyd,vxc,vyc;
+	fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
+	fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
+	fp32 axb,ayb,axd,ayd,a2,axc;
+	/******************************/
+	Q1 = PI + Q1;
+	cos_Q1 = arm_cos_f32(Q1);
+	sin_Q1 = arm_sin_f32(Q1);
+	cos_Q4 = arm_cos_f32(Q4);
+	sin_Q4 = arm_sin_f32(Q4);
+	xb = -L5/2.0f + L1*cos_Q1;
+	xd =  L5/2.0f + L4*cos_Q4;
+	yb = L1*sin_Q1;
+	yd = L4*sin_Q4;
+
+	Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) );
+	A0 = 2.0f*L2*(xd-xb);
+	B0 = 2.0f*L2*(yd-yb);
+	C0 = L2*L2+Lbd*Lbd-L3*L3;
+	Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
+	xc = xb + arm_cos_f32(Q2)*L2;
+	yc = yb + arm_sin_f32(Q2)*L2;
+
+	L0 = Sqrt( xc*xc + yc*yc );
+	Q0 = atan_tl(yc/xc);
+	if( Q0 < 0 )
+		Q0 += PI;
+	
+	Q3  = atan_tl((yc-yd)/(xc-xd))+PI;
+
+	
+
+	fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ));
+	fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
+	fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ));
+	fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
+	
+	
+	fp32 dett = J1 * J4 - J2 * J3;
+	fp32 Inv_J1 =  J4 / dett;
+	fp32 Inv_J2 = -J2 / dett;
+	fp32 Inv_J3 = -J3 / dett;
+	fp32 Inv_J4 =  J1 / dett;
+
+	ddebug = yc;
+
+	if( ce == 1.0f )
+	{
+		Suspend_Detect->chassis_posture_info.supportive_force_L = 
+			Inv_J1 * Suspend_Detect->joint_motor_1.torque_get + 
+			Inv_J2 * Suspend_Detect->joint_motor_2.torque_get;
+		// if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f )
+		// 	Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f;
+	}
+	else	
+	{
+		Suspend_Detect->chassis_posture_info.supportive_force_R = 
+			Inv_J1 * Suspend_Detect->joint_motor_4.torque_get + 
+			Inv_J2 * Suspend_Detect->joint_motor_3.torque_get;
+		Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f;
+		// if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f )
+		// 	Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f;
+	}
+
+}
--- a/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf
+++ b/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf