Longitudinal and lateral integrated moving horizon decision making method and apparatus for autonomous vehicle in snowy and icy environment based on trajectory prediction
Provided are a longitudinal and lateral integrated moving horizon decision making method and apparatus for an autonomous vehicle in a snowy and icy environment based on trajectory prediction, with an aim of reducing the complexity of planning a lane-changing trajectory. The method includes: screening out at least one obstacle, planning a lane-changing trajectory according to the at least one obstacle, establishing a decision making model according to the lane-changing trajectory, and controlling the autonomous vehicle by designing a longitudinal and lateral integrated moving horizon decision making controller according to the decision making model. Further provided are a longitudinal and lateral integrated moving horizon decision making apparatus for an autonomous vehicle in a snowy and icy environment based on trajectory prediction and a storage medium.
1. A longitudinal and lateral integrated moving horizon decision making method for an autonomous vehicle in a snowy and icy environment based on trajectory prediction, comprising:
screening out at least one obstacle;
planning a lane-changing trajectory according to the at least one obstacle;
establishing a decision making model according to the lane-changing trajectory; and
controlling the autonomous vehicle by designing a longitudinal and lateral integrated moving horizon decision making controller according to the decision making model;
wherein establishing the decision making model according to the lane-changing trajectory comprises:
determining the decision making model by taking a lane position 1 of the autonomous vehicle and a longitudinal displacement s of the autonomous vehicle as a system state and a system output and by taking a lane-changing time T, a longitudinal velocity v x , and a lane ID lc of a selected lane as input, and wherein the decision making model satisfies that:
{dot over (x)}=A c x+B c ,
y=C c x
wherein
A
c
=
[
0
0
0
-
1
T
]
;
B
c
=
[
1
0
0
1
]
;
C
c
=
[
1
0
0
1
T
]
;
x
=
[
s
l
]
;
u
=
[
v
x
lc
]
;
and
y denotes the system output.
2. The method of claim 1 , wherein screening out the obstacle comprises:
obtaining information of the at least one obstacle through one or a combination of the following devices:
a camera mounted on the autonomous vehicle;
a radar mounted on the autonomous vehicle; and
a map mounted on the autonomous vehicle.
3. The method of claim 2 , wherein the information of the at least one obstacle comprises:
at least one of an obstacle O(n,p) in a front direction or an obstacle O(n,p) in a rear direction in each lane of n l lanes extending in a same direction as a heading direction of the autonomous vehicle, wherein
the O(n,p) comprises a longitudinal distance D(n,p) in a lane direction, a relative velocity V(n,p), and an absolute acceleration a(n,p);
n denotes a lane identifier (ID) of each lane of the n l lanes, and is an integer from 1 to n l ; and
p denotes a relative position of the obstacle, and is equal to f or r, wherein f denotes an obstacle in the front direction, and r denotes an obstacle in the rear direction,
n l denotes a number of lanes; and
wherein screening out the at least one obstacle comprises:
screening out a closest obstacle in the front direction and a closest obstacle in the rear direction according to the following equations:
N
(
n
,
f
)
=
min
j
D
(
j
)
l
j
=
n
,
D
(
j
)
≥
0
N
(
n
,
r
)
=
max
j
D
(
j
)
l
j
=
n
,
D
(
j
)
<
0
,
wherein j denotes an obstacle ID, D (j) denotes a longitudinal distance of an obstacle, N denotes an obstacle ID of a screened-out obstacle, and l j denotes a lane ID of a lane to which the obstacle belongs, and
wherein the lane ID l j of the lane to which the obstacle belongs is determined according to the following equation:
l
n
=
{
n
f
nll
(
X
(
n
,
p
)
)
≤
Y
(
n
,
p
)
<
f
nll
+
1
(
X
(
n
,
p
)
)
other
other
,
wherein
X(n,p) denotes a longitudinal distance of the obstacle;
Y(n,p) denotes a lateral distance of the obstacle;
f nll (X(n,p)) denotes a lateral position of a lane line at a position with a longitudinal distance of X(n,p); and
n u denotes a lane ID of a lane line left to the obstacle, and ranges from 1 to n l in an ascending order from left to right.
4. The method of claim 1 , wherein planning the lane-changing trajectory according to the obstacle comprises:
determining, according to the following equation, a lateral reference trajectory during that the autonomous vehicle is changing a lane:
Y ( t )=∫ 0 t v ( t ) dt,
wherein Y(t) denotes a displacement curve, v(t) denotes a velocity curve, t denotes time, and v(t) is determined according to the following equation:
v ( t )=∫ 0 t a ( t ) dt,
wherein a(t) denotes a function of acceleration of the autonomous vehicle against the time.
5. The method of claim 4 , wherein the function a(t) of the acceleration of the autonomous vehicle against the time is determined according to the following equation:
a
(
t
)
=
{
a
max
t
0
≤
t
<
t
1
J
max
t
1
≤
t
<
t
2
-
J
max
t
-
t
2
t
3
-
t
2
+
J
max
t
2
≤
t
<
t
3
-
J
max
t
-
t
3
t
4
-
t
3
t
3
≤
t
<
t
4
-
J
max
t
4
≤
t
<
t
5
J
max
t
-
t
5
t
6
-
t
5
-
J
max
t
5
≤
t
<
t
6
,
wherein J max denotes an acceleration rate, and a max denotes a maximum lateral acceleration;
t 1 denotes a positive rise time of a(t), t 2 denotes a positive hold time of a(t), t 3 denotes a positive fall time of a(t), t 4 denotes a negative fall time of a(t), t 5 denotes a negative hold time of a(t), and t 6 denotes a negative rise time of a(t), wherein,
the positive rise refers to that the acceleration is larger than 0 and increased, the positive hold refers to that the acceleration is larger than 0 and remains unchanged, the positive fall refers to that the acceleration is larger than 0 and decreased, the negative fall refers to that the acceleration is smaller than 0 and decreased, the negative hold refers to that the acceleration is smaller than 0 and remains unchanged, and the negative rise refers to that the acceleration is smaller than 0 and increased; and
t 2 , t 3 , t 4 , t 5 , and t 6 are determined according to the following equations:
{
t
1
=
1
1
+
k
l
t
l
2
t
2
=
k
l
1
+
k
l
t
l
2
t
3
=
t
1
+
t
2
t
4
=
2
t
1
+
t
2
t
5
=
t
1
+
2
t
2
t
6
=
2
t
1
+
2
t
2
,
wherein t l denotes a lane-changing time, and k l =t 2 :t 1 ; and
t 1 and t 2 are determined according to the following equations:
{
t
1
=
a
max
J
max
t
2
=
-
t
1
2
+
1
2
t
1
2
+
4
d
t
1
J
max
,
wherein d denotes a lane width; and
a max and J max are determined according to the following equations:
{
a
max
=
4
d
(
1
+
k
l
)
t
l
2
k
l
J
max
=
8
d
(
1
+
k
l
)
2
t
l
3
k
l
.
6. The method of claim 1 , wherein,
the lane position l is determined according to a lane ID n u of a lane line left to the obstacle, a lateral displacement y l from the autonomous vehicle to the lane line left to the host autonomous vehicle and a lane width d, and wherein
the lane position l is determined according to the following equation:
l
=
n
ll
+
y
l
d
-
1
2
.
7. The method of claim 1 , wherein controlling the autonomous vehicle by designing the longitudinal and lateral integrated moving horizon decision making controller according to the decision making model, comprises:
discretizing the decision making model;
deriving a prediction equation;
constructing a decision making objective function according to a collision avoidance objective function, a maintaining desired velocity objective function, an objective function for reducing lane changes, and an objective function for reducing lane changing action changes;
determining an optimal decision making according to the prediction equation and an optimization problem; and
controlling the autonomous vehicle according to the optimal decision making,
wherein optimal control variables comprise: an optimal velocity, an optimal lane ID, and optimal lane-changing time.
8. The method of claim 7 , wherein the discretized decision making model satisfies that:
x ( k+ 1)= Ax ( k )+ Bu ( k )
y ( k )= Cx ( k ),
wherein A=e A c T s , B=∫ 0 T s e A c T s dtB c , C=C c , T s denotes discrete time;
C
c
=
[
1
0
0
1
T
]
,
and T denotes lane-changing time;
wherein the prediction equation satisfies that:
{
y
(
k
+
1
)
=
CAx
(
k
)
+
CBu
(
k
)
⋮
y
(
k
+
N
)
=
CA
N
x
(
k
)
+
CA
N
-
1
Bu
(
k
)
+
…
+
CBu
(
k
+
N
-
1
)
⋮
y
(
k
+
P
)
=
CA
P
x
(
k
)
+
CA
P
-
1
Bu
(
k
)
+
…
+
∑
i
=
1
P
-
N
+
1
CA
i
-
1
Bu
(
k
+
N
-
1
)
,
P denotes a prediction horizon, and N denotes a control horizon, and
wherein the collision avoidance objective function satisfies that:
J 1 =−Σ i=1 P ∥TTC f ∥ 2 +∥TTC r ∥ 2 , TTC f denotes time of collision between the autonomous vehicle and a closest obstacle in a front direction, and TTC r denotes time of collision between the autonomous vehicle and a closest obstacle in a rear direction.
9. The method of claim 7 , wherein a desired velocity is maintained by the autonomous vehicle, and the desired velocity is a preset vehicle driving velocity,
wherein
J
2
=
∑
i
=
1
N
Δ
v
x
2
,
Δ
v
x
=
v
ref
-
v
x
,
v
ref
denotes the desired velocity, v x denotes the longitudinal velocity, and N denotes a control horizon; and
wherein an objective function of reducing the lane changing action changes satisfies that:
J
4
=
∑
i
=
1
N
Δ
lc
2
,
Δlc=lc(i)−lc(i−1), lc denotes the lane ID of the selected lane, and Δlc denotes a lane changing action change.
10. A longitudinal and lateral integrated moving horizon decision making apparatus for an autonomous vehicle in a snowy and icy environment based on trajectory prediction, comprising:
a memory, a processor and a user interface, wherein:
the memory is configured to store a computer program;
the user interface is configured to interact with a user; and
the processor is configured to read the computer program in the memory, and execute the computer program to implement:
screening out an obstacle;
planning a lane-changing trajectory according to the obstacle;
establishing a decision making model according to the lane-changing trajectory; and
controlling the autonomous vehicle by designing a longitudinal and lateral integrated moving horizon decision making controller according to the decision making model;
wherein establishing the decision making model according to the lane-changing trajectory comprises:
determining the decision making model by taking a lane position 1 of the autonomous vehicle and a longitudinal displacement s of the autonomous vehicle as a system state and a system output and by taking a lane-changing time T, a longitudinal velocity v x , and a lane ID lc of a selected lane as input, and wherein the decision making model satisfies that:
{dot over (x)}=A c x+B c ,
y=C c x
wherein
A
c
=
[
0
0
0
-
1
T
]
;
B
c
=
[
1
0
0
1
]
;
C
c
=
[
1
0
0
1
T
]
;
x
=
[
s
l
]
;
u
=
[
v
x
l
c
]
;
and
y denotes the system output.