Model based predictive control for automated lane centering/changing control systems
A system and method for providing steering control for lane changing and lane centering purposes in an autonomous or semi-autonomous vehicle system. A vehicle vision system calculates roadway lane marking information, such as lateral offset, yaw angle and roadway curvature with respect to the vehicle's centered coordinate system. The roadway is then modeled as a second order polynomial equation. The method then predicts roadway lateral position and yaw angle over a pre-defined lane change completion time using a vehicle dynamic model. The method then compares a predicted vehicle path with a desired vehicle path to generate an error value, and calculates a steering angle command to minimize the error value, where the steering angle command is calculated as a function of vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle. The steering angle command is then sent to the vehicle steering system.
1. A method for providing vehicle steering control in an autonomous or semi-autonomous vehicle driving system, said method comprising:
determining a lateral position of the vehicle, a lateral speed of the vehicle, a yaw rate of the vehicle and a yaw angle of the vehicle at a particular point in time or over a predetermined time period;
determining roadway lane marking information of a roadway on which the vehicle is traveling;
modeling the roadway using the roadway lane marking information, wherein modeling the roadway includes modeling the roadway as a second order polynomial equation;
predicting a vehicle path using the roadway model;
providing a desired vehicle path;
comparing the predicted vehicle path and the desired vehicle path to generate a path error value; and
generating a steering angle command based on the path error value where the steering angle command is determined by a function that includes the vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle.
2. The method according to claim 1 wherein determining roadway lane marking information includes determining the vehicle lateral position, roadway curvature and the vehicle yaw angle.
3. The method according claim 1 wherein predicting the vehicle path using the roadway model includes predicting the roadway lateral position and the vehicle yaw angle.
4. The method according to claim 3 wherein predicting the roadway lateral position and the vehicle yaw angle includes using the equation:
x
.
r
=
A
r
x
r
+
B
r
δ
+
G
r
ρ
z
r
=
C
r
x
r
where
:
x
r
=
[
y
r
φ
r
v
y
r
]
T
B
r
=
[
0
0
C
f
m
aC
f
I
]
T
C
r
=
[
1
0
0
0
0
1
0
0
]
G
r
=
[
0
v
x
0
0
]
T
A
r
=
[
0
v
x
-
1
0
0
0
0
-
1
0
0
-
C
f
+
C
r
mv
x
bC
r
-
aC
f
mv
x
-
v
x
0
0
bC
r
-
aC
f
Iv
x
-
a
2
C
f
+
b
2
C
r
Iv
x
]
and where C f and C r are the concerning stiffnesses of front wheels and rear wheels of the vehicle, respectively, a and b are the distances from the center of gravity of the vehicle to the front and rear axles, respectively, m is the vehicle mass, δ is the steering angle and I z is the moment of inertia around the center of mass of the vehicle perpendicular to the plane where the vehicle is located.
5. The method according to claim 1 wherein providing the desired vehicle path includes providing the desired vehicle path using a roadway lateral offset of the vehicle and a vehicle heading angle.
6. The method according to claim 5 wherein providing a desired vehicle path includes generating a fifth order polynomial path based on the equation:
y d ( t )= a 5 x d 5 ( t )+ a 4 x d 4 ( t )+ a 3 x d 3 ( t )+ a 2 x d 2 ( t )+ a 1 x d 1 ( t )+ a 0
where y is the lateral offset.
7. The method according to claim 1 wherein providing a steering angle command includes providing a steering angle command defined by the equation:
δ
cmd
(
k
)
=
∑
i
=
0
N
-
1
(
z
d
(
k
+
i
+
1
)
-
CA
i
+
1
x
(
k
)
)
T
Q
(
k
+
i
+
1
)
(
CA
i
B
)
∑
i
=
0
N
-
1
(
CA
i
B
)
Q
(
k
+
i
+
1
)
(
CA
i
B
)
+
R
(
k
)
where x=[yφv y r] T , z d (k)=[y d φ d ] T , Q and R are weighting matrices used in the minimization with system matrices definitions
A
=
ⅇ
A
r
t
s
,
B
=
∫
0
t
s
ⅇ
A
r
α
B
r
ⅆ
α
and
C
=
[
1
0
0
0
0
1
0
0
]
,
y is lateral position, φ is yaw angle, v y is lateral speed and r is yaw rate.
8. A method for providing vehicle steering control in an autonomous or semi-autonomous vehicle driving system, said method comprising:
determining roadway lane marking information of a roadway on which the vehicle is traveling including determining vehicle lateral position, roadway curvature and vehicle yaw angle;
modeling the roadway as a second order polynomial equation using the roadway lane marking information;
predicting a vehicle path using the roadway model including predicting a roadway lateral offset and a vehicle yaw angle;
providing a desired vehicle path;
comparing the predicted vehicle path and the desired vehicle path to generate a path error value; and
generating a steering angle command based on the path error value, wherein providing a steering angle command includes providing a steering angle command defined by the equation:
δ
cmd
(
k
)
=
∑
i
=
0
N
-
1
(
z
d
(
k
+
i
+
1
)
-
CA
i
+
1
x
(
k
)
)
T
Q
(
k
+
i
+
1
)
(
CA
i
B
)
∑
i
=
0
N
-
1
(
CA
i
B
)
Q
(
k
+
i
+
1
)
(
CA
i
B
)
+
R
(
k
)
where x=[yφv y ] T , z d (k)=[y d φ d ] T , Q and R are weighting matrices used in the minimization with system matrices definitions
A
=
ⅇ
A
r
t
s
,
B
=
∫
0
t
s
ⅇ
A
r
α
B
r
ⅆ
α
and
C
=
[
1
0
0
0
0
1
0
0
]
,
y is lateral position, φ is yaw angle, v y is lateral speed and r is yaw rate.
9. The method according to claim 8 wherein predicting a roadway lateral position and vehicle yaw angle includes using the equation:
x
.
r
=
A
r
x
r
+
B
r
δ
+
G
r
ρ
z
r
=
C
r
x
r
where
:
x
r
=
[
y
r
φ
r
v
y
r
]
T
B
r
=
[
0
0
C
f
m
aC
f
I
]
T
C
r
=
[
1
0
0
0
0
1
0
0
]
G
r
=
[
0
v
x
0
0
]
T
A
r
=
[
0
v
x
-
1
0
0
0
0
-
1
0
0
-
C
f
+
C
r
mv
x
bC
r
-
aC
f
mv
x
-
v
x
0
0
bC
r
-
aC
f
Iv
x
-
a
2
C
f
+
b
2
C
r
Iv
x
]
and where C f and C r are the concerning stiffnesses of front wheels and rear wheels of the vehicle, respectively, a and b are the distances from the center of gravity of the vehicle to the front and rear axles, respectively, m is the vehicle mass, δ is the steering angle and I z is the moment of inertia around the center of mass of the vehicle perpendicular to the plane where the vehicle is located.
10. The method according to claim 8 wherein determining a desired vehicle path includes generating a fifth order polynomial path based on the equation:
y d ( t )= a 5 x d 5 ( t )+ a 4 x d 4 ( t )+ a 3 x d 3 ( t )+ a 2 x d 2 ( t )+ a 1 x d 1 ( t )+ a 0
where y is the lateral offset.
11. A system for providing vehicle steering control in an autonomous or semi-autonomous vehicle driving system, said system comprising:
means for determining a lateral position of the vehicle, a lateral speed of the vehicle, a yaw rate of the vehicle and a yaw angle of the vehicle at a particular point in time or over a predetermined time period:
means for determining roadway lane marking information of a roadway on which the vehicle is traveling;
means for modeling the roadway using the roadway lane marking information that models the roadway as a second order polynomial equation;
means for predicting a vehicle path using the roadway model;
means for providing a desired vehicle path;
means for comparing the predicted vehicle path and the desired vehicle path to generate a path error value; and
means for generating a steering angle command based on the path error value where the steering angle command is determined by a function that includes the vehicle lateral position, vehicle lateral speed, vehicle yaw rate and vehicle yaw angle.
12. The system according to claim 11 wherein the means for determining roadway lane marking information determines the vehicle lateral position, roadway curvature and the vehicle yaw angle.
13. The system according to claim 11 wherein the means for predicting the vehicle path predicts a roadway lateral position and the vehicle yaw angle over a predetermined steering completion time.
14. The system according to claim 13 wherein the means for predicting the roadway lateral position and the vehicle yaw angle uses the equation:
x
.
r
=
A
r
x
r
+
B
r
δ
+
G
r
ρ
z
r
=
C
r
x
r
where
:
x
r
=
[
y
r
φ
r
v
y
r
]
T
B
r
=
[
0
0
C
f
m
aC
f
I
]
T
C
r
=
[
1
0
0
0
0
1
0
0
]
G
r
=
[
0
v
x
0
0
]
T
A
r
=
[
0
v
x
-
1
0
0
0
0
-
1
0
0
-
C
f
+
C
r
mv
x
bC
r
-
aC
f
mv
x
-
v
x
0
0
bC
r
-
aC
f
Iv
x
-
a
2
C
f
+
b
2
C
r
Iv
x
]
and where C f and C r are the concerning stiffnesses of front wheels and rear wheels of the vehicle, respectively, a and b are the distances from the center of gravity of the vehicle to the front and rear axles, respectively, m is the vehicle mass, δ is the steering angle and I z is the moment of inertia around the center of mass of the vehicle perpendicular to the plane where the vehicle is located.
15. The system according to claim 11 wherein the means for providing the desired vehicle path provides the desired vehicle path using a roadway lateral offset of the vehicle and a vehicle heading angle.
16. The system according to claim 11 wherein the means for determining a desired vehicle path includes generating a fifth order polynomial path based on the equation:
y d ( t )= a 5 x d 5 ( t )+ a 4 x d 4 ( t )+ a 3 x d 3 ( t )+ a 2 x d 2 ( t )+ a 1 x d 1 ( t )+ a 0
where y is the lateral offset.
17. The system according to claim 11 wherein the means for providing a steering angle command provides a steering angle command defined by the equation:
δ
cmd
(
k
)
=
∑
i
=
0
N
-
1
(
z
d
(
k
+
i
+
1
)
-
CA
i
+
1
x
(
k
)
)
T
Q
(
k
+
i
+
1
)
(
CA
i
B
)
∑
i
=
0
N
-
1
(
CA
i
B
)
Q
(
k
+
i
+
1
)
(
CA
i
B
)
+
R
(
k
)
where x=[yφv y r] T , z d (k)=[y d φ d ] T , Q and R are weighting matrices used in the minimization with system matrices definitions
A
=
ⅇ
A
r
t
s
,
B
=
∫
0
t
s
ⅇ
A
r
α
B
r
ⅆ
α
and
C
=
[
1
0
0
0
0
1
0
0
]
,
y is lateral position, φ is yaw angle, v y is lateral speed and r is yaw rate.