Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots

Transcription

1 Thoma M. Howard Alonzo Kell Roboti Intitute Carnegie Mellon Unierit Pittburgh, PA , USA Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot Abtrat We preent an algorithm for wheeled mobile robot trajetor generation that ahiee a high degree of generalit and effiien. The generalit derie from numerial linearization and inerion of forward model of propulion, upenion, and motion for an tpe of ehile. Effiien i ahieed b uing fat numerial optimization tehnique and effetie initial guee for the ehile ontrol parameter. Thi approah an aommodate uh effet a rough terrain, ehile dnami, model of wheel-terrain interation, and other effet of interet. It an aommodate boundar and internal ontraint while optimizing an objetie funtion that might, for eample, inole uh riteria a obtale aoidane, ot, rik, time, or energ onumption in an ombination. The algorithm i effiient enough to ue in real time due to it ue of nonlinear programming tehnique that inole earhing the pae of parameterized ehile ontrol. Appliation of the preented method are demontrated for planetar roer. 1.1 Motiation While the preent generation of mobile robot i ontent to moe from point A to B, and perhap aoid obtale along the wa, trul ueful mahine mut often interat with the world in wa more general than impl driing oer it. For autonomou ehile, trajetor generation algorithm an form the bai of an apait to ahiee a deignated tate of motion. Further, real-time algorithm are needed to do o in repone to information gathered on the fl b pereption. KEY WORDS mobile robot, trajetor generation, rough terrain, ontrained optimization, optimal ontrol, path planning. 1 Introdution In order to operate ompetentl in an enironment, a mobile robot mut undertand the effet of it own dnami and of it interation with the terrain. It i therefore natural to inorporate model of thee effet in a trajetor generator that determine the ontrol neear to ahiee a preribed motion. Trajetor generation i the problem of determining a feaible motion (or a et of feaible motion) that will permit a ehile to moe from an initial tate to a final tate gien ome model of the aoiated dnami. While thi two-point boundar alue problem i laial and well tudied, it remain quite omple to ole adequatel in pratie. In order to generate a mooth, ontinuou path on flat terrain (whih atifie an arbitrar number of ontraint inoling poition, heading, linear and angular eloitie, and/or urature), a nonlinear differential equation mut be oled. The addition of rough terrain to the problem further ompliate matter b oupling thee nonlinear equation of motion. Numerial method, uh a the one preented in thi artile, are required to ole uh problem for arbitrar terrain due either to it tpiall ampled repreentation or to the nonlinearitie of the model. Figure 1: Motion Planning on Rough Terrain. For ompetent naigation in hallenging enironment, the terrain hape mut be onidered in the generation of ontinuou motion trajetorie. The preented trajetor generation algorithm generate motion plan that aount for arbitrar terrain hape, ehile dnami model, and wheel/terrain interation model b linearizing and inerting forward model of propulion, upenion, and motion. Continuou motion i the ore apait of ontemporar mobile robot. Yet, future mahine will be required to addre peifi plae in hallenging terrain at peifi attitude and heading (Figure 1). At thee plae, the will be required to deplo implement to do omething ueful like meauring the ompoition of a rok, digging a hole, or plaing a load of material on a truk. Competent operation in luttered enironment require the apait to undertand preiel the entire pae of feaible motion and to earh it for a (or the bet) olution. The triial olution of line egment joining wapoint i often not feaible due to kinemati or dnami limitation on urature een for ehile that an nominall turn in plae when topped.

2 Continuou urature trajetorie an hae ertain adantage oer thi triial alternatie. For eample, the time to omplete the miion or the epoure to rik (uh a wheel lip) inreae when the ehile mut top and hange diretion. In the ontet of emi-autonomou operation, trajetor generation an be ued to drie the ehile to an operatordeignated wapoint or wapoe. Thi point-and-lik approah redue both operator workload and telemetr bandwidth relatie to ontinuou ar-like driing. It potentiall proide a better olution than might be ahieed otherwie beaue loing the peed loop on the ehile an mitigate the effet of laten. For autonomou operation, trajetor generation an be ued to aquire peifi terminal tate when the ontet i one of aquiring a fied goal point. When following a path, trajetor generator an orret for path following error b reaquiring a moing goal point at ome forward poition on the path. Trajetor generation an alo be at a a ore omponent of global motion planning. It an be ued a a mehanim to enode the onnetiit of tate pae in lattie-like network a in (Pitoraiko and Kell 5). In thi ontet, trajetor generation i the ke to enoding a earh pae that intriniall meet all mobilit ontraint. 1.2 Related Work In the ontet of robot motion planning, mot reearh in trajetor generation ha dealt with finding obtale-free path ubjet to nonholonomi ontraint auming flat terrain and imple ehile model. Two bai tehnique eit. The firt i equential earh of a graph whoe edge onit of dnamiall feaible low-order ontrol (ar, lothoid, et ). Thi tehnique produe a olution equene of thee low-order geometri primitie. The eond tehnique i ontinuum optimization produing a ingle high-order parameterized geometri primitie. Graph-earh method generate the globall optimal olution in the diretized network, while parametri optimization method earh the ontinuum of olution to find a loall optimal olution. The hoie i between a ampled global olution and a ontinuou loal one. Some of the firt work in trajetor generation inoled ompoing optimal path from a equene of line egment, ar (Dubin 57), lothoid (Kanaama and Miake 85)(Shin and Singh 9), and ubi piral (Kanaama and Hartman 89). The deire for higher-order geometri primitie wa intended to enable higher leel of ontinuit at the boundarie of the primitie. B-pline hae been ued to meet arbitrar poition and heading boundar ondition b defining a equene of knot point along the path (Komoria and Tanie 89). The onept of differential flatne, a propert of a la of tem ideall uited to trajetor generation, wa introdued b (Flie 92). Method baed on inuoidal and Fourier erie input funtion alo appear throughout the literature (Brokett 81)(Tilbur et. al. 1992)(Murra and Satr 93). Thee method eploit the geometr of the problem to ole for the unknown path parameter diretl. The annot generall ole for olliionfree path in an obtale field. Graph-earh tehnique hae been ued for a long time in kinodnami planning. In the ontet of robot manipulator, optimal joint trajetorie were planned in (Heinzinger et. al. 199) uing grid-earh. Thee method alo appl to the problem of oling for obtale-free and minimum-length path whih atif nonholonomi and boundar ontraint (Cann et al 1988)(Jaob and Cann 89)(Barraquand and Latombe 89)(Reed and Shepp 199)(Laumond et al 9). The drawbak of uing graph-earh tehnique for trajetor generation i the reolution lot due to diretization of tate pae and/or ontrol pae. The onl boundar tate that an be reahed are thoe that alread eit in the network. Variational (optimization) tehnique for trajetor generation, whih earh the ontinuum for a loall optimal olution, are a old a optimal ontrol theor and hae been ued in mot field that emplo automati ontrol (Bett 98). Thi approah generall ue numerial method to atif ome et of boundar ondition and/or minimize ome ot funtion b earhing for the aoiated parameterized or ampled ontrol. Automati generation of joint trajetorie uing optimal ontrol and ubi polnomial primitie were ehibited in (Lin et al 1983). Minimum-time path between boundar tate are treated a a ontrol problem in (Baker 89). (Jakon and Crouh 91) implemented the hooting method to ole for trajetorie uing ubi pline primitie. Energ minimization wa ued in (Delingette et al 91) to ueiel deform a ure until it met the boundar ontraint, but it wa found to be unuitable for real-time appliation. In (Laumond 95), a holonomi geometri path i found in an obtale field and path egment are moothed uing optimal ontrol. A near real-time optimal ontrol trajetor generator i preented in (Reuter 98), whih ole eleen firt-order differential equation ubjet to the tate ontraint. A real-time trajetor generation algorithm for differentiall flat tem i preented in (Faiz et. al ), where an approimation of nonlinear ontraint are replaed b linear inequalit ontraint. (Kim and Tilbur 1) ued method baed on oling an approimate linearized problem (when tem are input-output linearizable) for UAV trajetor planning. Some of the mot reent work in optimal ontrol trajetor generation inlude (Kalmár-Nag et al 3), where near-optimal path are ontruted for omnidiretional ehile uing bang-bang optimal ontrol method. Their method generate minimum-time omnidiretional trajetorie ubjet to ompliated dnami and atuator model. In (Nag and Kell 1)(Kell and Nag 3), our group preented a real-time algorithm whih ole the planar trajetor generation problem between arbitrar boundar tate b linearizing and inerting the equation of motion. All of the mobile robot trajetor generation method diued o far hae aumed a flat world. B ontrat, at the leel of global motion planning where primitie trajetorie are equened together, the nonflat terrain hape i often known. It i normall onidered onl in term of it effet on the oerall objetie being optimized rather than in term of it lower leel effet on the motion itelf. Some of the firt uh rough terrain work inoled uing A* on a earh-pae baed on the ioline of a relief map whih inorporated energ ot aoiated with eleation hange (Gaw and Metel 89). More ompliated terrain and ehile model are introdued in (Shiller and Chen 91)(Amar 93)(Bonnafou et al 1), whih inlude kinemati and dnami

