最佳优先搜索（BFS），又称A算法，是一种启发式搜索算法(Heuristic Algorithm)。[不是广度优先搜索算法（ Breadth First Search , BFS )]
BFS算法在广度优先搜索的基础上，用启发估价函数对将要被遍历到的点进行估价，然后选择代价小的进行遍历，直到找到目标节点或者遍历完所有点，算法结束。
要实现最佳优先搜索我们必须使用一个优先队列（priority queue）来实现，通常采用一个open优先队列和一个closed集，open优先队列用来储存还没有遍历将要遍历的节点，而closed集用来储存已经被遍历过的节点。
最佳优先搜索的过程可以被描述为：
BFS不能保证找到一条最短路径。然而，它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic function）快速地导向目标结点。
这篇博客对BFS进行了详细的介绍用的是罗马尼亚度假问题
这篇则是用C++实现了BFS
1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
左图为Astar算法效果图，右图为Dijkstra算法效果图
Astar算法与Dijkstra算法的效果差不多，但Astar算法访问的节点数明显比Dijkstra算法少得多，说明其速度更快，运行时间更短。
A*算法在最短路径搜索算法中分类为：
A*算法启发函数表示为： f(n)=g(n)+h(n)
（对于路径搜索问题，状态就是图中的节点，代价就是距离）
Astar算法是启发式搜索算法，启发式搜索是在状态空间中对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。
这样可以省略大量无谓的搜索路径，提高效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。
启发函数（Heuristics Function）（估价函数）： H为启发函数，也被认为是一种试探。
由于在找到唯一路径前，不确定在前面会出现什么障碍物，计算H的算法（例如，H可采用传统的曼哈顿距离）具体根据实际场景决定。
父节点（parent）： 在路径规划中用于回溯的节点。
搜索区域（The Search Area）： 前面图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。
开放列表(Open List)： 将路径规划过程中待检测的节点存放于Open List中。
关闭列表(Close List)： 将路径规划过程中已经检查过的节点放在Close List。
单调性：单调性是Astar算法非常重要的一个性质，它决定了在用Astar算法进行路径搜索时，一定能找到一条最优路径。
设父节点的坐标为(x0,y0)，它的任意一个子节点的坐标为(xi,yi)，所以对两者之间的h(x)，就一定满足
h
(
x
0
)
≤
h
(
x
i
)
+
C
o
s
t
0
,
i
h({x_{\rm{0}}}) \le h({x_i}) + {\mathop{\rm Cos}\nolimits} {t_{0,i}}
h(x0)≤h(xi)+Cost0,iCost0,i——从父节点到下一个子节点的代价值,恒大于等于0，即要满足代价函数是单调递增的；
父节点到子节点的估价值为h(x0)-h(xi)，它总是小于或等于其代价值.
根据启发函数公式可知，对父节点和子节点的f(x)，总有 f ( x 0 ) = g ( x 0 ) + h ( x 0 ) f({x_{\rm{0}}}) = g({x_{\rm{0}}}) + h({x_{\rm{0}}}) f(x0)=g(x0)+h(x0) f ( x i ) = g ( x i ) + h ( x i ) f({x_i}) = g({x_i}) + h({x_i}) f(xi)=g(xi)+h(xi) 在Astar算法的搜索过程当中，实际代价g(x)是在不断增加的,可以由下式推出： g ( x i ) = g ( x 0 ) + C o s t 0 , i g({x_i}) = g({x_0}) + {\mathop{\rm Cos}\nolimits} {t_{0,i}} g(xi)=g(x0)+Cost0,i 子节点的代价值等于父节点的代价值加上从父节点到下一个子节点的代价值。代入第二条公式，得到下式： f ( x i ) = g ( x 0 ) + h ( x i ) + C o s t 0 , i f({x_i}) = g({x_0}) + h({x_i}) + {\mathop{\rm Cos}\nolimits} {t_{0,i}} f(xi)=g(x0)+h(xi)+Cost0,i 从而有 g ( x 0 ) + h ( x 0 ) ≤ g ( x 0 ) + h ( x i ) + C o s t 0 , i g({x_{\rm{0}}}) + h({x_{\rm{0}}}) \le g({x_{\rm{0}}}) + h({x_i}) + {\mathop{\rm Cos}\nolimits} {t_{0,i}} g(x0)+h(x0)≤g(x0)+h(xi)+Cost0,i 即 f ( x 0 ) ≤ f ( x i ) f({x_{\rm{0}}}) \le f({x_i}) f(x0)≤f(xi) 在Astar算法的运行过程中，后继的f(x)值时大于当前f(x)的值，即f(x)在之后对子节点的搜索扩展是单调递增的，不会像人工势场法一样出现局部极小值，因此使用Astar算法规划出的路径一定是最优路径.
曼哈顿距离函数在标准坐标系中，表示起始和目标两节点的绝对值总和，其估计值就是从当前点做水平和垂直运动，到达目标点的成本的估计，因此，曼哈顿距离函数中，两点的距离如下
h
(
x
)
=
K
∗
(
∣
x
1
−
x
2
∣
+
∣
y
1
−
y
2
∣
)
h(x) = K*(\left| {{x_1} - {x_2}} \right| + \left| {{y_1} - {y_2}} \right|)
h(x)=K∗(∣x1−x2∣+∣y1−y2∣)K——相邻两节点之间的距离，即单位距离；
(x1,y1)——起始节点的坐标;
(x2,y2)——目标节点的坐标;
欧几里得距离又称为欧式距离，它是衡量两点之间距离远近的最常用的方法之一。欧几里得距离函数的值可以直接看作起始节点和目标节点，在欧式空间中的直线距离，其表达式如下所示
h
(
n
)
=
K
×
(
x
i
−
x
k
)
2
+
(
y
i
−
y
k
)
2
h(n) = K \times \sqrt {{{({x_i} - {x_k})}^2} + {{({y_i} - {y_k})}^2}}
h(n)=K×(xi−xk)2+(yi−yk)2
K——相邻两节点之间的距离，即单位距离；
(xi,yi)——当前节点的坐标;
(xk,yk)——目标节点的坐标;
欧几里德距离函数作为启发代价的计算方法时，虽然寻径时搜索空间增加从而导致搜索效率的降低，但其所得的估计值最小；
而在使用曼哈顿距离函数时，虽然寻径需要遍历的栅格空间少于欧几里得距离函数，搜索效率较高，但这种方法得到的估计值与欧几里得距离函数相比，距离真实值更远。
已经确定了目标节点的坐标为，根据此目标节点的位置，和先前设置的方向存储函数之中的方向，从目标节点利用方向反推至起始节点。具体实现的示意图如下
我们需要对规划出的路径进行平滑处理，将路径的转折处转化为平滑的曲线，使之更加符合无人车的实际运动路径。
主要有基于B样条曲线的路径优化方法，基于Dubins圆的路径优化方法，和基于Clothoid曲线的路径优化方法，基于贝塞尔曲线的路径平滑算法。
红色为未平滑路径，绿色方块为最终路径，黄色为贝塞尔曲线拟合得到，蓝色为spcrv函数平滑得到。
优点：
相对于需要将所有节点展开搜寻的算法，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算法的毫无方向的向四周搜索。
具体流程可以参考这篇
以及古月居的这篇
如下图所示，绿色是起点A，红色是终点B，一系列蓝色是障碍物，从A移动到B,绕过障碍。
寻找到目标节点
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 是空的，此时没有路径。
- 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。
三维栅格地图，顾名思义不是简单的二维平面，它必须得有三维方向，也就是高度方向上的拓展。栅格地图在XY水平面上的栅格的投影颜色不尽相同，栅格黄色程度越高，表明此处的高度越高。
为了使启发代价的值应该尽量接近于真实值，三维栅格地图中仍然选用欧几里得距离作为估价函数计算启发代价的计算方法。
但本次实验所处的环境为三维避障空间，因此相较于二维空间的路径规划，我们将公式做三维化推广，具体形式如下：
h
(
n
)
=
K
⋅
(
x
0
−
x
k
)
2
+
(
y
0
−
y
k
)
2
+
(
z
0
−
z
k
)
2
h(n) = K \cdot \sqrt {{{({x_0} - {x_k})}^2} + {{({y_0} - {y_k})}^2} + {{({z_0} - {z_k})}^2}}
h(n)=K⋅(x0−xk)2+(y0−yk)2+(z0−zk)2
K——相邻两节点之间的距离，即单位距离；
(x0,y0,z0)——起始节点的坐标;
(xk,yk,zk)——目标节点的坐标;
若要追求最短时间，则可以引入新的代价函数:
f
(
n
)
=
g
(
n
−
1
)
+
D
(
n
−
1
,
n
)
+
h
(
n
)
f(n) = g(n - 1) + D(n - 1,n) + h(n)
f(n)=g(n−1)+D(n−1,n)+h(n)h(n)——当前节点到目标节点的启发代价值；
g(n-1)——当前节点到它的父节点之间的路径代价；D(n-1, n)——当前节点与它的子节点之间的代价值。
g(n-1)与二维规划中的距离代价计算方法一致，但D(n-1, n)在计算时用父节点与子节点之间的距离值除以三维避障环境中设定的栅格可通行程度，h(n)也是用当前节点到目标节点的启发代价值除以地图环境中预先设定的栅格可通行程度。
这里是代码的GitHub地址
实现效果
经典Astar算法路径规划是取两节点间曼哈顿距离作为距离估计为最优原则搜索路径，从而得到最短路径。搜索路径的过程中并没有考虑实际道路坡度和道路滚动阻力系数对行驶车辆最短路径搜索的影响，也没有考虑在道路上行驶的能量损耗问题，即经典Astar算法搜索的最短路径并非符合实际车辆行驶的最优路径。
最短路径启发函数:
L
f
(
n
)
=
L
g
(
n
)
+
L
h
(
n
)
{L_{\rm{f}}}\left( n \right) = {L_{\rm{g}}}\left( n \right) + {L_{\rm{h}}}\left( n \right)
Lf(n)=Lg(n)+Lh(n)n——当前节点栅格；
Lg(n)——起点栅格与当前节点栅格之间的间隔距离代价（m）；
Lh(n)——当前节点栅格与目标点栅格之间的间隔距离代价（m）。
L
h
(
n
)
=
∥
n
,
t
a
r
g
e
t
∥
{L_{\rm{h}}}\left( n \right) = \left\| {n,target} \right\|
Lh(n)=∥n,target∥当前节点与目标点之间的欧几里得距离（m）.
L
g
(
n
)
=
L
g
(
n
−
1
)
+
c
o
s
t
(
n
−
1
,
n
)
L
{L_{\rm{g}}}\left( n \right) = {L_{\rm{g}}}\left( {n - 1} \right) + cost{\left( {n - 1,n} \right)_{\rm{L}}}
Lg(n)=Lg(n−1)+cost(n−1,n)L上一节点栅格到当前节点栅格之间的间隔距离代价（m）
对以下两种情况做分析，求解出 c o s t ( n − 1 , n ) L cost{\left( {n - 1,n} \right)_{\rm{L}}} cost(n−1,n)L
最终得到的最短路径启发函数如下式：
L
f
(
n
)
=
L
g
(
n
−
1
)
+
1
2
∥
n
−
1
,
n
∥
(
1
cos
(
a
r
c
t
a
n
(
i
n
−
1
)
)
+
1
cos
(
a
r
c
t
a
n
(
i
n
)
)
)
+
∥
n
,
t
a
r
g
e
t
∥
{L_{\rm{f}}}\left( n \right) = {L_{\rm{g}}}\left( {n - 1} \right) + \frac{1}{2}\left\| {n - 1,n} \right\|\left( {\frac{1}{{\cos \left( {{\rm{arctan}}({i_{n - 1}})} \right)}} + \frac{1}{{\cos \left( {{\rm{arctan(}}{i_n})} \right)}}} \right) + \left\| {n,target} \right\|
Lf(n)=Lg(n−1)+21∥n−1,n∥(cos(arctan(in−1))1+cos(arctan(in))1)+∥n,target∥
道路损耗功启发函数:
W
f
(
n
)
=
W
g
(
n
)
+
W
h
(
n
)
{W_{\rm{f}}}\left( n \right) = {W_{\rm{g}}}\left( n \right) + {W_{\rm{h}}}\left( n \right)
Wf(n)=Wg(n)+Wh(n)n——当前节点栅格；
Wg(n)——起点栅格与当前节点栅格的道路损耗功价值（J）；
Wh(n)——当前节点栅格与目标点栅格的道路损耗功价值（J）。
W
h
(
n
)
=
G
δ
∥
n
,
t
a
r
g
e
t
∥
{W_{\rm{h}}}\left( n \right) = G\delta \left\| {n,target} \right\|
Wh(n)=Gδ∥n,target∥当前节点与目标点之间的欧几里得距离（m）.
W
g
(
n
)
=
W
g
(
n
−
1
)
+
c
o
s
t
(
n
−
1
,
n
)
W
{W_{\rm{g}}}\left( n \right) = {W_{\rm{g}}}\left( {n - 1} \right) + cost{\left( {n - 1,n} \right)_{\rm{W}}}
Wg(n)=Wg(n−1)+cost(n−1,n)W上一节点栅格到当前节点栅格之间的的道路损耗功代价（J）
对以下几种情况做分析，求解出
c
o
s
t
(
n
−
1
,
n
)
W
{cost{{\left( {n - 1,n} \right)}_{\rm{W}}}}
cost(n−1,n)W
最终得到的最短道路损耗功启发函数如下式：
W
f
(
n
)
=
W
g
(
n
−
1
)
+
{
G
cos
(
a
r
c
t
a
n
(
i
n
−
1
)
)
δ
n
−
1
+
G
sin
(
a
r
c
t
a
n
(
i
n
−
1
)
)
}
∥
n
−
1
,
n
∥
2
cos
(
a
r
c
t
a
n
(
i
n
−
1
)
)
+
{
G
cos
(
a
r
c
t
a
n
(
i
n
)
)
δ
n
+
G
sin
(
a
r
c
t
a
n
(
i
n
)
)
}
∥
n
−
1
,
n
∥
2
cos
(
a
r
c
t
a
n
(
i
n
)
)
+
G
δ
∥
n
,
t
a
r
g
e
t
∥
\begin{array}{ccccccccccccccc}{{W_{\rm{f}}}\left( n \right) = {W_{\rm{g}}}\left( {n - 1} \right) + \left\{ {G\cos \left( {{\rm{arctan}}({i_{n - 1}})} \right){\delta _{n - 1}} + G\sin \left( {{\rm{arctan}}({i_{n - 1}})} \right)} \right\}\frac{{\left\| {n - 1,n} \right\|}}{{2\cos \left( {{\rm{arctan}}({i_{n - 1}})} \right)}}}\\{\begin{array}{ccccccccccccccc}{}&{}\end{array} + \left\{ {G\cos \left( {{\rm{arctan}}({i_n})} \right){\delta _n} + G\sin \left( {{\rm{arctan}}({i_n})} \right)} \right\}\frac{{\left\| {n - 1,n} \right\|}}{{2\cos \left( {{\rm{arctan}}({i_n})} \right)}} + G\delta \left\| {n,target} \right\|}\end{array}
Wf(n)=Wg(n−1)+{Gcos(arctan(in−1))δn−1+Gsin(arctan(in−1))}2cos(arctan(in−1))∥n−1,n∥+{Gcos(arctan(in))δn+Gsin(arctan(in))}2cos(arctan(in))∥n−1,n∥+Gδ∥n,target∥
综合启发函数：
L
=
∣
∣
s
t
a
r
t
,
t
a
r
g
e
t
∣
∣
L = ||start,target||
L=∣∣start,target∣∣
W
=
G
δ
∣
∣
s
t
a
r
t
,
t
a
r
g
e
t
∣
∣
W = G\delta ||start,target||
W=Gδ∣∣start,target∣∣
F
(
n
)
=
σ
∗
L
f
(
n
)
/
L
+
τ
∗
W
f
(
n
)
/
W
F\left( n \right) = \sigma *{L_{\rm{f}}}\left( n \right)/L + \tau *{W_{\rm{f}}}\left( n \right)/W
F(n)=σ∗Lf(n)/L+τ∗Wf(n)/W
综合式启发函数统一最短路径启发函数和最小道路额外功函数，不同的权重大小决定最短路径启发函数和最小道路额外功函数在综合式启发函数中所占不同的比重。
传统A算法在大环境中搜索，存在着搜索效率低的问题。
传统A算法是从起点开始向终点搜索，直到寻到可行路径停止搜索，在搜索路径的前期阶段远g(n)小于h(n),搜索时横向搜索范围窄，纵向搜索范围宽，搜索速度快，在搜索的后期阶段g(n)远大于h(n),搜索时纵向搜索范围窄，横向搜索范围宽，搜索速度慢；**而改进后的双向A搜索算法分别从起点和终点开始搜索，当搜索到相同的当前节点时，搜索结束。**相比于传统A算法，双向A*搜索算法都处于g(n)远小于h(n)阶段，一定程度上提高了算法的搜索效率，缩短搜索时间。
用于初始化地图参数
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;
用于获取子节点信息
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
end
主程序
% 基于栅格地图的机器人路径规划算法
% 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);
可以先参考古月居的这篇文章
运行总时间
栅格地图大小（20x20）
栅格地图大小（30x30）
栅格地图大小（40x40）
可以看到Astar算法对于栅格地图越大的情况，搜索时间越长，其速度相比Dijkstra有优势（尤其是在地图比较大的时候，优势更明显）。但其总耗时较长，不适用于实时的路径规划，不适用于局部路径规划，适用于全局路径规划。
可以参考这篇文章
这篇文章介绍了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 g
可以参考这篇文章
本项目以Astar算法作为全局路径规划算法，DWA作为局部路径规划算法，实现效果如下。（具体原理与算法代码解释与说明会在之后的文章附上）
ROS_导航_Astar+DWA
本人所有文章仅作为自己的学习记录，若有侵权，联系立删。