A Path Planning Method Using Cubic Spiral with Curvature Constraint

Transcription

1 A Path Planning Metho Using Cubic Spiral with Curvature Constraint Tzu-Chen Liang an Jing-Sin Liu Institute of Information Science 0, Acaemia Sinica, Nankang, Taipei 5, Taiwan, R.O.C., an Abstract. This paper aresses a new path planning metho, whose objective is to buil a process to generate a path which connects two configurations of car-like mobile robots. The generate path is constitute by both cubic spirals an straight lines, an has continuous an boune curvature. We will show the proceure to fin the path with theoretical minimal length an to simplify it for the reason of practical use. Mobile robots with forwar an backwar riving abilities an only uni-irection riving ability are both consiere. This metho is flexible an is eligible to incorporate with other constraints like wall-collision avoiance. This will also be iscusse. I. Introuction Path planning problem of autonomous mobile robot has been wiely stuie in recent years. The essential topic is to generate an acceptable path to join two istinct configurations with some constraints an optimize it. L. E. Dubins first iscusse the shortest paths with boune curvature synthesize by arc an straight line in 957[8]. J. A. Rees an L. A. Shepp further stuie the same path with cusp, i.e. the vehicle can rive both forwar an backwar, in 990[9]. A complete characterization of path synthesize by arc of circle an straight line was aresse by P. Souères an J. Laumon[]. These stuies concentrate on the fining of the path with theoretical minimal length. They showe such kin of path has at most two cusps. However, its non-continuous curvature results in a control ifficulty. At the junction of a straight line an an arc, mobile robot nees to stop its wheel motion to make the perfect tracking achievable. A. Scheuer an Ch. Laugier [3][4]ae a new constraint that the erivative of

2 curvature is boune to the path planning problem an mae the planne path smoother. Different from Dubins path, the curvature value along the path has a trapezoi shape an is continuous. Y. Kanayama an N. Miyake suggeste another set of curve calle clothoi curves or Cornu spirals to form a smooth path which has continuous curvature [0]; A. Scheuer an Ch. Laugier in [4] also have a similar iscussion. A more natural an smoother set of curve calle cubic spiral is introuce by Kanayama an Hartman in 997[]. Cubic spiral is a kin of trajectory which irection function is cubic. Kanayama cut a cubic spiral in its two inflection points to obtain a curve with finite length an has zero curvature on both en-points. This portion of a cubic spiral can connect two configurations that are symmetric. Two configurations which are not symmetric can be joine through an intermeiate configuration, which is calle symmetric mean, by two cubic spirals. Instea of path length, Kanayama use path curvature an erivative of path curvature as criteria to optimize it. The physical meanings of these two criteria are in fact the centripetal (lateral) acceleration an the variation of it. Though the cubic spiral metho can generate smoother path than other ones, it oes not consier the boune curvature constraint of car-like mobile robots. An unreasonable high value of curvature happens when the initial an en configurations are too close, an the smoothest criteria make the path too long if two configurations are relative far. In this paper, we aress a new path planning metho. Following Kanayama s cubic spiral metho, we incorporate it with straight line segments in its zero curvature points. The result path has both continuous an boune curvature. It has two cubic spiral segments an at most two straight line segments an is optimize by a shortest-length criterion. This metho can be also applie to the vehicle with both forwar an backwar moving abilities. In the next section, the cubic spiral metho is briefly reviewe, an notations use in this paper are introuce. Section three we will aress the improve cubic spiral metho an the proceure to fin a shortest path. Other characteristics will be iscusse in section four, incluing wall collision avoiance an motion irection constraint of mobile robot. The last section is the conclusion. II. Review of Cubic Spiral Metho II. Backgroun Y. J. Kanayama an B. I. Hartman suggeste the using of cubic spirals to fin the smoothest path for mobile robot path planning problem. They claime the set of cubic spiral is theoretical more meaningful than the set of clothois. The characteristics of continuous curvature an criterion of minimal centripetal

