Poactive Kinodynamic Planning using the Extended Social Foce Model and Human Motion Pediction in Uban Envionments Gonzalo Fee and Albeto Sanfeliu Abstact This pape pesents a novel appoach fo obot navigation in cowded uban envionments whee people and objects ae moving simultaneously while a obot is navigating. Avoiding moving obstacles at thei coesponding pecise moment motivates the use of a obotic planne satisfying both dynamic and nonholonomic constaints, also efeed as kynodynamic constaints. We pesent a poactive navigation appoach with espect its envionment, in the sense that the obot calculates the eaction poduced by its actions and povides the minimum impact on neaby pedestians. As a consequence, the poposed planne integates seamlessly planning and pediction and calculates a complete motion pediction of the scene fo each obot popagation. Making use of the Extended Social Foce Model (ESFM) allows an enomous simplification fo both the pediction model and the planning system unde diffeential constaints. Simulations and eal expeiments have been caied out to demonstate the success of the poactive kinodynamic planne. I. INTRODUCTION The impact poduced by the deployment of sevice obots is of vital impotance fo the acceptance of obots in cowded uban envionments, specifically among humans in its natual habitat. In the pesent wok, we popose a planne that pedicts human motion and minimizes its impact on all those neaby pedestians. Time estictions ae significant in social envionments: people walk and change thei positions duing time. A cost-based navigation path is calculated while satisfying both dynamic and nonholonomic constaints, also efeed as kinodynamic constaints. Pediction methods ae of geat impotance. Most appoaches sepaate planning and pediction and although a joint appoach may seem to boost the poblem complexity, we will pesent a simple method to jointly account pedictions and planning by consideing a union state of people and obots. Human motion pediction can be achieved though leaning techniques, like in the woks of [1], [2] and [3], whee they make use of maximum entopy leaning methods using a linea combination of diffeent kinds of featues. In this wok, we apply geometical based pedictos such as the woks of [4] and [5] that infe human motion intentions and aftewads pedict human motion in a continuous space, accoding to the Social Foce Model (SFM) [6], and the Extended SFM [7]. Woks such as [8] and [9] aleady The authos ae with the Institut de Robòtica i Infomàtica Industial, CSIC-UPC. Lloens Atigas 4-6, 08028 Bacelona, Spain. {gfee,sanfeliu}@ii.upc.edu. Wok suppoted by the Spanish Ministy of Science and Innovation unde poject RobTaskCoop (DPI2010-17112). Fig. 1. Simulation envionment in a space time, whee people ae plotted as geen cylindes and thei pedictions ae dawn in the z axis, which coesponds to time. The tee of paths calculated by the obot appeas in blue and the best path is a ed line. poposed to model people as a summation of a Potential Field (PF), so it is not a novel idea. In addition, PF navigation algoithms [10] have been extensively studied in the liteatue. Despite thei advantages, thee exist many well known limitations, such as local minima o oscillations. Seveal appoaches ty to ovecome these limitations, such as [11], by using a andomized walking path when a local minimum is eached. The dynamic window appoach [12] and othe velocity constained appoaches [13] pemitted to conside obstacles and collisions. Unfotunately, they suffe fom local minima as well. Appoaches combining a DWA with a global planne like [14] solve the poblem by intoducing a global function. Ou appoach also elies on a global planne. In [15] they obtain a kinodynamic compliant tajectoy by decoupling the poblem into a seach in space and a posteio optimization of the path satisfying the estictions. Ou appoach integates the seach of a path avoiding obstacles as well as povides the inputs equied to execute that tajectoy consideing kinodynamic constaints. Duing the last decade sampling based techniques have become quite popula. Futhemoe, sampling based methods may take into account kinematic and dynamic constaints such as [16] and [17]. [18] poposes a social awae eactive planning in human envionments. A joint calculation of people s path and a obot path is done in [19] using Gaussian pocesses, and [9] uses a PF and minimizes its cost though people s Potential Fields.
Fig. 2. Oveview of the poposed planning scheme. II. PLANNING OVERVIEW On the one hand, people typically move on the scene changing thei positions duing some time, and theefoe, we must conside time-vaiant scenaios. On the othe hand, a obot tajectoy must satisfy those stong time estictions while consideing kinodynamic constaints. A state-space fomulation is explained in Sec. III. The Extended Social Foce Model is explained in Sec. IV and how we calculate the foces affecting people and obots. The use of the ESFM will simplify enomously the planning unde diffeential constaints since thee is no need to solve the bounday value poblem (BVP) to link poses. In Sec. V we popose to integate the pediction algoithm with the planning algoithm and solve the poblem in a holistic way. The pediction is caied out seamlessly and simultaneously while the planning is being calculated, as explained in Sec. V-E. Evey new obot popagation entails an estimation of the motion pediction of neaby people. As a esult, ou planne pesents poactive chaacteistics since we tend to initiate a change athe than a eaction to events. An impotant assumption is done: a global planne povides a valid path to the goal unobstucted by static obstacles. A geneal planning scheme can be seen in Fig. 2. Ou algoithm calculates fo each iteation a path to a goal avoiding moving obstacles like pedestians on the scene. The fist output action in {u(t ini ),...,u(t hoizon )} is executed, and in the next iteation, a new plan is calculated, and a new action is executed. This appoach pemits a fast adaptation to changing envionments, especially if the pediction estimation changes dastically. As we will see in Sec. VI and VII, ou algoithm is implemented in eal time to povide an adaptable local planning. The computed plan takes into account the eaction poduced by its actions and poduces the minimum distubances to othe neaby pedestians. III. STATE-SPACE FORMULATION Fo planning puposes in the pesent wok, we conside that both obots and people move in a two-dimensional space which epesents the uban envionment. Let X denote the wokspace and x X descibes the position x = [x,y] in a two dimensional space, as fo moving objects (including people) and as obots. The configuation space C z is defined as a configuation q z C z, whee z denotes diffeent configuation spaces fo people and obots. Since we take into account a kinodynamic teatment of the planning scheme, we define the phase space S z that only consides the fist ode deivative, whee s z S z is defined by s z = [q z, q z ]. In addition, we deal with stong time constaints, whee object movements alte the outcome of the planning calculations, and we should conside the augmented phase space ST z = S z time, whee time R +, and s z ST z is a state s z = [q z, q z,t] at time t. In geneal, we will addess the planning poblem using the state s z ST z. Finally, an action u z U z modifies the states s z, as it will be discussed below. A. People in the state-space People ae teated as fee moving paticles, and theefoe, no oientation is equied. Accodingly, the configuation space is equal to the wokspace C P = X and the peson s phase space ST P is descibed as s p ST P, whee s p = [x,y,v x,v y,t]. The input space U P fo the peson s action is u p U P which ae linea acceleations u p = [a x,a y ]. The kinodynamic model descibing the peson s motion is constained by the following diffeential equations: ṡ p = dc(s p,u p ) = v x v y a x a y 1. (1) It will be discussed late how the input vaiables u p ae calculated. B. Robot in the state-space We conside the obot model to be chaacteized by a unicycle obot model. Theeby, thee appea nonholonomic constaints in the obotic dynamic model due to the olling contacts between the igid bodies. The phase state of the obot ST R is descibed as s ST R, whee s = [x,y,θ,v,ω,t]. The obot action space is defined as u U R whee u = [a v,a ω ] ae the tanslation acceleation and the otation acceleation. Then, the esultant diffeential constaints ae: ṡ = dc(s,u ) = vcos(θ) vsin(θ) ω a v a ω 1. (2)
C. Joint state-space The joint state space ST consists of ST = ST R ST Pi, which consides the obot phase spacest R and the union of evey peson s phase space ST P. Coespondingly, the joint state s ST is defined as s = [s,s p1,...,s pn ]. Note that the vaiable time t = t(s) is equal to all the states that s consists of. We will efe to the obot state s (s) ST R and the peson ith state s pi (s) ST P. IV. EXTENDED SOCIAL FORCE MODEL We employ the Extended Social Foce Model (ESFM) [7], based on [6], fo navigation puposes since it povides a ealistic model descibing inteactions among humans in typical social envionments [5]. The ESFM consides humans and obots as fee paticles in a 2D space abiding the laws of Newtonian mechanics. The ESFM uses attactos and epulsos in the continuous space. The attaction foces assume that the pedestian n ties to adapt his o he velocity within a elaxation time k 1, f goal n whee v 0 n(q goal n (qn goal ) = k ( v 0 n(qn goal ) ) v n, (3) ) is the desied velocity vecto to each q goal n, and v n is the cuent velocity. The epulsive inteaction foces ae defined as follows: n,z = a ze (dz dn,z)/bzˆd n,z, (4) whee z Z, being Z = P O R is eithe a peson, o a static object of the envionment, o a obot. Fo each kind oeaction foce coesponds a set of foce paametes {k,a z,b z,λ z,d z }. The distance d n,z fom the peson n to the taget z and ˆd n,z is the unity vecto z n. Fo futhe details, see [7], [5]. Accodingly, the esultant foce is calculated as the summation: f n = f goal n (qn goal )+ n,j + j P\n o O n,o + R n,, (5) whee each taget on the scene, eithe a peson, o an obstacle, o a obot, contibutes to f n. A. SFM applied to the obot The objective is to teat a obot as a fee moving paticle in the space, similaly to people as explained above. Unfotunately, nonholonomic constaints educe the obotic platfom mobility, although it has full eachability in C R. We need to bidge the gap and povide an adjustment that pemits the obot being compatible with the ESFM. The esultant obot foce f = f θ + f θ consists of a component in the tanslation diection f θ, which diectly tansfoms into a tanslational acceleation and an othogonal foce f θ that does not contibute to the obot tanslation. The obot otation acceleation is computed in the following way: τ = f θ +k τ ω, (6) whee is the vecto adii of ou platfom, oiented to θ and k τ is a damping facto in ode to avoid oscillations. Algoithm 1 Poactive planning(q goal,s ini,t hoizon,k) 1: Initialize T (V,E) {ø} 2: V s ini 3: s paent = s ini 4: {qp goal i } = intentionality people pediction() 5: fo j = 1 to K do 6: if t(s paent ) > t hoizon then 7: q g = sample(c R,q goal ) 8: s paent = neaest vetex(q,t g ) 9: end if 10: u = calculate edge(s paent,q) g 11: s new = popagate vetex(u,s paent ) 12: J new = calculate cost(s new,u,q goal ) 13: V V {[s new,j new ]} 14: E E {u } 15: end fo 16: etun minimum cost banch(t ) V. PROACTIVE KINODYNAMIC PLANNING All the main featues of ou poposed planne have been discussed in Sec. II, and can be summaized as: A kinodynamic solution is calculated. Poactive planning in which planning uses pediction infomation, and pediction is dependent on the plath calculated. Pio equiement: a global planne povides a valid global path. At each iteation, the planne povides a locally valid path. The path computed minimizes the petubations on the scene, accoding to a cost function. Additionally, the poactive planne is fast enough fo a eal time implementation, as demonstated in Sec. VII. Algoithm 1 has fou inputs: the goal q goal, the initial state s ini, the hoizon time t hoizon, and the numbe of vetices K. The q goal povides the position and oientation of the final obot configuation. The initial state s ini ST contains the infomation of the obot state plus all people s states consideed on the scene. The hoizon time t hoizon specifies the tempoal window used to foecast the plan and the pedictions. The algoithm builds a tee T (V, E) and etuns the minimum cost banch. The edges E ae the obot contol inputs u U R, and the vetices V consist of the joint state s ST and the accumulated cost J R to each that vetex. With all those equiements in mind, we popose Alg. 1 which is inspied in a andomized kinodynamic planne [16], except fo some paticulaities that will be discussed below. A. Hoizon time and depth exploation The hoizon time paamete sets the amount of time that the planne foecasts in ode to obtain a path simila to a model pedictive contol (MPC). Although the set of inputs {u (t ini ),...,u (t hoizon )} is calculated, only the fist input command is executed and a new set of inputs is calculated
in the next iteation. The hoizon time bounds the egion of exploation C R to a cicle adii equal to t hoizon v max as depicted in Fig. 3. / C R due to the hoizon time limit, and accodingly, ou appoach obeys a depth seach stategy to develop banches of the tee T until the hoizon time is eached (Line 6 in Alg. 1). Usually q goal B. Space exploation The space exploation is done in C R whee a set of andom goals q g ae andomly calculated in ode to extend the tee T. These andom goals q g ae attactos that geneate valid obotic paths in ST until t hoizon is eached, as explained below. Since thee is a stong time estiction, q g ae sampled in the bounday of C R, to avoid bias and to ensue that the paths geneated indeed expand T. The andom goals q g can be seen in Fig. 3. The sampling is done using a Gaussian distibution centeed at the neaest and the vaiance values depend on the density of neaby people: when the density gows, the vaiance also augments. q C R toq goal C. Find neaest vetex Once a new andom goal q g is geneated, the planne finds the neaest vetex V in T to be the paent vetex fo the new banch to be calculated. If only pue distances ae calculated in C R, thee would exist a stong bias to select old vetices that ae nea t hoizon. The calculation of the new weighted distance is done as follows: d(s,q g ) = x x g +c θ θ θ g +c time t t ini. (7) D. Edge calculation Edges E ae obot contol inputs u U R. Despite that the joint state takes into account all people, we can only select the input actions fo the obot platfom. We calculate the esultant obot foce f by making use of the ESFM (5). This foce is tansfomed into an acceleation using (6), and thus, a obot action u = f /m that takes into account the mass of the obot m. The goal of the obot is the andom goalq g and at the same time eacts to the envionment obstacles, that is, it takes into Fig. 4. Tee T of paths in the space X time. On the left, the z axis epesents time. On the ight pojection of T in X. Algoithm 2 Vetex popagation(u,s paent ) 1: s new = s (s paent )+dc ( s (s paent ),u ) t 2: fo i = 1,...,N do 3: if s pi (s paent ) q goal 4: u pi = f ( q goal 5: s new 6: end if 7: end fo 8: etun s new = [s new then ),s paent,s new /mi = s pi (s paent )+dc ( s pi (s paent ),u pi ) t,s new p 1,...,s new p N ] account the states of the neaby people s p1,...,s pn. The diffeent paths ae computed by intoducing a set of andom goals q g that stee the obot to apidly exploe C R time. E. Vetex popagation fo evey peson on the scene. This calculation is caied out only once, at the initialization of the algoithm (line 4 in Alg. 1). We adapt the human pediction algoithm [5] to obtain a single popagation s new fom a given initial state s paent ST. Algoithm 2 fist popagates the obot state s accodingly to u and integates the diffeential equation (2) by using Eule integation. Then, fo evey peson on the scene, and Since the action calculated u only popagates the obot state s, we must update the joint state s fo evey peson consideed. We popose a poactive appoach in which the computed planning actionu is integated with the pediction algoithm. Fist of all, we need to infe human intentions. As poposed in [4], we calculate the most expectable q goal (line 3 in Alg. 2), an action u pi is calculated depending on the people on the scene and the new obot state s new (line 4 in Alg. 2). if the peson has not eached its infeed goal q goal F. Cost function and path selection We popose a metic that measues social distubances while navigating: the social wok [18]. The amount of social wok caied out by the obot fom t ini to t hoizon : Fig. 3. Random goals q g distibution, on the ight thee ae no people in C R and seach is concentated on the goal diection. On the left the density of neaby people on the scene is highe, and thus, the q g sampling distibution is widespead. W R = t hzn t=t ini f (t) x (t), (8)
whee f takes into account both the steeing foce (3) and the summation of social-foces due to neaby people o othe obstacles (5) and it is multiplied by the vaiation of position x at each t. Similaly, we can define the summation of social wok caied out by the people on the scene induced by the obot movement: W P = t hzn t=t ini i P f,i (t) x i (t). (9) We have measued the social wok of the people W P due to the obot plan, and the social wok caied out by the obot W R. Fo evey new vetex the total cost is: J = x x goal +k θ θ θ goal + k obot W R +k people W P. (10) Most of the time, eaching q goal may not be possible. Nevetheless, the algoithm calculates seveal banches in the tee T and etuns the banch with minimum cost function J at time t hoizon. VI. SIMULATIONS Real expeimentation is a delicate matte when thee ae people involved. Fo this eason, we validate ou planne in a simulated envionment befoe any eal inteaction with people takes place. The simulated envionment [7] consists of static obstacles and multiple people modeled as dynamic obstacles following the ESFM, quite simila to a eal uban envionment. The numbe of vetices K = 500 in the planne is fixed thoughout all simulations and expeiments. The pocessing time highly depends on the numbe of people evaluated since we ae consideing the popagation of the state fo evey peson on each vetex calculated. The aveage pocessing time, if an aveage of 8 people is consideed to appea on the scene, is aound 0.19s, pocessed in a Intel Coe2 Quad CPU Q9650 @ 3.00GHz and memoy 3.8 GiB. The simulated scenaio is as follows: the obot eceives a quey to a goal while a goup of pedestians walk in the aea in diffeent situations. A. Leaning paametes in simulations Befoe evaluating the method pefomance, we should povide an initial estimation of the planne paametes. Paametes in (7) wee manually chosen (c θ = 1.0, c time = 0.25) to build a tee without bias as explained in Sec. V-C. Fo the paametes in (10) and t hoizon, we have pefomed ove 7000 expeiments using a Monte Calo appoach to sample the cost paametes, and then, we aveaged the expeiment pefomances to find the optimal values. We povide statical obustness to the method by doing a lage numbe of expeiments and then calculate the expectation of the pefomance depending on the paametes. In Fig. 5 is depicted the leaning esults fo the t hoizon paamete. Nomalized social wok 1.9 1.8 1.7 1.6 1.5 1.4 1.3 Social wok w..t. hoizon time Robot wok Pesons wok Aveage wok 2 3 4 5 6 7 8 9 time hoizon [s] Fig. 5. Leaning esults in the simulated envionment fo the t hoizon paamete, showing a minimum in t hoizon = 5s fo the aveage function. These functions ae nomalized fo compaing puposes. A shot t hoizon esults on a staight tajectoy, whee the obot acceleates and stops if an obstacle appeas, which is not an efficient behavio. Intuitively, the highe t hoizon the bette. Howeve, as can be seen in Fig. 5 thee is a degadation of the pefomance of both social woks. As we obseved fo high hoizons, thee wee a shot numbe of banches in the tee to exploe good solutions, and thus, a highe numbe of vetices would be equied. The esults, k θ = 1, k obot = 2, k people = 8 and t hoizon = 5s. These paametes ae highly dependent on the leaning scenaio and the numbe of vetices K. B. Simulations testing We have tested the algoithm in the same simulated envionment using the paametes obtained above. We have compaed ou appoach with a eactive planne poposed in [18], which takes into account people on the scene. 1.8 1.6 1.4 1.2 1 0.8 0.6 0.4 0.2 0 Fig. 6. Reactive Poactive planning Simulation esults Execution time Robot social wok Pesons social wok Simulation esults, nomalized to the eactive appoach esults. We have compaed thee diffeent values as can be seen in Fig. 6. The execution time efes to the aveage time fo the obot to get to its goal unde the pesence of people. Ou appoach shows a bette pefomance fo the social wok poduced by the people and the obot. These magnitudes ae clealy coelated since we have obseved that in geneal the poactive planne ties to descibe tajectoies that avoid inteactions if possible, while the eactive appoach is not intelligent enough to avoid unnecessay inteactions on time. VII. EXPERIMENTS Finally, we have implemented the algoithm to un in a eal obot, the Tibi&Dabo obots [7] in a contolled eal envionment.
Fig. 7. Examples duing expeimentation in eal envionments. Dabo navigates while consideing othe people on the scene. In the bottom ow of the figue appeas an inteface, whee people ae plotted as geen cylindes and thei pedictions ae dawn in the z axis, which coesponds to time. The tee of paths calculated by the obot appea in blue and the best path is a ed line. In Fig. 7 is depicted a seies of expeiments with people. We obseved a bette behavio of the poactive kinodynamic planne with espect to ou pevious eactive appoach, although moe expeiments ae equied, in diffeent scenaios, and moe people on the scene. Fo moe details, check the videos at the autho s webpage http://www.ii.upc.edu/ people/gfee VIII. CONCLUSION AND FUTURE WORK In this pape we have pesented a poactive kinodynamic planne fo uban envionments that calculates in eal time a local path that minimizes the distubances to othe pedestians. The solution tajectoies take into account kinodynamic and nonholonomic estictions which ae mandatoy consideations fo a ealistic navigation in such highly time-vaiant scenaios like uban envionments. Additionally, ou appoach shows poactive taits since the obot tends to initiate change athe than eacting to events. The cost fo being poactive is a moe intensive pocessing since we must popagate the state of moving pedestians accodingly to the obot popagation. The tajectoy calculated minimizes the amount of social wok poduced by the obot navigation at the same time that minimizes its navigation wok and distance to goal. Fo futue woks we need to extensively test the algoithm in diffeent envionments and with moe people. REFERENCES [1] B. D. Ziebat, N. Ratliff, G. Gallaghe, C. Metz, K. Peteson, J. A. Bagnell, M. Hebet, A. K. Dey, and S. Sinivasa, Planningbased pediction fo pedestians, in Poceedings of the IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, 2009, pp. 3931 3936. [2] M. Kudee, H. Ketzschma, C. Spunk, and W. Bugad, Featuebased pediction of tajectoies fo socially compliant navigation, in Poc. of Robotics: Science and Systems (RSS), 2012. [3] M. Lube, L. Spinello, J. Silva, and K. O. Aas, Socially-awae obot navigation: A leaning appoach, in Poceedings of the IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, 2012, pp. 902 907. [4] G. Fee and A. Sanfeliu, Bayesian human motion intentionality pediction in uban envionments, Patten Recognition Lettes, vol. 44, pp. 134 140, 2014. [5], Behavio estimation fo a complete famewok of human motion pediction in cowded envionments, in Poceedings of the IEEE Intenational Confeence on Robotics and Automation, 2014, pp. 5940 5945. [6] D. Helbing and P. Molná, Social foce model fo pedestian dynamics, Physical eview E, vol. 51, no. 5, p. 4282, 1995. [7] G. Fee, A. Gaell, and A. Sanfeliu, Robot companion: A socialfoce based appoach with human awaeness-navigation in cowded envionments, in Poceedings of the IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, 2013, pp. 1688 1694. [8] E. A. Sisbot, L. F. Main-Uias, R. Alami, and T. Simeon, A human awae mobile obot motion planne, IEEE Tansactions on Robotics, vol. 23, no. 5, pp. 874 883, 2007. [9] M. Svenstup, T. Bak, and H. J. Andesen, Tajectoy planning fo obots in dynamic human envionments, in Poceedings of the IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, 2010, pp. 4293 4298. [10] O. Khatib, Real-time obstacle avoidance fo manipulatos and mobile obots, The intenational jounal of obotics eseach, vol. 5, no. 1, pp. 90 98, 1986. [11] J. Baaquand and J.-C. Latombe, Robot motion planning: A distibuted epesentation appoach, The Intenational Jounal of Robotics Reseach, vol. 10, no. 6, pp. 628 649, 1991. [12] D. Fox, W. Bugad, and S. Thun, The dynamic window appoach to collision avoidance, Robotics & Automation Magazine, vol. 4, no. 1, pp. 23 33, 1997. [13] R. Simmons, The cuvatue-velocity method fo local obstacle avoidance, in Poceedings of the IEEE Intenational Confeence on Robotics and Automation, vol. 4, 1996, pp. 3375 3382. [14] O. Bock and O. Khatib, High-speed navigation using the global dynamic window appoach, in Poceedings of the IEEE Intenational Confeence on Robotics and Automation, vol. 1, 1999, pp. 341 346. [15] C. Stachniss and W. Bugad, An integated appoach to goal-diected obstacle avoidance unde dynamic constaints fo dynamic envionments, in Poceedings of the IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, 2002, pp. 508 513. [16] S. M. LaValle and J. J. Kuffne, Randomized kinodynamic planning, The Intenational Jounal of Robotics Reseach, vol. 20, no. 5, pp. 378 400, 2001. [17] D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, Randomized kinodynamic motion planning with moving obstacles, The Intenational Jounal of Robotics Reseach, vol. 21, no. 3, pp. 233 255, 2002. [18] G. Fee, A. Gaell, and A. Sanfeliu, Social-awae obot navigation in uban envionments, in Euopean Confeence on Mobile Robotics, ECMR., 2013, pp. 331 336. [19] P. Tautman, J. Ma, R. M. Muay, and A. Kause, Robot navigation in dense human cowds: the case fo coopeation, in Poceedings of the IEEE Intenational Confeence on Robotics and Automation, 2013, pp. 2145 2152.