【路径规划】基于matlab A_star算法智能仓储机器人移动避碰路径规划【含Matlab源码 1180期】

一、简介

A算法
A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。
f(n)=g(n)+h(n)(1)
其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。
在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。
A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。
（1）P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。
（2）若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤（3）；否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT，进行下列步骤:

①计算NT的启发值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代价。
②如果NT在P表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，并将NT的父节点设为BT。
③如果NT在Q表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，将NT的父节点设为BT，并将NT移出到P表中。
④若NT既不在P表，也不在Q表中，则将NT的父节点设为BT，并将NT移到P表中。
⑤转到步骤（2）继续执行。
（3）从终点T回溯，依次找到父节点，并加入优化路径中，直到起点S，即可得出优化路径。

二、部分源代码

%main函数,robot_number的数目目前支持2~6；
clear all;
close all;
clear variables;
clc;
robot_number=3;
[map,grid1,Nrow,Ncol,sorting_table,Obstacle]=initial_input();
% Obstacle_index=max(round(rand(1,30)*length(Obstacle)),1);
%    Obstacle_index=randperm(length(Obstacle),10);Obstacle_index=[48,24,70,43,34,15,59,12,58,35,46,61, 4,79,30,63,60,31,67,37, 6,20,50,73,72,56,47,14,78,62];
mission_list=[];start_node=[];
for i=1:length(Obstacle_index)mission_list=[mission_list,Obstacle(Obstacle_index(i))];
end
mission=mission_list(1:robot_number);
%从N个数中不重复的选取一些数
%   start_node=Obstacle(randperm(length(Obstacle),robot_number));start_node=[166,352,5,12,120,220,240,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40];
for k1=1:robot_number    robot_ID(k1)=robot();robot_ID(k1).location=start_node(k1);
end[scedule_table,undistributed_mission]=Schedule1(robot_ID,mission,Nrow,Ncol);
%scedule_table结果的第一行为任务的索引，格式大多数情况为1:length(mission), 第二行为该任务分配给的小车。
for i=1:length(scedule_table(1,:))end_node(scedule_table(2,i))=mission(scedule_table(1,i));
end
for k2=1:robot_numbermap3=map;for map_i = 1:robot_numberif map_i~=k2[ia,ib]=ind2sub([Nrow,Ncol],start_node(map_i));map3(Ncol-ib+1,ia)=2;[ia1,ib1]=ind2sub([Nrow,Ncol],end_node(map_i));map3(Ncol-ib1+1,ia1)=2;endend[sorting_table_index]= assign_sorting_table(end_node(k2),sorting_table);allpath_ID(k2)=allpath([Astar_method(start_node(k2),end_node(k2),0,map3),Astar_method(end_node(k2),sorting_table(sorting_table_index,1),1,map3),...sorting_table(sorting_table_index,2:4),Astar_method(sorting_table(sorting_table_index,5),end_node(k2),1,map3)]);
endif length(robot_ID)>1% 所有的两两路径冲突检验all_conflict=nchoosek(1:length(robot_ID), 2);k1=1;iters=length(all_conflict(:,1))*2;while k1<=length(all_conflict(:,1))&iters>0;if k1==1[wait_path1,wait_path2,result1]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...allpath_ID(all_conflict(k1,2)).wait_path,1,map);[wait_path4,wait_path3,result2]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...allpath_ID(all_conflict(k1,1)).wait_path,1,map);if (result1==11)&(result2==11)disp('前两量车无法避让，需要重新规划路径')elseif (result2==11)|((result1==10)&(length(wait_path1)+length(wait_path2)<=length(wait_path3)+length(wait_path4)))allpath_ID(all_conflict(k1,1)).wait_path=wait_path1;allpath_ID(all_conflict(k1,2)).wait_path=wait_path2;elseallpath_ID(all_conflict(k1,1)).wait_path=wait_path3;allpath_ID(all_conflict(k1,2)).wait_path=wait_path4;endendk1=k1+1;else[wait_path5,wait_path6,result3]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...if result3==10allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;k1=k1+1;else [wait_path6,wait_path5,result3]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...if result3==10allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;k1=1;elseif result3==12k1=1;
%                     disp('某两量车无法避让，需要重新规划路径')endendendenditers=iters-1;if iters==0;disp('超出最大迭代次数，需要重新规划路径')endend% 参考程序Archive中astardemo.m
%存储可行的节点，朝向1，2，3，4
function [path]=Astar_method(start_node,end_node,loading,map,stop_car)
%     Nrow=20;start_node=115;shelf_node=225;end_node=143;loading=1;setOpen=[start_node];setOpenCosts=[0];setOpenHeuristics=[Inf];setClosed=[];setClosedCosts=[];%load /Users/jindi/Desktop/map.txtNrow=length(map(1,:));Ncol=length(map(:,1));
%     if %为该栅格右上角的坐标[xlabID_end,ylabID_end]=ind2sub([Nrow,Ncol],end_node);%将起点和终点改为没有障碍map(Ncol-ylabID_end+1,xlabID_end)=0;if loading==0for i=1:(Nrow*Ncol)connect_node_name=[0,0,0,0];%为该栅格右上角的坐标[xlabID,ylabID]=ind2sub([Nrow,Ncol],i);if (xlabID<Nrow)if (~ismember(map(Ncol-ylabID+1,xlabID+1),[2,99]))connect_node_name(1)=i+1;endendif (ylabID<Ncol)%map的坐标关系和栅格中的对于y轴和列数是相反的if(~ismember(map(Ncol-ylabID,xlabID),[2,99]))connect_node_name(2)=i+Nrow;end    endif (xlabID>1)if (~ismember(map(Ncol-ylabID+1,xlabID-1),[2,99]))connect_node_name(3)=i-1;endendif (ylabID>1)if(~ismember(map(Ncol-ylabID+2,xlabID),[2,99]))connect_node_name(4)=i-Nrow;endendconnect_node_ID(i)=connect_node([connect_node_name,connect_node_name]);endelsefor i=1:(Nrow*Ncol)connect_node_name=[0,0,0,0];%为该栅格右上角的坐标[xlabID,ylabID]=ind2sub([Nrow,Ncol],i);if (xlabID<Nrow)if(~ismember(map(Ncol-ylabID+1,xlabID+1),[1,2,3,99]))connect_node_name(1)=i+1;end endif (ylabID<Ncol)%             [xlabID1,ylabID1]=ind2sub([Nrow,Nrow],i+Nrow)if(~ismember(map(Ncol-ylabID,xlabID),[1,2,3,99]))connect_node_name(2)=i+Nrow;endendif (xlabID>1)if(~ismember(map(Ncol-ylabID+1,xlabID-1),[1,2,3,99]))connect_node_name(3)=i-1;endendif (ylabID>1)if(~ismember(map(Ncol-ylabID+2,xlabID),[1,2,3,99]))connect_node_name(4)=i-Nrow;endend%为了右侧优先，设定的重复路径connect_node_ID(i)=connect_node([connect_node_name connect_node_name]);endend

三、运行结果

四、备注

版本：2014a