AMC2014-Yokohama March 14-16, 2014, Yokohama, Japan

Triangular Geometry based Optimal Motion

Planning using RRT*-Motion Planner

1,2 Ahmed Hussain Qureshi, 1,2Saba Mumtaz, 1 Khawaja Fahad Iqbal, 1 Yasar Ayaz, 3Mannan Saeed Muhammad, 20sman Hasan, 3Whoi Yul Kim and 3Moonsoo Ra

1 Robotics And Intelligent Systems Engineering

(RISE) Lab, SMME, National University of

Sciences And Technology. Islamabad, Pakistan.

2 School of Electrical Engineering And

Computer Sciences (SEECS), National

University of Sciences And Technology.

3Department of Electronic

Engineering, College of

Engineering, Hanyang University

Seoul, South Korea Email: 10beeaqureshi@seecs.edu.pk Islamabad, Pakistan.

Abstract-RRT* is a recent and improved variant of the RRT path finding algorithm. While RRT concentrates on simply finding an initial obstacle-free path, RRT* guarantees eventual convergence to an optimum, collision-free path for any given geometrical environment. On the other hand, the main limitations of RRT* include its slow processing rate and high memory utilization due to the large number of iterations required to achieve optimal path solution. This paper presents Triangular Geometerised-RRT* (TG-RRT*) which incorporates Triangular geometrical methods in the RRT* algorithm and improves its processing time by decreasing the number of iterations required for optimal solution. Simulation results under

different environments demonstrate an improved convergence rate of TG-RRT*, in comparison with RRT*.

Index Terms - RRT*, Sampling based Optimal Motion Planning, Directed Sampling.

I. INTRODUCTION

The motion planning problem relates to the dynamic navigation of a robot from a given starting point to a chosen goal point while avoiding collisions with any obstacles that may occur in its path. This fundamentally important problem of automatic motion planning for any mobile object, surrounded by obstacles and moving from one point to another, has numerous applications in the fields of animation [1] and robotics [2]. Due to its widespread application, different techniques for motion planning such as neural networks [3], sampling based algorithms [4] , potential field methods [5] etc. have been proposed. In the effort of finding efficient solutions for practical motion planning problems, extensive research has been devoted to exploring incremental sampling based algorithms, like Rapidly-exploring Random Trees (RRT) [6] and the Probabilistic Road Map (PRM) [4]. These probabilisticaly complete algorithms generate a solution, provided that one exists, irrespective of an obstacle's geometry in the configuration space. In these algorithms, the probability of finding a path solution in a given environment is one, if number of iterations utilized are allowed to approach infinity. Rapidly exploring random trees quickly find their initial path in a given problem, and due to their effectiveness, have been improved in number of ways [7], [8], [9], [10], [11], [12]. Most recent advancement of RRTs algorithm is the RRT*, which ensures asymptotic optimality [12], unlike

978-1-4799-2323-6114/$3l.00 ©2014 IEEE 380

RRT. Just like the RRT algorithm, RRT* finds its initial path very quickly and then proceeds onwards to return optimal or near optimal path towards the goal as the number of iterations approach infinity. These additional capabilities make RRT* quite useful for applications running in real time [13]. However, problems of slow convergence to optimum path still exist in RRT*. The main scope of this paper is the Triangular Geometrised­RRT* (TG-RRT*) algorithm, which utilizes basic triangular geometrical techniques, like triangular in-center and centroid to reduce the number of iterations required to reach optimum path solution. Where RRT* [I2] samples the whole given configuration space, TG-RRT* reduces dispersion of random samples by bringing them towards the center of the triangle formed by the initial, goal and random nodes. Therefore, incorporating triangular geometry in RRT* makes it a rapidly converging and memory efficient algorithm. These features allow the TG-RRT algorithm to significantly reduce the required number of iterations.

The rest of the paper is organized as follows. Section II explains the RRT* motion planning algorithm in detail as well as the triangular geometric methods utilized in this paper to improve RRT* performance. Sections III explains the working of our Triangular Geometrised (TG-RRT*) algorithm. Simulation results of TG-RRT* in different environments are presented in Section IV. Finally, Section V concludes the paper besides providing some future areas of research in this domain.

