Continuous curvature path generation based on β -spline curves for parking manoeuvres

Robotics and Autonomous Systems 56 (2008) 360–372 www.elsevier.com/locate/robot

Continuous curvature path generation based on β-spline curves for parking manoeuvres F. G´omez-Bravo a , F. Cuesta b,∗ , A. Ollero b , A. Viguria b a Department of Ingenier´ıa Electr´onica, Sistemas Inform´aticos y Autom´atica. Escuela Polit´ecnica Superior, Univ. de Huelva. Carretera de Palos-La R´abida s/n.,

E-21071 Huelva, Spain b Department of Ingenier´ıa de Sistemas y Autom´atica, Escuela Superior de Ingenieros, Universidad de Sevilla, Camino de los Descubrimientos,

E-41092 Sevilla, Spain Received 8 March 2006; received in revised form 30 July 2007; accepted 10 August 2007 Available online 29 August 2007

Abstract This paper focuses attention on continuous curvature path generation for parallel parking manoeuvres of autonomous conventional vehicles. The method is based on β-spline curves generation from a collision-free path previously calculated. Several constraints have been considered in order to ensure the existence of collision-free and admissible curves. The efficiency of the proposed method is demonstrated using the nonholonomic autonomous vehicles ROMEO-3R and ROMEO-4R designed and built at the University of Seville. c 2007 Elsevier B.V. All rights reserved.

Keywords: Mobile robots; Autonomous parking; Nonholonomic systems; Path planning

1. Introduction Parallel parking manoeuvres of autonomous conventional vehicles are a particular case of planning and motion control. Usually, path-planning problem has to take into account nonholonomic and noncollision constraints [14,16], and the control problem is solved by means of path-tracking techniques. Path-planning techniques should consider also physical characteristics of the vehicle such as the steering velocity boundary [19]. Parallel parking manoeuvres usually present points where the curvature of the paths is discontinuous. If a curvature discontinuity exists then a steering discontinuity results during the path following. Thus, if these type of manoeuvres are executed, the vehicle has to stop at these points. Nevertheless, in many practical situations, stops are not desirable. For this reason, continuous curvature generation is an important element in nonholonomic robot navigation.

∗ Corresponding author. Tel.: +34 9 5448 7356; fax: +34 9 5448 7340.

E-mail addresses: [email protected] (F. G´omez-Bravo), [email protected] (F. Cuesta), [email protected] (A. Ollero), [email protected] (A. Viguria). c 2007 Elsevier B.V. All rights reserved. 0921-8890/$ - see front matter doi:10.1016/j.robot.2007.08.004

On the one hand, several planning approaches focus their attention on direct continuous curvature path generation [13,15,20,31,32]. An application of continuous driving to parallel parking manoeuvre is presented in [7,25–28,34,33,35]. In [25,26] an approach based on sinusoidal inputs is presented. In [7,34] car-like scale vehicles are remotely controlled by an external computer. In [35] human drivers are assisted by means of a screen during the parking manoeuvres. On the other hand, environment geometric constraints can be explicitly considered in the design of a discontinuous curvature collision-free manoeuvre by using arcs of circumference [12,21]. Therefore, the design process itself guarantees that the manoeuvre is collision free. These results are related to that by Dubins [6] and Reeds and Shepp [29] works, in the sense that they only deal with minimum constant value of the curvature, considering minimal length paths and resulting in noncontinuous curvature manoeuvres. In [9] a new approach for parallel and diagonal parking manoeuvres composed of arcs of circumference is presented (see also [4,5,11]). In these works not only the minimum constant value of the curvature is considered but also an adaptive method for generating parking manoeuvres based on a fuzzy logic selector that takes into account the dimensions of the environment is introduced. Then, a new path with curvature

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

361

Fig. 1. β-spline curve and control vertices.

continuity that is close enough to the discontinuous curvature collision-free manoeuvre is generated. If this path accomplishes several constraints, a new collision-free parallel parking is obtained. Therefore, it is not necessary to implement a recursive algorithm to detect collisions and a continuous curvature path can be achieved. This method has been extended to the case of a vehicle with a trailer [4]. This paper describes in detail the continuous curvature path generation technique and illustrates also its application to the autonomous vehicle ROMEO-4R. Curves with smooth properties, like β-splines, are applied [1,17]. This curve presents continuity of the derivatives of a given order (first and second order, for instance) at the joints between adjacent segments. This type of curves is specified by a set of points, called control vertices; the curve tends to mimic the overall shape of the polygon defined by these points, as shown in Fig. 1. Convenient selection of these points assures admissibility and existence of collision-free paths. It should be noted that there are other approaches for generating paths with continuous curvature from arcs of circumferences, like using Clothoids curves for instance. However, when noncollision constraints are considered, β-splines require less collision-free space. Moreover, clothoids require higher computational cost than β-splines [30]. This paper is organised as follows: Section 2 presents a brief description of the parallel parking method. In Section 3 a detailed description of the use of β-splines for parallel parking is presented. Steering curvature constraints and guidelines for selecting the parameters involved are also included. Section 4 presents experimental results of the proposed methods with two different real autonomous vehicles. The paper closes with the conclusions and references. 2. Parallel parking Autonomous robot manoeuvring has been largely studied in recent years. Different smooth motion planning approaches have been applied for car-like vehicles [13]. Particularly, sinusoidal input has been applied to accomplish parking manoeuvres without considering collision avoidance [20]. An extension of this method involving parallel and perpendicular parking and collision avoidance is presented in [25–28]. This method provides a collision-free parallel parking manoeuvre, but it does not provide the set of starting locations where a collision-free solution exists. Comparison between this technique and the approach presented in this article can be found in [9]. More recent approaches have considered fuzzy control for car-like robot parallel parking. In [34,33] two fuzzy parallel parking controllers, the backward and forward, are provided to manoeuvre the steering angle of the vehicle. In [35] parking manoeuvre generation is applied to driving assistance

