A Variable Look-Ahead Controller for Lateral ... - Semantic Scholar

A Variable Look-Ahead Controller for Lateral Guidance of Four Wheeled Vehicles1 Pushkar Hingwe

Department of Mechanical Engineering University of California, Berkeley, CA [email protected] and

Masayoshi Tomizuka

Department of Mechanical Engineering University of California, Berkeley, CA [email protected]

Abstract A variable look-ahead controller for the lateral guidance of vehicles for Automated Highway Systems is proposed. The control objective is to make the lateral error at a certain point ahead of the vehicle zero. The distance of this point from the vehicle is called the look-ahead distance. An input-output linearization controller to achieve this objective is proposed. It is shown that the yaw internal dynamics can be damped at all longitudinal velocities if the look-ahead distance is a certain quadratic function of the longitudinal velocity. 1

This work was supported by California PATH (Partners for Advanced Transit and Highways) Program.

1

1 Introduction This paper presents a lateral controller for four wheeled vehicles in the context of Automated Vehicle Systems (AHS). The earliest research in the automated lateral control of vehicles took place in early 1960's and 1970's (Gardels, 1960, Fenton, 1976). The classical control tools were used for the lateral guidance. Nevertheless, the recent revival of interest in AHS has given a further boost to research in automatic vehicle control (Varaiya, 1993). Modern and nonlinear control techniques have been applied to the problem of lateral guidance for AHS. Peng and Tomizuka (1992), applied Frequency Shaped Linear Quadratic (FSLQ) technique with preview control and demonstrated its eectiveness in achieving comfort and tracking. Ackermann et al. (1990) applied a yaw rate based control to achieve robust decoupling of the lateral and the yaw dynamics. Sliding Mode Control (SMC) has been applied in various forms by researchers to achieve robustness to parametric uncertainties (Guldner et al., 1995, Pham Hedrick and Tomizuka, 1994, Hingwe and Tomizuka, 1995 and Saraf and Tomizuka, 1997). Most of the research in lateral control has focused on the look-down approach. In a look-down approach, the lateral distance of the a point on the vehicle body from the road centerline is available for feedback. Recently Patwardhan et al. (1997) presented a study of the fundamental diculties in lateral control of vehicles with a look down approach. They have shown that if look-ahead information, which refers to the lateral error at distance ahead of the vehicle, is available for the feedback, the control task becomes easier. In this paper, an input-output linearization control (from the steering angle input to the lateral error output) is studied from the point of view of variable look-ahead lateral error measurement. It was shown in Hingwe and Tomizuka (1997), that with a look-down lateral error measurement, input-output linearization (which is also the rst step towards the SMC based designs) results in poor yaw damping. In this paper, a strategy to vary the look-ahead distance depending on the longitudinal velocity is proposed. It is shown that this strategy places the closed loop poles on the negative real axis for both the lateral and the yaw dynamics. Another way of looking

2

Velocity

R

Direction of the road radius .

.

y

l2

l1

dv δ

. y ε

yv

x . εd

yo yr

Y

ε

Vehicle orientation

.

yr

ya

x

xr

Road orientation

Vx εd

Lane centerline

X

Figure 1: The description of the states, input and the output at this controller is from the point of view of output rede nition. The yaw internal dynamics are damped by rede ning the output (which depends on the look-ahead distance) as a function of velocity. This paper is organized as follows. Section 2 describes the dynamic model of the vehicle. In section 3, a linearizing feedback controller is proposed for this model. Proof of the closed loop stability is presented in section 4. Typical simulation results are presented in section 5. Conclusions are presented in section 6.

2 The Vehicle Model In this section, the dynamic equations and the parameters of the vehicle model are described. For simplicity only the lateral and the yaw dynamics are considered. This simpli cation captures the dynamics that would eect control design. We write the dynamical equations for the vehicle in the co-rotational coordinates x; y (see Figure 1). The Euler-Newton equations for the planar rigid body of Figure 1 are

3