3 acceleration or minimal change of centripetal acceleration are inee rational. However, this metho has some constriction an rawbacks an is not suitable for practical use. Later we will briefly escribe this metho an its main rawbacks. II. Notations an Representation of a Curve In this paper, we follow most of Kanayama s notations. A triple q ( x, y, θ ) is to represent a vehicle configuration. For an arbitrary configuration q, [ q] enotes its position( x, y ), an( q) its irectionθ. For a configuration pair( q, q ), the size is the istance between the two points [ q] an[ q ], an the angle is the ifference between the two irections( q ) an( q ), i.e. size( q, q) ([ q],[ q]) () angle( q, q) Φ(( q) ( q)) where the angle-normalizing function Φ is efine as θ π Φ( θ) θ π π () If the function angle is applie to a vector v, it means angle( v) = atan( v, v ) (3) y x v an v are scalars enote the x an y components of v. If these two functions are x y applie to a cubic spiral, they are in fact applie to its two en-configurations. A irecte curve Π with finite length is efine by a triple, Π (, κ, q0 ) (4) where κ :[0, ] is its curvature an q 0 ( x 0, y 0, θ 0 ) is its initial configuration (Lipshutz 969). Its irection θ an position ( x, y) at arc length s are evaluate by θ() s = θ κ() t t 0 x() s = x cos θ () t t 0 ys () = y sin θ () tt 0 s 0 s 0 s 0 At the initial point ( x0, y 0), s is efine as 0. A configuration ( ) qs () = ( x s, ys (), θ ()) s is naturally efine by this set of simultaneous equations. (5) II.3 Cubic Spirals By efinition, cubic spiral is a set of trajectories that their irection functions θ are cubic. An entire cubic spiral has infinite length, but the useful portion is cut from its two inflection points an has finite length. The curvature function of this portion of

4 cubic spiral with length is represente as κ () s = As( s) (6) where A is a constant to be etermine. At the inflection points ( s = 0 an s = ) the cubic spiral has zero curvature. The constant A of a cubic spiral joining two separate configurations which have relative angle of α = θ( ) θ(0) can be solve by the first equation of (5) an the curvature function becomes, 6α κ () s = s( s) (7) 3 If the length of a cubic spiral is, its size is given by[] / 3 D( α) cos( α( t ) t) t (8) 0 There is no close form to represent the size of a cubic in any length. Since all cubic spirals are similar, a pre-calculate D( α ) table can evaluate the relation of an = size( q, q) by α using the following equation, = (9) D( α) The D( α) table is plotte in Fig.. II.4 Concept of Symmetric Configurations A configuration pair [ q, q] is sai to be symmetric if θ θ y y = tan( ), if x x,an x x θ θ π Φ ( )= ±, if x = x The symmetric property is essential in this metho because a cubic spiral can connect two symmetric configurations. (0) II.5 Sketch of Cubic Spiral Path Planning Metho Because one cubic spiral can connect two symmetric configurations, we nee at least two cubic spirals to connect any two configurations. A symmetric mean q of any configuration pair [ q, q] is a configuration that both [ q, q] an[ qq, ] are symmetric pairs. Kanayama prove that all symmetric means of a configuration pair[ q, q] forms a circle. The optimization is to choose one symmetric mean q from this circle to minimize the cost functions. II.6 Drawbacks Kanayama s metho can generates smoother path than other metho, an is easy to be implemente by software programming. However, there are two main rawbacks of

5 this metho that make it not fit practical use. First, the cost functions of Kanayama s metho either minimize the integration of centripetal force or the change of centripetal force; they o not consier the arc length an maximal (or minimal) curvature. As shown in Fig., Configurations pairs which have the same relative position an relative angle but ifferent size have similar smoothest path, yet the length of this path is too long when the size is large an the curvature is too high when the size is small. Seconly, the metho fails in the configuration pair that two configurations are originally symmetric. Though he eclare this kin of configuration pair can be joine by simple curve (one symmetric curve), but in some cases, like q = [0,0,0] an q = [ a,0,0], the simple curve may have infinite length. III. Our Solution: the Improve Cubic Spiral Metho The cubic spiral inee has many goo properties that are suitable to be the path of mobile robots. Therefore we improve Kanayama s metho to assemble a path by at most two cubic spirals an two straight lines. The assemble path still has continuous curvature. Different from original metho, the improvement as a maximal (or minimal) value of curvature as constraints an path length as cost criterion. It can be applie to path planning problem that allows or oesn t allow backwar motion. It also has simple wall-collision avoiance abilities. We will show the proceure to fin the theoretical shortest path an some simplification to fit practical use. III. Problem Statement The problem is to fin the path joining a given orere pair of configurations [ q, q ] with boune an continuous curvature along it. The path is assemble by at most two cubic spirals an two straight lines an is optimize to be shortest. III. Constraints of Maximal Curvature In general, the path of a mobile robot has its minimal raius of curvature which is constraine by wheel arrangement[5][6][7]. This constraint also may ynamically changes accoring to riving velocity or control performance. We use a constant κ max to escribe the absolute value of maximal curvature of the planne path. Because curvature of a straight line is zero, this constraint only affects the cubic spiral segment of the path. 6α For a specific cubic spiral with angleα, since κ () s = s( s) is the curvature 3 polynomial, it has the maximal (or minimal, ifα < 0 ) value at the mile point,

