动力学方程推导

%% Function   动力学方程推导  练习
clc;clear;
%% %%%%%%%%%%%%%%%%%%%%%%参数定义及赋值%%%%%%%%%%%%%%%%%%%%%%%%%%%%
syms Mp Mq Ip Iq rp rq 
syms thita_p thita_q dthita_p dthita_q deta ddeta de e t Tp Tq km cm
%% %%%%%%%%%%%%%%%%%%%%%%参数定义及赋值%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dpt=[thita_p;thita_q];     %振动位移
%% 方程推导
e=0.02*sin(t)+cos(1+0.5*t);
de=diff(e);
deta=rp*thita_p-rq*thita_q+e;
ddeta=rp*dthita_p-rq*dthita_q+de;
T=1/2*(Ip*dthita_p^2+Iq*dthita_q^2);
U=1/2*(km*deta^2);
R=1/2*(cm*ddeta^2);
L=T-U;
dp(1,1)=Ip\(Tp+diff(L,thita_p)-diff(R,dthita_p));
dp(2,1)=Iq\(Tq+diff(L,thita_q)-diff(R,dthita_q));

%% %%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dpt=[dpt;dp];

>> dpt
 
dpt =
 
                                                                                                                               thita_p
                                                                                                                               thita_q
 (Tp - km*rp*(cos(t/2 + 1) + sin(t)/50 + rp*thita_p - rq*thita_q) + cm*rp*(sin(t/2 + 1)/2 - cos(t)/50 - dthita_p*rp + dthita_q*rq))/Ip
 (Tq + km*rq*(cos(t/2 + 1) + sin(t)/50 + rp*thita_p - rq*thita_q) - cm*rq*(sin(t/2 + 1)/2 - cos(t)/50 - dthita_p*rp + dthita_q*rq))/Iq
 

版权声明:本文为qq_37761964原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
原文链接:https://blog.csdn.net/qq_37761964/article/details/89918593