Extension of Reeds & Shepp Paths to a Robot with Front and Rear Wheel Steer

Transcription

1 Extension of Reeds & Shepp Paths to a Robot with Font and Rea Wheel Stee Siddhath Sanan+, Dashan Santani+, K Madhava Kishna* and Heny Hexmoo $ +Manipal Institute of Technology, Manipal, India. sidsanan@yahoo.com *Intenational Institute of Infomation Technology, Hydeabad, India. mkishna@iiit.ac.in $ CSCE Depatment, Univesity of kansas Fayetteville, R bstact - This pape pesents an algoithm fo extending RS paths fo a obot with both font and ea wheel stee. We call such obots as FR stee. The occuence of such paths is due to the additional maneuve possible in such a obot which we call paallel stee, in addition to the ones aleady pesent in a vehicle with only font wheel steeing. Hence we extend the optimal path set Λ, containing only a single element to a set Ω, containing n elements, theeby extending its configuation set along the optimal path fom the initial to the final configuation. This extension of the set Λ to set Ω is made possible by intoducing a special set, which we call the Paallel Stee (PS) Set. Such an extension of the configuation set would incease the size of the final configuation set achievable by a path that is optimal in fee space. In the following discussion, we shall tem all paths whose length is equal to an RS path as optimal. Index Tems - Reeds and Shepp (RS) Paths, Paallel stee, FR stee, motion planning I. INTRODUCTION The poblem of motion planning constitutes the constuction of a feasible path fo a obot to move along, such that it achieves the final configuation fom the initial configuation. The tem feasible hee implies that the path confoms to the kinematics of the obot at evey point. Such poblems ae not only constained to the domain of mobile obotics but have been an aea of eseach since the vey beginning of the Robotic m. One of the main featues of motion planning in mobile obotics is non-holonomic motion planning [1], [2]. The essential chaacteistic of a nonholonomic constaint is that it educes the space of achievable velocities fo a obot to a dimension smalle than its configuation space. The essential outcome of this chaacteistic is that a geometically possible path fo the obot is not kinematically feasible. In this pape, we extend the RS paths to a obot with both font and ea wheeled stee that we call as FR stee. FR stee obot finds utility due to its smalle tuning adius than a nomal ca-like-obot that helps in navigating though tighte spaces. This smalle tuning adius is achieved by appopiate combination of font and ea steeing angles. Tajectoies of a FR stee obot ae a combination of staight lines and cicles as its kinematics eveal. Hence optimal paths fo a ca in the fom of RS cuves ae also optimal tajectoies fo a FR stee. The FR stee obot also has an exta maneuve called the paallel stee which is achieved when the font and ea steeing angles ae same in magnitude and diection. This maneuve esults in the extension of the optimal path set Λ containing a single element fo a nomal ca obot to a paallel stee (PS) set containing an abitay numbe of n elements, each of them giving the same distance between a given initial and taget configuation as would the oiginal RS tajectoy fo a nomal ca. The pimay advantage of this extension is that whee thee is an obstacle along the oiginal RS tajectoy fo the nomal ca, one could still come up with an equivalent optimal path that avoids the obstacle but etaining the optimal distance measue. This is achieved by selecting any one of the abitay n elements in the extended PS set. II. RELTED WORKS Motion planning with non-holonomic constaints fist appeaed in obotic liteatue though the seminal wok of Laumond [3]. Late Reeds and Shepp showed by geometic methods synthesis of optimal paths in thei famous pape [4] fo a ca-like obot that can move fowad and back. They illustated that a shotest path motion could always be achieved by means of tajectoies of a special kind, namely concatenation of at most five pieces each of which is a staight line o an ac and that these concatenations can be classified into 48 thee paamete families. y combining techniques of optimal contol such as Pontyagins Maximum Pinciple (PMP) and geometic methods like Lie lgebaic analysis of tajectoies, Sussman and Tang ecoveed the RS esults and impoved them by loweing the 48 to 46 [5]. simila effot was also published by oissonat et al. [6] on minimum paths fo an RS ca. In [1], aaquand and Latombe descibe a planne based on andomized techniques fo a multi-body nonholonomic obot in pesence of obstacles and in [7] a motion planne fo the RS ca was pesented. With the aival of andomized techniques, motion planning with nonholonomic constaints has been attacked unde the boade context of kinodynamic planning [8]. Howeve andomized techniques do not geneate optimal tajectoies both in pesence and absence of obstacles. In [11] an adaptive back stepping contolle fo a non holonomic obot with unknown paametes was designed and simulated. To the best of authos knowledge, thee has not been any aticle elated to motion planning fo a obot with FR stee. In