6 where s =, an 3α 3 α D( α) max( κ( s) ) = κ( ) = = () Therefore if we have a constraint of curvature which is κ () s κ max. It can be transforme to the size constraint, 3 α D( α) () κ max The minimal can be solve as a function of α min 3 α D( α) ( α) = (3) κ max III. Minimal Locomotion In this section, we will show the computation of the minimal locomotion from q through an intermeiate irection θ m to ( q ). The maximal curvature constraint iscusse in above section is applie, an there are 6 kins of ifferent minimal locomotion vector can be generate for a given θ m. We restrict the angle of a cubic spiral in the range [ π, π ], for an initial configuration q which has irection θ to the intermeiate configuration with irection θ m, there exists two cubic spirals that all have the maximal curvature. Angles of these two cubic spirals are, α =Φ( θ θ ) c m α = sgn( α ) ( π) α c c c (4) α is positive, thenα c must be negative, an vice versa. The above notation assures If c that α c [ ππ, ] an α c outsie this range, as shown in Fig.. To achieve both angles there are two kins of motion: forwar an backwar. Therefore the amounts of cubic spirals are four. We use another positive or negative sign to represent these four kins of motion, ( α ),( α ),( α ),an( α ) c c c c (5) Positive sign outsie the bracket means forwar motion, an negative one means backwar motion. Fig. 3 shows the four trajectories. Although they have the same κ constraints, four traveling istance are ifferent ue to the angles an motion max irections are ifferent. These four intermeiate configurations are name

7 q, q, q, an q respectively. The first superscript enotes the range of the angle m m m m an the secon superscript represents the motion irection. The four istance are also name by the same notation, ( α ) c min c ( α ) c min c ( α ) c min c ( α ) c min c (6) Note the result of min () i is always positive, but above four cmay be negative. The negative value means the motion is accomplishe by backwar motion of vehicle. Four vectors are efine as vc [ qm ] [ q] vc [ qm ] [ q] (7) vc [ qm ] [ q] v [ q ] [ q ] c m Use the same proceure again, if αc = Φ( θ θm), there are also four trajectories exist for each q m. The correspone vectors are vc, vc, vc, vc, an their length are,,,. As a result, for a specific intermeiate irectionθ m, there are 6 c c c c ifferent paths with maximal curvature to arrive at the en irection θ. The symmetric mean circle can only represent two of them. ij, The minimal locomotion vector of a given θm is v, efine by c v v v where ij, ij c = c c, { } i, j, k, l, (8) correspone minimal istance can be also represente as ij c an c. Fig. 4 shows the sixteen en positions of minimal locomotion. One of these cases is plotte to show the combination of minimal locomotion vector. III.3 Assemble a Path Following Kanayama s metho, we aapt two cubic spirals to form a path. There are

