【路径规划】自动化拣货最优路径含Matlab源码.zip

clear clc close all %% 参数设置 RandNum=20;% 随机取点数 MoveSpeed=1;% 小车移动速度 RoadW=2;% 通道宽 LaneW=1.3;% 巷道宽 LaneNum=20;% 巷道总数 ShelfW=0.6;% 货架宽 ShelfL=2;% 货架长 LaneShelfNum=5;% 每巷道货架数 UnitW=2*ShelfW+LaneW;%每巷道货架的宽 UnitL=2*ShelfL*LaneShelfNum+RoadW;%每巷道货架的宽 starnode=[-1,1]; %% 对各点赋坐标值及计算距离矩阵 nnode=zeros(LaneNum*(2*LaneShelfNum+3),3);% 1-点的编号，2-点的x轴坐标，3-点的y轴坐标 nnode(:,1)=(1:LaneNum*(2*LaneShelfNum+3))'; i2=0; for i=1:LaneNum for i1=1:2*LaneShelfNum+3 i2=i2+1; nnode(i2,3)=starnode(2)+ShelfW+LaneW/2+(i-1)*UnitW; % if i1<7 nnode(i2,2)=starnode(1)+ShelfL/2+(i1-1)*ShelfL; i2; % else % nnode(i2,2)=starnode(1)+ShelfL/2+(i1-1)*ShelfL+RoadW; % end end end clear i2 i i1 %%%%%%%%%&&&&&&&&&&&&&&&&&&&&&&%%%%%%%%%%%%%%%%%%%%% D=zeros(LaneNum*(2*LaneShelfNum+3));% D距离矩阵计算 for i1=1:LaneNum*(2*LaneShelfNum+3) for i2=i1+1:LaneNum*(2*LaneShelfNum+3) i11=ceil(i1/(2*LaneShelfNum+3));% 巷道编号 i12=mod(i1,2*LaneShelfNum+3);% 货架编号 if i12==0 i12=2*LaneShelfNum+3; end i21=ceil(i2/(2*LaneShelfNum+3));% 巷道编号 i22=mod(i2,2*LaneShelfNum+3);% 货架编号 if i22==0 i22=2*LaneShelfNum+3; end if i11~=i21 if i12<LaneShelfNum+2&i22<LaneShelfNum+2 D(i1,i2)=(i21-i11)*UnitW+min(i22+i12-2,2*(LaneShelfNum+2)-i12-i22)*ShelfL; elseif i12<LaneShelfNum+2&i22>LaneShelfNum+2 D(i1,i2)=(i21-i11)*UnitW+abs(i22-i12)*ShelfL; elseif i12>LaneShelfNum+2&i22<LaneShelfNum+2 D(i1,i2)=(i21-i11)*UnitW+abs(i22-i12)*ShelfL; elseif i12>LaneShelfNum+2&i22>LaneShelfNum+2 D(i1,i2)=(i21-i11)*UnitW+min(i12+i22-2*(LaneShelfNum+2),2*(2*LaneShelfNum+3)-i12-i22)*ShelfL; elseif (i12~=LaneShelfNum+2&i22==LaneShelfNum+2)||(i12==LaneShelfNum+2&i22~=LaneShelfNum+2) D(i1,i2)=(i21-i11)*UnitW+abs(i22-i12)*ShelfL; elseif i12==LaneShelfNum+2&i22==LaneShelfNum+2 D(i1,i2)=(i21-i11)*UnitW+abs(i22-i12)*ShelfL; else error('距离矩阵计算错误') end else D(i1,i2)=abs(i22-i12)*ShelfL; end end end D=D+D'; clear i1 i11 i12 i2 i21 i22 %% 货架任务数 starnode=[1,1]; % totalracknum=LaneShelfNum*2*LaneNum; % Task=unique(ceil(rand(1,RandNum)*totalracknum)); Task=[24,33,38,45,56,69,77,88,90,98,100,118,130,132,136,142,151,154,160,192]; %% Task1=Task; for i=1:length(Task) i1=ceil(Task(i)/LaneShelfNum/2);% ceil函数计算出了巷道编号 i2=mod(Task(i),LaneShelfNum*2);% mod函数此处计算出了货架的编号 if i2==0 i2=LaneShelfNum*2; end if i2<LaneShelfNum+1 Task1(i)=i2+1+(i1-1)*(2*LaneShelfNum+3); else Task1(i)=i2+2+(i1-1)*(2*LaneShelfNum+3); end if i==1 addnode(1)=1+(i1-1)*(2*LaneShelfNum+3);%增加这个点27，这是为什么呢？有什么作用？ elseif i==length(Task) addnode(2)=1+(i1-1)*(2*LaneShelfNum+3);%增加这个点248，这是为什么呢？有什么作用？ end end clear i i1 i2 Task1=sort([addnode(1),Task1,addnode(2)]');%增加了两个点27和248，然后进行了从小到大的排序，现在总共有22行数据了。 Task1=[Task1,ceil(Task1/(2*LaneShelfNum+3)),mod(Task1,2*LaneShelfNum+3)];%增加对22行数据的巷道编号和货架编号进行重新计算。 Task1(find(Task1(:,3)==0),3)=2*LaneShelfNum+3;%先判断Task1矩阵里的第3列数据值是否有等于0的值，如果等于0返回“TRUE”即逻辑值是1， %find函数会找到判断向量里非零的值，并求得其顺序位置的数值，然后 %然后将位于这个位置上（即Task1矩阵里的第3列数据值）的数值改成13。 % Task1 1-点编号，2-巷道号，3-货架编号 %% %% 图形 for i=1:LaneNum %% 奇数巷道 for i2=1:LaneShelfNum x=starnode(1)+(i2-1)*ShelfL; y=starnode(2)+(i-1)*(2*ShelfW+LaneW); rectangle('Position',[x,y,ShelfL*0.95,ShelfW],'Curvature',[0.1,0.1],... 'FaceColor','b'); end clear i2 x y for i2=1:LaneShelfNum x=starnode(1)+(i2-1)*ShelfL+LaneShelfNum*ShelfL+RoadW; y=starnode(2)+(i-1)*(2*ShelfW+LaneW); rectangle('Position',[x,y,ShelfL*0.95,ShelfW],'Curvature',[0.1,0.1],... 'FaceColor','b'); end clear i2 x y %% 偶数巷道 for i2=1:LaneShelfNum x=starnode(1)+(i2-1)*ShelfL; y=starnode(2)+(i-1)*(2*ShelfW+LaneW)+ShelfW+LaneW; rectangle('Position',[x,y,ShelfL*0.95,ShelfW],'Curvature',[0.1,0.1],... 'FaceColor','b'); end clear i2 x y for i2=1:LaneShelfNum x=starnode(1)+(i2-1)*ShelfL+LaneShelfNum*ShelfL+RoadW; y=starnode(2)+(i-1)*(2*ShelfW+LaneW)+ShelfW+LaneW; rectangle('Position',[x,y,ShelfL*0.95,ShelfW],'Curvature',[0.1,0.1],... 'FaceColor','b'); end clear i2 x y end clear i rectangle('Position',[-2,0,1,LaneNum*(2*ShelfW+LaneW)+1],'Curvature',[0,0],... 'FaceColor','b'); hold on xlim([-5,starnode(1)+LaneShelfNum*2*ShelfL+RoadW]+2) ylim([0,LaneNum*(2*ShelfW+LaneW)+1]) %% 每个巷道遍历到,对每个巷道都走遍的意义是什么呢？ Task2=[];%给定义了一个空的向量 tmpnode=Task1(1,:);%将Task1有第一行数据赋值给tmpnode:即27 3 1 id=find(Task1(:,2)==tmpnode(2));%找出Task1第二列数据值与tmpnode第二个值相等的顺序位置下标值，此处得到了下标值是1和2 if ~isempty(id)%isempty函数是当id里没有数据时（或内容为空），逻辑取值为1；如果有数据内容，逻辑值为0；“~”的意思是在逻辑上取反，只有0和1两种结果。此处最终得值1. Task2=[Task2;Task1(id,1)];%此处是把Task1的第一列的id下标值（即第一和第二个值）赋给了Task2 end tmpnode=Task1(id(end),1);%此处是把id最后的下标值对应的Task1里的数值，赋给了tmpnode，即31 Task1(id,:)=[];%此处是把Task1的id下标值对应的数据赋值为空，还不知道为什么要这样做。 clear id %清除id这个变量 % while ~isempty(Task1) %此处是确认Task1里面不是为空的，然后继续计算。 id=find(Task1(:,2)==min(Task1(:,2))); %批到Task1里巷道编号里最小的值，然后判断是否有与之相等的值，并列出他们的位置标签值。 tmp_N=Task1(id,1); %把所有的位置标签值对应的数据值赋给tmp_N if length(id)==size(Task1,1) %如果id的长度与Task1的长度一样，那么 Task2=[Task2;tmp_N(end:-1:1,1)]; else if D(tmpnode(1),tmp_N(1))<D(tmpnode(1),tmp_N(end)) Task2=[Task2;tmp_N]; elseif D(tmpnode(1),tmp_N(1))==D(tmpnode(1),tmp_N(end)) if Task2(end-1)<Task2(end) Task2=[Task2;tmp_N(end:-1:1)]; else Task2=[Task2;tmp_N]; end else Task2=[Task2;tmp_N(end:-1:1)]; end end Task1(id,:)=[]; clear id %% tmpnode=Task2(end); end %% 计算总距离 totaldist=0;%赋给总距离totaldist一个初值0 starnode=[-1,1]; %赋给starnode一个向量 for i=1:length(Task2) % lane_x=ceil(Task2(i)/(2*LaneShelfNum+3));% 巷道号 lane_y=mod(Task2(i),2*LaneShelfNum+3);% 货架编号 if lane_y==0 lane_y=2*LaneShelfNum+3; end %如果求得的货架编号是0，那么赋一个值13 x=starnode(1)+(lane_y-1)*ShelfL+ShelfL/3; y=starnode(2)+(lane_x-1)*(2*ShelfW+LaneW)+ShelfW+(LaneW-ShelfL/3)/2; rectangle('Position',[x,y,ShelfL/3,ShelfL/3],'Curvature',[0.1,0.1],... 'FaceColor','r'); %画一个长方形，位置Position：是由（x,y）这个点和宽(w)高(h)共同决定；曲率Curvature:是周边的曲率样式。 hold on if i<length(Task2) tmpnode=nnode(Task2(i:i+1),2:3); plot(tmpnode(:,1),tmpnode(:,2),'r','linewidth',3) hold on totaldist=totaldist+D(Task2(i),Task2(i+1)); end end title(['总距离：',num2str(totaldist)]) %num2str函数是将数值生成为字符型的结果 clear i lane_x lane_y x y pause(1)%比如pause(1.5)就是程序暂停1.5秒。 %% %%