3 model. In (Shiller and Chen 91)(Shiller and Gwo 91), optimal B-pline path are generated on a B-path repreentation of the terrain. (Amar 93) adapt a ub-optimal ubi pline path auming flat terrain to three-dimenional terrain uing a kinemati ehile model and enforing terrain ontat. A graphearh method uing a et of ar primitie wa ued in (Bonnafou et al 1) where ditane and rik aoiated with the robot orientation wa minimized. One tehnique, whih doe aount for the influene of terrain on motion, i the two-leel planner i preented in (Cherif et. al 1994)(Cherif 99). Thi work earhe for an optimal global path plan auming flat terrain but it enure onnetiit between the tate uing a loal trajetor generator that aount for terrain hape. The loal trajetor generator i et up a a graph-earh problem and i oled uing bet-firt earh. 1.3 Diriminator The method preented in thi artile differ from the bod of prior work diued aboe in eeral wa. The urrent tate of the art in nonholonomi trajetor generation ehibit two lae. Algorithm in the firt la produe mooth motion primitie on aumed flat terrain. The eond la produe rough terrain primitie that are generated from a earh, often of a graph, oer a diretized ontrol pae rather than the ontinuum. Suh tehnique hae not produed ontinuou motion at the juntion between motion primitie. Continuum motion generation algorithm to date hae not aounted for the effet of rough terrain and model of ehile dnami (e.g. dela, gain limit, wheel lip) at the leel of primitie motion. Of oure, the flat terrain aumption greatl implifie the problem beaue it deouple the nonlinear tate equation of the tem (Howard and Kell 5) but it doe o at the epene of introduing model error for whih ontroller mut later ompenate. B aounting for terrain hape and model of ehile dnami, we eliminate thi ignifiant oure of error preditiel at planning time rather than reatiel at eeution time (in feedbak ontrol). Terrain hape i generall known (to the aura of the pereption tem) and it i alread ued elewhere in the ontrol of mot rough terrain autonomou ehile, o terrain adaptie lower leel ontrol uh a trajetor generation are an ineitable deelopment that we deribe in thi artile. 1.4 Tehnial Approah A highl general deription of the trajetor generation problem i that of generating a et of ontrol (u) whih atif a et of tate ontraint (C) ubjet to a et of goerning differential equation (f(,u,t)) deribing the tem dnami: f (,u,t) (1) C (, t) (2) Our formulation i one that tranform the bai optimal ontrol problem into one of nonlinear programming. We aume ome parameterized et of ontrol, and then ompute the repone to an initial gue for the ontrol. Initial guee are generated baed on lookup table for flat plane olution. The Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot mapping from ontrol to repone i then linearized numeriall with repet to the parameter, and it i inerted numeriall to proide the firt order update to the gueed parameter that eplain the deiation of the omputed endpoint from the deired endpoint. Continued iteration refine the etimated ontrol until the terminal tate error deend below ome threhold. The earh pae we onider hae been obered to hae a large radiu of onergene and few loal optimum, making uh method effiient and reliable in pratie. The numerial linearization in the aboe formulation i able to inert a forward model of a ehile without epliitl enoding anthing peuliar to the ehile. Howeer, the forward model doe model the ehile peifi. An arbitraril omple ehile model an be ued to predit how the ehile will moe oer the terrain. In our upenion model, we generall enfore terrain ontat, poibl b artiulating io-elati degree of freedom, to determine attitude and eleation gien poition and heading. Howeer, dnami model admitting balliti motion are alo poible. In our propulion model, uh element a atuator dnami, rate limit, wheel lip, and the eloit kinemati are inluded. More ompliated model inoling fore, ma propertie, and terramehanial model are onitent with our framework. In ummar, thi artile will preent an approah to the problem that ue parameterized ontrol and nonlinear programming to earh the ontinuum of ontrol pae effiientl for an optimum trajetor while preering the abilit to do o in real-time. The numerial approah ued alo make the approah appliable to arbitrar terrain hape and arbitrar ehile model atuated in arbitrar wa. 1.5 Laout Thi paper i diided into een etion. Setion 2 deribe the trajetor generator tem arhiteture, inluding detail about the boundar tate definition, parameterized ontrol, numerial optimization, integration, and imulation tep in the proe. Setion 3 diue initial guee for the ehile ontrol parameter and etion 4 and 5 detail the eperiment ued to tet the apabilitie of the algorithm. Appliation, onluion, and future work are diued in etion 6 and 7. 2 Trajetor Generator Stem Arhiteture The trajetor generation algorithm ehibit a three-leel arhiteture whih eparate the trajetor generation (numerial optimization whih minimize ontraint error), motion predition (numerial integration to predit motion), and the ehile imulation method (Figure 2). The initial and final tate boundar pair, the parameterization of the ontrol and the ehile model omprie the input to the trajetor generator. The output i a trajetor, whih we define a the union of the path (ompried of a etor of ehile tate) and the orret parameter for the ontrol. The ehile model i defined eternall to render the approah ehile independent. Setion 2.1, 2.2, 2.3, 2.4, and 2.5 will diu the boundar tate definition, parameterized ehile ontrol, trajetor generation (numerial optimization), motion predition (numerial integration), and the ehile model repetiel. 3

4 (, u, t) (, u, t) (, u, t) (, u, t) ( tf ) ( tf ) ( tf ) ( t ) f1 f f2 f f3 C(,t) f k f4 kf k f M M M (5) Figure 2: Trajetor Generator Stem Arhiteture. The trajetor generation algorithm an be oneptualized a a three-leel hierarh. The highet leel i a numerial optimization that minimize the ontraint error b adjuting the free parameter in the ontrol. The net leel, motion predition, i impl the numerial integration of the equation of motion. The lowet leel, the ehile model, imulate the ehile dnami, motion, and upenion. Thi arhiteture i formalized and implemented to permit different ehile model to interfae with the ame underling algorithm. 2.1 State Contraint Definition One of the impler form of trajetor generation problem i a form of the two-point boundar alue problem where it i onenient to ontrain the ehile tate at the boundarie of the path. The integration of the equation of motion require the initial tate of the ehile, and the terminal tate define the target tate of the ehile at ome forward time t f (Figure 2). The mot bai tpe of tate ontraint inlude world-frame poition (,,z) and orientation (,,). Howeer, ine roll (), pith (), and eleation (z) are determined (under aumption of terrain ontat) b the poe and the interation between the ehile upenion and the terrain, onl the poition (,) or poe (,,) boundar ontraint on poition and orientation an generall be peified. Tpiall there are alo requirement on the ehile ontrol that mut be atified at the initial and terminal tate. Linear and angular eloitie () and aeleration (a) are often ontrained for a dnamiall feaible motion plan. A tate etor i formed for the initial ( ) and terminal tate ( f ) of the robot: f [,,,, K] T (3), [,,,, K] T (4) f f f f f, The initial tate ontraint i tpiall atified triiall and it alue i ued to eed the numerial integration while the terminal tate peifie the ontraint at the end of the trajetor. We will write the ontraint on the tem in the following manner: 2.2 Parameterized Vehile Control In addition to the boundar tate, the algorithm require a et of parameterized ontrol that enode the motion of the ehile (Figure 2). While the pae of parameterized ontrol ma repreent onl a ubpae of all feaible motion, an appropriate hoie of parameterization an repreent nearl all poible ontrol. The et of all parameterized ontrol (u) for the ehile i defined a a funtion of the parameter etor (p) and ome ditinguihed independent ariable (ζ): u f ( p,ζ) (6) The free parameter in the parameter etor repreent knob that allow the algorithm to hange the hape of the ontrol. The number of free parameter in the parameter etor minu the number of ontraint repreent the number of remaining degree of freedom in the tem Bod-Frame Vehile Parameterized Control Chooing the proper et and parameterization for ehile ontrol (u) require an eamination of tpial wheeled mobile robot mobilit tem (Figure 3): Figure 3: Mobilit Stem Model. The hoie of mobilit tem determine the ehile apait to eeute omple motion. Omnidiretional mobilit tem are mot apable, allowing intantaneou motion in an diretion in the loal tangent plane. Allwheel teering mobilit tem allow motion in an diretion in the loal tangent plane, but are retrited b nonholonomi ontraint. The kid-teering, Akermann teering and orner teering mobilit tem are the leat fleible mobilit tem beaue their linear eloit i ontrained along the forward -ai of the ehile frame.

