Nonlinear H∞ Control of Autonomous Vehicles

Nonlinear H∞ Control of Autonomous Vehicles Katsumi Moriwaki ∗ Katsuyuki Tanaka ∗ ∗

The University of Shiga Prefecture, Hikone, Shiga, Japan (e-mail: { moriwaki, k-tanaka } @mech.usp.ac.jp).

Abstract: The control problem of automotive vehicles is considered. Steering a car by hand means that the driver plans a path by preview and controls the lateral deviation of the vehicle from the planned path by the steering wheel. In an automatic car steering system, this path following is automated. The deviation is kept small by feedback control via the steering motors. The reference trajectory may be calculated from the data of a CCD camera and the compensation scheme will be derived by the data of a gyro or a GPS. Riding comfort is also an important factor for autonomous vehicles with passengers. In order to study automatic car steering and maintaining the stability of the car, the steering model must be extended. For the extended nonlinear steering model with output equation whose elements can be measured by equipped sensors, the nonlinear state feedback H∞ controller is introduced so that the vehicle can tracks the reference path and reduces angular accelarations. Keywords: Autonomous vehicles, Motion control, Nonlinear state feedback control, H∞ optimal control, Navigation 1. INTRODUCTION The passenger vehicles are evaluated from many points of views, such as riding comfort, vehicle position, stability, manipulability and so on (Fig. 1). In the recent year, the electric control systems of chassis functions – suspension, steering, drivetrain and brake – have been developed rapidly. These systems have been developed for the purpose of obtaining the maximum performance independently Matsuo et al. (1990).

Fig. 1. Integrated vehicle control (Matsuo et al. (1990)) The performance of vehicle control in technically is seperated into several control items and considered to each item independently, too. The mathematical model for steering control of an autonomous vehicle has usually two degrees

of freedom, which consider the lateral motion and the yawing motion Ackermann et al. (1993); Moriwaki (2001). The model for suspension dynamics, which is deeply related to riding comfort, has also two degrees of freedom, which consider the bouncing motion and the pitching motion Abe (1992); Nagai et al. (1999); Moriwaki (2005). The above mentioned models are not enough to treat the problem of total motion control for autonomous vehicles. The specifications of tires must also be considered in the whole motion control of vehicles and they have strong nonlinearity Andrezejewski and Awrejcewicz (2005). There are, furthermore, mutual interactions among them, which are inevitably to be taken into account in the problem of the whole motion control for autonomous passenger vehicles (Fig. 2). Steering a car by hand means that the driver plans a path by preview and controls the lateral deviation of the vehicle from the planned path by the steering wheel. In an automatic steering system of autonomous vehicles, this path following is automated. The lateral deviation from the reference path is kept small by feedback control via the steering motors. The reference trajectory may be calculated from the data of a TV camera or a CCD camera. In order to study automation of car steering, the steering model must be extended. The extended model must include not only velocities, but also the vehicle heading and lateral position of the sensor with respect to the reference path. For simplicity this extended model will only be derived using the linearized model of a nonlinear model that is valid for small deviations from a stationary circular path. Although the dynamics of an automotive vehicles are designed to be stable in the usual situation, they are described by nonlinear equations. The conventional controller for automatic vehicle steering is obtained via the problem formulation using linearized steering models. The H∞ optimal controller using nonlinear state feedback

"

Z Yawing Moment

fx fy mz

#

" =

− sin δ 0 cos δ 1 lf cos δ −lr

#

ff fr

 (1)

Via the dynamics model the forces cause state variables β, V, r. The equations of motions for three degrees of freedom in the horizontal plane are (1) longitudinal motion −mV (β˙ + r) sin β + mV˙ cos β = fx (2) lateral motion mV (β˙ + r) cos β + mV˙ sin β = fy

X Rolling Moment

(2) (3)

(3) yaw motion I r˙ = mz

Pitching Moment

Y

Fig. 2. Rigid-body model of four wheel vehicles

