1、A*算法原理
如上图所示，红色（2.3）起点，红色（6，3）终点，黑色表示障碍物。
如何走出如图所示的路径呢？
这里博主采用A*算法。原理如下：
1、基本定义
A*（A-Star）算法是一种常用的寻路算法，用于图形表达的环境中找到起点到目标点的最短路径。
代价函数𝑓(𝑛)由两部分组成：起点沿着已生成的路径到达当前节点的开销𝑔(𝑛)和当前节点到终点的预估开销ℎ(𝑛)。公式表示： 𝑓(𝑛) = 𝑔(𝑛) + ℎ(𝑛)
这里我更称之为：𝑔(𝑛)已走代价、 ℎ(𝑛)预估代价（预估代价常用的有两种，曼哈顿距离和欧几里得距离本文这里采用曼哈顿距离
h = abs(end_node(1) - openlist(i,1))+abs(end_node(2) - openlist(i,2));%预估代价这里是曼哈顿距离，欧几里得距离是开方那个）
open列表:一个记录下所有被考虑来寻找最短路径的格子
closed列表: 一个记录下不会再被考虑的格子
2、A*算法的详细原理
这里参考了其他博主的原理
【精选】A*算法（超级详细讲解，附有举例的详细手写步骤）-CSDN博客
这里重点注意下最后一张图片6(c)里面，当选中的节点a为当前父节点时，此时节点a的子节点里面有（c1、c2、.....c6等等），判断计算得到的子节点是否在open_list中，若此时的子节点c,若在，则比较更新，这个时候要计算下从a->c的g值，计算公式为
g = openlist_cost(min_index,1)+norm(parent_node - child_node);分为上一个的父节点的g加上从a->c的g值，对比下原先到c的g值，若比原先的小，则做如下操作：
2、详细代码 （非深蓝版本）
clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 栅格地图的行数和列数
m = 5;
n =7;
start_node = [2,3];
end_node = [6,3];
obs = [4,2;4,3;4,4];
% 画栅格地图
for i = 1:m
plot([0,n],[i,i],'k');%画横线
hold on;
end
for i = 1:n
plot([i,i],[0,m],'k');
end
axis equal;
xlim([0,n]);
ylim([0,m]);
%绘制起点、终点、障碍物
fill([start_node(1)-1,start_node(1),start_node(1),start_node(1)-1],...
[start_node(2)-1,start_node(2)-1,start_node(2),start_node(2)] ,'r');
fill([end_node(1)-1,end_node(1),end_node(1),end_node(1)-1],...
[end_node(2)-1,end_node(2)-1,end_node(2),end_node(2)] ,'r');
for i = 1:size(obs,1)%返回矩阵行数,
temp_node = obs(i,:);
fill([temp_node(1)-1,temp_node(1),temp_node(1),temp_node(1)-1],...
[temp_node(2)-1,temp_node(2)-1,temp_node(2),temp_node(2)] ,'k');
end
%%%%%%%%%%%%%%%%%%%%%%%% A*算法 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%初始化closelist
closelist = start_node;
closelist_path = {start_node,start_node};
closelist_cost = 0;
child_nodes = child_nodes_cal(start_node,m,n,obs,closelist);%子节点搜索函数
%%初始化openlist
%openlist_path这个元胞第一列表示搜索到子节点坐标，第二列是从当前父节点，到子节点的坐标
openlist = child_nodes;
for i = 1:size(openlist,1)
openlist_path{i,1} = openlist(i,:);
openlist_path{i,2} = [start_node;openlist(i,:)];
end
%%计算G()和h()
for i = 1:size(openlist,1)
g = norm(start_node - openlist(i,1:2));%当前代价
h = abs(end_node(1) - openlist(i,1))+abs(end_node(2) - openlist(i,2));%预估代价这里是曼哈顿距离，欧几里得距离是开方那个
f= g+h;
openlist_cost(i,:)=[g,h,f];
end
%%开始A*搜索
%从openlist_cost中搜索移动代价最小的节点
[~,min_index] = min(openlist_cost(:,3));%这里min函数返回最小值和其对应的索引,这里不需要最小是多少，只需要索引即可，因此用~表示占位符号
parent_node = openlist(min_index,:);
%%进入循环
flag = 1;
while flag
child_nodes = child_nodes_cal(parent_node,m,n,obs,closelist);
for i = 1:size(child_nodes,1)%这里是为了画出来每次搜索的子节点
path_tmp2 = child_nodes(i,:);
fill([path_tmp2(1)-1,path_tmp2(1),path_tmp2(1),path_tmp2(1)-1],...
[path_tmp2(2)-1,path_tmp2(2)-1,path_tmp2(2),path_tmp2(2)],'b');
end
hold on;
pause(0.5);%给个暂停时间，可以显示动图
%判断计算得到的子节点是否在open_list中，若在，则比较更新，这里更新是为了回溯父节点；若没在，则追加到openlist中
for i = 1:size(child_nodes,1)
child_node = child_nodes(i,:);
[in_flag,openlist_index] = ismember(child_node,openlist,'rows');%表示当前节点chile_node是否在openlist中，若在则in_flag = 1,openlist_index返回的是对应的索引
g = openlist_cost(min_index,1)+norm(parent_node - child_node);
h = abs(child_node(1) - end_node(1))+ abs(child_node(2) - end_node(2));
f = g+ h;
if in_flag == 1
if g < openlist_cost(openlist_index,1)%只有此时计算的g值小于原先的g，才更新，否则不计算
openlist_cost(openlist_index,1) = g;
openlist_cost(openlist_index,3) = f;
openlist_path{openlist_index,2} = [openlist_path{min_index,2};child_node];%这里更换了原先的父节点(原先的父节点为上一次的父节点第一次为起始点（2，3）)，选择此时的父节点也就是计算的openlist(min_index,:)最小的点（2.4）当原先的父节点，此时的openlist_path{min_index,2}，为元胞数组第二列
end
else
openlist(end+1,:) = child_node;
openlist_cost(end + 1,:) = [g,h,f];
openlist_path{end + 1,1} = child_node;
openlist_path{end,2} = [openlist_path{min_index,2};child_node];%这里因为 openlist_past{end + 1,1} = child_node，第一列已经扩展出来了，对应的这一行的第二列补齐为[]，因此这里的openlist_path{end,2}，就相当于没扩展之前的openlist_path{end+ 1,2}
end
end
%从openlist中移除代价最小的节点到closelist中
closelist(end + 1,:) = parent_node;
closelist_cost(end + 1,1) = openlist_cost(min_index,3);
closelist_path(end + 1,:) = openlist_path(min_index,:);
path_tmp = closelist_path{end,2};
for i = 1:size(path_tmp,1)%返回矩阵行数,
path_tmp1 = path_tmp(i,:);
fill([path_tmp1(1)-1,path_tmp1(1),path_tmp1(1),path_tmp1(1)-1],...
[path_tmp1(2)-1,path_tmp1(2)-1,path_tmp1(2),path_tmp1(2)] ,'g');
hold on
% pause(0.5);
end
openlist(min_index,:) = [];
openlist_cost(min_index,:) = [];
openlist_path(min_index,:) = [];
%重新搜索
[~,min_index] = min(openlist_cost(:,3));%这里min函数返回最小值和其对应的索引,这里不需要最小是多少，只需要索引即可，因此用~表示占位符号
parent_node = openlist(min_index,:);
%判断是否搜索到终点
if parent_node == end_node
closelist(end + 1,:) = parent_node;
closelist_cost(end + 1,1) = openlist_cost(min_index,3);
closelist_path(end + 1,:) = openlist_path(min_index,:);
flag = 0;
end
end
%%画路径
path_opt = closelist_path{end,2};
path_opt(:,1) = path_opt(:,1) - 0.5;%减去0.5是为了画图显示在正中间
path_opt(:,2) = path_opt(:,2) - 0.5;
scatter( path_opt(:,1), path_opt(:,2),'k');%画散点图
plot( path_opt(:,1), path_opt(:,2),'k')
子函数 child_nodes_cal
function child_nodes = child_nodes_cal(parent_node,m,n,obs,closelist)
child_nodes = [];
field = [1,1;n,1;n,m;1,m];%限定地图范围
%%第一个子节点
child_node = [parent_node(1) - 1,parent_node(2) + 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第二个子节点
child_node = [parent_node(1),parent_node(2) + 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第3个子节点
child_node = [parent_node(1) + 1,parent_node(2) + 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第4个子节点
child_node = [parent_node(1) + 1,parent_node(2)];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第5个子节点
child_node = [parent_node(1) + 1,parent_node(2) - 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第6个子节点
child_node = [parent_node(1),parent_node(2) - 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第7个子节点
child_node = [parent_node(1) - 1,parent_node(2) - 1];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%第8个子节点
child_node = [parent_node(1) - 1,parent_node(2)];
if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
child_nodes = [child_nodes;child_node];
end
end
%%排除已经存在于closelist的节点
delete_idx = [];
for i = 1:size(child_nodes,1)
if ismember(child_nodes(i,:),closelist,'rows')
delete_idx(end + 1) = i;
end
end
child_nodes(delete_idx,:) = [];
end3、路径结果
这里也可以换一个复杂路径
4、深蓝版本A*
本节作业主要是完成A_star_search.m，在随机的地图中生成一条安全的A*路径。
具体原理如上，其中作业里提供了大量的函数可供使用：
这里博主就采用这些函数，来完成这个作业
1、主函数代码：
% Used for Motion Planning for Mobile Robots
% Thanks to HKUST ELEC 5660
close all; clear all; clc;
set(gcf, 'Renderer', 'painters');
set(gcf, 'Position', [500, 50, 700, 700]);
% Environment map in 2D space
xStart = 4.0;
yStart = 1.0;
xTarget = 9.0;
yTarget = 9.0;
MAX_X = 20;
MAX_Y = 20;
map = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y);
% Waypoint Generator Using the A*
path = A_star_search(map, MAX_X,MAX_Y);
% visualize the 2D grid map
visualize_map(map, path);
2、地图环境构建代码
function map = obstacle_map(xStart,yStart,xTarget,yTarget,MAX_X,MAX_Y)
%This function returns a map contains random distribution obstacles.
rand_map = rand(MAX_X,MAX_Y);
map = [];
map(1,1) = xStart;
map(1,2) = yStart;
k=2;
obstacle_ratio = 0.45;
for i = 1:1:MAX_X
for j = 1:1:MAX_Y
if( (rand_map(i,j) < obstacle_ratio) && (i~= xStart || j~=yStart) && (i~= xTarget || j~=yTarget))
map(k,1) = i;
map(k,2) = j;
k=k+1;
end
end
end
map(k,1) = xTarget;
map(k,2) = yTarget;
end
3、可视化代码
% Used for Motion Planning for Mobile Robots
% Thanks to HKUST ELEC 5660
close all; clear all; clc;
set(gcf, 'Renderer', 'painters');
set(gcf, 'Position', [500, 50, 700, 700]);
% Environment map in 2D space
xStart = 4.0;
yStart = 1.0;
xTarget = 9.0;
yTarget = 9.0;
MAX_X = 20;
MAX_Y = 20;
map = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y);
% Waypoint Generator Using the A*
path = A_star_search(map, MAX_X,MAX_Y);
% visualize the 2D grid map
visualize_map(map, path);
4、 A*算法设计的函数function
function i_min = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget)
%Function to return the Node with minimum fn
% This function takes the list OPEN as its input and returns the index of the
% node that has the least cost
%
% Copyright 2009-2010 The MathWorks, Inc.
temp_array=[];
k=1;
flag=0;
goal_index=0;
for j=1:OPEN_COUNT
if (OPEN(j,1)==1)%未访问过的点
temp_array(k,:)=[OPEN(j,:) j]; %#ok<*AGROW>
if (OPEN(j,2)==xTarget && OPEN(j,3)==yTarget)
flag=1;
goal_index=j;%Store the index of the goal node
end
k=k+1;
end
end %Get all nodes that are on the list open
if flag == 1 % one of the successors is the goal node so send this node
i_min=goal_index;
end
%Send the index of the smallest node
if size(temp_array ~= 0)
[min_fn,temp_min]=min(temp_array(:,8));%Index of the smallest node in temp array
i_min=temp_array(temp_min,9);%Index of the smallest node in the OPEN array
else
i_min=-1;%The temp_array is empty i.e No more paths are available.
end
----------------------------------------------
function n_index = node_index(OPEN,xval,yval)
%This function returns the index of the location of a node in the list
%OPEN
%
% Copyright 2009-2010 The MathWorks, Inc.
i=1;
while(OPEN(i,2) ~= xval || OPEN(i,3) ~= yval )
i=i+1;
end
n_index=i;
end
----------------------------------------------------
function new_row = insert_open(xval,yval,parent_xval,parent_yval,hn,gn,fn)
%Function to Populate the OPEN LIST
%OPEN LIST FORMAT
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%-------------------------------------------------------------------------
%
% Copyright 2009-2010 The MathWorks, Inc.
new_row=[1,8];
new_row(1,1)=1;
new_row(1,2)=xval;
new_row(1,3)=yval;
new_row(1,4)=parent_xval;
new_row(1,5)=parent_yval;
new_row(1,6)=hn;
new_row(1,7)=gn;
new_row(1,8)=fn;
end
------------------------------------------------------------------------------
function exp_array=expand_array(node_x,node_y,gn,xTarget,yTarget,CLOSED,MAX_X,MAX_Y)
%Function to return an expanded array
%This function takes a node and returns the expanded list
%of successors,with the calculated fn values.
%The criteria being none of the successors are on the CLOSED list.
%
%Copyright 2009-2010 The MathWorks, Inc.
%EXPANDED ARRAY FORMAT
%--------------------------------
%|X val |Y val ||h(n) |g(n)|f(n)|
%--------------------------------
exp_array=[];
exp_count=1;
c2=size(CLOSED,1);%Number of elements in CLOSED including the zeros
for k= 1:-1:-1
for j= 1:-1:-1
if (k~=j || k~=0) %The node itself is not its successor
s_x = node_x+k;
s_y = node_y+j;%相当于扩展了8个节点
if( (s_x >0 && s_x <=MAX_X) && (s_y >0 && s_y <=MAX_Y))%node within array bound
flag=1;
for c1=1:c2
if(s_x == CLOSED(c1,1) && s_y == CLOSED(c1,2))
flag=0;
end
end%End of for loop to check if a successor is on closed list.
if (flag == 1)
exp_array(exp_count,1) = s_x;
exp_array(exp_count,2) = s_y;
exp_array(exp_count,3) = distance(xTarget,yTarget,s_x,s_y);%distance between node and goal,hn
exp_array(exp_count,4) = gn+distance(node_x,node_y,s_x,s_y);%cost of travelling to node，gn
exp_array(exp_count,5) = exp_array(exp_count,3)+exp_array(exp_count,4);%fn
exp_count=exp_count+1;
end%Populate the exp_array list!!!
end% End of node within array bound
end%End of if node is not its own successor loop
end%End of j for loop
end%End of k for loop
------------------------------------------------------------
function dist = distance(x1,y1,x2,y2)
%This function calculates the distance between any two cartesian
%coordinates.
% Copyright 2009-2010 The MathWorks, Inc.
%欧式距离
dist=sqrt((x1-x2)^2 + (y1-y2)^2);
5、使用说明
Use the script "math.m" as the main entry point.
使用math.m作为主函数入口
A_star: useful functions for A*
distance.m: This function calculates the distance between any two cartesian coordinates.
用于计算笛卡尔坐标系下任意两点的距离（此函数可以用作修改启发函数，例如将距离计算换做曼哈顿距离）
insert_open.m: Function to Populate the OPEN LIST，IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
用于将node加入OPEN_LIST中，OPEN的规定格式为一个8*1数组：|1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
expand_array.m：This function takes a node and returns the expanded list of successors,with the calculated fn values.
用于返回node n 周围的m个扩展node的列表，规定格式为一个m*5*1数组：|X val |Y val ||h(n) |g(n)|f(n)|
min_fn.m: This function takes the list OPEN as its input and returns the index of the node that has the least cost
用于返回OPEN列表中cost最小的node的索引
node_index.m: This function returns the index of the location of a node in the list OPEN
用于返回OPEN列表中坐标为（xval,yval）的node的索引
A_star_search.m: This unfinished function is your homework, you can use the functions in the folder A_star, or you can do the whole work yourself.
作业部分完成A_star_search.m，输出路径点列表，规定格式为n*2*1数组：|x_val | y_val，可以选择使用A_start里写好的函数，也可以独立完整完成所有工作。
-----------------------------------------------------------------------------------------------------------
obstacle_map: This function returns a map contains random distribution obstacles.
用于返回一个含有随意分布障碍物的地图（障碍物出现概率可以通过改变obstacle_ratio的数值大小来实现）
visualize_map: This function visualizes the 2D grid map consist of obstacles/start point/target point/optimal path.
用于可视化二维的栅格地图，包含障碍物、起点、终点和最优路径点
6、A*算法主代码
这里因为某些原因，这里只部分展示，需要完整代码可以私信我，相互交流。
function path = A_star_search(map,MAX_X,MAX_Y)
%%
%This part is about map/obstacle/and other settings
%pre-process the grid map, add offset
size_map = size(map,1);
Y_offset = 0;
X_offset = 0;
%Define the 2D grid map array.
%Obstacle=-1, Target = 0, Start=1
MAP=2*(ones(MAX_X,MAX_Y));
%Initialize MAP with location of the target
xval=floor(map(size_map, 1)) + X_offset;
yval=floor(map(size_map, 2)) + Y_offset;
xTarget=xval;
yTarget=yval;
MAP(xTarget,yTarget)=0;
%Initialize MAP with location of the obstacle
for i = 2: size_map-1
xval=floor(map(i, 1)) + X_offset;
yval=floor(map(i, 2)) + Y_offset;
MAP(xval,yval)=-1;
end
%Initialize MAP with location of the start point
xval=floor(map(1, 1)) + X_offset;
yval=floor(map(1, 2)) + Y_offset;
xStart=xval;
yStart=yval;
MAP(xStart,yStart)=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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_X
for j=1:MAX_Y
if(MAP(i,j) == -1)
CLOSED(k,1)=i;
CLOSED(k,2)=j;
k=k+1;
end
end
end
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node
xNode=xStart;
yNode=yStart;
OPEN_COUNT=1;
goal_distance=distance(xNode,yNode,xTarget,yTarget);
path_cost=0;
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,goal_distance,path_cost,goal_distance);
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
n_index = 1;%先定义第一个父节点的索引
%%
%This part is your homework
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while(1) %you have to dicide the Conditions for while loop exit
%先确认父节点
parent_nodex = OPEN(n_index,2);
parent_nodey = OPEN(n_index,3);
%根据父节点扩展子节点
exp_array=expand_array(parent_nodex,parent_nodey,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);%生成扩展节点
%将扩展后的节点按照准则放入OPEN中7、结果展示
这里因为障碍物是随机生成的，因此可能有时会搜索不到路径
同时可以按照作业布置的那样， 生成如下地图。
扫一扫
901
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
