Dynamical Equations of Four Wheeled Road Vehicles in the Context of AHS Pushkar Hingwe
[email protected] Masayoshi Tomizuka
[email protected]
Abstract This Note presents dynamic model of a four wheeled vehicle. The vehicle is assumed to be a rigid body mounted on a massless \unsprung mass" through suspension mechanisms at the four corners of the vehicle. Suspension mechanisms at the four corners are independent and generate verticle forces only. The road is assumed horizontal, i.e., there is no superelevation or gradient. No small angle approximation is made in the treatment. We hope to clear the confusion about selection of the coordinate frames and the dynamical equations representing the car model.
1 Introduction In this Note, we present dynamical model of a four wheeled vehicle based on Newton-Euler equations. Several researchers have derived detailed simulation models for the purpose of control design veri cation. The treatment in this note derives largely from Peng (1992) and Patwardhan (1994). Some portion of the derivation in both the sources above is not suciently clear. The derivation of the di erential equations for vehicle hinges critically on the rigid body dynamics. Casey (1983) has given a concise treatment of the rigid body dynamics. A more extensive treatment can be found in Greenwood (1988). We have borrowed notation from all the above references (most of which is standard). The vehicle model is a rigid body with suspension mechanism at the four corners. In the literature, this is referred to as the sprung mass. It is supported by the
1
suspension at four corners. The tires are at the other end of the suspension. Tires form the unsprung mass of the vehicle. In the present treatment, we will assume that the \unsprung mass" has no inertia. Because the purpose of the modeling is veri cation of lateral control algorithms, we will derive the motion of vehicle relative to the road centerline. The Euler's equations, however, are best written in the corotational basis of the \sprung mass". This observation necessitates several frames of reference to describe the kinematics of the vehicle. We have already mentioned the corotational frame. Another frame is corotational to the unsprung mass. Yet another frame, on the road centerline, is necessary to de ne kinematic quantities such as \lateral velocity" and position relative to the road centerline. The fourth frame of reference is the inertial reference frame. This note is organized as follows. Section 2 gives the description of various reference frames and the transformations between them. Section 3 presents kinematic equations of the vehicle in the appropriate reference frames. In section 4 we derive the external forces and moments on the vehicle. We combine results of Section 3 and Section 4 to write the dynamical equations. Measured quantities are described in terms of the coordinates in Section 5. Conclusions are presented in section 6.
2 Reference frames This section de nes various frames of reference for kinematical description of the vehicle sprung mass. We de ne four reference frames1 as shown in Figure 1. The inertial reference frame is described by an orthonormal basis (E1; E2; E3). A corotational reference aligned with the principle directions of the rigid body is given by orthonormal basis (es1; es2 ; es3). This frame is centered at the center of It is sucient to use three reference frames but the treatment may be confusing. We thank our colleague M. Tai for bringing this to our attention. Chen (1996) has also described the reference frames needed for a clear kinematical description of a road vehicle. 1
2
e3s es2
R e1s
S
E3
E2 E1
r
e3
u e2u e3 U e1u r e2
R
r
e1
Figure 1: Reference frames mass of the sprung mass S . Reference frame U , which represents the unsprung mass, is located in the direction ?E3 from point S on the sprung mass. Its origin U is in the plane (E1; E2). Plane (E1; E2) is the \ground" and the road is an arc of radius R on this plane. The unsprung mass's reference frame (corotational with the unsprung mass) has basis (eu1 ; eu2 ; eu3 ). eu1 is oriented along the projection of es1 on the plane (E1; E2)2. eu2 is in the plane (E1; E2). This means that < es1 ; eu2 >= 0. The \road reference frame" is constructed from the position of the unsprung mass's reference frame. The origin of the road reference frame, R, is located at the nearest point on the road centerline from the origin of the unsprung mass frame, U . This frame has basis vectors (er1; er2; er3 ). er1 is in the direction tangent to the road centerline, er2 is along the normal to the road (along the radius) and er3 completes the orthonormal basis and is therefore parallel to E3. T Notation We are going to use E to denote [ E1 E2 E3 ] . The same applies to es , eu and er . For any vector Z , we will use the notation Z to denote the representation in coordinates with basis . Zi will denote Z i . 2 This is only because the rotation from the frame eu to es is pitch followed by roll.
3
ε (yaw) θ
(pitch) e3’ = E3
E3
E2
e’2
e1’
E1
e"2 = e’2
e2s
e"3
es3 e1s = e1"
(roll) e"1
φ
Figure 2: Euler's Angles
3 Kinematics The position and the orientation of the rigid body in consideration is coordinatized by six parameters. (X; Y; Z ), the coordinates of the center of mass in the frame E , and Euler's angles ; and , again, with respect to the frame E . In the present treatment, we will not be using the global coordinates (X; Y; Z ) of the vehicle sprung mass. The Euler's angles are explained in gure 2. The rotation (of the sprung mass of the vehicle) is represented by the proper orthogonal rotation tensor Q such that es = QE . In terms of the coordinates ; and , this rotation has a matrix representation given by
32 3 2 1 0 0 77 66 cos 0 ? sin 77 66 77 = 66 0 cos sin 77 66 0 1 0 54 5 4 sin 0 cos 0 ? sin cos e3
2 66 es1 66 es2 4 s
4
32 3 32 cos sin 0 77 66 E1 77 77 66 77 66 ? sin cos 0 77 66 E2 77 54 5 54 0
0
1
E3
Q is given by
3
2 66 0 CC 1 66 B ?SC C 6@ A Q = 666 0 +CSS 1 66 ?SS C 64 B A @
0 SC 1 ?S 77 B@ CC CA CS 777 77 (1) 77 0 +SSS 1 B@ ?CS CA CC 775 +SSC +CSC where C refers to cosine function and S refers to the sine function. Note that the order of rotation is , and . The time derivative of the corotational basis es is given by es = QE and using the fact QT Q = I . _ T QE e_s = QQ = es
(2) (3)
where is the angular velocity tensor associated with the sprung mass. Since
is skew symmetric,
= ! s where ! s is the angular velocity vector associated with the sprung mass and is any vector. We now calculate in terms of the coordinates ; and . Di erentiating equation (1), we get
1 0 2 ? SC _ CA B 66 @ 66 _ ?CS 1 66 0 _ 6 ? SS ? CC _ CC Q_ = 66 B CC B 66 B _ ? SSS _ + CCS B A 66 @ +CSC _ 64
1 0 CC _ CA B@ _ ?SS SC _ ? CS _
0 BB BB +CSS _ + SCS _ @ +SSC _ ??
?? It can be veri ed that
_ T12 = _ cos cos ? _ sin QQ _ T13 = ?_ sin cos ? _ cos QQ _ T23 = _ ? _ sin QQ
5
?C_
1 CC 0 CC _ CC B@ A ?SS _ ?? (5) (6) (7)
3 77 77 1 777 CA 777 (4)777 75
Therefore,
!s = ( _ ? _ sin )es1 + (_ sin cos + _ cos )es2 + (_ cos cos ? _ sin )es3 If we choose to write Euler's equations in the corotational basis, then we get,
M1s = Ix !_ 1s + (Iz ? Iy )!2s!3s M2s = Iy !_ 2s + (Ix ? Iz )!1s!3s M3s = Iz !_ 3s + (Iy ? Ix )!2s!1s
(8) (9) (10)
where
!_ 1s = ? sin ? __ cos !_ 2s = cos ? _ _ sin ? sin cos + _ _ cos cos ? __ sin sin !_ 3s = cos cos ? _ _ sin cos ? __ cos sin ? sin ? _ _ cos and Mis are external moments acting on the sprung mass about axes esi . Notice that the ! s is in terms of ; and . However, we would like to represent ws in terms of the rotation of the road reference frame R and the rotation of the sprung mass relative to the road reference frame. The rotation of the road reference frame is given by er = Q1 E , where
3 2 cos sin 0 d d 77 66 6 Q1 = 6 ? sin d cos d 0 77 5 4 0
0
(11)
1
The rotation of the unsprung mass relative to the road reference is given by es = Per where
32 2 1 0 0 77 66 cos 0 ? sin 66 P = 66 0 cos sin 77 66 0 1 0 54 4 sin 0 cos 0 ? sin cos 6
3 32 cos ~ sin ~ 0 7 77 66 77 66 ? sin ~ cos ~ 0 777 5 54 0
0
1 (12)
where e~ = e ? ed . It is obvious that Q = PQ1 . Therefore, _ T = PP _ T + P Q_ 1 QT1 P T QQ
3 3 2 2 0 _ 0 0 ! ? ! d 3 27 77 66 77 666 6 = 6 ?!3 0 !1 7 + P 6 ?_d 0 0 77 P T 5 5 4 4 0 0 0 !2 ?!1 0 0
0
0
(13)
0
0
0
where
!1 = _ ? ~_ sin !2 = ~_ sin cos + _ cos !3 = ~_ cos cos ? _ sin 0
0
(14)
0
After some algebra, we get,
2 3 2 0 _d cos cos ?_d sin cos 66 66 0 _d 0 77 T P 66 ?_d 0 0 77 P = 66 ?_d cos cos 0 ?_d sin 4 5 4 0
0 0
_d sin cos
_d sin
0
3 77 77 5
(15)
Using equation (13) and equation (15), we get _ T )23 = !1s = _ ? ~_ sin ? _d sin (QQ _ T )13 = !2s = ~_ sin cos + _d sin cos + _ cos ?(QQ
_ T )12 = !3s = ~_ cos cos + _d cos cos ? _ sin (QQ
(16)
Calculation of the moment M s will be presented in the next section. We now write acceleration of the center of mass of the sprung mass. There are two ways of proceeding. If the global position of the vehicle center of mass is of interest, then we need to write the kinematic description of the center of mass in terms of the position vector, di erentiate to obtain the expression for velocity and di erentiate again to obtain the expression for acceleration.
If the global position of the center of mass is not of interest, but only the position and velocity relative to the road centerline is, then we can write
7
the kinematic description of the center of mass in terms of the velocity of the center of mass. Di erentiating once will then give the expression for acceleration. We assume that we are not interested in the global position of the sprung mass. This assumption is reasonable because for the purpose of lateral control, we are restricted to take position measurement relative to the the road centerline. Speci cally, we are interested only in the relative position and velocity of frame es with respect to the frame er . In Figure 3, v = r_ s, where v is the velocity of e1s
R
v rs/r
rs E3
E2
e1r
rr E1
Figure 3: Position vectors of road reference frame and sprung mass the center of mass of the sprung mass and rs is the position vector. We de ne
r_s = x_ r er1 + y_ r er2 + z_ r er3
(17)
er = Q1E
(18)
Note,
Q1 is given by equation (11). From Figure 3, we see rs = rr + rs=r =) r_ s = r_ r + r_ s=r Because of the way frame er is constructed from frame es and eu ,
r_ s=r = y_ r er2 + z_ r er3
8
Thus, the velocity of the frame u along the road centerline given by
r_r = r_ s ? r_ s=r = x_ r er1 Furthermore, because the road centerline is a planar curve , we get r _ = x_ d
R
where R is the radius of curvature of the road at the position of the frame u. The acceleration of the sprung mass's center of mass is given by
rs = [ xr yr zr ]er + [ x_ r y_ r z_ r ]e_r We know,
e_r = 1er where
1 = Q_ 1 QT1 Simpli ed, this reduces to
rs = (xr ? y_ r _d )er1 = (y r + x_ r _d )er2 = zr er3
(19)
In the next section, we calculate the forces generated by the suspension and the tires. We will use symbolic models for suspension and tires.
4 External Forces and Moments Acting on the Vehicle We rst derive the forces acting due to the suspension and the tires and later use them to nd the total external force and the total external moment about the center of mass.
9
1
mg
2 4
zr 3
σ2 Fz 2 σ 2’
E3
y
E2
Fy 2
r
Fx 2
E1
Figure 4: Suspension and resultant tire forces l1 l2
sb1 p
1
zr sb2
E3
p4
p
2
p3 yr
E2 E1
Figure 5: Extensions of the suspension
4.1 suspension forces Figure 4 give the synopsis of the typical forces acting on the vehicle. Note that the wind forces have not been considered. As stated earlier, the unsprung mass is assumed to have no inertia. The suspension is assumed to be attached to the sprung mass at points 1; 2; 3 and 4 as shown in gure 4. For simplicity in drawing, only the forces acting at point 2 and the tire forces because of second tire are shown. The suspension force acting at point 2 is Fz2 . The tire forces are Fx2 and Fy2 . Force due to gravity is mg and acts on the center of mass . The projection of points 1; 2; 3 and 4 on the ground move relative to each other as the vehicle pitches or rolls. This is a reasonable model if the vehicle has four wheel independent suspension. In passenger cars, however, the rear wheels are often connected by an axle which (among other things) keeps them separated by a constant rear track width. This feature is relatively easy to incorporate in
10
the existing scheme. The tire forces instead of acting at the verticle projections of the points 1; 2; 3 and 4, will have to act at points in the plane (eu1 ; eu2 ) which are xed relative to each other. Anyhow, for simplicity, we are will assume that the suspension forces act in the direction eu3 . The value of suspension forces Fz ; i = (1; :::; 4) is given by a functions F^z (pi ; p_ i; hi ) where pi are the suspension length as shown in Figure 5. hi is the uncompressed length of the ith suspension. The expression for pi are given as3 i
i
p1 = zu ? l1 + sb21 p = z ? l ? sb1 2
u
1
2
4
u
2
2
p3 = zu + l2 ? sb22 p = z + l + sb2
(20)
where l1; l2; sb1 and sb2 are shown in Figure 5. A linear suspension model may be represented by Fz = Ki (hi ? pi ) + Di p_i i
where Ki is a sti ness constant and Di is a damping coecient.
4.2 Tire forces Forces Fx and Fy (i = 1; :::; 4) come from the ith tire. These forces are functions of slip angle, slip ratio, velocity of the contact point, tire and pavement characteristics. The tire models give the value of forces generated parallel and normal to the tire plane (and along the pavement).These are Fa ans Fb respectively (see Figure 6. From Figure 6 it is clear that i
i
i
i
Fx = Fa cos i ? Fb sin i i
i
i
Fy = Fb cos i + Fa sin i i
i
i
where 1 = 2 = is the steering angle and 3 = 4 = 0.
Note that equation (20) is an approximate equation. An exact relation is p1 = jjz r eu3 + l1 es1 + sb1 s sb1 s sb1 s s s u u 2 e2 ? f(l1 e1 + 2 e2 ) ? [(l1 e1 + 2 e2 ) e3 e3 ]gjj and so on. 3
11
δ Fy
1
e u2
Fy
1
Fy
4
Fx
Fx
2
e1u
Fx
2
4
Fy
3
Fx
3
Figure 6: Tire con guration and tire forces
4.3 External forces Let the total external forces be represented by F . Because Newton's equations will be written in frame er , we represent the external forces F in frame er .
F=
4 X i=1
Fz ? mg i
!
X X eu3 + Fyi eu2 + Fxi eu1 i=1 i=1 4
4
(21)
We know
eu = Q2 er where
(22)
3 2 cos ~ sin ~ 0 77 66 6 Q2 = 6 ? sin ~ cos ~ 0 77 5 4 0
0
1
where ~ = ? d . Using equations (21) and (22), we get,
F= + +
4 X i=1
!
Fz ? mg er3 i
" X ! 4 i=1
" X 4 i=1
Fy cos ~ + i
!
Fx cos ~ ? i
12
!
#
! 4 X
#
4 X i=1
i=1
Fx sin ~ er2 i
Fy sin ~ er1 i
(23)
4.4 External moments From Figure 4 it is clear that the total external moment about the center of mass of the sprung mass is given by
M=
4 X i=1
(i (Fx eu1 + Fy eu2 + Fz eu3 )) i
i
(24)
i
where i are given by
i = i ? z r eu3 0
Here,
1 = (l1es1 + sb21 es2 ) ? [(l1es1 + sb21 es2 ) eu3 ]eu3
(25)
0
Let es = Q3 eu 4 where
32 2 1 0 0 77 66 cos 0 ? sin 66 Q3 = 66 0 cos sin 77 66 0 0 1 54 4 sin 0 cos 0 ? sin cos
3 77 77 5
We will require the inverse relation, which is given by
3 2 cos sin sin sin cos 77 66 ? 1 6 Q3 = 6 0 cos ? sin 775 4 ? sin sin cos cos cos
(26)
Using equation (26) we rewrite equation (25) as
1 = (l1es1 + sb21 es2 ) + (l1 sin ? sb21 sin cos ) 0
(? sin es1 + sin cos es2 + cos cos es3 )
4
Note that Q = Q3Q2 Q1
13
(27)
similarly,
2 = (l1es1 ? sb21 es2 ) + (l1 sin + sb21 sin cos ) 0
(? sin es1 + sin cos es2 + cos cos es3 )
3 = (?l2es1 ? sb22 es2 ) + (?l2 sin + sb22 sin cos ) 0
(? sin es1 + sin cos es2 + cos cos es3 )
4 = (?l2 es1 + sb22 es2 ) + (?l2 sin ? sb22 sin cos ) 0
(? sin es1 + sin cos es2 + cos cos es3 )
(28)
and
z r eu3 = ?z r sin es1 + z r sin cos es2 + z r cos cos es3
(29)
The forces Fx eu1 , Fy eu2 and Fz eu2 can be expressed in the basis es as i
i
i
Fx eu1 = Fx (cos es1 + sin sin es2 + sin cos es3 ) Fy eu2 = Fy (cos es2 ? sin es2) Fz eu3 = Fz (? sin es1 + sin cos es2 + cos cos es3 ) i
i
i
i
i
i
(30)
Using equations (24), (27), (28), (29) and (30) we get M s . The derivation of Euler's equation or the sprung mass is thus complete. Using equations (19) and (23), we get Newton's equations.
5 Measurements So far we have written down the equations of motion in terms of the coordinates xr ; y r; z r; ; d ; and . xr is the distance traveled by the road reference frame along the road centerline (which is approximately the distance traveled by the vehicle). y r is the lateral error at the center of mass. z r is the height of the center of mass from the origin of the unsprung mass's reference frame along the eu3 direction. In the absence of superelevation and gradient, this is the verticle height of the center of mass. Euler's angles ( ? d ); and give us the orientation of the sprung mass relative to the road reference frame.
14
In the PATH scenario of AHS, there are various inertial sensors mounted on the vehicle sprung mass. Also, there are magnetometers to give the lateral errors as measured from the road centerline. One such magnetometer is mounted on the front bumper of the car, at a distance ds from the center of mass. The distance of this magnetometer from the road centerline is called ys . It is clear that ys y r + ds( ? d ) Various inertial sensors are given below with the quantity they measure (in terms of the coordinates xr ; y r ; z r ; ; d; and ). The yaw rate sensor measures !3s. There are accelerometers mounted along the es1 and es2 directions on the sprung mass. To express measurements of these accelerometers in terms of the coordinates that we have selected for writing the dynamical equations, we have to represent rs in terms of xr ; y r ; z r ; ; d ; and and in the basis es . Recall equation (19)
rs = (xr ? y_ r _d )er1 = (y r + x_ r _d )er2 = zr er3
(31)
er = (Q3Q2)?1 es
(32)
Also,
Using equation (32) we can rewrite equation (31) as
rs = [(xr ? y_ r _d ) cos ~ cos + (y r + x_ r _d) sin ~ cos ? zr sin ]es1 + [(xr ? y_ r _d )(cos ~ sin sin + sin ~ cos ) + (y r + x_ r _d )(sin ~ sin sin ? cos ~ cos ) + zr sin sin ]es2 + [(xr ? y_ r _d )(cos ~ sin cos + sin ~ sin ) + (y r + x_ r _d )(sin ~ sin cos + cos ~ sin ) + zr cos cos ]es3 a es + a es + a es = 11 22 33
15
(33)
The accelerometer in the direction es1 measures a1 and the accelerometer in the direction es2 measures a2 .
6 Conclusion
Euler-Newton's equations were derived for the four wheeled vehicle from control point of view. The equations of motion give the relative orientation and position of the vehicle with respect to the road. Also, the distance traveled along the road is obtained. The vehicle used had independent suspension at all wheels. Modi cation can be done to accommodate other con gurations. Moreover, the suspensions were assumed to be attached to the sprung mass at the plane (es1; es2) for lack of more information on the suspension geometry. Modi cation in i (equations (27) and (28)) will be needed to take care of other geometries. Importantly, the concept of pitch and roll center was not used. It is necessary to verify that for given vehicle parameters the roll center and pitch center - as determined by simulating the model for roll and pitch disturbances - is in the neighborhood of experimentally determined roll and pitch center. Acknowledgment Authors would like to acknowledge discussions with P. Wong, Dr. C. Chen, Dr. S. Patwardhan and M. Tai which clari ed several of the issues and motivated writing of this Note. 0
References [1] Casey, J., 1983, \A Treatment of Rigid Body Dynamics", Journal of Applied Mechanics, Vol 50, pp. 905-907. [2] Chen, C., 1996, \Backstepping Design of Nonlinear Systems and Its Application to Vehicle Lateral Control in Automated Highway Systems", PhD Thesis, UC Berkeley, CA. [3] Greenwood, D. T., 1988, \Principles of Dynamics", Second Edition, Prentice-Hall, Englewood Cli s, N.J.
16
[4] Peng, H., 1992, \Vehicle Lateral Control for Highway Automation," PhD Thesis, UC Berkeley, CA. [5] Patwardhan, S., 1994, \Fault Detection and Tolerant Control for Lateral Guidance of Vehicles in Automated Highways," PhD thesis, UC Berkeley, CA.
17
