概述
A算法与D算法在路径规划中应是用较为广泛的一类算法，D是A的增强版本，先来看看A算法。
一、A原理
A算法结构相对来说较为复杂。
（1）首先注意区分两个词，当前已选定的栅格&当前已选定栅格的相邻栅格（后面简称相邻栅格）。
（2）两个概念：
启发距离：h=该相邻栅格到目标点的距离。
代价距离：g=已选定栅格到已选定栅格的某一相邻栅格的代价距离+已选定栅格与起点之间已生成的代价距离。
f=g+h
(3)两个列表：
openlist:该表中存放的栅格点都是以可达的相邻栅格的身份进入的。
closelist:该表中的栅格都是以可以作为父栅格身份进入的，父栅格很重要，因为直到寻路完成，所得到的所有父栅格构成起点至终点的路线。
A过程大致如下：（与之后程序中的变量名称相对应）
1、从起点出发，进行初始化。
将起点（start）放入open list中。初始化将其作为当前已选定栅格（cell），并将起点本身作为自己的父栅格从openlist中移除放入closelist中。
2、提取cell的8个相邻栅格（s），判断每个相邻点是否为可达相邻点，即是否在地图范围内，是否为障碍物。
3、计算可达相邻栅格的h、g、f，并将当前选定栅格的可达相邻栅格放入openlist中。过程：
依次检查每个可达相邻栅格（s）是否已经存在于openlist中。
若已存在，比较两者的f值，（1）若当前值小，则将对应的已存在的栅格的父栅格替换为当前已选定栅格（cell），并更新对应的f、h、g值。（2）若原有值小，则不对其进行任何操作。
若不存在，则将其加入至openlist。
4、在当前的openlist中选择f值最小的相邻栅格作为下一个已选定栅格，并将当前已选定栅格（cell）作为该相邻栅格的父栅格，移入closelist中。
5、循环至将终点作为相邻栅格的身份加入到openlist中。
6、将父栅格依次连接起来，完成寻路。
二、具体代码：其中valid中的第一位为列表标志位，1为openlist成员，0为closelist成员
valid=[flag|s_x |s_ y | parent_x(cell) | parent_y(cell) | h(n) | g(n) | f(n)
1、主程序：
%valid中的第一列为列表标志位，1：代表该栅格在openlist中，0：代表该栅格在closelist中。
%in_valid中为障碍物节点+当前点作为夫栅格的点。
%(cell_x,cell_y)当前选定点
%(s_x,s_y)当前选定点的相邻点
clc
clear
close all
%坐标轴最大范围
max_x=20;
max_y=20;
environment=2*(ones(max_x,max_y));%20*20矩阵中每个元素值为2
j=0;
x_val = 1;
y_val = 1;
axis([1 max_x+1 1 max_y+1]) %axis设置坐标轴范围
grid on;
hold on;
n=0;
%设置终点坐标
xval=18;
yval=18;
x_target=xval;
y_target=yval;
environment(xval,yval)=0; %初始化环境中的目标点置0
plot(xval+.5,yval+.5,'gd'); %将目标点设置为绿色菱形
text(xval+1,yval+.5,'Target')%并在目标点上做Target标注
% 设置障碍物位置，障碍物位置处元素值为-1,显示效果填充为黑色
for i = 3:5
for j = 13:16
environment(i,j) = -1;
%plot(i+.5,j+.5,'rd');
end
end
x = [3, 6, 6, 3];
y = [13, 13, 17, 17];
fill(x,y,'k'); %fill：填充为黑色（k）
% obstacle 2
for i = 7:9
for j = 11:14
environment(i,j) = -1;
%plot(i+.5,j+.5,'rd');
end
end
x = [7, 10, 10, 7];
y = [11, 11, 15, 15];
fill(x,y,'k');
% obstacle 3
for i = 11:13
for j = 11:17
environment(i,j) = -1;
%plot(i+.5,j+.5,'rd');
end
end
x = [11, 14, 14, 11];
y = [11, 11, 18, 18];
fill(x,y,'k');
% obstacle 4
for i = 12:13
for j = 6:9
environment(i,j) = -1;
%plot(i+.5,j+.5,'rd');
end
end
x = [12, 14, 14, 12];
y = [6, 6, 10, 10];
fill(x,y,'k');
% obstacle 5
for i = 15:16
for j = 6:17
environment(i,j) = -1;
%plot(i+.5,j+.5,'rd');
end
end
x = [15, 17, 17, 15];
y = [6, 6, 18, 18];
fill(x,y,'k');
%plot(19+.5,19+.5,'gd');
%设置起始点位置
xval=3;
yval=3;
x_start=xval;%Starting Position
y_start=yval;%Starting Position
environment(xval,yval)=1;%起始点位置元素值为1
plot(xval+.5,yval+.5,'bo');%起点处用蓝色圆圈表示
valid=[]; % x | y | parent_x | parent_y | h(n) | g(n) | f(n)
in_valid=[]; % x | y
%获取障碍物位置
k=1;
for i=1:max_x
for j=1:max_y
if(environment(i,j) == -1) % check if obstacle
in_valid(k,1)=i;
in_valid(k,2)=j;
k=k+1;
end
end
end
in_valid_size=size(in_valid,1);%障碍物个数
%A*过程开始
cell_x=x_start;%将起点作为当前已选定
cell_y=y_start;
valid_size=1; % initialize size of valid_list
path_cost=0;
goal_distance=sqrt((cell_x-x_target)^2 + (cell_y-y_target)^2);%起点与终点的欧式距离
new_row=[1,8];
new_row(1,1)=1;
new_row(1,2)=cell_x;%cell_x为当前选定点坐标
new_row(1,3)=cell_y;%cell_y当前选定点坐标
new_row(1,4)=cell_x; % parent x
new_row(1,5)=cell_y; % parent y
new_row(1,6)=path_cost;
new_row(1,7)=goal_distance;
new_row(1,8)=goal_distance;
valid(valid_size,:)=new_row; % 用开始位置初始化valid
valid(valid_size,1)=0;
in_valid_size=in_valid_size+1;
in_valid(in_valid_size,1)=cell_x; % make it invalid for further iterations
in_valid(in_valid_size,2)=cell_y;
path_not_found=1;
while((cell_x ~= x_target || cell_y ~= y_target) && path_not_found == 1)
%h=distance（当前选定点，该当前选定点的一个相邻点），g=distance(目标点，该当前选定点的一个相邻点)，f=h+g
successors=explore_successors(cell_x,cell_y,path_cost,x_target,y_target,in_valid,max_x,max_y);%successors返回当前点的所有周围可选点的 的x | y | h | g | f 信息
successors_size=size(successors,1);%获取当前选定点相邻点可达的个数
for i=1:successors_size%遍历当前选定点的可达相邻点
flag=0;%标志位清零
for j=1:valid_size %遍历valid中之前的点，判断当前选定点的可达相邻点是否已经存在
if(successors(i,1) == valid(j,2) && successors(i,2) == valid(j,3) ) % 判断当前点的周围可达点是否已存在于valid中
valid(j,8)=min(valid(j,8),successors(i,5)); %如果已经存在，判断当前的f值与之前的比哪个小，若以前的小则不管，若现在的小，将当前选定点
if valid(j,8) == successors(i,5) %在判断一遍valid中的f值是否与当前的f值相同，若上一步判断现在的f值小，则不相等，需要执行以下步骤
valid(j,4)=cell_x;% 将当前选定点替换该点作为父节点parent x，并更新记录其h,g值
valid(j,5)=cell_y;% parent y
valid(j,6)=successors(i,3); % h
valid(j,7)=successors(i,4); % g
end;
flag=1;%该标志位置1表示valid矩阵中已经含有该节点。
end;
end;
if flag == 0 % if new cell with minimum f(n) then add to valid_path，若当前点周围可选点不在valid中，标志位为0，并将其加入valid中
valid_size= valid_size+1;
new_row=[1,8];
new_row(1,1)=1;%valid标志位，为0相当于该点已经移入closelist，不可在用，为1可用相当于在openlist中
new_row(1,2)=successors(i,1);
new_row(1,3)=successors(i,2);
new_row(1,4)=cell_x; % valid中4，5位为当前选定点相邻点的当前点，作为parent x
new_row(1,5)=cell_y; % parent y
new_row(1,6)=successors(i,3); % h
new_row(1,7)=successors(i,4); % g
new_row(1,8)=successors(i,5); % f
valid(valid_size,:)= new_row;
end;
end;
index_min_cell = min_f(valid,valid_size,x_target,y_target);%获取valid中的最小值
if (index_min_cell ~= -1) % 得到下一个最小值的点，if index with minimum fn is obstacle no path exists
cell_x=valid(index_min_cell,2);%将f值最小的相邻点设为新的当前点
cell_y=valid(index_min_cell,3);
path_cost=valid(index_min_cell,6);%记录目前的h值
in_valid_size=in_valid_size+1; % 将最小f值的相邻点移入in_valid中，put the cell in_valid so we dont come back on it again
in_valid(in_valid_size,1)=cell_x;
in_valid(in_valid_size,2)=cell_y;
valid(index_min_cell,1)=0;%将该相邻点在valid中所在行的第一个元素置0，不在检查。其对应的当前选定点设为父节点
else
path_not_found=0;
end;
end; %
% backtracking to find the path回溯找到路径
i=size(in_valid,1);%i为parents个数
path=[];
xval=in_valid(i,1); % 取in_valid最后一点，pick last in in_valid_list that must be target
yval=in_valid(i,2);
i=1;
path(i,1)=xval;%取in_valid最后一个点
path(i,2)=yval;
i=i+1;
%若in_valid中最后一点为目标点，则将其作为父节点
if ( (xval == x_target) && (yval == y_target)) %判断in_valid最后一点是否为终点
inode=0;
parent_x=valid(find((valid(:,2) == xval) & (valid(:,3) == yval),1),4);%find返回valid的第二列、第3列中第一个同时等于xval，yval的行数，即找到终点的上一个点，即终点的父节点
parent_y=valid(find((valid(:,2) == xval) & (valid(:,3) == yval),1),5);
while( parent_x ~= x_start || parent_y ~= y_start) %如果当前父节点不是起始点
path(i,1) = parent_x;%将当前父节点写入路径
path(i,2) = parent_y;
inode=find((valid(:,2) == parent_x) & (valid(:,3) == parent_y),1);%查找valid中第一个等于当前父节点的点所在的行数，inode为行数
parent_x=valid(inode,4);%取当前父节点的父节点，循环往复，直到当前的夫节点为起点，路点记录为从终点向起点方向
parent_y=valid(inode,5);
i=i+1;
end;
% plottin path画图
%path中记录的路点从上往下依次为由终点向起点方向的路点，因此此处path取值应从下想上取才为从起点向终点运动的方向
j=size(path,1);
p=plot(path(j,1)+.5,path(j,2)+.5,'bo');
j=j-1;
for i=j:-1:1%画从起点向终点的运动过程
pause(.25);%pause(n) 暂停执行 0.25秒，然后继续执行。
set(p,'XData',path(i,1)+.5,'YData',path(i,2)+.5);
drawnow ;%刷新屏幕，实时显示plot
end;
plot(path(:,1)+.5,path(:,2)+.5);%得出路径
else
disp( 'Sorry, No path exists to the Target!');
end
explore_successors.m
for k= 1:-1:-1 %分别获取当前点周围8个相邻点的信息
for j= 1:-1:-1
if (k~=j || k~=0) %把当前选定点排除掉，只获取当前选定点周围的可选点信息
s_x = x_cell+k; %（s_x,s_y）是8个相邻点坐标
s_y = y_cell+j;
if( (s_x >0 && s_x <=max_x) && (s_y >0 && s_y <=max_y)) % 该相邻点是否在地图范围内
flag=1; %当前点在地图范围内标志位为1
for c1=1:c2 % 遍历所有障碍物位置，检查当前点是否为障碍物
if(s_x == in_valid(c1,1) && s_y == in_valid(c1,2))
flag=0;%当前点为不可达点，标志位为0
end;
end;
if (flag == 1) %当前点可达，将当前点加入successors中
successors(successor_count,1) = s_x;
successors(successor_count,2) = s_y;
successors(successor_count,3) = h+distance(x_cell,y_cell,s_x,s_y);%h
successors(successor_count,4) = distance(x_target,y_target,s_x,s_y);%g
successors(successor_count,5) = successors(successor_count,3)+successors(successor_count,4);%f
successor_count=successor_count+1;
end
end
end
end
end
min_f.m
% 相当于取openlist中f值的最小值。index_min_cell = min_f(valid,valid_size,x_target,y_target)
function index_of_min = min_f(valid,valid_len,x_target,y_target)
temp_array=[];
k=1;
flag=0;%标志位清零
goal_index=0;
for j=1:valid_len %遍历valid中已加入的所有值，判断是否为终点
if (valid(j,1)==1)%若节点状态标志位为1相当于该相邻点在openlist中，为1该相邻点的父节点已经移入close list，不必再看
temp_array(k,:)=[valid(j,:) j]; %将valid中第一位=1的自由点依次加入到temp_array中，并在该行最后一位记录其在valid中的行数
if (valid(j,2)==x_target && valid(j,3)==y_target) %判断该相邻点是否为为终点，若是，标志位置1
flag=1;
goal_index=j;%目标索引指向valid中对应的该相邻点的行数
end;
k=k+1;
end;
end;
if flag == 1 %若是终点，最小值索引为终点
index_of_min=goal_index;
end
if size(temp_array ~= 0)%统计自由节点的个数
[min_f,temp_min]=min(temp_array(:,8)); %取其中f值最小的点的最小值及在temp_array中的行数，
%min_f
index_of_min=temp_array(temp_min,9);%取该行最后对应的valid的行号
fprintf('Cell with minimum f found to be x : %d y : %d with f = %f',valid(index_of_min,2),valid(index_of_min,3),valid(index_of_min,8));
else
index_of_min=-1;
end
最后
以上就是等待红牛为你收集整理的A*算法原理及matlab代码的全部内容，希望文章能够帮你解决A*算法原理及matlab代码所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复