It is obtaind that from (2) to (4)   " # #" fx − sin β cos β 0 mV (β˙ + r)   fy = cos β sin β 0 mV˙ 0 0 1 mz I r˙

(4)

(5)

The side force fy is known to be the nonlinear function of the tire sideslip angles βf , βr , i.e., fy = ff (βf ) + fr (βr ) (6) Two wheel model (5) and (6) is nonlinear and we will introduce the additional assumptions Ackermann et al. (1993) as follows. (A1) The sideslip angle β is assumed to be small. Then, (5) becomes   " # #" −β 1 0 fx mV (β˙ + r)  = 1 β 0 fy (7) mV˙ 0 0 1 m z I r˙ (A2) The velocity is constant, V˙ = 0. Then, the second row of (7) is eliminated.      fy mV (β˙ + r) = 1 0 (8) mz 0 1 I r˙ The velocity V is treated as an uncertain constant parameter.

Fig. 3. Two wheel model for car steering to the strict nonlinear steering models for automatic vehicle steering system is proposed and the validity of the proposed controller is discussed. 2. DYNAMIC MODEL OF AUTONOMOUS VEHICLES 2.1 Dynamic model of steering motion The features of car steering dynamics in a horizontal plane, which is referred as a four wheel model for car steering, are described in Abe (1992); Ackermann et al. (1993). It is well known that two wheel model Ellis (1969) can be regarded as the equivalent model to the four wheel model (Fig. 3) under appropriate assumptions. The side forces ff := 2Yf , fr := 2Yr are projected through the steering angle into chassis coodinate (x, y), where they appear as forces fx , fy and the torque mz around a z-axis which is pointing upward from the center of gravity (P).

(A3) The nonlinear characteristic of (6) is approximated by the nominal value of the tangent at βf = βr = 0 and small nonlinear functions, i.e., ff (βf ) = cf µ(βf + ∆1 (β, r)) (9) fr (βr ) = cr µ(βr + ∆2 (β, r)) where ∆i (β, r), (i = 1, 2) is C ∞ nonlinear function and ||∆i (β, r)||  1 van der Schaft (1992). The constant coefficients cf , cr are called ”cornering stiffness”, and µ is the adhesion factor (treated here as a disturbance) between road surface and tire. Typical experimental values of µ Ackermann et al. (1993) are µ= 1 dry road µ = 0.5 wet road µ = 0.15 ice. The steering model follows from (7) to (9) and using (1) as     mV (β˙ + r) = 1 1 lf −lr I r˙

 ×

cf µ(δ − β − lf r/V + ∆1 (β, r)) cr µ(−β + lr r/V + ∆2 (β, r))

 (10)

The uncertain parameters in this model are mass m, moment of inertia I, velocity V and road friction factor µ. Solving (10) for β˙ and r˙ and rearranging terms yields the nonlinear state space model          g1 (β, r) β b1 β˙ = a11 a12 δ+ µ , (11) + b2 g2 (β, r) a21 a22 r r˙ where cf + cr cr lr − cf lf , a12 = −1 + mV ˜ mV ˜ 2 cf lf2 + cr lr2 cr lr − cf lf , a22 = − a21 = ˜ I˜ IV cf cf lf b1 = , b2 = mV ˜ I˜ I m ˜ , I := m ˜ := µ µ a11 = −

and g1 (β, r) = cf ∆1 (β, r) + cr ∆2 (β, r) , g2 (β, r) = lf cf ∆1 (β, r) − lr cr ∆2 (β, r) 2.2 Suspension model of an autonomous vehicle The features of car vertical dynamics in (X-Z)-plane are described in Fig. 4 Abe (1992); Nagai et al. (1999); Andrezejewski and Awrejcewicz (2005) with active suspension system in which the external control forces is used to suppress the uncomfortable bounceing motion and pitching motion.

Fig. 4. Active suspension control (Nagai et al. (1999)) The equations of motions for two degrees of freedom in (X-Z)-plane with the constant velocity V are i. bouncing motion m¨ z = Ff + Fr

