**1. Introduction**

With the popularity of the robotics industry, simultaneous localization and mapping (SLAM) technology has developed rapidly. SLAM technology can be divided into three categories, i.e., LiDAR-SLAM [1–4], visual-SLAM [5–8] and LiDAR fusion visual SLAM [9–11], and it is widely used for robot navigation tasks. In the application of robot navigation, a by-product of SLAM is the map, including metric and topological maps. The metric map emphasizes accurately representing the positional relationships of objects, while the topological map emphasizes the relationships between map elements.

In smaller spaces, such as corridors and houses, occupancy grid maps [12] are preferred over topological maps. The topology maps, on the other hand, are more appropriate for path planning in large areas where the occupancy grid maps are computationally expensive. In this paper, the visibility graph [13], a topology-based type of map, will be constructed for route planning and navigation.

Path planning has been an emerging trend in research nowadays to cater to the needs of autonomous systems. The visibility graph (v-graph) is an efficient map representation

**Citation:** Li, Q.; Xie, F.; Zhao, J.; Xu, B.; Yang, J.; Liu, X.; Suo, H. FPS: Fast Path Planner Algorithm Based on Sparse Visibility Graph and Bidirectional Breadth-First Search. *Remote Sens.* **2022**, *14*, 3720. https:// doi.org/10.3390/rs14153720

Academic Editor: Andrzej Stateczny

Received: 16 June 2022 Accepted: 31 July 2022 Published: 3 August 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

for path planning, which allows the robot to move from one node to another, but it has some drawbacks. Firstly, it is not only hard to map the visibility graph completely in the 3D world [14], but it is also difficult to extract the outlines of obstacles. Secondly, when the number of nodes in the graph increases, the associated edges will be doubled, which causes increased computational cost in terrain where complex obstacles exist [15]. In this case, it is necessary to simplify complex obstacles to reduce vertices and edges for path planning.

Most existing v-graph generation methods [16–21] store the pointcloud explored by the LiDAR into the local grid and then perform plane mapping to extract the vertices of the polygon. However, these algorithms, such as [17–20], face the following problems. Firstly, if the local grid is too sparse, the sampling accuracy will be decreased. On the other hand, if the local grid is dense, the shape of a polygon will be more accurately determined, but the amount of calculation will increase dramatically. Secondly, the quantity of vertices and edges affects how complicated the v-graph-based path search algorithm is. There will be a lot of redundant vertices and edges in maps with a lot of intricate barriers, which makes path search a time-consuming task. Finally, the large number of vertices and edges in the v-graph slows down its traversal speed, which leads to a decrease in the maintenance speed of the v-graph. Although WonheeLee et al. [21] proposed a v-graph-based obstacle avoidance strategy, they did not address the issue of dense v-graph in complicated scenes. A sparse v-graph-based path planner is proposed as a solution to these issues, which lowers the cost of v-graph maintenance, increases the effectiveness of v-graph building, and decreases space waste during the path search. Overall, the main contributions of the paper are summarized as follows:


#### **2. Related Work**

The current mainstream of path planning research is divided into the following categories: search-based, sampling (probability)-based, genetic algorithm (GA)-based, and learning-based. According to the planning results, it is further divided into complete planning algorithms and probabilistic complete planning algorithms.

Search-based planning methods: These methods mainly include Dijkstra [22] and its variants, such as A\* [23], D\* [24], etc. The Dijkstra and A\* algorithms are often used to search on discrete grids. Such algorithms are re-initialized for each search cycle, thus taking a long time to plan routes. An incremental version of the Dijkstra-derived algorithm was proposed to reduce re-planning time by adjusting the local information to the planning result in the previous cycle. However, similar to D\* Lite [25], when encountering complex environments, the computational load of the incremental algorithm to re-evaluate the current environment is even greater than that of A\* without increment. Many improved A\* algorithms have been proposed to decrease the memory space and achieve a better trajectory [26,27], but they are all based on the occupancy grids and thus cannot avoid the shortcomings of the occupancy grids.