8 three zero curvature points at a two-cubic-spiral path. These points can be extene by straight lines without lose the feature of continuous curvature. Combining with two cubic spirals, we now have five segments to assemble a path to connect a given configuration pair. The five irections are θ, θ, θ, θ, an θ, where c m c θ θ θc Φ θ θ θc Φ m ( ), m ( ), Their correspone vectors are v, v, v, v, an, v c m c an (9) Five vectors are all ajustable; a successful combination of these vectors is to fulfill the below equation, [ q] = [ q] v vc vm vc v () Since the irections of the five vectors are known, we nee only to solve their length. Define unit vectors of the above five vectors as n, n, n, n, an, n c m c The positive irection of these five unit vectors are forwar motion irection. Then () becomes [ q ] = [ q ] n c nc mnm c nc n where, c, m, c,an, are length to be ecie. By (6), the constraints are m ij ij c > c if c > 0, else ij c< c c > c if c > 0, else c < c (0) () (3) (4),, an coul be positive or negative. We must emphasis again that positive value of each enotes forwar motion an negative oes backwar one. III.4 Criterion of Minimal Length The objective of this section is to fin a minimal length solution. Assume θm an i, j, k, an l have been selecte alreay. The cost criterion can be efine as cost( Π ) = c c i m k D( α ) D( α ) (5) α an α as efine in (4). i k c c c m c c c,,,,an shoul be solve by (3) uner the constraint (4) an to

9 ij minimize the above cost criterion. Because c an c have been chosen, ( i k D αc) an c D( α ) are constant now. The cost of minimal locomotion is ij Π = c c m D( α ) D( α ) (6) ij, cost( θ ) This part of cost cannot be reuce. We efine i k c c = * ij c c c = * c c c (7) Then to minimize (5) is equivalent to minimize * ij, cost ( Π ) = cost( Π) cost( Πθ ) = * * c c i m k D( αc) D( αc) m (8) An the constraints (4) become * ij c > 0 if c > 0, else * c < 0 * c > 0 if c > 0, else * c < 0 (9) III.5 Vector Choose Substitute (7) to (3), we have [ q ] = [ q ] n ( n n ) n ( n n ) n ij * * c c c c m m c c c c (30) Define v = [ q ] [ q ] ( n n ) ij goal c c c c (3) ij where c nc cnc is the minimal locomotion vector v ij, c solve in (8). Then the equation to solve becomes v = n n n n n * * goal c c m m c c (3),,an can be positive or negative, but the signs of m an are pre-ecie * * c c by (9). It is not convenient to solve such a problem. Therefore we efine

10 n = n an n = n (33) {, c, m, c, }. Define a vectors set, (34) {,,,,,, j, l n n nm nm n n nc nc} jan l is or, an its correspone coefficient set, {,,,,,, j, l m m c c} (35) Then (3) becomes v = n n n n n n n n j j l l goal c c m m m m c c (36) where j * c c l * c c { } =,, m,, an = = (37) Then all unknowns are positive, an we have total eight unknowns to be solve. The criterion (8) becomes cost ( Π ) = (38) j l * c c i m m k D( αc) D( αc) The solution is not as har as it looks like. In fact, to minimize (38) we nee only two terms in (35), an let others remain zero. The reason will be aresse in appenix. Assume now we have chosen a pair of vector n an n, an the correspone an, (36) becomes a b By Appenix B, the solution of where v = n n goal a a b b a b an is a = v = v goal goal b a sinθ sin( θ θ ) sinθ sin( θ θ ) b goal b (39) (40) θ =Φ( angle( na) angle( vgoal)) (4) θ =Φ( angle( n ) angle( v )) The choose of n an n will letθ be positive an θ negative. Then, by (39),(40), a b

11 an (4), coefficients, c, m, c,an is solve an (5) is minimize. Note because ij can cis usually non-zero, the amounts of non-zero coefficients are between two an four, i.e., there is at least one zero coefficient. III.6 Proceure to Fin a Best Path The proceure to fin a best path is to search all possible combination of cubic spiral pairs, an fin the one that minimizes the cost criterion (5). From previous section, ij, we have foun the shortest path of a specific θ an one of its v. To fin the best path, we shoul compute the cost of each case that θ m from π to π an its ij, sixteen v. Though there may be thousans of cases to check, the task is in fact not c so har. This is because we o not have to compute each cubic spiral ata to measure the path length, but only have to generate five to compute the cost. The Proceure is m forθm = π to π step θ i for i =, ( αc = αc, αc) for j =, ( Direction C = Forwar, Backwar) k for k =, ( αc = αc, αc) for l =, ( Direction C = Forwar, Backwar) Compute_Cost (4) next next next next next θ can be chosen accoring to requirements, as we will further iscuss later. Some paths planne by the algorithm are shown in Fig. 5 an 6. Fig. 7 is an example to represent the proceure to optimize. c IV Further Discussion Some important feather of this path planning metho is iscusse in this section. IV. Forwar an Backwar Motion The path planning algorithm suggeste in this paper is for the vehicle which can rive both forwar an backwar. However, it also can be applie to the uni-irection path