(12)

Iy θ¨ = −lf Ff + lr Fr

(13)

ii. pitching motion where z is the vertical displacement of the center of gravity (CG) of a car (Fig. 4), θ is the pitching angle of the center

of gravity (CG) of a car and the external forces acting on a front wheel and the rear wheel from the road surface are written by Ff and Fr , respectively. The state space model of the vertical dynamics is derived from Eqs.(12), (13) as follows Nagai et al. (1999).      z˙ z 0 0 1 0  θ˙   0 0 0 1   θ    = a ˜31 a ˜32 a ˜33 a ˜34   w  w˙ q a ˜41 a ˜42 a ˜43 a ˜44 q˙   0 0    0 0  Uf  +  ˜b31 ˜b32  Ur ˜b41 ˜b42   0 0    0 0  z0f   (14) +˜ ˜  z0r h31 h32 ˜ ˜ h41 h42 Using sensors for the height of CG of the car and the pitching angle of CG of the car, it can be assumed that the output equation for Eq. (14) is given by       z w 0 0 1 0 θ (15) = q 0 0 0 1 w q 3. OPTIMIZED SERVO-CONTROLLERS FOR AUTOMATIC VEHICLE STEERING 3.1 Extended state space model for the case : |V | > 1 , |V˙ | < 1 In order to consider the problem of automatic car steering, the extended model of vehicle is introduced. The extended model must include not only velocities, but also the vehicle heading and the lateral position of the sensor with respect to the reference path. This extended model is derived using a nonlinear model that is valid for deviations from a stationary path. It is assumed that the reference path is given as an arc with radius Rref and center M (See Fig. 5)Ackermann et al. (1993). For a straight path segment the radius is Rref = ∞. It is more convenient to introduce the curvature ρref := 1/Rref as input that the generates the reference path. The curvature is defined positive for left cornering and negative for right cornering. The radial line from the center M passing through the center of gravity (P) of the vehicle intersects a unique point zM on the desired path. It is assumed that there is a small deviation from the reference point zM to the center of gravity which is the deviation yp and that a vehicle fixed coordinate system (x, y) is rotated from the inertially fixed coordinate system (X, Y ) by the yaw angle ψ. The tangent to the reference path at zM is rotated by a reference yaw angle ψt with respect to X. Thus, the rate of change of yp is given by V sin(β + 4ψ) where β is the vehicle sideslip angle and 4ψ := ψ − ψt is the angle between the tangent to the reference path at zM and the center line of the vehicle. With the linearization sin(β + 4ψ) ≈ β + 4ψ the deviation yp changes according to y˙ p = V (β + 4ψ) (16)



P y z M M

reference path y S V

Y

X

Fig. 5. Scheme of automatic vehicle steering If the sensor S is mounted in a distance ls in front of the center of gravity with ls  Rref , the measured deviation ys from the reference path changes both with y˙ p and under ˙ Taking this into the influence of the yaw rate r = ψ. account, the rate of change of the measured deviation is y˙ s = V (β + 4ψ) + ls r (17) Determination of y˙ s requires knowledge of three variables β, r and 4ψ. The variables β and r are given by (11). The angle 4ψ will be obtained by 4ψ˙ = r − V ρref (18) Combining (11), (17) and (18), the extended state space model is obtained as Ackermann et al. (1993), Yoshikawa and Imura (1994)      β a11 a12 0 0 β˙  r˙   a21 a22 0 0   r     4ψ˙  =  0 1 0 0   4ψ  ys V ls V 0 y˙ s       g1 (β, r) 0 b1  g2 (β, r)   0  b  (19) ρ + + 2 δ +  µ 0 0 −V  ref  0 0 0 Using sensors for the yaw rate r (ex. a gyro) and the deviation ys (ex. a GPS), it can be assumed that the output equation for (19) is given by   β     4ψ 0010  r  (20) = ys 0 0 0 1  4ψ  ys The system (19), (20) with the control input δ, the reference input ρref , the disturbance input µ and the output 4ψ, ys is shown to be controllable and observable w.r.t. δ. 3.2 State space model for the case : |V | < 1 , |V˙ | < 1 In this case, the automatic vehicle steering can be described by (17) and (18)

