The Implementation of RRTs for a Remote-Controlled Mobile Robot

Transcription

1 ICCAS5 June -5, KINEX, Gyeonggi-Do, Korea he Implementation of RRs for a Remote-Controlled Mobile Robot Chi-Won Roh*, Woo-Sub Lee **, Sung-Chul Kang *** and Kwang-Won Lee **** * Intelligent Robotis Researh Center, KIS, Seoul, Korea (el : ; wroh@kist.re.kr) ** Intelligent Robotis Researh Center, KIS, Seoul, Korea (el : ; robot@kist.re.kr) *** Intelligent Robotis Researh Center, KIS, Seoul, Korea (el : ; kash@kist.re.kr) **** Department of Eletronis Engineering, Ajou University, Suwon, Korea (el : ; lkw@ajou.a.kr) Abstrat: he original RR is iteratively epanded by applying ontrol inputs that drive the system slightly toward omly-seleted states, as opposed to requiring point-to-point onvergene, as in the probabilisti roadmap approah. It is generally known that the performane of RRs an be improved depending on the seletion of the metris in hoosing the est verte and bias tehniques in hoosing om states. We designed a path planning algorithm based on the RR method for a remote-ontrolled mobile robot. First, we onsidered a bias tehnique that is -biased Gaussian om distribution along the ommand diretions. Seondly, we seleted the metri based on a weighted Eulidean distane of om states and a weighted distane from the region. It an save the effort to eplore the unneessary regions and help the mobile robot to find a feasible trajetory as fast as possible. Finally, the onstraints of the atuator should be onsidered to apply the algorithm to physial mobile robots, so we selet ontrol inputs distributed with ommanded inputs and onstrained by the maimum rate of input hange instead of om inputs. Simulation results demonstrate that the proposed algorithm is signifiantly more effiient for planning than a basi RR planner. It redues the omputational time needed to find a feasible trajetory and an be pratially implemented in a remote-ontrolled mobile robot. Keywords: RR, Path Planning, Obstale Avoidane, Mobile Robot, Bias ehnique, Metri Choie 1. INRODUCION he inreasing performane of miroproessor and development of various sensors enable mobile robots to be used for various areas. In indoor ases, personal servie robots perform the missions of museum tour guide, room leaning and nursing for the elderly. In outdoor ases, mobile robots have been used for the purpose of patrol, reonnaissane, surveillane and eploring planets, et. he remote-ontrolled mobile robots that are operated by the remote operators should be able to navigate with ommanded translational and rotational veloities and pereive stati and varying environments and avoid dynami obstales suh as vehiles or pedestrians. Mobile robots often find themselves in a situation where they must find a trajetory to another position in their environments, subjet to onstraints posed by obstales and the apabilities of the robot itself. Rapidly-eploring Random rees (RRs) are a reently developed representation on whih fast ontinuous domain path planners an be based and a omized data struture that is designed for a broad lass of a path planning problems [1-3]. he advantage of RRs is that they an be diretly applied to nonholonomi and kinodynami planning. Moreover, RRs has been used to solve nonli ontrol problems and etended to the ase of hybrid systems [-5]. he original RR is iteratively epanded by applying ontrol inputs that drive the system slightly toward omly-seleted states, as opposed to requiring point-to-point onvergene, as in the probabilisti roadmap approah. It is generally known that the performane of RRs an be improved depending on the seletion of the metris in hoosing the est verte and bias tehniques in hoosing om states []. We designed a path planning algorithm based on the RR method for a remote-ontrolled mobile robot. First, we onsidered a bias tehnique that is -biased Gaussian om distribution along the ommand diretions. Seondly, we seleted the metri based on a weighted Eulidean distane of om states and a weighted distane from the region. It an save the effort to eplore the unneessary regions and help the mobile robot to find a feasible trajetory as fast as possible. Finally, the onstraints of the atuator should be onsidered to apply the algorithm to physial mobile robots, so we selet ontrol inputs distributed with ommanded inputs and onstrained by the maimum rate of input hange instead of om inputs. Simulation results demonstrate that the proposed algorithm is signifiantly more effiient for planning than a basi RR planner. It redues the omputational time needed to find a feasible trajetory and an be pratially implemented in a remote-ontrolled mobile robot. he remainder of this paper is organized as follows. Setion introdues a brief formulation of the RR algorithm. Setion 3 onsiders the kinematis and pratial implementation of a RR for a remote-ontrolled mobile robot. Setion presents the simulation results. Finally, some onlusions are presented in Setion 5..1 Basi RR Algorithm. RR ALGORIHM.1.1 Problem Desription he lass of problems onsidered in RRs an be formulated in terms of si omponents [3]: n 1) State Spae: A bounded manifold, X R ) Boundary Values: init X and X X 3) Collision Detetor: A funtion, D: X { true, false}, that determines whether global onstraints are satisfied from state. his ould alternatively be a real-valued funtion that indiates distane from the onstraint

