Professional Documents
Culture Documents
Investigation of Automatic Path Tracking Using An Extended Kalman Filter
Investigation of Automatic Path Tracking Using An Extended Kalman Filter
Abstract
This paper proposes a lane marker recognition method that uses the steering angle in addition to image information. A Kalman
lter was recongured regarding the yaw motion and lateral motion of lane markers, previously treated as a stochastic process, as
the states of a vehicle model driven on the basis of the steering angle. Driving tests conducted with an actual vehicle veried that this
method provides good tracking at the time of steering input and avoids misrecognition of lane marker candidate points in inclement
weather. r 2002 Society of Automotive Engineers of Japan, Inc. and Elsevier Science B.V. All rights reserved.
0389-4304/02/$22.00 r 2002 Society of Automotive Engineers of Japan, Inc. and Elsevier Science B.V. All rights reserved.
PII: S 0 3 8 9 - 4 3 0 4 ( 0 1 ) 0 0 1 7 1 - 0 JSAE20024010
62 H. Mouri et al. / JSAE Review 23 (2002) 6167
cf C f Cf
b11 ; b31 ;
IN mN
where cf cr is the distance from the front (rear)
wheels to the vehicles center of gravity, Cf Cr the
cornering power of the front and rear wheels (for
two wheels), m the vehicle mass, y the steering angle,
V the vehicle velocity, I the moment of yaw inertia of
the vehicle, N the steering gear ratio and f the yaw
rate.
Fig. 5. Road model (side view). The behavior of the stochastic state variables x, such
as the road curvature r; pitching angle Z; amount of
bounce h and various disturbances, was approximated
increasing the orders of the model. From Figs. 4 and 5, as a linear system driven by white noise and expressed
the following equations are obtained: as [1,2]
r 2 W x lx qln; 7
X Z fr Z ycr iW; 3
2 2
where l is used to set the frequency region of
Y ZZ h; 4 disturbances applied to the vehicle, q corresponds to
the standard deviation of process noise, and n is
where r is the road curvature, fr the vehicles yaw angle normalized white Gaussian noise.
relative to the center line of the road, ycr the lateral 2 3
displacement at the vehicles center of gravity relative to f.
6 7
the center line of the road, i the lane marker number (left 6 fr 7
6 7
side=0; right side=1), W the lane width, Z the distance 6 y. 7
6 cr 7
ahead of the vehicle from the lens center, X the distance 6 7
6 ycr 7
to the left side of the vehicle from the lens center, Z the 6 7
6 r 7
vehicle pitching angle and h the height from the road 6 7
6 7
surface to the lens center. 6 Z 7
6 7
From Eqs. (1)(4), Eq. (5) is obtained as the out- 6 7
6 h 7
put equation when an extended Kalman lter is 6 7
4 5
6M 7
formulated. The x-coordinate is calculated with this
equation using the state quantities of the vehicle, road F
conguration and the y-coordinate in the CCD camera 2 3
a11 a12 a13 0 0 0 0 1=I 0
image. 6 1
6 0 0 0 V 0 0 0 0 77
ycr W=2 W f 2 hr 6 7
x i y f Z f fr : 5 6 a31 a32 a33 0 V 2 0 0 0 1=m 7
h h 2y f Z 6 7
6 7
6 0 0 1 0 0 0 0 0 0 7
The objective of this investigation was to estimate the 6 7
6 0 0 0 0 lr 0 0 0 0 7
road curvature and state quantities of the vehicle from 6 7
6 7
6 0 0 0 0 0 lZ 0 0 0 7
the pair of (x; y) coordinates and the steering angle. 6 7
Based on a general two-wheel model, the vehicle model 6 0 0 0 0 0 0 lh 0 0 7
6 7
was described with the following equations of state: 6 7
4 0 0 0 0 0 0 0 lM 0 5
f. a11 f a12 f a13 yc b11 y; 0 0 0 0 0 0 0 0 lF
2 3 2 3
y.c a31 f a32 f a33 yc b31 y; 6 f b11
6 7 6 0 7
f
6 r7 6 7
where each of the coecients is determined by the 6 7 6 7
6 ycr 7 6 b31 7
vehicle specications. 6 7 6 7
6 7 6 7
6 ycr 7 6 0 7
c2f Cf c2r Cr cf Cf cr Cr 6 7 6 7
a11 ; a12 ; 6 7 6
6 r 7 6 0 7y
7
VI I 6 7 6 7
cf Cf cr Cr 6 Z 7 6 0 7
a13 ; 6 7 6 7
VI 6 h 7 6 0 7
6 7 6 7
6 7 6 7
cf Cf cr Cr Cf Cr Cf Cr 4M5 4 0 5
a31 ; a32 ; a33 ; F 0
mV m mV
64 H. Mouri et al. / JSAE Review 23 (2002) 6167
2 3
0 0 0 0 0
6 0 Pk 1 As MkATs GQGT ; 14
6 0 0 0 0 7 7
6 7 2 3
6 0 0 0 0 0 7 nr Mk PkfI CT CPkC T R1 CPkg; 15
6 7
6 7 6 7
6 0 0 0 0 0 7 6 nZ 7
6 7 6 7 where Mk is the covariance of the state determined by
6
6 qr lr 0 0 0 0 7 7
6 nh 7 :
6 7 8 the process, Pk the covariance of the state after
6 7 6 7
6 0 qZ lZ 0 0 0 7 4 nM 5 obtaining the observed value and Kk the Kalman lter
6 7
6 0 0 qh lh 0 0 7 nF gain.
6 7
6 7 @gxs k; yk
4 0 0 0 qM l M 0 5
CD :
0 0 0 0 qF lF @xs
f ycr ycr ;
x f z z1 ; yT ;
w r M FT ; u y; weighting of the lateral displacement. In this way,
frequency adjustment is also possible in addition to
2 3 2 3 scalar weighting. The feedback gain is given as K
a11 a12 a13 0 0 1=I 0
6 1 BT2 H and H is the positive solution to the following
6 0 0 07
7
6 V
6 0 0 77
A6 7; B1 6 7; Riccati equation:
4 a31 a32 a33 05 4 V 2 0 1=m 5
0 0 0 0 0 0 0 AT H HA HB2 BT2 H C1T C1 0 18
2 3
b11 Fig. 9 shows an example of the lane markers used in a
6 0 7 simulation of a vehicle traveling along a leftward curve.
6 7
B2 6 7; The shape and position of the lane markers projected
4 b31 5
onto the screen are suitably changed according to the
0 road curvature and state quantities of the host vehicle,
" # " #
0 0 0 Q1=2 0 similar to the situation for an actual vehicle. The
C1 ; D12 ; D11 023 : simulation results are shown in Fig. 10, which compares
0 0 0 0 1
the values estimated with the Kalman lter and the real
17 state quantities used in the simulation. This example is
Equation (17) congures a generalized plant that is for automatic steering of the host vehicle as it enters a
equivalent to the optimum regulator and Q is the curve without any easement curve. The actual and
H. Mouri et al. / JSAE Review 23 (2002) 6167 67
estimated state quantities show good agreement even both capabilities was conrmed by theoretical calcula-
though a relatively fast steering angle was input. tions and the results of actual driving tests.
(2) An optimum steering control system is also
proposed which is constructed such that the image
6. Conclusions
processing system directly estimates the dynamic char-
acteristics of the host vehicle and uses the results for
This paper proposed a new path tracking control
steering control. The eectiveness of this system was
system that incorporates an image processing system
veried by computer simulations.
for detecting forward lane markers by means of a
vehicle-mounted camera. The following new tracking
method is proposed from the standpoint of modern
control theory: References
(1) The new image processing system proposed here [1] Mouri, H., Furusho, H., Research on automatic lane tracking
incorporates a method for considering the host vehicles using linear quadratic (LQ) control: control procedure for a curved
dynamic characteristics into an extended Kalman lter, path (II) (in Japanese with English summary), Proc. Spring
which is used for estimating the road curvature and state Convention of JSAE, Vol. 103, No. 972 (1997).
quantities of the vehicle. By using steering angle [2] Mouri, H., Furusho, H., Research on lane tracking control (in
Japanese with English summary), Trans. JSAE, Vol. 30, No. 2,
information, this method makes it possible to provide (1999).
both good convergence of the estimated values and [3] Kimura, H., Digital Signal Processing and Control (in Japanese),
improved anti-noise performance. The attainment of Shokodo, Tokyo, 1982.