Random sampling planning methods: These methods mainly include rapidly expanding random tree (RRT)[28] and a series of its variants, such as RRT\* [29], informed RRT\* [30], and RRT-Connect [31]. These methods are designed for a known environment. Some algorithms derived from RRT are used for planning in an unknown or partially known environment. These methods must perform maintenance or regeneration of random trees frequently to account for newly observed environments.

Genetic algorithm methods (GA): This algorithm performs path planning through crossover and mutation of chromosomes. Tu et al. [32] proposed a method for generating collision-free paths based on GA. Chen et al. [33] proposed an improved GA to minimize the total distance of the UAV. The method in [34] combined deep learning and GA to design a followable path for multi-robots. Moreover, there are many different improvements to GA [35–37] to achieve better results. In the genetic algorithm, the choice of parameters such as crossover rate and mutation rate has a significant impact on the quality of the solution, but these values are typically chosen based on experience.

Planning by deep learning: These methods [38–40] require a large number of groundtruth labels for training. For example, the essence of training based on deep learning is to encode and decode environmental information. During testing, these methods can handle scenarios similar to the training environment; the essence of training based on reinforcement learning is to encode and map the information of the state of motion every time. During testing, the results are output according to the encoded motion state information. GuichaoLin et al. [41] proposed a collision-free model based on deep reinforcement learning to allow robots to avoid obstacles. Tutsoy et al. [42] provided a minimum time path for a balancing task through reinforcement learning, and [43] considered the energy consumption to design a general dynamic control model based on deep reinforcement learning. However, learning-based methods are inherently data-driven, and the magnitude of the data limits their ability to scale to different environments.

The majority of the aforementioned path planning techniques, including common search-based algorithms, RRT algorithms, and evolutionary algorithms, are based on occupancy grids. When the scope of the searched scene is large, the drawbacks of occupancy grids will constrain the speed of these algorithms, i.e., as shown in Figure 1, in a 10 × 10 grid map, the algorithm based on the occupancy grid map traversed 20 grids to reach the goal, while the algorithm based on the v-graph only traversed five vertices of the obstacle. This paper mainly studies the visibility graph-based and path-planning methods. Although the literature [13,44–46] studied the use of the visibility graph for robot navigation, and FAR Planner [19] applied the theoretical part to the exploration and navigation of the actual 3D world, the maintenance of a graph in complex situations is still expensive, and the way-point generated in unknown environments is easy to detour. In order to address these issues, a sparse v-graph path planner is proposed. This path planner enhances the efficiency of v-graph building while decreasing space waste and v-graph maintenance costs.

**Figure 1.** Comparison of the occupancy grids and the visibility graph.

Similar to FAR Planner [19], the pointcloud is extracted from obstacles and mapped into polygons, from which vertices and edges are extracted to construct the v-graph for navigation. The improvement of our approach is that each data frame of pointcloud in the local area does not fully participate in the construction of the global layer. The complementary hole structure for iteratively updating the local grid is used to store the pointcloud information in the current local area, which means that only half of the points in each data frame need to be processed each time. The method continuously updates the pointcloud information on each data frame until a global map is formed. Compared with the original algorithm of FAR Planner, the proposed algorithm can reach the target point within a shorter distance and take less time.

In simulation experiments, the feasibility of the method is evaluated through the simulated physical environment. The environment of the simulation experiment includes medium-scale, complex-scale, and large-scale environments in the Autonomous Exploration Development Environment provided by CMU [47], and medium-scale indoor environments and complex large-scale indoor environments provided by Matterport3D [48]. In the physical experiment, the LiDAR, and an Inertial Measurement Unit (IMU) are coupled to generate state estimation of the mobile robot [4], and the proposed replacednavigationnavgiation algorithm will be tested in a real garage.