2 [9], a technique fo planning a velocity pofile on an a pioi given path fo a fou-wheel stee obot is pesented. ut [9] does not talk of a motion planne as such. In this egad, the authos believe that this could be one of the fist attempts to pesent a motion planne fo an FR stee obot that is optimal in the sense of RS path lengths. III. PROLEM DEFINITION ll pevious wok dealing with minimal length optimal path planning [3], [4], [5], [6], [10] has dealt with a ca model of a mobile obot with font wheel steeing only. The poblem hee is to find a motion planning scheme fo a vaiation of the RS ca model with both font and ea wheel steeing which we efe to as the FR stee model. deivatives of its configuation paametes in tems of the above paametes illustated in Fig. 1. The kinematics of the ca based on the above paametes is defined as: whee X& v cos( + ) Y & = v sin( + ) v f & cos (tan δ tanδ ) l f + l l = actan f tan δ + l l f + l tan δ f (1) and Fig. 1 icycle model of the FR stee ca. Model of the Ca The configuation space of the obot is in R 2 S 1 and given by the vaiables [X Y ]. The kinematic model of an FR stee obot is consideed hee unde assumptions of nonslippage of tie, plana motion and igid body. n FR stee ca unde such assumptions can be modeled as a bicycle [9] as illustated in Fig. 1. Refeence point C is chosen at the cente of gavity of the body and all kinematic analysis is with efeence to this point. The following paametes ae needed in such a model: γ R v f v δ f δ heading angle: the angle made by the longitudinal axis of the vehicle and the X-axis couse angle: the angle made by the velocity of point C with the X-axis side slip angle: the angle of the velocity of point C with the longitudinal axis of the ca the distance between the point C and the instant cente of otation (ICR) O velocity of fowad wheel velocity of ea wheel steeing angle of the font wheel steeing angle of the ea wheel To define the tajectoy of the ca at any time, we place the obot in a Catesian plane and epesent the time v f cosδ f + v cosδ v = 2 cos In the above model, thee ae fou contol vaiables, namely v f, v the two velocity inputs and δ f, δ the two steeing inputs. Integating (1) confims that the ca taveses a cicle l f + l with tuning adius centeed at O (Fig. cos ( tan δ f tan δ ) 1). This adius is the distance fom O to C. The paallel stee condition occus when δ f = δ esulting in an infinite tun adius. This implies that the ca tanslates without changing its oientation.. Popeties of the Ca Fo implementation pupose, conside a simplified model, in ou case v f = v always and δ = ±δ f. Due to this simplification, we can now set the efeence point C on the obot at the midpoint of the longitudinal axis. The condition δ = δ f gives the least tuning adius and is always used when having to tavese a cicula ac. Fo the paallel stee maneuve, we have δ = δ f and while moving along the egula staight line path δ = δ f = 0. We also add the following inequality constaints to the contol vaiables δ f,δ f. (2) δmax δ f δmax which implies (3) δmax δ δmax whee δ max is the mechanical limit of the steeing system. This constaint also causes the slip angle to be bounded by the following condition: (4) δmax δ max Constaints (2) and (3) impose a limitation on the lowe bound of the adius of cuvatue. We shall assume without any loss of geneality fo geneating RS equivalent paths that

