forcontroltiaoshi.asv

水平直线挖掘的源程序

L0=0;L(1)=0;L(2)=3.72;L(3)=1.62;L(4)=1.05;x0=5;xf=3.5;z0=-2;zf=-2;%挖掘机参数
af=atan((z0-zf)./(x0-xf));%判断所给直线的角度
%while 23*pi/180<=beita<=149.5*pi/180 | -51*pi/180<afa<75*pi/180
t=0:0.1:20;
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;%动臂转角
beita=acos(((L(2)^2+(L(3)+L(4))^2)-x.^2-z.^2)./(2*L(2)*(L(3)+L(4))));%斗杆转角
%end
for i=1:1:length(t)-2;
    if afa(i)<afa(i+1) & afa(i+2)<afa(i+1)
        largenumber=i+1
    end
end
afa1=afa(1):0.0009:afa(largenumber);
beita1=-asin((2+L(2)*sin(afa1))./(L(3)+L(4)))-afa1+pi;
if beita1<0;
    beita1=beita1+pi;
end
afa2=afa(largenumber+1):0.0009:afa(length(afa));
beita2=-asin((2+L(2)*sin(afa2))./(L(3)+L(4)))-afa2+pi;
if beita2<0;
    beita2=beita2+pi;
end
afa3=[afa1 afa2];
beita3=[beita1 beita2];
subplot(3,1,1)
plot(afa3*180/pi,beita3*180/pi)
title('挖掘机斗杆与动臂夹角随动臂转角变化曲线')
xlabel('动臂转角（度）')
ylabel('斗杆与动臂夹角（度）')
 grid
b=L(2)*cos(afa)+(L(3)+L(4))*cos(beita+afa-pi);
a=L(2)*sin(afa)+(L(3)+L(4))*sin(beita+afa-pi);
subplot(2,1,2)
plot(b,a)
title('挖掘机铲斗轨迹曲线')
xlabel('时间（秒）')
ylabel('轨迹（米）')
grid