Fig. 2. Parallel parking manoeuvre.

so that the manoeuvre is not autonomously executed. In [7] a method for generating cubic spline reference paths is presented, the curvature of the spline is used to compute the steering angle of a remotely controlled scale car-like vehicle. The scenario is detected by a camera over the parking place. In this article, an alternative method for autonomous parallel parking is used. This technique was presented in [9], there, a practical approach for parking an intelligent autonomous conventional vehicle was introduced. The scenario was perceived by means of ultrasonic sensors carried out by the robot. The method was implemented in the frame of a fuzzy behaviour-based control architecture combining reactivity and planning [3,5]. This technique has been even applied for parking articulated vehicles (see also [4,5,11]). This section is devoted to present the basis of the method. When parallel parking manoeuvres are considered the vehicle is driven reactively, parallel to a line of vehicles, looking for a parking place (step 1 in Fig. 2). Once the parking place has been found, a set of starting points for the parking manoeuvre is calculated on-line (step 2 in Fig. 2). Then fuzzy logic is used to select the best starting point taking into account the environment where the vehicle has to park in. When the vehicle is stopped, a simple collision-free manoeuvre is generated (step 3 in Fig. 2). Then from this manoeuvre, a continuous curvature path is computed and a direct motion into the parking is generated. In the following subsections, details on the parking procedure are given. 2.1. Car and cart vehicles In recent years, many applications on autonomous vehicles has been developed from the adaptation of cars and carts vehicles [24]. The kinematics of these types of vehicles can be expressed in the form [15]      x˙ cos θ 0  v(t)  y˙  =  sin θ 0 (1) v(t)ρ(t) 0 1 θ˙ where (x, y) are the coordinates of the reference point i (see Fig. 3), θ is the vehicle’s heading, v(t) is the linear velocity of point i in the time t, and ρ(t) is the curvature of the path described by point i. The curvature radius can be written as R(t) =

1 1 = . ρ(t) tan φ

(2)

The value of R(t) is usually bounded by Rmin , the minimum curvature radius.

362

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 3. Cart and car-like vehicle.

The requirements for a collision-free path are (see [9] for details) |R1 | < |R1 max |

|R2 | ∈ [|R2 min |, |R2T |]

(4)

with |R1 max | =

Fig. 4. Parameters of parallel parking manoeuvre.

2.2. Manoeuvre design In [9], the parallel parking manoeuvre (see Fig. 4) is composed of a straight line segment L P and two arcs of circumference: L 2 (P2 → P1 ) of radius R2 and L 1 (P1 → P0 ) of radius R1 . The equations defining the manoeuvre are ∆y = (|R1 | + |R2 |)(1 − cos θ1 )

2C y

+ l 2 − (H − ∆y)2 | 2(H − ∆y − W ) ∆y − |R1 | |R2T | = 1 − cos θT |R2 min | =

0 = (|R1 | + |R2 |) sin θ1 − L p

|C x2 + C y2 − l 2 |

(3)

where θ1 and ∆y are defined in Fig. 4. Each value of the parameters [R1 , R2 , L p ] determines a manoeuvre. Collisions are avoided by planning a collision-free path for a virtual vehicle leaving the parking place. Final manoeuvre is obtained using this design in a reverse sequence. The first requirement for a continuous collision-free path leaving the parking place consists of avoiding the collision of the front of the vehicle with the obstacle ahead (see Fig. 5(a)). Even more, the planned path should avoid the collision of the vehicle’s lateral side with the obstacle ahead along the path L 2 (see Fig. 5(b)). The last possible collision is shown in Fig. 5(c). It represents the collision with the wall or line of vehicles opposite to the parking place.

|W 2

(5)

where W and H are defined in Fig. 4 and θT is the value of θ1 when the path is tangent to the point (C x , C y ). Even more, admissible paths have to accomplish the minimum curvature radius constraint |R1 |, |R2 | > Rmin .

(6)

The minimum forward displacement for a collision-free trajectory is obtained when L p gets its minimum value (R1 = Rmin and R2 = R2 min ) q L p min = (|Rmin | + |R2 min |)(2∆y) − ∆y 2 (7) and the minimum lateral displacement is obtained when L p gets its maximum value (R1 = Rmin and R2 = R2T ) q L p max = (Rmin + R2T )(2∆y) − ∆y 2 . (8) 2.3. Selection of parking manoeuvre by fuzzy logic In practice, it is necessary to select just one manoeuvre from all the feasible solutions. In the previous section, different optimal manoeuvres have been introduced. Each one of such

