移除matlab代码

matlab/LQR_calc.m

@@ -1,85 +0,0 @@
-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.0675;      %轮子半径
-mw = 3;          %轮子质量
-Iw = mw*R^2/2;   %轮子转动惯量
-
-% 大腿
-l_active_leg = 0.15;
-m_active_leg = 0.39;
-% 小腿
-l_slave_leg = 0.25;
-m_slave_leg = 0.56;
-% 关节间距
-joint_distance = 0.108;
-% 摆杆
-mp = (m_active_leg + m_slave_leg)*2;
-Ip = mp*L^2/3;  %摆杆转动惯量
-
-% 机体
-M = 15;      %机体重量,应为整车的一半
-l = 0.02;    %机体质心到关节电机转轴的距离
-IM = M*(0.2^2 + 0.1^2)/12;
-
-%% 经典力学方程
-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