3 min = ± 1 (5) (+1 when the obot goes fowad left o backwad ight). This assumption is outinely used [4], [5]. With the model completely defined, we now look at the poblem of finding minimal length optimal paths fo such a obot between an initial configuation [0 0 0] to a final configuation of [X Y ]. V. OPTIML PTHS In [4], the optimal paths fo a ca like obot ae found to consist of i) staight lines ii) cicula acs of adius of cuvatue min. They poved that the optimal path belongs to a minimal and sufficient set consisting of 48 paths o 9 path families. These ae given below: C C C, CC C, CSC, CC u C u C, C C u C u C, C C (π/2) SC, C C (π/2) SC (π/2) C, C CC, CSC (π/2) C (a) Hee C indicates motion along a cicula ac, S indicates motion along a staight line, indicates a cusp point o a point whee the diection of movement is evesed and the subscipt indicates the length of segment on the paticula ac o staight line. We use the modified notations of Sussman and Tang given in [5] to epesent a paticula path. l + indicates a left tun in the fowad diection, + indicates a ight tun fowad, the lette l indicates a stee which causes an incease in the state space vaiable and the lette indicates a stee causing a eduction in the same, the supescipt + o indicates the diection i.e. fowad o backwad. On substituting each of the above 9 families with the lettes defined above, a total of 48 paths ae obtained. We divide ou path planning pocedue into two phases. Phase I geneates standad RS paths between the initial configuation and final configuation. Phase II seaches fo paths whee multiple optimal solutions can exist and geneates these altenate paths.. The Paallel Stee Maneuve In the simplified model of the obot consideed, we tace the mid-point of the longitudinal axis. The state space of the FR stee obot consists of thee vaiables X, Y and. path along an ac of a cicle causes a change in all the thee state space vaiables while a path along a staight line causes a change in only two Catesian space vaiables X and Y. Fo a font wheel stee obot, the diection of the staight line coincides with the oientation of the obot. Howeve as seen in Fig. 2, the obot can move in a diection with an angula offset of ± fom the oientation of its longitudinal axis. Such a maneuve is known as paallel stee shown in Fig. 2 and Fig. 3. In Fig. 2, the oigin of the vecto indicates the obots position [X Y] and the diection of the vecto indicates the configuation vaiable. Such a maneuve intoduces what we call the Paallel Stee (PS) set which we shall discuss in the following section. In Fig. 3, it can be seen that in such a maneuve δ f = δ = and we call this angle δ and intechangeably dopping subscipt f and fom δ. (X,Y ) δ max (X 1, Y 1 ) 1 Fig. 2 Paallel stee at slip Fig. 3 The paallel stee maneuve (X 2, Y 2 ) a 1 a1 = a 2 2 Fig. 4 Elements of a PS set fo a configuation. The Paallel Stee Set In the RS paths, the configuation set which is the set of all configuations achieved by the obot in moving fom the initial to the final configuation contains elements, which define the path of the obot. Let us name this set Λ. The possible paths ae listed in (a). The Paallel Stee Set ( ) is geneated at the end points of the path i.e. between and by constucting paths fom and by using the PS maneuve only. We shall pove late that the PS sets thus geneated ae sufficient and contain elements that would yield a path that is equivalent in length to the RS path. Each element of the PS set is defined by: = {X,Y, X = X + Kcos, Y = Y + Ksin, = } (6) Hee K is a paamete epesenting the distance tavesed on the PS stee maneuve and satisfies (4). The physical desciption of these elements is given in Fig. 4 whee a 1 and a 2 ae two elements of the PS set at ( ).

