【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】

一、获取代码方式

获取代码方式1：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

获取代码方式2：
通过紫极神光博客主页开通CSDN会员，凭支付凭证，私信博主，可获得此代码。

获取代码方式3：
完整代码已上传我的资源：【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】

备注：开通CSDN会员，仅只能免费获得1份代码（有效期为开通日起，三天内有效）；
订阅紫极神光博客付费专栏，可免费获得2份代码（有效期为订阅日起，三天内有效）；

二、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

图3 A Star算法流程

三、部分源代码

%% 使用A *搜索演示%% 参数的设定
% 定义二维地图框架
MAX_X = 10;
MAX_Y = 10;
MAP = zeros(MAX_X,MAX_Y) % 此数组存储地图的坐标和每个坐标中的对象
% 获得障碍，目标和机器人位置
% 使用输入值初始化MAP
% 障碍= -1，目标= 1，起点= 1，空格= 0
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X+1 1 MAX_Y+1])  % 框图的大小
grid on;                     % 加入网格
hold on;                     % 与新图共存
n=0;                         % 障碍物的数量for BB = 1:3
%% 确定目标点
pause(1);                           % 一般是为了动态观察变化过程 pause(a)暂停a秒后执行下一条指令
h=msgbox('请使用鼠标左键选择目标');  % 显示提示窗口
uiwait(h,5);
if ishandle(h) == 1delete(h);
end
xlabel('请使用鼠标左键选择目标','Color','black');
but=0;                               % 这个地方可以设置成1,这样的话，统一目标点
while (but ~= 1)                     % 重复，直到没有单击“向左”按钮[xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
xTarget=xval;                        % X目标的坐标
yTarget=yval;                        % Y目标的坐标
MAP(xval,yval)=1;                    % 初始化地图中的目标位置
plot(xval+.5,yval+.5,'gd');
text(xval+1,yval+.5,'目标点')%% 确定障碍物
pause(2);
h=msgbox('使用鼠标左键选择障碍物，使用右键选择最后一个障碍物');xlabel('使用鼠标左键选择障碍物，使用右键选择最后一个障碍物','Color','blue');
uiwait(h,10);
if ishandle(h) == 1delete(h);
end
while but == 1[xval,yval,but] = ginput(1);xval=floor(xval);yval=floor(yval);MAP(xval,yval)=-1;             %Put on the closed list as wellplot(xval+.5,yval+.5,'ro');
end%% 确定起点
pause(1);
h=msgbox('请使用鼠标左键选择初始位置');
uiwait(h,5);
if ishandle(h) == 1delete(h);
end
xlabel('请选择初始位置','Color','black');
but=0;
while (but ~= 1)                    % 重复，直到没有单击“向左”按钮[xval,yval,but]=ginput(1);xval=floor(xval);yval=floor(yval);
end
xStart=xval;                       % 起始位置
yStart=yval;
MAP(xval,yval)=1;plot(xval+.5,yval+.5,'bo');
% End of obstacle-Target pickup%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM  用于算法的列表
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE       开放列表结构
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE 封闭的列表结构
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];%Put all obstacles on the Closed list 将所有障碍物放在关闭列表中
k=1;   % Dummy counter
for i=1:MAX_Xfor j=1:MAX_Yif(MAP(i,j) == -1)CLOSED(k,1)=i; CLOSED(k,2)=j; k=k+1;endend
end
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node  将起始节点设置为第一个节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1)
%  plot(xNode+.5,yNode+.5,'go');exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);exp_count=size(exp_array,1);%UPDATE LIST OPEN WITH THE SUCCESSOR NODES  更新列表打开SUCCESSOR NODES%OPEN LIST FORMAT%--------------------------------------------------------------------------%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|%--------------------------------------------------------------------------%EXPANDED ARRAY FORMAT%--------------------------------%|X val |Y val ||h(n) |g(n)|f(n)|%--------------------------------for i=1:exp_countflag=0;for j=1:OPEN_COUNTif(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %#ok<*SAGROW>if OPEN(j,8)== exp_array(i,5)%UPDATE PARENTS,gn,hnOPEN(j,4)=xNode;OPEN(j,5)=yNode;OPEN(j,6)=exp_array(i,3);OPEN(j,7)=exp_array(i,4);end;%End of minimum fn checkflag=1;end;%End of node check
%         if flag == 1
%             break;end;%End of j forif flag == 0OPEN_COUNT = OPEN_COUNT+1;OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5));end;%End of insert new element into the OPEN listend;%End of i for%END OF WHILE LOOP% Find out the node with the smallest fn  找出fn最小的节点index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);if (index_min_node ~= -1)    %Set xNode and yNode to the node with minimum fnxNode=OPEN(index_min_node,2);yNode=OPEN(index_min_node,3);path_cost=OPEN(index_min_node,6);  % Update the cost of reaching the parent node 更新到达父节点的成本%Move the Node to list CLOSED     将节点移动到列表CLOSEDCLOSED_COUNT=CLOSED_COUNT+1;CLOSED(CLOSED_COUNT,1)=xNode;CLOSED(CLOSED_COUNT,2)=yNode;OPEN(index_min_node,1)=0;else% No path exists to the Target!!NoPath=0;   % Exits the loop!end;            % End of index_min_node check
end;
% 算法运行后，通过从最后一个节点（如果它是目标节点）开始生成最佳路径，然后识别其父节点，直到它到达起始节点。这是最佳路径i=size(CLOSED,1);
Optimal_path=[];
xval=CLOSED(i,1);
yval=CLOSED(i,2);
i=1;
Optimal_path(i,1)=xval;
Optimal_path(i,2)=yval;i=i+1;
if ( (xval == xTarget) && (yval == yTarget))inode=0;% Traverse OPEN and determine the parent nodes 遍历OPEN并确定父节点parent_x=OPEN(node_index(OPEN,xval,yval),4); % node_index returns the index of the node     node_index返回节点的索引parent_y=OPEN(node_index(OPEN,xval,yval),5);while( parent_x ~= xStart || parent_y ~= yStart)Optimal_path(i,1) = parent_x;Optimal_path(i,2) = parent_y;%Get the grandparents:-)inode=node_index(OPEN,parent_x,parent_y);parent_x=OPEN(inode,4);    % node_index returns the index of the node  node_index返回节点的索引parent_y=OPEN(inode,5);i=i+1;end;j=size(Optimal_path,1);% Plot the Optimal Path!  % 绘制最佳路径！p=plot(Optimal_path(j,1)+.5,Optimal_path(j,2)+.5,'bo');j=j-1;for i=j:-1:1h=msgbox('不好意思，没有找到目标的路径!','warn');uiwait(h,5);
end
Optimal_path
[m,n]=size(Optimal_path);
for ii=1:m;
%     for jj=1:1a=Optimal_path(ii,1)b=Optimal_path(ii,2)MAP(a,b)=1c=a+0.5;d=b+0.5;
%         Optimal_path(i,j)=1;
%     end
plot(c,d,'*k');hold on;
end% MAP(Optimal_path(2,1),Optimal_path(2,2))=1
MAP1=rot90(MAP)
number = size(Optimal_path,1)if (BB==1)
Optimal_path1 = Optimal_pathhold on;
else if (BB==2)Optimal_path2 = Optimal_pathhold on;elseOptimal_path3 = Optimal_pathend
endend

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)