5 Nonholonomi ontraint are often onenientl epreed in the bod frame when the an be epreed b impl eliminating degree of freedom. Negleting artiulation, wheeled mobile robot moe rigidl with three degree of freedom in the loal tangent plane. Hene, bod-frame linear (, ) and angular eloitie ( z ) are natural andidate for the ontrol beaue the redue the dimenion of the input pae to a minimum. Thi hoie an be made without lo of generalit beaue a peifi implementation might elet to inlude a mapping from wheel leel ontrol onto bod motion before the tep diued below. Wheel eloitie and teering angle are found b mapping the bod frame linear and angular eloitie through the upenion kinemati. Skid-teered, Akermann, and orner-teered mobilit tem are the implet loomotion method beaue their linear eloit i ontrained to lie along the forward -ai of the ehile ( ). Omnidiretional mobilit tem hae holonomi wheel whih allow tranlation in an diretion in the loal tangent plane (, ). All-wheel teering mobilit tem allow tranlation in an diretion in the loal tangent plane o long a the path heading arie ontinuoul. The parameterization of the ontrol in term of the linear eloit in the and -diretion i not alwa onenient. When the number of ontrol ariable eeed the number of degree of freedom in the tem, epliit ontraint mut be formulated. A onenient et of ontrol for all-wheel teering mobilit platform enode the independent linear eloitie in the loal tangent plane in term of the peed in the loal tangent plane ( ) and the bod frame referened eloit diretion (δ) (Howard and Kell 6). o in () δ ( δ) 1 δ tan ( ) Notie that the kid-teered, Akermann teered, and ornerteered mobilit tem are impl a peial ae of the allwheel teering mobilit tem definition where the ontinuou diretion funtion (δ) i zero. Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot (7) Linear Veloit Profile Tpiall, it i onenient for loal motion planning algorithm to ontrol the peed and hape of the trajetor independentl. The initial peed, traeral peed, and the terminal peed are often known along with the deired linear eloit profile. An arhetpal linear eloit profile for ehile i the trapezoidal profile (Figure 4), defined b an initial eloit ( ), initial aeleration (a ), traere eloit ( traere ), terminal deeleration (a f ), terminal eloit ( f ), and the traeral time (Δt). Smoother (where aeleration and it deriatie are defined), impler (linear profile between and f ), or more omple profile (eeral intermediate eloitie) an alo be applied in the ame framework. [ a a Δ ] T p traere f f t (8) Figure 4: Trapezoidal Linear Veloit Profile. Tpiall mot of the parameter defining the hape of the linear eloit ontrol are known. For a trapezoidal eloit profile, the parameter in the ontrol are the initial ( ), intermediate ( traere ), and terminal ( f ) eloitie, initial (a ) and terminal (a f ) aeleration, and the duration (Δt). Angular Veloit Profile The hape of trajetorie are primaril determined b the angular eloit profile, o it i effetie to repreent the mot poible feaible motion with a minimum number of parameter. One effetie parameterization of an angular eloit ontrol i a polnomial funtion of time (Figure 5): Control Funtion Parameterization Now that the ontrol andidate for peifi ehile mobilit tem hae been defined, appropriate parameterized funtion mut be hoen to repreent the freedom in eah ehile ontrol. The tpial hoie for the independent ariable (ζ) in the parameterized ontrol are time (t) and ar-length (). Thi etion will detail eeral logial option for the parameterization of the ontrol. In general, an parameterized funtion an be applied proided it permit numerial linearization of the olution to the equation of motion. It i important to remember that thee parameterized ontrol are the input to the tem, not neearil the repone, o arbitrar parameterization do not iolate ehile dnami. Een diontinuou ontrol will generate a mooth repone when paed through the dnami. Figure 5: Polnomial Angular Veloit Profile. A polnomial funtion of time i an effetie parameterization of the angular eloit ontrol. The parameter in the ontrol are the polnomial oeffiient (a,b, ) and the duration (Δt). [ a b d Δt] T p z K (9) 2 3 (,t) a + bt + t + dt + K z p z (1) B the Talor remainder theorem, the polnomial an repreent all feaible motion to an deired degree of preiion gien enough term. It i alo differentiable, proiding an deired degree of ontinuit. When oling parameterized optimal ontrol problem, the omputation are bet onditioned if all of the freedom are of