4ψ˙ y˙ s



 =

0 0 V 0



     4ψ 1 −V + r+ ρref ys ls 0

(21)

Using the approximation tan δ ≈ δ + w(δ), in which w(δ) is small nonlinear function (∈ C ∞ ), we have        V 1 4ψ 4ψ˙ = 0 0 + δ V 0 ys y˙ s l ls     V 1 −V + ρref + w(δ) (22) 0 l ls Using sensors for the yaw rate r (ex. a gyro) and the deviation ys (ex. a GPS), it can be assumed that the state variables of (22) is directly obtained. The system (22) with the control input δ and the reference input ρref is shown to be controllable. 3.3 Nonlinear state feedback H∞ optimal controller We consider to construct the H∞ optimal controller so that the output [4ψ T , ysT ]T can be driven to zero as t → ∞ (Moriwaki and Akashi (1993); Moriwaki (2001, 2005)). From the system (19), (20) and the system (22), the vehicle system is able to be rewritten as the same form x˙ i (t) = Ai xi (t) + Bi δ(t) + Di ρref (t) +Gi (∆)di (t) y(t) = Ci xi (t) , (i = 1, 2)

(23)

where x1 := [ β T rT 4ψ T ysT | z T θT wT q T ]T , (for the case : |V | > 1 , |V˙ | < 1) x2 := [ 4ψ T ysT | z T θT wT q T ]T , (for the case : |V | < 1 , |V˙ | < 1) ui := [ δ T | UfT UrT ]T , (i = 1, 2) and d1 := µ (for the case : , |V | > 1 , |V˙ | < 1) d2 := w(δ) (for the case : , |V | < 1 , |V˙ | < 1) Each coefficient matrix is adopted appropriately. Now consider the problem of nonlinear state feedback H∞ optimal control for (23), in which it is to find, if existing, the smallest value γ ∗ ≥ 0 such that for any γ > γ ∗ there exists a state feedback δ = l(x)

(24)

such that L2 - gain from d to [ y T , δ ]T is less than or equal to γ, where the system (23) is said to have L2 - gain less than or equal to γ if ZT 0

k y k2 dt ≤ γ 2

ZT

k δ k2 dt ,

(25)

0

for all T ≥ 0 and all δ ∈ L2 (0 , T ). We obtain the following theorem van der Schaft (1992),van der Schaft (1996).