my = ?m_x_ ? Cr (y_ ? _l2)=x_ ? Cf (y_ + _l1)=x_ + Cf  Iz  = l2Cr (y_ ? _l2 )=x_ ? l1Cf (y_ + _l1)=x_ + l1Cf 

(1)

yv = y a + y o

(2)

where y_ and x_ are the components of the vehicle velocity as shown in Figure 1. _ is the yaw rate. m and Iz are the mass and the yaw moment of inertia respectively. l1 and l2 are distances of front and rear axle from the center of gravity. Cf and Cf are the front and rear cornering stiness respectively.  is the steering angle. The model described by equation (1) is independent of road reference. To describe the vehicle relative to the road, a road reference frame (xr ; yr ) is used (see Figure 1). The rotation rate of this frame is de ned as _d . Note that _d = Vx, where Vx is the component of the vehicle velocity along the xr axis and  is the road curvature. The distance of the vehicle center of gravity from the origin of frame (xr ; yr ) is de ned as ycg . In this paper, the equations of motion are written for the lateral error, yv 2 . This is the lateral error at a distance dv ahead of the center of gravity (refer Figure 1). From Figure 1, we see that where ya is the actual lateral error at dv and yo is the oset. yo is signi cant for large dv even if radius of curvature is small. Assuming that the vehicle is on a curve of constant radius,

yo = R ? fR2 ? (dv cos r )2 g1=2

(3)

where R is the radius of curvature of the road in Figure 1 and r is the relative yaw between the road and the vehicle. For r  0, which is true most of the time, equation (3) can be approximated as

yo = R ? fR2 ? d2v g1=2

(4)

The subscript v denotes \virtual" or \vision based" depending on the road reference system and the lateral error sensing system used to obtain yv . 2

4

The control objective is to regulate ya to zero. It is, however, convenient to write the equations of motion in terms of yv . If the equations of motion are written in terms of yv , the tracking objective needs to be rede ned as regulating (yv ? yo ) to zero. Because, yo can be computed for a known R and dv , without loss of generality, the problem of regulating yv to zero is considered. The lateral error yv can be expressed as

yv = ycg + dv r

(5)

y_v = y_cg + dv _r + d_v r

(6)

Dierentiating equation (5), we get

Dierentiating equation (6), we get

yv = ycg + dv r + 2d_v _r + dv r

(7)

y_cg = y_ + x _r

(8)

From Kinematics, we get

Substituting equation (8) in equation (6), we get

y_v = y_ + x _ r + d_v r + dv _r

(9)

ycg = y + _x_ ? Vx2

(10)

Also, where  is the curvature. Substituting equation (10) in equation (7), we get

yv = y + _x_ ? Vx2 + dv r + 2d_v _r + dv r

(11)

In subsequent treatment, we will assume dv = 0. Substituting equation (1) in (11) and using equation (9), we get _ _ yv = ? (1 +x_ 2) y_v + (dv x+_ x_ ) (1 + 2)r + 1(dv ? l1) + 2x_(dv + l2) + 2dv x_ _r 2 + 2 l2 ? x_1 l1 ? x_ _d + 1 (12)

5

where 1 = Cf ( m1 + l1Idzv ), 2 = Cr ( m1 ? l2Idzv ). A compact notation for equation (12) is

yv = fv + bv 

(13)

where _ _ fv = ? (1 +x_ 2) y_s + (dv x+_ x_ ) (1 + 2)r + 1 (dv ? l1) + 2x_(dv + l2) + 2dsx_ _r 2 + 2 l2 ? x_1 l1 ? x_ _d and bv = 1 For d = 0, r is given by the second equation in (1): i.e. l C ?l C l C ?l C _ r = ? 1 fI x_ 2 r y_v + 1 f I 2 r (dv x+_ x_ ) r z z (Cf l12 + Cr l22) Cf (l12 ? l1dv ) + Cr (l22 + l2dv )  _ ? _d ? r Iz x_ Iz x_ C l + If 1  z

(14)

