Multibody System Dynamics 11: 365–394, 2004. C 2004 Kluwer Academic Publishers. Printed in the Netherlands.
365
A Control Strategy for Vehicle Trajectory Tracking Using Multibody Models 2 ´ PAVEL ANTOS1 and JORGE A. C. AMBROSIO 1 Faculty
of Mechanical Engineering, Department of Mechanics, Czech Technical University, Prague, Czech Republic, E-mail:
[email protected]; 2 Instituto de Engenharia Mecˆanica, Instituto Superior T´ecnico, Lisboa, Portugal; E-mail:
[email protected] (Received: 7 July 2003; accepted in revised form: 12 April 2004) Abstract. The development of a virtual driver for a vehicle running in a given track constitutes an important application of vehicle dynamics. For this work a multibody based methodology for the design of a vehicle virtual driver is proposed. A simplified control model for the vehicle, which makes use of extended bicycle model, is implemented. The outcome of the controller based on the bicycle model is used to control a detailed multibody model of the real vehicle, including all suspension elements and tire/road forces, which provide the observed motion. The only variables that the control acts upon are the steering wheel angle and vehicle wheels’ torques, which represent the traction and braking. The reference trajectory that has to be followed by the vehicle is defined by using a spline curve that represents its geometry. This description of the trajectory may be as complex as required by any particular application. The control model is implemented in the MATLAB environment while for the vehicle multibody model a stand alone multibody computer code is used. These two computational environments are interfaced using the available MATLAB communication functions. The virtual driver model is finally demonstrated for a sports vehicle that runs in prescribed track and the simulation results are discussed in face of the methods used. Keywords: lateral control, bilinear control, track modeling, vehicle dynamics
1. Introduction Advanced control methods are applied in different kinds of vehicle handling problems. The most popular automotive applications of control theories include traction, longitudinal and lateral control or engine, transmission and suspension controls. Reasons for using advanced control methods include economical aspects such as fuel economy and hardware costs, or impacts on environment such as emissions, safety, maneuverability and passenger comfort [1]. The use of powerful computer processors enables the implementation of more complex models for real-time controllers. However, the computational demand and performance is still the limit criterion for the choice of the vehicle model degree of modeling detail and the control method. In automotive applications the Linear Control, Sliding Mode Control, Nonlinear Control, Adaptive Control and Fuzzy Control are the most often used approaches. Based on the amount of the preview information available to the controller, the control methods can be categorized into three types: the feedback control, the feedback/feedforward control, and the preview
366
´ P. ANTOS AND J. A. C. AMBROSIO
control. Another point of view to classify the controllers is based on vehicle models used in controller development. In the vehicle lateral control the bicycle model is the methodology more widely applied to provide a reduced representation of the car. A methodology based on the multivariable bilinear vehicle control is presented by Langson and Alleyne [2]. This work is an extension of earlier control methodology proposed by Alleyne [3, 4], where a Linear Quadratic (LQ) controller based on the simplified dynamics of two degrees of freedom bicycle vehicle model is used. For the design of the controller a transformation of the dynamic model from vehicle-fixed coordinate system to the inertia frame coordinates is used. Such transformation uses a formulation developed for lane tracking by Peng and Tomizuka [5]. Another work by Peng and Tomizuka [6] dealing with the lateral vehicle control presents a continuous time deterministic optimal preview control algorithm. The preview control algorithm is obtained by minimizing a quadratic performance index which includes terms representing the passenger ride quality as well as the lateral tracking error, being each of these terms multiplied by a frequency dependent weight. The controller is based on the Frequency Shaped Linear Quadratic (FSLQ) optimal control approach. The preview control law consists of a feedback control term and two feedforward control terms. The feedback term is exactly the same as that of traditional LQ control algorithm. It is shown that the feedforward preview control action significantly improves the tracking performance and ride quality. A variable look-ahead controller for the lateral guidance of vehicles for Automated Highway Systems was proposed by Hingwe and Tomizuka [7]. There, an input–output linearization controller is proposed with the objective to eliminate the lateral error at a certain point ahead of the vehicle. It is shown that the yaw internal dynamics can be damped for all longitudinal velocities if the look-ahead distance is a quadratic function of the longitudinal velocity. Matsumoto and Tomizuka [8] discuss the use of two independent control inputs, which include the front steering angle and an extra control input. They consider three alternative choices for the extra control input: (1) a differential driving torque between the front wheels, (2) a differential driving torque between the two rear wheels, and (3) the rear steering angle. They show that the front and rear independent steering allows a wider variation of lateral velocity and yaw rate during steady state maneuvers. Furthermore, Matsumoto and Tomizuka [8] discuss a control algorithm with front and rear independent steering, which features continuously changing gains dependent on vehicle velocity and road conditions. The use of optimal control theory for vehicle control is proposed in the work of Higuchi and Saitoh [9]. The authors describe a methodology of controlling four wheel steering (4WS) which is based on a linear vehicle model of two degrees of freedom. The purpose of the proposed control is to minimize the sideslip angle at the center of gravity. The control method feeds forward the steering wheel angle and feeds back the yaw velocity and the sideslip angle to the front and rear wheel angles. It is shown that the control logic with yaw
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
367
velocity and sideslip angle feedback is superior to using only yaw velocity feedback in the nonlinear range of tire adhesion. Young and Kim [10] use an optimal control law formulation, which is based on the linear quadratic regulator (LQR) theory. A system model that includes the vehicle, a human driver and the rear wheel steering system is used. Two new designs of the vehicle speed function (VSF) for the fourwheel steering (4WS) vehicle are proposed. The first system is designed for the maximum stability while the second system is designed to emulate the response of the optimal 4WS vehicle. The objective of this work is to present a methodology of the vehicle control to follow the trajectories for prescribed maneuvers. The methodology presented here is based on the bilinear control approach and is applied to the extended bicycle model of the four-wheel vehicle, using multivariable bilinear vehicle control presented by Langson and Alleyne [2]. The developed controller acts upon the vehicle steering and on the individual wheel torques that represent the traction and breaking efforts. Steering dynamics is modeled as a steer system driven by an actuator with separate inner loop control scheme. The control method is tested and discussed based on the simulation of a vehicle that undergoes a lane change maneuver. 2. Multibody Formulation for the Complex Vehicle The position of a body reference frame is defined, in what follows, by a set of Cartesian coordinates. The position and orientation of rigid body i is defined by ri = [x
z]T
y
(1)
and by a set of rotational coordinates that in this formulation are Euler parameters, written as pi = [e0
e1
e2
e3 ] T .
(2)
The position of body i of the multibody system is described by the set of coordinates given by qi∗ = riT
piT
T
,
(3)
The velocities and accelerations of body i are given by vectors q˙i = r˙iT q¨ i = r¨ iT
T ω iT , T ω˙ iT .
(4) (5)
The velocities and accelerations of body i use the angular velocities ω i and accelerations ω˙ i instead of the time derivatives of the Euler parameters. The relation
´ P. ANTOS AND J. A. C. AMBROSIO
368
between the time derivatives of the Euler parameters and the angular velocity for a body i is p˙i = 1/2LiT ω˙ i , being Li a 3 × 4 matrix given in ([11], p. 175). In terms of the Cartesian coordinates, the equations of motion of an unconstrained multibody system are written as Mq¨ = g,
(6)
where M is a mass matrix and g is a force vector that contains the external and Coriolis forces acting on the bodies of the system and q¨ is the vector of the accelerations made with the accelerations vectors of the individual bodies. For a constrained multibody system the kinematical joints are described by a set of holonomic algebraic constraints Φ(q∗ , t) = 0 .
(7)
Using the Lagrange multipliers technique the constraints are added to the equations of motion. These are written together with the second time derivatives of the constraint Equation (7). The set of equations that describe the motion of the multibody system is q¨ g M Φq T = , (8) λ γ Φq 0 where λ is the vector of Lagrange multipliers and γ is vector that groups all the terms of the acceleration constraint equations that depend on the velocities only, i.e., ˙ q q˙ − Φtt − 2Φqt q. ˙ γ = −(Φq q)
(9)
Equation (8) is a differential-algebraic equation that has to be solved and the resulting accelerations integrated in time. Notice that the first row constitutes the differential part of the equation and the second row the second time derivative of the algebraic part. 2.1.
SOLUTION OF THE SYSTEM EQUATIONS OF MOTION
The equation of motion Equation (8) does not use explicitly the position and velocity constraint equations. Therefore, during the integration process there may be a drift in the system constraints due to the accumulation of numerical errors in the positions and velocities. To minimize constraints violation during numerical integration the Baumgarte stabilization is used leading to a modified Equation (8) written as g M ΦqT q¨ (10) = ˙ − β 2Φ , λ γ − 2α Φ Φq 0
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
369
where α and β are positive constants that represent the feedback control param˙ is the velocity eters for the velocities and position constraint violations and Φ constraint equation. The interested reader is referred to [11] for further details on the formulation used. 2.2.
VEHICLE STEERING CONSTRAINT
The steering control is used to drive the vehicle such that a desired path is followed. The steering system connects the steering wheel, which is controlled by the driver, to the knuckles of the wheels. The attitude angle imposed on each of the steered wheels is defined by the steering geometry. When a road vehicle is performing a curved path, the outside wheels have a different radius of curvature than the inside wheels. So the inside wheel must steer to a larger angle than the outside wheel. This may be achieved by asymmetry of the steering system geometry. The ideal difference in angles is defined by the Ackerman geometry [12]. The relationships between input steering angle δ f and the angles of the steered wheels are −1 d 1 δfL = atan − , tan(δ f ) a + b −1 d 1 R + , δf = atan tan(δ f ) a + b
(11)
(12)
where the positions of wheels with respect to the vehicle center of mass are given by dimensions a, b and d, which represent the distances from the vehicle center of mass to the front and rear axle centers, and the half width of the track, respectively. Moreover, the steering angle of the left wheel is denoted by δ Lf while for the right wheel the steering angle is δ Rf . The steering system is applied to the vehicle model through two constraint equations Φ(sr,1) = ξ kT ξi − cos δ Rf = 0, 1
(13)
Φ(sr,1) 2
(14)
=
ξ kT ξ j
− cos δ Lf = 0,
where vector ξ˜k is the axis of the local coordinate system of the chassis and vectors ξ˜i and ξ˜ j are the axes of the local coordinate systems of the right-front knuckle and the left-front knuckle, respectively. It is assumed that the body fixed coordinate systems of the chassis and the knuckles are aligned when the steering angle is null and the vehicle is in static equilibrium. The use of the steering constraint requires that the state of the steering angle δf and its first and second time derivatives, δ˙f and δ¨f respectively, are known. The time derivatives of the steering angle are required to evaluate the acceleration constraint equations, which are needed to construct the equations of motion (10).
370
´ P. ANTOS AND J. A. C. AMBROSIO
3. Integrated Program System for Vehicle Control In this work the control of the vehicle is based on using the steering input and the wheels traction and braking possibilities to follow a given trajectory. The steering input is described in this model by the kinematic constraint that imposes that the angle between the forward direction of the front wheels and the chassis follows prescribed values, calculated by the controller. These kinematic constraints, defined by Equations (13) and (14) are added to the set of Equation (7). The traction and braking of the vehicle wheels are described by torques applied to the wheels and the chassis, being also calculated by the controller. The traction and braking torques are added to the force vector g in Equation (10). Integrated Program System (IPS) used to model the vehicle and controller consists of two independent parts, the solver and the controller. The solver is a stand alone multibody dynamics code written in FORTRAN, named DAP3D [11], while the controller is a MATLAB function [13]. These two components of the solution procedure are integrated by means of the MATLAB External Interfaces. The structure of the IPS is schematically shown in Figure 1. Upon its launch the IPS starts a new MATLAB engine session, initiates the controller and starts the solver DAP3D. During the initialization, the controller reads a set of data that includes the controller settings and the prescribed trajectory geometric description. Afterwards the solver reads input data of the detailed vehicle model and starts the analysis. During the simulation the DAP3D code calls the controller MATLAB function to get suitable values of the steering angle, including its first and second time derivatives, and values of the driving torques for all wheels, i.e., the traction and braking torques. The strategy proposed here uses the system simulator program and the controller as independent programs that communicate only through an interface. Therefore, the procedure described in this work can also be used with any other formulation
Figure 1. Scheme of the Integrated Program System (IPS).
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
371
for the system analysis or even with real vehicles for which the position, orientation and velocities are monitored. 4. Vehicle Reduced Model for the Control Strategy The controller dynamics is derived based on the simplified planar model shown in Figure 2. The planar vehicle model consists of a vehicle body, described by mass m and the yaw moment of inertia i zc . The only forces that act on the body are the tire ground interaction forces being the aerodynamic forces not taken into account. It is assumed that the width is the same for front and rear axles. The equations of transient motion for the planar vehicle model during a turning maneuver are given by [3, 14]: m(U˙ − V ε˙c ) = Fxc1 + Fxc2 + Fxc3 + Fxc4 , m(V˙ + U ε˙c ) = Fyc1 + Fyc2 + Fyc3 + Fyc4 ,
(15) (16)
I Z c ε¨ c = a(Fyc1 + Fyc2 ) − b(Fyc3 + Fyc4 ) + d(−Fxc1 + Fxc2 − Fxc3 + Fxc4 ).
(17)
where U and V are longitudinal and lateral velocities in the body-fixed coordinate system (X Y )c and ε˙c is yaw rate of the vehicle. The force components Fxci , with i = 1, 2, 3, 4 being the number of each of the wheels, are functions of the longitudinal tire force Ti and the lateral tire force L i generated at each wheel. Using small angle approximation the force components are: Fxci = Ti − L i δi ,
i = 1, 2, 3, 4
Figure 2. Planar vehicle model.
(18)
´ P. ANTOS AND J. A. C. AMBROSIO
372 Fyci = Ti δi + L i ,
i = 1, 2, 3, 4
(19)
where δ i is the steering angle of the i-th wheel. Neglecting the dynamics of wheels, the longitudinal tire forces, Ti , are assumed to be directly proportional to the torques Mwi . Ti =
Mwi , i = 1, 2, 3, 4, rwi
(20)
where rwi is the effective wheel radius. For a simplified model, the lateral forces are proportional to the wheel slip angle L i = Cαi αi , i = 1, 2, 3, 4,
(21)
where Cαi is cornering stiffness and α i is wheel slip angle of the i-th tire. The wheel slip angle α i is defined as the angle between the direction of the wheel plane and the direction that the wheel is traveling [3]. To simplify the model description the cornering stiffness of the front and rear wheels, Cα f and Cαr , are defined respectively as: Cα f = Cα1 + Cα2 ,
(22)
Cαr = Cα3 + Cα4 .
(23)
Using a small angle approximation, the wheel slip angles for each wheel are calculated as: V + a ε˙c V + a ε˙c − δ1 , α2 = − δ2 , U U V − bε˙c V − bε˙c − δ3 , α4 = − δ4 . α3 = U U
α1 =
(24) (25)
The proposed controller is determined to control a front-wheel-steering vehicle. Therefore, the steering angles of the rear wheels are equal to zero, i.e. δ3 = δ4 = 0.
(26)
Furthermore, it is assumed in the reduced model that both front wheels have equal steering angles. δ1 = δ2 = δ f .
(27)
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
373
The simplifications described above, which correspond to the use of Equations (18) to (27) in Equations (16) and (17), lead to the extended bicycle model described by Cα f + Cαr V m(V˙ + U ε˙c ) = U aCα f − bCαr Mw1 Mw2 + + − Cα f δ f ε˙c + U rw1 rw2 I Z c ε¨ c =
a 2 Cα f + b2 Cαr aCα f − bCαr V+ ε˙c U
U Mw1 Mw2 +a + − Cα f δ f rw1 rw2
Mw1 Mw2 Mw3 Mw4 +d − + − + rw1 rw2 rw3 rw4
(28)
(29)
The equations of motion of the vehicle model are written in the body-fixed coordinates. However, the tracking problem is often defined in the global coordinate system. As proposed by Peng and Tomizuka [5] the bicycle model dynamics is described in the track coordinates, which are defined in Figure 3. The track reference frame (X Y )t is defined such that the X t axis is tangent to the track and the Yt axis passes through the origin of the vehicle reference point Oc . This ensures that the point Ot in the reference trajectory is the closest to the actual position Oc of the vehicle. The relationships between the angular positions and velocities are εc = εt + εct ,
(30)
ε˙c = ε˙ t + ε˙ ct ,
(31)
Figure 3. Vehicle and track coordinate systems.
´ P. ANTOS AND J. A. C. AMBROSIO
374
where εct is angular position of the vehicle model frame (X Y )c with respect to the track coordinate system (X Y )t . The angular position of the track frame is determined by angle εt . The lateral velocity of the vehicle relatively to the track reference frame is determined as y˙ ct = V cos εct + U sin εct .
(32)
Assume that the instantaneous radius of the track Rt is much larger than the lateral deviation of Oc with respect to Ot , expressed by yct , i.e., Rt yct . Then the longitudinal velocity of the track reference frame Ut is Ut = U cos εct − V sin εct .
(33)
The rotation rate ε˙ t of the track reference frame in the inertial coordinate system is ε˙ t = Ut /Rt ,
(34)
Substituting Equations (28) to (34) into the first time derivative of Equation (32) leads to the equation of the lateral acceleration of the reduced vehicle model, written in the track reference frame as y¨ ct =
Cα f + Cαr Cα f + Cαr aCα f − bCαr y˙ ct − εct + ε˙ct m
mU
mU Cα f aCα f − bCαr 1 Mw2 Mw1 2 δf + −U + + − mrw1 mrw2 m m Rt
(35)
Following a similar procedure of that used in Equation (35) for Equation (29) the yaw angular acceleration is written as ε¨ ct =
aCα f − bCαr aCα f − bCαr a 2 Cα f + b2 Cαr y˙ ct − ε˙ct εct + I Z cU IZc I Z cU
a Mw1 Mw2 + + − Cα f δ f I Z c rw1 rw2
2
a Cα f + b2 Cαr 1 d Mw2 Mw3 Mw4 Mw1 + + − + − + IZc rw1 rw2 rw3 rw4 IZc Rt (36)
At this point let the steering dynamics be added to the reduced model. The steering dynamics is described by a second order differential equation as I f δ¨ f + b f δ˙ f = M f .
(37)
375
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
where I f is an equivalent moment of inertia of the steering system, b f is an equivalent damping factor and M f is the steering torque. The complete dynamics of the reduced model described by Equations (35), (36) and (37) are rewritten in the bilinear form as z˙ = Cz + Bu + δ f N f u + d,
(38)
where the new matrices and vectors introduced in Equation (38) are given by
0 0 0 0 0 0 Cα f +Cαr C = 0 − m 0 − aCα f −bCαr IZc 0 0 0 0 0 0 0 0 d B= 0 0 IZc 0 − 1 rw1 IZc 0 dIf
0 0 0 Nf = 0 0 0
0 0 0
0 0 0
1 mrw1 a I Z c rw1
1 mrw2 a I Z c rw2
0
0
0 0 0
1 0 0
Cα f +Cαr mU aCα f −bCαr IZcU
Cα f m aCα f − IZc
−
0 0 0 0 0
0 0 0 0
1 rw2
1 rw3
0
0
0 1 0
aCα f −bCαr mU a 2 Cα f +b2 Cαr IZcU
0 0 0 0 , 0 1 rw4 0
0
0 0 0
0 0 1 0
, 0 b − I ff
(39a)
0 0 0 0 0 0 , d = 1 aCα f −bCαr 2 − U 0 0 Rt m 2 2 a Cα f +b Cαr 0 0 IZc 0 0 0
(39b)
Note that matrices B and N f are constant while matrix C and vector d depend on the forward velocity U of the model. Vector d futher depends on the track radius Rt . The state and the control vectors of the system, used in Equation (38) are z = [yct u = [M f
εct
δf
Mw1
y˙ ct Mw2
ε˙ ct Mw3
δ˙ f ]T
(40)
Mw4 ] . T
(41)
The control variables required for in the equations of motion of the complex vehicle are partially contained in vector z.
´ P. ANTOS AND J. A. C. AMBROSIO
376 5. Design of the Controller
The task of the controller is to force the vehicle to follow a prescribed trajectory. The controller is based on a bilinear control methodology [2]. The continuous-time state-space regulator for such bilinear system model is given by z˙ = Cz + Bu + Nu,
(42)
Notice that the form of the controller is a simplification of Equation (38) where vector d that represents the addictive disturbance has been removed. Furthermore, matrix N∈ n×m includes the sum of the product of state variables by matrices such as N f , written in as: N≡
n
z jNj,
(43)
j=1
For the controller now proposed j = 1 and N j = N f while z j is simply δ f . The initialization of the controller model dynamics is obtained by prescribing the initial values of z as z(0) = z0 ,
(44)
The state vector z ∈ n and the control vector u ∈ m , are related by the statefeedback law u = −K F B (z) z,
(45)
where the optimal gain matrix K F B is such that the state-feedback law minimizes the performance index 1 t∞ T (z Qz + uT Ru) dt, (46) J= 2 t0 of the continuous-time state-space model given by Equation (42). In the quadratic cost function expressed by Equation (46) the symmetric weighting matrices Q ∈ n×n and R ∈ m×m are positive semi-definite and definite pre-defined matrices respectively. Notice that the values of matrices Q and R are set by the user and they have a major influence on the behavior of the controller. Though Q and R can be full matrices, in the examples presented throughout this work only diagonal matrices are considered. The optimal gain K F B ∈ m×n is calculated through the solution of the statedependent form of the algebraic Riccati equation, associated to the problem, given by [15] CT S(z) + S(z)C − S(z) (B + N) R−1 (B + N)T S(z) + Q = 0,
(47)
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
377
The relation between the solution of Equation (47) S(z) and the optimal gain is written as K F B = R−1 (B + N)T S (z) ,
(48)
The use of this approach requires the fulfillment of the assumptions [2]: (a) the differential Equation (42) has a solution defined on [0, +∞[ for each admissible input u and z(t) → 0 as t → 0, which means that the vehicle trajectory tends to follow the track reference trajectory shown in Figure 3; (b) the pair (C, B + N) in Equation (42) is completely controllable and z stays in the controllability domain defined by Zc ≡ {z ∈ n |(C, B + N) controllable};
(49)
(c) the full state vector z is always available for use in the control law. The calculations of the gain matrix KFB and of the solution of the Riccati equation S(z) are obtained by using function lqr of MATLAB [13], according to the methodology proposed by Arnold and Laub [15].
6. Computer Implementation of the Controller The formulation of the controller described before is developed to force an automotive vehicle to follow a prescribed track by changing the steering angle and the torque at each wheel. The vehicle longitudinal velocity is assumed to be constant or slightly perturbed. At every integration step the dynamic analysis code DAP3D calls the controller model implemented in MATLAB, providing it with the information on the integration time t, vehicle position q∗c , in inertial coordinates, and velocities q˙ c . The translation velocities expressed in the inertial coordinate system and the angular velocities in the local coordinate system of the vehicle. These vectors are given by q∗c = xc q˙ c = x˙ c
yc
zc
e0c
e 1c
y˙ c
z˙ c
ωξc
ωηc
e2c
T e3c ,
T ωζc .
(50) (51)
The relation between the time derivatives of the Euler parameters and the angular velocities of the reference body are described in ([11] p. 175). Note that if the controller is to be applied with any other formulation, or even with a real world vehicle, all information required by the controller are the quantities appearing in Equations (50) and (51).
´ P. ANTOS AND J. A. C. AMBROSIO
378
Figure 4. Body fixed coordinate frame (ξ ηζ )c for the real vehicle and (X,Y )c for the reduced model.
6.1.
POSITION AND VELOCITY OF THE REDUCED MODEL
The position of the center of the reduced vehicle in the plane X Y , represented in Figure 4, which is required for the bicycle model used by the controller, is oc ≡ [ xc yc ]T
(52)
where oc is the vector with the coordinates of point Oc . The orientation of the reduced vehicle in the plane X Y , given by εc is similar to the rotation of the vehicle about Z . To find such rotation let the transformation matrix A between the inertial frame X Y Z and the vehicle model reference frame (ξ ηζ )c be written as [11] 2 e1 e2 − e0 e3 e1 e3 + e0 e2 e0 + e12 − 1/2 e02 + e22 − 1/2 e2 e3 − e0 e1 . (53) Ac = 2 e1 e2 + e0 e3 2 2 e1 e3 − e0 e2 e2 e3 + e0 e1 e0 + e3 − 1/2 c To interpret the transformation matrix, the set of Bryant angles ϕ1 , ϕ2 and ϕ3 associated to the transformation are obtained as [11] ϕ2 = arcsin A13c
A23c π π ϕ1 = arcsin − and |ϕ1 | ≤ . (54) for |ϕ2 | < cos(ϕ2 ) 2 2
A11c A12c ϕ3 = arctan − , cos(ϕ2 ) cos(ϕ2 ) The angular orientation of the vehicle reduced model, i.e., the bicycle model, is given approximately by the third Bryant angle, as implied in Figure 4, εc = ϕ3 .
(55)
379
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
The longitudinal velocity x˙ , lateral velocity y˙ and off-plane velocity z˙ , all expressed in the local coordinate system (ξ ηζ )c of the chassis of the complex vehicle model, are x˙ c x˙ c T y˙ c = A y˙ c . z˙c z˙c
(56)
The longitudinal velocity U and the lateral velocity V of the reduced bicycle model are similar to x˙ and y˙ , respectively, i.e., U = x˙ ,
(57)
V = y˙ .
(58)
In order to calculate the angular velocity of the reduced model let the time derivatives of the Bryant angles ϕ˙ 1 , ϕ˙ 2 , ϕ˙ 3 be calculated as [11] cosϕ3 ϕ˙ 1 1 sinϕ3 · cos ϕ2 ϕ˙ 2 = cosϕ2 − cosϕ3 · sin ϕ2 ϕ˙ 3
− sinϕ3 cosϕ3 · cos ϕ2 sinϕ3 · sin ϕ2
0 ωξc 0 ωηc . (59) cos ϕ2 ωζc
Therefore, the yaw rate of the planar model frame (XY )c in the inertial coordinate system is approximately equal to the time derivative of the third Bryant angle time derivative, i.e., ε˙c = ϕ˙ 3 . 6.2.
(60)
DESCRIPTION OF THE PREDEFINED VEHICLE TRAJECTORY
The required car trajectory, shown in Figure 3 as the path reference, is obtained by adding a prescribed deviation to the definition of the road centerline, as presented in Figure 5. The deviation in each control point is set to be normal to the road centerline tangent being its value bounded by the half-width of the road. This descriptive form allows representing different required car trajectories for the same road by keeping the same centerline representation and changing the deviation in the trajectory control points. The path centerline is described as a succession of line segments, which are either linear or curve arc segments. Let the ith segment be defined by its length l i and by its curvature k i , and let n s be the number of segments used. The input data for the path centerline is represented by the table Tt = [ kt
lt ]
(61)
´ P. ANTOS AND J. A. C. AMBROSIO
380
Figure 5. Determination of the required car trajectory.
where Tt ∈ n s ×2 . As the vehicle prescribed trajectory is defined in the plane X Y its path camber and elevation are not considered. Let a mesh of n p points be defined on the path centerline, obtained from the table represented by Equation (61), as a function of a parameter s. The coordinates of each point P j in the path centerline are defined as (x j , y j ) = (x(s j ), y(s j )),
j = 1, . . . , n p ,
(62)
where parameter s j is the length of the arc of curve measured from the origin of the path up to the mesh point Pj . Furthermore, besides the inertial coordinates (xj , yj ) of the centerline points let the tangent angle ε j , represented in Figure 5, be stored for every point P j . For a point Pj of the path centerline a track reference trajectory point is obtained by adding a prescribed deviation pj (sj ) in the direction normal to the path centerline tangent. The coordinates of the mesh of points that describe the required trajectory, shown in Figure 5, are xt j = x j − pj (sj ) sin(εj ),
(63)
yt j = y j + pj (sj ) cos(εj )
(64)
In order to have a continuous description of the track reference the n p points are interpolated by a cubic spline (sp xt (s),sp yt (s)). Note that the cubic spline interpolates satisfy (spxt (s j ),
sp
yt (s j )) = (xt j , yt j ),
j = 1, . . . , n p .
(65)
The cubic spline interpolation is implemented in the MATLAB controller function environment by using the function spline [13].
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
381
For any given point of the track reference the path tangent angle εt (s) is defined as
δ sp yt (s) δ sp xt (s) εt (s) = arctan , . δs δs
(66)
The curvature of the track, at a given point, is determined by the relation kt (s) =
1 = Rt (s)
sp xt
(s) sp yt (s) − sp yt (s) sp xt (s) 3 . sp x (s) 2 + sp y (s) 2 t t
(67)
where sp yt (s) = δ sp yt (s) δs and sp yt (s) = δ 2sp yt (s) δs 2 are the first and second parametric derivatives of the parametric curve representing the yt coordinate of the reference trajectory, respectively. The track curvature kt (s) is the inverse of the track radius Rt (s) for the point defined by parameter s. Subscript t always refers to points in the track reference. 6.3.
TRACK REFERENCE POINT
At any given time the position of the vehicle, centred in Oc may not be coincident with the track reference. Therefore, it is necessary to calculate the point Ot , shown in Figure 3, where the vehicle should be if it had followed the track reference exactly. Let it be assumed that Ot is the point in the track reference closest to the actual position of the vehicle Oc . The determination of the track reference point Ot is obtained by solving the problem of finding the parameter s that minimizes the distance P(s) Oc , i.e., Ot ≡ P (st ) ∧ st = s: min (xc − sp x(s)t )2 + (yc − sp y(s)t )2 , (68) where st is the parameter that minimizes the distance function. Therefore, (xt , yt ) ≡ (sp x(st )t , sp y(st )t ).
(69)
The minimization methodology applied in this work uses the MATLAB function fminbnd, which is based on the quadratic fit method and the golden section search proposed by Atkinson [16]. The search of the parameter s for each new state of the controller starts by adding an estimate of the length of track covered by the vehicle between the previous and current time to the optimal parameter s of the previous time step. Such length is calculated as the product of the longitudinal velocity of the vehicle U by the time step. Note that generally there is no assurance that the point Ot found by this procedure is unique. However, the assumption that Rt yct , used to define Equation (33), also ensures the uniqueness of point Ot .
´ P. ANTOS AND J. A. C. AMBROSIO
382 6.4.
PREVENTION OF A STEADY STATE CONTROLLER ERROR
To avoid a steady state controller error Alleyne [3] suggests to redefine the dynamics of the system by adding the integral of the path error as a new state as z¯ =
!
yct yct εct δ f y˙ ct ε˙ct δ˙ f
T
(70)
Thus, the continuous-time state-space regulator for the bilinear system of the simplified model dynamics is now redefined as ¯ z + [B¯ + δ f N ¯ ¯ f ]u¯ + d, z¯˙ = C¯
(71)
where the time derivative of the new state vector is z¯˙ = [ yct y˙ ct ε˙ct δ˙ f y¨ ct ε¨ ct δ¨ f ]T
(72)
and the control vector is still the same as that defined in Equation (41), re-written here as b b b b T Mw2 Mw3 Mw4 u¯ = M f Mw1 .
(73)
¯ B, ¯ N ¯ f and d¯ appearing in Equation (71) are equal to those The new matrices C, defined in Equation (39) but with null rows added in the top of all of them and a null column inserted in the left of matrix C. The only nonnull coefficient added to the matrices is C¯ 12 = 1. 6.5.
COMPUTER ALGORITHM FOR THE CONTROLLER
The computer implementation of the controller requires the initialization of the track data and of the steering dynamics, which is done in a preprocessing stage. During the motion of the vehicle, at every time step, the controller calculates the control vector and the state-variable vector. The computer algorithm of the controller is described as Initialization: Step 0: Read the track centerline data Tt in the form of Equation (61), the prescribed deviation p j (s j ), j = 1, . . . , n p and generate the track centerline points (x j , y j ) and angles ε j for j = 1, . . . , n p ; Step 1: Generate the mesh of points for the track reference (xt j , yt j ) for j = 1, . . . , n p using Equations (63) and (64); Step 2: Initialize the steering dynamics by prescribing δ f = δ f (0) and δ˙ f = δ˙ f (0); For every time step:
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
383
Step 3: Get the positions q∗c and velocities q˙ c of the real vehicle, Equations (50) and (51); Step 4: Calculate the position (xc , yc ) and orientation εc of the reduced model by using Equations (52) to (55); Step 5: Calculate the translation velocity (U, V) and the angular velocity ε˙ c of the reduced model by using Equations (56) to (60); Step 6: Find the point (xt , yt ) in the track reference that is closest to the position of the center of the reduced model (xc , yc ) by finding the parameter st that minimizes the distance between Oc and the track reference, using Equation (68); Step 7: Find the coordinates (xt , yt ), the path tangent angle ε t and the track curvature kt using Equations (65) to (67); Step 8: Calculate the state variables ε ct , ε˙ ct and y˙ ct using Equations (30) to (34). Note that the state variable yct is the minimum distance given by Equation (68); Step 9: Integrate the time derivatives of the steering angle of the previous time step, δ˙f and δ¨f and the vehicle lateral deviation yct to obtain the state ! ˙ variables δf and δ f and yct of the current time step; Step 10: Construct the state-variable vector z¯ specified by Equation (70); ¯ B¯ and N ¯ f as specified in Equation (39) and Step 11: Construct the matrices C, modified in order to accommodate for the redefined state-variable vector; Step 12: Calculate the optimal gain K F B using Equation (48) and evaluate the ¯ B¯ and N ¯ f instead of control vector u using Equation (45) but using C, C, B and N f ; Step 13: Find the time derivative of the state-variable vector z˙¯ using Equation (72); Step 14: Exit MATLAB and return to the analysis program with the new steering angle and its time derivatives, δ f , δ˙ f and δ¨ f and the new wheel torques Mw1 , Mw2 , Mw3 and Mw4 to be applied in the real vehile. In Step 9, of the algorithm outlines, the controller has to provide state of steering angle, δ f , and its first and second time derivatives, δ˙ f and δ¨ f respectively, to steer the real vehicle. However, the control theory only provides second derivative of the steering angle. To get the steering angle and its first derivative, the second derivative has to be integrated in time. The integration of the time derivatives of the steering angle is done inside the controller independently of the integration of the accelerations and velocities of the real vehicle model, which is done in the DAP3D. The integration of the steering angle time derivatives is controlled by the DAP3D integrator variable time steps. The variable step size Adams–Bashforth integration method is used for the integration of the time derivatives of the steering angle [17].
´ P. ANTOS AND J. A. C. AMBROSIO
384
Figure 6. Prototype of the sports car.
Figure 7. Multibody model of the sports vehicle.
7. Vehicle Model The methodology proposed in this work is used to control the complex multibody model of a sport car, the Lancia Stratos, presented in Figure 6. A complete description of this model is presented by Ambr´osio and Gon¸calves [18] and outlined here. The vehicle is a front-wheel steering and a rear-wheel driving vehicle. A multibody model of the vehicle, composed of 17 bodies, is considered here. The system components include the front double A-arm suspension system, the rear McPherson suspension system, wheels and chassis as depicted in Figure 7. To model the tire interaction with the ground an analytical tire model with comprehensive slip is used [19–21]. 8. Application to the Control of a Lane Change Maneuver The lane change maneuver, also known as S200-maneuver or the maneuver in the S-shaped track with 200 m radius, is used to exemplify the application of the methodology presented in this work to the tracking control of the detailed vehicle model. This maneuver requires the vehicle to follow the trajectory presented in Figure 8(a) and further described in Table I. The inertia and geometric properties of the reduced model, which is used in the control model, are presented in Table II.
385
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
Table I. S200-maneuver geometric definition. Segment Curvature kti (1/m)
Segment i
Segment Length lti (m)
1
0.005
157.07963
2
− 0.005
157.07963
3
0
∞
Table II. Planar model properties. Vehicle mass
m
825.5
kg
Vehicle moment of inertia
IZc
752.7
kg.m2
a
1.247
m
Distance from vehicle CG to front axle Distance from vehicle CG to rear axle
b
0.843
m
Half-width of the vehicle track
d
0.723
m
Front cornering stiffness
Cα f
−60000
N/rad
Rear cornering stiffness
Cαr
−60000
N/rad
Effective tire radius
rwi
0.293
m
The evaluation of the performance of the controller is done in terms of its ability to keep the car in the prescribed trajectory and, when the vehicle is away from the desired path, to force it back to the given track. These two scenarios differ in the initial conditions given to the models, which are referred to as ‘position on the track’ and ‘position away from the track.’ 8.1.
CASE 1: INITIAL POSITION OF VEHICLE COINCIDENT WITH PREDEFINED TRAJECTORY
The initial position of the vehicle has its position Oc coincident with the prescribed trajectory and its heading aligned with the tangent to the track. The position and orientation of the vehicle are xc = 0 m, yc = 0.015 m and εc = 0 m. The vehicle initial forward speed is U(t = 0) = 33 ms−1 . Let the weighting matrices for the controller be chosen as Q = diag([10000000 200000 1 10000 1 1 1]) , R = diag([500 0.008 0.008 0.008 0.008]). and the steering dynamics selected as If = 0.3,
bf = 1.
The complex vehicle model is simulated until the completion of the prescribed trajectory. The results presented in Figures 8 to 11, in terms of the vehicle position and some of the state variables allow analyzing how the control algorithm handles the path following task.
386
´ P. ANTOS AND J. A. C. AMBROSIO
Figure 8. Prescribed and vehicle actual rajectories during S200-maneuver with initial position on the track: (a) Complete trajectory; (b) Detail of the first part of the simulation results.
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
387
Figure 9. Integral of lateral relative position and lateral and angular relative positions of the vehicle CG during S200-maneuver with initial position on the track.
Figure 10. Steering angle and its time derivatives during S200-maneuver with initial position on the track.
388
´ P. ANTOS AND J. A. C. AMBROSIO
As displayed in Figures 8 to 11, in the first second after the simulation is started the car achieves the steady state and keeps it until the sign of the track curvature changes. After that it takes another second for the car to achieve the steady state again. Finally, to finish the maneuver the vehicle is required to follow the straight track. The change of the track curvature causes oscillations in the system. Nevertheless, these oscillations are damped in a short period of time and the vehicle gets back to the steady state. For the straight track, the curvature and the elements of the additive disturbance vector d¯ in Equation (60) are null. Therefore, the controller experiences very small lateral and angular errors, as shown in Figure 9. In the complex vehicle model there is no provision to maintain the longitudinal velocity of the vehicle. Furthermore, the controller is not designed to maintain it either. It is observed that during the course of the simulation the forward velocity of the complex vehicle varies from the initial 33 ms−1 to 28.7 ms−1 .
Figure 11. Wheel torques during S200-maneuver with initial position on the track.
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
389
Figure 12. Prescribed and vehicle actual rajectories during S200-maneuver with initial position away from the track: (a) Complete trajectory; (b) Detail of the first part of the simulation results.
´ P. ANTOS AND J. A. C. AMBROSIO
390 8.2.
CASE 2: INITIAL POSITION OF THE VEHICLE AWAY FROM THE PRESCRIBED TRAJECTORY
In this case the initial position of the vehicle is xc = 0 m, yc = −0.985 m, εc = 0 rad, which ensures that the car starts away from the prescribed trajectory. The initial forward speed of the vehicle is U (t = 0) = 33 ms−1 . The weighting matrices for the controller are chosen to be Q = diag([10000 70000 1 20000 1 1 1]), R = diag([50 0.01 0.01 0.01 0.01]). The steering dynamics is selected as I f = 0.3,
b f = 1.
In the current application case the complex vehicle is simulated until a distance of 600 m along X is run. By letting the vehicle run for 300 m in the straight track it is possible to evaluate the behavior of the controller during a long period without any change on the curvature of the track. The results presented in Figures 12 to
Figure 13. Integral of lateral relative position and lateral and angular relative positions of the vehicle CG during S200-maneuver with initial position away from the track.
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
391
Figure 14. Steering angle and its time derivatives during S200-maneuver with initial position away from the track.
15 allow analyzing how the control algorithm proposed handles the path following task. Figures 12 to 15 show the controlled vehicle behavior when the initial lateral position of the vehicle ! is away from the track. At the beginning of the simulation the added state variable yct in Equation (61) acquires high absolute values because of the high lateral error. Then, to cancel this value, the controller keeps a higher lateral error with ! opposite sign. Nevertheless, as soon as the absolute value of the state variable yct is minimized the controller enables the vehicle to follow prescribed trajectory with acceptable error again. Observing the figures where the state variables are plotted it is seen that in the initial period of the simulation and during short periods after changes of the prescribed trajectory there are some perturbations on the state variables and wheel moments. However, after a short period the state variables directly associated to the deviation of the vehicle from the prescribed trajectory become null, showing that the prescribed path is effectively reached. When observing the steering angle that is required by the controller to direct the vehicle to the prescribed trajectory it is noticed that values of 13◦ are achieved. In the applications now reported there are no limits to the value of the steering angle, which may be less realistic for many general applications. As in the previous application case in the course of the simulation the forward velocity of the complex vehicle varies from the initial 33 ms−1 to 25.7 ms−1 . In other
´ P. ANTOS AND J. A. C. AMBROSIO
392
Figure 15. Wheel torques during S200-maneuver with initial position away from the track.
practical applications of the methodology proposed the variation of the forward velocity of the vehicle is an issue in itself that requires appropriate treatment. There is no reason to keep the velocity constant in a general application and, therefore, its controlled change must be specified to achieve prescribed objectives specific to the application case. 9. Conclusions A control methodology that enables a vehicle to follow a prescribed trajectory is proposed here based on a bilinear control. The controller acts upon the steering angle of the front wheels and on the wheels’ torques. The implementation aspects of the theory are presented with particular attention to the steering angle integration. Therefore, two different integration schemes are used for the system
A CONTROL STRATEGY FOR VEHICLE TRAJECTORY TRACKING
393
velocities and accelerations and for the steering angle velocities and accelerations respectively. A methodology for the parametric description of a road is introduced. This methodology enables to design of various tracks, for the same basic ‘road.’ The methodology is demonstrated by an application of the vehicle that follows a prescribed trajectory represented by a lane change maneuver. The results show that, even without using any preview information, the controller is able to force the vehicle to follow the prescribed trajectory with lateral and angular errors that are acceptably small. It was shown that when the vehicle starts on the prescribed trajectory and aligned with it the controller is efficient in keeping it on track. When the initial position of the vehicle is not on the prescribed trajectory the controller is still able to lead it to the track. Nevertheless, to achieve a small lateral error during simulation the weighting matrices must be defined with respect to the expected lateral error. The choice of the optimal values for the weighting matrices for the type of application researched requires some extra investigations. Acknowledgments The authors appreciate the support of the NATO Science Affairs Division through the project Design of Vehicle Optimal Trajectories by Means of Multibody Dynamics Tools which is included in the NATO Science Fellowship Programme and the support by Funda¸ca˜ o para a Cient´ıfica e Tecnologia with the PRAXIS Project 2/2.1/TPAR/2041/95. References 1. Tomizuka, M. and Hedrick, J., ‘Advanced control methods for automotive applications’, Vehicle System Dynamics 24, 1995, 449–468. 2. Langson, W. and Alleyne, A., ‘Multivariable bilinear vehicle control using steering and individual wheel torques’, Journal of Dynamics Systems, Measurement, and Control 121, 1999, 631–637. 3. Alleyne, A., ‘A comparison of alternative intervention strategies for unintended roadway departure (URD) control’, Vehicle System Dynamics 27, 1997, 157–186. 4. Alleyne, A., ‘A comparison of alternative obstacle avoidance strategies for vehicle control’, Vehicle System Dynamics 27, 1997, 371–392. 5. Peng, H. and Tomizuka, M., ‘Lateral control of front-wheel-steering rubber-tire vehicles’, PATH Report, UCB-ITS-PRR-90-5, University of California, Berkeley, 1990. 6. Peng, H. and Tomizuka, M., ‘Preview control for vehicle lateral guidance in highway automation’, Journal of Dynamics Systems, Measurement, and Control 115, 1993, 679–686. 7. Hingwe, P. and Tomizuka, M., ‘A variable look-ahead controller for lateral guidance of four wheeled vehicles’, in: Proceedings of the American Control Conference 1998, Pennsylvania 1998. 8. Matsumoto, N. and Tomizuka, M., ‘Vehicle lateral velocity and yaw rate control with two independent control inputs’, Journal of Dynamics Systems, Measurement, and Control 114, 1992, 606–613. 9. Higuchi, A. and Saitoh, Y., ‘Optimal control of four wheel steering vehicle’, Vehicle System Dynamics 22, 1993, 397–410. 10. Cho, Y.-H. and Kim, J., ‘Design of optimal four-wheel steering system’, Vehicle System Dynamics 24, 1995, 661–682.
394
´ P. ANTOS AND J. A. C. AMBROSIO
11. Nikravesh, P., Computer-Aided Analysis of Mechanical Systems, Prentice Hall, Englewood Cliffs, NJ, 1988. 12. Gillespie, T., Fundamentals of Vehicle Dynamics, Society of Automotive Engineers, Warrendale, PA, 1992. 13. MATLAB, The Language of Technical Computing: Using MATLAB, Mathworks Inc., Natick, MA, 2002. 14. Wong, J. Y., Theory of Ground Vehicles, John Wiley & Sons, Inc., New York, 1993. 15. Arnold, W. and Laub, A., ‘Generalized eigenproblem algorithms and software for algebraic Riccati equations’, in Proceedings of IEEE Vol. 72, 1984. 16. Atkinson, K., ‘Modelling a road using spline interpolation’, Reports on Computational Mathematics, Vol. 145, Department of Mathematics, University of Iowa, 2002. 17. Shampine, L. and Gordon, M., Computer Solution of Ordinary Differential Equations: The Initial Value Problem, W.H. Freeman, San Francisco, 1975. 18. Ambr´osio, J. and Gon¸calves, J., ‘Complex flexible multibody systems with application to vehicle dynamics’, Multibody System Dynamics 6, 2001, 163–182. 19. Gim, G. and Nikravesh, P., ‘An analytical model of pneumatic tyres for vehicle dynamic simulations. Part 1: Pure slips’, International Journal of Vehicle Design 11(6), 1990, 589–618. 20. Gim, G. and Nikravesh, P., ‘An analytical model of pneumatic tyres for vehicle dynamic simulations. Part 2: Comprehensive slips’, International Journal of Vehicle Design 12(1), 1991, 19–39. 21. Gim, G. and Nikravesh, P., ‘An analytical model of pneumatic tyres for vehicle dynamic simulations. Part 3: Validation against experimental data’, International Journal of Vehicle Design 12(2), 1991, 217–228.