II. RELATED WORK

A. RRT*

The RRT* [13], [I 2] motion planning algorithm generates an input u : [0, T] E U. Applying this input signal to any dynamic system represented by x = f(x(t),u(t)), outputs an optimal path solution that directs a robot from an initial starting point, Xinit E X, to a goal point, Xgoal E X. The time taken to move between the two points is t E [0, T]. The configuration space is denoted by X E �d while U E �rn is used to represent the input space. Xobs E X is the region in the configuration space taken up by obstacles while the

X configuration region free of obstacles is Xjree = -­Xobs

The RRT* algorithm utilizes the following functions for its operation: Random Sampling: This function picks random samples from the obstacle free configuration space X free E X and assigns them to the random state variable Zrand E Xfree . Distance: This function returns the Euclidean distance between two input points. For the purpose of this algorithm, this distance also represents the cost of a trajectory connecting the two input nodes. Nearest Node: This function locates the nearest node in the random tree, T, using the random state variable Zrand and the output of the distance function. Near Nodes: This function determines a set of nodes that are

log n within the confines of a sphere having volume 1* ( __ )d n around the state Zrand E X. The number of vertices in any given tree T is n while d is the number of dimensions and I is a fixed number [14]. Steering: The Steering function determines a control input Unew that connects to the path joining Xnew to Znearest in the configuration space. Znew is located at a small incremental distance, 6, from the point Znearest in the direction of Znearest to Zrand . Collision checking: This function returns true if a given path intersects with any obstacle region in the configuration space. Insert Node: This function inputs the new node Znew in the tree T by connecting it to its parent node zparent . The Insert Node function also maintains the information about the cost of Znew. This is done with the help of the distance function, which determines the cost of the path between zparent and Znew and the cost already associated with zparent .

I: T +--I nitializeTreeO; 2: T +--I nsertN ode (e, Zinit, T); 3: i +--0; 4: while i � N do 5: zrand +--Sample(i); 6: znearest +--N earest(T, Zrand); 7: (Xnew, unew, Tnew) +--Steer(Znearest, Zrand); 8: if Obstaclefree(xnew) then 9: Znear +--Near(T, znew, IVI);

10: Zrnin +--ChooseParent(Znean znearest, znew, Xnew);

11: T +--InsertNode(zrnin' znew, T); 12: T +--Rew'ire(T, Zrnin, Znear, Znew); 13: end if 14: i+--i+l; 15: end while 16: returnT

Algorithm 1: T = (V, E) +--RRT * (Zinit)

B. Triangular Geometrical Methods

There are four common triangular centres, namely Circum­centre, Incentre, Orthocentre and Centroid. For the purpose of this paper, all four methods were applied to RRT* and performance results in each case were separately investigated. The results obtained indicated that out of the four methods used, only two types of centres, i.e, the circumcentre and cen­troid, were able to produce satisfactory results. The other two methods, when applied in a number of different environments,

381

caused RRT* to take an infinite amount of time to locate the optimum path solution. This is due to the fact that Orthocentres and Circumcentres do not necessarily lie inside the triangle formed by the goal, root and random nodes. Therefore, there exists a possibility that these centres take the random sample away from the goal region instead of closer to it, resulting in increase in the execution time of the algorithm. Following is the brief description of each type of triangular center shown in Fig 1.

i) Circumcentre: Fig. la shows the circumcentre of any triangle. The point where the perpendicular bisectors of all three sides of a triangle intersect is known as Circumcentre.

2) Orthocentre: This is a point where all three altitudes of a triangle meet as shown in Fig. 1 b. Altitude of a triangle is a line passing through a vertex and perpendicular to the opposite side of the triangle.

3) Centroid: Also referred to as the centre of gravity, is a point inside the triangle where its medians intersect. Medians are line segments joining a vertex to the bisectors of the opposite side. Fig. lc shows the location of a triangle's Centroid.

4) Incentre: A point inside the triangle where its angle bisectors intersect. This is essentially the centre of the incircle, the largest circle that can be enclosed inside the triangle. Fig. 1 d shows the location of a triangle's Incentre.