12 planning problem. We shoul simply restrict Direction C an Direction C be forwar, an use f { n, nm, n, nc, nc, } (43),,,, { } f m c c to replace an. Then the solution can fit our use. But one shoul note that when solve (39), the requirement for positive solution ( an ) is π π θ < an θ > (44) In the forwar motion case, for some θ m an ij, c va, v a b v, such a pair of ( ) b may not exist, then the algorithm is failure to generate a path. Its cost is set to be an extreme large value to avoi being chosen as solution. Fig. 8 an 9 show some uni-irection path planne by this algorithm. IV. Wall-Collision Avoiance For practical use a vehicle path usually has more constraints, because the space is not obstacle-free or is restricte by some walls. A complete path planning metho shoul also consier these restrictions. Our revise path planning metho can generate many acceptable paths speeily. Though in previous section we only choose the shortest path to be the best one, this oes not means other paths are not usable. Here we introuce another term in the cost function to simply avoi wall-collision. There are six points along a planne path can be compute when is solve; they are p0 = [ q] p = p0 v p = p vc (45) p3 = p vm p4 = p3 vc p = p v = [ q ] 5 4 where p ( p, p ) is a coorinate in i ix iy. Some of these points may be ientical, because some are zero. A wall-type obstacle is moele as a straight line equation in space, mx c0 y = 0 (46) Points p0an p5are automatically collision-free, so they must lie on the same sie of above straight line equation. Therefore, efine function O( p) as, O p mp c p (47) ( ) x 0 y

13 Then if O( p0 ) O( pi ) > 0,for i =,,3,4 (48) Then we can conclue that all five points are collision-free. For a (46) type obstacle an a straight line segment of a path, its two en points collision free means this segment is collision free. However for a cubic spiral segment, this may not be true. Therefore we shoul check more points to make sure the safety. There is no easy way to compute the coorinate of selecte points on a cubic spiral without using equation (5). But the integration costs too much computation time. Therefore we implement a simplifie metho to generate check points, as escribe below. For a cubic spiral segment, we esign to a two more check points. We generate three straight line segments to fit a cubic spiral one, an use the two more vertex as check points. The total lengths of these three straight line segments are equal to the length of cubic spiral. As shown in Figure 0, assume a cubic spiral has size can irection v. The secon portion is parallel to the v, an its length is h c, we have c = hc c hc S( α), where D( α) α sin( ) S( α) sin( α) Rearrange above equation, we obtain sgn( c hc) S( α) hc D( α) = c sgn( c hc) S( α) Lengths of the other two line segments are, l h cos / ( α ) (50) c c c = (5) From fig 0, the two more check points can be solve by vector aition. * p = p lc n * p = p l n (5) c These two points are close to the original cubic spiral, an check them can be a goo approximation of collision etection of this segment. Furthermore, one may note that this two more check points are collinear with the two straight line segments that connect with this cubic spiral. For a line type segment we nee only to check its two en points to know whether the collision happens or not. Therefore, finally the check points remain five or less than five. To use the points of (5) or to the original ones is epenent on the motion irection change of the path. m (49)

14 We can simply set cost( Π) to be an extreme large value to avoi choosing path that collision will happen, i.e. some check points that let (48) be failure to hol. This metho is effective an can eliminate most unqualifie trajectories. Some examples are shown in Fig. IV.3 An Effective Search In (4) if we set θ = , which is approximately equal to 5 egree, then we shoul search 5 cases to fin a best one. But in the forwar-an-backwar an free space case one can foun a cubic spiral which angle outsie the range[ π, π ] selom appears in the result path. We shall explain this an try to fin a more efficient proceure to search an acceptable solution. The abstract of the explanation is that first we will assume a result path inclues a cubic spiral with angle outsie the range[ π, π ], an then prove that in most cases there exists at least one shorter path an the shorter one will replace the original one ue to the linear programming optimization. Following are the etails. Assume in a path optimize by the metho escribe in previous sections there is one cubic spiral with an angle outsie the range [ π, π ]. By our efinition, its angle α has a superscript -, i.e. α. An the following equation must hol ( α ) min (53) where is the size of the cubic spiral. The length of this portion of path is = (54) α D( α ) Because the correspone angleα insie the range[ π, π ] also steers the vehicle to the same irection, we can also buil another path byα to substitute this one without contraict the continuous curvature constraint. Following are two cases: Case, ( α ) min In this case an alternate path can be constructe by a single cubic spiral with angle α an size, the path length is = (55) α D( α ) By Fig., we can irectly figure that for any two angle β an β, D( β ) > D( β ) > 0, if β < π an β > π (56) which means

