|
|
1.INTRODUCTIONWith the continuous development of automated driving technology, the control research of intelligent vehicles is gradually becoming a research hotspot. Path planning is regarded as a very important task of autonomous driving technology, which directly affects the safety of vehicle operation[1]. Path planning is the process of finding a collision-free path from the starting point to the target point when the map environment is known or partially known. It can be divided into two categories : global and local path planning. The commonly used path planning methods are : graph search algorithm[2], artificial potential field method[3], intelligent algorithm[4] and deep learning algorithm[5]. RRT* is a path planning method based on sampling method. Compared with the traditional path planning algorithm, RRT*algorithm has the advantages of complete probability[6] and easy implementation. However, the path generated by this method is often tortuous and non-optimal. The path has the problems of long planning time and low efficiency. In view of this, domestic and foreign scholars have made a lot of improvement work on the RRT method. In order to speed up the convergence rate, the RRT-Connect algorithm simultaneously establishes a random tree from the starting point and the target point and performs multi-tree bidirectional expansion[7]. The Bias-RRT algorithm[8] affects the convergence speed of the algorithm by generating the probability distribution of the random sample of the random tree, and adds the target point to the distribution with a higher probability. This method greatly improves the expansion speed of the algorithm to the target point. In order to obtain the optimal or near-optimal path, Reference[9] proposed the RRT * algorithm. The core is to select the optimal parent node and reconnect the two optimization processes, and then obtain the optimal solution through continuous iteration, but this algorithm still has the disadvantage of long calculation time. Informed RRT * algorithm is proposed in[10]. The algorithm considers that the simple RRT * scatters points randomly in the whole region, and the optimization effect is too scattered. In order to accelerate the convergence speed and improve the optimization accuracy, the concept of hypersphere is established to reduce the sampling range of nodes. In order to concentrate the optimized path points, the RRT*-SMART algorithm is proposed in[11], which limits the sampling range of nodes by selecting key nodes, thereby improving the speed of path planning. In[12], an improved RRT * FN algorithm is proposed. Based on the ellipsoid subset sampling, this method combines the target bias method for path iteration to improve the convergence accuracy of the algorithm. However, this method uses uniform sampling to cause a large number of iteration points. Redundancy, path iteration efficiency is still low. Reference [13] proposed a random tree passive growth method, which combined with the gravity function to improve the running speed of the algorithm. The Q-RRT* algorithm proposed in Reference [14] can greatly shorten the path length and achieve a sub-optimal path by increasing the selection range of parent nodes and skipping unnecessary redundant nodes in the path through triangular inequalities. On the whole, the current researchers have improved the RRT* method from the two dimensions of time and path length, and achieved good planning results. However, there are still problems of low efficiency of path planning and non-optimal path length. In view of this, this paper combines the particle swarm optimization method with the RRT* method, and combines the corner constraint to propose a mobile robot RRT* path planning method (PSO-RRT*) that combines particle swarm optimization and corner constraint. The proposed method is verified in many cases. The main innovations are : 2.PSO-RRT*2.1RRTThe RRT algorithm is a path planning algorithm that can quickly search the state space. Firstly, the random point xrand is obtained by the random sampling function Sample(). Using Nearest(V, xrand) search, the nearest point xnearest to xrand is obtained, and xnew can be obtained by fixed step size. If there is no collision with obstacles between xnew and xnearest, find the neighborhood Vnear of xnew, and then optimize the path through the ChooseParent (Vnear, xnew) and Rewire (Vnear, xnew) functions. The iterative optimization process gives the RRT * algorithm asymptotic optimality. The pseudo-code of the RRT * algorithm is shown in Algorithm 1 : 2.2PSOPSO algorithm[15] was proposed by Kennedy in 1995. PSO algorithm mainly searches the global optimal value of the objective function by imitating the foraging behavior of birds. In the standard particle swarm optimization algorithm, it is assumed that the particle swarm size is m, each particle has an n-dimensional search area, xi=(xi1,xi2,xi3,…,xin) is the search position of the i-th particle in space, vi=(vi1,vi2,vi3,…,vin) is the speed of the i-th particle, which represents the moving distance of the particle in each position update, pi=(pi1,pi2,pi3,…,pin) records the search optimal value of the ith particle. pg=(pg1,pg2,pg3,…,pgn) records the optimal particle position in the current population. The traditional PSO algorithm updates the position and velocity of the particles by the following two mathematical Eq. 1 and Eq.2. In the Eq. 1 and Eq.2, w is the inertia weight factor; c1 and c2 are learning factors; R is a random number between 0 and 1; when vij(t+1) is the number of iterations of t + 1, the j-dimensional velocity component of the i-th particle; xij(t+1) is the j-dimensional position component of the i-particle at the number of t+1 iterations. pij(t) is the j-dimensional optimal position component of the i-th particle at the t-th iteration; pgj(t) The j-dimensional position component is obtained by the optimal solution in the population during the t-th iteration; where, 1≤i, g≤m, 1≤j≤n. 2.3Sampling under target and obstacle constraintsIn view of the randomness and blindness of the sampling points obtained by the RRT* algorithm, the search efficiency of the algorithm is low. Therefore, this paper uses the search method shown in Fig.1(a) to complete the acquisition of sampling points. First, the current parent node xparent is obtained, and the node and the target point (Goal) are connected. Then, the parent node xparent and the two sides of the obstacle are tangented to obtain the line segments xparent-xob1 and xparent-xob2. The region xparent-xob1-xob2 is a forbidden search region. Scan along xparent-xob1 and xparent-xob2 to both sides respectively, and the scanning search probability gradually decreases. Finally, the next sampling point is obtained. 2.4Corner RestrictionAs shown in Fig.1(b), the path point selection method under the fusion corner constraint is proposed in this paper x1, x2 and x3 are the path points of the previous moment, the current moment and the next moment, respectively. When selecting the path point x3, the position of the path point at the first two moments is considered, and the rotation angle ϕ is less than the minimum rotation angle of the mobile robot. 2.5PSO-RRT*The pseudo-code of the calculation process of the PSO-RRT * method proposed in this paper is shown in Algorithm 2. Compared with the original RRT* method, the improved method mainly has the following functions.
3.SIMULATION VERIFICATION3.1Simulation Environment SettingsThe proposed method is verified in three simulation environments shown in Fig.2, and compared with RRT*, informed-RRT*,and Q-RRT* methods to effectively illustrate the effectiveness of the proposed method. In the simulation, each method was tested 10 times, and the average planning time(s), path length(m) and total angle were used to evaluate each method. It should be noted that simulation environment 3 is the superposition of simulation environment 2 and simulation environment 1. Relatively more complex. In each test, the starting point is set to (2,2) and the end point is set to (490,370). The parameters of the PSO-RRT* method in the simulation process are set as follows : the particle swarm size is 30, c1 and c2 are 0.8, and the minimum rotation angle of the robot is set to 60 degrees. The programming language used in this paper is Python3.7, the computer configuration is : 64-bit Window10, processor Intel (R) i5-9600, memory 16GB. 3.2Simulation environment 1The test results in simulation environment 1 are shown in Fig.3. The results in the figure show that compared with the other methods, the path obtained by the PSO-RRT * method is relatively more stable and the number of turns is less. In terms of path length, the Q-RRT * method has the worst effect, followed by the informed-RRT * method. The planning time and path length in the simulation environment are statistically analyzed, and the results are shown in Table xxx. The results in the table show that the PSO-RRT * method proposed in this paper achieves the optimal value in terms of planning time, path length and total rotation angle. Specifically, the planning time of PSO-RRT* is 37.2% shorter than that of the sub-optimal informed-RRT * method. In terms of path length, it is 9.7% shorter than the RRT * method. From the perspective of the total rotation angle, the rotation angle is reduced by 24.1%. The comparison results further reflect the advantages of the proposed method, indicating that the proposed method has advantages in improving planning efficiency and reducing planning path length. In order to further illustrate that the method in this paper has a stable advantage, as shown in the Fig.4 is the result of the path length and the total rotation angle. The results show that the method in this paper is more stable. Table 1Comparison of planning results of various methods
3.3.Simulation environment 2The test results in simulation environment 2 are shown in Fig.5. The results in the figure show that the path obtained by the PSO-RRT * method is relatively more stable and the number of turns is less than other methods. From the perspective of path length, the Q-RRT * method has the worst effect, followed by the informed-RRT * method. The planning time and path length in the simulation environment are statistically analyzed, and the results are shown in Table xxx. The results in the table show that the PSO-RRT * method proposed in this paper has reached the optimal value in terms of planning time, path length and total rotation angle. Specifically, the planning time of PSO-RRT * is 5.6 % shorter than that of the suboptimal informed-RRT * method. From the perspective of path length, it is shortened by 6.6 % compared with the RRT * method. From the perspective of the total rotation angle, the rotation angle is reduced by 34.8 %. The comparison results further reflect the advantages of the proposed method, indicating that the proposed method has advantages in improving planning efficiency and reducing planning path length. In order to further illustrate that the method in this paper has a stable advantage, as shown in the Fig.6 is the result of the path length and the total rotation angle. The results show that the method in this paper is more stable. It also further reflects the advantages of this method. Table 2Comparison of planning results of various methods
3.4Simulation environment 3In the simulation environment 3, the simulation environment 1 and the simulation environment 2 are mixed, and the test results are shown in Fig.7. The results in the figure show that the path obtained by the PSO-RRT * method is relatively more stable and the number of turns is less, as the results of simulation environment 1 and simulation environment 2. The planning time of PSO-RRT* is 11.1% shorter than that of the sub-optimal informed-RRT * method. From the perspective of path length, it is shortened by 4.3% compared with the RRT * method. From the perspective of the total rotation angle, the rotation angle is reduced by 20.7%. The comparison results further reflect the advantages of the proposed method, indicating that the proposed method has advantages in improving planning efficiency and reducing planning path length. It is also further illustrated that the method in this paper has stable advantages, as shown in the Fig.8, which is the result of path length and total rotation angle. The results show that the method in this paper is more stable. It also further reflects the advantages of this method. Table 3Comparison of planning results of various methods
4.EXPERIMENTAL VERIFICATION OF MOBILE ROBOTIn order to further illustrate the practicability of this method, the experimental verification is carried out on the mobile robot shown in Fig.9(a). The mobile robot uses raspberry pie as the main control system, Fig.9(b). In the verification process, the planned path is first smoothed by B-spline curve. Then, the MPC method is used for trajectory planning to obtain the rotational speed of the robot wheel. Finally, the actual test is carried out. This experiment was tested in a simple laboratory environment as shown in Fig10.(a). The results of the planning are shown in Fig.10(b). The red curve in the Fig.10(b) is the original planning result, and the black curve is the result of smoothing with the B-spline curve. 1,2,3,4 are the position points of the capture robot at this position, as shown in Fig.10(c),(d),(e),(f) The results show that the PSO-RRT* method can complete the path planning task well. After using the B-spline curve smoothing, the path tracking of the mobile robot can be well realized based on the predictive control method. And the whole process has no collision behavior, which further verifies the advantages and applicable value of this method. 6.6.REFERENCESFang Lijin, Wu Zhenghan, Wang Huaizhen,
“Multi scene motion planning of mechanical arm based on improved RRT *FN algorithm [J],”
China Mechanical Engineering, 32
(21), 2590
–2593-2594
(2021). Google Scholar
Elhoseny, Mohamed, Tharwat, et al.,
“Bezier Curve Based Path Planning in a Dynamic Field using Modified Genetic Algorithm[J],”
Journal of computational science, 25
(Mar), 339
–350
(2018). https://doi.org/10.1016/j.jocs.2017.08.004 Google Scholar
Wang H D, Hao C, Zhang P, Zhang M Q, Yin P H, Zhang Y S.,
“Path planning for mobile robot based on A~* algorithm and artificial potential field method [J],”
China mechanical engineering, 30
(20), 2489
–2496
(2019). Google Scholar
Kang Y X,Jiang C Y,Qin Y H,Ye C L,
“Robot path planning and experiment based on improved PSO algorithm [J],”
Robot, 42
(01), 71
–78
(2020). Google Scholar
Xiong J T, Li Z X, Chen S M, Zheng Z H.,
“Transactions of the Chinese Society for Agricultural Machinery,”
51
(S02), 10
(2020). Google Scholar
KAPAMAN S,FRAZZOLI E.,
“Sampling-based Algorithms for Optimal Motion Planning [J],”
The International Journal of Robotics Research, 30
(7), 846
–894
(2011). https://doi.org/10.1177/0278364911406761 Google Scholar
Yechen Li;Shaochun Ma,
“Navigation of Apple Tree Pruning Robot Based on Improved RRT-Connect Algorithm[J],”
Agriculture, 13
(1495), 1495
(2023). https://doi.org/10.3390/agriculture13081495 Google Scholar
Urmson C, Simmons R.,
“Approaches for heuristically biasing RRT growth[C],”
in Intelligent Robots and Systems, 2003. (IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on. IEEE,
(2003). https://doi.org/10.1109/IROS.2003.1248805 Google Scholar
Karaman S, Frazzoli E.,
“Sampling-based Algorithms for Optimal Motion Planning[J],”
The International Journal of Robotics Research, 30
(7), 846
–894
(2011). https://doi.org/10.1177/0278364911406761 Google Scholar
Gammell J D, Srinivasa S S, Barfoot T D.,
in Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems,
2997
–3004
(2014). Google Scholar
Nasir J, Islam F, Malik U, et al.,
“RRT*-SMART: A rapid convergence implementation of RRT[J],”
International Journal of Advanced Robotic Systems, 10
(7), 299
(2013). https://doi.org/10.5772/56718 Google Scholar
Fang Lijin, Wu Zhenghan, Wang Huaizhen,
“Multi scene motion planning of mechanical arm based on improved RRT *FN algorithm [J],”
China Mechanical Engineering, 32
(21), 2590
–2593-2594
(2021). Google Scholar
Li Y, Xu D.,
“Cooperative Path Planning for Dual-arm Robot Based on Gravity Adaptive Step Size RRT [J],”
The robot, and, 2020
(5), 606
–616 Google Scholar
Jeong I B, Lee S J, Kim J H.,
“Quick-RRT*: Triangular inequality-based implementation of RRT* with improved initial solution and convergence rate[J],”
Expert Systems with Applications, 123 82
–90
(2019). https://doi.org/10.1016/j.eswa.2019.01.032 Google Scholar
Kennedy J, Eberhart R.,
“Particle swarm optimization[C],”
in IEEE International Conference on Neural Networks. Piscataway,USA: IEEE,
1942
–1948
(1995). Google Scholar
|
