目录
1. 最佳优先搜索（Best-First Search）
最佳优先搜索（BFS），又称A算法，是一种启发式搜索算法(Heuristic Algorithm)。[不是广度优先搜索算法（ Breadth First Search , BFS )]
BFS算法在广度优先搜索的基础上，用启发估价函数对将要被遍历到的点进行估价，然后选择代价小的进行遍历，直到找到目标节点或者遍历完所有点，算法结束。
要实现最佳优先搜索我们必须使用一个优先队列（priority queue）来实现，通常采用一个open优先队列和一个closed集，open优先队列用来储存还没有遍历将要遍历的节点，而closed集用来储存已经被遍历过的节点。
1.1 最佳优先搜索的过程
最佳优先搜索的过程可以被描述为：
- 将根节点放入优先队列open中。
- 从优先队列中取出优先级最高的节点X。
- 根据节点X生成子节点Y:
3.1 X的子节点Y不在open队列或者closed中，由估价函数计算出估价值，放入open队列中。
3.2 X的子节点Y在open队列中，且估价值优于open队列中的子节点Y，将open队列中的子节点Y的估价值替换成新的估价值并按优先值排序。
3.3 X的子节点Y在closed集中，且估价值优于closed集中的子节点Y，将closed集中的子节点Y移除，并将子节点Y加入open优先队列。 - 将节点X放入closed集中。
- 重复过程2,3,4直到目标节点找到，或者open为空，程序结束。
BFS不能保证找到一条最短路径。然而，它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic function）快速地导向目标结点。
这篇博客对BFS进行了详细的介绍用的是罗马尼亚度假问题
2. A-Star算法
1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
- 和Dijkstra一样，A*能用于搜索最短路径。
- 和BFS一样，A*能用启发式函数引导它自己。
左图为Astar算法效果图，右图为Dijkstra算法效果图
Astar算法与Dijkstra算法的效果差不多，但Astar算法访问的节点数明显比Dijkstra算法少得多，说明其速度更快，运行时间更短。
2.1 Astar算法所属分类
A*算法在最短路径搜索算法中分类为：
- 直接搜索算法：直接在实际地图上进行搜索，不经过任何预处理；
- 启发式算法：通过启发函数引导算法的搜索方向；
- 静态图搜索算法：被搜索的图的权值不随时间变化（后被证明同样可以适用于动态图的搜索 ）
2.2 Astar算法基本概念
A*算法启发函数表示为： f(n)=g(n)+h(n)
- f(n) 是从初始状态经由状态n到目标状态的代价估计
- g(n) 是在状态空间中从初始状态到状态n的实际代价
- h(n) 是从状态n到目标状态的最佳路径的估计代价
（对于路径搜索问题，状态就是图中的节点，代价就是距离）
Astar算法是启发式搜索算法，启发式搜索是在状态空间中对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。
这样可以省略大量无谓的搜索路径，提高效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。
启发函数（Heuristics Function）（估价函数）： H为启发函数，也被认为是一种试探。
由于在找到唯一路径前，不确定在前面会出现什么障碍物，计算H的算法（例如，H可采用传统的曼哈顿距离）具体根据实际场景决定。
父节点（parent）： 在路径规划中用于回溯的节点。
搜索区域（The Search Area）： 前面图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。
开放列表(Open List)： 将路径规划过程中待检测的节点存放于Open List中。
关闭列表(Close List)： 将路径规划过程中已经检查过的节点放在Close List。
2.3 启发函数单调性的推导
2.6 确定最终路径
已经确定了目标节点的坐标为，根据此目标节点的位置，和先前设置的方向存储函数之中的方向，从目标节点利用方向反推至起始节点。具体实现的示意图如下
2.7 路径平滑
我们需要对规划出的路径进行平滑处理，将路径的转折处转化为平滑的曲线，使之更加符合无人车的实际运动路径。
主要有基于B样条曲线的路径优化方法，基于Dubins圆的路径优化方法，和基于Clothoid曲线的路径优化方法，基于贝塞尔曲线的路径平滑算法。
红色为未平滑路径，绿色方块为最终路径，黄色为贝塞尔曲线拟合得到，蓝色为spcrv函数平滑得到。
2.8 Astar算法的优缺点
优点：
相对于需要将所有节点展开搜寻的算法，A*算法最大的优点就是引入了启发信息作为向目标点移动的决策辅助，所以不再需要遍历整个地图，降低了计算复杂度，减少了时间损耗少。
缺点：
基于栅格法来分割地图，精度越高，栅格分的就越小，就会使工作量几何式的增长。
估价函数选取的不合理也有可能导致无法规划出最优路径。
- 如果 h(n) <= n到终点的实际距离，A*算法可以找到最短路径，但是搜索的点数多，搜索范围大，效率低。
- 如果 h(n) > n到终点的实际距离，搜索的点数少，搜索范围小，效率高，但是得到的路径并不一定是最短的。
- h(n) 越接近n到终点的实际距离，那么A*算法越完美。（个人理解是如果用曼哈顿距离，那么只需要找到一条长度小于等于该距离的路径就算完成任务了。而使用对角线距离就要找到一条长度大于等于对角线距离且最短的路径才行。）
- 若 h(n)=0，即 f(n)=g(n)，A*算法就变为了Dijkstra算法（Dijstra算法会毫无方向的向四周搜索）。
- 若 h(n) 远远大于 g(n) ，那么 f(n) 的值就主要取决于 h(n)，A*算法就演变成了BFS算法
距离估计与实际值越接近，估价函数取得就越好。估价函数f(n)在g(n)一定的情况下，会或多或少的受距离估计值h(n)的制约，节点距目标点近，h值小，f值相对就小，能保证最短路的搜索向终点的方向进行，明显优于Dijkstra算法的毫无方向的向四周搜索。
2.9 Astar算法流程
如下图所示，绿色是起点A，红色是终点B，一系列蓝色是障碍物，从A移动到B,绕过障碍。
- 用方格（三角形、五角形）划分空间，简化搜索区域。空间被划分为二维数组，数组中每个元素代表空间中的一个方格，可被标记为可行或不可行。未来的路径就是一系列可行方块的集合。Nodes的概念涵盖了一系列可行方块（或其它方块）
- 将起点A放到Openlist中，Openlist存放着一系列需要检查的节点（方块）
- 给Openlist中每个节点赋值F=G+H （起点为0，横向和纵向的移动代价为 10 ，对角线的移动代价为 14）
- 检查Openlist，选取F值最小的节点（此处为A点）。
- 将A点从Openlist中删除，放入Closelist中（Closelist中存放不可寻点，即已被访问过的点），把A点临近节点放入Openlist中，并将A点设为临近节点的父节点。
- 给Openlist中每个节点赋值，选取F值最小的节点设为当前节点Node，(若当前节点Node为终点，则寻路结束，若Openlist中没有可寻节点，则寻路失败）
- 检查当前节点Node周围临近节点。忽略Closelist中的节点和不可行节点（障碍），如果临近节点在Openlist中，则对比一下是否从当前节点到临近节点的G值比原G值（直接到临近节点的实际代价值）小，若是，把当前节点作为父节点。否，不做改动。如果临近节点不在Openlist中，将其加入到Openlist中，将当前节点设为它的父节点。
- 若上步骤中新节点未造成任何改动，将新节点作为当前节点，重复6,7）中的步骤，直到找到目标节点。
寻找到目标节点
- 从目标节点回溯可以找到初始点，从而确定路径
上述描述路径的图片有些错误，具体步骤如下图所示。
A*算法寻路过程步骤总结：
- 将起点A添加到open列表中
- 检查open列表，选取花费F最小的节点M（检查M如果为终点是则结束寻路，如果open列表没有则寻路失败，直接结束）。
- 对于与M相邻的每一节点N
如果N是阻挡障碍，那么不管它。
如果N在closed列表中，那么不管它
如果N不在open列表中：添加它然后计算出它的花费F(n)=G+H。
如果N已在open列表中：当我们使用当前生成的路径时，检查F花费是否更小。如果是，更新它的花费F和它的父节点。- 重复2，3步。
- 停止，当你把终点加入到了 openlist 中，此时路径已经找到了或者 查找终点失败，并且 openlist 是空的，此时没有路径。
- 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。
3. 其他Astar算法
3.1 Astar——三维地图规划
3.1.1 3D-Astar原理
4. MATLAB实现Astar算法
4.1 defColorMap.m
用于初始化地图参数
function [field,cmap] = defColorMap(rows, cols)
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-青色-动态规划的路径
% 构建颜色MAP图
colormap(cmap);
% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);
% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;
4.2 getChildNode.m
用于获取子节点信息
function childNodes = getChildNode(field,closeList, parentNode)
% 选取父节点周边8个节点作为备选子节点，线性化坐标
% 排除超过边界之外的、位于障碍区的、位于closeList中的
[rows, cols] = size(field);
[row_parentNode, col_parentNode] = ind2sub([rows, cols], parentNode);
childNodes = [];
closeList = closeList(:,1);
% 第1个子节点(右节点)
childNode = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
% 第2个子节点（右上节点）
childNode = [row_parentNode-1, col_parentNode+1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
end
% 第3个子节点（上节点）
childNode = [row_parentNode-1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
% 第4个子节点（左上）
childNode = [row_parentNode-1, col_parentNode-1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
end
% 第5个子节点(左节点)
childNode = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
% 第6个子节点（左下）
childNode = [row_parentNode+1, col_parentNode-1];
child_brother_node_sub1 = [row_parentNode, col_parentNode-1];
child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
end
% 第7个子节点（下）
childNode = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
% 第8个子节点（右下）
childNode = [row_parentNode+1, col_parentNode+1];
child_brother_node_sub1 = [row_parentNode, col_parentNode+1];
child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
childNode(2) < 1 || childNode(2) > cols)
if field(childNode(1), childNode(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
if ~ismember(childNode_LineIdx, closeList)
childNodes(end+1) = childNode_LineIdx;
end
end
end
end4.3 Astar.m
主程序
% 基于栅格地图的机器人路径规划算法
% A*算法
clc
clear
close all
%% 栅格界面、场景定义
% 行数和列数
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);
% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;
%% 预处理
% 初始化closeList
parentNode = startPos;
closeList = [startPos,0];
% 初始化openList
openList = struct;
childNodes = getChildNode(field,closeList,parentNode);
for i = 1:length(childNodes)
[row_startPos,col_startPos] = ind2sub([rows, cols],startPos);
[row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);
[row,col] = ind2sub([rows, cols],childNodes(i));
% 存入openList结构体
openList(i).node = childNodes(i);
openList(i).g = norm([row_startPos,col_startPos] - [row,col]); % 实际代价采用欧式距离
openList(i).h = abs(row_goalPos - row) + abs(col_goalPos - col); % 估计代价采用曼哈顿距离
openList(i).f = openList(i).g + openList(i).h;
end
% 初始化path
for i = 1:rows*cols
path{i,1} = i; % 线性索引值
end
for i = 1:length(openList)
node = openList(i).node;
path{node,2} = [startPos,node];
end
%% 开始搜索
% 从openList开始搜索移动代价最小的节点
[~, idx_min] = min([openList.f]);
parentNode = openList(idx_min).node;
% 进入循环
while true
% 找出父节点的8个子节点，障碍物节点用inf，
childNodes = getChildNode(field, closeList,parentNode);
% 判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
for i = 1:length(childNodes)
% 需要判断的子节点
childNode = childNodes(i);
[in_flag,idx] = ismember(childNode, [openList.node]);
% 计算代价函数
[row_parentNode,col_parentNode] = ind2sub([rows, cols], parentNode);
[row_childNode,col_childNode] = ind2sub([rows, cols], childNode);
[row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);
g = openList(idx_min).g + norm( [row_parentNode,col_parentNode] -...
[row_childNode,col_childNode]);
h = abs(row_goalPos - row_childNode) + abs(col_goalPos - col_childNode); % 采用曼哈顿距离进行代价估计
f = g + h;
if in_flag % 若在，比较更新g和f
if f < openList(idx).f
openList(idx).g = g;
openList(idx).h = h;
openList(idx).f = f;
path{childNode,2} = [path{parentNode,2}, childNode];
end
else % 若不在，追加到openList
openList(end+1).node = childNode;
openList(end).g = g;
openList(end).h = h;
openList(end).f = f;
path{childNode,2} = [path{parentNode,2}, childNode];
end
end
% 从openList移出移动代价最小的节点到closeList
closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
openList(idx_min)= [];
% 重新搜索：从openList搜索移动代价最小的节点
[~, idx_min] = min([openList.f]);
parentNode = openList(idx_min).node;
% 判断是否搜索到终点
if parentNode == goalPos
closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
break
end
end
%% 画路径
% 找出目标最优路径
path_target = path{goalPos,2};
for i = 1:rows*cols
if ~isempty(path{i,2})
field((path{i,1})) = 7;
end
end
field(startPos) = 4;
field(goalPos) = 5;
field(path_target(2:end-1)) = 6;
% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
[y0,x0] = ind2sub([rows,cols], path_target);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
points = [x',y'];
M = 10;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:), 'b','LineWidth',2.5);可以先参考古月居的这篇文章
4.4 算法效果
运行总时间
栅格地图大小（20x20）
栅格地图大小（30x30）
栅格地图大小（40x40）
可以看到Astar算法对于栅格地图越大的情况，搜索时间越长，其速度相比Dijkstra有优势（尤其是在地图比较大的时候，优势更明显）。但其总耗时较长，不适用于实时的路径规划，不适用于局部路径规划，适用于全局路径规划。
5. python实现Astar算法
python 版本的伪代码（来源：https://brilliant.org/wiki/a-star-search/）如下：
make an openlist containing only the starting node
make an empty closed list
while (the destination node has not been reached):
consider the node with the lowest f score in the open list
if (this node is our destination node) :
we are finished
if not:
put the current node in the closed list and look at all of its neighbors
for (each neighbor of the current node):
if (neighbor has lower g value than current and is in the closed list) :
replace the neighbor with the new, lower, g value
current node is now the neighbor's parent
else if (current g value is lower and this neighbor is in the open list ) :
replace the neighbor with the new, lower, g value
change the neighbor's parent to our current node
else if this neighbor is not in both lists:
add it to the open list and set its g6. Java实现Astar算法
可以参考这篇文章
7. 实践案例——基于ROS实现Astar与DWA算法
本项目以Astar算法作为全局路径规划算法，DWA作为局部路径规划算法，实现效果如下。（具体原理与算法代码解释与说明会在之后的文章附上）
ROS_导航_Astar+DWA
声明
本人所有文章仅作为自己的学习记录，若有侵权，联系立删。
评论（1）
您还未登录，请登录后发表或查看评论