6 the ame ale. Sine interpolation heme often ue polnomial funtion to join a et of ontrol point, the ontrol point themele are an equialent repreentation. Unlike polnomial oeffiient, though, the hae roughl equal ale (Figure 6): Figure 6: Spline Angular Veloit Profile. An alternate repreentation of a polnomial funtion i a pline funtion. The parameter in thi ontrol are the knot point ( 1, 2, 3 ) that define the hape of the angular eloit ontrol and the duration (Δt). Thi repreentation i benefiial for numerial optimization heme beaue all of the knot point hae roughl equal ale. where p [ K n Δt] T z ( t ), 1 ( t + Δt n), L n ( tf ) z 2 3 ( p z, t) â + bˆt + ĉt + dˆ t + K f ( p ),bˆ f ( p ),ĉ f ( p )K, where â 1 z 2 z 3 z (11) (12) 2.3 Trajetor Generation (Numerial Optimization) Gien the boundar tate and the parameterized ontrol, the method ued for finding the orret ontrol i nonlinear programming (Figure 2). Slightl different olution method appl depending on the degree of problem ontraint. For full ontrained problem, a ontraint reidual an be defined and when the algorithm i properl eeded, it will quikl onerge to an aeptabl mall reidual. Underdetermined problem enode a famil of olution from whih one bet olution mut be hoen. The riterion of bet i enoded b peifing ome utilit funtional to be ued to judge alternatie Contrained Trajetor Generation The ontrained trajetor generation formulation i the mot traightforward approah. The proe modifie the ariable in the parameterized ontrol (p) until the terminal tate of the forward imulated trajetor ((t f )) i equal to the defined target terminal tate ( f ), thereb atifing the ontraint equation (C()). Some of thee boundar ontraint are triial to ole beaue the an be equated diretl to ertain parameter in the ontrol (e.g. initial eloit). Non-triial ontraint (e.g. poition) mut be oled numeriall uing numerial method. The numerial method applied i Newton method, where the Jaobian of the olution to the equation of motion i inerted to find a orretion to the parameter in the tem needed to minimize the ontraint error (Δ f (p)), whih we define for the moment to be the differene in the imulated terminal tate and the terminal boundar ontraint: Δf ( p) Δ p ( ( t )) Δ ( p) (13) f f f 1 Δf ( p) Δ p Δ ( p) (14) If the Jaobian of the ontraint i non-quare (when the ontraint etor and the parameter etor are not of equal length), a peudo-inere an be applied to produe the leat quare or leat norm olution in order to generate a parameter orretion etor. Sine the partial deriatie of the integral of the equation of motion annot generall be found analtiall, (Howard and Kell 5) etimate mut be found numeriall. Forward (eq. 15) or entral differene (eq. 16) linearization of the forward equation of motion an be ued to etimate thee partial deriatie. The algorithm derie it ehile independene from the numerial etimation of all of the partial deriatie. Δi, j( p) Δi, j( pk + e, p) Δi, j( p) (15) e Δi, j k k ( p) q ( p + e, p) q ( p e, p) i, j k 2e f i, j k (16) Contrained Optimization Trajetor Generation In addition to the peudo-inere, optimal ontrol method an be ued to ole the underontrained formulation of the problem. In thi ae, the optimization proe reate the etra needed ontraint to define a loall unique olution. In the optimal ontrol formulation of the problem, parameter in the linear and angular eloit ontrol (p) mut be adjuted to atif the ontraint and minimize ome utilit funtional (J(p)). A in (Kell and Nag 3), thi i aomplihed uing the method of Lagrange multiplier The Hamiltonian (H) i defined a the um of the ot funtion and the produt of the Lagrange multiplier etor (λ) with the ontraint: H T T ( p, λ) J( p) + λ Δ ( p) J( p) + λ ( ( t )) (17) f The utilit funtional i a deription of what we want to optimize oer the path. In general, it take the form of: t f t f ( p ) Y(, p,t) dt (18) J The utilit funtional i oneied a a line integral of a potentiall time-aring utilit funtion (Y(,p,t)) along an unknown path. Equialentl, the problem an be formulated in term of ot rather than utilit. The time-aring utilit funtion an be onidered to be a (potentiall time aring) field oer the tate etor. It repreent an weighted ombination of utilitie or ot that are propertie of a gien tate. It ma inlude intantaneou energ onumption, wheel lip, lo of mobilit, rik, lope, path moothne, proimit to an obtale, or anthing ele of interet. Y (, p, t) w1yrik ( ) + w2yenerg(, t) + w3ymoothne( ) + K (19) The weight in the utilit funtional ineitabl repreent tradeoff like how far the tem hould be willing to go around an obtale in order to redue the rik, at a ot of lengthening the time to the goal. The tuning of thee weight i f

7 unaoidable and both appliation and platform peifi, and i outide the ope of thi artile. The firt-order neear ondition for optimalit are well known: H( p, λ) J( p) T Δf ( p) T + λ (n equation) (2) H( p, λ) Δf ( p) (m equation) (21) λ λ There are a total of n+m equation for thi tem, where n i the length of the parameter etor (and the number of unknown parameter in the tem) and m i the length of the Lagrange multiplier etor (and the number of ontraint in the tem). Thi tem i oled b linearizing the firt-order neear ondition. The initial gue of free ontrol parameter and the Lagrange multiplier are adjuted at eah iteration until a loall optimal olution i found. At thi point, the gradient of the Heian and the error in the boundar tate both approah zero. Eah iteration of the optimization inole a numerial olution of: 2 (, λ) Δ ( p) H p 2 Δf ( p) f T H p Δp Δλ Δf (, λ) T ( p) (22) A orretion fator for the ontrol parameter and Lagrange multiplier etor i found b inerting the relationhip in Equation 22: 2 H p 2 Δp β Δλ Δf (, λ) Δ ( p) ( p) f 1 T H p Δf (, λ) T ( p) (23) A tep ize aling fator (β) i ued to enhane numerial tabilit when initial guee are far from the loal olution. In plae of the ale fator, the optimal tep ize ould be determined b performing a more epenie line earh. The Heian of the Hamiltonian (H), like the Jaobian of the forward olution, annot be oled analtiall in our ae. Again, forward or entral differene are ued to etimate the partial deriatie: 2 k ( p) H ( p + e, p + e, p) H ( p + e, p) H ( p + e, p) H ( p) H i,j i,j k l i,j l i,j k + 2 e l i,j (24) 2.4 Motion Predition (Numerial Integration) The proe of numeriall integrating the differential equation that goern the motion of the ehile i eentiall one of imulation (Figure 2). Motion imulation are required to generate the trajetor orreponding to the parameterized ontrol and onequentl the Jaobian and the Heian of the Hamiltonian (beaue the are found b numerial linearization of forward olution). The proe i broken into three ditint Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot 7 tep: ontrol dnami, modeling of the wheel-terrain interation, and motion imulation. The firt tep, modeling of the ontrol dnami, i er important for auratel imulating real-world tem. Real tem hae motor aeleration and torque limit, laten, and joint limit that mut be modeled in order to produe an aurate imulation of how the ehile will repond to it ommand. The ontrol dnami modeling portion of the motion predition tage impl determine the ehile repone to the bod-frame linear and angular eloit ontrol: ( u, t) fontrol dnami(, u, t) (25) The ontrol dnami portion of the model determine the fore that the ehile will eert on the enironment, and therefore the reulting repone fore from the enironment. It i here where model of wheel lip, liding, and the ation of eternal fore on the tem are applied. The lat portion of the motion predition i the atual motion imulation, whih determine the hange in ehile poe (r world, o world ) oer a mall time tep (Δt): ( t + Δt) ( t) + (, u, t) Δt (26) Sine ground ehile generall annot ontrol their roll, pith, or eleation rate (the are funtion of the interation between the enironment and the upenion), new etimate of thee tate are omputed b the upenion model at the end of eah motion predition tep. 2.5 Vehile Model (Vehile Simulation) From a oding perpetie, the ehile model portion of the trajetor generation algorithm i an intane of the abtrat ehile model la that determine how the robot will repond to the ommanded bod-frame linear and angular eloit ontrol, hange it poition and orientation, and interat with it enironment (Figure 2). The ehile model i generall broken down into four element: a dnami model, a wheel/terrain interation model, a motion model, and a upenion model Vehile Dnami Model Vehile dnami model imulate the repone of the bodframe ehile ommand to the torque on and/or eloitie of the wheel. B modeling the repone to the parameterized ontrol, the olution found are dnamiall feaible. A few element of ehile dnami model are rate limit, joint limit, and laten. Rate limit repreent ontraint on how fat atuator an turn. Modeling thee effet an aount for drie wheel that moe lower than requeted or teering ero that lag behind their deired orientation. B ontrat, joint limit bound entire region of ontrol pae that ontain infeaible motion. An eample of uh infeaible motion i the impoible turn-in-plae maneuer in automobile. The front wheel an neer reah the teer angle required for thi motion. Aounting for laten in the tem i eential for generating orret trajetorie when the ehile tate an hange dramatiall oer the ale of the laten.