2 ICCAS5 boundary. ) Inputs: A set, U, whih speifies the omplete set of ontrols or ations that an affet the state. 5) Inremental Simulator: Given the urrent state, (t), and inputs applied over a time interval, { u( t') t t' t + t}, the inremental simulator yields ( t + t). his usually ours through numerial integration of a state transition equation, & = f (, u). ) Metri: A real-valued funtion, ρ : X X [, ) whih speifies the distane between pairs of points in X (however, ρ is not neessarily symmetri). rajetory planning will generally be viewed as a searh in a state spae, X, for a ontrol u that brings the system from an initial state, init to a region X X or state X X. It is assumed that a ompliated set of global onstraints is imposed on X, and any solution path must keep the state within this set. A ollision detetor reports whether a given state satisfies the global onstraints. Loal, differential onstraints are imposed through the definition of a set of inputs (or ontrols) and an inremental simulator. aken together, these two omponents speify possible hanges in state. he inremental simulator an be defined by numerial integration of a state transition equation of the form & = f (, u) or an simply be ahieved by a simulation software pakage. Finally, a metri is defined to indiate the loseness of pairs of points in the state spae..1. Basi RR Algorithm he basi RR onstrution algorithm is given in able 1 [1]. A simple iteration is performed in whih eah step attempts to etend the RR by adding a verte that is biased by a omly-seleted state, X. he EXEND funtion selets the est verte already in the RR to. he est verte is hosen aording to the metri, ρ. he funtion NEW_SAE makes a motion toward by applying an input u U for some time inrement t. his input an be hosen at om, or seleted by trying to all possible inputs and hoosing the one that yields a state as lose as possible to the sample, (if U is infinite, then a finite approimation or analytial tehnique an be used). NEW_SAE impliitly uses the ollision detetion funtion to determine whether the state and all intermediate states satisfy the global onstraints. For many problems, this an be performed quikly ( almost onstant time ) using inremental distane omputation algorithms by storing the relevant invariants with eah of the RR verties. If NEW_SAE is suessful, the state and input are represented in and u, respetively. Figure 1 shows an RR grown from the enter of a square region in the plane. In this eample, there are no differential onstraints (motion in any diretion is possible from any point). he inremental onstrution method biases the RR to rapidly eplore in the beginning, and then onverge to a uniform overage of the spae. he eploration is naturally biased towards verties that have larger Voronoi regions. his auses the eploration to our mostly on the uneplored portion of the state spae. In addition to growing a tree from the starting state, many RR implementations grow a seond tree from the tree. Suh trees grow in four steps. June -5, KINEX, Gyeonggi-Do, Korea 1) Grow start-tree towards a om uneplored onfiguration. ) Grow -tree towards a om uneplored onfiguration. 3) Grow start tree towards tree. At eah iteration, selet a om verte in the tree to grow towards it. ) Grow tree towards start tree. A solution path is found when the two trees finally onnet. able 1 he Basi RR Algorithm Build_RR ( ) init.init ( ); init For k = 1 to K do = RANDOM_SAE(); EXEND (, ); Return EXEND (, ) = NEARES(,); If NEW_SAE (,,, u ).add_verte ( );.add_edge (, u ), Fig. 1 Eample of a basi RR algorithm. Other RRs If a dual-tree approah offers advantages over a single tree, then it is natural to ask whether growing three or more RRs might be even better. hese additional RRs ould be started at om states. Of ourse, the onnetion problem will beome more diffiult for nonholonomi problems. Also, as more trees are onsidered, a ompliated deision problem arises. he omputation time must be divided between attempting to eplore the spae and attempting to onnet RRs to eah other. It is also not lear whih onnetions should be attempted. Many researh issues remain in the development of this and other RR-based planners. 3. RR FOR REMOE-CONROLLED MOBILE ROBO 3.1 Kinematis We onsider a mobile robot with a -wheel differential-drive skid-steering onfiguration, the two wheels on the same side move in unison, with eah pair on opposite side apable of being driven independently. If both pairs are

