3. Improved RRT\* motion planning algorithm for mobile robot

Some path planning algorithms such as ant colony algorithm, genetic algorithm, artificial potential field algorithm, and dealing with some planning have its unique superiority. However, in the complex environment and high-dimensional space, the complex of the algorithm will increase sharply, which lead the convergence time too long to solve the problem. Because the algorithm based on potential field or heuristic function does not consider the kinematic and dynamic constraints, its result cannot be executed by the real case [12–17].

In order to solve the problem of high-dimensional path planning, the sampling algorithms have been introduced [18]. Compared with another advanced algorithm, the main advantage of the algorithm based on sampling is avoiding constructing the explicit configuration space, and it has been shown to be an effective solution to the path planning [19]. RRT algorithm based on sampling with rapid convergence rate can be used in unknown obstacle environment [20]. However, there are still some shortcomings in RRT algorithm [21–23]:


The domestic and foreign scholars have been studied on these deficiencies and proposed various RRT algorithms to adapt to different application scenarios. In order to improve the efficiency of the expansion of the node, Kuffner and La Valle proposed the RRT-connect [24]. Karaman and Frazzoli first proposed RRT\* algorithm, which convergence to the optimal solution [25]. A.H. Qureshi and S. Mumtaz proposed the TG-RRT\*, using triangular geometry to select node, in order to reduce the number of iterations required for the optimal solution, so as to make the algorithm rapid convergence [26]. C. Wouter Bac and Tim Roorda proposed by RRT algorithm on ROS platform for mobile robot path planning research and applied to the motion planning problem in the dense obstacles environment [27].

#### 3.1. Nonholonomic constraints for mobile robot

The integrity constraints include a system of generalized coordinates derivative and integral constraints. The mobile robot is a typical nonholonomic constraint system. The mobile robot's simplified moving model is shown in the Figure 6.

Mobile robot's state variables in configuration space <sup>x</sup>; <sup>y</sup>; <sup>θ</sup>; <sup>v</sup>; <sup>ϕ</sup> � �, let ð Þ <sup>x</sup>; <sup>y</sup> represent the center of mobile robot rear axle in the system coordinates, θ represent the angle between the forward direction of the mobile robot and the x-axis, φ represent the angle between the forward direction of the mobile robot and the front wheel direction, v as the speed of the mobile robot, due to the nonholonomic constraints, the wheel is point contact with the ground, and only pure roll no relative sliding at contact.

$$\begin{cases} \dot{\mathbf{x}} = v \cos \theta \\ \dot{y} = v \sin \theta \\ \dot{\theta} = v \tan \phi /\_{L} \\ \dot{v} = u\_{0} \\ \dot{\phi} = u\_{1} \end{cases} \tag{4.1}$$

And the constraint equations:

3. Improved RRT\* motion planning algorithm for mobile robot

Table 2. Algorithm implementation time and field strength.

156 Advanced Path Planning for Mobile Entities

and dynamic constraints, its result cannot be executed by the real case [12–17].

[20]. However, there are still some shortcomings in RRT algorithm [21–23]:

1. Its convergence rate is very slow in achieving the optimal solution;

determining an optimal path much more rapid.

calculate the optimal path;

Some path planning algorithms such as ant colony algorithm, genetic algorithm, artificial potential field algorithm, and dealing with some planning have its unique superiority. However, in the complex environment and high-dimensional space, the complex of the algorithm will increase sharply, which lead the convergence time too long to solve the problem. Because the algorithm based on potential field or heuristic function does not consider the kinematic

In order to solve the problem of high-dimensional path planning, the sampling algorithms have been introduced [18]. Compared with another advanced algorithm, the main advantage of the algorithm based on sampling is avoiding constructing the explicit configuration space, and it has been shown to be an effective solution to the path planning [19]. RRT algorithm based on sampling with rapid convergence rate can be used in unknown obstacle environment

2. Its memory requirements are significantly large due to a large number of iterations used to

