Abstract
In view of the shortcomings of low search efficiency and many path turning points of Probabilistic Roadmaps (PRM), a bidirectional search PRM global path planning algorithm is proposed. The algorithm improves the search connection rules by using the positive and negative directions to search the path alternately, so that the connection of unnecessary nodes reduces, thereby speeding up the efficiency of path planning. Besides, the algorithm incorporates cubic spline interpolation. That will increase the smoothness of path planning and ensure that the mobile robot can realize the path planning task more smoothly and safely. The simulation results show that the improved algorithm can effectively improve the convergence speed and path smoothness of the algorithm. Finally, the improved algorithm is applied to the actual mobile robot navigation experiment. The experimental results have proven that the path planning strategy was able to a superior advantage over traditional PRM in path quality and computational time.
1. Introduction
Recently, the technological development of mobile robots has made great progress, and the development of path planning technology has become an important research field of mobile robots [1, 2]. Nowadays, path planning technology has been extensively used in industry, service industry, entertainment industry, and so on. Effective and feasible path planning technology can make mobile robots more secure and reliable to complete the tasks required by human beings. The traditional path planning algorithms include Dijkstra algorithm, A algorithm, and artificial potential field method [3–7]. A series of intelligent bionic algorithms gradually emerge with the increase of the complexity of the working space of mobile robots, such as genetic algorithm, particle swarm algorithm, artificial bee colony algorithm, and so on [8–13].
In the early 1990s, Svestka et al. proposed the Probabilistic Roadmaps (PRM) [14]. The PRM is based on random sampling strategy. The difficulty of constructing effective path graph in high-dimensional space is well solved because of small calculation amount and good real-time performance of the algorithm [15]. However, search efficiency will decrease significantly in the case of narrow passages. Moreover, it is easy to fall into local optimum. In view of the above problems, a variety of optimization methods is proposed by domestic and foreign scholars. Reference [16] proposes an improved PRM, by optimizing the sampling strategy of traditional PRM, thereby increasing the number of sampling points in narrow passages. The algorithm increases search sampling efficiency. Velagić et al. change the connection distance between sampling points, so that the useless lines in the path network diagram are reduced. This method improves the efficiency of the overall path planning [17]. An improved PRM is proposed, which introduces an artificial potential field method in PRM, so that the point falling within the obstacle moves from the obstacle to the mobile area by the repulsive force of the potential field, thereby increasing the utilization rate of the sampling points [18]. Reference [19] presents Lazy PRM, an improved PRM that minimizes the number of collision checks performed during the planning and therefore minimizes the running time of the planner. Reference [20] presents improved connectivity of the roadmap by connecting previously generated connected components. PRM an optimal variant of the original PRM algorithm is presented in [21]. Reference [22] addresses the problem of generating navigation roadmaps for uncertain and cluttered environments represented with probabilistic occupancy maps, proposed the topology-informed growing neural gas algorithm that leverages estimates of probabilistic topological structures computed using persistent homology theory. Reference [23] presents a K-order surrounding roadmap path planner which constructs a roadmap in an incremental manner to produce high-quality roadmaps. Reference [24] proposes method using a layered hybrid Probabilistic Roadmap and the Artificial Potential Field method for global planning. In [25], the main PRM methods are analyzed through a huge set of simulations with a robot into complex statics environments. Reference [26] presents a guidance mechanism that encourages the rapid construction of well-connected roadmaps with PRM methods. It leverages a topological skeleton of the workspace to track the algorithm’s progress in both covering and connecting distinct neighborhoods, and employs this information to focus computation on the uncovered and unconnected regions. Reference [27] proposes a new approach that combines the technique of particle swarm optimization with PRM to improve both the utilization rate of sampling points and the success rate of narrow passages path planning. The above algorithms can speed up the efficiency of path search. However, path smoothness is also an important index of path planning task. In [28], spline interpolation is applied to robot navigation algorithm, and a fast search random tree algorithm based on spline is proposed, which ensures the continuity of curvature along the path and eliminates the sharp change of control in traditional RRT. In [29], the proposed method combines the classic bidirectional RRT algorithm with the cubic Ferguson’s spline technique to generate trajectories that directs smoothness and continuation without any computation. In [30], Cubic Bezier curves are applied to PRMs to shepherd nonholonomic robots to the target while maintaining the whole configuration. In [31], to generate such trajectories, combination of the cubic spline technique based on cubic Ferguson’s curve with the PRM method, which eases the production of smoothened curved trajectories for wheeled mobile robots.
In this paper, a bidirectional PRM search algorithm is proposed in dealing with the problem of low search efficiency and excessive number of paths turning points in PRM algorithm. The search strategy of PRM is improved. Firstly, the initial node and the target node of the traditional PRM are searched simultaneously, which reduces the time waste of unnecessary node connection in the search process and effectively improves the search efficiency and safety performance of the algorithm. Moreover, the cubic spline curve smoothing processing is proposed to ensure that the mobile robot can work effectively and realize the path planning task more smoothly and safely.
This paper is structured as follows. Section II explains the basic principle of PRM. In section III, a bidirectional PRM search algorithm is generated. Section IV introduces the basic principle of the cubic spline interpolation and presents bidirectional PRM algorithm combined with cubic spline interpolation. The simulation and experimental results are demonstrated in Section V. Section VI concludes this paper.
2. Basic Principle of PRM
The traditional PRM is an algorithm based on graph search, which constructs an undirected path network diagram by screening and selecting random sampling points, and then selects the most suitable path. Its basic idea is to use a random network diagram to represent the trajectory planning of mobile robot, and use a probability map to show the undirected cycle property of the diagram. The undirected path network diagram is represented by , where denotes the optimal path, is set of nodes, and denotes a path set between nodes. The path set represents the local feasible path. Then using heuristic search algorithm to find the optimal path, such as Breadth first algorithm, A etc.
The PRM can be completed in two phases, namely the offline learning phase and the online query phase. Offline learning phase mainly establishes an undirected path network diagram. Online query phase mainly selects an appropriate path in the undirected path network diagram according to the start node and the target node.
2.1. Offline Learning Phase
Offline learning phase primarily constructs an undirected path network diagram . Firstly, Sampling in the map and there are obviously two possible outcomes. One is that the sampling point is on the obstacles, the point is discarded. The other is that the sampling point is in the free area, the point is constructed into the node set . After that, the local planner is used to find the next-door node for connection in , and the path set is obtained [32, 33]. The specific process is shown in Figure 1.
As can be seen from the above flow chart, in the offline learning phase of PRM, first of all, the initial work is carried out to create the point set , the path set , and the uniform random sampling point . Secondly, the validity of the sampling point is judged. If the sampling point is on the obstacle or not in the built map, the sampling point is discarded, otherwise it is the available point, which needs to be added to the point set to facilitate the connection of the points in the point set. Then, it is judged whether the path of the two-point connection encounters an obstacle. If so, the path is discarded, otherwise the available path is stored in the path set. An undirected network graph is created in this way.
For the undirected path network diagram building, the continuous workspace is transformed into a discrete workspace. Then, executing path planning, so that the complexity of the algorithm only depends on the complexity of the search path, which reduces algorithm complexity. The optimal path is taken by the random sampling points of PRM and the collision detection of these points. When constructing the network map, it starts from the nearest sampling point of the start node and spreads to the surrounding sampling points. If the two sampling points can be connected without touching obstacles, then the path is added to the network path set, otherwise, the path is discarded, as shown in Figure 2. Among them, the upper left corner is the start node and the lower right corner is the target node. Figure 2(a) is a sampling diagram. Figure 2(b) is a network path set and the line is the path.
(a) Sampling diagram
(b) Network path diagram
2.2. Online Query Phase
The main objective of the online query phase is to select an appropriate path between the start node and the target node based on the previously generated undirected path network diagram in the offline learning phase. For this purpose, the well-known A algorithm is used. Specific for the following three steps. (1)Connect two points in the map nearest to the start node and the target node, respectively(2)Search the path by using A algorithm(3)Smoothen the route and get the final path
The results are shown in Figure 3.
It can be known from the principle of PRM that the algorithm has good real-time performance and small amount of calculation, but there is still shortcoming for its efficiency. Therefore, this paper proposes an optimization algorithm to improve the efficiency of PRM.
3. PRM Optimization
In the offline learning phase, the traditional PRM makes the sampling points evenly distributed in the map and then constructs an undirected path network diagram by connecting each sampling point. Finally, the A algorithm is used to query the optimal path. In the whole process of PRM, the offline learning phase takes much less time than the online query phase. Therefore, for the efficiency of online query phase, this paper proposes a bidirectional PRM search algorithm.
3.1. The Bidirectional PRM
The basic idea of the bidirectional PRM is that the search strategy is divided into positive and negative directions after obtaining sampling diagram. The forward search starts from the start node and the reverse search starts from the target node. The two paths will meet at the intermediate node, respectively, and their integration is a final path. This method not only improves the path security, but also further accelerates the search efficiency, which greatly decreases the time waste of connection between unnecessary paths. However, there are two completely unrelated paths in the real path planning of mobile robot. They are emitted from the start node and the target node, and end at the target node and the start node, respectively. These two paths may not meet in this process.
Therefore, in order to solve the above problems, a bidirectional PRM proposed in this paper will use the positive and negative alternate search method. The specific process is as follows: The search starts from the start node. When the forward PRM searches from the start node to the next node, the reverse PRM starts the reverse search from the target node. When the reverse search catches a sampling node, the forward search starts again, and then alternately. In this way, the overlapping nodes in the middle are found, and then the optimal path is sought.
The improvement of PRM significantly accelerates the path search efficiency. In Figure 4, there are black obstacles, a start node () and a target node () after rasterizing the map. In order to prevent confusion, the positive search uses the solid line display, and the negative search uses the dotted line display. The specific search process is shown in Figure 4.
(a)
(b)
(c)
(d)
(e)
(f)
In Figure 4, firstly, the bidirectional PRM searches the sampling points in the positive direction, and then starts to search the next sampling point. Four sampling points, , , , and will be searched at the same time. If these four paths , , , and are all effective paths that conform to the search path rules, so these four paths can be stored in the path set , as shown in Figure 4(a). Then, the feasible sampling points are , , , and in the reverse direction. Similarly, if the four paths , , , and meet the requirements of the search path rules, so these four paths are also added to the path set , as shown in Figure 4(b). Then continue to switch forward search and find nodes , , etc. Determines whether , , etc. meet the path set requirements. Corresponding paths are stored in the path set , as shown in Figures 4(c) and 4(d). In this way, if the overlapping node in the middle is found and the forward PRM finds out overlapping positions as well as the reverse PRM, which means that the bidirectional PRM finds a complete path. As shown in Figures 4(e) and 4(f).
4. Path Generation Using Cubic Spline Curve
The bidirectional PRM meaningfully increases the path search efficiency. However, there are still too many inflection points, which is obviously a shortcoming in the algorithm. Therefore, this paper adopts the cubic spline interpolation method to make the path smoother [34, 35]. The cubic spline interpolation forms a smooth curve through a series of cubic multitype piecewise interpolation. Fitting the path lines of the mobile robot with the cubic spline interpolation method will make the line segments of the path smoother, meanwhile, ensure the high efficiency of the mobile robot and avoid the damage to its hardware caused by the sudden stop or sudden turn when moving.
4.1. Basic Principles
Definition: set [, ] had equal interpolation nodes, , corresponding for the cubic spline function . If function values satisfy the following two conditions, is called cubic spline interpolation function. (1)Requirement of only in each subinterval [] () and determine a cubic polynomial, set to
Among them, , , , and are undetermined coefficients. (2) is continuous on [, ] and to satisfy it:
From the above definition, the cubic polynomial has undetermined coefficients because there are cells on [, ], with 4 undetermined coefficients above each interval. Therefore, interpolation conditions must be needed in order to determine the cubic polynomial according to Equation (2), it can be seen that there are interpolation conditions. According to Equation (3), is continuous at the interpolation node and has conditions. Equations (4) and (5) represent that the first and second order derivatives of are continuous at the interpolation node, with interpolation conditions. Therefore, interpolation conditions can be obtained from the above definition. However, two boundary conditions are required to deal with the cubic spline interpolation function. The commonly used boundary conditions are as follows:
(1) Natural boundary conditions. The second order derivative at the two endpoints is zero, namely:
(2) Clamped boundary conditions. The first order derivative at the first and last node is specified, namely:
(3) Non-twisting boundary conditions. The third order derivatives of the splines at the first node are the same as at the second node. The third order derivatives at the last node are also equal to it at the penultimate node, namely:
4.2. PRM Optimization Algorithm Combined with Cubic Spline Interpolation
On the optimal path searched by the bidirectional PRM algorithm, interpolation algorithm is carried out according to the position of random path nodes. Spline interpolation is distributed interpolation in the searched node set. The specific algorithm process is as follows: (1)Store key nodes. The first step in our algorithm is to build a txt file, which is used to store each inflection point in the PRM path, namely, the key node. Store these key nodes in the form of a file to facilitate later process of spline interpolation algorithm(2)Presuppose sampling point. The linspace function is used to generate the linear spacing vector, and the N-point row vector between and is generated. represents the number of samplings point between two sampling points. In this paper, the sampling point is set to 50 at the beginning. Then, the sampling points are set to 100 and 150, respectively. The optimal combination of PRM algorithm and bidirectional PRM algorithm will be confirmed according to different data information obtained and simulation results(3)Ensure Spline interpolation obtains a cubic polynomial by analyzing the function equation. The cubic polynomial will measure discrete data breakpoints (, ) in the manner of approximations. According to the theorem, it can be known that only one line can be determined between two points. However, it is not limited to one for the cubic polynomial. Therefore, in this paper, apply non-twisted boundary conditions to obtain the exact expression of the cubic polynomial. The cubic spline interpolation is used to calculate the value of the unitary function determined by vector and at the interpolation point . Confirm the value of after finding . Match each column of matrix with and get interpolation point function value matrix(4)Set the return value. Return the coefficient matrix of piecewise spline polynomial determined by vector and for the calculation of other commands, and then draw the function curve after cubic spline interpolation
From the above path optimization process, it can be seen that the number of turning points of the smoothed path is less, and the path is smoother, which can reduce the turning times of the mobile robot in navigation, make the mobile robot move more flexible in the working environment, reduce the loss of running time and energy consumption of the mobile robot, and improve work efficiency.
In this paper, the flow diagram is used to show the whole optimization process according to the path optimization method of bidirectional PRM described above and the interpolation method of integrating cubic spline, as shown in Figure 5.
As can be seen from the above flow chart, in the path optimization process of cubic spline bidirectional PRM algorithm, firstly, initial work is carried out to create point set and path set . Then, the bidirectional PRM search path is used, that is, the positive direction and the negative direction are alternately searched. The path points are connected in turn, and the path connection points are added to the path set. When the middle coincident nodes are searched in both positive and negative directions, the optimal path has been found, otherwise continue the cycle. Finally, according to the path set obtained by bidirectional PRM, the four steps of the above interpolation algorithm are used to smoothen the path and obtain the final path.
5. Simulation and Experimental Verification
5.1. Simulation Verification
In this paper, adopt a computer with a processor of Rui Long R4800H, a display card of RTX2060, and a memory size of 16GB to carry out simulation experiments in order to verify the feasibility of the bidirectional PRM and the effectiveness of the cubic spline smoothing curve. The operating system is Windows 10, and the simulation platform is Matlab2020. The experimental results of PRM algorithm, bidirectional PRM, and bidirectional PRM with cubic spline interpolation are simulated, respectively. In the simulation process, use different grid maps to carry out simulation experiments to ensure that different positions of obstacles can also bring about the comparative effect of experimental data, so as to make the experimental results more convincing.
In this paper, three different map environments are selected for simulation experiments, as shown in Figures 6–8, where the map size is . Coordinate (10, 10) is the start node of the mobile robot, and coordinate (490, 490) is the target node. The black part is the obstacle part, and the white part is the free action area of the mobile robot. The broken line is the path diagram without the fusion of cubic spline interpolation. The curve is the path diagram with the introduction of cubic spline interpolation. The sampling point .
(a) PRM
(b) Bidirectional PRM diagram
(c) Cubic spline bidirectional PRM
(a) PRM
(b) Bidirectional PRM diagram
(c) Cubic spline bidirectional PRM
(a) PRM
(b) Bidirectional PRM diagram
(c) Cubic spline bidirectional PRM
The above simulation results show that the search time and path length of three algorithms are different. Therefore, based on a large number of simulation experiments, this paper makes data statistics on the time and path length obtained by the three algorithms, and takes the average as a reference. The specific data is shown in Table 1.
The search time is set as the running time of the algorithm, which can be determined by time function in MATLAB. Set the path length unit as the map size unit step size. In Table 1, the bidirectional PRM is optimized compared with the traditional PRM in time of the average search and in length of the shortest path. The data show that the average search time of bidirectional PRM is shortened by 24%, 10%, and 31% compared with traditional PRM, and the shortest path optimization is improved by 2%, 2%, and 4%. That exhibits the improvement of the bidirectional PRM is not obvious in the path length, but the search efficiency has been greatly improved. The time optimization results of 31% and path optimization results of 4% in map 3 show that the bidirectional PRM has more obvious effect with the increase of map complexity. Similarly, the cubic spline bidirectional PRM algorithm has improved the average search time and the shortest path length compared with the traditional PRM algorithm. From map 1 to map 3, the average search time is reduced by 14%, 9%, and 28%, and the shortest path is optimized by 1%, 4%, and 12%. Although the bidirectional PRM with cubic spline inevitably loses a certain search time, it has obvious improvement compared with the traditional PRM algorithm, as shown in Figures 6(c), 7(c), and 8(c). Smooth optimization makes the broken route smoother, so that the mobile robot walking will be safer and more stable.
The value of sampling point is crucial to the search efficiency of the algorithm. At present, the setting of this parameter is basically determined by experimental data and empirical values. In this paper, the values of are set to 50,100, and 150, respectively, to obtain the experimental data of the improved algorithm path finding time and path length, so as to determine the optimal value of . The specific experimental data are shown in Table 2.
In Table 2, the path length decreases with the increase of , but the corresponding search time increases due to the increase of calculation amount in the search process. After selecting the appropriate , it can be clearly seen that the original broken line path will be smooth, and the smooth curve is helpful for the path walking of the mobile robot. In this paper, a large number of experimental results show that when , the bidirectional PRM has the highest profit ratio when searching the optimal path.
In summary, the bidirectional PRM has faster convergence rate, higher path planning efficiency than traditional PRM. More, the obtained path quality is the best. After introducing the cubic spline algorithm, the path is smoothed, so that the actual walking of the mobile robot is more stable and safer.
5.2. Experimental Validation
In order to verify the effectiveness of the improved algorithm and smoothing process, the experiment applies the bidirectional PRM and the smoothing processing algorithm after inserting the cubic spline to the actual robot navigation based on ROS. The mobile robot used in the experiment is a four-wheel walking wheeled robot with two rear wheels and two front wheels. The rear wheel is the driving wheel and the front wheel is the passive wheel. Figure 9 shows:
In this paper, a complete mobile robot control platform is built, which is mainly composed of laser radar, core control board STM32F103ZET6 (hereinafter referred to as STM32), grating encoder, motor driver, brushless DC motor, battery module, front and rear wheels, and multiple interfaces. ROS sends out the encoder query instruction. The STM32 transmits the encoding information to ROS by serial communication. According to the information, ROS sends the topic of/cmd_vel to help feature pack subscribe topic information, then sends the speed instruction to STM32. After receiving the instruction, STM32 sends it to the DC brushless motor driver in the form of PWM pulse wave. The rotation of the motor is controlled by the voltage of the driver. After the motor rotates, the grating encoder will work. Pulse signal obtained by the information of the rotation is transmitted to STM32, which is integrated into the encoding information for ROS query. This loop realizes autonomous navigation of mobile robot. The overall structure is shown in Figure 10.
In Figure 10, Industrial Personal Computer (IPC) is the decision-making unit. As the upper computer, IPC carries all instructions send and receive by ROS. STM32 is connected to IPC through the USB line. Upper computer sends control and query instructions to STM32 and STM32 can also send encoder values and corresponding messages to upper computer.
In order to ensure the reliability of experimental data, this paper will take the laboratory map as the working environment. The traditional PRM, the bidirectional PRM, and the cubic spline bidirectional PRM are, respectively, carried out the physical experiment. The results are shown in Figures 11 and 12.
(a) PRM
(b) Bidirectional PRM diagram
(c) Cubic spline bidirectional PRM
(a) PRM
(b) Bidirectional PRM diagram
(c) Cubic spline bidirectional PRM
The actual path walking data of mobile robots are shown in Tables 3 and 4, where .
Set the path length unit in the table to the map size unit step. The search time is the algorithm running time and can be determined using a time function in the ROS script. The walking time is the time taken by the robot from the starting point to the end point. Tables 3 and 4 are the experimental data comparison of traditional PRM, the bidirectional PRM, and the cubic spline bidirectional PRM in the case of same environment and same conditions. In Table 3, in the case of target node 1, compared with traditional PRM, the path length of the bidirectional PRM is shortened by 8.5%, and the pathfinding time and walking time are reduced by 27.9% and 9.7%, respectively. The path length of the cubic spline bidirectional PRM is shortened by 19.0%, and the path searching time and walking time are reduced by 12.5% and 15.3%, respectively. Table 4 shows that in the case of target node 2, compared with PRM algorithm, the path length of the bidirectional PRM is reduced by 6.3%, and the pathfinding time and walking time are reduced by 29.7% and 4.7%, respectively. The path length of cubic spline bidirectional PRM is shortened by 12.5% and the efficiency of path finding time and walking time are improved by 8.3% and 13.7%, respectively.
According to the above analysis, compared with traditional PRM, the path length searched by cubic spline PRM is better, and the path search time and robot walking efficiency are higher. Therefore, the optimization algorithm in this paper is an effective and feasible path planning algorithm.
6. Conclusion
In order to solve the shortcomings of the traditional PRM with slow convergence speed and large number of turning points in path optimization, a cubic spline bidirectional PRM algorithm is proposed in this paper. Firstly, the improved bidirectional PRM uses the alternate search method of positive and negative directions to save time and improve the convergence speed of the algorithm. Secondly, the cubic spline interpolation method is integrated to improve the smoothness of the path and ensure that the mobile robot can work more safely. Finally, through the simulation experiment and the physical experiment, the results show that the cubic spline bidirectional PRM not only has a better execution time, but also has better path length than other methods. Moreover, the improved PRM can effectively solve the path planning task of mobile robot.
Data Availability
Some or all data, models, or code that support the findings of this study are available from the corresponding author upon reasonable request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported in part by the National Natural Science Foundation of China under Grant 61472282; in part by the University Natural Science Research Project of Anhui Province under Grant KJ2019A0065; in part by Major Special Project of Science and Technology in Anhui Province under Grant 202003a05020028; and in part by Anhui Province Key Laboratory of Special Heavy Load Robot under Grant ZJQR004-2020.