- Email: [email protected]
Path Generation for a Robot Vehicle Using Composite Clothoid Segments
Copyright © IFAC Intelligent Components and Instruments for Control Applications , Malaga, Spain, 1992
PATH GENERATION FOR A ROBOT VEHICLE USING COMPOSITE CLOTH OlD SEGMENTS D.H. Shin, S. Slngh and W. Whlttaker Fi~ld Robolics
CUller,
Carfl~gu M~llOfl U"ill~rsily,
PiJ/sburgh. USA
Abstract. We present a method to generate fine grained. smooth paths for wheeled mobile robots. This method takes as input, a sequence of objective points that the robot must attain to avoid obstacles and to progress toward its goal. Segments of clothoid curves are used to join the objective points and hence provide a finer level of details for the robot controller. Previous path generation methods have sought to simplify a path by using arcs, superarcs, polynomial curves, and clothoid curves to round corners, which result from poly-line fits through a given sequence of points. The proposed method generates smooth and continuous curved paths directly from a given sequence of points without adhering to any straight line segments. By virtue of the property of clothoid curves, a generated path is continuous with respect to position, tangent direction, and curvature, and is linear in curvature. Aside from the properties innate to clothoid curves, the generated paths transition smoothly into turns, pass through all the way points, and sweep outside the corners. Keywords.
Robot vehicles; path generation; clothoid curve; linear curvature.
INTRODUcrION Mobile robot navigation is typically composed of three phases- perception, planning, and, control. A robot must perceive
its state with respect to the world and use this information to plan its motion. Once a plan has been composed, it is necessary to enact the plan with requisite control actions. Typically, path planning occurs at two levels. FlI'St, path planning uses the information about the world to obtain an ordered sequence of objective points that the robot must attain to avoid obstacles and to progress towards its goal. These objective points constitute a coarse plan and a second step is necessary to generate a path in finer detail for the purposes of robot control. This paper reports a method of path generation to develop a fine grained specification of a path from a sequence of objective points that immediately suggests a control law for the mobile robot. This problem has been studied by other researchers (Hongo, 1985; Kanayama, 1985; Nelson, 1989), Our work is an extension of work reported by Kanayama and accrues several advantages over the other reported schemes.
entation and instantaneous path curvature. In Section 4, we evaluate the pe!'formance of the presented method. Fmally. Section 5 advances the conclusions and suggests the directions for future research. CONTINUOUS PATHS Guide Point For our analysis a bicycle model is used as an archetype for modeling robot vehicles with two degrees of freedom, steering, and propulsion. The guide point is the point of the vehicle that is controlled to follow the given path. The choice of the guide point is an important decision-it affects the desired steering and propulsion functions required to follow the given path and speed. We have chosen the guide point to be at the midpoint of the rear axle (Fig. 1) resulting in the following advantages: • The steering angle at any point on the path is determined geometrically, independent of speed, in the following manner:
In the following section we first discuss common kinematic models for mobile robots. We have used a "bicycle" model to represent a robot vehicle. The advantage of such a model is that there exist simple geometric relationships between the curvature of the reference path and steering angle for the robot. Given such a model, we suggest criteria for "goodness" of candidate path segments that may be used to join the objective points. As with any control system, the response of a mobile robot in tracking a path is partly dependant on the nature of the reference path. We show that if the type of path generated is intrinSically easier for the robot to track, a prototypical robot following such a pa th experiences fewer errors in following it. We suggest that clothoid curves known to satisfy the criteria of goodness, be used to join the objective points. In Section 3 we present a method to join two arbitrary (within bounds) objective points that are each uniquely specified by position, ori-
tan~
I = r
(1)
where 1is the wheelbase of the vehicle, r is the radius of the path, and c is the curvature of the path. Also, the angular velocity of the driving rear wheel is determined only by the vehicle speed (v) and the radius of the wheel (R w): 11
Q)
=-
(2)
Rw
If the guide point is placed eIsewhe!'e, expressions for ~ and CIJ are more complex than shown in Eq. (1) and Eq. (2). The reference steering angle and angular velocity of the driving wheel must then be obtained by numerical integration.
443
y(s». Tangent direction (9(s» and curvature (c(s» can be derived along a path: dy (s) (3) 9 (s) '" dx (s)
r
C
(s)
d9 (s)
(4)
"'ds
The continuity of (x, y) is guaranteed if a path is continuous. But the effect of the continuity of vehicle heading and steering on the path depends on the guide point. Since the guide point is chosen at the cente!" of the rear axle, the heading of the vehicle 8y is aligned with the tangent direction of the path 9, and the steering angle $ is determined by the curvature of the pa th, as in Eq. (1). Thus, the continuity of (x, y, 8y, $) is tantamount to the continuity of (x,y,9,c) . If we define posture as the quadruple of parameters (x,y,9,c), a posture describes the state of a conventionally steered vehicle, and a path is required to be posture-continuous for easy tracking.
I Fig. 1. Geometry of a bicycle model • The vehicle is able to tollow the minimum turning radius for the maximum steering angle (Nelson, 1989). In other words, the peak steering angle is smaller than that for any other guide point. • The heading of the vehicle is aligned with the tangent direction of the path. This gives a more reasonable vantage point for a vision camera or a range scanner mounted at the front of the vehicle, as in Fig. 2, and a smalle!" area is swept by the vehicle.
Further, the rate of change of curvature (sharpness) of the path is important, too, since the linearity of curvature of the path dictates the linearity of steering motion along the path. If we assume that less control effort is required for an actuator to provide a linear velocity profile than an arbitrary nonlinear one, the extent to which steering motions are likely to keep a vehicle on a desired path can be correlated to the linearity of curvature of the path. In the following subsection, clothoid curves are introduced and, are compared the performances of a vehicle tracking a clothoid path and an arc path.
Fig. 2. The heading alignment along a path
Oothoid Curves
For the guide point at the center of the rear axle, paths with discontinuities of curvature will require infinite acceleration of the steering wheel. Consider the motion of the vehicle along a path which consists of two circular arcs. There is a discontinuity in curvature at the point where the two circular arcs meet. An infinite acceleration in steering is required for the vehicle to stay on the specified path provided that it does not come to a stop at the transition point, simply because it takes a finite amount of time to switch to the new curvature. In reality, moving through a transition point with non-zero velocity results in an offset error along the desired path. likewise, rotary body inertia requires continuity of heading, and steering inertia requires continuity of curvature. This is not the case if the guide point is moved to the front wheel. However, this choice is at the cost of the advantages discussed above.
Oothoid curves (Kanayama, 1985; Yates, 1952) are a family of curves that are posture-continuous, and are distinct in that their curvature varies linearly with the length of the curve: C (s)
'" /cs
+ cj
(5)
where k is the rate of change of curvature (sharpness) of the curve and c j is the initial curvature and s is the curve length. Given an initial posture, sharpness, and the distance along the curve, the curve can be described by Eq. (5), (6), (7) and (8): 9(s) '"
9.+Jsoc(Od~ I
(6) (7)
y I. +JsOSin(~~2+c .~+9 .)d~ 2"
(8)
I
yes) '"
~s2+c .s+9 I. 2 I
(~~2+c .~+9 I.)d~ 2 I
x(s) '" x .+Jsocos
Selection of the guide point is important to the formulation of the path tracking problem. When the guide point is chosen on the center of the rear axle, steering and driving reference inputs are computed from the path parameters and vehicle speed, respectively, as in Eq. (1) and (2). This enables the planning and controlling of steering along a path to be independent of speed. It is referred to as path tracking, in contrast to trajectory tracking in which steering and speed are controlled along a time history of position (Shin, 1990).
'"
The response of a robot vehicle to different reference paths was compared. Open loop responses of a vehicle following a path composed of arcs and straight lines, and another composed of clothoid segments, are shown in Fig. 3. The vehicle is made to follow a set of steering commands, which are oir tained from the path specification using Eq. (1), without trying to compensate for tracking errors. The dynamiC responses of the vehicle are modeled as first order lag systems with hard limits on acceleration. In each case, the reference path is indicated by dashed lines, while the actual path is indicated by solid lines. Simulation results show that an arc path results in larger steady state tracking errors caused by discontinuities in curvature and support the claim that "good" paths are intrinsically easier to track.
Vehicle Path Modeling The configuration of a conventionally steered vehicle moving on a plane surface can be described completely by a set of coordinates (x, y, 8y, $); vehicle position, heading, and steering angle, respectively. Continuity of (x, y, 8y, $) is recommended, because the vehicle has inertia and finite control response, which preclude discontinuous motions. Otherwise, tracking errors will be grea ter. Since a path specifies an ideal motion of a conventionally steered vehicle, a path must guarantee the continuity of (x, y, 9 v, $) of a vehicle.
PA TH GENERATION This section presents a method to generate paths that are intrinsically easy to track. The problem is to produce a unique, easily trackable, continuous path from a given sequence of oir
A path can be parameterized in te!"ms of path length s as (x(s),
444
y[""
. .. ..
1be following tw<>-step method generates a unique posturecontinuous path from a sequence of points. The first step is to derive a sequence of unique postures from the objective points; the second, is to interpolate between those postures with clothoid segments. Heading and curvature at the starting and ending positions are presumed from the.configura tion of the vehicle.
position error avg 1.091 max 2.621
10
-10
-to
-~~-I~o~o--~I~o-',~o--.. ;-~ ..~~~'-~m'-·ro--
,["" (A) Along a path composed of arcs and straight lines y[mj
.. .. ,.
position error avg 0.520 max 0.991
Fig. 4. Postures and associated circles from objective points IX= (Xo• ...• X,,) = {(xO.yO) • ...• (x".y,,)}) be a Let sequence of objective points. The associated circle at X. is defined to be the circle which passes through' points X. _ l' X • X. + 1 as in Fig. 4. Then the heading of the vehicle at is taIlen'as the direction of the tangent to the associated circfe at X,, and the curvature is the reciprocal of the oscuIating radius 6f the associated circle, denoting the posture thus obtained as the associated postures, The next step is to connect neighboring associated postures with clothoid segments.
10
it.
-10 -20
-30
-"L_I~O~O--~I'o-''''~-'';-~''~~~'-~~'-'ro--'[mj
It is not always possible to connect two neighboring postures with one clothoid curve segment, because the four governing and (8) cannot be satisfied simultaneousequations (5), (6), ly with only the two parameters (sharpness kand length s) that a clothoid curve provides. To satisfy these four equations, at least two clothoid segments are needed. However, the general problem cannot be solved with only two clothoid segments. Fig. 5 shows two pairs of associated postures and their associated circles. Let Pi, Pf denote the starting and the ending postures, respectively. Ci and Cfdenote the circles corresponding to the curvatures at Pj, Pt- They are drawn by solid lines and dotted lines, respectively.
(B) Along a path composed of clothoid segments
m,
Fig. 3. Open-loop performance: time constant = 0.1 s, vehicle speed =5m/s jective points. This problem is similar to those addressed by other researchers (Hongo, 1985; Kanayama, 1985; Nelson, 1989).
Existing Path Generation Methods Hongo (1985) proposed a method to generate continuous paths composed of connected straight lines and circular arcs from a sequence of objective points. While paths composed solely of arcs and straight lines are easy to compute, such a scheme leaves curvature discontinuities at the transitions of the segments, as discussed previously.
(X)
.... * • •
~
..
'.
x:--:/' (O)~~ (Xl
Pi
Pi ~ ,: C
",. , /. .... ,
Certain polynomial spline curves are candidates for path segments, because they guarantee continuity of posture. Nelson (1989) proposed quintic spline and polar spline curves obtained from tw<>-point boundary conditions. However, these spline curves do not guarantee linear gradients of curvature. clothoid curves, by contrast, do vary linearly with the distance along the curve.
.
\.
'··'·,-~L_. , -··'· (A)
i
(B)
Fig. 5. Determination of the sign of the first clothoid segment As in (A) of Fig. 5, if the orientation of Pf is outward from Cj. the ending part of a solution curve should be inside Cj. Then, it is plausible that the starting part of a solution curve also lies inside Ci. Similarly, if the direction of the Pfis inward into Cj, as in (B) of Fig. 5, it is plausible that the starting part of a solution curve lies outside Ci. Note that the sign of the sharpness determines the side of the associated circle on which the dothoid segment lies: (1) If the sharpness is zero, then the dothoid curve remains on the associated circle in question. (2) If the sharpness is positive, then the clothoid curve will be on the left side of the associated circle. (3) Otherwise, it will be on the right side of the associated circle. Hence, a solution curve should satisfy the following proposition:
Kanayama (1985) proposed clothoid curves for transitions between straight line paths, where symmetric pairs of clothoid curves with zero curvature are used to round the corners of straight line junctions. The abov~ mentioned approaches have in general sought to simplify path representation by rounding corners of poly-line fits through a given sequence of points. However, in this paper, smooth and continuously curved paths are directly generated from a given sequence of objective points without adhering to any straight line segments.
If the direction of Pf is outward from Cj, the sign of k of the
Path Generation from a Sequence of Points
445
first clothoid segment must be choscn so that the curve lies insideCj.
If the direction of Pf is inward into Cj, the sign of le of the first clothoid segment must be chosen so that the curve lies outsideCj Otherwise, le of the first clothoid segment is chosen so that the curve remains on Cj.l
(A) when
Fig. 6. Determination of the sign of the last clothoid segment
cj
~ cf~
lej,lef~
0,
(B)when
Cj~O?cf'
lej,lef ? 0
0
As a corollary to the above proposition, the sign of le of the last clothoid segment can be determined (Fig. 6):
If the direction of P j is outward from Cf the sign of le of the last clothoid segment must be chosen SO that the curve lies outside
Cf.
If the direction of Pj is inward into Cf, the sign of le of the last clothoid segment must be chosen so that the curve lies inside
Cf.
Otherwise, le of the last clothoid segment must be chosen so that the curve lies on Cf Fig. 7 shows all possible cases of curvature variations between a pair of neigh boring postures. Notice that the signs of idor the first and the last segments are the same for each case. However, the sign is the opposite of that required for the curvature variation between the postures for all the cases except (0 and (D) of Fig. 7. Thus, the general problem to connect a pair of neighboring associated postures cannot be solved with two clothoid curve segments.
(q when
O? c j ~ c '
(D) when
f
lei,Ie,~O
O~cf?Cj '
lej,lef ? 0
One adequate solution set of the clothoids is the set of three
cl~~ho~~\. s:~~_n~. ~~sl)' ~~~~2),~~~si' ~e ~b~p~ I ' This _om!:.
d_.,ok ~,e u.~~. ~,th" clOt.'10'_">"o_ .~ •. .s ~ 0I:1. nation is plausible for the following reasons:
~)
1. The signs of sharpness for the first and last clothoid segments are the same. 2. The sharpness for the second clothoid segment is equal in magnitude and opposite in sign to the first and last segments. This enables the curve of three clothoid segments to satisfy the curvature variation between the starting and the ending postures by varying SI' $2' $3' even though the sign of the first and the last clothoid segments satisfies the curve location requirement.
(E) when
cf~
0
~
cj
,
(F) when
Iej,lrJ~O
cf? c i ? 0 , Iej,lefS 0
Fig. 7. Cases by curvature variation of Pi and P
f
3. There are four variables in the combination: k,sl' s2' s3 ' It is possible to find a unique solution satisfying the following four equations which describe the mathematical relationship between the starting ane..; ending postures:
Sf= Sj+cj(sl +s2+ s 3) +1e(sls2- s 2s 3+ s 3s l) le 2 2 2 +2 (sl-s2+ s 3)
(9)
X
f
1.
= Xj + S~' cos SI (~) d~ + +
In this case, Pj and Pfshare the same associated circle, two postures are connected with the part of their associated circle, which is a clothoid curve of zero sharpness.
446
S:: cosS 2 (~) d~
S:: cos 8 3 (~) d~
(10)
(11)
Y/=Y;+ f~' sine l
+
f::
(~)d~+ j;:Sine2(~)d~ (12) sine 3 (~)d~
where,
Since Equations (11) and (12) contain Fresnel integrals, for which there is no closed form . solution, the values of /c, $ I' $2' $3 are computed using the numerical method (Press, 1986) outlined in Fig. 8. Initial values for .r 1 and $2 are chosen to be 1/3 of the average of the lengths of two of the arcs that connect Pi and Pf Eq. (9) and (10) are used to compute k and s. Then, xc' Ye can be computed for the quadruple (.t, S ,S ,s ) . S'unpson ,s approxunation. .. Idea 11 y (xc' Ye =XI- Y and 2 3in usmg fact values of SI and s2 are adjusted until the dif erence is within a threshold.
fe
Fig. 9. Continuous path from a sequence of seven points where goals are commonly posed as an evolving string of points. Paths generated by the proposed method pass through all the objective points, as in Fig. 10. • The method guarantees continuity of position, heading, a~d curvature along the path. Further, sharpness is pieceWlSC constant. Paths generated by the method always sweep outside the acute angles formed by straight line connection of the way points, as in Fig. 10. The resulting paths are especially useful for interpolating around obstacles that are commonly on the inside of angles. In contrast, the paths by other methods pass always inside the corners. CONCLUSION We have presented two conditions for paths that are intrinsically easy to track: posture continuity and linearity of curvature along the path. Oothoid curves are good candidates to satisfy these conditions. Simulations show better performance when a vehicle tracks paths of clothoids versus when it tracks paths composed of arcs.
RESULTS Connecting a pair of neighboring associated postures was accomplished successfully, as in Fig. 7, using three clothoid curves. Fig. 9 shows the graphic result of a posture-continuous path generated by clothoid curves through the given seven points. As the first step of the proposed method, a sequence of seven postures were generated. (Seven arrows in Fig. 9). Then, three clothoid segments are used to interpolate between neigh boring postures. Fig. 10 shows a comparison of the performance of the proposed method using redshank Kanayama's method (1(anayama, 1985). Parameters of curvature and sharpness were constrained equally for all methods. Tne maximum ~harpnt!SS of Kanayama 's method and the maximum curvature used in the arc method are set at the same levels as in the proposed method. Paths, curvatures and sharpness along the paths are compared. Paths resulting from the proposed method have the following advantages over other methods: • The method proceeds from an arbitrary sequence of points. Generation of postures is essential to exploratory planning
A method for generating a continuous path was developed. This method uses clothoid segments and consists of two steps: First, a sequenO! of the postures is obtained using the objective points. Then, each pair of neighboring postures is connected with three clothoid curve segments. The method provides additional advantages in that preprocessing of the objective points is not necessary, as with arc and zero curvature clothoids. Further, the geometry of the paths generated always sweeps outside the acute angles formed by a straight-line connection of the way points. These are especially useful for interpolating around obstacles that are commonlyon the inside of angles. The method of obtaining postures, as in Fig. 4, requires that the circles formed by the radii of curvature of two postures intersect. Relaxing such a constraint would require heuristics to determine in termed.ia te postures. Assuming in termed.ia te p0stures could be found, such that the associated circles intersect the method could thclI be used 0,1 the new set of postUIL",: Thus far, the search for a completely general method that would generate a path between two completely arbitrary postures has not been fruitful. Directions for future rescardl include the following: (1) optimization of the lengths of more than three clothoid segments on the premise that some cost function can be used to find J
447
path
path
I
~
curvature alonQ the path.
curvature alon 'A
'A
.....
..
_'A J
..
•
derNatlV
ot
•
,
•
•
la
II
~~~.-~~~.-~~~.-~~~~~~.~.~~
11
curwture along the path •
..... ~~~~--~~~'-7-~~.-~.-..~.-7",-,~.-,,~.-7,••~ Fig. 10 - (A). Oothoid path with zero curvature transition
Fig. 10 - (B). Path with circular arcs and straight lines
path
gOrithm. In Proceedings IECON 1985. 535-540. Kanayama, Y. and N. Miyake (1985). Trajectory generation for mobile robots. In Robolic Re5eilrch: The Third Inlenullional Symposium on Robolics Re5eilrch, Gouvieux, France. 333340. Kanayama, Yand B. Hartman (1989). Smooth local path planning for Autonomous Vehicles. In Procudings IEEE InloMlioMl Conference on Robolics IInd Aulo71llllion. 1265-1790. Nelson, W. L. (1989). Continuous steering function control of robot cart. IEEE Trllnsllclions on Induslrilll Electronics, Vol. HFE-7. 330-337.
curvature alon
J
..
derNatille
•
ot
•
,
h.
•
•
curvature alon
10
Press, W. H., B. P. Flannery, S. A. Teukolsky, W. T. Vetterling (1986). Parabolic interpolation and brent method. In NumeriCIIl Recipes, Cambridge University Press. 299-302.
11
the
Shin, D. H. and S. Singh (1990). Chapter 13: Vehicle and path models for autonomous navigation. In Vision and Navigalion, Kluwer Academic Publisher. 283-307.
'2
th.
Yates, R. C. (1952). Co.
•
.0
,.
12
IJ
I"
Fig. 10 - (C). Oothoid path with the proposed method
better soluhon than the results here; (~) irr.provement ot the numerical method to connect postures through the Fresnel Integral to improve speed and accuracy. REFERENCES
Hongo, T., H. Arakawa, G. Sugimoto, K. Tange and Y. Yamamoto (1985). An Automatic guidance system of a seUcontrolled vehicle-the command system and control al-
448
CU7WS
and Properties, Oassics Publishing
PATH GENERATION FOR A ROBOT VEHICLE USING COMPOSITE CLOTH OlD SEGMENTS D.H. Shin, S. Slngh and W. Whlttaker Fi~ld Robolics
CUller,
Carfl~gu M~llOfl U"ill~rsily,
PiJ/sburgh. USA
Abstract. We present a method to generate fine grained. smooth paths for wheeled mobile robots. This method takes as input, a sequence of objective points that the robot must attain to avoid obstacles and to progress toward its goal. Segments of clothoid curves are used to join the objective points and hence provide a finer level of details for the robot controller. Previous path generation methods have sought to simplify a path by using arcs, superarcs, polynomial curves, and clothoid curves to round corners, which result from poly-line fits through a given sequence of points. The proposed method generates smooth and continuous curved paths directly from a given sequence of points without adhering to any straight line segments. By virtue of the property of clothoid curves, a generated path is continuous with respect to position, tangent direction, and curvature, and is linear in curvature. Aside from the properties innate to clothoid curves, the generated paths transition smoothly into turns, pass through all the way points, and sweep outside the corners. Keywords.
Robot vehicles; path generation; clothoid curve; linear curvature.
INTRODUcrION Mobile robot navigation is typically composed of three phases- perception, planning, and, control. A robot must perceive
its state with respect to the world and use this information to plan its motion. Once a plan has been composed, it is necessary to enact the plan with requisite control actions. Typically, path planning occurs at two levels. FlI'St, path planning uses the information about the world to obtain an ordered sequence of objective points that the robot must attain to avoid obstacles and to progress towards its goal. These objective points constitute a coarse plan and a second step is necessary to generate a path in finer detail for the purposes of robot control. This paper reports a method of path generation to develop a fine grained specification of a path from a sequence of objective points that immediately suggests a control law for the mobile robot. This problem has been studied by other researchers (Hongo, 1985; Kanayama, 1985; Nelson, 1989), Our work is an extension of work reported by Kanayama and accrues several advantages over the other reported schemes.
entation and instantaneous path curvature. In Section 4, we evaluate the pe!'formance of the presented method. Fmally. Section 5 advances the conclusions and suggests the directions for future research. CONTINUOUS PATHS Guide Point For our analysis a bicycle model is used as an archetype for modeling robot vehicles with two degrees of freedom, steering, and propulsion. The guide point is the point of the vehicle that is controlled to follow the given path. The choice of the guide point is an important decision-it affects the desired steering and propulsion functions required to follow the given path and speed. We have chosen the guide point to be at the midpoint of the rear axle (Fig. 1) resulting in the following advantages: • The steering angle at any point on the path is determined geometrically, independent of speed, in the following manner:
In the following section we first discuss common kinematic models for mobile robots. We have used a "bicycle" model to represent a robot vehicle. The advantage of such a model is that there exist simple geometric relationships between the curvature of the reference path and steering angle for the robot. Given such a model, we suggest criteria for "goodness" of candidate path segments that may be used to join the objective points. As with any control system, the response of a mobile robot in tracking a path is partly dependant on the nature of the reference path. We show that if the type of path generated is intrinSically easier for the robot to track, a prototypical robot following such a pa th experiences fewer errors in following it. We suggest that clothoid curves known to satisfy the criteria of goodness, be used to join the objective points. In Section 3 we present a method to join two arbitrary (within bounds) objective points that are each uniquely specified by position, ori-
tan~
I = r
(1)
where 1is the wheelbase of the vehicle, r is the radius of the path, and c is the curvature of the path. Also, the angular velocity of the driving rear wheel is determined only by the vehicle speed (v) and the radius of the wheel (R w): 11
Q)
=-
(2)
Rw
If the guide point is placed eIsewhe!'e, expressions for ~ and CIJ are more complex than shown in Eq. (1) and Eq. (2). The reference steering angle and angular velocity of the driving wheel must then be obtained by numerical integration.
443
y(s». Tangent direction (9(s» and curvature (c(s» can be derived along a path: dy (s) (3) 9 (s) '" dx (s)
r
C
(s)
d9 (s)
(4)
"'ds
The continuity of (x, y) is guaranteed if a path is continuous. But the effect of the continuity of vehicle heading and steering on the path depends on the guide point. Since the guide point is chosen at the cente!" of the rear axle, the heading of the vehicle 8y is aligned with the tangent direction of the path 9, and the steering angle $ is determined by the curvature of the pa th, as in Eq. (1). Thus, the continuity of (x, y, 8y, $) is tantamount to the continuity of (x,y,9,c) . If we define posture as the quadruple of parameters (x,y,9,c), a posture describes the state of a conventionally steered vehicle, and a path is required to be posture-continuous for easy tracking.
I Fig. 1. Geometry of a bicycle model • The vehicle is able to tollow the minimum turning radius for the maximum steering angle (Nelson, 1989). In other words, the peak steering angle is smaller than that for any other guide point. • The heading of the vehicle is aligned with the tangent direction of the path. This gives a more reasonable vantage point for a vision camera or a range scanner mounted at the front of the vehicle, as in Fig. 2, and a smalle!" area is swept by the vehicle.
Further, the rate of change of curvature (sharpness) of the path is important, too, since the linearity of curvature of the path dictates the linearity of steering motion along the path. If we assume that less control effort is required for an actuator to provide a linear velocity profile than an arbitrary nonlinear one, the extent to which steering motions are likely to keep a vehicle on a desired path can be correlated to the linearity of curvature of the path. In the following subsection, clothoid curves are introduced and, are compared the performances of a vehicle tracking a clothoid path and an arc path.
Fig. 2. The heading alignment along a path
Oothoid Curves
For the guide point at the center of the rear axle, paths with discontinuities of curvature will require infinite acceleration of the steering wheel. Consider the motion of the vehicle along a path which consists of two circular arcs. There is a discontinuity in curvature at the point where the two circular arcs meet. An infinite acceleration in steering is required for the vehicle to stay on the specified path provided that it does not come to a stop at the transition point, simply because it takes a finite amount of time to switch to the new curvature. In reality, moving through a transition point with non-zero velocity results in an offset error along the desired path. likewise, rotary body inertia requires continuity of heading, and steering inertia requires continuity of curvature. This is not the case if the guide point is moved to the front wheel. However, this choice is at the cost of the advantages discussed above.
Oothoid curves (Kanayama, 1985; Yates, 1952) are a family of curves that are posture-continuous, and are distinct in that their curvature varies linearly with the length of the curve: C (s)
'" /cs
+ cj
(5)
where k is the rate of change of curvature (sharpness) of the curve and c j is the initial curvature and s is the curve length. Given an initial posture, sharpness, and the distance along the curve, the curve can be described by Eq. (5), (6), (7) and (8): 9(s) '"
9.+Jsoc(Od~ I
(6) (7)
y I. +JsOSin(~~2+c .~+9 .)d~ 2"
(8)
I
yes) '"
~s2+c .s+9 I. 2 I
(~~2+c .~+9 I.)d~ 2 I
x(s) '" x .+Jsocos
Selection of the guide point is important to the formulation of the path tracking problem. When the guide point is chosen on the center of the rear axle, steering and driving reference inputs are computed from the path parameters and vehicle speed, respectively, as in Eq. (1) and (2). This enables the planning and controlling of steering along a path to be independent of speed. It is referred to as path tracking, in contrast to trajectory tracking in which steering and speed are controlled along a time history of position (Shin, 1990).
'"
The response of a robot vehicle to different reference paths was compared. Open loop responses of a vehicle following a path composed of arcs and straight lines, and another composed of clothoid segments, are shown in Fig. 3. The vehicle is made to follow a set of steering commands, which are oir tained from the path specification using Eq. (1), without trying to compensate for tracking errors. The dynamiC responses of the vehicle are modeled as first order lag systems with hard limits on acceleration. In each case, the reference path is indicated by dashed lines, while the actual path is indicated by solid lines. Simulation results show that an arc path results in larger steady state tracking errors caused by discontinuities in curvature and support the claim that "good" paths are intrinsically easier to track.
Vehicle Path Modeling The configuration of a conventionally steered vehicle moving on a plane surface can be described completely by a set of coordinates (x, y, 8y, $); vehicle position, heading, and steering angle, respectively. Continuity of (x, y, 8y, $) is recommended, because the vehicle has inertia and finite control response, which preclude discontinuous motions. Otherwise, tracking errors will be grea ter. Since a path specifies an ideal motion of a conventionally steered vehicle, a path must guarantee the continuity of (x, y, 9 v, $) of a vehicle.
PA TH GENERATION This section presents a method to generate paths that are intrinsically easy to track. The problem is to produce a unique, easily trackable, continuous path from a given sequence of oir
A path can be parameterized in te!"ms of path length s as (x(s),
444
y[""
. .. ..
1be following tw<>-step method generates a unique posturecontinuous path from a sequence of points. The first step is to derive a sequence of unique postures from the objective points; the second, is to interpolate between those postures with clothoid segments. Heading and curvature at the starting and ending positions are presumed from the.configura tion of the vehicle.
position error avg 1.091 max 2.621
10
-10
-to
-~~-I~o~o--~I~o-',~o--.. ;-~ ..~~~'-~m'-·ro--
,["" (A) Along a path composed of arcs and straight lines y[mj
.. .. ,.
position error avg 0.520 max 0.991
Fig. 4. Postures and associated circles from objective points IX= (Xo• ...• X,,) = {(xO.yO) • ...• (x".y,,)}) be a Let sequence of objective points. The associated circle at X. is defined to be the circle which passes through' points X. _ l' X • X. + 1 as in Fig. 4. Then the heading of the vehicle at is taIlen'as the direction of the tangent to the associated circfe at X,, and the curvature is the reciprocal of the oscuIating radius 6f the associated circle, denoting the posture thus obtained as the associated postures, The next step is to connect neighboring associated postures with clothoid segments.
10
it.
-10 -20
-30
-"L_I~O~O--~I'o-''''~-'';-~''~~~'-~~'-'ro--'[mj
It is not always possible to connect two neighboring postures with one clothoid curve segment, because the four governing and (8) cannot be satisfied simultaneousequations (5), (6), ly with only the two parameters (sharpness kand length s) that a clothoid curve provides. To satisfy these four equations, at least two clothoid segments are needed. However, the general problem cannot be solved with only two clothoid segments. Fig. 5 shows two pairs of associated postures and their associated circles. Let Pi, Pf denote the starting and the ending postures, respectively. Ci and Cfdenote the circles corresponding to the curvatures at Pj, Pt- They are drawn by solid lines and dotted lines, respectively.
(B) Along a path composed of clothoid segments
m,
Fig. 3. Open-loop performance: time constant = 0.1 s, vehicle speed =5m/s jective points. This problem is similar to those addressed by other researchers (Hongo, 1985; Kanayama, 1985; Nelson, 1989).
Existing Path Generation Methods Hongo (1985) proposed a method to generate continuous paths composed of connected straight lines and circular arcs from a sequence of objective points. While paths composed solely of arcs and straight lines are easy to compute, such a scheme leaves curvature discontinuities at the transitions of the segments, as discussed previously.
(X)
.... * • •
~
..
'.
x:--:/' (O)~~ (Xl
Pi
Pi ~ ,: C
",. , /. .... ,
Certain polynomial spline curves are candidates for path segments, because they guarantee continuity of posture. Nelson (1989) proposed quintic spline and polar spline curves obtained from tw<>-point boundary conditions. However, these spline curves do not guarantee linear gradients of curvature. clothoid curves, by contrast, do vary linearly with the distance along the curve.
.
\.
'··'·,-~L_. , -··'· (A)
i
(B)
Fig. 5. Determination of the sign of the first clothoid segment As in (A) of Fig. 5, if the orientation of Pf is outward from Cj. the ending part of a solution curve should be inside Cj. Then, it is plausible that the starting part of a solution curve also lies inside Ci. Similarly, if the direction of the Pfis inward into Cj, as in (B) of Fig. 5, it is plausible that the starting part of a solution curve lies outside Ci. Note that the sign of the sharpness determines the side of the associated circle on which the dothoid segment lies: (1) If the sharpness is zero, then the dothoid curve remains on the associated circle in question. (2) If the sharpness is positive, then the clothoid curve will be on the left side of the associated circle. (3) Otherwise, it will be on the right side of the associated circle. Hence, a solution curve should satisfy the following proposition:
Kanayama (1985) proposed clothoid curves for transitions between straight line paths, where symmetric pairs of clothoid curves with zero curvature are used to round the corners of straight line junctions. The abov~ mentioned approaches have in general sought to simplify path representation by rounding corners of poly-line fits through a given sequence of points. However, in this paper, smooth and continuously curved paths are directly generated from a given sequence of objective points without adhering to any straight line segments.
If the direction of Pf is outward from Cj, the sign of k of the
Path Generation from a Sequence of Points
445
first clothoid segment must be choscn so that the curve lies insideCj.
If the direction of Pf is inward into Cj, the sign of le of the first clothoid segment must be chosen so that the curve lies outsideCj Otherwise, le of the first clothoid segment is chosen so that the curve remains on Cj.l
(A) when
Fig. 6. Determination of the sign of the last clothoid segment
cj
~ cf~
lej,lef~
0,
(B)when
Cj~O?cf'
lej,lef ? 0
0
As a corollary to the above proposition, the sign of le of the last clothoid segment can be determined (Fig. 6):
If the direction of P j is outward from Cf the sign of le of the last clothoid segment must be chosen SO that the curve lies outside
Cf.
If the direction of Pj is inward into Cf, the sign of le of the last clothoid segment must be chosen so that the curve lies inside
Cf.
Otherwise, le of the last clothoid segment must be chosen so that the curve lies on Cf Fig. 7 shows all possible cases of curvature variations between a pair of neigh boring postures. Notice that the signs of idor the first and the last segments are the same for each case. However, the sign is the opposite of that required for the curvature variation between the postures for all the cases except (0 and (D) of Fig. 7. Thus, the general problem to connect a pair of neighboring associated postures cannot be solved with two clothoid curve segments.
(q when
O? c j ~ c '
(D) when
f
lei,Ie,~O
O~cf?Cj '
lej,lef ? 0
One adequate solution set of the clothoids is the set of three
cl~~ho~~\. s:~~_n~. ~~sl)' ~~~~2),~~~si' ~e ~b~p~ I ' This _om!:.
d_.,ok ~,e u.~~. ~,th" clOt.'10'_">"o_ .~ •. .s ~ 0I:1. nation is plausible for the following reasons:
~)
1. The signs of sharpness for the first and last clothoid segments are the same. 2. The sharpness for the second clothoid segment is equal in magnitude and opposite in sign to the first and last segments. This enables the curve of three clothoid segments to satisfy the curvature variation between the starting and the ending postures by varying SI' $2' $3' even though the sign of the first and the last clothoid segments satisfies the curve location requirement.
(E) when
cf~
0
~
cj
,
(F) when
Iej,lrJ~O
cf? c i ? 0 , Iej,lefS 0
Fig. 7. Cases by curvature variation of Pi and P
f
3. There are four variables in the combination: k,sl' s2' s3 ' It is possible to find a unique solution satisfying the following four equations which describe the mathematical relationship between the starting ane..; ending postures:
Sf= Sj+cj(sl +s2+ s 3) +1e(sls2- s 2s 3+ s 3s l) le 2 2 2 +2 (sl-s2+ s 3)
(9)
X
f
1.
= Xj + S~' cos SI (~) d~ + +
In this case, Pj and Pfshare the same associated circle, two postures are connected with the part of their associated circle, which is a clothoid curve of zero sharpness.
446
S:: cosS 2 (~) d~
S:: cos 8 3 (~) d~
(10)
(11)
Y/=Y;+ f~' sine l
+
f::
(~)d~+ j;:Sine2(~)d~ (12) sine 3 (~)d~
where,
Since Equations (11) and (12) contain Fresnel integrals, for which there is no closed form . solution, the values of /c, $ I' $2' $3 are computed using the numerical method (Press, 1986) outlined in Fig. 8. Initial values for .r 1 and $2 are chosen to be 1/3 of the average of the lengths of two of the arcs that connect Pi and Pf Eq. (9) and (10) are used to compute k and s. Then, xc' Ye can be computed for the quadruple (.t, S ,S ,s ) . S'unpson ,s approxunation. .. Idea 11 y (xc' Ye =XI- Y and 2 3in usmg fact values of SI and s2 are adjusted until the dif erence is within a threshold.
fe
Fig. 9. Continuous path from a sequence of seven points where goals are commonly posed as an evolving string of points. Paths generated by the proposed method pass through all the objective points, as in Fig. 10. • The method guarantees continuity of position, heading, a~d curvature along the path. Further, sharpness is pieceWlSC constant. Paths generated by the method always sweep outside the acute angles formed by straight line connection of the way points, as in Fig. 10. The resulting paths are especially useful for interpolating around obstacles that are commonly on the inside of angles. In contrast, the paths by other methods pass always inside the corners. CONCLUSION We have presented two conditions for paths that are intrinsically easy to track: posture continuity and linearity of curvature along the path. Oothoid curves are good candidates to satisfy these conditions. Simulations show better performance when a vehicle tracks paths of clothoids versus when it tracks paths composed of arcs.
RESULTS Connecting a pair of neighboring associated postures was accomplished successfully, as in Fig. 7, using three clothoid curves. Fig. 9 shows the graphic result of a posture-continuous path generated by clothoid curves through the given seven points. As the first step of the proposed method, a sequence of seven postures were generated. (Seven arrows in Fig. 9). Then, three clothoid segments are used to interpolate between neigh boring postures. Fig. 10 shows a comparison of the performance of the proposed method using redshank Kanayama's method (1(anayama, 1985). Parameters of curvature and sharpness were constrained equally for all methods. Tne maximum ~harpnt!SS of Kanayama 's method and the maximum curvature used in the arc method are set at the same levels as in the proposed method. Paths, curvatures and sharpness along the paths are compared. Paths resulting from the proposed method have the following advantages over other methods: • The method proceeds from an arbitrary sequence of points. Generation of postures is essential to exploratory planning
A method for generating a continuous path was developed. This method uses clothoid segments and consists of two steps: First, a sequenO! of the postures is obtained using the objective points. Then, each pair of neighboring postures is connected with three clothoid curve segments. The method provides additional advantages in that preprocessing of the objective points is not necessary, as with arc and zero curvature clothoids. Further, the geometry of the paths generated always sweeps outside the acute angles formed by a straight-line connection of the way points. These are especially useful for interpolating around obstacles that are commonlyon the inside of angles. The method of obtaining postures, as in Fig. 4, requires that the circles formed by the radii of curvature of two postures intersect. Relaxing such a constraint would require heuristics to determine in termed.ia te postures. Assuming in termed.ia te p0stures could be found, such that the associated circles intersect the method could thclI be used 0,1 the new set of postUIL",: Thus far, the search for a completely general method that would generate a path between two completely arbitrary postures has not been fruitful. Directions for future rescardl include the following: (1) optimization of the lengths of more than three clothoid segments on the premise that some cost function can be used to find J
447
path
path
I
~
curvature alonQ the path.
curvature alon 'A
'A
.....
..
_'A J
..
•
derNatlV
ot
•
,
•
•
la
II
~~~.-~~~.-~~~.-~~~~~~.~.~~
11
curwture along the path •
..... ~~~~--~~~'-7-~~.-~.-..~.-7",-,~.-,,~.-7,••~ Fig. 10 - (A). Oothoid path with zero curvature transition
Fig. 10 - (B). Path with circular arcs and straight lines
path
gOrithm. In Proceedings IECON 1985. 535-540. Kanayama, Y. and N. Miyake (1985). Trajectory generation for mobile robots. In Robolic Re5eilrch: The Third Inlenullional Symposium on Robolics Re5eilrch, Gouvieux, France. 333340. Kanayama, Yand B. Hartman (1989). Smooth local path planning for Autonomous Vehicles. In Procudings IEEE InloMlioMl Conference on Robolics IInd Aulo71llllion. 1265-1790. Nelson, W. L. (1989). Continuous steering function control of robot cart. IEEE Trllnsllclions on Induslrilll Electronics, Vol. HFE-7. 330-337.
curvature alon
J
..
derNatille
•
ot
•
,
h.
•
•
curvature alon
10
Press, W. H., B. P. Flannery, S. A. Teukolsky, W. T. Vetterling (1986). Parabolic interpolation and brent method. In NumeriCIIl Recipes, Cambridge University Press. 299-302.
11
the
Shin, D. H. and S. Singh (1990). Chapter 13: Vehicle and path models for autonomous navigation. In Vision and Navigalion, Kluwer Academic Publisher. 283-307.
'2
th.
Yates, R. C. (1952). Co.
•
.0
,.
12
IJ
I"
Fig. 10 - (C). Oothoid path with the proposed method
better soluhon than the results here; (~) irr.provement ot the numerical method to connect postures through the Fresnel Integral to improve speed and accuracy. REFERENCES
Hongo, T., H. Arakawa, G. Sugimoto, K. Tange and Y. Yamamoto (1985). An Automatic guidance system of a seUcontrolled vehicle-the command system and control al-
448
CU7WS
and Properties, Oassics Publishing