1_O? A.A9 Pittsburgh, Pennsylvania Path Generation for Robot Vehicles Using Composite Clothoid Segments. Dong Hun Shin and Sanjiv Singh

Transcription

1 044 CN (V) N4 Pth Genertion for Robot Vehicles Using Composite Clothoid Segments Dong Hun Shin nd Snjiv Singh CMU-RI-TR T C. re The Robotics Institute, LCT.Cmegie-M-2llon University A.A9 Pittsburgh, Pennsylvni December, Crnegie-Mellon University * S _O?

2 i REPORT DOCUMENTATION PAGE 1 Form Approved No. oo -oe ofm Public repoirting burden for this coliection of informtion - estimted to oef ge i our be, resboise. including the time for reviewing istructions, serching existing dt sources. gther nd mintining the dt needed, nd comletig nd reviewmng the collection of informtion Send comments regrding this burden estimte or ny other spect of this collection of in'ormton. nclud g suggesions for reducing this uren,. to Wshnqton Hediurters Services. Directorte for!nformtion Ooertlons nd Reoorts Jefferson Dvis miighwv. Suite Arlington. VA , nd to the Office of Mngement nd Budget. PPerwork Reduction Project ( ). Wshington, DC AGENCY USE ONLY (Leve blnk) 2. REPORT DATE 3. REPORT TYPE AND DATES COVERED I December 1990 Technicl 4. TITLE AND SUBTITLE S. FUNDING NUMBERS Pth Genertion for Robot Vehicles Using Composite Clothoid Segements 6. AUTHOR(S) Dong Hun Shin nd Snjiv Singh 7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) 8. PERFORMING ORGANIZATION REPORT NUMBER The Robotics Institute Crnegie Mellon University Pittsburgh, PA CMU-RI-TR SPONSORING; MONITORING AGENCY NAME(S) AND ADDRESS(ES) 10. SPONSORING:MONITORING AGENCY REPORT NUMBER 11. SUPPLEMENTARY NOTES 12. DISTRIBUTION AVAILABILITY STATEMENT 12D. D)STRIBUT;ON CODE Approved for public relese; Distribution unlimited 13. ABSTRACT,x;rrmi.'0wors) The response of n utonomous vehicle in trcking reference pth depends prtly on the nture of the pth. The co.dition for pths tht re intrinsiclly menble to follow re briefly presented, nd then method for the genertion of menble pths is proposed in this pper. Previous pth genertion methods hve sought to simplify pth by using rcs, superrcs, polynomil curves, nd clothoid curves to round corners, which result from poly-line fits through given sequence of points. The developed method consists of two steps: First, sequence of postures is obtined using given points, then ech pir of neighboring postures is connected with three clothoid curve segments. In the second step, completely generl method to connect pth of clothoid curves between two completely rbitrry postures ws not envisioned nd method for pir of djcent postures ws developed. By virtue of the property of clothoid curves, generted pth is continuous with respect to position, tngent direction, nd curvture, nd is liner in curvture. Aside from the properties innte to clothoid curves, the generted pths trnsition smoothly into turns, pss through ll the wy points, nd sweep outside the corners. For interpolting round obstcles tht re commonly inside the comer, these properties re especilly useful. 14. SUBJECT TERMS 15. NUMBER OF PAGES 16. PRICE CODE-" 17. SECURITY CLASS;FICATION 18. SECURITY CLASSIFICATION 19. SECURITY CLASSIFICATION 20. LIMITATION OF ABSTRAC OF REPORT OF THIS PAGE OF ABSTRACT_ unlimited unlimited unlimited

3 Tble of Contents Tble of Contents List of Figures List of Tbles Abstrct 1 Introduction 1 2 Continuous Pths Guide Point Vehicle Pth Modeling Clothoid Curves 7 3 Generting Posture-Continuous Pths Existing Pth Genertion Methods Pth Genertion from Sequence of Points 12 4 Results 22 5 Conclusion 25 Reference 28 Acceski, Fi..- NT:S CP.. DTIC ith,: ii iii iv By... D /t'b::.! I,,., L ***O... Dst Av

