基于遗传算法的避障轨迹规划（六自由机械臂）

function [fit]=fitness(ZG,BG,EG,ZhangAiWuZuoBiao,R) ZAWS=size(ZhangAiWuZuoBiao); %障碍点数 %% 第一段 %计算关节角 T=ZG(19); t=0:0.1:T; for m=1:6 XSJZ=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 T T^2 T^3 T^4 T^5; 0 1 2*T 3*T^2 4*T^3 5*T^4; 0 0 2 6*T 12*T^2 20*T^3]; GC=[BG(m);BG(m+6);BG(m+12);ZG(m);ZG(m+6);ZG(m+12)]; XS=XSJZ\GC; alpha1(m,:)=XS(1)+XS(2)*t+XS(3)*t.^2+XS(4)*t.^3+XS(5)*t.^4+XS(6)*t.^5; end %判断第一段是否干涉 point1=T/0.1+1; for m=1:point1 R1=eye(4)*trotz(alpha1(1,m)); %关节1位姿矩阵 R2=R1*transl(0,0,0)*trotx(pi/2)*trotz(alpha1(2,m)); %关节2位姿矩阵 R3=R2*transl(18,0,0)*trotx(0)*trotz(alpha1(3,m)); %关节3位姿矩阵 R4=R3*transl(0,0,0)*trotx(-pi/2)*trotz(alpha1(4,m)); R5=R4*transl(0,0,12)*trotx(pi/2)*trotz(alpha1(5,m)); R6=R5*transl(0,0,0)*trotx(-pi/2)*trotz(alpha1(6,m)); RY=R6*transl(5,0,0); MDZB1(:,m)=RY(1:3,4); %末端坐标 %判断杆件是否和障碍物干涉 %杆1 X1=R2(1,4);Y1=R2(2,4);Z1=R2(3,4); X2=R3(1,4);Y2=R3(2,4);Z2=R3(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; disp('杆1干涉'); return; end end end %杆2 X1=R4(1,4);Y1=R4(2,4);Z1=R4(3,4); X2=R5(1,4);Y2=R5(2,4);Z2=R5(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; disp('杆2干涉'); return; end end end %杆3 X1=R6(1,4);Y1=R6(2,4);Z1=R6(3,4); X2=RY(1,4);Y2=RY(2,4);Z2=RY(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; disp('杆3干涉'); return; end end end end %% 第二段 %判断第二段是否干涉 %计算关节角 T=ZG(20); t=0:0.1:T; for m=1:6 XSJZ=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 T T^2 T^3 T^4 T^5; 0 1 2*T 3*T^2 4*T^3 5*T^4; 0 0 2 6*T 12*T^2 20*T^3]; GC=[ZG(m);ZG(m+6);ZG(m+12);EG(m);EG(m+6);EG(m+12)]; XS=XSJZ\GC; alpha2(m,:)=XS(1)+XS(2)*t+XS(3)*t^2+XS(4)*t^3+XS(5)*t^4+XS(6)*t^5; end %判断第一段是否干涉 point2=T/0.1+1; for m=1:point2 R1=eye(4)*trotz(alpha2(1,m)); %关节1位姿矩阵 R2=R1*transl(0,0,0)*trotx(pi/2)*trotz(alpha2(2:m)); %关节2位姿矩阵 R3=R2*transl(18,0,0)*trotx(0)*trotz(alpha2(3,m)); %关节3位姿矩阵 R4=R3*transl(0,0,0)*trotx(-pi/2)*trotz(alpha2(4,m)); R5=R4*transl(0,0,12)*trotx(pi/2)*trotz(alpha2(5,m)); R6=R5*transl(0,0,0)*trotx(-pi/2)*trotz(alpha2(6,m)); RY=R6*transl(5,0,0); MDZB2(:,m)=RY(1:3,4); %末端坐标 %判断杆件是否和障碍物干涉 %杆1 X1=R2(1,4);Y1=R2(2,4);Z1=R2(3,4); X2=R3(1,4);Y2=R3(2,4);Z2=R3(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; %disp('杆1干涉')； return; end end end %杆2 X1=R4(1,4);Y1=R4(2,4);Z1=R4(3,4); X2=R5(1,4);Y2=R5(2,4);Z2=R5(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; %disp('杆2干涉')； return; end end end %杆3 X1=R6(1,4);Y1=R6(2,4);Z1=R6(3,4); X2=RY(1,4);Y2=RY(2,4);Z2=RY(3,4); W=((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2)^(1/2); for k=1:ZAWS(1) X=ZhangAiWuZuoBiao(k,1); Y=ZhangAiWuZuoBiao(k,2); Z=ZhangAiWuZuoBiao(k,3); for n=1:ceil(W) if n==ceil(W) G1(:,n)=[X2;Y2;Z2]; else G1(:,n)=[X1+(X2-X1)*(n-1)/ceil(W); Y1+(Y2-Y1)*(n-1)/ceil(W); Z1+(Z2-Z1)*(n-1)/ceil(W)]; end pick=((G1(1,n)-X)^2+(G1(2,n)-Y)^2+(G1(3,n)-Z)^2)^(1/2); if pick<R+0.5 fit=0; %disp('杆3干涉')； return; end end end end %% 最终适应度 s_alpha=[alpha1 alpha2]; MDZB=[MDZB1 MDZB2]; alpha_z=s_alpha(:,1:(end-1))-s_alpha(:,2:end); MDZB_x=MDZB(:,1:(end-1))-MDZB(:,2:end); sum_alpha=sum(sum(abs(alpha_z))); www=size(MDZB_x); sum_path=0; for m=1:www(2) sum_path=sum_patj+norm(MDZB_x(:,m)); end fit=1/(sum_alpha+sum_path+(ZG(19)+ZG(20))); %也可取fit=1/sum_path end