3. The rejection of samples, which might not be connectable directly with the existing nodes in the tree, but may lie closer to the goal region and hence could aid the algorithm in

The domestic and foreign scholars have been studied on these deficiencies and proposed various RRT algorithms to adapt to different application scenarios. In order to improve the efficiency of the expansion of the node, Kuffner and La Valle proposed the RRT-connect [24]. Karaman and Frazzoli first proposed RRT\* algorithm, which convergence to the optimal solution [25]. A.H. Qureshi and S. Mumtaz proposed the TG-RRT\*, using triangular geometry to select node, in order to reduce the number of iterations required for the optimal solution, so as to make the algorithm rapid convergence [26]. C. Wouter Bac and Tim Roorda proposed by

$$
\begin{pmatrix} dx/\_{dt} \\ dy/\_{dt} \\ d\phi/\_{dt} \\ d\boldsymbol{\phi}/\_{dt} \\ d\boldsymbol{\phi}/\_{dt} \end{pmatrix} = \begin{pmatrix} \boldsymbol{v}\cos\theta \\ \boldsymbol{v}\sin\theta \\ \boldsymbol{v}\tan\phi/\_{\boldsymbol{L}} \\ \boldsymbol{0} \\ \boldsymbol{0} \end{pmatrix} + \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \\ 0 \end{pmatrix} u\_0 + \begin{pmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} u\_1 \tag{4.2}$$

Figure 6. Geometrics of the mobile robot.

u<sup>0</sup> (acceleration) and u<sup>1</sup> (angular acceleration) of the mobile robot control variables. The minimum turning radius (maximum curvature) of the mobile robot is:

$$K\_{\text{max}} = \frac{\tan \phi\_{\text{max}}}{L} = \frac{1}{R\_{\text{min}}} \tag{4.3}$$

Xa

Xb

3.3. Experimental results

performance.

3.3.1. The algorithm simulation

see the I-RRT\* is better than the others.

Table 4. Experimental results for computing optimal path solution.

near ≔ v Va : v ∈Βxrand,r

near ≔ v Vb : v ∈Βxrand,r

In the case of both sets of near vertices being found empty, these sets are filled with the closest vertex from their respective trees instead. The procedure Best Selected Tree returns the nearest vertex on the Best Selected Tree, which is eligible to become the parent of the random sample. Hence, unlike the connect heuristic, the I-RRT\* is not greedy since the connection is only made inside the ball region. Finally, the tree connection generates the end-to-end global path.

This section presents simulations performed on a 2.4 GHz Intel Core i5 processor with 4 GB RAM. Here, performance results of our I-RRT\* algorithm are compared with RRT\* and B-RRT\*. For proper comparison, experimental conditions and size of the configuration space were kept constant for all algorithms. Since randomized sampling-based algorithms exhibit large variations in results, the algorithms were run up to 50 times with different seed values for each type of environment. Maximum, minimum, and an average number of iterations i as well as time t utilized by each algorithm to reach the optimal path solution is presented in Table 4. In the second environment simulation, chooses some representative to verify the algorithm

Figures 7–9 are three kinds of the optimization algorithm for the same problems. And we can

In the third environment model simulation test, using the three different kinds of obstacle environment simulation experiment, we tested the performance of the algorithm in Figure 10.

Environment ALG imin imax iavg tmin tmax tavg C fail 3D-Random obstacles RRT\* 107,810 110,851 108,965 17.8 19.3 18.8 \*\* 6

3D-Multiple barriers RRT\* 1,430,381 1,440,619 1,437,342 242.1 247.4 244.1 205.6 14

3D-Narrow passage RRT\* 1,277,376 1,301,698 1,290,674 216.9 221.9 218.5 345.1 9

B-RRT\* 30,901 38,086 36,128 6.4 8.1 7.4 \*\* 4 I-RRT\* 19,507 22,525 21,290 4.4 5.3 5.1 \*\* 1