3 Input Output Linearization In consonance with the goal of regulating the lateral error yv to zero, the following feedback linearization is used

 = b1 (?fv ? 2yv ? 1y_v ) v

(15)

Substituting equation (15) in equations (12) and (14), we get,

yv = ?1 y_v ? 2 yv r = ?2 yv + ( x_ ? 1)y_v (l C ? l C _ ? ( + dv ? 1 f I 2 r ) dx_v )r z ( l + d ) ? ( 2 x_ v + 2d_v)_r

6

(16)

(17)

where and

1  = I +ml z m l 1 dv

(18)

1 + l2 )

= (CI r+(lml d)

(19)

z

1

v

Under the assumption that the longitudinal velocity, x_ , is constant and dv is constant, the zero dynamics is given by, (20) r = ? r ? (dv x+_ l2) _r + ( x_ 2 ? l2) The zero dynamics given by equation (20) are excited by the disturbance . The distance dv can be designed such that these dynamics are well damped. One way is to place both the characteristic roots of the zero dynamics equal and on the real line: i.e., equation (20) is written as r + 2p _r + r = ( x_ 2 ? l2)

Therefore, the following equation has to be satis ed (l2 + dv )2 = 4 x_ 2

The equation (22) is a quadratic in dv and can be written as " # 2 2 4 x _ ml 1 2 dv + dv 2l2 ? C (l + l ) + l22 ? C 4(x_l I+z l ) = 0 r 1 2 r 1 2 The solutions of this equation are (

2 4 2 2 2 dv = C 2x(_ l ml+1l ) ? l2  24x_ m l1 2 + 4x_C(Iz(l?+mll 1)l2 ) Cr (l1 + l2 ) r 1 2 r 1 2

(21) (22) (23) ) 12

(24)