Fig. 5. Possible collisions in parallel parking.

363

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 7. Spline segments and control vertices. Fig. 6. β-spline curve and control vertices.

manoeuvres is optimal only with respect to a given simple criterion. In [9], a fuzzy selection system incorporating the knowledge of a human driver is applied for choosing one manoeuvre. This fuzzy system computes the best forward displacement (i.e. L p value) based on the lateral distances, from the inner and outer parking lines, and how large the parking place is, by providing a manoeuvre selection in the same way that a human driver selects the stopping point when parking a car. 3. β-splines in parallel parking The main drawback of the manoeuvres presented above is the discontinuity in the value of the curvature in the computed paths. Thus, the path P2 → P1 → P0 (see Fig. 4) composed of the arcs of circumference L 1 and L 2 , can be approximated by a β-spline curve as shown in Fig. 6. 3.1. β-spline curves A β-spline is a parametric piece-wise curve which presents continuity of the derivatives of a given order (first and second order, for instance) even at the joints between adjacent segments. This type of curves is specified by a set of points called control vertices. These vertices connected in sequence build up a control polygon. The curve tends to mimic the overall shape of the control polygon (see Fig. 1). Let the control polygon be composed of the sequence of control vertices [V0 , V1 , . . . , Vm ], each one specified by its cartesian coordinates. Then, the curve will be composed of m − 2 segments. Thus, the ith curve segment (denoted Q i ) (see Fig. 7) is determined by four control vertices Vi+r , r = −2, −1, 0, 1; and any point on this segment is a weighted average of these vertices. The coordinates of the point Q i (u) on the ith curve segment are Q i (u) =

1 X

br (β1 , β2 , u)Vi+r

(9)

r =−2

with 0 ≤ u < 1; i = 2, 3, . . . , where the weighting factors br are scalar functions evaluated at any value of the domain parameter u and on each shape parameter β1 and β2 . The

Fig. 8. Control polygon.

shape parameters determine the way in which the curve tends to mimic the shape of the control polygon. Previous works have experimentally shown that cubic splines represent the best choice when smooth changes in curvature are desired [17,18]. In that case, β1 = 0 and β2 = 0, and the weighting factors are defined by 1   6 2 b−2 (u)  b−1 (u)  3  =  b0 (u)   1  6 b1 (u)  0

−

1 2

1 2

0

−1

1 2

1 2

0

0

1 6   1 1     u 2 ×  . 1  u 2  −  u3 2  1 6 −

(10)

Then, from the collision-free manoeuvre, a control polygon is built by choosing points from the arcs of circumference (see Fig. 8). In the following it is described the technique to select these points in order to assure the existence and to build up an admissible β-spline. 3.2. The Hull-convex property One of the most significant property of the β-spline curves, is the Hull-convex property [1]. By this property, it is assured

364

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 10. Frontal collision.

Fig. 9. Control vertices over an arc of circumference.

that the surface where the curve goes across is bounded by the control polygon (see Fig. 9). Then, if the control vertices are over an arc of circumference and the distance between them (denoted δ) is regular, an important consequence can be derived [18]. Let the vertices of a β-spline segment be over an arc of the circumference C of radius R (see Fig. 9). If the distance between them is δ, where δ ≤ (π/6)R, then the β-spline segment is out of the circle C 0 whose radius R 0 is   3δ 0 . (11) R < R cos 2R

Fig. 11. Noncollision condition.

This constraint also assures that the segment will be in between both arcs of circumference.

Concerning the first condition, the spline will avoid the frontal collision with the obstacle ahead if along the curve the segment tangent to the path, with longitude equal to l, does not cross the straight line y = C x (see Fig. 10). Geometrically, it can be seen that the β-spline segments generated by the control vertices over the arc L 1 are collisionfree paths if the curvature radius is below R1 max . According to the previous section the curvature radius of these segments are upper bounded by R1 . Therefore, from (4) these segments are collision free. Concerning the second noncollision condition, the β-spline segments generated by the control vertices over the arc L 2 will be collision-free paths if they are contained between the original manoeuvre and the point C, see Fig. 11. Thus the spline has to be placed in between the circumference of radius Rc , see Fig. 11, and the arc of circumference of radius R2 defined by the original manoeuvre. The value of Rc is given by q Rc = (C x − Ox )2 + (C y − O y )2 (13)

3.3. Generation constraints

where (Ox , O y ) is the centre of the arc L 2 , which, from Fig. 11 and Eq. (3) can be written as

In order to obtain an admissible path using β-splines, four constraints have to be considered:

Ox = |L p | = |(|R2 | + |R1 |) sin θ1 |

(1) The spline must be a collision-free path. (2) Curvature must be bounded along the spline. (3) The spline must contain the starting and ending points defined by the original manoeuvre. (4) Change in curvature must accomplish velocity steering constraints.

Then, according to (11) the spline is a collision-free path if the following sufficient condition is satisfied   Rc 2R2 acos . (15) δ f ≤ δ1 = 3 R2

Even more, it can be experimentally shown that the curvature radius of the segment is upper bounded by R and lower bounded by R 0 [17,18]. Therefore, if the value of the curvature radius of the segment has to be kept in between R and R 0 then δ should accomplish  0 R 2R δ< acos . (12) 3 R

