TMCnet News
Bidirectional RRT Algorithm for Collision Avoidance Motion Planning of FFSR [Sensors & Transducers (Canada)](Sensors & Transducers (Canada) Via Acquire Media NewsEdge) Abstract: The nonholonomic kinematics characteristic of free-floating space robot (FFSR) in a microgravity environment is a special difficulty in its motion planning. First, kinematics model for FFSR system and state transition equation have been established by applying linear momentum and angular momentum conservation laws followed by FFSR in a microgravity environment. Second, aiming at the nonholonomic characteristic of FFSR, a collision avoidance motion planning algorithm based on bidirectional rapidly-exploring random tree (RRT) has been proposed. This paper focuses on explaining the basic theory of FFSR motion planning, the basic principle for such core algorithms as EXTEND, CONECT and RRT-Connect and the realization of such algorithms. Finally, the correctness of the algorithms proposed has been verified via computer simulation. Copyright © 2014 IFSA Publishing, S. L. Keywords: Free-floating space robot (FFSR), Rapidly-exploring random tree (RRT), Motion planning, Nonholonomic, Collision avoidance, Kinematics. (ProQuest: ... denotes formulae omitted.) 1. Introduction With the great progress of human's exploration on the moon, free-floating space robot (FFSR) has become a research focus in space explorations by every country [1, 2]. FFSR is composed of satellite body and mechanical arm mounted on the body (as shown in Fig. 1); it can fly freely and complete all kinds of operations in space. FFSR mainly fulfils such tasks as invalid satellite capturing, maintaining and redeploying as well as space station building. FFSR system is operated in a space microgravity environment, and it is constrained by linear momentum conservation law and angular momentum conservation law in which the former is a holonomie constraint and the latter is a nonholonomic constraint. This will make FFSR system a nonholonomic dynamics system. It can be reflected as follows: when FFSR moves along different paths from the same configuration, the attitude angles of its satellite body will vary even though the final vectors of joint angles of mechanical arm are the same. Many scholars have made lots of researches on the impact of nonholonomic characteristic of FFSR on its motion control. The most representative achievements include resolved motion rate control method [3] proposed by Y. Umetani and K. Yoshida, virtual manipulator (VM) method [4, 5] proposed by Z. Vafa and S. Dubowsky and bidirectional method (positive solution and inverse solution) based on kinematics and dynamics [6] proposed by Y. Nakamura and R Mukheijee. However, the existing researches are mainly about motion control of end-effector and attitude angles of satellite body as well as motion planning without obstacles in operating environment, and few researches are involved in collision avoidance motion planning of nonholonomic FFSR. Apparently, the traditional motion planning algorithms such as grid method [7], octree search method, A* search method, C-Space path search method, Roadmap method and artificial potential field method [8] cannot be directly applied in nonholonomic dynamics system. Therefore, Lavalle et al. proposed rapidly-exploring random tree (RRT) algorithm to solve the problems in motion planning of nonholonomic system [9-11]. RRT algorithm is a single query random search method. In this algorithm, all images of configuration space (C-Space) are not necessarily built, and only C-Space sampling is conducted by certain strategies (such as even random, low discrete degree, low difference and various resolutions). This algorithm can effectively avoid the exponential explosion of calculated amount caused by increased degrees of freedom (DoFs) of the robot in the traditional algorithms. According to the current environmental state, it does not require preprocessing before planning and can adapt to the complex and changing external environments. Therefore, it has been successfully applied in many nonholonomic systems such as motion planning of mobile robot. A FFSR collision avoidance motion planning algorithm based on bidirectional RRT is proposed in this paper, which can efficiently and reliably solve the problems in collision avoidance motion planning of nonholonomic FFSR system with high DoFs in the complex and static environment. 2. Kinematics Model for FFSR When moving in a microgravity environment, FFSR system follows linear momentum and angular momentum conservation laws. According to these two laws and through a series of inference, FFSR kinematic model can be obtained as follows [3, 12, 13]: ... (1) where 0 = [a, ß, yf e R3 represents the attitude angle of FFSR's satellite body, and 0«=k,..,0jeR" represents the vector of joint angle of FFSR's mechanical arm. Formula (1) describes the impact of joint motion of mechanical arm on the satellite body during the motion of FFSR system. G(0,0M) matrix is related to the attitude angle© of current FFSR's satellite body and the configuration of mechanical arm. It is a kind of complex nonlinear mapping about FFSR configuration q=[0r,0Mr]r = [cc,ß, y, 0,,..., @n]T, which is different from the robot system with pedestals fixed on the ground. Formula (1) is not integral. That is, the attitude angle 0 cannot be integrally expressed as a function for the vector of joint angle 0M. Therefore, numerical integration must be conducted along the motion paths of FFSR according to formula (1) in order to obtain the attitude angle of FFSR system. This indicates that the attitude angle 0 of FFSR system is related to the vector of joint angle 0M upon motion completion and historical paths of each joint motion of FFSR's mechanical arm. The nonholonomic characteristic of FFSR will bring challenges to FFSR collision avoidance motion planning. 3. Bidirectional RRT Motion Planning of FFSR FFSR motion planning can be defined as: Given start configuration qstart = [©Li^Lwtf and goal configuration qgoal = [®Tgoal,tfMgoal]T, a motion sequence for the vector of joint angle of mechanical arm 0"=[6>,.."6>JeR" can make FFSR system move according to the sequence, reach the goal configuration (\goal and avoid the collision with the obstacles in environment. In the bidirectional RRT algorithm, the planned FFSR dynamics system can be abstracted to be a control system which has a certain state, can respond to the input and conduct state transition and output it. If the vector of joint angle of mechanical arm 0M = e R" is set to express the input of FFSR system, and q = [0r,0^ ]T represents the current state of FFSR system, then the state transition equation of FFSR system can be expressed as: ... (2) Combining Formula (1), the state equation of FFSR can be expressed as: ... (3) Formula (3) indicates that a linear relation is shown between the state change rate q and the input 0M in FFSR system, which is called a symmetric system. That is, if the system starts from the configuration node Qstart and reaches the configuration node qgoal along a certain path T, then the system can start from the configuration qgoal and return to the configuration node qstart [6] along the original path T. According to this characteristic, FFSR configuration can be defined as: after its state, configuration space C [14] of FFSR can be divided into two parts, namely feasible free space Cfree and forbidden space (obstacle space) Cobs. When FFSR state is located in ' it indicates that FFSR collides with the obstacles. When it is located in Cfree, it indicates that FFSR does not collide with the obstacle. Bidirectional RRT motion planning researched in this paper includes two core algorithms, namely EXTEND and CONECT. RRT-Connect algorithm is made up of EXTEND and CONECT. The basic principles of each algorithm and the realization of pseudo codes are described as follows. Bidirectional RRT algorithm is the combination of forward RRT search and backward RRT search. A tree grows up from start configurations while other tree grows up from the goal configurations. Opposed to single tree RRT motion planning, it will be more convenient and efficient to use bi-directional RRT for motion planning, because the bi-directional search tree does not only start from the start point but also take the goal point as its root. Two quickly extensible search trees are built respectively, and the existence of feasible paths is determined by whether there is any cross point between the two trees. For bidirectional RRT, its quickly extensibility needs to be guaranteed, and the cross connection of the two spanning trees must be taken into consideration. In addition, the balanced growth of these two spanning trees needs to be guaranteed. Only in this way can the consistence of building time and space for the two trees be ensured. 3.1. EXTEND Algorithm The entire C-Space is continually explored in RRT algorithm by generating RRT trees. Taking the start configuration qstart of FFSR as a root, a RRT search tree T'start is built. Taking the goal configuration q gual as a root, anther RRT search treeTg0fl/ is built. In each step of the algorithm, a random configuration qrand is randomly sampled in and then EXTEND and CONNECT algorithms are applied to extend RRT trees until the two trees are intersected (as shown in Fig. 2). Pseudo code of EXTEND (T ,qrarld ,qnew) algorithm is described as follows. Algorithm EXTEND (T ,qralrf,q«J ! 1 q^=NEAREST_NEIGHBOR(T,qw); 2 if ( q»OT =new_ configuration ... 3 T .AddVertex (q^); 4 T .AddEdge (qmar, q^, 0^ J; 5 if( Qnew = Qrand ) ( 6 return REACHED;} 7 else{ 8 return ADVANCED;}} 9 return TRAPPED;} 3.2. CONNECT Algorithm As for random sampling configuration Qrand = [0^,0^]r, the configuration node qmar that is nearest to qrand on the RRT tree to be extended is selected in EXTEND algorithm. Starting from qnear, the configuration qmw that is nearest to qrand in all configuration states for FFSR system reached within a tiny time slice At is calculated in the algorithm. When At is small enough, qnew can be obtained via the following formulas: ... (4) ... (5) where G(qrand)+ represents a pseudo-inverse matrix for G(qw). After q^ is obtained, a collision detection will be conducted in the algorithm. There may be three circumstances as follows: If failed, q^ is not in Cfree; If reached, qnew has no collision, and its distance from qrand is less than a certain threshold value A||q|| . ; II ^llmin If advanced, qmw without collision is found. A step is advanced towards qrand based on the original qmar. In (ii) and (iii), qmw is added to the current RRT tree by EXTEND algorithm. It can be seen that the current RRT tree will externally extend one configuration node when EXTEND algorithm is conducted each time. To accelerate the algorithm speed, CONNECT algorithm is applied. After one configuration sampling node qra/)¿is given> CONNECT algorithm keeps calling EXTEND algorithm to guide RRT tree to grow towards qrand until FFSR configurations cannot advance further. The pseudo codes of this algorithm are described as follows: ... 3.3. RRT-Connect Algorithm EXTEND and CONNECT algorithms are used in RRT algorithm, and one of them keeps extending Ts,art and until the distance between the configuration qgoai and a certain leaf node is less than the error value. According to the different selections of EXTEND and CONNECT algorithms, RRT algorithm will have various forms. After experiment, RRT-Connect algorithm is more suitable for motion planning of FFSR system [12] (the schematic diagram is shown in Fig. 3): ... In the algorithm, a randomly sampled configuration node qrand is firstly generated in each iteration, and then a RRT tree is alternatively selected from T and T goal to execute EXTENDalgorithm.Thus,a newconfiguration nodeq new is obtained, and then another RRT tree has CONNECT extension for the new configuration node until the two trees meet each other. 4. Computer Simulation To verify the effectiveness of bidirectional RRT motion planning algorithm, this algorithm has been designed and realized, and three-dimensional visualization simulation is conducted in this paper. During simulation, a parallel collision detection algorithm based on hybrid bounding box is adopted to judge whether FFSR collides with the obstacles [15-20]. All the simulation experiments were conducted in a computer whose configurations are as follows: Intel Pentium Dual Core G640 Processor (dominant frequency: 2.8 GHz), 4 GB RAM, 3 MB three-level buffer. The compilation tool is VS2008 C++. The simulation system framework is built by typical MCV design mode in this paper (as shown in Fig. 4). Some of the key functions for bidirectional RRT motion planning software are described as follows: i) CBBRrt: It is derived from CMotionPlanner, realizing the sample-based bidirectional RRT planner proposed in this paper and adopting extension and improvement strategy. ii) CSpace target can be used to set the complex system configurations, robot's DoFs and collision detection strategy etc. iii) CRobot: It defines the robot target, which can load the model data from XML files. iv) CObstacle: It defines the obstacle. Call createBox ( ) or load vi files from XML, create obstacle model. v) CSpace: It defines the C-Space of motion planning, including CRobot, CRobotNodeSet and CDManager. vi) CSpaceSampled: It is derived from CSpace, indicating sampling-based C-Space. CRobotNodeSet: It appoints the size and boundary of robot's node set in CSpace. vii) CDManager: It is used to detect whether CRobot collides with the obstacle of CObstacle, realizing the collision detection algorithm proposed in this paper. Fig. 5 is three-dimensional scene of bidirectional RRT motion planning of FFSR. 6. Conclusions Bidirectional RRT algorithm is quite suitable for the motion planning of nonholonomic dynamics system. A large number of simulation experiments and researches show that the bidirectional RRT algorithm proposed in this paper is applicable to FFSR collision avoidance motion planning. Due to the special nonholonomic characteristic of FFSR, the calculation of the configuration qnew node that is nearest to qrand is dependent on the pseudo-inverse matrix of G(qraw¿), and it will be affected by the singularity of dynamics during motion planning. Since the motion of FFSR's mechanical arm joints will interfere with the configurations of satellite body, and the sampling time interval needs to be small enough. This has increased the sampling quantity of configuration space and reduced the planning speed. In addition, a lot of kinematics value calculations are required for the configuration nodes extended by bidirectional RRT tree, which has increased the calculated amount of motion planning. How to effectively reduce the sampling configuration nodes and improve the planning efficiency and the theory and argument of probabilistic completeness is the key point for next step. Acknowledgements This work was supported in part by a grant from Harbin Institute of Technology Robotics and System National Key Laboratory (SKLRS-2012-MS-06), Shenzhen Science and Technology Program (JC201006020820A and JCYJ20120615101640639), Team of Scientific and Technological Innovation of Shenzhen Institute of Information Technology (CXTD2-002), Natural Science Foundation of Guangdong Province, China (S2013010013779 and S2011040000672), Guangdong Vocational Education Information Technology Fund ( XXJS-2013-1019). References [1] . F. Aghili, Coordination control of a free-flying manipulator and its base attitude to capture and detumble a non-cooperative satellite, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009, pp. 2365-2372. [2] . Satoko Abiko and Gerd Hirzinger, Computational efficient algorithms for operational space formulation of branching arms on a space robot, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Acropolis Convention Center Nice, France, September, 2008, pp. 3312-3317. [3] . Y. Umetani, K. Yoshida, Resolved motion rate control of space manipulators with generalized Jacobian matrix, IEEE Transactions on Robotics and Automation, Vol. 5, Issue 3, 1989, pp. 303-314. [4] . Z. Vafa, S. Dubowsky, The kinematics and dynamics of space manipulators: the virtual manipulator approach, International Journal of Robotics Research, Vol. 9, Issue 4, 1990, pp. 3-21. [5] . Z. Vafa, S. Dubowsky, On the dynamics of space manipulator using the virtual manipulator with application to path planning, Journal of the Astronautical Science, Vol. 38, Issue 4, 1990, pp. 441^172. [6] . Y. Nakamura, R. Mukheijee, Nonholonomic path planning of space robots via a bidirectional approach, IEEE Transactions on Robotics and Automation, Vol. 7, Issue 4, 1991, pp. 500-514. [7] . Tae-Kyeong Lee, Sang-Hoon Baek, Young-Ho Choi, Se-Young Oh, Smooth coverage path planning and control of mobile robots based on high-resolution grid map representation, Robotics and Autonomous Systems, Vol. 59, Issue 10,2011, pp. 801-812. [8] . Y. Hwang and N. Ahuja, Gross motion planning - a survey, ACM Computing Surveys, Vol. 24, Issue 3, 1992, pp. 219-291. [9] . S. M. LaValle and J. J. Kuffher, Randomized kinodynamic planning, International Journal of Robotics Research, Vol. 20, Issue 5, 2001, pp. 378-400. [10] . S. M. LaValle, Planning algorithms, Cambridge University Press, 2006. [11] . S. M. LaValle and J. J. Kufther, Rapidly-exploring random trees: Progress and prospects, Algorithmic and Computational Robotics: New Directions. A. K. Peters, Wellesley, MA, 2001, pp.293-308. [12] . Li Hua-Zhong, Liang Yong-Sheng, Hong Bing-Rong, Modeling and simulation of continual motion trajectory control for space robot, Robot, Vol. 30, Issue 5,2008, pp. 410-415. [13] . Li Hua-Zhong, On motion control with minimum attitude disturbance for redundant space robots, Computer Engineering and Applications, Vol. 45, Issue 19,2009, pp. 12-16. [14] . J. J. Kufther, and S. M. LaValle, RRT-connect: An efficient approach to single-query path planning, in Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, CA, 2000, Vol. 2. pp. 995-1001. [15] . S. Gottschalk, M. C. Lin and D. Manocha, OBBTree: A hierarchical structure for rapid interference detection, in Proceedings of SIGGRAPH'96, 1996, pp. 171-180. [16] . Anwesha Mukheijee and Debashis De, DAS: An Intelligent Three Dimensional Cost Effective Movement Prediction of Active Users in Heterogeneous Mobile Network, Journal of Computational Intelligence and Electronic Systems, Vol. 2, Issue 1,2012, pp. 31-47. [17] . Rajeev Ranjan and Shirshu Varma, Collision-free time synchronization for multi-hop wireless sensor networks, Journal of Computational Intelligence and Electronic Systems, Vol. 2, Issue 1, 2012, pp. 200-206. [18] . Li Huazhong, Liang Yongsheng, Wang Meini, Dan Tangren, Design and implementation of improved RRT algorithm for collision free motion planning of high-dimensional robot in complex environment, in Proceedings of the 2nd International Conference on Computer Science and Network Technology (ICCSNT'2012), Changchun, China, 2012, pp. 1391-1397. [19] . Li Huazhong, Liang Yongsheng, Dan Tangren, Zheng Hongying, Wu Xianfeng, Collision-free motion planning algorithm based on RRT for robot, Journal of Shenzhen Institute of Information Technology, Vol. 10, Issue 3,2012, pp. 20-27. [20] . Chuan Hong, Khaled Benkrid, Xabier Iturbe, and Hanaa Hussain, Efficient run-time system support for high performance reliable reconfigurable systems, Journal of Computational Intelligence and Electronic Systems, 2012, Vol. 2, Issue 1, pp. 213-219. * Huazhong Li, Yongsheng Liang Software Department, Shenzhen Institute of Information Technology, No. 2188 Longxiang Road, Longgang District, Shenzhen, 518029, Guangdong, China * Tel: +86-0755-89226179, fax: 0755-89226555 E-mail: [email protected], [email protected] Received: 8 March 2014 /Accepted: 31 July 2014 /Published: 31 August 2014 (c) 2014 IFSA Publishing, S.L. |