4 Without any loss of geneality, we can limit the constuction at only. The configuations of the obot along these paths constitute the elements of the PS set as defined in (6). Ou objective is to find paths that have a nonempty PS set whose lengths ae equivalent to RS paths. t this stage, we can emodel ou poblem to i) finding a pai of elements (a,b) whee a is an element of the PS set at ( ) and b is an element of the PS set at ( ), ii) finding an RS path between these two elements fomed fom the configuation set Λ ab, and iii) connecting a, and b, with PS maneuve such that the length of the path found is equivalent to the length of the optimal RS path between the two configuations. Hence, the total configuation set in any path of the obot may be defined as Ω ( ) Λ ab (7) fo optimal set Ω o Ω o ( o o ) Λ abo (8) such that length{ω o } = length{λ ) (9) Within the PS set, evey path is a staight line with v = ±1 and obeys (4). Hee the speed v is taken as ±1 without any loss of geneality. C. Phase I We now poceed to develop a methodology to geneate RS paths between any two configuations though the method descibed in [10]. We develop an efficient algoithm to educe the seach among the families given in (a). We achieve this by developing a domain plane that contains the boundaies fo all path families listed in (a) fo evey final oientation f using [10] consideing the initial oientation to be = 0. This is obtained by applying the backwad and time-flip tansfoms to all the paths in the domain map of [10] since in [10], the initial configuation is set as [X Y ] and the final configuation is set as [0 0 0] while in ou model, the obot moves fom [0 0 0] to final configuation [X Y ]. Fo a futhe undestanding of these tansfoms, the eade is efeed to [10]. Once the domain map is built, it is seen that all paths have mutually exclusive egions of optimality. The algoithm, thus developed, yields an optimal RS path between any two given configuations. The gaphical simulation of one such path is shown in Fig. 5. D. Phase II Refeing to Fig. 6, is the initial configuation of the obot, is the final configuation of the obot and C is an intemediate configuation of the obot. n RS path in the above situation constitutes movement of the obot along cicle C1, then catching the tansvese tangent to cicle C2 and tavesing the shotest ac on C2 to each configuation. Such a path would occu in the following path families: 1. CSC, 2. C C π/2 SC π/2 C, 3. C C π/2 SC, 4. CSC π/2 C (b) C1 Fig. 5 Simulation of RS Paths In a font-only steeed obot, one such path would be the unique-distance-minimal path. Howeve in an FR stee obot, whee the paallel stee maneuve is possible, an intemediate configuation C which is an element of the PS set is obtainable which is connected by a standad RS path with the initial configuation (Fig. 6). The configuation C and whee C, ae finally connected though the paallel stee maneuve by steeing to a slip angle δ such that the movement of the obot is along a line that is at an inclination. It is equied to find C to satisfy (4) and (9), hence we have the condition + = θ C p C3 q = θ (10) It can be seen by simple geomety that ac(p q ) on C3 = ac(s t u) on C2 and u = ps. Hence the length of the path connecting the configuations -C- would be equal in length to the path connecting the configuations - within a given ange of K, K being the length of paallel stee. It is obvious that fo equivalent paths, K < length of tangent fom cicle C1 to C2. Of the fou path families mentioned in (b), it should be obseved that path family 2 will neve geneate a path with paallel stee maneuve fo a obot with a finite tun cicle adius. In path families 1, 3 and 4, the path between the CSC θ K Fig. 6 Path with PS set element at u C2 t s

