Contents Introduction 介绍
With the rapid development of robotics and artificial intelligence, mobile robots have been widely used. To accomplish the navigation task on the basis of the existing grid map, reasonable path planning becomes crucial. Therefore, many researchers have done a lot of work on path planning algorithms.
随着机器人技术和人工智能的迅速发展，移动的机器人得到了广泛的应用。要在现有栅格地图的基础上完成导航任务，合理的路径规划就变得至关重要。因此，许多研究者在路径规划算法方面做了大量的工作。
At present, many path planning algorithms have been proposed, including artificial potential field method [1], A-star algorithm [2], ant colony algorithm [3], Dijkstra [4] and RRT [5] algorithm and various optimization algorithms for RRT [6]–[8]. Among them, the A-star algorithm is widely used for path planning of robots. Wang [9] et al. proposed to control the node weights so that the A-star algorithm can approach the target point quickly when it is far away from the target point and can search locally and carefully when it is close to the target point to ensure that the target is reachable when there are more obstacles near the target point. It solves the problem of poor real-time path planning for mobile robots in complex indoor environments. Lu et al. [10] proposed to add a judgment threshold to the set open of A-star algorithm to improve the real-time path planning and reduce the number of search nodes. Chen et al [11] combined the idea of artificial potential field with the traditional A-star algorithm to give the repulsive field function to the obstacles in the grid map and calculate the repulsive magnitude of the surrounding grid to solve the problem that the traditional A-star algorithm does not consider the influence of the obstacle distribution on the path selection when planning the path. Zhao et al [12] combined the A-star algorithm with the jump point search algorithm to extend by screening jump points, which reduces unnecessary node computation and solves the problems of large memory overhead and long computation time of A-star pathfinding algorithm in larger scenarios.
目前，已经提出了许多路径规划算法，包括人工势场法 [1] 、A-star算法 [2] 、蚁群算法 [3] 、Dijkstra算法 [4] 和RRT算法 [5] 以及RRT算法 [6] - [8] 的各种优化算法。其中，A-star算法被广泛用于机器人的路径规划。Wang [9] 等提出控制节点权重，使A-star算法在远离目标点时能快速接近目标点，在接近目标点时能局部仔细搜索，保证目标点附近障碍物较多时目标可达。解决了室内复杂环境下移动的机器人路径规划实时性差的问题。Lu等 [10] 提出在A-star算法的集合开度中增加判断阈值，以提高路径规划的实时性，减少搜索节点数。 Chen等 [11] 将人工势场的思想与传统的A-star算法相结合，对栅格地图中的障碍物给予排斥场函数，并计算周围栅格的排斥大小，解决了传统A-star算法在规划路径时没有考虑障碍物分布对路径选择的影响的问题; Zhao等 [12] 将A-star算法与传统的A-star算法相结合，星星算法通过筛选跳转点，对跳转点搜索算法进行扩展，减少了不必要的节点计算，解决了A-星星寻路算法在较大场景下内存开销大、计算时间长的问题。
To address the problems of low computational efficiency and long convergence time of A-star algorithm, this paper proposes to improve the A-star algorithm based on using topological maps to enhance the efficiency of A-star in path planning under large scenes. Firstly, the grid map is constructed by using laser SLAM. Secondly, a topological mapping system is proposed to construct the topological map. Finally, the node connectivity relationship in the topological map is used to improve the path planning problem of A-star algorithm in large scenes. The time cost is shortened and the computation is reduced. The planning efficiency of the algorithm is improved.
针对A-star算法计算效率低、收敛时间长的问题，提出利用拓扑地图对A-star算法进行改进，以提高A-star算法在大场景下的路径规划效率。首先，利用激光SLAM构建栅格地图。其次，提出了一个拓扑映射系统来构造拓扑图。最后，利用拓扑图中节点的连通关系，改进了A-star算法在大场景下的路径规划问题。缩短了计算时间，减少了计算量。提高了算法的规划效率。
A-Star Algorithm Principle
A-Star算法原理
The A-star algorithm is the most effective direct search method for solving the shortest path, and is currently widely used in path planning. Applicable to static grid maps, the cost function expression is:
\begin{equation*}
f(n)=g(n)+h(n)\tag{1}\end{equation*}
where \begin{equation*}
h(n)=\sqrt{(x_{n}-x_{goal})^{2}+(y_{n}-y_{goal})^{2}}\tag{2}\end{equation*}A-star算法是求解最短路径最有效的直接搜索方法，目前在路径规划中得到广泛应用。适用于静态网格地图，成本函数表达式为：
\begin{equation*}
f(n)=g(n)+h(n)\tag{1}\end{equation*}The specific implementation steps of the traditional A-star algorithm are given below [13].
传统A-star算法的具体实现步骤如下 [13] 。
A-star algorithm A-star算法
Input: start
close
open
current_node
current_node
open
close
if current_node
return best_path 返回最佳路径
end if
neighbor_nodes
neighbor_nodes
for N
对于N个
if N
如果N
do nothing 什么都不做
else if N
否则，如果N
G[N_new _calculated]'H, F
G[N_new _calculated] N ′ H，F
if
N's parent
N的父节点
end if
N's parent
N的父节点
end if
end for 端
end while
Improving the A-Star Algorithm
A-Star算法的改进
A. Topological Map Acquisition Method
A.一种拓扑图获取方法
Occupancy grid maps use a binarized representation. This static grid map is convenient for various path search algorithms for path planning. The 2D grid map is first created using laser SLAM, and the current mainstream map building algorithm is Cartographer, which has loopback detection to create maps with high accuracy and is suitable for map creation of large scenes.
占领区网格地图使用二进制表示。该静态栅格地图便于各种路径搜索算法进行路径规划。2D栅格地图首先使用激光SLAM创建，目前主流的地图创建算法是Cartographer，它具有快速检测以创建高精度的地图，适用于大型场景的地图创建。
In indoor environments, several kinds of objects are generally present in different areas, and the features and locations of some of these objects can be used for the construction of topological nodes. As shown in Figure 1, the flowchart of the acquisition method of semantic topological map is shown, firstly, RGB images and depth images are acquired using realsense camera. The semantic objects in the room are obtained by combining with solov2 instance segmentation algorithm. Secondly, according to the obtained semantic objects are transformed to the camera coordinate system, according to the inverse imaging principle, the pixel coordinates
\begin{equation*}\left\{\begin{array}{c}
Z_{c}=d / 1000 \\
X_{c}=\left(u-c_{x}\right) Z_{c} / f_{x} \\
Y_{c}=\left(v-c_{y}\right) Z_{c} / f_{y}\end{array}\right.\tag{3}\end{equation*}
\begin{equation*}\mathbf{P}_{m}=\mathbf{R}\times \mathbf{P}_{c}+\mathbf{T}\tag{4}\end{equation*}在室内环境中，几种类型的对象通常存在于不同的区域中，并且这些对象中的一些对象的特征和位置可以用于拓扑节点的构造。如图 Figure 1 所示，示出了语义拓扑图的获取方法的流程图，首先使用realsense相机获取RGB图像和深度图像。结合solov 2实例分割算法得到房间内的语义对象。其次，根据得到的语义对象变换到摄像机坐标系，根据逆成像原理，将像素坐标
\begin{equation*}\left\{\begin{array}{c}
Z_{c}=d / 1000 \\
X_{c}=\left(u-c_{x}\right) Z_{c} / f_{x} \\
Y_{c}=\left(v-c_{y}\right) Z_{c} / f_{y}\end{array}\right.\tag{3}\end{equation*}\begin{equation*}\mathbf{P}_{m}=\mathbf{R}\times \mathbf{P}_{c}+\mathbf{T}\tag{4}\end{equation*}According to the nature of graph, it is known that a graph is composed of several vertices and edges connected to each other. Among them, each edge connects two vertices, and a graph with no direction between two vertices is called an undirected graph. The robot is bi-directional during the motion of two points, so this paper proposes to connect the semantic landmarks of different regions using undirected graphs. As shown in Figure 2, a semantic topological map of 14 m long and 10 m wide is created.
根据图的性质，我们知道一个图是由若干个相互连接的顶点和边组成的。其中，每条边连接两个顶点，两个顶点之间没有方向的图称为无向图。机器人在两点运动时是双向的，因此本文提出用无向图连接不同区域的语义标志。如 Figure 2 所示，创建了14米长、10米宽的语义拓扑图。
B. A-Star Algorithm Topological Node Search Strategy
B。A—Star算法的拓扑节点搜索策略
Based on the created semantic topological map, the navigation of the mobile robot has a priori information. The mobile robot performs path planning based on the topological grid map. the heuristic function of the A-star algorithm acts on the search effect. In large scenarios, the grid base of the A-star search becomes inflated extremely fast, the complexity of the search is high, and the computational effort is large. Therefore, the search takes a long time and significantly reduces the speed of path planning.
基于所创建的语义拓扑地图，移动的机器人的导航具有先验信息。移动的机器人根据拓扑栅格地图进行路径规划。A-star算法的启发函数影响搜索效果。在大型场景中，A星搜索的网格基础变得非常快，搜索的复杂性很高，计算量很大。因此，搜索需要很长的时间，并大大降低了路径规划的速度。
To improve the path planning speed and success rate of A -star algorithm in large scenes, this paper proposes the A-star path search algorithm based on semantic topological map. The path planning in large scenes is divided into two parts: path planning based on grid map and path planning based on semantic nodes to complete. The semantic topological map completes the planning of the path trunk, and then connects it with the traditional path planning algorithm. In order to improve the efficiency of search and save time, this paper adopts a two-way A-star algorithm to connect different semantic nodes, as shown in Figure 3, when the mobile robot goes from the starting point
为提高A -星星算法在大场景下的路径规划速度和成功率，提出了基于语义拓扑图的A-星星路径搜索算法。将大场景中的路径规划分为两部分：基于栅格地图的路径规划和基于语义节点的路径规划来完成。语义拓扑图完成路径主干的规划，然后将其与传统的路径规划算法相衔接。为了提高搜索效率和保存时间，本文采用双向A星算法连接不同的语义节点，如图 Figure 3 所示，当移动的机器人从起点
Next, the semantic nodes whose projections fall between the line of the two points are identified according to the positions of the starting point and the target point under the map coordinate system. As shown in Figure 4, five points from
接着，根据起点和目标点在地图坐标系下的位置，识别投影落在两点连线之间的语义节点。如 Figure 4 所示，图中从
Because the OutDegree and InDegree of some vertices are greater than one, the paths of the starting point and the target point are also not unique. To solve the path non-uniqueness problem, this paper first calculates the right between two vertices. As shown in Figure 5, the weights
\begin{equation*}
k_{A B}=\sqrt{\left(x_{A}-x_{B}\right)^{2}+\left(y_{A}-y_{B}\right)^{2}}+\left\vert l_{A}-l_{B}\right\vert\tag{5}\end{equation*}由于某些顶点的OutDegree和InDegree大于1，因此起始点和目标点的路径也不是唯一的。为了解决路径的不唯一性问题，本文首先计算两个顶点之间的距离。如图 Figure 5 所示，连接顶点
According to Equation 5, the corresponding weights of all edges are found. Finally, Floyd's algorithm is used to find the shortest path between two vertices in the graph with weights. The core idea of this algorithm is whether there exists a path whose weight and is less than the weight of vertex A directly to vertex B. If there exists this updated path. It can be expressed as follows.
\begin{equation*}
{Dis}\left(P_{A}, P_{O}\right)+{Dis}\left(P_{O}, P_{B}\right)<{Dis}\left(P_{A}, P_{B}\right)\tag{6}\end{equation*}
where 根据公式 5 求出所有边对应的权值，最后用弗洛伊德算法求出带权图中两个顶点之间的最短路径。该算法的核心思想是判断是否存在一条权和小于顶点A到顶点B的权的路径。如果存在此更新路径，则可以表示为：
\begin{equation*}
{Dis}\left(P_{A}, P_{O}\right)+{Dis}\left(P_{O}, P_{B}\right)<{Dis}\left(P_{A}, P_{B}\right)\tag{6}\end{equation*}
Data Simulation and Analysis
数据模拟与分析
To verify the feasibility and search efficiency of the algorithm in this paper, the improved A-star algorithm is simulated with the original A-star algorithm and the two-way A-star algorithm in a large environment, and the average data of 60 experiments are counted as the experimental results to compare the time cost and the search computation cost. The experiments are conducted on a computer with Intel Core i7-9750H, 2.6 GHz CPU and 16G memory, and the algorithm experiment platform is matplotlib environment introduced by Python3.
为了验证算法的可行性和搜索效率，在大环境下对改进的A-star算法与原A-star算法和双向A-star算法进行了仿真，并将60次实验的平均数据作为实验结果，比较了时间开销和搜索计算开销。实验在Intel Core i7- 9750 H、2.6 GHz CPU和16 G内存的计算机上进行，算法实验平台是Python 3. 0引入的matplotlib环境。
The simulation environment of Fig. 2 is simulated as a map of [140], [100] for verifying the algorithm's ability to search between the start and end points, with the start position set to [5], [5] and marked with blue and the end position set to [130], [90] and marked with red. The experimental results are shown in Figure 6.
Fig. 2 的模拟环境被模拟为[140]、[100]的地图，用于验证算法在起点和终点之间搜索的能力，其中起始位置被设置为 [5] 、 [5] 并且用蓝色标记，并且结束位置被设置为[130]、[90]并且用红色标记。实验结果见 Figure 6 。
The experimental results obtained by each of the three algorithms after 60 iterations of path planning are summarized in Table 1.
在路径规划的60次迭代之后通过三种算法中的每一种获得的实验结果总结在 Table 1 中。
表I.算法模拟数据对比表
It can be seen from Figure 6 that the original A-star and bidirectional A-star algorithms have a large search range, which wastes computational resources and has low planning time efficiency. The improved A-star algorithm avoids the invalid grid search and improves the computational efficiency, as can be seen from Table 1, the improved A-star algorithm improves the time cost by more than 60%.
从 Figure 6 可以看出，原有的A-star和双向A-star算法搜索范围大，浪费计算资源，规划时间效率低。改进后的A-star算法避免了无效网格搜索，提高了计算效率，从 Table 1 可以看出，改进后的A-star算法时间开销提高了60%以上。
Conclusions 结论
This paper addresses the problem of low planning efficiency of the traditional A-star algorithm in large scenes. By improving the A-star algorithm using topological maps, the efficiency of A-star in path planning under large scenes is improved. First, a grid map is constructed using Cartographer algorithm. Then, use the depth camera to capture indoor object instances and build an instance mapping system to construct the topological map. Finally, the node connectivity relationship in the topological map is used to improve the path planning problem of A-star algorithm in large scenes. Simulation experiments show that the improved algorithm can plan the paths quickly in large scenes. Compared with the original A-star algorithm, the algorithm is less computationally intensive and improves more than 60% in terms of time cost. The improved algorithm in has higher planning efficiency and has certain theoretical and application values.
针对传统A-star算法在大场景下规划效率低的问题。利用拓扑图对A-star算法进行改进，提高了A-star算法在大场景下的路径规划效率。首先，使用Cartographer算法构建栅格地图。然后，使用深度相机捕获室内物体实例，并建立实例映射系统来构建拓扑图。最后，利用拓扑图中节点的连通关系，改进了A-star算法在大场景下的路径规划问题。仿真实验表明，改进后的算法能够在大场景下快速规划路径。与原A-star算法相比，该算法计算量小，时间开销提高了60%以上。改进后的算法具有更高的规划效率，具有一定的理论和应用价值。
ACKNOWLEDGMENT 确认
This research is supported by National Natural Science Foundation of China(61803058), and China Postdoctoral Science Foundation (2021MD703939).
本研究得到了国家自然科学基金（61803058）和国家博士后科学基金（2021MD703939）的资助。