B-RRT\* 495,961 503,240 498,972 102.1 106.5 105.1 205.6 6 I-RRT\* 97,885 112,857 111,139 22.3 27.3 26.2 205.6 3

B-RRT\* 533,276 561,347 551,771 110 117.1 115.9 345.1 3 I-RRT\* 127,363 143,806 134,421 30.3 35.2 31.4 345.1 0

(4.4)

Motion Planning for Mobile Robots http://dx.doi.org/10.5772/intechopen.76895 159

(4.5)

Therefore, it is unreasonable without considering the nonholonomic constraints of the robot planning algorithm.

#### 3.2. Description and implementation of the I-RRT\*

I-RRT\* algorithm showed in Table 3 is specifically designed for motion planning in complex, cluttered environments where exploration of configuration space is difficult. Let the sets of near vertices from the tree Ta and Tb be denoted by X<sup>a</sup> near and Xb near, respectively. The path is connecting Pa init and Prand is denoted by σ<sup>a</sup> <sup>0</sup> : <sup>0</sup>;sa ½ �, while the path is connecting <sup>P</sup><sup>b</sup> init and Prand is denoted by σ<sup>b</sup> <sup>0</sup> : ½ � 0;sb .

It starts by picking a random sample Prand from the obstacle-free configuration space Xfree that is, xrand ∈ Xfree. It then populates the set of near vertices, X<sup>b</sup> near for both trees using the NearVertices procedure. It should be noted that a ball region centered at Prand of radius r is formed, and the sets of the near vertices from both trees are computed that is:


Table 3. The algorithm of I-RRT\*.

$$X\_{new}^{a} \coloneqq \left\{ \upsilon \leftarrow V\_a : \upsilon \in \mathcal{B}\_{\chi\_{null}, r} \right\} \tag{4.4}$$

$$X\_{near}^b := \left\{ \upsilon \leftarrow V\_b : \upsilon \in \mathcal{B}\_{x\_{null}, r} \right\} \tag{4.5}$$

In the case of both sets of near vertices being found empty, these sets are filled with the closest vertex from their respective trees instead. The procedure Best Selected Tree returns the nearest vertex on the Best Selected Tree, which is eligible to become the parent of the random sample.

Hence, unlike the connect heuristic, the I-RRT\* is not greedy since the connection is only made inside the ball region. Finally, the tree connection generates the end-to-end global path.

#### 3.3. Experimental results

u<sup>0</sup> (acceleration) and u<sup>1</sup> (angular acceleration) of the mobile robot control variables. The

Therefore, it is unreasonable without considering the nonholonomic constraints of the robot

I-RRT\* algorithm showed in Table 3 is specifically designed for motion planning in complex, cluttered environments where exploration of configuration space is difficult. Let the sets of

It starts by picking a random sample Prand from the obstacle-free configuration space Xfree that

NearVertices procedure. It should be noted that a ball region centered at Prand of radius r is

<sup>L</sup> <sup>¼</sup> <sup>1</sup>

Rmin

near and Xb

<sup>0</sup> : <sup>0</sup>;sa ½ �, while the path is connecting <sup>P</sup><sup>b</sup>

(4.3)

near, respectively. The path is

near for both trees using the

init and Prand is

<sup>K</sup>max <sup>¼</sup> tan <sup>ϕ</sup>max

minimum turning radius (maximum curvature) of the mobile robot is:

3.2. Description and implementation of the I-RRT\*

near vertices from the tree Ta and Tb be denoted by X<sup>a</sup>

is, xrand ∈ Xfree. It then populates the set of near vertices, X<sup>b</sup>

formed, and the sets of the near vertices from both trees are computed that is:

init and Prand is denoted by σ<sup>a</sup>

<sup>0</sup> : ½ � 0;sb .

; Ea <sup>ϕ</sup>; Ta ð Þ Va; Ea ;

; Eb <sup>ϕ</sup>; Tb ð Þ Vb; Eb ;

NearVertices xð Þ rand; Ta; Tb

