多体动力学——四连杆运动学与动力学仿真

Hwbxan

已于 2024-05-28 09:13:40 修改

阅读量202

点赞数 2

文章标签：机器人

于 2024-05-28 09:12:07 首次发布

本文链接：https://blog.csdn.net/qq_64161215/article/details/139094500

版权

Matlab源码

动力学建模

clear all
clc
syms a1 r11 r12 da1 dr11 dr12 dda1 ddr11 ddr12 L1
syms a2 r21 r22 da2 dr21 dr22 dda2 ddr21 ddr22 L2
syms a3 r31 r32 da3 dr31 dr32 dda3 ddr31 ddr32 L3 L4
syms m1 m2 m3 Tw1 g
syms t
L1=1;L2=1;L3=1;L4=1;
q1=[r11;r12;a1];q2=[r21;r22;a2];q3=[r31;r32;a3];
q=[q1;q2;q3];
q_i=a1;
q_d=[r11;r12;q2;q3];
qn=[q_i;q_d];
A1=[cos(a1) -sin(a1);sin(a1) cos(a1)];
A2=[cos(a2) -sin(a2);sin(a2) cos(a2)];
A3=[cos(a3) -sin(a3);sin(a3) cos(a3)];
dA1=[-sin(a1) -cos(a1);cos(a1) -sin(a1)];
dA2=[-sin(a2) -cos(a2);cos(a2) -sin(a2)];
dA3=[-sin(a3) -cos(a3);cos(a3) -sin(a3)];

Fg1=[0 -m1*g].';
u1_0=[0 0].';
G1=[eye(2) dA1*u1_0];
Q1g=G1.'*Fg1;

Q1t=[0 0 Tw1].';
Q1=Q1g+Q1t;

Fg2=[0 -m2*g].';
u2_0=[0 0].';
G2=[eye(2) dA2*u2_0];
Q2g=G2.'*Fg2;

Fg3=[0 -m3*g].';
u3_0=[0 0].';
G3=[eye(2) dA3*u3_0];
Q3g=G3.'*Fg3;

Q=[Q1;Q2g;Q3g];
Mc=jacobian(q,qn);
Q_n=Mc.'*Q;

M1=[m1 0 0;
    0 m1 0;
    0 0 (L1^2*m1)/12];
M2=[m2 0 0;
0 m2 0;
0 0 (L2^2*m2)/12];
M3=[m3 0 0;
0 m3 0;
0 0 (L3^2*m1)/12];
M=[M1 zeros(3,6);
zeros(3,3) M2 zeros(3);
zeros(3,6) M3;];
M_n=Mc.'*M*Mc;

func=[r11-cos(a1)/2
r12-sin(a1)/2
r21-r11-cos(a1)/2-cos(a2)/2
r22-r12-sin(a1)/2-sin(a2)/2
r31-r21-cos(a2)/2+cos(a3)/2
r32-r22-sin(a2)/2+sin(a3)/2
r31-cos(a3)/2-1
r32-sin(a3)/2];
q_i=a1;
dq_i=da1;
ddq_i=dda1;
q_d=[r11,r12,r21,r22,a2,r31,r32,a3].';
dq_d=[dr11,dr12,dr21,dr22,da2,dr31,dr32,da3].';
ddq_d=[ddr11,ddr12,ddr21,ddr22,dda2,ddr31,ddr32,dda3].';
q_n=[q_i;q_d];
dq_n=[dq_i;dq_d];
ddq_n=[ddq_i;ddq_d];
Cq_d=jacobian(func,q_d);
Cq_i=jacobian(func,q_i);
C_t=zeros(8,1);

C_di=-Cq_d^-1*Cq_i;
B_di=[1;C_di];
C_tt=diff(C_t,t);
Cq_n=jacobian(func,q_n);
Cq_nt=diff(jacobian(func,q_n),t);

Qc=-(jacobian(Cq_n*dq_n,q_n)*dq_n+2*Cq_nt*dq_n+C_tt);
Q_c=[0;((Cq_d)^-1)*Qc];
Q11=-M_n*Q_c;
Q_v=zeros(9,1);