3 ICCAS5 driven forward with the same speed, then the robot moves forward, but if they are driven in opposite diretions, the robot will turn in plae (i.e., eeuting a zero-radius turn). he nominal model equation an be desribed by the following Eq. (1). & osθ θ + ω θ y & = sin v & 1 (1) he state vetor X = [ y θ ] represents the robot position and orientation. he ontrol inputs applied to the mobile robot are u = [ v ω] where v is the translational veloity and ω is the rotational veloity of the robot. 3. Implementation of RR When we onsider not the autonomous mobile robot but the remote-ontrolled mobile robot (RC-mobile robot), it has to keep the ommanded translational and rotational veloities and avoid some obstales suh as stati walls and dynami moving obstales (e.g., ars, people). Most eisting ollision avoidane methods are purely reative in the sense that they searh for safe robot ontrol ommands based on the robot s urrent proimity sensor data without any projetion of the robot s future state. hese methods differ in the way this searh is arried out. he Vetor Field Histogram method [7] and Potential filed methods [] do not epliitly take the onstraints imposed by the dynamis of the robot into aount. Another popular method is the dynami window approah to ollision avoidane [9]. his method searhes for the trajetory the robot should take within the net time step based on a loal map of the robot s surrounding built from the latest sensor measurements. In order to redue the searh spae, the robot s dynami onstraints are taken into aount by onsidering only veloities whih an be reahed within the net time interval. Konolidge [1] uses dynami programming on the loal map to ompute the gradient towards the target. o make this approah omputationally feasible only the two dimensional spae of possible robot positions is onsidered for planning. As mentioned in Setion, RR algorithms have the advantage that they an be diretly applied to nonholonomi and kinodynami planning. We will suggest a method that effetively modifies the basi RR to be used for the RC-mobile robot and simultaneously plans ollision free paths and takes the kinemati onstraints of the robot into aount. here are several issues in the appliation of RR algorithms. he main issues that should be onsidered to improve the performane of algorithms are as followings: - sampling strategy (bias tehnique) - metri hoie - input seletion Sampling strategy is related with how to bias the probability distribution density of om samples. he original RR has a uniform om distribution and takes a long time to find a path to the. Some methods were suggested to improve the omputational time. he -biased tehnique simply an be applied by the assignment the probability at the point [3]. And also, adaptive sampling bias tehnique was suggested with appliations to test generation [11]. hey initially bias the distribution so that states the unsafe set are seleted and monitor the growth June -5, KINEX, Gyeonggi-Do, Korea of the tree. As the growth rate of the tree delines, the sampling distribution is less biased. o find a metri that yields good performane an be a very diffiult task. he ideal metri is the optimal ost-to-go, whih is the optimal ost for the robot to move from one state to another state []. Calulating the optimal ost-to-go is at least the same diffiulty as the trajetory design problem. In general, a simple Eulidean metri is used. For a partiular system, it may be possible to derive a metri from several alternatives, inluding a Lyapunov funtion, a steering method, a fitted spline urve, or an optimal ontrol law for a loally-liized system. In [1], the ost-to-go funtion from a hybrid ontroller was used as the metri in an RR to generate effiient plans for a nonli model of a heliopter. Input seletion to make a motion toward a omly seleted state from the est neighbor state and minimize the distane between them an be hosen at om, or seleted by trying all possible inputs and hoosing the one that yields a state as lose as possible to the sample (if the input range is infinite, then a finite approimation or analytial tehnique an be used). he determined state by the input impliitly uses the ollision detetion funtion to determine whether the state satisfy the global onstraints We onsider the pratial implementation of a RR algorithm for the remote-ontrolled mobile robot. In this ase, the situation is so different from the autonomous mobile ase. We assume the followings. Assumptions: - he mobile robot has a differential drive onfiguration. - he translational and rotational veloities are limited. - he translational and rotational aelerations are also limited. - he mobile robot reeives the target veloity ommands every one seond from the remote operator. - he mobile robot knows the environments from various sensors. - he mobile robot knows its position and attitude. We need to modify a RR algorithm in order to adapt the above assumptions in a remote-ontrolled mobile. hese assumptions make the path planning as a loal path planning and ollision avoidane in some aspets. Net are our modifiations on the RR fators in this paper. Bias ehnique Most of RRs have a uniform distribution over the onfiguration spae or a -biased Gaussian distribution as seleting a om state. In our ase, we assumed that the RC-mobile robot reeives the translational and angular veloity ommands from the operator every one seond ( t ). hese ommands indiate the diretion of the region after one seond. A simple region bias tehnique is utilized in seleting a om state. It has a Gaussian distribution entered along the ommanded diretion. he mean and standard deviation of the distribution an be alulated as shown in Eqs. () ~ (). ~ N( µ, σ ), () where µ is mean and σ is standard deviation.