4 ii List of Figures Figure 1: Control Structure for the Autonomous Vehicle 1 Figure 2: Geometry of Bicycle Model 3 Figure 3: The Heding Alignment long Pth 4 Figure 4: Steering long Circulr Arcs 5 Figure 5: A Clothoid Curve 7 Figure 6: Open-loop Performnce:Time Constnt = 0.1 s, Vehicle Speed - 5 m/s 9 Figure 7: Open-loop Performnce:Time Constnt = 0.5 s, Vehicle Speed = 2 m/s 10 Figure 8: Pths Generted by Existing Methods 11 Figure 9: Obtining Postures nd Associted Circles from Objective Points 12 Figure 10: Determintion of the Sign of the First Clothoid Segment 13 Figure 11: Determintion of the Sign of the Lst Clothoid Segment 14 Figure 12: Cses by Curvture Vrition of Pi nd Pf 18 Figure 12: Cses by Curvture Vrition of Pi nd Pf 19 Figure 12: Cses by Curvture Vrition of Pi nd Pf 20 Figure 13: Numericl Method to Compute the Fresnel Integrls 21 Figure 14: Posture-continuous Pth From Sequence of Seven Points 22 Figure 15: Comprison of the Proposed Method with Three Others. 24 Figure Al: Reltive Geometry of Associted Postures 26

5 i List of Tbles Tble 1: Curve Loction by Shrpness nd Curvture 16

6 iv Abstrct The response of n utonomous vehicle in trcking reference pth, depends prtly on the nture of the pth. The condition for pths tht re intrinsiclly menble to follow re briefly presented, nd then method for the genertion of menble pths is proposed in this pper. Previous pth genertion methods hve sought to simplify pth by using rcs, superrcs, polynomil curves, nd clothoid curves to round comers, which result from poly-line fits through given sequence of points. The developed method consists of two steps: First, sequence of postures is obtined using given points, then ech pir of neighboring postures is connected with three clothoid curve segments. In the second step, completely generl method to connect pth of clothoid curves between two completely rbitrry postures ws not envisioned nd method for pir of djcent postures ws developed. By virtue of the property of clothoid curves, generted pth is continuous with respect to position, tngent direction, nd curvture, nd is liner in curvture. Aside from the properties innte to clothoid cu. yes, the generted pths trnsition smoothly into turns, pss through ll the wy points, nd sweep outside the comers. For interpolting round obstcles tht re commonly inside the comer, these properties re especilly useful.

7 1 Introduction The robot vehicle nvigtion problem cn be divided into four subproblems: world perception, pth plnning, pth genertion, 1 nd pth trcking, which cn be solved seprtely (Figure 1). World perception senses the world nd symbolizes it into fetures. Pth plnning typiclly uses these fetures to obtin n ordered sequence of objective points tht the robot must ttin. Pth genertion converts the sequence of objective points into pth. Then, pth trcking controls the vehicle to follow the pth. Tsk & World Mp Pth Pth Genertor iasempoints in crtesin spce A mimou ptd" On-fn incsrtsinspsce Rel-time Pth V1U& P =SPositiOn Controllers i-iactutors Figure 1: Control Structure for the Autonomous Vehicle As with ny control system, the response of n utonomous vehicle in trcking reference pth depends prtly on the nture of the reference pth. Actutors re not perfect; thus, if pth is esier for robot to follow, there will be fewer errors. I Pth plnning nd pth genertion together re often referred just s pth plnning. Some nvigtion schemes do not generte pth, but compute the sfe instnt motion of the vehicle s pth plnning. [Khtib85, Krogh84, Feng89]

8 2 not perfect; thus, if pth is esier for robot to follow, there will be fewer errors. For robot vehicle whose guide point is t the center of its rer xle, discontinuities re troublesome becuse n infinite ccelertion of the steering wheel is required to trck the reference pth. A rel robot (with finite ccelertion) trveling through such trnsition points will experience n offset error long the desired pth. In ddition, the extent to which steering motions re likely to keep the vehicle on the desired pth cn be correlted to the linerity of curvture of the pth, since liner curvture long pth requires liner steering velocity. In this pper, the condition for pths tht re intrinsiclly menble to follow is briefly presented, nd then method for the genertion of menble pths is proposed. Numerous pth representtion methods hve ppered in the literture. In the simplest method, pth is composed of sequence of stright lines nd circulr rcs; others hve used polynomil splines or clothoid curves. Previous methods hve, in generl, sought to simplify pth by rounding corners, which result from stright lines connecting given sequence of points [Knym85, Nelson88]. The method developed in this pper, however, genertes continuous pth with clothoid curves directly from the given points. In ddition, the method gurntees continuity of position, heding, nd curvture long the generted pth. Further, the steering rte is piecewise constnt. The following section presents criteri for pths tht re suited to pth trcking. The performnce of vehicle trcking pth tht ws designed without concern for ese of trcking is compred with performnce on pth tht is intrinsiclly esier to trck. Section 3 presents method for generting pths with clothoid curves. Then, in Section 4, the results re shown to evlute the performnce of the proposed method. Finlly, Section 5 dvnces the conclusions nd suggests the directions for future reserch.

