内容列表
4.1 Astat路径规划算法原理
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法。注意其是最有效的直接搜索算法，之后涌现了很多预处理算法（如ALT，CH，HL等等），在线查询效率是A*算法的数千甚至上万倍。
- 公式表示为： f(n)=g(n)+h(n),
- 其中， f(n) 是从初始状态经由状态n到目标状态的最小代价估计，
- g(n) 是在状态空间中从初始状态到状态n的最小代价，
- h(n) 是从状态n到目标状态的路径的最小估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离）
参考文档
Astat算法-百度百科
Astat中文博客
Astat最火英文博客
4.2 Astat路径规划例子示例
4.3 Astat路径规划算法MATLAB代码
4.3.1 MATLAB代码示例
地图信息和示例是一致的，最后得到的最短路径，和搜寻的地图区域如图所示：
4.3.2 主代码：Astat.m
%% 定义基础数据，包括地图行列的长度，起点位置等clc;clear;close allrows = 6;cols = 7; % 地图的尺寸dy_SearchArea =[]; % 动态绘制搜索区域searchHead = 2;searchEnd = searchHead; % 动态绘制搜索区域的速度startSub = [3,2]; % 起点行列位置goalSub = [3,6]; % 终点行列位置obsSub = [2,4;3,4;4,4]; % 障碍物行列位置(n*2)
%% 定义栅格地图，并初始化，此部分以后内容，均不用修改-------------------------------------------------% 初始化地图数值为1，代表全空白区域field = ones(rows, cols);
% 起点、终点、障碍物区域的权值% Tips:对于单个点，Filed(x,y) = 2是可以的，但是多个点需要转成索引数组才能field(startSub(1),startSub(2)) = 4;field(goalSub(1),goalSub(2)) = 5;
% 后续路径规划算法，对于点的位置，均用索引值进行表示（输入变量只需要一个就行）obsR = obsSub(:,1);obsC = obsSub(:,2);obsIndex = sub2ind([rows,cols],obsR,obsC);startIndex = sub2ind([rows,cols],startSub(1),startSub(2));goalIndex = sub2ind([rows,cols],goalSub(1),goalSub(2));field(obsIndex) = 2;
%% ******建立openList(n*4)和closeList(n*2)并初始化************************************% 初始时，openList只有起点，closeList为空% openList(n*4),分别记录记录点的位置信息，G、H、F值openList = [startIndex,0,0,0];
% closeList(n*2)记录位置信息和距离权值F值，初始化时候为空closeList = [];
%% *************初始化path，即从起点到地图任意点的路径矩阵(n*2)***************************for i = 1:rows*colspath{i,1} = i; % 存放地图任意点的索引path{i,2} = []; % 存放起点到该点的路径end% 对于起点，其路径是已知的，写入起点路径path{startIndex,2} = startIndex;
%% 绘制地图--------------------------------------------------------------------------% 定义函数，列数，以及障碍物坐标cmap = [1 1 1; ... % 1-白色-空地0 0 0; ... % 2-黑色-静态障碍1 0 0; ... % 3-红色-动态障碍1 1 0;... % 4-黄色-起始点1 0 1;... % 5-品红-目标点0 1 0; ... % 6-绿色-到目标点的规划路径0 1 1]; % 7-青色-动态规划的路径colormap(cmap);image(1.5,1.5,field);
% 设置栅格属性------------------------------------------------------------------------------grid on;hold on;set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);set(gca,'xtick',1:cols+1,'ytick',1:rows+1);set(gca, 'XAxisLocation','top')axis image;
%% ******A*算法*************************************************************************while true% 1、从openList开始搜索移动代价最小的节点，min函数返回值为[值，位置][~,idxNode] = min(openList(:,4));node = openList(idxNode,1);
% 2、判断是否搜索到终点if node == goalIndexbreakend% ******3、在openList选中最小的F值点作为父节点nextNodes = Astat_NextNode(field,closeList,node);
% *******4、判断父节点周围子节点情况，并将子节点依次添加或者更新到openList中for i = 1:length(nextNodes)% 需要判断的子节点nextNode = nextNodes(i);% 计算代价函数[rowNode,colNode] = ind2sub([rows, cols], node);[row_nextNode,col_nextNode] = ind2sub([rows, cols], nextNode);[row_goalPos,col_goalPos] = ind2sub([rows, cols],goalIndex);g = openList(idxNode,2) + norm( [rowNode,colNode] -[row_nextNode,col_nextNode]);h = abs(row_goalPos - row_nextNode) + abs(col_goalPos - col_nextNode);f = g + h;% 判断该子节点是否存在在openList中[inOpen,idx_nextNode] = ismember(nextNode, openList(:,1));% ******如果存在，则需要比较F值，取F值小的更新F和G、H同时更新路径if inOpen && f < openList(idx_nextNode,4)openList(idx_nextNode,2) = g;openList(idx_nextNode,3) = h;openList(idx_nextNode,4) = f;path{nextNode,2} = [path{node,2}, nextNode];end% *******如果不存在，则添加到openList表中if ~inOpenopenList(end+1,:) = [nextNode,g,h,f];path{nextNode,2} = [path{node,2}, nextNode];endend% 将父节点从openList中移除，添加到closeList中closeList(end+1,: ) = [openList(idxNode,1), openList(idxNode,4)];openList(idxNode,:)= [];% =======绘制动态搜索区域dy_SearchArea = [openList(:,1)',closeList(:,1)']; % 添加节点到动态区域中field(dy_SearchArea) = 7; % 动态区域的颜色field(startIndex) = 4; % 起点颜色保持不变field(goalIndex) = 5; % 终点颜色保持不变if mod(searchHead,searchEnd) == 0 % 控制动图绘制速度image(1.5,1.5,field); % 联合drawnow函数绘制动态效果drawnow;endsearchHead = searchHead + 1;end
%% 绘制最优路径的折线图*******************************************************************% 由于绘图函数plot针对的窗口是x,y，它们与row，col是的先后顺序是相反的,即X对应col，y对应rowimage(1.5,1.5,field); % 将动画速度未显示完的搜索区域全部显示field(dy_SearchArea) = 1; % 最后将搜索区域的值改回为空白值optPath = path{goalIndex,2}; % 最优路径[plotr,plotc] = ind2sub([rows,cols],optPath);plot(plotc+0.5,plotr+0.5,'LineWidth',2.5);
% % function nextNodes = Astat_NextNode(field,closeList,node)% % % ASTAT_NEXTNODE 对父节点周围的8个节点进行判断% % % 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类% %% % [rows, cols] = size(field); % 获取地图尺寸% % [r,c] = ind2sub([rows, cols], node); % 得到父节点行列值，方便进行上下左右移动% % movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1]; % 移动方向矩阵% % nextNodes = []; % 存放子节点线性索引位置的的矩阵(1*n)% %% % % closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中% % closenode = []; % 初始化时候closeList为空，不能执行(:,1)操作，% % % 故函数中的closenode作为临时变量初始为空% % if ~isempty(closeList) % if的目的就是为了保证初始的closeList=[]也有效% % closenode = closeList(:,1);% % end% %% % for i = 1:8% % if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= cols% % nextSub = [r + movePos(i,1), c + movePos(i,2)];% % nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));% % if field(nextSub(1), nextSub(2)) ~= 2% % if ~ismember(nextIndex, closenode)% % nextNodes(end+1) = nextIndex;% % end% % end% % end% % end
4.3.3 函数代码：Astat_NextNode.m
function nextNodes = Astat_NextNode(field,closeList,node)% ASTAT_NEXTNODE 对父节点周围的8个节点进行判断% 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类
[rows, cols] = size(field); % 获取地图尺寸[r,c] = ind2sub([rows, cols], node); % 得到父节点行列值，方便进行上下左右移动movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1]; % 移动方向矩阵nextNodes = []; % 存放子节点线性索引位置的的矩阵(1*n)
% closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中closenode = []; % 初始化时候closeList为空，不能执行(:,1)操作，% 故函数中的closenode作为临时变量初始为空if ~isempty(closeList) % if的目的就是为了保证初始的closeList=[]也有效closenode = closeList(:,1);end
for i = 1:8if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= colsnextSub = [r + movePos(i,1), c + movePos(i,2)];nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));if field(nextSub(1), nextSub(2)) ~= 2if ~ismember(nextIndex, closenode)nextNodes(end+1) = nextIndex;endendendend
4.4 Astat路径规划算法Python代码
下面代码逻辑和MATLAB版本是一致的
本次A*在Python中采用存取行列位置的来依次搜索，和之前Dijkstra和Floyd采用线性索引不同
由于数组索引的一致性，在openList和closeList等均采用索引来修改对应值的方式
注意数组的深拷贝和浅拷贝
可以参考：PythonRobotics：（https://github.com/redglassli/PythonRobotics#a-algorithm）
4.4.1 Python实现示例
4.4.2 辅助函数PathPlanning.py
参考博客3 移动机器人路径规划（1- 栅格地图绘制）中的1.4.2复制命名即可。
4.4.3 Astar路径规划Python代码
import numpy as npimport matplotlib.pyplot as pltimport copy
import PathPlanning
'''Astat_NextNode(field, closeList_Sub, nodeSub)：输入地图和closeList_Sub集合和父节点行列位置# 返回子节点的行列位置矩阵nextNodes_Sub'''def Astat_NextNode(field, closeList_Sub, nodeSub):# 获取地图尺寸rows = len(field)cols = len(field[0])# 移动方向矩阵movePos = [[-1, 1], [0, 1], [1, 1], [-1, 0],[1, 0], [-1, -1], [0, -1], [1, -1]]# 存放子节点行列位置的矩阵nextNodes_Sub = []
for i in range(8):r = nodeSub[0] + movePos[i][0]c = nodeSub[1] + movePos[i][1]
# 在地图内if -1 < r and r < rows and -1 < c and c < cols:# 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵if field[r, c] != 2:if [r, c] not in closeList_Sub:nextNodes_Sub.append([r, c])
return nextNodes_Sub
'''# 测试地图信息---------------------------------------------------------------'''
rows = 5cols = 6startSub = [2,1]goalSub = [2,5]obsSub = [[1,3],[2,3],[3,3]]
# 栅格地图属性field = np.ones((rows, cols))
field[startSub[0], startSub[1]] = 4field[goalSub[0], goalSub[1]] = 5
for i in range(len(obsSub)):field[obsSub[i][0], obsSub[i][1]] = 2
'''# 最关键一点：=========利用数组下标长度一致，顺序一致的关系，创建表分别存放值和点=====# openList有四个存储内容,分别记录记录点的行列位置信息，G、H、F值# 初始时，openList只有起点，closeList为空'''openList_Sub = [startSub]openList_G = [0]openList_H = [0]openList_F = [0]
# closeList记录位置信息和距离权值F值，初始化时候为空closeList_Sub = []closeList_F = []
# 初始化path，即从起点到地图任意点的路径矩阵pathSub = []path = []
# 初始化所有存放路径的矩阵for c in range(cols+1):for r in range(rows+1):pathSub.append([r,c])# 添加的元素是数组append([])path.append([[-1,-1]])
# 对于起点，其路径是已知的，写入起点路径idx = pathSub.index(startSub)path[idx] = [startSub]
'''# ============================开始执行A*算法迭代========================'''while True:# 1、从openList开始搜索移动代价最小的节点，min函数返回值为[值，位置]idx_nodeSub = openList_F.index(min(openList_F))nodeSub = openList_Sub[idx_nodeSub]
# 2、判断是否搜索到终点if nodeSub[0] == goalSub[0] and nodeSub[1] == goalSub[1]:break# ******3、在openList选中最小的F值点作为父节点nextNodes_Sub = Astat_NextNode(field, closeList_Sub, [nodeSub[0], nodeSub[1]])
# *******4、判断父节点周围子节点情况，并将子节点依次添加或者更新到openList中for i in range(len(nextNodes_Sub)):
# 需要判断的子节点nextSub = nextNodes_Sub[i]
# 计算代价函数g = openList_G[idx_nodeSub] + np.sqrt((nodeSub[0]-nextSub[0])**2+(nodeSub[1]-nextSub[1])**2)h = np.abs(goalSub[0]-nextSub[0]) + np.abs(goalSub[1]-nextSub[1])f = g + h
# 判断该子节点是否存在在openList中if nextSub in openList_Sub:# ******如果存在，则需要比较F值，取F值小的更新F和G、H同时更新路径idx_nextSub = openList_Sub.index(nextSub)if f < openList_F[idx_nextSub]:openList_G[idx_nextSub] = gopenList_H[idx_nextSub] = hopenList_F[idx_nextSub] = f
idx_NextPath = pathSub.index(nextSub)idx_NodePath = pathSub.index(nodeSub)path[idx_NextPath] = copy.deepcopy(path[idx_NodePath])path[idx_NextPath].append(nextSub)
# *******如果不存在，则添加到openList表中else:openList_Sub.append(nextSub)openList_G.append(g)openList_H.append(h)openList_F.append(f)
idx_NextPath = pathSub.index(nextSub)idx_NodePath = pathSub.index(nodeSub)path[idx_NextPath] = copy.deepcopy(path[idx_NodePath])path[idx_NextPath].append(nextSub)# 将父节点从openList中移除，添加到closeList中closeList_Sub.append(nodeSub)closeList_F.append(f)openList_Sub.pop(idx_nodeSub)openList_G.pop(idx_nodeSub)openList_H.pop(idx_nodeSub)openList_F.pop(idx_nodeSub)
idx = pathSub.index(goalSub)optpath = path[idx]
for i in range(len(optpath)):field[optpath[i][0],optpath[i][1]] = 6
field[startSub[0], startSub[1]] = 4field[goalSub[0], goalSub[1]] = 5
PathPlanning.DrawHeatMap(field)plt.show()
4.5 对角线路径过滤
4.5.1 优化对角线的例子
4.5.2 优化方法
主要是在对A*算法父节点周围子节点进行过滤，所以只需要修改nextnode函数即可
4.5.3 MATLAB子节点过滤函数Astat_NextNode.m
将其替换上面的Astat_NextNode.m代码运行即可
function nextNodes = Astat_NextNode(field,closeList,node)% ASTAT_NEXTNODE 对父节点周围的8个节点进行判断% 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类
[rows, cols] = size(field); % 获取地图尺寸[r,c] = ind2sub([rows, cols], node); % 得到父节点行列值，方便进行上下左右移动movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1]; % 移动方向矩阵nextNodes = []; % 存放子节点线性索引位置的的矩阵(1*n)nextObs = [];
% closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中closenode = []; % 初始化时候closeList为空，不能执行(:,1)操作，% 故函数中的closenode作为临时变量初始为空if ~isempty(closeList) % if的目的就是为了保证初始的closeList=[]也有效closenode = closeList(:,1);end
for i = 1:8if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= colsnextSub = [r + movePos(i,1), c + movePos(i,2)];nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));% 如果为障碍物，且为父节点上下左右四个方向的障碍物，则记录到障碍物表中if field(nextSub(1), nextSub(2)) == 2if movePos(i,1)==0 || movePos(i,2)==0nextObs(end+1) = nextIndex;end% 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵elseif ~ismember(nextIndex, closenode)nextNodes(end+1) = nextIndex;endendend% 保证父节点上下左右障碍物不为空才能进行for循环if ~isempty(nextObs)for i = 1:length(nextObs)% 保证子节点集合不为空才能进行for循环if ~isempty(nextNodes)for j=1:length(nextNodes)[ro,co] = ind2sub([rows,cols],nextObs(i));[rn,cn] = ind2sub([rows,cols],nextNodes(j));% 判断距离，如果为1则删除d = norm([ro,co]-[rn,cn]);if d == 1nextNodes(j) = []; % 删除元素break;endendendendendend
4.5.3 Python子节点过滤函数Astat_NextNode
类似，将Python的def Astat_NextNode(field, closeList_Sub, nodeSub):替换下面代码即可
'''Astat_NextNode(field, closeList_Sub, nodeSub)：输入地图和closeList_Sub集合和父节点行列位置# 返回子节点的行列位置矩阵nextNodes_Sub'''def Astat_NextNode(field, closeList_Sub, nodeSub):# 获取地图尺寸rows = len(field)cols = len(field[0])# 移动方向矩阵movePos = [[-1, 1], [0, 1], [1, 1], [-1, 0],[1, 0], [-1, -1], [0, -1], [1, -1]]# 存放子节点行列位置的矩阵nextNodes_Sub = []nextObs_Sub = []
for i in range(8):r = nodeSub[0] + movePos[i][0]c = nodeSub[1] + movePos[i][1]
# 在地图内if -1 < r and r < rows and -1 < c and c < cols:# 如果为障碍物，且为父节点上下左右四个方向的障碍物，则记录到障碍物表中if field[r, c] == 2:if movePos[i][0] == 0 or movePos[i][1] == 0:nextObs_Sub.append([r,c])# 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵else:if [r, c] not in closeList_Sub:nextNodes_Sub.append([r, c])
# 如果子节点到父节点上下左右障碍物的距离为1，则不添加进子节点列表for i in range(len(nextObs_Sub)):for j in range(len(nextNodes_Sub)):if np.sqrt((nextNodes_Sub[j][0]-nextObs_Sub[i][0])**2 + (nextNodes_Sub[j][1]-nextObs_Sub[i][1])**2) == 1:nextNodes_Sub.pop(j)break
return nextNodes_Sub
评论（1）
您还未登录，请登录后发表或查看评论