2013 10th International Conference Ubiquitous Robots Ambient Intelligence (URAI) The 10th International Conference onon Ubiquitous Robots andand Ambient Intelligence (URAI 2013) October 31-November 2013 / Ramada Plaza Jeju Hotel, Jeju, Korea Oct. 30 – Nov. 2, 2013 at2,Ramada Plaza, Jeju, Korea

Motion Planning of Unicycle-Like Robot using Single RRT with Branch and Bound Algorithm Rui-Jun Yan1, Jing Wu1, Ji-Yeong Lee2* 1

Department of Mechatronics Engineering, Hanyang University, Ansan, 426-791, Korea (E-mail: [email protected]; [email protected]) 2 Department of Robot Engineering, Hanyang University, Ansan, 426-791, Korea * * (E-mail: [email protected] ) Corresponding author

Abstract – This paper presents the motion planning of unicycle-like robot using single Rapidly-exploring Random Tree (RRT) algorithm, combining with Branch and Bound (BB) method. RRT can construct the searching tree efficiently between initial configuration and goal configuration. Based on the constructed searching tree, the optimal path can be extracted with BB algorithm rapidly. Obstacles avoidance is realized by checking whether the contour points of robot locate in the space of obstacles. The simulation result in a complex environment with unicycle-like robot proves the validity of proposed algorithms. Keywords – Motion planning, RRT, Unicycle-like robot, Branch and Bound algorithm.

1. Introduction Motion planning is to find an optimal path, taking the robot from initial state to goal state avoiding collision with obstacles in the environment. Rapidly-exploring Random Tree (RRT) is an sampling-based algorithm designed for efficiently searching non-convex high-dimensional spaces in [1], which is applied in large quantity of single-query path planning research [2-3]. The sampling-based method, Probabilistic Roadmap Method (PRM), primarily designed for multiple-query motion planning is proposed in [4]. The combined algorithm of RRT and PRM is proposed for efficiently parallelization in [5]. In this paper, the RRT algorithm is adopted to obtain the searching tree in unknown environment. To optimize the path between initial configuration and goal configuration, the Branch and Bound algorithm, proposed in [6], is used based on the result of searching tree. Similar to our work, the RRT, BB tree adaptation and committed trajectories are combined and used in the anytime motion planning for real-time implementation in [7]. However, the BB algorithm is used for the tree expansion. The branch and bound method in our research optimize the path extracted from searching tree. The constructed searching tree of unicycle-like robot with RRT algorithm is shown in Figure 1. The rest of this paper is organized as follows. RRT algorithm, obstacles avoidance and BB method are presented in Section 2-4 respectively. The simulation results of unicycle-like robot with proposed algorithms are shown in Section 5.

978-1-4799-1197-4/13/$31.00 ©2013 IEEE

‫ٻ‬ Fig. 1. Constructed tree of unicycle-like robot with RRT

2. Rapidly-Exploring Random Tree The RRT algorithm is shown in the following table, in which the initializations are interval distance, initial configuration and goal configuration. RandomFreeConfig() get the sample configuration in non-collision space. Then nearest neighbor close to sample state in constructed tree in the previous steps are found. Then new configuration is got along the direction of prand and pnear with interval ǻt. The iteration terminates when the new configuration is the same as goal configuration. Input: interval ǻt initial state pinit and goal state pgoal Output: searching tree T 1: IsGoal = false; 2: T.AddVetex(pinit); 3: T.AddEdge(pinit, pinit); 4: while (IsGoal); 5: prand ÅRandomFreeConfig(); 6: pnear ÅNearestVertex(prand, T); 7: pnew ÅNewConfig(pnear, ǻt); 8: T.AddVetex(pnew); 9: T.AddEdge(pnear, pnew); 10: if (pnew == pgoal) 11: IsGoal = true; 12: end if 13: end while

Rapid Exploring Random Tree 100 90 80 70

Input: searching tree T Output: Optimal path Popt 1: Popt = [];Piter = []; 2: Piter.push(T.begin(), 1); 3: FindNextVertex(Piter);

60 50 40 30 20 10 0 0

20 40 60 80 100 ‫ٻ‬ Fig. 2. Rapidly-exploring random tree with 3000 iterations

The rapidly-exploring random tree in Figure 2 is constructed with 3000 iterations in the space [0, 100]ൈ[0, 100].

3. Obstacle Avoidance The unicycle-like robot is shown in Figure 3, in which there the robot rotates with vertical axis and horizontal axis respectively. The size of unicycle-like robot in following simulation is with 0.5m in radius and 0.15m in height. The height of the obstacles is set as about 0.7m, a little bigger than the radius. Hence the robot can be simplified as rectangle. To avoid the collision with obstacles, the contour points of the square is calculated in each new configuration.