5 lettes would be checked fo a possible paallel stee path that exists within the given bounds of δ and ( is always lesse than 90 ). path would also be seached fo in the backwad and time-flipped tansfoms of each path i.e. by geneating an optimal PS set and yielding a configuation element C which has oientation paallel to. If eithe of the seaches yields a paallel stee path, a value of K within the pemissible ange would be set and a Paallel Stee path etuned to the motion planne. ny value of K within the pemissible ange can be taken and an equivalent path found. sets shall be sufficient to geneate a path with the RS path length howeve these sets ae not exhaustive with espect to foming paths of length equal to RS paths. We do not attempt C VI. SIMULTION RESULTS gaphical simulation of the actual path geneated by the optimal configuation set Ω o is shown in the Fig. 7 and computational esults ae illustated in Table I. Fig. 7 Simulation of Path with optimal elements of PS set TLE I Lengths of Paths with PS Sets fo Vaious Values of K, Ψ K = 0 coesponds to RS path and K>0 coesponds to equivalent RS paths obtained by PS maneuve = = K 30º 30º Path : + s + l + Path : + s + + Path Length In Fig. 7, the path connecting the configuations - by an + s + + path is the RS path while the path connecting the configuations - with an intemediate configuation C is a path with the PS maneuve equivalent in length to the RS path. The diffeent cicles shown in Fig. 7 and Fig. 8 coespond to diffeent values of K which is the length of the PS maneuve. E. Non optimal elements in PS sets The elements in the PS sets which do not satisfy (10), if selected, geneate nonoptimal paths i.e. paths which ae not equal in length to RS paths. This fact was fist ealized by computational means using the algoithm developed. The computational esults fo vaious values of δ ae shown in Table II. We have seen though extensive computational simulations these elements belonging to PS set do not give RS equivalent paths but paths that ae longe. ased on computational simulations and esulting data we find (9) is violated fo a path fo which (10) is not satisfied and the paths ae longe. In (b) and (d), we stated that the PS sets shall be geneated at the end configuations and only and these K C ~ Fig. 8 Simulation of Paths with non-optimal elements of PS set TLE II Lengths of Paths with nonoptimal elements of PS set K = 0 coesponds to RS path and K>0 coesponds to paths obtained by PS maneuve = 30 30º 60º Path : + s + + Path : + s + + Path Length to pove this fact, but the case of PS sets at any configuation along the acs of a CSC path satisfying (9) may be sited as an instance of this fact. consequence of paallel stee is that it geneates RS path length even in the pesence of obstacles. Such an instance is shown in Fig. 9. In figue 9 an RS path is constucted between two configuations and along cicles C1 and C2. Due to the pesence of an obstacle, (shaded

6 ectangle) this path is not pemissible. y using the method descibed in the pevious sections, a path is constucted using the optimal elements of the PS set at. This path consists of a fowad ight tun on C1 ( + ), a fowad path connecting the cicles C1 and C3 (s + ), a fowad left tun on C3 ( l + ) and then a PS maneuve with satisfying (6) and v = 1. This path is topologically pemissible. The length of this path is equal to the oiginal RS path. This is a consequence of the PS stee maneuve. Fig. 9 Constuction of paths with optimal elements of PS set in an envionment with obstacles The Path planne was made fo ou FR Stee Robot that we call FRot. The FRot obot has a unique 4-wheel Steeing system (double ckemann) and has a fou wheel independent suspension. The fame eplicates the chaacteistic of Monste tucks built in the U.S.. It is built to move on multiple teain types fom smooth sufaces to ough teain with high fequency suface vaiations. The ot is being used as a eseach platfom/tested at ou lab. It was developed in-house at ou univesity though offthe-shelf components. It has povisions fo integating sona and CCR cameas and the PI fo these have been developed. Seveal clients can connect to it wielessly and the low level contol is un on-boad a Motoola s DSP based pocesso. The algoithm was coded in C language using OpenGL (Mesa 3D Libay) on the Linux platfom (Fedoa Coe 3.0). lgoithmica, vol. 10, pp , [2] Robot Motion Planning and Contol, J. P. Laumond, edito, LS- CNRS, Toulouse, Fance, ugust [3] J. P. Laumond, Feasible tajectoies fo mobile obots with kinematic and envionmental constaints, in Intelligent utomation Systems, F. C.. Goen, ed: Noth Holland, [4] J.. Reeds and L.. Shepp, Optimal paths fo a ca that goes both fowads and backwads, Pacific Jounal of Mathematics, vol. 145, no 2. pp , [5] Hécto J. Sussman and Guoqing Tang, Shotest paths fo the Reeds- Shepp ca: a woked out example of the use of geometic techniques in nonlinea optimal contol, Technical Repot SYCON-91-10, Depatment of Mathematics, Rutges Univesity, [6] J. D. oissonnat,. Ceezo, and J. Leblond, Shotest paths of bounded cuvatue in the plane, IEEE Intenational Confeence on Robotics and utomation, Nice, Fance, [7] J. P. Laumond, et al., nonholonomic motion planne fo mobile obots, IEEE Tansactions on Robot. and utomat., 10(5): [8] S. M. LaValle and J. J. Kuffne, Randomized kinodynamic planning, Poc. IEEE Int'l Conf. on Robotics and utomation, , [9] Danwei Wang and Feng Oi, Tajectoy planning fo a fou-wheelsteeing vehicle, Poceedings of 2001 IEEE Intenational Confeence on Robotics and utomation, May 2001, Koea, pp [10] Philippe Souées and Jean-Paul Laumond. Shotest Path Synthesis fo a Ca-Like Robot, IEEE Tansactions on utomatic Contol, 41(5): , May [11] T Fukao, H Nakagawa and N dachi, daptive Tacking Contol of a Non Holonomic Mobile Robot, IEEE Tansactions on Robotics and utomation, 16(5): VII. CONCLUSIONS We find that paths which ae equivalent in length to RS paths in fee space can be constucted by using the optimal elements of the PS sets. Such paths lead to the expansion of the configuation set fo obtaining a path which is equal in length to an RS path in fee space. n inteesting consequence of the exta degee of feedom obtained though evese steeing is that a path equivalent to RS can still be found while the oiginal path is not collision fee. This inteesting popety can be made use in motion planning in pesence of obstacles while etaining the path length of RS. REFERENCES [1] J. aaquand and J. C. Latombe, Nonholonomic multibody mobile obots: Contollability and motion planning in pesence of obstacles,