[Theorem 1] Consider the nonlinear system with disturbances (23). Let γ > 0. Suppose there exists a C ∞ solution V ≥ 0 to the Hamilton-Jacobi equation ∂V (x) 1 ∂V (x) 1 Ax + [ G(∆)GT (∆) − BB T ] ∂x 2 ∂x γ 2 ∂V (x) T 1 T T ) + x C Cx = 0 , V (x0 ) = 0 (26) ×( ∂x 2 or to the Hamilton-Jacobi inequality 1 ∂V (x) 1 ∂V (x) Ax + [ G(∆)GT (∆) − BB T ] ∂x 2 ∂x γ 2 ∂V (x) T 1 T T ×( ) + x C Cx ≤ 0 , V (x0 ) = 0 (27) ∂x 2 then the closed-loop system for the feedback ∂V (x) T ) δ = −B T ( ∂x

(28)

has L2 - gain (from d to [ y T , δ ]T ) less than or equal to γ. The subscript i (= 1 , 2) in (23) is abbreviated in the Theorem 1. 3.4 Suboptimal solutions of Hamilton-Jacobi equation We consider the following linearized system derived from (23). x˙ i (t) = Ai xi (t) + Bi δ(t) +Di ρref (t) + Ei di (t) (29) y(t) = Ci xi (t) , (i = 1, 2) Then, we obtain the following corollary from Theorem 1. (Corollary 2) Consider the linearized system (29) derived from the nonlinear steering system (23) and assume Ai is asymptotically stable. The L2 - gain of the linearized system (29) is less than or equal to γ if and only if there exists a solution Pi ≥ 0 of the algebraic Riccati equation ATi Pi + Pi Ai + CiT Ci 1 +Pi ( 2 Ei EiT − Bi BiT )Pi = 0 , (i = 1, 2) (30) γ The H∞ optimal controller for (29), is given by Yoshikawa and Imura (1994), van der Schaft (1992) u(t) = −BiT Pi xi (t) , (i = 1, 2).

(31)

We suppose an approximate solution of the HamiltonJacobi equation (26), ((27)) as 1 V (x) = xT P x + Vh (x) , (32) 2 where P is a solution of the Riccati equation (30) and Vh (x) is satisfied the following higher order equation ∂Vh (x) 1 ∂Vh (x) 1 − A∗ x = [ EE T (∆) − BB T ] ∂x 2 ∂x γ 2 ∂Vh (x) T 1 ∂V (x) ∂V (x) T ×( ) + Gh (x)( ) , (33) ∂x 2 ∂x ∂x where A∗ := A − BB T P + γ12 EE T P and Gh (x) is the higher term of γ12 G(∆)GT (∆) − BB T in (26), ((27)). The

m-th order terms V (m) (x) of V (x) , m = 3, 4, · · · , can be computed inductively starting from V (2) (x) = 12 xT P x by using the approximation scheme (van der Schaft (1996)). 4. NUMERICAL SIMULATIONS In this section, we consider the numerical examples for the linearized system (29). The specification data of a typical passenger car is given in Table 1. (1) The case : V = 15[m/sec] , |V˙ | < 1 The extended state space model is obtained as follows.  ˙    β 0 0 0 −2 −1 0 0 0  r˙   6.7 2.5 0 0 0 0 0 0   ˙   4ψ   0 1 0 0 0 0 0 0     0 0 0 0  15 1.8 15 0  y˙ s      = 0 0 0 0 0 0 1 0   z˙      ˙   0 0 0 0 0 0 0 1   θ     w˙  0 0 0 0 −7.6 −33 −3 −1.4 0 0 0 0 −40 −5.7 −1.1 −4 q˙     0 0 ∗2  1 0 ∗1 β  0 0 ∗2  δ  r   10.8 0 ∗1  4ψ   0 −15 0 0 0 0  ρ       ref   ys   0 0 0 0  0 0  µ      ×  U (34) + 0 0 0 0 0  f    z   0  θ   0 0 0 0 0 0   Ur      w 0 0 0 0.001 0.001 ∗3 d1 q 0 0 0 −0.006 0.001 ∗3 where ∗1, ∗2, ∗3 are small random factors according to (23). The system (34) with the second equation of (34) is unstable with poles : { 0, 0, 0.27 ± 1.15j, −1.09 ± 5.26j, −2.43 ± 6.17j }. where j is the imaginary unit. The system (34) has the stabilizing controller by pole assignment method with the feedback gain :  0 0 0 0  0 0 0 0  0 0 0 0  K = −  −0.00004 0.00005 0.00006 0.00012  0.0002 0.00001 −0.00006 −0.00004 0 0 0 0  0 0 0 0 0 0 0 0   0 0 0 0  (35) 0.00002 0 0 0    −0.0002 0.0004 −0.00009 0.0009 0 0 0 0 with stable poles : { −1 ± 5.3j, −2 ± 2j, −2.5 ± 6.2j, −3 ± 3j }. The system (34) has the H∞ controller derived from (31) with the feedback gain :   0 000 −0.17 −0.18 −0.48 −0.45  1.7 0.98 1.46 0.098 0 0 0 0   0 0 0 0 0 0 0  0 T (36) Bi Pi =  0 0 0 −0.0002 0 0 0    0  0 0 0 0 −0.0001 0 0 0  0 0 0 0 0 000

Table 1. Specification data of a passenger car 0.5

= = = = = = = = = =

2.54 0.97 1.57 1.83 25,000 25,000 15 , ( or 0.05 ) 1,170 0.7 1.341

K Bi’Pi

0.4

[m] [m] [m] [m] [N/rad] [N/rad] [m/sec] [kg]

0.3

0.2

0.1

Amplitude

l lf lr ls cf cr V m µ i2

0

!0.1

[m2 ]

!0.2

!0.3

!0.4

!0.5 0

1

2

3

4

5 Time (sec)

6

7

8

9

10

0.5 K Bi’Pi

Fig. 7. Step response from rhoref to ys REFERENCES

Amplitude

0

!0.5

!1 0

"

#

Time (sec)

$

%

10

Fig. 6. Step response from delta to ∆ψ with stable poles : { −1.08± 5.26j, −2.31 ± 2.09j, −2.43 ± 6.17j, −12.96 ± 7.59j }. The proposed controller is tested in a simulation where the measurement output data are obtained from a gyro (w.r.t. the yaw rate r ) and a GPS (w.r.t. the deviation ys derived from the curvature ρref of the prescribed reference path) each 2 seconds and the controller decides the steering angle δ so that the deviation ys is driven to zero and the yaw rate r follows V ρref . (Fig. 6) and (Fig. 7) show the simulation results of step response from input to the output ∆ψ and ys , respectively. 5. CONCLUSIONS It has been considered the problem of automatic steering of autonomous vehicles. The extended model including small nonlinear factors of the steering motion is derived. The optimal regulator for the extended steering system is introduced so that the autonomous vehicle can be driven along the reference path. The proposed controller is supposed to be considered for autonomous vehicles in which no passengers ride. For the autonomous vehicles with passengers, the proposed controller does not seem to give them a comfortable ride because there are many problems (oscillation and yawing, etc.) to be remained unsolved for a comfortable ride.

Abe, M. (1992). Vehicle Dynamics and Control. Saikai-do Press, Tokyo. Ackermann, J., Bartlett, A., Kaesbauer, D., Sienel, W., and Steinhauser, R. (1993). Robust Control Systems with Uncertain Physical Parameters. Springer-Verlag, Berlin. Andrezejewski, R. and Awrejcewicz, J. (2005). Nonlinear Dynamics of a Wheeled Vehicle. Springer-Verlag, New York. Ellis, J. (1969). Vehicle Dynamics. Business Book ltd., London. Matsuo, Y., Harada, H., Yamamoto, M., and Kubota, Y. (1990). Development of experimental vehicle with integrated chassis control. Toyota Engineering, 40, 120– 126. Moriwaki, K. (2001). A method of autonomous motion control with optimization. Proc. 4th IFAC Symp. Intelligent Autonomous Vehicles, 393–398. Moriwaki, K. (2005). Autonomous steering control for electric vehicles using nonlinear state feedback h-infinity control. Nonlinear Analysis, 63, e2257–e2268. Moriwaki, K. and Akashi, H. (1993). The dynamic regulation of linear discrete-time systems with unknown input (a method using adaptive state observer). Proc. 12th IFAC World Congress, 883–886. Nagai, M., Okada, A., Komoridani, K., Suda, Y., Tani, K., Amijima, H., Nakajiro, S., Harada, H., Miyamoto, M., and Yoshioka, H. (1999). Dynamics and Control of Vehicles. Yoken-do Press, Tokyo. van der Schaft, A. (1992). l2 -gain analysis of nonlinear systems and nonlinear state feedback h∞ control. IEEE Trans. on AC, AC-37, 770–784. van der Schaft, A. (1996). L2 -gain and Passivity Techniques in Nonlinear Control. Springer-Verlag, London. Yoshikawa, T. and Imura, J. (1994). Modern Control Theory. Shokoh-do Press, Tokyo.