8 Thee tpe of ontraint an be handled in the trajetor generator b uing the repone to the input ontrol in the integration of the kineti motion model intead of the ontrol themele. Hard limit an be impoed on teering angle and model of atuator dnami an imulate dela in the motor ontroller. The impoition of uh joint and rate limit in the ehile model require a method for omputing wheel diretion and eloitie gien ome bod-frame linear and angular eloit. For determining the mapping from bod-frame linear and angular eloit to wheel eloitie, upenion artiulation rate an be temporaril negleted to implif the omputation. For uh an aumed rigid bod, the eloit of an point on the ehile an be determined from the linear and angular eloit of the bod frame. Speifiall at the loation of a wheel: wheel bod bod wheel r + (27) In the linear and angular eloit etor, onl the ontrollable eloitie (,, z ) need to be onidered. The etor r wheel i the diplaement from the bod frame to wheel ontat point. The wheel peed and diretion an then be determined from the omputed wheel eloit etor Wheel/Terrain Interation Model B prediting the wheel/terrain interation in the planning tage of the oerall autonom tem, rather than aounting for it in the eeution tage, more dnamiall feaible ehile motion an be generated. Traering high wheel lip enironment i a major hallenge for urrent planetar roboti tem, for eample and the preent algorithm promie to help ompenate for uh lip to the degree that it an be predited b the peialized pereption algorithm that are appearing (Angeloa 5) Vehile Motion Model The ehile motion model map bod frame linear and angular eloitie to world frame poition and orientation rate. A bodframe oordinate tem i defined with the poitie -ai pointing forward, the poitie -ai pointing to the right, and the poitie z-ai pointing down. The mapping of linear eloitie from the bod-frame ( bod ) to world frame ( world ) i ompleted b rotating the bod-frame,, and z ae b the Euler angle roll (), pith (), and aw () repetiel: ( ) ( ) ( ) bod z world world ROT ROT ROT r (28) + + z z world r (29) Thi relationhip an be inerted to determine the bodframe eloit etor in term of the global poition rate: + + z z bod (3) The mapping between the Euler angle rate and the bodframe angular eloitie ( bod ) an alo be found b tranforming the indiidual Euler rotation rate from their intermediate frame to the robot-fied frame: ( ) ( ) ( ) + + rot rot rot z bod (31) 1 z bod (32) Jut a with global poition rate and bod-frame linear eloitie, thi relationhip an be inerted to determine the Euler rate in term of the bod-frame angular eloitie: Ψ z t t 1 world (33) Equation (29) and (33) form the bai of the time-baed eloit kinemati. The rate of hange of global poition and Euler angle an be determined gien a et of ontrollable bodframe linear and angular eloitie: + z (34) Vehile Supenion Model Vehile upenion modeling i the problem of properl determining the attitude and eleation of a ehile gien other element of it urrent tate. Thi i an embedded optimization problem beaue it i often highl underdetermined. We ole thi problem b oneptuall allowing the ehile to float in altitude and attitude, and artiulate in upenion, while minimizing the reidual between the wheel and the terrain eleation under (or aboe) them. 3 Initial Guee for the Vehile Control Parameter Good initial guee for the ehile ontrol parameter are er important to the effiien of the trajetor generation algorithm. When initial guee of parameter are loe to the atual olution, fewer iteration of the algorithm are required to atif the boundar tate ontraint. Furthermore, when loal minima eit, a good initial gue i inurane againt falling into the wrong minimum. Sine initial guee for three-dimenional terrain, arbitrar ehile dnami, and arbitrar terrain model are diffiult to alulate (beaue of their dimenionalit), flat urfae olution with limited dnami are onidered. Hitoriall, approimation of the olution hae been found b hand-tuning polnomial funtion to data from a few dimenion (Nag and Kell, 21). Thi etion diue the problem of generating the dataet for the initial guee and two method of toring them: a initial gue lookup table and a neural network.

9 3.1 Lookup Table A lookup table i an effiient mean of toring initial guee for the ehile ontrol parameter gien that the pae of olution i low dimenional and i mooth. Thi i et another reaon for uing parameterized ontrol beaue the enode the entire hape of the trajetorie in relatiel little memor. Generall the mot important boundar ontraint whih influene the hape of the path are the initial and terminal poition (,), heading (), and initial and final urature (k,k f ), reulting in a fie-dimenional lookup table of olution parameter etor. More dimenion, uh a eloitie, an be inorporated into the lookup table if the hae dramati effet on the aura of the initial gue. In order to generate the initial gue lookup table effiientl, we ue preiou olution to eed neighboring trajetor generation problem in the table. Thi require a atifator initial gue for the firt trajetor generation problem, whih i ideall entrall loated in the lookup table. Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot important for appliation with limited torage uh a planetar roer. The olution are good andidate for mahine learning algorithm beaue the parameter of the olution are generall mooth and ontinuou. The downide to appling neural network for uh ituation i the upfront ot of learning the entire funtion. 4 Eperimental Setup Uing the method and algorithm preioul deribed, we hae built a ontinuou primitie trajetor generator for arbitrar ehile that aount for terrain geometr in the motion predition. The eperiment and imulation will ue a ehile model baed on the Rok 8 prototpe Mar roer (Figure 8). The ehile ha an all-wheel teering mobilit tem that i artifiiall ontrained in ome of the ubequent eample in order to demontrate trajetor generation for le apable mobilit tem. Thi etion will outline the tate ontraint (Setion 4.1), ontrol parameterization (Setion 4.2), trajetor generation algorithm implementation (Setion 4.3), upenion model (Setion 4.4), ehile dnami model (Setion 4.5), and the imulated enironment ued in the eperiment. 4.1 State Contraint For the erie of eample in Setion 5, we will ole the twopoint boundar alue problem tpiall aoiated with trajetor generation. It i important to note howeer that in general the algorithm doe not require that the tate ontraint be peified at the boundarie. The implemented error threhold ued to delare onergene of the algorithm i hown in Table 1. Table 1: Conergene Criterion 9 Figure 7: Neighboring Solution for Initial Gue Lookup Table Generation. The initial gue lookup table i generated b ontinuall oling neighboring trajetor generation problem in uh a wa that the lat olution i a good initial gue for the net quer. In pratie it ma be neear to adjut the order of the progreion through eah dimenion of the initial gue lookup table if onergene problem (due to dnami infeaibilit or parit of the diretization) are obered. The reolution and dimenionalit of the tored lookup table i a funtion of the robot torage and omputing power and i therefore platform dependent. 3.2 Neural Network Another method to generate the initial gue ued in the trajetor generator i to ue mahine-learning algorithm to fit a funtion to a large training et of trajetor generation eample. The initial gue lookup table from Setion 3.1 (or a ampled erion of it) an be ued a a good training et beaue the boundar tate pair are regularl eparated. The motiating fator behind uing uh a repreentation i the pae aing. A high-dimenional initial gue lookup table an require ten or hundred of megabte of torage wherea a neural network, whih require that onl the weight of the learned funtion be tored, require onl a few kilobte. Thi i State Contraint Poition (,) Heading () Diretion (δ) Curature () Required Aura.1 meter.1 radian.1 radian.1 radian Onl the terminal poition, heading, diretion, and urature mut be atified b the trajetor generator optimization. The poition, heading, diretion, and urature initial tate ontraint are oled triiall uing the urrent tate of the ehile. 4.2 Control Parameterization Thi implementation emplo a fifth-order polnomial pline funtion in urature and linear profile for the linear eloit and diretion. A urature profile i ued in the ame wa that the angular eloit i ued and the two are interhangeable. There are artifiial ontraint on the initial ontrol - that the be equal to the initial urature, linear eloit, and diretion tate ontraint for maimum ontinuit. 4.3 Trajetor Generation Algorithm Implementation Sine the length of the parameter etor (8) eeed that of the ontraint etor (6), the tem an be oled uing the ontrained or ontrained optimization trajetor generation