15 <,.. ie D( α ) D( α ) α < α (57) Case, ( α ) min The alternate path can be constructe by a single cubic spiral with angleα an size ( α ) min an two straight lines, the path length can be compute as, α sin( ) min( α ) ( = min( α ) ) (58) α D( α ) sin( α ) Then, if α α α sin( ) (59) min( α ) ( min( α ) ) 0 hols D( α ) sin( α ) D( α ) The above conition can be rearrange as S( α ) D( α ) S( α ) min( α ) D( α ) min( α ) Multiply both sie of (56) by, we get min( α ) S( α ) D( α ) min( α ) S( α ) min( α ) min( α ) D( α ) (6) (60) Fig. 3 shows the value of LHS of (6). By the assumption, RHS of (6) must be larger or equal than, but in the figure, it is easy to point that when α < 39 o, all values of LHS are less than. Furthermore only when <.0734, (6) is ( α ) min possible to hol. The gray region on the figure is where α α hols. Apparently it is small; so that the conition that cubic spiral with angleα cannot be replace by correspone α one selom happens. Both on the above two case we can almost fin a shorter path to replace the original

16 cubic spiral with angleα. By the shortest path criterion, this original cubic spiral can not appear in the final result if the shorter is foun. This conclues that in a two-irection an no-obstacle path planning problem, we nee not to check the αcan α ccases, an can save half of computation time with little optimization sacrifice. V. Conclusion In this paper we iscuss an improve path planning metho using both cubic spiral an straight line segments. A planne path is constitute by at most two cubic spirals an two straight lines, an can be assure the shortest one of this combination. The improve metho is suitable for various vehicles or mobile robots path planning because it consiers the boune curvature constraint an can generate paths with or without cusp, i.e., irection change. Moreover, the continuous curvature of generate path makes controller esign easier. The complete metho searches all possible paths via the change of intermeiate configuration, but we also show that a faster search is achievable with few optimization loss. As for practical use, a mobile robot usually executes its task in a constraint workspace, so a simple wall collision avoiance scheme is iscusse. It checks a few points of the path an exclues angerous ones. This metho is being applie to a robot soccer game project that we esign a ball passing strategy an use multiple mobile robots to realize it by change their formation ynamically. It works well. Appenix A Minimal Length If we let j l T x= [,, m, m,,, c, c] j l cosθ cosθ cosθm cosθm cosθ cosθ sgn( c) cosθc sgn( c) cosθ c A = j l sinθ sinθ sinθm sinθm sinθ sinθ sgn( c) sinθc sgn( c) sinθc b= vgoal T c = [,,,,,,, ] i k D( α ) D( α ) c c (A) Then the original problem becomes a canonical form of linear programming problem, maximize * T -cost ( Π)= c x subject to Ax = b (A) x 0 which can be solve by simplex metho suggeste by George B. Dantzig in 947[].