Q_ii=B_di.'*(Q_n+Q11+Q_v);
symvar(Q_ii);
M_ii=B_di.'*M_n*B_di;

dyn_fun=M_ii*ddq_i-Q_ii;
symvar(dyn_fun);

Tw1=simplify(solve(dyn_fun,Tw1));
Tw1=(3*g*m2*cos(a1 - 2*a2) - 10*dda1*m2 - 3*dda1*m3 - 5*dda1*m1 - 3*g*m2*cos(a1 - 2*a3) + 3*g*m3*cos(a1 - 2*a2) - 3*g*m3*cos(a1 - 2*a3) + dda1*m1*cos(2*a1 - 2*a2) + 6*dda1*m2*cos(2*a1 - 2*a2) + 4*dda1*m1*cos(2*a2 - 2*a3) - 2*dda1*m2*cos(2*a1 - 2*a3) + 3*dda1*m3*cos(2*a1 - 2*a2) + 6*dda1*m2*cos(2*a2 - 2*a3) - 2*da2^2*m1*sin(a1 - a2) - 10*da2^2*m2*sin(a1 - a2) - 6*da2^2*m3*sin(a1 - a2) + da3^2*m1*sin(a1 - a3) + 2*da3^2*m2*sin(a1 - a3) + 3*da3^2*m3*sin(a1 - a3) + 3*g*m1*cos(a1 - 2*a2 + 2*a3) + 3*g*m1*cos(a1 + 2*a2 - 2*a3) + 6*g*m2*cos(a1 - 2*a2 + 2*a3) + 3*g*m2*cos(a1 + 2*a2 - 2*a3) + 3*g*m3*cos(a1 - 2*a2 + 2*a3) - da1^2*m1*sin(2*a1 - 2*a2) - 6*da1^2*m2*sin(2*a1 - 2*a2) + 2*da1^2*m2*sin(2*a1 - 2*a3) - 3*da1^2*m3*sin(2*a1 - 2*a2) + 2*da2^2*m2*sin(a1 + a2 - 2*a3) + da3^2*m1*sin(a1 - 2*a2 + a3) + 6*da3^2*m2*sin(a1 - 2*a2 + a3) + 3*da3^2*m3*sin(a1 - 2*a2 + a3) - 6*g*m1*cos(a1) - 9*g*m2*cos(a1) - 3*g*m3*cos(a1))/(12*(cos(2*a2 - 2*a3) - 1));
symvar(Tw1)

运动学建模

%% 四连杆运动学仿真
clear all;
clc;
syms r11 r12 a1 r21 r22 a2 r31 r32 a3 L1 L2 L3 L4
syms dr11 dr12 da1 dr21 dr22 da2 dr31 dr32 da3
syms ddr11 ddr12 dda1 ddr21 ddr22 dda2 ddr31 ddr32 dda3
syms m1 m2 m3 Tw1 g
dR1=[dr11;dr12];dR2=[dr21;dr22];dR3=[dr31;dr32];
L1=10;L2=10;L3=10;L4=10;
m1=1;m2=1;m3=1;g=10;

% a1=60/180*pi;
%% 运动副A点约束方程
A1=[cos(a1) -sin(a1);sin(a1) cos(a1)];
R1=[r11;r12];
u_1A=[-0.5*L1;0];
r1A=R1+A1*u_1A;
rA=[0;0];
fun1=r1A-rA;
%% 运动副B点约束方程?
A1=[cos(a1) -sin(a1);sin(a1) cos(a1)];
R1=[r11;r12];
u_1B=[0.5*L1;0];
r1B=R1+A1*u_1B;
%构件2运动副B点?
A2=[cos(a2) -sin(a2);sin(a2) cos(a2)];
R2=[r21;r22];
u_2B=[-0.5*L2;0];
r2B=R2+A2*u_2B;
fun2=r2B-r1B;
%% 运动副C点约束方程?
A2=[cos(a2) -sin(a2);sin(a2) cos(a2)];
R2=[r21;r22];
u_2C=[0.5*L2;0];
r2C=R2+A2*u_2C;
%构件3运动副C点?
A3=[cos(a3) -sin(a3);sin(a3) cos(a3)];
R3=[r31;r32];
u_3C=[0.5*L3;0];
r3C=R3+A3*u_3C;
fun3=r3C-r2C;
%% 运动副D点约束方程?
A3=[cos(a3) -sin(a3);sin(a3) cos(a3)];
R3=[r31;r32];
u_3D=[-0.5*L3;0];
r3D=R3+A3*u_3D;
rD=[L4;0];
fun4=r3D-rD;
%%
fun=[fun1;fun2;fun3;fun4];
symvar(fun);