10 tehnique. The ontrained trajetor generation tehnique (Setion 2.3.1) i ued in Setion wherea the ontrained optimization tehnique (Setion 2.3.2) i ued in Setion Supenion / Kinemati Model In order to map bod-frame eloitie to wheel eloitie and to determine the orientation and onfiguration of the ehile on arbitrar terrain, the upenion of the ehile mut be modeled. The bod to wheel kinemati equation and were omputed in a manner imilar to (Tarokh 25). The ehile an be modeled a a erie of reolute joint and link a een in Figure 8: Figure 8: Kinemati Model for Rok 8. A kinemati model an eail be generated for mobile robot with Roker-Bogie upenion uing a erie of reolute joint. The kinemati model i ued to map bod frame eloitie to wheel frame eloitie and to determine the attitude and eleation of the robot gien it poition. The upenion model ha been implemented a a numerial optimization (a different appliation of Newton Method) that minimize the ditane between wheel ontat point and the terrain b adjuting the three roker-bogie freedom, roll, pith, and eleation. In general, the partial deriatie required b the optimization mut be found numeriall. Howeer an etimate of the Jaobian an be found analtiall b taking the partial deriatie of the forward kinemati equation of the wheel ontat point with repet to the bod-frame eleation, attitude, and upenion freedom. Thi olution i effiientl omputed online in the forward olution b uing the preiou tate a the initial etimate for the upenion optimization. 5 Eperiment and Reult Thi etion demontrate ome ue of the deeloped trajetor generator. A omparion of path generated uing the rough terrain and traditional trajetor generation i hown firt to demontrate the need to inlude the terrain geometr in the forward model. Then, the obered rate and behaior of onergene of the algorithm i diued followed b eample of motion generation for different mobilit tem, ae where ehile dnami are important, and the generation of optimal trajetorie. Reult are preented for a imulated ehile beaue thi i the bet wa to tet the algorithm on a tatitiall ignifiant et of ae. The main purpoe of the algorithm i to inert a model to produe feaible motion that meet the dnami ontraint enoded in the model and the boundar ontraint enoded in the problem peifiation. The fidelit of the model ued i an important but eparate and independent quetion that an onl be ealuated on a real ehile. We aert that, howeer the parameter of the ehile model ma hange in order to alibrate it better to realit; our trajetor generator will till be able to inert it. The algorithm ha been integrated into the CLARAt tem at the Jet Propulion Laborator and uefull field teted on the Rok 8 prototpe mobile robot platform. Due to pae limitation, a relatie few eample are preented in detail but the algorithm ha been under ontinuou ue and ealuation for oer two ear. 5.1 Rough Terrain Trajetor Generation Aounting for terrain geometr in trajetor generation i important in rough terrain enironment. Figure 9 how two forward imulation of trajetorie generated b alternatel auming and then not auming flat terrain. The rough terrain olution meet the tringent terminal ontraint (in three iteration), wherea in thi eample the flat terrain olution i off b 24.1% in relatie poition and 11.5% in relatie heading. 4.5 Vehile Dnami Model Varing dnami model, inluding model of wheel lip and liding are demontrated in etion 5.5. The eample in Setion and 5.6 appl ideal ehile dnami model in order to iolate effet of different apet of modeling. In general, all of thee appliation an ue arbitraril omple ehile model (joint limit, motor model, et ) at the burden of higher omputational ot. 4.6 Simulated Enironment The terrain in the eperiment i repreented a an eleation map generated uing fratal. A third-order Lagrangian interpolation heme wa implemented to determine the eleation at an gien poition in the enironment. Thi method wa preferred oer impler and le otl linear interpolation heme beaue third-order Lagrangian interpolation proide ontinuou deriatie at the boundarie between map ell.

11 Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot 11 Figure 9: Trajetor Generation Compenating for Terrain Geometr. An eample motion plan between a pair of boundar tate i hown with and without ompenating for terrain geometr. The trajetor plan ignoring the terrain geometr doe not reah the target terminal tate. There are three important point to take from thi figure: 1) Negleting the influene of the terrain geometr in the motion model lead to inorret trajetorie. 2) An path generated auming flat terrain will be horter than the real three-dimenional path between two arbitrar tate on general urfae. Trajetorie generated b ignoring terrain geometr will make turn too earl beaue the ehile will not reognize that it ha onl diplaed a fration of it apparent (,) poition hange. 3) The flat terrain olution i loe enough to the real olution to proide an initial gue to initialize the rough terrain trajetor generation algorithm. Een in the preene of large terrain undulation like the one hown in Figure 1, onergene tpiall require le than three iteration. Of oure, in modern roboti tem, uh error are tpiall treated uing feedbak ontrol and path traking. Howeer, the main point here i that uh error an be aoided entirel, before the fat, uing the method of the artile beaue pereption ued in loal planning an inform the algorithm on terrain geometr. 5.2 Algorithm Conergene To demontrate robutne to poor initial guee for the ehile ontrol parameter, the ame problem a etion 5.1 wa oled uing an artifiiall poor initial gue for the parameter of the ontrol and a maller tep ize aling fator (α.625). Figure 1 and 11 how trajetorie repreenting eah of the nine iteration tep required b the trajetor generator to onerge to a olution that reahe the target terminal tate. Figure 1: Trajetor Generation Conergene. The ame problem in Figure 8 i oled uing an artifiiall poor initial gue to demontrate robutne of the algorithm. A the method progree, ueie iteration of the optimization minimize the ditane between the imulated tate and the target terminal tate until the error reahe an aeptable leel. Figure 11: Trajetor Generation Conergene. The poition and heading error are hown for eah trajetor in Figure 1. Gien an artifiiall poor initial gue of the ehile ontrol parameter, the trajetor generator determined the orret olution in nine iteration of the optimization. Figure 11 demontrate the onergene propert of Newton Method a the parameter orretion i proportional to the urrent tate error. Een when proided with a relatiel large error in the initial gue, the algorithm i able to onerge to the orret olution beaue of the relatie oneit of the olution pae. 5.3 Rough Terrain Trajetor Generation for Intrument Plaement with Retrited Mobilit Intrument plaement problem for mobile robot tpiall require a loal planning algorithm to generate a trajetor to a terminal tate where it poition and heading are defined. The poition and heading boundar ontraint are tpiall et uh that the target i within the range of motion of ientifi intrument or the field of iew of enor or amera. To demontrate a more general eample of the preented trajetor generation algorithm, we hae ued the algorithm to plan trajetorie to iit een equential ientifi target a een in Figure 12. Here the imulated mobile robot all-wheel teering apabilit i ontrained to demontrate motion planning for kid-teered, Akermann, and orner-teering mobilit tem (linear eloit i retrited to be along the -ai of the ehile). The generated motion plan onit of een forward and fie reere trajetorie all ubjet to the ehile nonholonomi ontraint. The trajetorie ued trapezoidal eloit profile with zero initial and terminal eloit, ±2. meter/eond 2 aeleration and deeleration, and a traere peed of ±1. meter/e. Figure 12 how iew of the imulated intrument plaement enario and Figure 13 plot the poition and heading error a funtion of the number of iteration of the algorithm.

12 etion 5.3 i preented in Figure 14 eept that now the robot an ue the all-wheel teering mobilit tem. The motion plan illutrate how the algorithm an eploit all-wheel teering to generate moother, more effiient motion plan for multiple equened intrument plaement problem. Figure 15 plot the poition and heading error a funtion of the number of iteration of the algorithm. Figure 12: Intrument Plaement uing Corner Steering Mobilit Stem. Sequene of trajetorie are planned for an eample intrument plaement tak uing a orner teering mobilit tem. The target mut be aligned with the front of the robot hai beaue of the loation of the ientifi intrument. Eah of the een iene target are ahieed b planning twele trajetorie that inlude forward and bakward motion. Figure 14: Intrument Plaement uing All-Wheel Steering Mobilit Stem. The ame intrument plaement problem in Figure 12 i oled uing an all-wheel teering mobilit tem, allowing the ehile path heading to hange independentl from the bod aw. Eah of the een iene target are ahieed b planning onl een motion. The oerall motion plan (blak path) i more effiient than the orner teering motion plan (white path) b effetiel eploiting the all-wheel teering mobilit. Figure 13: Trajetor Generator Conergene for Intrument Plaement Tak uing Corner Steering Mobilit Stem. For the equene of trajetorie hown in Figure 12, the maimum, aerage, and minimum poition and heading error of the twele planned trajetorie are hown a a funtion of the number of iteration eeuted b the trajetor generation algorithm. In thi ituation, the termination ondition are atified b all twele trajetorie in fewer than four iteration of the algorithm when onidering the terrain geometr. A in the preiou eample, thi demontrate fat onergene of the algorithm in relatiel diffiult terrain geometr while illutrating a tpial appliation for the preented method. 5.4 Rough Terrain Trajetor Generation for Intrument Plaement and All-Wheel Steering Mobilit Stem The abilit to generate path in arbitrar terrain for all-wheel teering mobilit tem an endow the robot with the apait to moe effiientl through the enironment. Thi i epeiall important for planetar roboti appliation where energ i limited, the enironment ma be luttered with obtale, and the orientation of the robot i important (to deplo ientifi intrument). The ame intrument plaement enario a in Figure 15: Trajetor Generator Conergene for Intrument Plaement Tak uing All-Wheel Steering Mobilit Stem. For the equene of trajetorie hown in Figure 14, the maimum, aerage, and minimum poition and heading error of the een planned trajetorie are hown a a funtion of the number of iteration eeuted b the trajetor generation algorithm. In thi ituation, the termination ondition are atified b all een trajetorie in fewer than i iteration when onidering the terrain geometr and the all-wheel teering apabilit of the ehile. The ontraint error i larger beaue the initial gue ued doe not aount for the all-wheel teering apabilit of the ehile. In pratie, uh olution ould be enoded into the initial guee for the ehile ontrol parameter.