4 ICCAS5 os( θ ) v µ = α sin( θ ) + ur t ω os( θ ) v σ = α sin( θ ) ω t where, v : ommanded translational veloity ω : ommanded rotational veloity α : diretion saling fator θ = θ + ω t (3) () June -5, KINEX, Gyeonggi-Do, Korea able Input Seletion Algorithm Selet_Input (,, v, ω, vma, a ) ma For k = 1 to K1 do u = a ma * UNI_RANDOM(); u = v + u ; If t u t v ma u t = v ma ; CALCULAE_MINIMUM_MERIC_INPU( u ); t Return u Choie of Metri Metris are used when we hoose the est verte and selet a ontrol input that produes a state a om state. Finding metris that yield good performane an be a very diffiult task. Generally, the same metris are used in those steps. In this paper, different metris are used. In ase of hoosing the est verte, a simple metri based on a weighted Eulidean distane of state is utilized. But in ase of seleting a ontrol input, the distane between the est state and the region state that normalized with respet to the distane between and is added. his improves the performane of eploring to the region. Eq. (5) shows the metri that is used in the routine of finding the est verte and Eq. () shows the metri that is used in the routine of seleting the ontrol input. ρ = (5) ρ input = + w g, () where, w is weighting fator g Input Seletion he ontrol input that drives the system slightly toward omly-seleted points is usually seleted omly. From a pratial point of view, it is neessary to onsider the onstraints of inputs in path planning. We onsiders the onstraints of the maimum veloity v and the maimum ma aeleration a. able shows the input seletion ma algorithm that takes these ontrol input onstraints and the ommanded veloities into aount. Heuristially Pruning the ree As performing the input seletion algorithm presented in able, the funtion CALCULAE_MINIMUM_MERIC _INPU simulates the mobile robot atuated by the seleted ontrol input u t that meets onstraints. If the alulated states of the mobile robot are oupied by a wall or other obstales, his ontrol input is invalid. he number of invalid input that ourred during the Selet_Input loop is ounted and haraterized in the tree verte. his information is used in the NEARES routine that finds the est verte from the omly seleted state in able 1. If the invalid input ount is over the predetermined perentage γ of all tried inputs, this verte is pruned from the tree and will not be seleted after then. his strategy that pruning the tree ould make the overall omputational time fast and prevent the mobile robot from being trapped in the loal minimum.. SIMULAION RESULS We performed various simulations to identify the effet of different fators in designing a RR algorithm for a remote-ontrolled mobile robot. he parameters that used in simulations are shown in able 3. able 3 Simulation Parameters Parameter Value v maimum translational veloity 1[ m / s] t ma π v a ma maimum angular veloity [ rad / s] a maimum translational aeleration.5[ m / s ] t ma a a ma maimum angular aeleration π [ rad / s ] t path planning step time 1 [se] w angular distane weighting fator.7 a w weighting fator in g ρ input 1 γ perentage in pruning tree routine [%] ε region radius.5 K maimum loop ount 15 K1 number of the omly seleted inputs 1 region diretion weighting α fator 5 Net subsetions show the effet of the fators that have an influene on the algorithm s performane..1 Comparison of Different Bias ehniques First, we onsider the effet of om state bias. he mobile robot initially loates in the state = [3,, π ]. It is assumed that the mobile robot reeives a ommand veloities and alulates the region the state = [3,, ]. Fig. shows the result when we hoose the -biased Gaussian distribution and Fig. 3 shows when we hoose om distribution over the onfiguration spae. We adapt the same om input sequenes and take the same simulation onditions eept the distribution. We an know that the -biased distribution gives better performanes than the om distribution. First ase diretly goes to the region, but seond ase eplores wide areas and takes muh time to perform the algorithm.