In the following sections it is shown how the proposed technique deals with these requirements. The idea consists of choosing a value for the distance between each vertex, δ f , in such a way that the resulting spline is an admissible path. For this propose, several constraints should be accomplished by δ f . 3.4. Collision-free path The spline will be a collision-free path if it accomplishes the three noncollision criteria presented in Section 2.2.

O y = y f − |R2 |.

(14)

Fig. 12(a) shows a manoeuvre violating condition (15) and exhibiting a lateral collision. On the other hand Fig. 12(b) illustrates a collision-free manoeuvre satisfying condition (15). Finally, the spline should also accomplish the last noncollision condition presented in Section 2.2. Due to the fact that R2 min is the lowest value of R2 that allows in avoiding collision with the obstacles opposite to the parking place, the β-spline segments generated by the control vertices over the arc L 2 are collision-free paths if their curvature radius is over R2 min . Thus, according to (11) the spline is a collision-free path if

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

365

Fig. 12. (a) Manoeuvre with lateral collision; (b) Collision-free manoeuvre.

Fig. 13. (a) β-spline curve for a parallel parking manoeuvre. (b) Original discontinuous curvature vs. continuous curvature from the β-spline.

δ f ≤ δ2 =

2R2 acos 3



R2 min R2



.

(16)

Thus, if the first vertex is V0 and p˙ s , p¨ s are the first and the second derivative vector specifications, respectively, then three phantom vertices are given by

3.5. Bounded curvature The value of the radius of curvature of an admissible path has to be over the minimum radius of the vehicle Rmin (i.e. the curvature is over bounded by ρmax = 1/Rmin ). Then, according to the previous section the segments of the spline generated by the first arc of circumference should accomplish this condition if   Rmin 2R1 acos (17) δ f ≤ δ3 = 3 R1 and the segment generated by the second arc should accomplish this condition if   2R2 Rmin δ f ≤ δ4 = acos . (18) 3 R2 3.6. Conditions on the starting and ending points In order to generate an admissible path, not only the starting and ending points should belong to the spline, but also, in these configurations the curve has to be tangent to the vehicle orientation. In this paper, auxiliary vertices are created at each extreme in order to achieve such conditions. These auxiliary vertices are known as phantom vertices. The methods to obtain these vertices are described in [1].

V−1 = V0 + p˙ s + V−2 = V0 −

1 p¨ s 3

1 p¨ s 6

(19)

1 p¨ s . 3 If Vm is the last vertex, three more vertices are defined: Vm+1 , Vm+2 and Vm+3 . The values are determined by an equation similar to (19). In this paper the value p¨ s = 0 has been used. There are other techniques that can be used to accomplish starting and ending specifications (like the so-called multiple vertices technique, for instance) [1]. A combination of such methods and the one presented in this paper can also be applied in order to achieve better results. A β-spline and the control vertices for a parallel parking manoeuvre are shown in Fig. 13(a). On the other hand, Fig. 13(b) shows the original discontinuous steering angle vs. the continuous steering angle resulting when following the β-spline. V−3 = V0 − p˙ s +

3.7. Steering velocity constraint Although β-spline curves are continuous curvature paths, a change in curvature should be smooth enough for a

366

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

successful path tracking. It can be experimentally illustrated that changes in curvature along β-spline curves for parallel parking manoeuvres (see Fig. 14) can be approximated by the linear relations [8,18] dρ s1 |ρ1 | < m 1 = k1 · ds s0 3δ s3 dρ |ρ1 | + |ρ2 | < m 2 = k2 · (20) ds s2 3δ dρ s5 |ρ2 | < m 3 = k3 · . ds s4 3δ Then, if v (v = ds/dt) represents the vehicle linear velocity, the curvature change velocity along the spline will accomplish ρ˙ |tt10 < m 1 · v t

ρ˙ |t32 < m 2 · v

(21)

t

ρ˙ |t54 < m 2 · v where [t0 , t1 ], [t2 , t3 ], [t4 , t5 ] represent the period of time when the curvature is changing. On the other hand, from (2) the curvature change velocity due to the steering system is ρ(φ) ˙ =

1 + tan φ 2 l

˙ φ.

(22)

If φ˙ max is the maximum angular velocity of the steering system, each value of φ presents a maximum value for the curvature change velocity ρ˙max (φ). According to (22) this value is lower bounded as φ˙ max . |ρ˙max (φ)| ≥ ρ˙m = (23) l Therefore, the vehicle could follow the β-spline if the curvature velocity change accomplishes |ρ| ˙ ≤ ρ˙m along the curve (notice that it represents a sufficient condition). Thus, from (21) and (23) the steering mechanism can properly follow the evolution of the curvature if δ f accomplishes the following constraints k1 · |ρ1 |v · l 3φ˙ max k2 · (|ρ1 | + |ρ2 |)v · l δ f ≥ δ6 = 3φ˙ max k3 · |ρ2 |v · l δ f ≥ δ7 = 3φ˙ max δ f ≥ δ5 =

(24)