17 Because one of the extreme point is the optimal solution, we can irectly know that the optimal solution of x has only two non-zero elements xa an x b. A reuce vector is efine as [, ] x = x x (A3) r a b an the correspone columns of matrix A is also collecte, A = [ A A ] (A4) Then the below equation also hols r a b A x = v (A5) r r goal This equation is equivalent to (39) if let = x an = x. a a b b The number of iterations is of orer to 4 times the number of rows of matrix A, an the total running time has been foun roughly to the equation Time K No of Rows of A 3 = (. ) (A6) However, ue to the specific geometric relations, we o have a faster metho to fin which two constraints, i.e. which two rows of A, forms the extreme point. The metho is escribe here. A. A Faster Metho Let Θ { Angle( angle( n) angle( vgoal )): n } (A7) Then let θ θ θ θ mp Mn vmp vmn = the minimal positive element of Θ = the maximal negative element of Θ = the vice minimal positive element of Θ = the vice maximal negative element of Θ If the correspone n mp Mn s of both θ an θ are not j l n or n c c (A8), then Else if either the correspone mp Mn θ = θ an θ = θ (A9) mp Mn θ or θ is j l n or n, then we shoul check vmp vmn θ an θ to compare which solution is maximal. Two more combinations of solutions coul exist, vmp Mn θ = θ an θ = θ, or mp θ = θ an θ = θ vmn c c (A0)

18 Finally if correspone n s of both mp Mn θ an θ are j l n or n, one more combination of solution shoul be check, vmp vmn θ = θ an θ = θ (A) Therefore, we shoul check at most four combinations of solutions to fin the extreme point. c c B. Notes of S( α ) As shown in Fig.4, use of two unit vectors n an n to represent a known vector v 0 usually happens in our iscussion. The angle between n an v is 0 γ ; v 0an n is γ. Two lengths an can be solve by, v0 x cosγ cosγ v = 0 y sinγ sinγ (B) The solution of above equation can be obtaine by sin( γ ) = v0 sin( γ γ ) sin( γ ) = v0 sin( γ γ ) If γ γ an =, we have which is the form of S( α ). v sin( γ ) (B) = = 0 (B3) sin( γ ) v sin( γ ) = 0 (B4) sin( γ ) References [] Y. J. Kanayama an B. I. Hartman, Smooth Local Path Planning for Autonomous Vehicles, The International Journal of Robotics Research, vol.6, no. 3, June 997, pp [] P. Souères an J. Laumon, Shortest Path Synthesis for a Car-Like Robot, IEEE Transactions on Automatic Control, vol. 4, no. 5, May 996, pp [3] A. Scheuer an Ch. Laugier, Planning Sub-Optimal an Continuous-Curvature Paths for Car-Like Robots, Proceeings of the 998 IEEE/RSJ International Conference on Intelligent Robots an Systems, Victoria, B. C., Canaa, October 998. [4] A. Scheuer an Th. Fraichar, Collision-Free an Continuous-Curvature Path

19 Planning for Car-Like Robots, Proceeings of the 997 IEEE International Conference on Robotics an Automation, Albuquerque, New Mexico, April 997, pp [5] A. M. Shkel an V. J. Lumelsky, On Calculation of Optimal Paths with Constraine Curvature: The Case of Long Paths, Proceeings of the 996 IEEE International Conference on Robotics an Automation, Minneapolis, Minnesota, April 996, pp [6] A. M. Shkel an V. J. Lumelsky, Curvature-Constraine Motion Within a Limite Workspace, Proceeings of the 997 IEEE International Conference on Robotics an Automation, Albuquerque, New Mexico, April 997, pp [7] J. Boissonnat, A. Cérézo, an J. Leblon, Shortest Paths of Boune Curvature in the Plane, Proceeings of the 99 IEEE International Conference on Robotics an Automation, Nice, France, May 99, pp [8] L. E. Dubins, On Curves of Minimal Length with a Constraint on Average Curvature an with Prescribe Initial an Terminal Position an Tangents, American Journal of Mathematics, vol. 79, pp , 957. [9] J. A. Rees an R. A. Shepp, Optimal Paths for a Car that Goes Both Forwar an Backwars, Pacific Journal of Mathematics, vol. 45, no., 990. [0] Y. J. Kanayama an N. Miyake, Trajectory Generation for Mobile Robots, Robotic Research, vol. 3, Cambrige, MA: MIT Press,986, pp [] B. Kolman an R. E. Beck, Elementary Linear Programming with Applications, Acaemic Press, New York, 980 Fig. Distance function of cubic spirals. The ashe line represents α = 80 o ; left sie of this line is α region an right sie is α region []. Fig. Two paths planne by Kanayama s cubic spiral metho. Fig. 3 Four trajectories from an initial configuration q to an intermeiate irection ( q m) with maximal (or minimal) curvature value at their mile points. Fig. 4 Sixteen ifferent en positions of q through minimal locomotions. The cubic spirals of one of them is plotte to show the combination of minimal locomotion vector. ( i :, j :, k :, an, l : ) Fig. 5 Selecte path planning results. Each path has the same ( q ) but ifferent[ q ]. Fig. 6 Selecte path planning results. Each path has the same [ q ] but ifferent( q ). Fig. 7 This figure shows a example of fining an shortest path if θm an i, j, k,an l ij have been ecie. First we use the minimal locomotion vector vc vc to fin

