Multi-sensor fusion method and system for intelligent driving vehicles
A multi-sensor fusion method and system for intelligent driving vehicles are provided, which relate to the technical field of intelligent driving vehicles. The method includes: establishing an extended target tracker based on a GM-PHD algorithm and a rectangular target model of a detected object; processing detection information of a millimeter-wave radar by using the extended target tracker to obtain millimeter-wave radar track information of the detected object; processing detection information of a laser radar by using the established bounding-box detector and a JPDA tracker provided with an IMM-UKF to obtain laser-radar track information of the detected object; processing the millimeter-wave radar track information and the laser-radar track information by performing time-space conversion to obtain a central fusion node; and processing the central fusion node by using an IMF algorithm to obtain global track information.
1 . A multi-sensor fusion method for intelligent driving vehicles, comprising:
S1: establishing an extended target tracker based on a Gaussian mixture probability hypothesis density (GM-PHD) algorithm and a rectangular target model of a detected object, and processing two-dimensional detection information of a millimeter-wave radar by using the extended target tracker to obtain millimeter-wave radar track information of the detected object;
S2: establishing a bounding-box detector and establishing a joint probabilistic data association (JPDA) tracker provided with an interacting multiple model-unscented Kalman filter (IMM-UKF), and processing three-dimensional detection information of a laser radar by using the bounding-box detector and the JPDA tracker provided with the IMM-UKF to obtain laser radar track information of the detected object; and
S3: processing the millimeter-wave radar track information and the laser radar track information by performing time-space conversion to obtain a central fusion node, processing the central fusion node by using an information matrix fusion (IMF) algorithm to obtain global track information, tracking the detected object based on the global track information, and controlling a driving process of the intelligent driving vehicles based on the global track information;
wherein the establishing an extended target tracker comprises:
obtaining a rectangular extended target state of the detected object based on the rectangular target model of the detected object; and
calculating, based on the rectangular extended target state of the detected object, a multi-target predicted PHD at a time instant k and a multi-target posterior PHD at the time instant k by using the GM-PHD algorithm to obtain the extended target tracker.
2 . The multi-sensor fusion method for intelligent driving vehicles according to claim 1 , wherein the rectangular extended target state is expressed as:
ξ
=
(
γ
,
x
,
X
)
where ξ represents a state of a detected-object extended target, y represents a measurement rate state of the detected-object extended target, x represents a motion state of the detected-object extended target, and X represents an extended state of the detected-object extended target.
3 . The multi-sensor fusion method for intelligent driving vehicles according to claim 1 , wherein the establishing a bounding-box detector comprises:
pre-processing laser radar data by using a random sample consensus (RANSAC) based plane fitting algorithm to obtain target point clouds;
clustering the target point clouds by using an Euclidean algorithm; and
establishing, based on clustered target point clouds, a state vector of the bounding-box detector to obtain the bounding-box detector.
4 . The multi-sensor fusion method for intelligent driving vehicles according to claim 3 , wherein the state vector of the bounding-box detector is expressed as:
x
′
=
[
x
,
y
,
v
,
θ
,
ω
,
z
,
z
.
,
L
,
W
,
H
]
T
where x′ represents the state vector, x represents an abscissa of a detected target, y represents an ordinate of the detected target, v represents a speed of the detected target, θ represents a direction angle of the detected target, ω represents an angular speed of the detected target, z represents vertical coordinates of the detected target, Ż represents a vertical speed of the detected target, L represents a length of the detected target, W represents a width of the detected target, and H represents a height of the detected target.
5 . The multi-sensor fusion method for intelligent driving vehicles according to claim 1 , wherein the establishing a JPDA tracker provided with an IMM-UKF comprises:
configuring the JPDA tracker provided with the IMM-UKF to comprise an input interaction module, a UKF filtering module, a probability updating module, a JPDA data association module and an output fusion module;
calculating, by the input interaction module based on a first state estimation and a first covariance matrix of a UKF filter in the UKF filtering module at a time instant k, a second state estimation and a second covariance matrix, and outputting, by the input interaction module, the second state estimation and the second covariance matrix;
outputting, by the UKF filter in the UKF filtering module based on an output of the input interaction module and an effective observation vector at the time instant k, a third state estimation and a third covariance matrix at a time instant k+1;
calculating, by the probability updating module based on residual information of the UKF filtering module, a conditional probability of a motion model at the time instant k+1;
calculating, by the JPDA data association module based on the third state estimation, the third covariance matrix, and first measurement information of a target under the motion model, second measurement information of the target under the motion model at the time instant k+1; and
calculating, by the output fusion module based on the conditional probability of the motion model at the time instant k+1, the second measurement information, the third state estimation and the third covariance matrix, a fused state estimation and a fused covariance matrix.
6 . The multi-sensor fusion method for intelligent driving vehicles according to claim 1 , wherein the processing the central fusion node by using an IMF algorithm comprises:
updating a covariance by using the following equation:
P
(
k
❘
"\[LeftBracketingBar]"
k
)
=
P
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
-
1
+
∑
i
=
1
N
k
(
P
i
(
k
❘
"\[LeftBracketingBar]"
k
)
-
1
-
P
i
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
-
1
)
-
1
;
and
updating a state estimation by using the following equation:
x
ˆ
(
k
❘
"\[LeftBracketingBar]"
k
)
=
P
(
k
❘
"\[LeftBracketingBar]"
k
)
(
P
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
-
1
x
ˆ
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
)
+
∑
i
=
1
N
k
(
P
i
(
k
❘
"\[LeftBracketingBar]"
k
)
-
1
x
ˆ
i
(
k
❘
"\[LeftBracketingBar]"
k
)
-
P
i
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
-
1
x
ˆ
i
(
k
❘
"\[LeftBracketingBar]"
k
-
1
)
)
,
where P(k|k) represents a global covariance of sensors from a time instant 0 to a time instant k, P(k|k−1) represents a global covariance of the sensors from the time instant 0 to a time instant k−1, P i (k|k) represents a local covariance of an i-th sensor from the time instant 0 to the time instant k, P i (k|k−1) represents a local covariance of the i-th sensor from the time instant 0 to the time instant k−1, N k represents the number of the sensors, {circumflex over (x)}(k|k) represents a global state estimation of the sensors from the time instant 0 to the time instant k, {circumflex over (x)}(k|k−1) represents a global state estimation of the sensors from the time instant 0 to the time instant k−1, {circumflex over (x)} i (k|k) represents a local state estimation of the i-th sensor from the time instant 0 to the time instant k, and {circumflex over (x)} i (k|k−1) represents a local state estimation of the i-th sensor from the time instant 0 to the time instant k−1.