where the values of the constants (k1 = 6.6, k2 = 3.2 and k3 = 6.6) have been experimentally established to improve practical performance. Fig. 15(a) shows a manoeuvre violating condition (24) (solid line is the path reference and dotted line is the path depicted by the vehicle); it presents a bad performance in the path following since the vehicle is not able to follow the change of the steering angle (see Fig. 15(b), where the solid and dotted lines are the reference and the actual vehicle steering angle, respectively). In Fig. 15(c) a correct path following is performed by maintaining

Fig. 14. Spline curvature.

the value of δ f and decreasing the value of v (i.e. decreasing the values of δ5 , δ6 , δ7 ). On the other hand, Fig. 15(e) shows a correct performance achieved by maintaining the value of v and increasing the value of δ f until it accomplishes (24). 3.8. Selection of δ When the β-spline is generated, a single value of δ should be used. Thus, from (15)–(18) and (24) the value of δ has to accomplish max(δ5 , δ6 , δ7 ) = δvel ≤ δ f ≤ δcol = min(δ1 , δ2 , δ3 , δ4 ). (25) For practical reasons the value δvel is chosen. Further evaluation assures that this selection accomplishes the constraint δ f ≤ δcol . Taking into account that (δ1 , δ2 , δ3 , δ4 ) are mainly related to geometrical constraints while, from (24), (δ5 , δ6 , δ7 ) depend on the vehicle velocity and the velocity of the steering system, it could be possible to have δvel > δcol . In other words, it could be possible to compute a feasible continuous curvature manoeuvre that is not collision free. In this case, in order to obtain a collision-free manoeuvre the value of δ should be chosen as δ f = δcol . Nevertheless, according to (24), if this criterion is applied, the original velocity value must be changed, due to the fact that, at this velocity, the steering system cannot perform properly the change in curvature. Therefore, in this situation, the value of v should be decreased in order to make it possible that the vehicle follows correctly the spline. Thus, from (24), the expressions which provide the upper bounded values of v are: 3 · φ˙ max · δ f k1 · |ρ1 | · l 3 · φ˙ max · δ f v ≤ v2 = k2 · (|ρ1 | + |ρ2 |) · l 3 · φ˙ max · δ f v ≤ v3 = . k3 · |ρ2 | · l v ≤ v1 =

(26)

Then, vmax = min(v1 , v2 , v3 ) is chosen as the new value of v. The next algorithm has been implemented in order to tune the right value of v allowing the vehicle to perform a correct

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

367

Fig. 15. (a), (b) Bad manoeuvre performance; (c), (d) decreasing velocity; (e), (f) increasing δ.

path following: Parking place detection(C x , C y , ∆ y , H ) Selection of L p and stop the vehicle (Forward displacement) Discontinuous curvature collision-free manoeuvre generation (R1 and R2 ) v = vini Determine δ6 , δ7 , δ8 : Eq. (24) δ f = δvel = max(δ5 , δ6 , δ7 ) continuous manoeuvre = false Determine δ1 , δ2 , δ3 , δ4 : Eqs. (15)–(18) IF δ f ≤ δcol = min(δ1 , δ2 , δ3 , δ4 ) THEN continuous manoeuvre = true ELSE δ f = δcol v = vmax = min(v1 , v2 , v3 ) IF v > vthr THEN continuous manoeuvre = true END

IF continuous manoeuvre = true THEN generate β-spline follow β-spline ELSE follow a discontinuous curvature manoeuvre. END

The calculus of v may provide too low values for the vehicle velocity. However, the velocity controller of the robots usually cannot assure enough accuracy in practice if the reference velocity is very low, due to terrain changes, friction, etc. For this propose, a threshold value vthr has been established so that the continuous curvature manoeuvre can be only performed if v > vthr . If v is bellow this value, the vehicle has to perform a discontinuous curvature manoeuvre. In this case, the manoeuvre to follow will be the original one but the vehicle should stop in order to change the steering angle. A specific technique for this strategy is presented in [10]. The threshold

368

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 16. ROMEO-3R performing an autonomous parallel parking.

Fig. 17. ROMEO-3R parking manoeuvre experiments.

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

369

Fig. 18. ROMEO-4R performing an autonomous parallel parking.

should be set according to the vehicle characteristic. In the case of the experimental results shown in the next section vthr was set to 0.08 m/s and 0.1 m/s for ROMEO-3R and ROMEO-4R vehicles, respectively. The spline computation depends on the size and topology of the parking place. It is obtained while the vehicle is reactively navigating looking for the parking place [9]. As is shown in the experiments, the path-tracking algorithm can successfully solve the possible deflection in the following of the original path. However, in order to present a robust behaviour, and due to the simplicity of the path generation, the spline could be re-computed in each control iteration considering the actual vehicle position as one of the extremes of the spline and by discarding the control vertex associated with the previous followed sections of the spline. This strategy is similar to the approach presented in [7]. 4. Experimental results The method presented in the above sections has been implemented in the autonomous vehicles ROMEO-3R and ROMEO-4R (see Figs. 16 and 18). These vehicles have