‫ٻ‬ Fig. 3. Unicylce-like robot

4: FindNextVertex(Piter); 5: if (Piter.end() == pend) 6: Popt = Piter; 7: else 8: for i = Piter.end()->index +1 to T.size() 9: dis = | Piter.end() – T[i]|; 10: if (dis < 1e-10) 11: Piter.push(T[i], i); 12: FindNextVertex(Piter); 13: Piter.pop(Piter.end()); 14: end if 15: end for 16: end if Based on current constructed optimal path, the next vertex will be found through the same procedure. If there is no vertex satisfying the condition and the end vertex of the path is not the goal state, this path must delete the end member until new optional vertex can be added in the end of current path.

5. Simulation Results The simulation result of unicycle-like robot in a complex environment is done for checking the feasibility of proposed algorithms. The searching tree constructed with RRT algorithm is shown in Figure 1, based on which the optimal path is found with BB method in Figure 4. There are 3181 vertexes included in searching tree part and only 269 vertexes contained in the final optimal path. To show the motion of unicycle-like robot clearly, the robot moves with the optimal path in Figure 5. In the figure, the contour points of the robot in each configuration have been drawn. Even if sometimes the free space is very narrow, the robot can across without conflicting with obstacles.

All of the contour points are checked whether they locate in the obstacle space or not. If any one of them conflicts with the obstacles, this new configuration is ignored. Due to the finite number of those points, the location of the vertexes of the obstacles is also compared with the space of current robot state. If at least one vertex is included in the robot, the next new configuration will be calculated.

4. Branch and Bound Algorithm Branch and Bound algorithm is a recursion method, which terminates when the goal position is reached. This algorithm is explained in detail in the following table. BB method begins with the initial vertex in the constructed searching tree. In each step, the end vertex of current optimal path compares with the goal point. If the path has not arrived to the goal position, the next nearest vertex of the tree to the end of current path is searched and it is stored as new end vertex in the path.

Fig. 4. Optimal path with Branch and Bound algorithm

'Advanced Robot Manipulation Research Center' support program supervised by the NIPA (National IT Industry Promotion Agency) (NIPA-2012-H1502-12-1002).

References

Fig. 5. Optimal path with contour of unicycle-like robot

6. Conclusions In this paper, the motion planning of unicycle-like robot in unknown environment with RRT algorithm and BB method is presented. BB method optimizes the path between initial state and goal state based on the constructed searching tree using RRT algorithm. Finally, the simulation results with proposed method show the validity of the combination of two algorithms in the application of motion planning.

Acknowledgement This research was supported by the Ministry of Knowledge Economy, Korea, under the Industrial Foundation Technology Development Program supervised by the Korea Evaluation Institute of Industrial Technology (No. 10038660, Development of the control technology with sensor fusion based recognition for dual-arm work and the technology of manufacturing process with multi-robot cooperation). Also, this research was under the

[1] S. LaValle and J. Kuffner, “Rapidly-exploring random trees: Progress and prospects,” in Proc. Algorithmic Comput. Robot., 2001, pp. 293–308. [2] L. Jaillet, J.M. Porta, “Path Planning under Kinematic Constraints by Rapidly Exploring Manifolds,” Robotics, IEEE Transactions on, vol.29, no.1, pp.105-117, Feb. 2013. [3]‫ٻ‬D. Devaurs, T. Simeon, J. Cortes, “Parallelizing RRT on Large-Scale Distributed-Memory Architectures,” Robotics, IEEE Transactions on, vol.29, no.2, pp.571-579, April 2013. [4] L.E. Kavraki, P. Svestka, J.C. Latombe, M.H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” Robotics and Automation, IEEE Transactions on, vol.12, no.4, pp.566-580, Aug 1996 [5] E. Plaku, K.E. Bekris, B.Y. Chen, A.M. Ladd, E.E. Kavraki, “Sampling-Based Roadmap of Trees for Parallel Motion Planning,” Robotics, IEEE Transactions on, vol.21, no.4, pp.597-608, Aug. 2005. [6] E.L. Lawler and D.E. Wood. “Branch-and-bound methods: A survey.” Operations Research, 14:699–719, 1966. [7] S. Karaman, M.R. Walter, A. Perez, E. Frazzoli, S. Teller, “Anytime Motion Planning using the RRT*,” Robotics and Automation, 2011 IEEE International Conference on , vol., no., pp.1478-1483, May 2011.