A* 算法用于路径规划
用于机器人路径规划和导航的 A* 图搜索算法的实现
引言
在本文中，我们将探讨用于机器人路径规划和导航的 A* 图搜索算法的实现。
A* 路径规划
路径规划算法的目的是找到从源位置到目标位置的路径。
我们在以下假设下工作
- 具有理想定位的点机器人
- 工作空间是有限且已知的
- 静态源、目标和障碍物位置
- 障碍物数量有限
- 障碍物具有有限厚度
将离散路径规划任务视为图搜索问题。
A* 是一种广泛使用的图遍历算法，它使用最佳优先搜索方法来查找从源节点到目标节点的成本最低的路径。
- 工作空间划分为离散网格
- 每个网格代表图的顶点
- 边由网格的连接组件定义
- 如果相邻网格点位于障碍物内部，则不连接
- 如果相邻网格点位于自由空间，则连接
工作空间的相邻网格被视为通过边连接。每条边都关联一个使用成本函数 $f(n)$ 定义的成本。
成本可以使用基于有关源、目标、障碍物、图中当前节点位置的信息的各种标准来定义。
典型的图搜索算法从源节点到目标节点找到成本最低的路径。
对于路径查找应用程序，成本直接与网格/节点到源节点和目标节点的距离相关。
最低成本路径提供了一条从源网格到目标网格的最短距离路径。
输入到模拟环境的形式是一个图像。
有信息搜索算法
广度优先或深度优先搜索是无信息搜索算法，其中探索图中所有节点沿着广度或深度直到找到目标节点。
在此过程中会探索大量节点。
我们可以使用有关问题的附加信息来减少搜索空间。
这些方法称为有信息搜索算法，它们考虑路径的成本、到目标状态的启发式距离等，以确定最有可能导致目标的路径，或者考虑手头问题的某些信息来选择最有可能导致目标位置的节点。
A* 使用最佳优先搜索策略，根据预定义的标准通过扩展最佳节点（最短路径）来探索图。
它选择成本函数值最低的节点，该成本函数定义为 $f(n) = g(n) + h(n)$，其中 $g(n)$ 是从初始节点到当前节点的路径的确切成本。
$h(n)$ 是从当前节点到目标的估计启发式成本
A* 算法可以视为在 2 种状态下运行
- 查找与最低成本相关的节点
- 扩展节点
通过扩展节点，我们的意思是节点的所有连接组件都被视为算法下一次迭代中最佳节点的潜在候选者。
实现细节：算法 1
最简单的数据结构是 $ SET $。
我们将该集合称为 $OPENLIST$。最初，$OPENLIST$ 只包含源节点。
算法如下
- 从集合中找到成本最低的元素
- 如果到达目标节点，则停止并回溯路径
- 将元素的所有后继节点（扩展节点）添加到集合中，前提是它们位于工作空间的自由区域
- 如果集合为空，则找不到到目标的路径
此算法未完成。即使路径存在，它也并非总是能在源和目标之间找到路径。
它可能会陷入循环，尤其是在死胡同中，或者遇到大障碍物时，尤其是在遇到唯一的后继节点是其父节点的情况下。
下面是使用仅开放列表时算法陷入循环的模拟输出。
下面是当我们不考虑时算法陷入循环的模拟
以下是具有开放列表和关闭列表更改的模拟输出
同样，我们可能会访问一个已存在于开放列表中的节点，因此算法将陷入循环。这可能发生在遇到大障碍物时。
为解决此问题，我们维护另一个集合，称为 $CLOSEDLIST$。
它将包含已扩展的节点，即最低成本节点。如果节点已存在于关闭列表中，则不会再次考虑扩展，或者我们只能访问该节点有限次数。
这将避免陷入循环的问题。
此外，如果节点已存在于开放列表中，我们会检查列表中节点的成本。成本越高，节点将被新节点替换，否则新节点不会被考虑扩展。
因此，我们将在开放列表中考虑一个新节点进行扩展，只有当通过该节点到达的路径比通过列表中已存在的节点到达的路径成本更低时。
可接受性
如果启发式函数不过度估计到目标的真实成本，则 A* 算法将始终找到最优解，并且 A* 被称为使用了可接受的启发式函数。
可接受的启发式函数过于乐观，它将导致 A* 搜索的路径比最优路径成本更高。
这不会阻止 A* 扩展最优路径上的节点，如果启发式值太高，则可能发生这种情况。
有些启发式函数比其他函数更好。我们希望启发式函数尽可能接近真实成本。如果到目标的真实成本与估计值相比非常大，启发式函数将产生误导。
如果我们有两个可接受的启发式函数 $h_1,h_2$，那么如果 $h_1 > h_2$ 对于所有 N，则称 $h_1$ 支配 $h_2$ 或 $h_1$ 优于 $h_2$。
下面是启发式函数 $h(n)=0$ 的模拟，这是最差的启发式函数，它根本无助于减少搜索空间。这称为一致成本搜索。
我们可以看到它是可接受的并且找到了最优路径。
如果我们考虑欧几里得距离作为启发式，真实距离将始终大于或等于欧几里得距离，因此将导致最优路径。
让我们考虑启发式函数 $(1+\varepsilon) h(n),\varepsilon >1$。
这将导致对到目标的真实距离/成本的过度估计。过度估计会导致成本偏向于启发式，而不是到节点的真实旅行成本。
路径将倾向于偏向目标位置。
下面的模拟是 $\varepsilon=5$ 的输出
下面的模拟是 $\varepsilon=1$ 的输出
因此，我们可以看到启发式函数的过度估计会产生非最优路径。
非最优路径
A* 扩展所有具有同等潜力的节点，分析大量节点。
如果我们能够放宽最优性条件，我们可以从更快的执行时间中受益。
例如，我们可以使用 static 成本函数 $f(n) = g(n) + (1+ \varepsilon) h(n)$ 的加权，如果 $h(n)$ 是可接受的启发式，则算法将找到成本为 $L$ 的路径。$f(n)=g(n)+h(n) \le L$。
如果我们高估了成本，则会选择不在最优路径上的点。算法将忽略最优解。
设 $h^*(n)$ 为从节点到目标的真实成本。 \begin{eqnarray*}\\ h(N) \le c(N,P) + h(P) \\\\ h(N_m) \le h^*(N_m) \\\\ h(N_{m+1}) \le C(N_m,N_{m+1})+h(N_m) \le C(N_m,N_{m+1}) + h^*(N_m) = h^*(N_{m+1})\\\end{eqnarray*} 因此，如果我们估计成本为 $h_1(n)$，它不是可接受的启发式
\begin{eqnarray*}\\(1+\varepsilon)h(N_{m+1}) > h^*(N_{m+1}) \\\\h_1(N_{m+1})=(1+\varepsilon)h(N_{m+1}) < (1+\varepsilon)h^*(N_{m+1}) \\\\\end{eqnarray*}. 因此，最差的启发式函数可以产生最佳路径的 $(1+\varepsilon)$ 倍。
实现细节
$GNode$ 是表示图节点的类，它还包含有关图的相邻节点的信息。
$AStar$ 是一个包含 $AStar$ 算法方法和对象的类。它包含一个用于开放列表和关闭列表的队列。我们使用 C++ $dequeue$ 数据结构，因为它提供动态分配。
所需例程之一是检查节点是否存在于关闭列表中 <script class="brush: cpp" type="syntaxhighlighter"> <![CDATA[ bool AStart::isClosed(GNode c) //遍历关闭列表中的元素 std::deque
另一个所需方法是将元素添加到开放列表。
除了集合，我们还可以使用优先级队列数据结构进行实现。
队列根据与节点关联的成本进行排序。成本越高优先级越低，成本越低优先级越高。
最高优先级的节点位于队列的顶部，而较低优先级的节点位于底部。
因此，在向队列添加新元素时，必须将其添加到正确的位置。
此外，如果一个元素已存在于开放列表中，我们仅在成本低于要插入的元素时才替换它。
我们仅在要插入的节点的优先级更高时才将元素插入优先级队列。
为了确保我们在单次传递中完成操作，我们将节点插入，而不考虑它是否已存在于队列中，并设置一个名为 insertflag 的标志以指示这一点。
如果插入标志设置为 true 后遇到相同的节点，则表示该节点的优先级较低，我们将删除遇到的节点，因为优先级较高的节点已插入队列。
如果我们遇到开放列表中的节点并且其优先级更高，则我们不插入该节点。
<![CDATA[ bool addOpen(GNode &node) std::deque::iterator it=open.begin();
std::deque::iterator it1=open.end();
bool flag=false; //return status
bool insert_flag=false;//insert flag //compute the cost of node being inserted
float val2=node.getCost(goal,maxc);
for(it=open.begin();it!=open.end();it++) //compute cost of current node in queue
float val1=(*it).getCost(goal,maxc);
if(((*it).position==node.position))
if(insert_flag==true) //reopening node it=open.erase(it);rccopen++;
insert_flag=true; break; if(val2
我们还需要确定节点是否可访问。预先计算一个密集网格，告诉我们当前位置是否在障碍物内。变量 $obstaclemap$ 包含此映射。
因此，我们得到了当前节点和后继节点。
我们计算沿运动方向的单位向量，并沿该方向每隔一段时间采取小步，每次检查点是否在障碍物内。
这种方法即使在网格尺寸较大的情况下也能告诉我们节点是否可访问，因为节点可能位于障碍物的对面。
<![CDATA[ bool isAccessible(GNode &n,GNode &n1,int mode) float dx=n.position.x-n1.position.x;
float dy=n.position.y-n1.position.y; float mag=sqrt(dx*dx+dy*dy);
dx=dx/mag; dy=dy/mag; Point2f p=n1.position; p.x=n1.position.x;
p.y=n1.position.y; while(1)
if(p.x==n.position.x && p.y==n.position.y) break; //current position and initial position
float dx2=p.x-n1.position.x; float dy2=p.y-n1.position.y;
float mag1=sqrt(dx2*dx2+dy2*dy2); //current position and goal position
float dx1=p.x-n.position.x; float dy1=p.y-n.position.y;
float dd=sqrt((dx1*dx1)+(dy1*dy1)); //goal position lies withing minimum grid resolution
if(dd<=minr && mode==1) return true; //no obstacle found if(mag1>=mag)
return true; prev=p; Point2f ff=Point2f(p.x,-p.y);
if(ff.y >=obstacle_map.rows ff.x>=obstacle_map.cols ff.x<0 ff.y<0) n1.open=false;
return false; int val=(int)obstacle_map.ptr(((int)ff.y))[((int)ff.x)]; if(val>0) n.open=false;
return false; //take small increment p.x=p.x+(1)*dx; p.y=p.y+(1)*dy; return true; ]]</script>
模拟
模拟输出可以在 此处 找到。
代码
文件 AStar.hpp、AStar.cpp 定义了 AStart 算法。文件 gsim.hpp、gsim.cpp 定义了模拟环境。代码可以在 此处 找到。
测试模拟的图像和输出模拟视频也可以在存储库中找到。
按如下方式运行代码
AStar obstales1.png 10
其中 10 是节点之间的距离
文档的 PDF 版本可以在 此处 找到。