9 3 2 Continuous Pths 2.1 Guide Point For our nlysis bicycle model is used s n rchetype for modeling robot vehicles with two degrees of freedom, steering, nd propulsion. The guide point is the point of the vehicle tht is controlled to follow the given pth. The choice of the guide point is n importnt decision-it ffects the desired steering nd propulsion functions required to follow the given pth nd speed. We hve chosen the guide point to be t the midpoint of the rer xle (Figure 2) resulting in the following dvntges: The steering ngle t ny point on the pth is determined geometriclly, independent of speed, in the following mnner: tn = r; * = tn"l C I where I is the wheelbse of the vehicle nd c is the curvture of the pth. Figure 2: Geometry of Bicycle Model Also, the ngulr velocity of the driving rer wheel is determined only by the vehicle speed (v) nd the rdius of the wheel (Rw): V (0 = RW (2)

10 4 If the guide point is plced elsewhere, expressions for nd 0) re more complex thn shown in Equtions 1 nd 2. The reference steering ngle nd ngulr velocity of the driving wheel must then be obtined by numericl integrtion. * The vehicle is ble to follow the minimum turning rdius for the mximum steering ngle [Nelson88]. In other words, the pek steering ngle is smller thn tht for ny other guide point. * The heding of the vehicle is ligned with the tngent direction of the pth. This gives more resonble vntge point for vision cmer or rnge scnner mounted t the front of the vehicle, s in Figure 3, nd smller re is swept by the vehicle. Figure 3: The Heding Alignment long Pth For the guide point t the center of the rer xle, pths with discontinuities of curvture will require infinite ccelertion of the steering wheel. Consider the motion of the vehicle long pth which consists of two circulr rcs (Figure 4). There is discontinuity in curvture t the point where the two circulr rcs meet. An infinite ccelertion in steering is required for the vehicle to sty on the specified pth provided tht it does not come to stop t the trnsition point, simply becuse it tkes finite mount of time to switch to the new curvture. In relity, moving through trnsition point with non-zero velocity results in n offset error long the desired pth. Likewise, rotry body inerti requires continuity of heding, nd steering inerti requires continuity of curvture.

11 5 Figure 4: Steering long Circulr Arcs This is not the cse if the guide point is moved from the rer xle, for exmple, to the front wheel. However, this choice loses the dvntges discussed bove. Selection of the guide point is importnt to the formultion of the pth trcking problem. When the guide point is chosen on the center of the rer xle, steering nd driving reference inputs re computed from the pth prmeters nd vehicle speed, respectively, s in Eqution (2). This enbles the plnning nd controlling of steering long pth to be independent of speed. It is referred to s pth trcking, in contrst to trjectory trcking in which steering nd speed re controlled long time history of position [Shin90]. 2.2 Vehicle Pth Modeling The configurtion of conventionlly steered vehicle moving on plne surfce cn be described completely by set of coordintes (x, y, 0v, 0); vehicle position, heding, nd steering ngle, respectively. The continuity of (x, y, 0v, 0) is recommended, becuse the vehicle hs inerti nd finite control response, which preclude discontinuous motions. Otherwise, trcking errors will be greter. Since pth specifies n idel motion of conventionlly steered vehicle, pth must gurntee the continuity of (x, y, 0 v, 0) of vehicle. A pth cn be prmeterized in terms of pth length s s (x(s), y(s)). Tngent direction (0(s)) nd curvture (c(s)) cn be derived long pth:

