Lab=1.735;Lah=0.4;Lhe=0.340;gama1=27*pi/180;L0=0;L(1)=0;L(2)=3.72;L(3)=1.62;L(4)=1.05;x0=5.5;xf=3;z0=0;zf=0;
Lfc=0.525;Lci=1.832;beita1=36*pi/180;beita2=150*pi/180;beita3=7*pi/180;
%挖掘機參數(shù),beita3是因為傾角傳感器的安裝位置的不同所引入的角度！
af=atan((z0-zf)./(x0-xf));%判斷所給直線的角度
t=0:0.1:25;
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;%動臂轉(zhuǎn)角（注意這個公式是直接利用兩自由度推導(dǎo)而來！）
beita=acos(((L(2)^2+(L(3)+L(4))^2)-x.^2-z.^2)./(2*L(2)*(L(3)+L(4))));%斗桿轉(zhuǎn)角
length1=sqrt((Lab*sin(afa+gama1)+Lah).^2+(Lab*cos(afa+gama1)-Lhe).^2);%動臂液壓缸
length2=sqrt(Lfc^2+Lci^2-2*Lfc*Lci*cos(beita1+beita2+beita3+beita));%斗桿液壓缸
subplot(2,2,1)
theta22=length1;
 plot(t,theta22)
 title('挖掘機動臂液壓缸位移曲線')
 xlabel('時間（秒）')
 ylabel('位移（米）')
 grid
 subplot(2,2,2)
 theta33=length2;
 plot(t,theta33)
 title('挖掘機斗桿液壓缸位移曲線')
 xlabel('時間（秒）')
 ylabel('位移（米）')
 grid
 subplot(2,2,3)
 theta44=sita1./pi*180;
 plot(theta33,theta22)
 title('挖掘機鏟斗液壓缸位移曲線')
 xlabel('時間（秒）')
 ylabel('位移（米）')
 grid
%鏟斗與斗桿的相對轉(zhuǎn)角為零！
%2005.3.14