13 In omparing the motion plan for the intrument plaement enario preented in etion 5.3 and 5.4, the all-wheel teering motion plan i learl horter and more effiient. The planned all-wheel teering mobilit tem apable i 42.3% horter (24.33 eond eond) than the orner teering mobilit tem path. Of partiular interet are the all-wheel teering trajetor equene (2-3) and (6) in Figure 14, whih allow the mobile robot to irle a target while ontinuoul pointing it intrument toward the target. Trajetor generation algorithm that an eploit all-wheel teering tem an enable for more apable and effiient loal motion planning algorithm. Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot Trajetor Generation Conidering Vehile Dnami One adantage of the numerial linearization i that arbitrar model of ehile dnami an be inorporated. Often, dnami effet are negleted at the loal motion planning leel and dealt with at a lower leel through feedbak ontrol. Our formulation allow dnami model of wheel-terrain interation and other ehile dnami to be treated beforehand in the trajetor generation olution Trajetor Generation Conidering Wheel Slip Model Compenating for wheel lip i preentl one of the mot important mobilit problem for planetar mobile robot (Bieiadeki 5). Algorithm onergene i not guaranteed for an wheel lip model ine a reaonable initial gue of the olution i required. Howeer, if a reaonable initial gue i proided and onergene i till not ahieed, then there i likel no dnamiall feaible motion. Sine the trajetor generator ue the repone of bod-frame eloit ontrol, it i neear to firt determine indiidual wheel eloitie, appl a lip model on eah wheel, and then inert the bod kinemati to etimate of the bod-frame eloit repone. While thi approah i not a prinipled a fore-baed model of wheel lip, thi approimation i often ued in pratie, o we will ue it here. An often enountered problem inoling wheel lip i planning trajetorie for planetar roer while limbing hill a een in Figure 16. Tpial wheel lip model for uh appliation, repond with an attitude dependent perent of the ommanded eloit. When wheel lip i not modeled in the trajetor generator, path ome up hort. We an, howeer, ue the preented trajetor generation algorithm to ompute path that meet the target tate ontraint ubjet to thee wheel lip model. Figure 16: Compenating for Wheel Slip in Trajetor Generation. Wheel lip model an be inluded in the ehile model ued b the trajetor generator to plan motion. Notie that the forward imulation of the olution generated b the motion without a wheel lip model lide moe ignifiantl le than predited and it doe not reah the target terminal tate. Improed performane of path traking algorithm an be epeted b modeling thee effet intead of reling on feedbak ontrol to aount for uh error. The wheel lip model ued here alulate a perent wheel lip for eah wheel baed on the ehile attitude. The model parameter ued were onitent with oberation from field eperiment. In general, an wheel lip model an be implemented in the ehile model for uh ituation.

14 Figure 17: Compenating for Wheel Slip in Trajetor Generation. Wheel lip approimation an be ued in the forward model to enable preditie ompenation for thee effet in the trajetor generator. The plot on the left how the ommanded and repone linear and angular eloitie of the unmodelled olution. The plot on the right how the ommanded and repone linear and angular eloitie of the olution that model thee effet. Notie that the olution that aount for wheel lip take longer to eeute (9.21 eond eond) beaue the repone net eloitie on the lope are alwa le than the ommanded net eloitie. Figure 17 how the ommanded and repone linear and angular eloitie of the two path hown in Figure 16. Notie that the path generated b inorporating the wheel lip model take longer to eeute (9.21 eond eond) beaue the net eloit repone of the wheel lip model i alwa lower than the ommanded eloit. Alo, note that the repone eloitie are not partiularl mooth funtion depite the moothne of the ommand. Figure 18: Compenating for Sliding Dnami in Trajetor Generation. Sliding dnami model an be inluded in the ehile model ued b the trajetor generator to plan motion. Notie that the forward imulation of the olution generated b the motion without a liding dnami model lide down the lope and doe not reah the target terminal tate. Improed performane of path traking algorithm an be epeted b modeling thee effet intead of reling on feedbak ontrol to ompenate for uh error. In thi eample, the trajetor generator wa able to meet the boundar tate ontraint in fie iteration of the algorithm. Figure 19 how the ommanded and repone eloitie for the modeled and unmodeled ehile dnami olution. Notie that the unmodeled olution doe nothing to aount for liding down the hill, o the angular eloit remain ontantl at zero (the terminal tate i traight ahead of the initial tate). The olution that inorporate thee effet undertand that to ompenate for thee effet, it mut initiall turn up againt the lope, and hene it ha a non-zero angular eloit profile Trajetor Generation Conidering Vehile Dnami Model Wheel lip i not the onl form of dnami that an be aommodated. Sliding or lipping dnami an be inorporated in the ontrol dnami model in order to generate trajetorie that ompenate for thee motion automatiall. For eample, the planetar robot in Figure 18 i attempting to follow a proided trajetor while roing a lope. If the plan i generated and followed without modeling the liding effet of the lope, the robot will lide down the hill. Thi effet an eail be modeled a a eloit proportional to the gradient of the terrain, reulting in a planned trajetor that drie up the hill to ompenate for the downward liding effet. Figure 19: Compenating for Vehile Sliding Dnami in Trajetor Generation. Vehile liding dnami model an be inorporated in the forward model of the ehile to preditabl ompenate for thee effet in trajetor generation. 5.6 Rough Terrain Trajetor Generation uing Contrained Optimization The optimal ontrol formulation i appliable wheneer there are uffiient degree of freedom to optimize omething. We illutrate two different ae of trajetor optimization, peifiall uing minimum-ot and minimum-lope dwell utilit funtional. It i important to note that thee trajetorie are optimal onl oer the pae of feaible motion panned b the polnomial ontrol et. Howeer, we hae alo argued that