1 l2 ) Note that the term 4x_C(Irz(?l1ml in equation (24) is small in comparison to the term +l2 ) 4 2 2 4x_ m l1 3 C2 (l1 +l2 )2 for high velocities . If this term is neglected, we get 2

r

dv = C 4x(_ l ml+1l ) ? l2 ; ?l2 2

r

3

1

2

Numerical calculations show that this is indeed a good assumptions for x_ > 10m=s

7

(25)

10

vel = 5 m/s vel = 10 m/s vel = 15 m/s vel = 20 m/s

5

vel = 30 m/s vel = 40 m/s

0

−5

−10 −25

−20

−15

−10

−5

0

Figure 2: Characteristic roots of the zero dynamics. This approximation results in two characteristic roots not numerically equal and on the real line. The negative solution does not make sense and will be discarded. Therefore the distance that is chosen for lateral error feedback is (

2 dv = max ds; C 4x(_ l ml+1l ) ? l2 r 1 2

)

(26)

where ds refers to the distance of the front bumper from the center of gravity. Because dv is a function of longitudinal velocity x_ , the assumptions that dv and x_ are constant are consistent. Figure 2 shows the location of the characteristic roots of equation (20) with dv as de ned in equation (26) for various values of the longitudinal velocity x_ . Note that the characteristic roots are well damped for all values of x_ in the operating range. In practice, the assumption that longitudinal velocity is constant is restrictive. The feedback given by the equation (15) may be acceptable if it is robust to violation of these assumptions. In the next section, the stability of the linearizing feedback in face of varying longitudinal velocity is examined.

8

2.5 2

vel = 5 m/s vel = 10 m/s

1.5

vel = 15 m/s vel = 20 m/s

1

vel = 30 m/s vel = 40 m/s

0.5 0 −0.5 −1 −1.5 −2 −2.5 −5

−4

−3

−2

−1

0

Figure 3: Portion of Figure 2 zoomed in.

4 Stability Analysis The dynamics of r (given by equation (17) and for dv set by equation (25)), can be written as 3 2 2  d 64 r 75 = 64 dt _r

0

1 ? ?4x _

3 32  76 r 7 5 54 _r

Let P be the solution of the following Lyapunov equation

AT P + P A = ?I where

2 A = 64

0

1 ? ?4x _

3 7 5

Solution P of equation (27) is given by 2 P = 64

x

(1+ ) 8_

+

1 2

9

x

2_

(27)

1 2



x

( +1) 8 _

3 7 5

Consider the following Lyapunov function candidate

V = xT P x

(28)

where x = [r _r ]T . V is positive de nite. To be more precise, for a given  > 0, such that x_ > , 9 > 0, such that xT P x > xT x . Dierentiating equation (28) along the zero dynamics, we get

V_ = xT (PA + AT P)x + xT P_ x Therefore,

V_ = x (?I + P_ )x P_ is given by P_

2     3 _ d 1 d (1+ ) + 2x _ dt 2  7   = 64 dt 8dx 5 1 d ( +1) dt 2 dt 8 x _

From the de nitions of and  (equations (19) and (18)), we get, d  (1 + )  = xml _ 1d_v ? x(Iz + ml1dv ) ? xCr (l1 + l2) dt 8x _  8ml1x_ 2 8ml1x_ 2 _ = 2xml1 d 2x dt Cr (l1 + l2 )   d ( + 1) = 2x_ (Iz + ml1dv )ml1d_v ? x(Iz + ml1dv )2 dt 8 x_  8Cr (l1 + l2)ml1x_ 2 _ 1d_v ? x(Iz + ml1dv ) + xml 8ml1x_ 2   d 1 = ml1d_v dt 2 Cr (l1 + l2 )

10

(29)

The bounds on the entries of equation (29) are,     d (1 +

) 2 x _ d 2ml1 jxj + 1 j d_v j j dt 8x + j  _ dt Cr (l1 + l2) max 8 x_ max l2 ) + ml1dvmax j x j (30) + Iz + Cr (l1 + 8ml1 x_ 2 max     _ d I + ml (

+ 1) 1 dv j z 1 dvmax j dt 8 x + j  j _ 8 4Cr (l1 + l2) x_ max ml1dv ) 1 + (Iz + ml1dv )  j x j + (Iz +8ml Cr (l1 + l2) x_ 2 max 1 



j dtd 21 j  C (ml jd_v j r l + l ) max 1

1

2

(31) (32)

These bounds and the Theorems on perturbation theory of matrices will be used to analyze V_ . Theorem 1 (Bauer-Fike) (Demmel, 1997) Let A be symmetric. Then the eigenvalues i of A + E lie in the union of intervals [i ? jjE jj2 ; i + jjE jj2 ] centered at the eigenvalues i of A. In the present case E = P_ . Because P_ is symmetric, jjP_ jj2 = maxi ji(P_ )j. In order to verify that P_ ? I has negative eigenvalues, the following equation has to be satis ed max j (P_ )j < 1 i=1;2 i Because P_ 2 R2, the formula for the largest eigenvalue can be explicitly written. Let

P_

2 = 64

1 3 3 2

3 7 5

where i correspond to the matching entries in equation (29). It can be veri ed that the maximum eigenvalue of P_ is given by q

max = ( 1 + 2 2)max + 12 ( 1 ? 2)2max + 4 32max

11

Parameter symbol m Iz l 1

l

2

Cf Cr

Description Mass of the sprung mass Yaw moment of inertia Distance between the front wheel and the center of gravity of the sprung mass Distance between the rear wheel and the center of gravity of the sprung mass Cornering stiness of the front tires Cornering stiness of the rear tires

Value 1485 kg 2872 kg=m

2

1:1 m 1:58 m 80; 000 N=rad 80; 000 N=rad

Table 1: The geometric and inertial parameters of the complex model. After some algebraic manipulation and using equations (30)-(32), it can be shown that, 1 j d_v j 1 ( 1 + 2 )max = C 2(ml j x  j + max 8 x_ max r l1 + l2) I + C ( l + l ) + ml d 1 vmax x j x_2 jmax + z r 1 8ml2 1  1 + Iz + ml1dvmax  j d_v j 8 4Cr (l1 + l2) x_ max ml1dv ) 1 + (Iz + ml1dv )  j x j + (Iz +8ml Cr (l1 + l2) x_ 2 max 1 (Iz + ml1dvmax ) j d_v j 1 ( 1 ? 2 )max = C 2(ml j x  j + max 4Cr (l1 + l2) x_ max r l1 + l2)   ( I + ml ( I + ml z 1 dv ) z 1 dv ) 1 + C (l + l ) j x_x2 j + 8ml 1

1 3max = C (ml jd_v j r l1 + l2) max

r

1

2

max

For the parameters values of Pontiac 6000 given in the Table 4 and assuming the following bounds,

 Bound on acceleration/deceleration: jxj < 3:5m=s ,  Bound on the longitudinal velocity: x_ 2 [10; 37]m=s, 2

12

 Bound on dv : dvmax = 40m, numerical calculations show that

jjP_ jj < 0:477 2

Therefore,

V_ < 0 Another easy method for nding the range of eigenvalues of ?I + P_ is to use Gershgorin's Theorem (Demmel, 1997).

Theorem 2 (Gershgorin) For any Matrix A 2 Rnn and Aij = bij ; bij 2 R, The P eigenvalues of A lie in n discs centered at ci = bii and with radius ri = nj6 i jbij j. =

In the present case A = ?I ? P_ and therefore,

b11 = max ?1 x;_ x 1 b22 = max ?1 x;_ x 2 and r1 = r2 = max x;_ x 3 From equations (30)-(32), we get _ 1 j x jmax + 1 j dv j b11 = C 2(ml 8 x_ max r l1 + l2)  j ?1 I + C ( l + l ) + ml d 1 vmax x j + z r 1 2 8ml1 x_ 2 max  _  dv j 1 dvmax j b22 = 18 + I4zC+ ml x_ max r (l1 + l2)   ( I + ml d ) ( I + + z 8ml 1 v 1 + Cz (lml+1dl v)) j x_x2 j 1

1 jd_vj r1 ; r2 = C (ml r l1 + l2) max

r

1

2

max

?1

Using aforementioned bounds on the acceleration and longitudinal velocity and using the parameter values given in Table 4, we get, b11 = ?0:4502; b22 = ?0:8133 and r1 = r2 = 0:0395.

13

It is clear that ?I + P_ has both eigenvalues negative. Therefore

V_  < 0

(33)

Similarly, it can be shown that for the Lyapunov function candidate

Vy = xTy Py xy where

2 2 2 (2 +1 +2 ) 6 Py = 4 211 2 22

(34)

1

22  

( 2 +1) 2 1 2

3 7 5

(35)

and xy = [ys y_s ]2 ,

V_ y = ?xTy xy

(36)

The stability of the combined system given by equations (16) and (17) can be analyzed by considering the Lyapunov function candidate

V

2 V = xT 64 y

0

0 V

3 7x 5

where x = [yv y_v r _r ]. Using equations (33) and (36), it can be shown that

V_ < 0 This proves the stability of the close loop for varying x_ .

5 Simulations Simulations were performed for the following scenario. The road was straight for two seconds (_d = 0). At two seconds, the road had a right curve of radius 400 m. The initial lateral and orientation errors were zero. The initial lateral velocity and the yaw rate were also zero. Simulations were performed for various longitudinal velocities.

14

ds ε r, rad

Two controllers were simulated for comparison. Figure 4 shows the simulation results of the input-output linearization controller for a xed value of dv , 1.96 m. This value of dv corresponds to the distance of the front bumper from the center of gravity of the vehicle. It can be seen from Figure 4 that for high longitudinal velocities, the yaw dynamics are underdamped. Figure 5 presents the closed loop simulation with the controller proposed in this paper. The value of dv for each of the longitudinal velocities is shown in Figure 5. It is clear that the yaw dynamics are well damped for all a range of longitudinal velocities. However, the lateral tracking performance deteriorates. 0.04

20 m/s 10 m/s

0.02

30 m/s 37 m/s

steering angle, δ

tracking error, m

0 1 −3 1.5 x 10

2

2.5 time, sec

3

3.5

4

1

1.5

2

2.5 time, sec

3

3.5

4

1

1.5

2

2.5 time, sec

3

3.5

4

2 0 −2 1

0.5 0 −0.5

Figure 4: Tracking performance of the controller for dv = 1:96 m. The peak lateral tracking error of 4cm is larger than the peak lateral errors achieved by simple input-output linearization with dv = 1:96m (see Figure 4). However, this tracking error is acceptable.

15

ds ε r

0.02 0.01 0 1

2

0.04

3 4 time, sec

5

6

0.02 0 v = 30 m/d, dv = 18.2 m v = 37 m/s, dv = 22.8 m

−0.02 −0.04

steering angle, δ

tracking error, m

−0.01

1

2

3 4 time, sec

5

6

v = 20 m/s, dv = 11.6m v = 10 m/s, dv = 5.0 m

1 0.5 0

−0.5

1

2

3 4 time, sec

5

6

Figure 5: Tracking performance of the controller for dierent longitudinal velocities.

16

6 Conclusion A lateral controller was designed based on the input-output linearization. The control input to the system was the steering angle. This input was used to regulate the output, lateral error at a point ahead of the vehicle (called the look ahead distance). It was shown that one way to damp the yaw internal dynamics, is to choose the look ahead distance as an appropriate function of the longitudinal velocity. Currently, for the systems which have look down sensors on board, measurements from two such sensors, placed at each end of the vehicle, can be extrapolated to give the lateral error at the desired point ahead of the vehicle. While it is not considered in this paper, the measurement noise in ampli ed for increase in dv , which may set a practical limit in the selection of the look-ahead distance. Alternately, a vision based sensor can provide direct measurement of the lateral error at a desired point ahead of the vehicle.

References [1] Ackermann, J., 1990, \Robust Car Steering by Yaw Rate Control," Proceedings of the IEEE Conference on Decision and Control, pp 2033-2034. [2] Demmel, J., W., 1997, \Applied Numerical Linear Algebra", SIAM, Philadelphia, USA. [3] Fenton, R., Melocik, G. C. and Olson, K. W.,June 1976, \On Steering of Automated Vehicles: Theory and Experiment," IEEE Transactions on Automatic Control, vol. AC-21, no. 3 pp 306-315. [4] Gardels, K., 1960, \Automatic Car Controls for Automatic Highways," GMR, General Motors Corporation, Warren, Mi, Report, GMR-276. [5] Hingwe, P. and Tomizuka, M., 1995, \Two Alternative Approaches to the Design of Lateral Controllers for Commuter Buses based on Sliding Mode Control," ASME International Mechanical Engineering Congress and Exposition, DSC-vol.56/DEvol.86, pp. 99-104.

17

[6] Hingwe, P. and Tomizuka, M., 1997, \Experimental Evaluation of a Chatter Free Sliding Mode Control for Lateral Control in AHS", Proceedings of the American Control Conference, Albuquerque, New Mexico, pp. 3365-3369. [7] Patwardhan, S, Tan, H.S., Guldner, J., 1997 \A general framework for Automated Steering Control: System Analysis", Proceedings of the American Control Conference, Albuquerque, New Mexico, pp 1598-1602. [8] Peng, H. and Tomizuka, M., 1993, \Preview Control for Vehicle Lateral Guidance in Highway Automation," ASME Journal of Dynamic Systems, Measurement and Control, Vol. 115, No. 4, pp. 678-686. [9] Pham, H., Hedrick, K. and Tomizuka, M., 1994, \Combined Lateral and Longitudinal Control of Vehicles," Proceedings of the American Control Conference, Baltimore, Maryland, pp. 1205-1206. [10] Saraf, S., 1997, \Advanced Steering Control of Road Vehicles", Ph.D. Thesis, UCB, CA. [11] Varaiya, P., 1993, \Smart Cars on Smart Roads: Problems of Control," IEEE Transactions on Automatic Control, vol. 38, no. 2, pp 195-207.

18