20 the v goal. Then two vectors to be extene are selecte from eight caniates. In this l case, v an v are chosen, so the result path plotte in soli line inclues one straight c line an two cubic spirals. Fig. 8 Selecte path planning results of uni-irection cases. Each path has the same ( q ) but ifferent[ q ]. Fig. 9 Selecte path planning results of uni-irection cases. Each path has the same [ q ] but ifferent( q ). Fig. 0 This figure shows how to generate two more check points * * pan pform p an p. Note that p * will replace p in check equation (48), but * p will not replace p. This is because * p lie between p an p 3, an we nee only check two enpoints of a straight line. Fig. Selecte cases to show the wall-collision avoiance path planning results. Fig. This figure shows the metho to generate an alternate path by α cubic spiral to replace α one if < ( α ) min. The α path is plotte in otte line an the alternate path is ashing line, which inclues two straight line segments an an α cubic spiral. Fig. 3 The value of LHS of (6). Fig. 4 Use two vectors n an n to represent a known vector v. γ 0 an γ are angles between ( n, v 0)an ( v 0, n ).

21 D( α) α α α (egree) Fig. Distance function of cubic spirals. The ashe line represents sie of this line is α region an right sie is α region []. α = 80 o ; left En configuration En configuration Initial configuration Fig. Two paths planne by Kanayama s cubic spiral metho.

22 ( α ) q m q m q q m ( α ) ( α ) ( α ) q m c c c c Fig. 3 Four trajectories from an initial configuration q to an intermeiate irection ( q m) with maximal (or minimal) curvature value at their mile points. ij, v c v c v goal q ij v c q Fig. 4 Sixteen ifferent en positions of q through minimal locomotions. The cubic spirals of one of them is plotte to show the combination of minimal locomotion

23 vector. ( i :, j :, k :, an, l : ) q Fig. 5 Selecte path planning results. Each path has the same ( q ) but ifferent[ q ]. q q Fig. 6 Selecte path planning results. Each path has the same [ q ] but ifferent( q ).

24 v m = 0 vc = v v = 0 ij c v goal q v c v v l c c q ij v c v v c n n m n n j c l n c n m n n l v c v v goal Fig. 7 This figure shows a example of fining an shortest path if θm an i, j, k,an l ij have been ecie. First we use the minimal locomotion vector vc vc to fin the v goal. Then two vectors to be extene are selecte from eight caniates. In this l case, v an v are chosen, so the result path plotte in soli line inclues one straight c line an two cubic spirals.

25 Fig. 8 Selecte path planning results of uni-irection cases. Each path has the same ( q ) but ifferent[ q ]. Fig. 9 Selecte path planning results of uni-irection cases. Each path has the same [ q ] but ifferent( q ).

26 p 3 * p * p h c lc l c p c p p 0 Fig. 0 This figure shows how to generate two more check points * * pan pform p an p. Note that p * will replace p in check equation (48), but * p will not replace p. This is because * p lie between p an p 3, an we nee only check two enpoints of a straight line.

27 q Fig. Selecte cases to show the wall-collision avoiance path planning results. D( α ) ( α ) min α sin( ) ( min( α ) ) sin( α ) ( ) min α D( α ) q q α Fig. This figure shows the metho to generate an alternate path by α cubic spiral to replace α one if < ( α ) min. The α path is plotte in otte line an the

28 alternate path is ashing line, which inclues two straight line segments an an α cubic spiral S( α ) D( α ) min( α ) S( α ) min( α ) D( α ) α = 39 o α Fig. 3 The value of LHS of (6). n n γ γ v 0 Fig. 4 Use two vectors n an n to represent a known vector v. γ 0 an γ are angles between ( n, v 0)an ( v 0, n ).