%% 独立变量与非独立变量
qi=a1;
qd=[R1;R2;a2;R3;a3];
dqi=da1;
dqd=[dR1;dR2;da2;dR3;da3];
qn=[qi;qd];
dqn=[dqi;dqd];
ddqi=dda1;
%% 位置正解
resu=solve(fun,qd);
r11v=resu.r11;
r12v=resu.r12;
r21v=resu.r21;
r22v=resu.r22;
r31v=resu.r31;
r32v=resu.r32;
a2v=resu.a2;
a3v=resu.a3;
r11_=r11v(1);
r12_=r12v(1);
r21_=r21v(1);
r22_=r22v(1);
r31_=r31v(1);
r32_=r32v(1);
a2_=a2v(1);
a3_=a3v(1);
%% 速度正解
Cqd=jacobian(fun,qd);
Cqi=jacobian(fun,qi);
Cqn=jacobian(fun,qn);
dqd=-simplify(inv(Cqd)*Cqi*dqi);
Qc=-jacobian(Cqn*dqn,qn)*dqn;
Cdi=-simplify(inv(Cqd)*Cqi);
dqd1=Cdi*dqi;
dr11=dqd1(1);
dr12=dqd1(2);
dr21=dqd1(3);
dr22=dqd1(4);
da2=dqd1(5);
dr31=dqd1(6);
dr32=dqd1(7);
da3=dqd1(8);

%% 加速度正解
ddqd1=Cdi*ddqi+inv(Cqd)*Qc;
ddr11=ddqd1(1);
ddr12=ddqd1(2);
ddr21=ddqd1(3);
ddr22=ddqd1(4);
dda2=ddqd1(5);
ddr31=ddqd1(6);
ddr32=ddqd1(7);
dda3=ddqd1(8);
%% 数值仿真?
t_m=0:0.01:5;
nn=length(t_m);
w=10;