15 thi pae i a er good approimation to the ontinuou funtion pae of arbitrar ontrol Minimum-Cot Performane Inde In thi tpe of formulation, obtale are repreented a ot in a field that the robot traere. If a high-ot obtale i obered to be in the planned trajetor, tpiall the ehile mut either replan at the global leel or temporaril were from the target path and reaquire it behind the obtale. The were motion an be generated dnamiall in the optimal ontrol formulation of trajetor generator, where a tradeoff between minimum-ot and minimum-time i enoded in the oerall ot funtional. To find a bet ompromie, the utilit funtional (Y(,p,t)) i defined a the um of 1 and the weighted alue of the ot map at the urrent poition (,). tf t ( ) ( ) f u Y, u,t dt 1+ αot(, ) dt (35) J The 1 term in the integral repreent the time that it take to traere a ell and the ot(,) i the ot of it traeral. The parameter α ontrol the trade-off between finding the hortet path and the one that minimize the ot line integral. A α approahe zero, the minimum-time path will be found. Conerel a α approahe infinit, the loall minimum ot path will be found. A traightforward eample demontrating the effetiene of thi utilit funtion i that of aoiding a imulated loalized obtale. A ot field an be generated b penalizing proimit to the obtale a hown in Figure 2. Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot the ot of longer path length. Thi eample i appliable, for eample, to operator interfae where the operator might deignate the obtale b liking at it loation Minimum- Slope Dwell Performane Inde For planetar roer operating in rough terrain, it i ometime neear to minimize the amount of time pent on lope to redue rik. An adantage of the rough terrain ontrained optimization trajetor generation formulation i that we an gather information about ehile orientation along the path at no added omputational ot. We an therefore define a utilit funtion that penalize high roll and pith alue along the path to aoid lope: tf t ( ) ( ) f 2 2 Y, u, t dt 1+ α( + ) J u dt (36) Jut a with the minimum-ot eample from etion 5.6.1, the α term repreent the tradeoff between the minimum-time and minimum lope-dwell olution. Sine the numerial tehnique that we ue are deent algorithm, we will find onl loal optima o the tehnique annot be ued without modifiation when man uh minima are prealent. To demontrate the ue of thi utilit funtional, we tr to find the hortet path that drie oer a large pramid-haped hill. On flat terrain, the trajetor i triial it i a traight line between the two tate. Howeer, driing on the ide of a hill an be dangerou (tip-oer, liding, lo of tration, et.). We get a traight-line motion when our weight (α) i equal to zero beaue it i the minimum-time olution to the problem. A the weight (α) i inreaed toward unit, the algorithm onerge to a path that moe around mot of the hill. It doe not plan entirel around the hill beaue it i till looking for the hortet olution ubjet to thi penalt for high attitude. An inreae of the weight (α) toward two aue the path to aoid more of the hill at the ot of inreaing the time to the goal. Figure 21 how how the path differ from one another gien the different weight in thi formulation of the problem. 15 Figure 2: Minimizing Path Cot. The optimal ontrol formulation of the trajetor generator an be ued to minimize an arbitrar path ot funtion oer the oure of a trajetor. Thi eample demontrate aoidane of high ot region (proimit to a imulated obtale) at the epene of longer path length. The olution for zero α i triial, there i no ot ontribution from the imulated obtale and the minimum-time olution (a traight-line path diretl to the terminal tate) i found. A α i inreaed, the path deflet awa from the high ot region at

16 Figure 21: Minimizing Slope Dwell. Jut a in the minimum path ot eample from etion 5.6.1, the optimal ontrol formulation of the trajetor generation algorithm an be ued to mitigate rik inoling high lope when naigating rough terrain. Thi eample demontrate how inreaing the lope weight (α) aue the trajetor to aoid the hill at the ot of longer path length. 5.7 Runtime Performane The ompleit of the ehile model and the terrain hape ued in trajetor generation ha ignifiant effet on the epeted runtime of the algorithm. Thi etion benhmark the epeted performane with the preented rough terrain trajetor generation algorithm with a real ehile model. All tet were run on a GHz Pentium M notebook omputer with 1 GB of RAM. Runtime an be er dependent on the terrain roughne beaue of the ompleit of the upenion model ued ine, at eah tep of the numerial integration, the eleation, attitude, and the new upenion angle mut all be omputed ia nonlinear optimization. In order to inetigate the effet of terrain roughne on runtime, we ran 4,96 trajetor generation querie uing the Rok 8 ehile model on a erie of fift different terrain of inreaing roughne. Eah world wa generated b aling a ingle eleation map b roughne inde. View of the different height map ued throughout the tet are hown in Figure 22 and the aerage runtime. terrain roughne plot i hown in Figure 23. Figure 23: Runtime. Terrain Roughne. A erie of tet were onduted to determine the effet of terrain roughne on the runtime of the algorithm. The number of optimization tep required b the algorithm and hene the runtime of the algorithm i obered to inreae proportionall with repet to the terrain roughne. The aerage runtime of the algorithm i obered to inreae proportionall with terrain roughne. Thi i epeted beaue the upenion optimization mut perform more iteration to adapt the ehile to rough terrain and the initial gue of the ontrol parameter (whih are baed on the flat terrain olution) are going to be wore on rougher terrain. It i important to note that the ompleit an be aled appropriatel depending on the omputational apabilitie of the platform. 6 Appliation Our algorithm an endow a mobile robot with an unpreedented apabilit to predit the onequene of it own ation in relatiel hallenging enironment. Uing it, we an epet intelligent behaior in appliation uh a loal planning and obtale aoidane, path following, and global planning a outlined in thi etion. 6.1 Loal Motion Planning The mot logial appliation for the deeloped trajetor generation method i loal motion planning in omple enironment. One uh approah to loal motion planning i ego-graph, whih are bod-entered earh pae often ued for obtale aoidane (Laaze et al 98). The preented algorithm i highl effetie for generating ego-graph (Figure 24) beaue it i effiient and it an enode ehile dnami ontraint. Figure 22: Terrain Roughne Tet. In order to ondut a fair erie of runtime. terrain roughne tet, motion plan are generated on a erie of terrain of inreaing roughne ranging from a roughne inde of. (nominall flat terrain) to 1. (er rough terrain). Figure 24: Ego-graph Generation. The preented trajetor generation algorithm an be ued to generate ego-graph, whih are bod-frame fied obtale aoidane and loal naigation earh pae. The aboe two ego-graph, ompried of 63 indiidual trajetorie eah, were generated with different initial tate uing the preent algorithm. Note that onl the firt leel of the earh pae i dependent on the urrent ehile tate. Ego-graph an be initiall generated with a flat terrain aumption and ubequentl adapted to arbitrar rough terrain and dnami model in order to better ealuate feaibilit and true path ot (Figure 25). Sine the ego-graph generated uing thi algorithm are impl a olletion of path oled between

17 pair of boundar tate, the algorithm an effiientl adapt the olution to rough terrain uing the flat terrain olution a it initial gue. Howard and Kell / Optimal Rough Terrain Trajetor Generation for Wheeled Mobile Robot orretie trajetor i hoen that minimize ome utilit funtion baed on ro-trak error, moothne, and an other arbitrar fator (Figure 27). 17 Figure 25: Terrain-Adaptie Ego-graph. The ego-graph generated in Figure 21 an be adapted to rough terrain uing the deeloped trajetor generation algorithm in order to better ealuate dnami feaibilit and path ot. The fat that indiidual trajetorie all pa eatl through the intended terminal tate indiate ompenation for terrain hape. 6.2 Path Following Path following i the problem of finding ehile ontrol whih will allow the mobile robot to trak it target path. Tpiall path following algorithm rel on a relatiel fat feedbak ontrol loop uing a low-order motion primitie that reaquire the target path at ome forward point (e.g. pure puruit). Thi method ha been implemented uefull for man ear beaue a fat update rate an ompenate for unmodelled error. Figure 27: Reaquiring the Target Path. The rough terrain trajetor generation algorithm an be emploed in a ontrained optimization ene to determine the optimal orretie path to reaquire the target path. A ot funtion baed on ro-trak error, moothne, and other fator i minimized in order to determine the optimal orretie trajetor. 6.3 Global Motion Planning Trajetor generation an be ued to reate an inherentl feaible earh pae for global motion planning. One tehnique, illutrated in Figure 28, i to reate a regular lattie of tate and to onnet them with feaible motion that ere a the edge to tranition between tate. Initiall, the edge an be generated baed on a flat terrain lookup table and er few ditint hape are needed ine the node relationhip are mmetri under tranlation and rotation whoe magnitude are onitent with the ell ize. Thereafter, the edge an be adapted indiiduall to terrain hape to enfore ontinuit one the are atuall traered in earh, and potentiall one again when pereption information refine the terrain hape during plan eeution. Figure 26: Corretie Trajetor Continuit. Thi figure demontrate eeral different trajetor generation boundar ondition that lead to inreaingl higher leel of ontinuit for orretie trajetorie. The orretie trajetor beome inreaingl ompliated at the benefit of higher degree of terminal tate ontinuit. A more robut path traking algorithm an generate ehile ontrol baed on realiti model of dnami and wheel-terrain interation a in (Howard et. al 6). In thi approah, a et of andidate path following motion are generated b the optimal rough terrain trajetor generation algorithm that reaquire the path at ome forward ehile poture, enuring poition, heading, and urature ontinuit (Figure 26). An optimal eletion of the Figure 28: Connetiit of a State Spae Lattie in Rough Terrain. The tate of a lattie whih i equall ditributed in (,) an be onneted with minimum-time path in general rough terrain uing our trajetor generator.