been designed and built from the adaptation of conventional electrical vehicles at the University of Seville for the experimentation of intelligent components and autonomous navigation strategies [24]. They carry several sensors including cameras for teleoperation and navigation, GPS, laser and ultrasonic sensors, but just a minimal configuration is required to perform the experiments presented here (a couple of sensors on each side of the vehicle). Their perception capabilities have been described in [2,5,23]. Videos of the ROMEO vehicles performing autonomous parking can be watched at http://www.esi2.us.es/˜fcuesta/videos/parking.htm. On the one hand, ROMEO-3R is the result of the adaptation of a conventional three-wheel vehicle. The vehicle’s length and width are 1.34 m and 0.7 m, respectively, and the value of l (see Fig. 3) is 0.94 m. In the experiments presented in this paper the maximum steering angle was 0.95 rad which represents a maximum curvature of 1.4 m−1 . The maximum angular velocity of the steering system is 0.38 rad/s, vini is equal to 0.2 m/s and vthr was set to 0.08 m/s. On the other hand, ROMEO-4R is the result of the adaptation of a four-wheel vehicle with Ackerman steering. Vehicle’s

370

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 19. ROMEO-4R parking manoeuvres experiments.

width and length are 1.20 m and 2.1 m, respectively, and the value of l is 1.64 m. The maximum steering angle is 0.58 rad. which represents a maximum curvature of 0.4 m−1 . The maximum angular velocity of the steering system is 0.38 rad/s, vini is equal to 0.3 m/s and vthr was set to 0.1 m/s. The autonomous parking has been implemented in the frame of a behaviour-based control architecture, combining planning and reactivity [5,2,3]. Pure Pursuit, Generalized Predictive Path Tracking, and Fuzzy Path Tracking, among others, have been implemented [22]. Fig. 17 shows scenes and data from two parallel parking experiments with the ROMEO-3R vehicle. For each one, the manoeuvre in cartesian space and the rear point curvature vs. the path length are represented. In the parking manoeuvre shown in Fig. 17(a), a correct path following is performed; observe that δ f accomplishes the constraints defined by δcol . However, in Fig. 17(b) the effect of not accomplishing the maximum velocity constraint (26) (i.e. v > vmax ) is shown. In this manoeuvre δcol < δvel (with δvel calculated for v = 0.2 m/s). According to Section 3.8 the value of δ f was then chosen as δ f = δcol , but the original value of v was held (i.e. v = 0.2 m/s). As a consequence of

not reducing the velocity, the steering system cannot follow appropriately the curvature change. Notice that a feasible manoeuvre would be obtained with δ f = 0.59 m and v = 0.123 m/s. Two examples of ROMEO-4R parking manoeuvres are shown in Fig. 19. In this figure the manoeuvre in cartesian space and the rear point curvature vs. the time are represented for both manoeuvres. On the one hand, in Fig. 19(a) a continuous curvature manoeuvre is shown. In this experiment, again δcol < δvel (with δvel calculated for v = 0.3 m/s), but this time according to Section 3.8 the value of δ f was chosen as δ f = δcol , and the value of v was decreased (v = vmax = 0.25 m/s). Therefore, the manoeuvre was successfully executed. On the other hand, in the parking manoeuvre presented in Fig. 19(b), the continuous path was discarded since vmax does not accomplish the constraint vmax > vthr . Therefore, a discontinuous curvature path is followed. This figure illustrates that the steering angle presents a constant value during the navigation along each arc of a circumference. The steering system changes the steering angle once the vehicle stops at the point where the curvature of the path is discontinuous.

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

Fig. 18 presents snapshots of ROMEO-4R during this last experiment. 5. Conclusions In this paper a method to generate continuous curvature manoeuvres for nonholonomic wheeled vehicles has been presented. The method is based on computing a β-spline curve from an original discontinuous manoeuvre, providing a collision-free smoother path. Several constraints have been considered to assure the existence of collision-free admissible curves. This technique has been implemented in the frame of a fuzzy behaviour-based control architecture combining planning and reactivity. Moreover fuzzy logic has been used in order to select a manoeuvre from the solution set according to the environment. The efficiency and low hardware requirements of the proposed method have been demonstrated using the nonholonomic autonomous vehicles ROMEO-3R and ROMEO-4R. Acknowledgement This work has been supported in part by the Spanish Direcci´on General de Investigaci´on under project DPI200502293. References [1] B. Barsky, Computer Graphics and Geometric Modelling using BetaSplines, Springer-Verlag, 1987. [2] F. Cuesta, F. G´omez-Bravo, A Ollero, A combined planned/reactive system for motion control of manoeuvres, in: IFAC IMC98, Grenoble, 1998. [3] F. Cuesta, A. Ollero, B.C. Arrue, R. Braunstingl, Intelligent control of nonholonomic mobile robots with fuzzy perception, Fuzzy Sets and Systems 134 (2003) 47–64. [4] F. Cuesta, F. G´omez-Bravo, A. Ollero, Parking manoeuvres of industriallike electrical vehicles with and without trailer, IEEE Transactions on Industrial Electronics: Special Session on Automotive Electronic Systems 51 (2) (2004) 257–269. [5] F. Cuesta, A. Ollero, Intelligent mobile robot navigation, in: STAR: Springer Tracks in Advanced Robotics, vol. 16, Springer-Verlag, 2005. [6] L.E. Dubins, On curves of minimal length with a constraint on average curvature and with prescribed initial and terminal position and tangents, Amer. J. Math. 79 (1957) 497–516. [7] M. Florencio, P Agostinho, J.S. Sequeira, Automatic Parallel Parking of a Car-like Robots, in: 37th International Symposium on Robotics, ISR 2006, Germany, May 2006. [8] F. G´omez-Bravo, Planificaci´on de maniobras en sistemas rob´oticos no hol´onomos. Aplicaciones en robots m´oviles, Ph.D. Thesis, Univ. of Seville, 2001. [9] F. G´omez-Bravo, F. Cuesta, A. Ollero, Parallel and diagonal parking in nonholonomic autonomous vehicles, Engineering Applications of Artificial Intelligence 14 (1) (2001) 419–434. [10] F. G´omez Bravo, F. Cuesta, A. Ollero, A new sequential hybrid control structure for car and tractor-trailer parallel parking, in: 7th International IFAC Symposium on Robot Control, SYROCO, 2003. [11] F. G´omez Bravo, F. Cuesta, A. Ollero, Application of intelligent control to autonomous search of parking place and parking of vehicles, in: A. Ruano (Ed.), Intelligent Control Systems using Computational Intelligence Techniques, in: IEE Control Series, vol. 70, 2005.