for ii=1:nn
    ii;
    t=t_m(ii);
    a1=exp(-t)*cos(t*w)^1;
    da1=-exp(-t)*cos(t*w)-w*exp(-t)*sin(t*w);
    dda1=exp(-t)*cos(t*w)-w^2*exp(-t)*cos(t*w)+2*w*exp(-t)*sin(t*w);

    %%
    r11=5*cos(a1);
    r12=5*sin(a1);
    r21=-(5*(tan(a1/2)^2 - 3))/(tan(a1/2)^2 + 1);
    r22=(20*tan(a1/2))/(tan(a1/2)^2 + 1);
    r31=(5*(tan(a1/2)^2 + 3))/(tan(a1/2)^2 + 1);
    r32=(10*tan(a1/2))/(tan(a1/2)^2 + 1);
    a2=0;
    a3=2*atan(tan(a1/2));
    
    dr11=-5*da1*sin(a1);
    dr12=5*da1*cos(a1);
    dr21=-(5*da1*(cos(a1)*sin(a2)*sin(a3) - 2*cos(a2)*sin(a1)*sin(a3) + cos(a3)*sin(a1)*sin(a2)))/sin(a2 - a3);
    dr22=-(5*da1*(cos(a1)*cos(a2)*sin(a3) - 2*cos(a1)*cos(a3)*sin(a2) + cos(a2)*cos(a3)*sin(a1)))/sin(a2 - a3);
    da2=-(da1*sin(a1 - a3))/sin(a2 - a3);
    dr31=(5*da1*sin(a3)*sin(a1 - a2))/sin(a2 - a3);
    dr32=-(5*da1*cos(a3)*sin(a1 - a2))/sin(a2 - a3);
    da3=-(da1*sin(a1 - a2))/sin(a2 - a3);

    ddr11 = - 5*cos(a1)*da1^2 - 5*dda1*sin(a1);
    ddr12 = - 5*sin(a1)*da1^2 + 5*dda1*cos(a1);
    ddr21=(5*da3^2*cos(a3)^2*sin(a2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*dda1*(cos(a1)*sin(a2)*sin(a3) - 2*cos(a2)*sin(a1)*sin(a3) + cos(a3)*sin(a1)*sin(a2)))/sin(a2 - a3) - (sin(a2)*sin(a3)*(5*sin(a1)*da1^2 + 5*sin(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (sin(a2)*sin(a3)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - ((5*cos(a1)*da1^2 + 5*cos(a2)*da2^2)*(2*cos(a2)*sin(a3) - cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (cos(a3)*sin(a2)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (5*da3^2*sin(a2)*sin(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da1^2*cos(a1)*(2*cos(a2)*sin(a3) - cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da1^2*sin(a1)*sin(a2)*sin(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));
    ddr22=(cos(a2)*cos(a3)*(5*cos(a1)*da1^2 + 5*cos(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - ((5*sin(a1)*da1^2 + 5*sin(a2)*da2^2)*(cos(a2)*sin(a3) - 2*cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da3^2*cos(a2)*cos(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*dda1*(cos(a1)*cos(a2)*sin(a3) - 2*cos(a1)*cos(a3)*sin(a2) + cos(a2)*cos(a3)*sin(a1)))/sin(a2 - a3) + (cos(a2)*cos(a3)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da3^2*cos(a2)*sin(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (cos(a2)*sin(a3)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da1^2*sin(a1)*(cos(a2)*sin(a3) - 2*cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (5*da1^2*cos(a1)*cos(a2)*cos(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));
    dda2=(cos(a3)*(5*cos(a1)*da1^2 + 5*cos(a2)*da2^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (cos(a3)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (da3^2*sin(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (dda1*sin(a1 - a3))/sin(a2 - a3) + (sin(a3)*(5*sin(a1)*da1^2 + 5*sin(a2)*da2^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (sin(a3)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (da3^2*cos(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (da1^2*cos(a1)*cos(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (da1^2*sin(a1)*sin(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));
    ddr31=(5*da3^2*sin(a2)*sin(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (sin(a2)*sin(a3)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (cos(a2)*sin(a3)*(5*cos(a1)*da1^2 + 5*cos(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (cos(a2)*sin(a3)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (sin(a2)*sin(a3)*(5*sin(a1)*da1^2 + 5*sin(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (5*dda1*sin(a3)*sin(a1 - a2))/sin(a2 - a3) - (5*da3^2*cos(a3)*(cos(a2)*sin(a3) - 2*cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da1^2*cos(a1)*cos(a2)*sin(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da1^2*sin(a1)*sin(a2)*sin(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));
    ddr32=(cos(a2)*cos(a3)*(5*cos(a1)*da1^2 + 5*cos(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da3^2*cos(a2)*cos(a3)^2)/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (cos(a2)*cos(a3)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*dda1*cos(a3)*sin(a1 - a2))/sin(a2 - a3) + (cos(a3)*sin(a2)*(5*sin(a1)*da1^2 + 5*sin(a2)*da2^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (cos(a3)*sin(a2)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (5*da3^2*sin(a3)*(2*cos(a2)*sin(a3) - cos(a3)*sin(a2)))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (5*da1^2*cos(a1)*cos(a2)*cos(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (5*da1^2*cos(a3)*sin(a1)*sin(a2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));
    dda3=(cos(a2)*(5*cos(a1)*da1^2 + 5*cos(a2)*da2^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (cos(a2)*(5*cos(a2)*da2^2 - 5*cos(a3)*da3^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (dda1*sin(a1 - a2))/sin(a2 - a3) + (sin(a2)*(5*sin(a1)*da1^2 + 5*sin(a2)*da2^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (sin(a2)*(5*sin(a2)*da2^2 - 5*sin(a3)*da3^2))/(10*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (da1^2*cos(a1)*cos(a2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (da3^2*cos(a2)*cos(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) + (da1^2*sin(a1)*sin(a2))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2))) - (da3^2*sin(a2)*sin(a3))/(2*(cos(a2)*sin(a3) - cos(a3)*sin(a2)));

    r11_m(ii)=r11;
    r12_m(ii)=r12;
    r21_m(ii)=r21;
    r22_m(ii)=r22;
    a2_m(ii)=a2;
    r32_m(ii)=r32;
    a3_m(ii)=a3;
    a1_m(ii)=a1;
    dr11_m(ii)=dr11;
    dr12_m(ii)=dr12;
    dr21_m(ii)=dr21;
    dr22_m(ii)=dr22;
    da1_m(ii)=da1;
    da2_m(ii)=da2;
    dr31_m(ii)=dr31;
    dr32_m(ii)=dr32;
    da3_m(ii)=da3;
    ddr11_m(ii)=ddr11;
    ddr12_m(ii)=ddr12;
    ddr21_m(ii)=ddr21;
    ddr22_m(ii)=ddr22;
    dda1_m(ii)=dda1;
    dda2_m(ii)=dda2;
    ddr31_m(ii)=ddr31;
    ddr32_m(ii)=ddr32;
    dda3_m(ii)=dda3;
    Tw1_m(ii)=(3*g*m2*cos(a1 - 2*a2) - 10*dda1*m2 - 3*dda1*m3 - 5*dda1*m1 - 3*g*m2*cos(a1 - 2*a3) + 3*g*m3*cos(a1 - 2*a2) - 3*g*m3*cos(a1 - 2*a3) + dda1*m1*cos(2*a1 - 2*a2) + 6*dda1*m2*cos(2*a1 - 2*a2) + 4*dda1*m1*cos(2*a2 - 2*a3) - 2*dda1*m2*cos(2*a1 - 2*a3) + 3*dda1*m3*cos(2*a1 - 2*a2) + 6*dda1*m2*cos(2*a2 - 2*a3) - 2*da2^2*m1*sin(a1 - a2) - 10*da2^2*m2*sin(a1 - a2) - 6*da2^2*m3*sin(a1 - a2) + da3^2*m1*sin(a1 - a3) + 2*da3^2*m2*sin(a1 - a3) + 3*da3^2*m3*sin(a1 - a3) + 3*g*m1*cos(a1 - 2*a2 + 2*a3) + 3*g*m1*cos(a1 + 2*a2 - 2*a3) + 6*g*m2*cos(a1 - 2*a2 + 2*a3) + 3*g*m2*cos(a1 + 2*a2 - 2*a3) + 3*g*m3*cos(a1 - 2*a2 + 2*a3) - da1^2*m1*sin(2*a1 - 2*a2) - 6*da1^2*m2*sin(2*a1 - 2*a2) + 2*da1^2*m2*sin(2*a1 - 2*a3) - 3*da1^2*m3*sin(2*a1 - 2*a2) + 2*da2^2*m2*sin(a1 + a2 - 2*a3) + da3^2*m1*sin(a1 - 2*a2 + a3) + 6*da3^2*m2*sin(a1 - 2*a2 + a3) + 3*da3^2*m3*sin(a1 - 2*a2 + a3) - 6*g*m1*cos(a1) - 9*g*m2*cos(a1) - 3*g*m3*cos(a1))/(12*(cos(2*a2 - 2*a3) - 1));
end

subplot (2,4, 1),plot(t_m,Tw1_m)
subplot (2,4, 2),plot(a1_m*180/pi,Tw1_m)

subplot (2,4, 3),plot(t_m,r21_m,t_m,dr21_m,t_m,ddr21_m)
subplot (2,4, 4),plot(t_m,r21_m)
subplot (2,4, 5),plot(r21_m,r22_m)

subplot (2,4, 6),plot (t_m, a1_m*180/pi)
subplot (2,4,7),plot(t_m, da1_m*180/pi)
subplot (2,4,8),plot(t_m, dda1_m*180/pi)