III. TRIANGULAR GEOMETRISED-RRT*

Triangular Geometrised RRT* (TG-RRT*) algorithm fur­ther extends RRT* to include the Geometrical methods dis­cussed above. Every time a random sample is picked from the configuration space, a geometrical centre of the starting point, goal point and random sample is generated and assigned to the new random sample variable denoted as Znrand. From here onwards, this point is treated as the random sample.

GeometricCentre: This function returns the new value of the random sample and assigns it to Znrand. If j=O, then the Centroid function is called and if j= 1, the Incentre function is called.

A. GeometricCentreo Algorithm 3 describes the Centroid function. Once this

function is called, Znrand contains the coordinates of the centroid of the starting, goal and random points. RRT* incor­porated with this procedure will now be denoted as Centroid­RRT* (C-RRT*).

B. GeometricCentrel Algorithm 4 describes the Incentre function. Once this func­

tion is called, Znrand contains the coordinates of the Incentre of the starting, goal and random points. RRT* using this function is denoted as Incentre-RRT* (IC-RRT*).

IV. EXPERIMENTAL RESULTS

This section presents simulation results of the proposed TG-RRT* in various environments. The new algorithm is run with both the Centroid and Incentre extensions separately and

c B

A Midpoint

c

(a) Circumcentre (b) Orthocentre (c) Centroid (d) Incentre

Fig. 1: Triangular Geometric Centres

1: T +-I nitializeTreeO; the trajectory and is denoted by C, time t represents the time 2: T +-I nsertN ode (e, Zinit, T); taken by the algorithms to obtain the path solution and n is the 3: i +-0; number of iterations utilized in calculating the optimum path 4: j +-[0, 1]; solution. 5: while i < N do

. 2 h h ' . II h . I 6: Z +- Sample(i)' Fig. s ows t e performance of a t ree types of a go-

7:

8:

9: 10:

11:

12:

13:

rand 'G t : C t (X X ). rithms in finding an initial path from starting to goal node, Znrand '---- eome rzc en rej initial, Goal, Zrand ,

. . . Z +-N earest(T Z ) . III a cluttered environment. It can be seen that out of the nearest , nrand , hr I . h RRT* . h b f . . (x U T ) +-Steer(z z )· t ee a gont ms, reqUIres t e most num er 0 IteratIOns

if Ob;ta�l;jren;(xnew) then

nearest, nrand , (n=I?063) as well as time (t�0.57s) to present an initial path

Z "--- N '"(T IVI)' solutIOn. Between the CentrOid and Incentre methods, Incentre near ""'----- ear . Znew - , . . . . . Z . +-

. . reqUIres the least number of Iterations (n=163) and uses least

CrhnornosePa�ent(Z Z Z x ) , time (t=0.02s) as well. However, cost of the initial path given , neaT) nearest, new, new ,

b C 'd h d . I (C 39 "'7) d T +-I nsertN ode(z . z T)' Y entrOi met 0 IS very ess = .. J as compare to . rmn, new, ,

IC-RRT* and RRT* 14: T +-Rewzre(T, Zmin, Znean Znew); . 15: end if 16: i+-i+l; 17: end while 18: retuTnT

Algorithm 2: T = (V, E) +-TG - RRT*(Zinit)

Znrand +-(XGoal + Zrand + Xinitial)/3; Algorithm 3: Znrand +-GeometTicCentTeo(Zrand)

Fig. 3 further compares the number of iterations required and processing time for each type of algorithm in converging to optimum path solution in the same cluttered environment. Among the three algorithms, RRT* was not able to converge to any optimal solution even after 10 million iterations. However, from the figure 3 it can be seen that among C-RRT* and IC­RRT*, the IC-RRT* takes lesser number of iterations (n=7300) and time (t=0.88s) as compared to C-RRT*.

Fig. 5 represents number of iterations required and time consumed for locating an initial path in a different cluttered environment. From the simulation results, it can be deduced