371

[12] K. Jiang, D. Sereviratne, A sensor guided autonomous parking system for nonholonomic mobile robots, in: 1999 IEEE Int. Conf. on Robotics and Autom., 1999, pp. 311–316. [13] F. Lamiraux, J.-P. Laumond, Smooth motion planning for car-like vehicles, IEEE Transactions on Robotics and Automation 17 (4) (2001) 498–502. [14] J.C. Latombe, Robot Motion Planning, Kluwer Academic Publishers, 1991, pp. 424–425. [15] J.P. Laumont, P.E. Jacobs, M. Taix, M. Murray, A motion planner for nonholonomic mobile robots, IEEE Transactions on Robotics and Automation 10 (5) (1994) 577–593. [16] R. Mukherjee, A. Anderson, A surface integral approach to the motion planning of nonholonomic systems, Journal of Dynamic Systems, Measurement and Control (1993) 315–325. [17] V. Mu˜noz, J. Mart´ınez, A. Ollero, New continuous Curvature Local Path Generators for mobile Robot, in: SICICA’92, 1992, pp. 551–556. [18] V.F. Mu˜noz, Planificaci´on de Trayectorias para Robots M´oviles. Ph.D. Thesis. Dept. Ing. Sistemas y Autom´atica, Univ. M´alaga, 1995. [19] V. Mu˜noz, A. Ollero, M. Prado, A. Sim´on, Mobile robot trajectory planning with dynamic and kinematic constraints, in: Proc. IEEE Int. Conf. on Robotics and Autom., vol. 4, 1994, pp. 2802–2807. [20] R. Murray, S.S. Sastry, Steering Nonholonomic system using sinusoids, in: Proc. of the IEEE Int. Conf. on Decision and Control, 1990, pp. 1136–1141. [21] W. Neff Patten, H. Wu, W. Cai, Perfect parallel parking via pointryagin’s principle, Journal of Guidance, Control and Dynamics 116 (1994) 723–729. [22] A. Ollero, A. Garc´ı-a-Cerezo, J.L. Martinez, Fuzzy supervisory path tracking of mobile robots, Control Engineering Practice 2 (2) (1994) 313–319. [23] A. Ollero, F. Cuesta, R. Braunstingl, B.C. Arrue, F. G´omez-Bravo, Perception for autonomous vehicles based on proximity sensors. Application to Autonomous Parking, in: 14th IFAC World Congress 1999, vol. B, B-1e-02-4, 1999, pp. 451–456. [24] A. Ollero, B.C. Arrue, J. Ferruz, G. Heredia, F. Cuesta, F. L´opez-Pichaco, C. Nogales, Control Engineering Practice 07 (10) (1999) 1291–1299. [25] I. Paromtchik, C. Laugier, Motion Generation and Control for parking an autonomous vehicle, in: Proc. 1996 IEEE Int. Conf. on Rob. and Autom. 1996, pp. 3117–3122. [26] I. Paromtchik, C. Laugier, S.V. Gusev, S. Sekhavat., Motion Control for autonomous an autonomous vehicle, in: Proc. of the Int. Conf. on Control, Automation, Robotics and Vision, vol. 1, December, 1998, pp. 136–140. [27] I. Paromtchik, Planning control commands to assist in car maneuvers, in: Proceedings of the 11th IEEE International Conference on Advanced Robotics, Coimbra, Portugal, June 30–July 3, 2003, pp. 1308–1313. [28] I. Paromtchik, Steering and velocity commands for parking assistance, in: Proceedings of the 10th IASTED International Conference on Robotics and Applications, Honolulu, HI, USA, August 23–25, 2004, pp. 178–183. [29] J.A. Reeds, R.A. Shepp, Optimal paths for a car that goes both forward and backward, Pacific Journal of Mathematics 145 (2) (1990) 367–393. [30] A. Scheuer, Th. Fraichard, Continuous-curvature path planning for carlike vehicles, in: IROS 97, 1997, pp. 997–1003. [31] S. Sekhavat, Planification de mouvements sans collision pour syst`emes non holonomes. Ph.D. Thesis, LAAS-CNRS, 1997. [32] S. Sekhavat, F. Lamiraux, J.P. Laumont, G. Bauzil, A. Ferrant, Motion planning and control for Hilare pulling a trailer: Experimental issues, in: Proc. of the 1997 IEEE Int. Conf. on Robotics and Autom., 1997, pp. 3306–3311. [33] C. Shih-Jie Chang, L. Tzuu-Hseng, Design and implementation of fuzzy parallel-parking control for a car-type mobile robot, Journal of Intelligent and Robotic Systems vol 34 (2) (2004) 175–194. [34] L. Tzuu-Hseng, C. Shih-Jie, Autonomous fuzzy parking of a car-like mobile robot, IEEE Transactions on Systems, Man, and Cybernetics 33 (4) (2003) 451–465. [35] M. Wada, K.S. Yoon, H. Hashimoto, Development of advanced parking assistance system, IEEE Transactions Industrial Electronics 50 (2003) 4–17.

372

F. G´omez-Bravo et al. / Robotics and Autonomous Systems 56 (2008) 360–372

F. G´omez-Bravo received the M.S. degree in Industrial Physics in the National University (UNED), Madrid, Spain in 1991, and the Ph.D. degree from the University of Sevilla, Sevilla, Spain, in 2001. From 1984 to 2001, he was with the National Railway Company. Since 2001 he is in the Departamento de Ingenier´ıa Electr´onica, Sistemas Inform´aticos y Autom´atica, Escuela Polit´ecnica Superior, Universidad de Huelva, Spain. In his research experience he has been involved in several research and development projects and also in stays as researcher in the Institute for System and Robotics at the Instituto Superior Tecnico in Lisbon, Portugal, and in the Departament of Mechanics of the Technical University of Graz, Austria. He is the author or coauthor of more than 30 publications, including book chapters and papers published in journals and conference proceedings. His current research interests include control of nonholonomic systems, intelligent control, mobile robots and manipulators. F. Cuesta Ph.D. in Computer Science (2000) with honors from the Univ. of Seville where he is Associate Professor at the Department of Automatic Control, since 2003. He received the Excellence Doctorate Award 1999/2000 from the Univ. of Seville, and was one of five persons nominated for the First EURON Georges Giralt Ph.D. Award devoted to the best Ph.D. thesis in Robotics in Europe. His research experience includes participation in several European Projects and stays as a Researcher in the Technical University of Graz, Lund Institute of Technology, and the Delft University of Technology. He is the author or coauthor of more than 50 publications, including the book (Intelligent Mobile Robot Navigation, Springer-Verlag). His current research interests include fuzzy logic, stability analysis, nonlinear control, mobile robots, unmanned aerial vehicles, and teleoperation tools. A. Ollero (http://grvc.us.es/aollero) received his Electrical Engineering degree (1976) and the Doctor Engineer degree (1980) with doctoral award from the University of Seville. He worked in an engineering office and was assistant professor at the University of Seville (1976–1980). Later he was full professor at the Spanish Universities of Santiago de Compostela in Vigo (Head of Department and Vice-Director of the Engineering School), M´alaga (Head of two Departments and Di-

rector of the Engineering School). He has also been “Stagiaire” at the Laboratoire d’Automatique et d’Analyse des Systemes (LAAS-CNRS), Toulouse, France (1979), and visiting scientist (1990–1991) at the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA. Since December 1992 he is Professor at the University of Seville where he has been Vice-Director of the Engineering School. He is currently leading a Research and Development Group http://grvc.us.es with more than 30 members. He has R&D activities mainly in robotics, control, and autonomous systems in general. In the last years he is leading European and Spanish projects on unmanned aerial and ground vehicles http://grvc.us.es/aollero/project/UAVs en.html, coordination of multiple autonomous systems, environment perception, and distributed systems. Currently he is coordinator of the European FP6 IST Project AWARE on the integration of Unmanned Aerial Vehicles and the Wireless Sensor and actuator Networks. He developed prototypes, industrial products and control/automation applications for aeronautics, space, agriculture, forestry, aquaculture, forest fire fighting, optical tracking systems, wind turbine generator control and others. He participated or leaded 91 R&D projects including 14 projects funded by the European Commission. He is the author of two books on computer control (“Premio Mundo Electr´onico” Spanish award) and robotics, co-author of the book “Intelligent Mobile Robot Navigation” (Springer Star), editor/co-editor of eight books, including the recent Multiple Heterogeneous Unmanned Aerial Vehicles (Springer Star, 2007), and author/co-author of about 350 papers in journals, book chapters, and conference Proceedings. He is also co-author of three patents. Professor Ollero is Vice-Chair of the Technical Board of IFAC and has been Chair of the Coordinating Committee of “Mechatronics, Robotics and Components” (02–05), Chair of the Coordinating Committee on “Manufacturing and Instrumentation” (99–02), Chair of the Technical Committee on “Components and Instruments” (93–99), and Chair of the Working Group on “Intelligent Components and Instruments” (91–93). Professor Ollero is the recipient of eight national and international awards. A. Viguria received the “Engineering” degree in telecommunication from the University of Seville, Spain, in 2004. From 2004 to 2006 he was working as a researcher at the University of Seville. Since 2006, he is a Fulbright scholar and a graduate student at the Georgia Institute of Technology. His main research interests are in multi-robot coordination and cooperation and robot architectures.