NearestVertex xrand; Ta ð Þ ; Tb

GetBestTreeParent L<sup>a</sup>

near ¼ φ then

<sup>s</sup>; L<sup>b</sup> <sup>s</sup>;Connection

near 

near 

near <sup>¼</sup> <sup>φ</sup> && <sup>X</sup><sup>b</sup>

<sup>s</sup> GetSortedList xrand; <sup>X</sup><sup>a</sup>

<sup>s</sup> GetSortedList xrand; Xb

15. Ta InsertVertex xð Þ rand; xmin; Ta 16. Ta RewireVertex xð Þ rand; xmin; Ta

18. Tb InsertVertex xð Þ rand; xmin; Tb 19. Tb RewireVertex xð Þ rand; xmin; Tb

22. return Ta ð Þ f g ; Tb ¼ V; E

Table 3. The algorithm of I-RRT\*.

planning algorithm.

158 Advanced Path Planning for Mobile Entities

connecting Pa

denoted by σ<sup>b</sup>

1. Va <sup>x</sup><sup>a</sup>

2. Vb xb

7. Xa

9. Xa

11. L<sup>a</sup>

12. L<sup>b</sup>

17. else

8. if X<sup>a</sup>

3. σ<sup>f</sup> ∞; E ϕ; 4. Connection True 5. for i 0 to N do 6. xrand Sample ið Þ

near; X<sup>b</sup> near

near; X<sup>b</sup> near

10. Connection False

13. xmin; flag; σ<sup>f</sup>

14. if flag ð Þthen

20. E Ea∪Eb 21. V Va∪Vb

init

init

#### 3.3.1. The algorithm simulation

This section presents simulations performed on a 2.4 GHz Intel Core i5 processor with 4 GB RAM. Here, performance results of our I-RRT\* algorithm are compared with RRT\* and B-RRT\*. For proper comparison, experimental conditions and size of the configuration space were kept constant for all algorithms. Since randomized sampling-based algorithms exhibit large variations in results, the algorithms were run up to 50 times with different seed values for each type of environment. Maximum, minimum, and an average number of iterations i as well as time t utilized by each algorithm to reach the optimal path solution is presented in Table 4.

In the second environment simulation, chooses some representative to verify the algorithm performance.

Figures 7–9 are three kinds of the optimization algorithm for the same problems. And we can see the I-RRT\* is better than the others.

In the third environment model simulation test, using the three different kinds of obstacle environment simulation experiment, we tested the performance of the algorithm in Figure 10.


Table 4. Experimental results for computing optimal path solution.

utilizes the partial greedy heuristic approach as discussed earlier, which significantly reduces its ability of convergence to the optimal path solution. Table 4 shows the convergence from the initial path solution to the optimal path solution by I-RRT\* and RRT\*, respectively. For determination of the optimal path, the I-RRT\* algorithm takes the least number of average iterations (iavg = 111,139) as compared to B-RRT\*(iavg = 498,972) and the extraordinarily large number

Motion Planning for Mobile Robots http://dx.doi.org/10.5772/intechopen.76895 161

The simulation of the environment was established under the gazebo. In building a model, a mobile robot called ROS mapping packages build implementation scenario map in Figures 11–13.

Figure 11. The process of SLAM. (a) SLAM show under gazebo (b) SLAM show under RVIZ.

Figure 12. Map information in RVIZ.

of iterations taken up by RRT\*(iavg = 1,437,342).

3.4. Mobile robot simulation experiment on ROS

Figure 7. RRT\* performance in 2-D environment.

Figure 8. B-RRT\* performance in 2-D environment.

Figure 9. I-RRT\* performance in a 2-D environment.

Figure 10. I-RRT\* performance in 3-D environment.

To restrain the computational time within reasonable limits, the maximum limit for the number of tree nodes was kept at 3 million.