12 6 6(s) - dy(s) dx(s) c(s) = ~)(4) (3) The continuity of (x, y) is gurnteed if pth is continuous. But the effect of the continuity of vehicle heding nd steering on the pth depends on the guide point. Since the guide point is chosen t the center of the rer xle, s mentioned in Section 2.1, the heding of the vehicle Ov is ligned with the tngent direction of the pth 0, nd the steering ngle * is determined by the curvture of the pth, s in eqution (1) Thus, the continuity of (x, y, Ov, ) is tntmount to the continuity of (x,y,9,c). If we define posture s the qudruple of prmeters (x,y,o,c), posture describes the stte of conventionlly steered vehicle, nd pth is required to be posture-continuous for esy trcking. 3 Further, the rte of chnge of curvture (shrpness) of the pth is importnt, too, since the linerity of curvture of the pth dicttes the linerity of steering motion long the pth (1). If we ssume tht less control effort is required for n ctutor to provide liner velocity profile thn n rbitrry nonliner one, the extent to which steering motions re likely to keep vehicle on desired pth cn be correlted to the linerity of curvture of the pth. Certin spline curves (Nelson88I re good cndidtes for good pths, becuse they gurntee posture-continuity long pths. However, these spline curves do not gurntee liner grdients of curvture. Clothoid curves, by contrst, do not hve the sme properties of curvture nd vry linerly. In the following subsection, clothoid curves re introduced nd, re compred the performnces of vehicle trcking dothoid pth nd n rc pth. 3 Omni directionl locomotion systems, such s n ilontor wheel system [Dniel85] nd legged systems, cn follow pths with discontinuities of tngent direction nd curvture, since they cn subtend pure rottion or pure lterl motion. However, continuity of curvture is still recommended becuse it provides smooth chnges in the centrifugl force: Fcg = = c v 2 r

13 7 2.3 Clothoid Curves Clothoid curves [Knym85, Ytes52] re fmily of curves tht re posturecontinuous, nd re distinct in tht their curvture vries linerly with the length of the curve: c(s) - ks + ci (5) where k is the rte of chnge of curvture (shrpness) of the curve nd subscript i denotes the initil stte. A clothoid curve segment is shown in Figure 5. Figure 5: A Clothoid Curve Given n initil posture, shrpness of the clothoid segment, nd the distnce long tht segment, position, orienttion, nd curvture t ny point long the curve re clculted s (5, 6, 7, nd 8): e- O(s) = ei + 4) dt S2 + Cis + Oi (6)

14 8 s 0 X(s) = Xi + cose(4)d4 =, cos Ci 4 + 8i)d4 + xi (7) u s s 0 Y(s) = Yi+ J sin 0() d4 = f sink(1-2+ ci + 0i dt+ yi (8) u S The response of robot. vehicle to different reference pths ws compred. Open loop responses of vehicle following pth composed of rcs nd stright lines, nd nother composed of clothoid segments, re shown in Figures 6, nd 7: The vehicle is mde to follow set of steering commnds, which re obtined from the pth specifiction using (1), without trying to compenste for trcking errors. The dynmic responses of the vehicle re modeled s first order lg systems with hrd limits on ccelertion. In ech cse, the reference pth is indicted by dshed lines, while the ctul pth is indicted by solid lines. Simultion results show tht n rc pth results in lrger stedy stte trcking errors cused by discontinuities in curvture, s well s the prmeter vrition of ctutor nd vehicle speed. While the results shown bove support the clim tht "good" pths re intrinsiclly esier to trck, it turns out tht when using n optimized closed scheme nd fst ctutors, trcking performnce is only mrginlly better for smooth, linercurvture pths thn for pths composed of rcs nd stright lines, especilly t low speeds. At low speeds, the compensting controller mkes up for errors from curvture discontinuities, non-linerities, nd imperfect ctutors. For trcking pths with significnt curvture, it is essentil to ensure curvture continuity t high speeds or with slow ctutors.

15 Pottimn er u 0.4S1 d 0.37S w" velocitvj u.00 9d mx 5..' lite] (A) Along pth composed of rothnd srghetns Figure Ope-loop 6: Perfor ce:ime Contn, 0.ieped = 0

16 Pttm en r "s LMd m 1.46 e vlecitt we 4.M i 0.AS N c S s -ISt 9 -x W -10 IS U w M W 45 M IS (A) Along pth composed of rcs nd stright lines Figur wimoits 7: O -ops -e Co LQn 4.A@ d S.M.st -n- L.s x (] ' * S S -w I o,h irn S (B) Along pth composed of clothoid segments Figure 7: Open-loop Perforznnce:Time Constnt = 0.5 s, Vehicle Speed = 2 rn/s

17 11 3 Generting Posture-Continuous Pths The previous section discussed the condition for pth tht is intrinsiclly esy to follow. This section presents method to generte such pths. The problem posed in generting pths is: how to produce unique, esily trckble, continuous pth from given sequence of points. This problem is similr to those ddressed by previous pth genertion methods [KnymS5, Nelson8]. 3.1 Existing Pth Genertion Methods Hongo et l. proposed method to generte continuous pths composed of connected stright lines nd circulr rcs from sequence of objective points (Figure 8) [Hongo85]. While pths composed solely of rcs nd stright lines re esy to compute, such scheme leves curvture discontinuities t the trnsitions of the segments, s discussed previously. 1' '--"" -"- (A) By Arc method (B) By Cubic Spline Method (C) By Knym's Method Figure 8: Pths Generted by Existing Methods Knym proposed pirs of clothoids segments s method to interpolte between given points [Knym85]. The problem with clothoids is tht though they gurntee smoothness nd linerity, it is non-trivil to generte clothoid segments for rbitrry strting nd ending postures; the expressions for these curves re underconstrined, nd further, no closed form expression is vilble. (The

18 12 expression must be evluted using numericl methods.) Knym's scheme is tenble under the simplifiction mde by the requirement tht the strting nd the ending curvture t the vi points re lwys zero. This model is dequte for simple pths for which symmetric pirs of clothoid curves cn be found or for pths with mild curvtures tht cn be esily broken down in seprte clothoid segments. Certin polynomil spline curves [Mortenson85] re cndidtes for pth segments, becuse they gurntee continuity of posture. Nelson proposed quintic spline nd polr spline curves [Nelson88] obtined from two-point boundry conditions. However, these spline curves do not gurntee liner grdients of curvture. Clothoid curves, by contrst, do vry linerly with the distnce long the curve. 3.2 Pth Genertion from Sequence of Points The following two-step method genertes unique posture-continuous pth from sequence of points. The first step is to derive sequence of unique postures from the objective points; the second, is to interpolte between those postures with clothoid segments. Heding nd curvture t the strting nd ending positions re presumed from the configurtion of the vehicle, using eqution (1). Figure 9: Obtining Postures nd Associted Circles from Objective Points

