L0=0;L(1)=0;L(2)=3.72;L(3)=1.62;L(4)=1.05;x0=5.0;xf=3.5;z0=-2;zf=-2;%挖掘機(jī)參數(shù)
af=atan((z0-zf)./(x0-xf));%判斷所給直線的角度
t=0:0.1:15;
x=x0-0.1*t.*cos(af);%速度為0.1m/s
z=z0-0.1*t.*sin(af);
y=0;
sita1=atan(y./x);
sita4=0;
gama=atan(z./x);
afa=acos((x.^2+z.^2+L(2)^2-(L(3)+L(4))^2)./(2*sqrt(x.^2+z.^2)*L(2)))+gama;%動(dòng)臂轉(zhuǎn)角
beita=acos(((L(2)^2+(L(3)+L(4))^2)-x.^2-z.^2)./(2*L(2)*(L(3)+L(4))));%斗桿轉(zhuǎn)角
w1=diff(sita1./pi*180)/0.1;%求導(dǎo)
w2=diff(afa./pi*180)/0.1;
w3=diff(beita./pi*180)/0.1;
w4=diff(sita1./pi*180)/0.1;
u=0.1:0.1:15;
subplot(2,2,1)
theta11=sita1./pi*180;
plot(t,theta11)
title('挖掘機(jī)回轉(zhuǎn)角速度曲線')
xlabel('時(shí)間（秒）')
ylabel('角速度（度/秒）')
 grid
 subplot(2,2,2)
 theta22=w2;
 plot(u,theta22)
 title('挖掘機(jī)動(dòng)臂轉(zhuǎn)動(dòng)角速度曲線')
 xlabel('時(shí)間（秒）')
 ylabel('角速度（度/秒）')
 grid
 subplot(2,2,3)
 theta33=w3;
 plot(u,theta33)
 title('挖掘機(jī)斗桿轉(zhuǎn)動(dòng)角速度曲線')
 xlabel('時(shí)間（秒）')
 ylabel('角速度（度/秒）')
 grid
 subplot(2,2,4)
 theta44=sita1./pi*180;
 plot(t,theta44)
 title('挖掘機(jī)鏟斗轉(zhuǎn)動(dòng)角速度曲線')
 xlabel('時(shí)間（秒）')
 ylabel('角速度（度/秒）')
 grid
%鏟斗與斗桿的相對(duì)轉(zhuǎn)角為零！