Although both I-RRT\* and B-RRT\* were successful in finding the optimal solution, B-RRT\* took an extremely large number of iterations to converge in comparison with I-RRT\*. B-RRT\* utilizes the partial greedy heuristic approach as discussed earlier, which significantly reduces its ability of convergence to the optimal path solution. Table 4 shows the convergence from the initial path solution to the optimal path solution by I-RRT\* and RRT\*, respectively. For determination of the optimal path, the I-RRT\* algorithm takes the least number of average iterations (iavg = 111,139) as compared to B-RRT\*(iavg = 498,972) and the extraordinarily large number of iterations taken up by RRT\*(iavg = 1,437,342).

## 3.4. Mobile robot simulation experiment on ROS

The simulation of the environment was established under the gazebo. In building a model, a mobile robot called ROS mapping packages build implementation scenario map in Figures 11–13.

Figure 11. The process of SLAM. (a) SLAM show under gazebo (b) SLAM show under RVIZ.

Figure 12. Map information in RVIZ.

To restrain the computational time within reasonable limits, the maximum limit for the num-

Although both I-RRT\* and B-RRT\* were successful in finding the optimal solution, B-RRT\* took an extremely large number of iterations to converge in comparison with I-RRT\*. B-RRT\*

ber of tree nodes was kept at 3 million.

Figure 10. I-RRT\* performance in 3-D environment.

Figure 7. RRT\* performance in 2-D environment.

160 Advanced Path Planning for Mobile Entities

Figure 8. B-RRT\* performance in 2-D environment.

Figure 9. I-RRT\* performance in a 2-D environment.

Author details

References

6.2000-4370

2015;42(12):5177-5191

Xiangrong Xu\*, Yang Yang and Siyu Pan

\*Address all correspondence to: xuxr88@yahoo.com

pore: McGrawHill. Inc; 1987. ISBN:0070226253

sium on Industrial Electronics, Pusan. 2001. pp. 1530-1535

on Intelligent Control Piscataway: IEEE. 1993. 536-541

nonlinearities. Automatica. 1998;34(4):505-508

2008, Vol. 7135. DOI: 10.2514/6.2008–7135

School of Mechanical Engineering, Anhui University of Technology, Ma'anshan, Anhui, China

Motion Planning for Mobile Robots http://dx.doi.org/10.5772/intechopen.76895 163

[1] Fu KS, Gonzales RC, Lee CS. Robotics: Control, Sensing, Vision, and Intelligence. Singa-

[2] Jung D, Tsiotras P. On-line path generation for small unmanned aerial vehicles using B-spline path templates. AIAA Guidance, Navigation and Control Conference, AIAA.

[3] Chandler P, Rasmussen S, Pachter M. UAV cooperative path planning. AIAA Guidance, Navigation, and Control Conference and Exhibit. 2000. pp. 1255-1265. DOI: 10.2514/

[4] Oscar M, Ulises O, Roberto S. Path planning for mobile robots using bacterial potential field for avoiding static and dynamic obstacles. Expert Systems with Applications. July

[5] Park M, Jeon J, Lee M. Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing. Proceedings of the 2001 IEEE International Sympo-

[6] Kitamura Y, Tanaka T, Kishino F. 3-D path planning in a dynamic environment using an octree and an artificial potential field. Proceedings of the 1995 IEEE International Confer-

[7] Janabi S, Vinke D. Integration of the artificial potential field approach with simulated annealing for robot path planning. Proceedings of the 1993 IEEE International Symposium

[8] Hsu CK. Variable structure control design for uncertain dynamic systems with sector

[9] Ashiru I, Czarnecki C, Routen T. Characteristics of a genetic based approach to path planning for mobile robots. Journal of Network and Computer Applications, 1996;19(2):149-169

[10] Chen L, You B. Dynamic robot motion tracking and obstacle avoidance based on artificial

potential field method. Control Theory and Application. 2007;26(4):8-10

ence on Intelligent Robots and Systems. Piscataway: IEEE; 1995. 2474-2481

Figure 13. The process of planning. (a) Start planning, (b) the process of planning, and (c) planning complete.