19 13 Let [X = (Xo,...,Xv) = ((xo, yo),...,(xn, yn))] be sequence of objective points. The ssocited circle t Xi is defined to be the circle which psses through points X 1-1, Xj, Xj+ 1 s in Figure 9. Then the heding of the vehicle t Xj is tken s the direction of the tngent to the ssocited circle t Xj, nd the curvture is the reciprocl of the osculting rdius of the ssocited circle, denoting the posture thus obtined s the ssocited postures, The next step is to connect neighboring postures with clothoid segments. It is not lwys possible to connect two neighboring postures with one clothoid curve segment, becuse four governing equtions (5, 6, 7, nd 8) cnnot be stisfied simultneously with only the two prmeters (shrpness k nd length s) tht clothoid curve provides. To stisfy these four equtions, t lest two clothoid segments re needed. However, the generl problem cnnot be solved with only two clothoid segments. Figure 10 shows two pirs of ssocited postures nd their ssocited circles. Let Pi, Pf denote the strting nd the ending postures, respectively. And C i, C, denote the ssocited circles corresponding to the curvtures t Pi, Pf. They re drwn by solid lines nd dotted lines, respectively. ((0) - * - S 'Pi, Ci Figure 10: Determintion of the Sign of the First Clothoid Segment As in (A) of Figure 10, if the orienttion of Pf is outwrd from Ci, the ending prt of solution curve should be inside Ci. Then, it is plusible tht the strting prt of solution curve lso lies inside Ci. Similrly, if the direction of the Pf is inwrd into Ci, s in (B) of Figure 10, it is plusible tht the strting prt of solution curve lies outside Ci. Note tht the sign of the shrpness determines the

20 14 side of the ssocited circle on which the clothoid segment lies: (1) If the shrpness is zero, then the clothoid curve remins on the ssocited circle in question. (2) If the shrpness is positive, then the clothoid curve will be in the left side of the ssocited circle. (3) Otherwise, it will be in the right side of the ssocited circle. Tble 1 lists ll the possible cses of reltive geometry of the first nd lst segments of the solution curve to the circles Ci nd C 1 (The convention used is tht negtive curvture equtes to right turn nd positive curvture equtes to left turn). Hence, solution curve should stisfy the following proposition: If the direction of P 1 is outwrd from C,, the sign of k of the first clothoid segment is chosen so tht the curve lies inside Ci. If the direction of Pf is inwrd into Ci, the sign of k of the first clothoid segment is chosen so tht the curve lies outside Ci. Otherwise, k of the first clothoid segment is chosen so tht the curve remins on Ci. 4,,""" O) f %%% Figure 11: Determintion of the Sign of the Lst Clothoid Segment As corollry to the bove proposition, the sign of k of the lst clothoid segment cn be determined (Figure 11): 4 In this cse, Pi nd Pf shre the sme ssocited circle, two postures re connected with the prt of their ssocited circle, which is clothoid curve of zero shrpness.

21 15 If the direction of Pi is outwrd from C, the sign of k of the lst clothoid segment is chosen so tht the curve lies outside Cf. If the direction of Pi is inwrd into Cf, the sign of k of the lst clothoid segment is chosen so tht the curve lies inside Cf. Otherwise, k of the lst clothoid segment is chosen so tht the curve lies on Cf. Figure 12 shows ll possible cses of curvture vritions between pir of neighboring postures. Notice tht the signs of k for the first nd the lst segments re the sme for ech cse. 5 However, the sign is the opposite of tht required for the curvture vrition between the postures for ll the cses except (C) nd (D) of Figure 12. Thus, the generl problem to connect pir of neighboring ssocited postures cnnot be solved with two clothoid curve segments. 5 Determintion of the sign of shrpness cn be quntittively summrized, s in Appendix.

22 16 curvture shrpness curve loction illustrtion inside the ssocited k i > 0 circle of'the strting ki < 0 outside the ssocited circle of the strting posture ki > 0 outside the ssocited circle of the strting ki < 0 inside the ssocited circle of the strting posture Cf > 0 kf > 0 kf < 0 outside the ssocited circle of the ending inside the ssocited circle of the ending posture inside the ssocited Af > 0 circle of the ending C t < 0 ka < 0 outside the ssocited circle of the ending posture Tble 1: Curve Loction by Shrpness nd Curvture

23 17 One dequte solution set of the clothoids is the set of three clothoid segments (k, si), (-k, s2), (k, s3). The subscripts denote the order of the clothoid segments from Pi. This combintion is plusible for the following resons: 1. The signs of shrpness for the first nd lst clothoid segments re the sme. 2. The shrpness for the second clothoid segment is equl in mgnitude nd opposite in sign to the first nd lst segments. This enbles the curve of three clothoid segments to stisfy the curvture vrition between the strting nd the ending postures by vrying si, s2, s3, even though the sign of the first nd the lst clothoid segments stisfies the curve loction requirement. 3. There re four vribles in the combintion: k, sj, s2, s3. It is possible to find unique solution stisfying the following four equtions which describe the mthemticl reltionship between the strting nd ending postures: Cf = ci + k (Sl-S2 +S3) (9) Of = ei + ci (s, +s2+s3) + k(ss 2 -s 2 s 3 +s3s,) + IL(s?-s-+s ) 2(10) Xf = xi + cos 01(4) d4 + cos e 2 ( ) dt CO 0 () dt Yf = Yi + sin 0 1 (4) dt + sin 02(E) d +f sin 3(A) d4 where 01(Q) = Oi+ cit (l) = 0, + ci s, + Is?+ (c + ksj)4 - I 2 2 0()= 9, + ci (s +5s2) + k (sjs2) + IL (s? -4) + (ci +k (s, -s 2 ))4 + 2

24 18 (A) when ci > c1>o0, ki,,kf > 0 (B) when ci >O0>cf, ki, kf > 0 (C) when 0 > c > Cf, ki,kf < 0 Figure 12. Cses by Curvture Vrition of Pi nd Pf

25 19 (D) when 0O> cf> ci, ki,kf > 0 (E) when cf >O0>ci, ki, kf< 0 (F) when cf >ci >0, ki,kf < 0 Figure 12: Cses by Curvture Vrition of Pi nd Pf

26 20 (G) when c > cf =O, kikf > 0 (H) when 0 c > cf (I) when 0 = cf> ci ki,kf < 0 (J)when cf >Ci = 0, ki k <0 Figure 12: Cses by Curvture Vrition of Pi nd Pf

27 21 Since Equtions (11) nd (12) contin Fresnel integrls, for which there is no dosed form solution, the vlues of k, si, s2, s3 re computed using the numericl method (Press86] outlined in Figure 13. gu~ess 8,8$2 fix S 2 nd fix S, nd 1 perturbs 15 times perturb 8 25 times I forech forech S 1,2 compute X,,Y compute :: Sfind new S I to II findnew S2 to disttnce no (Xc ~yes end Figure 13: A Numericl Method to Compute the Fresnel Integrls 1 Initil vlues for sl nd s2 re chosen to be i of the verge of the lengths of two of the rcs tht connect Pi nd P 1. Equtions (9) nd (10) re used to compute k nd s. Then, xc, Yc cn be computed for the qudruple (k, S1, S2, S3) using Simpson's pproximtion. Idelly xc, y = xff, yf nd in fct vlues of sl nd s2 re djusted until the difference is within threshold.

28 22 4 Results Connecting pir of neighboring ssocited postures ws ccomplished successfully, s in Figure 12, using three clothoid curves. Figure 14 shows the grphic result of posture-continuous pth generted by clothoid curves through the given seven points. As the first step of the proposed method, sequence of seven postures were generted. (Seven rrows in Figure 14). Then, three clothoid segments re used to interpolte between neighboring postures. Figure 14: Posture-continuous Pth From Sequence of Seven Points Figure 15 shows comprison of the performnce of the proposed method using rcs, splines, nd Knym's method [Knym85]. Prmeters of curvture nd shrpness were constrined eqully for ll methods. The mximum shrpness of Knym's method nd the mximum curvture used in the rc method re set t the sme levels s in the proposed method. Pths, curvtures nd shrpness long the pths re compred. Pths resulting from the proposed method hve the following dvntges over other methods: * The method proceeds from n rbitrry sequence of points. Genertion of postures is essentil to explortory plnning where gols re commonly

29 23 posed s n evolving string of points. Pths generted by the proposed method pss through ll the objective points, s in Figure 15; wheres pths from Knym's method nd the rc method re only proximte to mny of the points becuse these methods strt from sequence of postures tht must be modified to ensure curvture t the vi points is zero. * The method gurntees continuity of position, heding, nd curvture long the pth. Further, shrpness is piecewise constnt. * Pths generted by the method lwys sweep outside the cute ngles formed by stright line connection of the wy points, s in Figure 15. The resulting pths re especilly useful for interpolting round obstcles tht re commonly on the inside of ngles. In contrst, Knym's pths re lwys inside pth ngles.

30 Pte pth 24 S ' i.e., C S 5 4 I S is i. ii 48 i4 curtwe dm the ntim I S * S I S S is ii is is I. d.*~h~ t e~w..1mw the eths i p p. g *, p p is ii is is.. *. v. is ii II *j.4 (1) Clthoid with zero curvqture trorelun (2) Circulr rcs nd stright runes C 8. i 5 4 I *i ii ii 85,4 I, 3. * is ii ig is i. cuevtjwe dm the mths '.. I 4 i is ii,. *p,. dm mthe ' r.~.,,.., ~. ~ ~ - I S S isil is *5i4 (3) Cubic spllnes (4) Proposed method Figure 15: Comprison of the Proposed Method with Three Others.

31 25 5 Conclusion Posture-continuity is presented s the condition for pth tht is intrinsiclly esy to trck. In ddition, the linerity of curvture of the pth is correlted to liner steering motion, fcilitting pth trcking. Clothoid curves re good pth cndidtes to stisfy the condition. Simultion shows better performnce of vehicle trcking pths of clothoids versus pths of rcs. A method for generting continuous pth ws developed. This method uses clothoid segments nd consists of two steps: First, sequence of the postures is obtined using the objective points. Then, ech pir of neighboring postures is connected with three clothoid curve segments. The method provides dditionl dvntges in tht preprocessing of the objective points is not necessry, s with rc nd zero curvture clothoids. Further, the geometry of the pths generted lwys sweeps outside the cute ngles formed by stright-line connection of the wy points., These re especilly useful for interpolting round obstcles tht re commonly on the inside of ngles. The method of obtining postures, s in Figure 9, requires tht the circles formed by the rdii of curvture of two postures intersect. Relxing such constrint would require heuristics to determine intermedite postures. Assuming intermedite postures could be found, such tht the ssocited circles intersect, the method could then be used on the new set of postures. Thus fr, the serch for completely generl method tht would generte pth between two completely rbitrry postures hs not been fruitful. Directions for future reserch include the following: (1) optimiztion of the lengths of more thn three clothoid segments on the premise tht some cost function cn be used to find better solution thn the results here; (2) improvement of the numericl method to connect postures through the Fresnel Integrl to improve speed nd ccurcy.

32 26 Appendix Determintion of the Sign of Shrpness Of Figure Al: Reltive Geometry of Associted Postures When pir of neighboring ssocited postures re connected with clothoid curves s in Section 3.3.2, the sign of shrpness for the first nd the lst clothoid segments cn be determined quntittively s follows. Let ui, uf be unit vectors which hve the direction of Pi nd Pf respectively, s in Figure Al. A stright forwrd test cn be pplied to determine whether the posture is inwrd or outwrd from Ci or Cf: If uf. FOi < 0, If uf.foi > 0, Pf is outwrd from Ci. Pf is inwrd intoci. If uf.foi = 0, Pf is on Ci. If Ui. 'of < 0, Pi is outwrd from Cf. (A3.1)

33 27 If Ui. IOf > 0, Pi is inwrd into Cf. If ui. lof = 0, Pi is on C 1. While the If condition of the Proposition is described s Eqution (A3.1), the then result is quntittively described, s in Tble 1. For exmple, if the first clothoid segment lies inside Ci, its sign is determined by ci (the curvture of Ci): If cj > 0, ki > 0 If cj < 0, ki < 0 (A3.2) If ci = 0, the sign of ki is determined s the sign of kf. Combining (A3.1) nd Tble 1, the sign of shrpness is determined s follows: If uf. FOi. ci < 0, ki > 0 If uf. FOi. ci > 0, ki < 0 If uf. FOi ei = 0, the sign of ki is determined s the sign of kf. If ui. FOf.f < o, kf > 0 If ui of. cf > 0, kf < 0 If ui. FOf. cf > 0, the sign of kf is determined s the sign of ki. Note tht the proposed method cnnot be generlized to the cse where both neighboring curvtures re zero. However, this configurtion is never generted by the first step shown in Figure 8. If necessry, this cse cn be solved using clothoids with zero curvture trnsitions [Knym85].

34 28 Reference [Dniel85] [Feng89] [Hongo85] Dniel, D. J., B. H. Krogh, nd M. B. Friedmn, "Kinemtics nd Open-Loop Control of n Ilontor-Bsed Mobile Pltform," 1985 Interntionl Conference on Robotics, St. Louis, Mrch Feng, D., Stisfying Feedbck Strtegies for Locl Nvigtion of Autonomous Mobile Robots, Ph.D. Thesis, Crnegie-Mellon University, Pittsburgh, Pennsylvi, Hongo, T., H. Arkw, G. Sugimoto, K. Tnge nd Y. Ymmoto, "An Automtic Guidnce System of Self-Controlled Vehicle--The Commnd System nd Control Algorithm," Proceedings IECON. 1985, MIT Press, 1985 [Knym85] Knym, Y. nd N. Miyke, "Trjectory Genertion for Mobile Robots," Robotic Reserch: The Third Interntionl Symposium on Robotics Reserch, Gouvieux, Frnce, 1985, pp [Khtib85] [Krogh84] Khtib, 0., "Rel-Time Obstcle Avoidnce for Mnipultors nd Mobile Robots," IEEE Intl. Conference on Robotics nd Automtion, St. Louis, Mr. 1985, pp Krogh, B. H., "A Generlized Potentil Field Approch to Obstcle Avoidnce Control," Proceedings of the Robotics Interntionl Robotics Reserch Conference, Bethlehem, PA, August (Mortenson85] Mortenson, M. E., Geometric Modeling, John Wiley & Sons, [Nelson88] (Press86] Nelson, W. L, "Continuous Steering Function Control of Robot Crt", IEEE Trnsctions on Industril Eletronics, Submitted for publiction 1988, AT&T Bell Lbortories. Press, W. H., B. P. Flnnery, S. A. Teukolsky, W. T. Vetterling, Numericl Recipes, Section 10.2 Prbolic Interpoltion nd Brent Method, Cmbridge University Press, 1986.

35 29 [Shin9OJ Shin, D. H. nd S. Singh, "Chpter 10: Vehicle nd Pth Models for Autonomous Nvigtion," Nvib, Kiuwer Press, April [Ytes52] Ytes, K. C., Curves nd Properties, Clssics Publishing Co., 1952.