5 ICCAS5. Comparison of Different Metri Choie o identify the effet of metri hoies, we performed the following simulation. We also let the simulation onditions same for both ases eept the input seletion metri. he mobile robot initially loates in the state = [1,, ]. It is assumed that the mobile robot reeives a ommand veloities and alulates the region the state = [, 7.5, ]. Fig. shows the results in the ase of adding the weighting fator w when we hoose the ontrol input as you an see g in able. We an know that the robot diretly moves toward the. However, If the weighting fator is removed, as you an see in Fig. 5, the robot produes zigzag trajetory..3 he Effets of Pruning ree o redue the omputational time, it ould be need to get rid of the invalid vertees in tree. As we referred in Setion 3, we performed simulation to know how the pruning tree an inrease the performane. he mobile robot initially loates in the state = [.5,, π / ]. It is assumed that the mobile robot reeives a ommand veloities and alulates the region the state = [ 1,, π / ]. Fig. shows the simulation result without pruning proess. In the enter of the onfiguration spae, there are many vertees tried to epand the tree, but, they failed to etend any more. In order to make the est searh routine effetively, we removed the vertees that ounted invalid trials over [%]. As you an see in Fig. 7, this proess improved the performane of the eploration and redued the omputational time over [%] than the result without pruning tree. 1 June -5, KINEX, Gyeonggi-Do, Korea 1 1 Fig. Simulation result with weighting fator in the input seletion 1 1 Fig. 5 Simulation result without weighting fator in the input seletion 1 1 Fig. Simulation result with -biased Gaussian distribution 1 1 Fig. Simulation result without pruning tree 1 Fig. 3 Simulation result with om distribution

6 ICCAS5 1 1 Fig. 7 Simulation result with pruning tree 5. CONCLUSION We proposed a path planning algorithm based on the RR method for a remote-ontrolled mobile robot. First, we onsidered a bias tehnique that is -biased Gaussian om distribution along the ommand diretions. Seondly, we seleted the metri based on a weighted Eulidean distane of om states and a weighted distane from the region. It an save the effort to eplore the unneessary regions and help the mobile robot to find a feasible trajetory as fast as possible. Finally, the kinemati onstraints of the mobile robot and the onstraints of the ontrol inputs were onsidered in order to apply the algorithm to physial mobile robots. Simulation results demonstrate that the proposed algorithm is signifiantly more effiient for planning than a basi RR planner. It redues the omputational time needed to find a feasible trajetory and an be pratially implemented in a remote-ontrolled mobile robot. In the future, we plan to eperiment with a physial mobile robot to identify the performane of the proposed algorithm. June -5, KINEX, Gyeonggi-Do, Korea [] Peng Cheng, Reduing metri sensitivity in omized trajetory design, Pro. Of the 1 IEEE/RSJ Int l Conf. on Intelligent Robots and Systems, pp. 3-, 1. [7] J. Borenstein and Y. Koren, he vetor field histogram-fast obstale avoidane for mobile robots, IEEE Journal of Robotis and Automation, Vol. 7, No. 3, pp. 7-, [] J.-C. Latombe, Robot Motion Planning, Kluwer Aademi Publishers, Boston, MA, [9] D. Fo, W. Burgard, and S. hrun, he dynami window approah to ollision avoidane, IEEE Robotis and Automation Magazine, Vol., No. 1, [1] K. Konolige, A gradient method for real-time robot ontrol, In Pro. of IEEE/JSR Conf. on Intelligent Robots and Systems,. [11] Jongwoo Kim and Joel M. Esposito, "Adaptive sample bias for rapidly-eploring om trees with appliations to test generation," Amerian Control Conferene, 5 [1] E. Frazzoli, M.A. Dahleh, and E. Feron,. Robust Hybrid Control for Autonomous Vehile Motion Planning, In IEEE Conf. on Deision and Control, Sydney, Australia, pp. 1-, REFERENCES [1] S. M. LaValle and J. J. Kuffner, Rapidly- eploring om trees: A tool for path planning, R 9-11, Computer Siene Dept., Iowa State University, Ot [] J. J. Kuffner and S. M. LaValle, RR-onnet: An effiient approah to single-query path planning, Pro. IEEE Int l Conf. on Robotis and Automation, pp ,. [3] S. M. LaValle and J. J. Kuffner, Rapidly-eploring om trees: Progress and prospets, In B. R. Donald, K. M. Lynh, and D. Rus, editors, Algorithmi and Computational Robotis: New Diretions, pp. 93-3, A K Peters, Wellesley, MA, 1. [] M. S. Braniky, M. M. Curtiss, J. Levine, and S. Morgan, RRs for nonli, disrete, and hybrid planning and ontrol, Pro. IEEE Conf. Deision and Control, Deember 9-1, 3. [5] J. M. Esposito, J. Kim, and V. Kumar, Adaptive RRs for validating hybrid roboti ontrol systems, In Int l Workshop on the Algorithmi Foundations of Robotis, Netherlands,.