1: drg +-d(Xinitial, XGoal); that even in this environment, the number of iterations required 2: dsg +-d(Zrand, XGoal); by RRT* are extremely large (n=43683), especially in compar-3: drs +-d(Xinitial, Zrand); ison with C-RRT* (n=2574) and the extremely low number of 4: P +-(drg + dsg + drs); iterations utilized by IC-RRT* (n=13). In Fig. 6, the number of 5: znrand +-(drs * XGoal + drg * Zrand + dsg * Xinitial)/ P; iterations required by RRT* to reach near optimum solution are

Algorithm 4: Znrand +-GeometricCentrel (Zrand)

iterations required to reach optimum path solution in each environment are calculated. RRT* algorithm is also run in the same environment for comparison purposes. In all figures depicting simulation results, obstacles in the configuration space are seen as black objects while Starting and Goal regions are indicated as blue circles. The pink network of lines represents a tree while the single red line indicates the path deduced by the algorithm after n iterations. C-RRT* indicates RRT* extended by applying the Centroid geometrical method while IC-RRT* represents RRT* extended using the Incentre geometrical method. In all performance evaluations presented in this paper, cost is the sum of the Euclidean distances of subsequent nodes connecting the starting and ending nodes of

382

extremely large (n=80000), compared to C-RRT* (n=20000) and IC-RRT* (n=150). Both these figures also indicate that in this environment as well, RRT* takes the most amount of time to locate initial(t=1.52s) and optimum paths (t=2.8s), while Incentre requires the least initial path time (t=Os) and optimum path time(t=0.012s).

Figures 4 represents another complex environment. The start and goal regions, while very close together, require a path solution that traverses the length of a maze and stretches away from the goal. In this environment, IC-RRT* was not able to find any path solution since it tries to bring the random sample close to the starting and goal points. Among C-RRT* and RRT*, C-RRT* consumes very less iterations(n=25194) and time (t=0.96s) to find initial path compared to RRT* (n=808306, t=29s). Consequently, RRT* also consumes a very

•

(a) RRT*: n=16063, C=39.6169, (=0.57s (b) IC-RRT*: n=163, C=41.65, (=0.02s (e) C-RRT*: n=1246, C=39.5727, (=0.05s

Fig. 2: Environemnt 1: Initial Path (a) RRT* (b) IC-RRT* (c) C-RRT*

(a) RRT*: n=10million, C=39.46, 1=238.65 (b) IC-RRT*: n=7300, C=38.6, 1=0.88s (e) C-RRT*: n=24000, C=38.6, 1=0.915

Fig. 3: Environemnt 1: Final Path (a) RRT* (b) IC-RRT* (c) C-RRT*

(a) RRT*: n=43683, C=49.07, (=1.52s

t � ... (iII­

...

...

-

-

-

(b) IC-RRT*: n=13, C=51.108, I=Os (e) C-RRT*: n=2574, C=49.81, 1=0.097s

Fig. 5: Environment 2: Initial Path (a) RRT* (b) IC-RRT* (c) C-RRT*

(a) RRT*: n=80000, C=48.1, 1=2.85 (b) IC-RRT*: n=150, C=47.8, 1=0.012s (e) C-RRT*: n=20000, C=47.2, 1=0.765

Fig. 6: Environment 2: Final Path (a) RRT* (b) IC-RRT* (c) C-RRT*

383

(a) C-RRT* Initial Path: n=25194, (b) RRT* Initial Path: n=808306,

t=0.96s, C= 1 70 . 78 t=295, C= 177.05

(e) C-RRT* Final Path: n=2600000, (d) RRT* Final Path: n=4000000,

t=98.80s, C= 163.39 t= 146.85, C= 163.19

Fig. 4: C-RRT* and RRT* in Complex Environment

large number of iterations (n=4 million) and large amount of time (t=146.8s) to find the near optimal path while C-RRT* consumes comparatively less of both (n=2.6million, t=98.8s).

Fig. 7 represents the Local Minima environment. Once again IC-RRT* is unable to work in this environment. How­ever, the other two algorithms i.e. RRT* and C-RRT* deduced the initial and final trajectory as shown in Fig. 7. Here, it can be seen that RRT* takes almost ten times the number of iterations required by C-RRT* for computing an optimal path of almost same Euclidean distance (C=51).

Fig. 8 shows a performance comparison of all three algo­rithms i.e. RRT*, IC-RRT* and C-RRT*. Fig. 8a represents processing time versus iteration graph in an environment with no static obstacle i.e. environment containing only starting and goal region. For the same number of iterations, it can be seen from Fig. 8a that IC-RRT* consumed higher processing time as compared to RRT* and C-RRT*, whereas the processing time taken by C-RRT* and RRT* for same number of iterations is almost identical. Time complexity for all algorithms is the same i.e. O(nlogn) as no extra looping is involved. Differences in processing times of the algorithms are due to the extra calculations being performed for every sample to direct them closer to the goal region.

Fig. 8b shows comparison between the three algorithms in terms of number of iterations required by each to converge to an initial path. In all ten environments, RRT* requires much greater number of iterations to reach the same initial solution that is reached by C-RRT* and IC-RRT* in significantly less number of iterations. Comparing C-RRT* and IC-RRT*, it can be observed that IC-RRT* gives a better performance in all environments since it requires least number of iterations. Fig. 8c shows performance evaluation in terms of time required by each type of algorithm to obtain the initial path between root and goal regions. The same trend of performance as before can be observed here. In all environments, IC-RRT* utilizes

384

(a) C-RRT* Initial Path: n=6978, (b) RRT* Initial Path: n=2000,

t=0.265, C=51.7 t=0.0735, C=57.4

(e) C-RRT* Final Path:

t=0.325, C=51.09

n=9000, (d) RRT* Final Path: n=850000,

t=30.265, C=51.07

Fig. 7: C-RRT* and RRT* in Local Minima Environment

the least amount of time to return initial solution.

Figures 8d and 8e perform the same comparative analysis as Figures 8b and 8c, except on optimum path solutions gen­erated by all three algorithms. As expected, IC-RRT* utilizes the least number of iterations to converge to an optimum solution in all of the 10 test environments. However, from fig. 8e, a change in the trend can be observed with the C-RRT* algorithm consuming the least processing time to achieve optimal path in all environments. Comparing IC-RRT* and RRT*, IC-RRT* consumes lesser time than RRT*.

Fig. 81' compares memory consumption by the algorithms in the same 10 environments. All of the algorithms presented in this paper have the same space complexity, O(n). However, IC-RRT* takes the least number of iterations and therefore consumes the least memory, whereas RRT* takes s large num­ber of iterations and therefore consumes very large amounts of memory to achieve optimal path solution.

V. CONCLUSION

RRT*, a sampling based motion planning algorithm, is not only able to find a path without requiring complete information about the obstacles in its environment but also guarantees opti­mised path solution. However, its effectiveness is compromised by its slow convergence rate and inefficient use of memory. Our TG-RRT* has proven to be quite successful at addressing these shortcomings by significantly reducing the number of iterations required and time utilized to reach the optimum path solution in comparison with RRT*. Since there exist situations in which IC-RRT* gives a poor performance, it can be concluded that C-RRT* is the most robust algorithm of the three, allowing efficient motion planning in almost every type of environment. Improving upon the work showcased in this paper, in the near future we hope to present hardware implementation of our algorithm on Pioneer 3AT Mobile Robotic platforms. We also

14,-----------------------

12

10

!a t-------------+--------­� 6 1----------------;-,L-----------

10 20 30 40 50 60 70 SO 90 100 Iterationsl kilo-units

-RRT*

-C-RRT*

-IC-RRT*

Iterations Used To Achieve Initial Path 14

� 12 c , 10 � " ::::,

£ c

-� � -�

Environments

Time Consumed To A,niev. Initial Patn

0.8

0.7

0.6

u 0.5 . • RRT* � 0.4 • RRT* .

! 0.3 • C-RRT* • C-RRT' .IC-RRT*

.IC-RRT*

Environments

(al Time Vs Iteration Graph. (bl Iterations required to find initial path in 10 (cl Time required to find initial path in 10 different environments. ditferent environments.

Iterations Used To Achieve Optimal Patn Time Consumed To A,nieve Optimal Patn MemoryConsumedTo A,hieve Optimat Path 90

.� 80

:;' 70 � 60 ::. .:. 50

. a 40

.� :� .I

10

o

--

• RRT* • • C-RRT* I I I I I

.IC-RRT*

• • L L L L . 1 2 3 4 5 6 7 8 9 10

3.5

� 7 ;:.

2.5 .. � iii 5

-::. • RRT* :!! . ";:: 4

! 1.5 .C-RRT'" ;; C

tI-----If----------J-------- . RRT* tI-----.---If----------J-------- . C-RRP

.IC-RRT* :!! 2 tI----I::-If----------.------J--------.IC-RRT* 0.5

0 0

Environments Environments Environments

(d) Iterations required to find final path in 10 (e) Time required to find final path in 10 different (f) Memory Utilized to Find Optimal Path in 10 different environments. environments. ditferent environments.

Fig. 8: Comparison between RRT*, C-RRT* and IC-RRT*.

plan to investigate similar motion planning issues in dynamic environments.

VI. ACKNOWLEDGMENTS

This work is supported by the collaborative research and funding of RISE Lab. (Dept. Robotics and AI, SMME, NUST, Pakistan) and IE Lab. (Dept. of EE, College of Engineering, HYU, Korea, under the grant number NIPA-2013-H0301-13-1011 of MSIP, Korea)

REFERENCES

[I] Y. Liu and N. I. Badler, "Real-time reach planning for animated characters using hardware acceleration," in Computer Animation and

Social Agents, 2003. i6th international Conference on. IEEE, 2003, pp.86-93.

[2] J.-c. Latombe, "Motion planning: A journey of robots, molecules, digital actors, and other artifacts;' The International Journal of Robotics Research, vol. 18, no. II, pp. II 19-1128, 1999.

[3] X. Yang and M. Meng, "Neural network application in robot motion planning," in Communications, Computers and Signal Processing, 1999 iEEE Pacific Rim Conference on. IEEE, 1999, pp. 611-614.

[4] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, "Prob­abilistic roadmaps for path planning in high-dimensional configuration spaces," Robotics and Automation, IEEE Transactions on, vol. 12, no. 4, pp. 566-580, 1996.

[5] O. Khatib, "Real-time obstacle avoidance for manipulators and mobile robots," The international journal of robotics research, vol. 5, no. I, pp. 90-98, 1986.

385

[6] S. M. LaValle, "Rapidly-exploring random trees a ew tool for path planning," 1998.

[7] S. R. Lindemann and S. M. LaValle, "Incrementally reducing dispersion by increasing voronoi bias in rrts," in Robotics and Automation, 20(M. Proceedings. ICRA'04. 2004 IEEE International Conference on, vol. 4. IEEE, 2004, pp. 3251-3257.

[8] A. Yershova, L. Jaillet, T. Simeon, and S. M. LaValle, "Dynamic­domain rrts: Efficient exploration by controlling the sampling domain;' in Robotics and Automation, 2005. ICRA 2005. Proceedings oj'the 2005 IEEE International Conference on. IEEE, 2005, pp. 3856-3861.

[9] S. Khanmohammadi and A. Mahdizadeh, "Density avoided sampling: an intelligent sampling technique for rapidly-exploring random trees;' in Hybrid Intelligent Systems, 2008. HIS'08. Eighth International Con­ference on. IEEE, 2008, pp. 672-677.

[10] I. Garcia and J. P. How, "Improving the efficiency of rapidly-exploring random trees using a potential function planner;' in Decision and

Control, 2005 and 2005 European Control Conference. CDC-ECC'05.

44th iEEE Conference on. IEEE, 2005, pp. 7965-7970. [II] F. Militao, K. Naden, and B. Toninho, "Improving rrt with context

sensitivity," 2010. [12] S. Karaman and E. Frazzoli, "Sampling-based algorithms for optimal

motion planning," The International Journal of Robotics Research, vol. 30, no. 7, pp. 846-894, 2011.

[13] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, "Anytime motion planning using the rrt*," in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 1478-1483.

[14] S. Karaman and E. Frazzoli, "Optimal kinodynamic motion planning using incremental sampling-based methods," in Decision and Control (CDC), 20iO 49th iEEE Conference on. IEEE, 2010, pp. 7681-7687.