CN115808682A - Stable multi-target tracking method and system based on vehicle-mounted millimeter wave radar - Google Patents

Stable multi-target tracking method and system based on vehicle-mounted millimeter wave radar Download PDF

Info

Publication number
CN115808682A
CN115808682A CN202310059397.0A CN202310059397A CN115808682A CN 115808682 A CN115808682 A CN 115808682A CN 202310059397 A CN202310059397 A CN 202310059397A CN 115808682 A CN115808682 A CN 115808682A
Authority
CN
China
Prior art keywords
track
motion
model
current
moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310059397.0A
Other languages
Chinese (zh)
Other versions
CN115808682B (en
Inventor
张正文
汪俊延
廖桂生
汪福林
徐开彦
张峻嘉
张振平
熊小泽
汪震
刘永康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hubei University of Technology
Original Assignee
Hubei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hubei University of Technology filed Critical Hubei University of Technology
Priority to CN202310059397.0A priority Critical patent/CN115808682B/en
Publication of CN115808682A publication Critical patent/CN115808682A/en
Application granted granted Critical
Publication of CN115808682B publication Critical patent/CN115808682B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention provides a stable multi-target tracking method and a system based on a vehicle-mounted millimeter wave radar, wherein the method comprises the following steps: obtaining a target data set based on a vehicle-mounted millimeter wave radar; the target data set comprises a plurality of target points and measurement information of each target point; acquiring a track list, wherein the track list comprises a plurality of tracks and last-time state information of the tracks; acquiring the motion direction of the track, determining a motion model based on the motion direction of the track, and acquiring the current moment prediction state information of the track; determining a shape of the associated gate and a threshold condition of the associated gate based on the direction of motion of the trajectory; performing target association on the target data set and the track based on the associated wave gate and the threshold condition, and judging whether the association is successful; and when the association is successful, performing interactive multi-model filtering estimation on the track to obtain the state information of the track at the current moment, and updating the motion direction of the track. The invention realizes accurate and stable multi-target tracking under complex environment.

Description

Stable multi-target tracking method and system based on vehicle-mounted millimeter wave radar
Technical Field
The invention relates to the technical field of multi-target tracking, in particular to a stable multi-target tracking method and system based on a vehicle-mounted millimeter wave radar.
Background
The vehicle-mounted millimeter wave radar takes other vehicles on a road and the surrounding environment as targets, utilizes electromagnetic waves as a detection carrier, accurately measures the distance, the speed and the angle of a target vehicle on the road to determine the position and the motion information of the target to form a target point, clusters and agglomerates the mass center of the target point to obtain an observed value of the target, establishes a tracker, and uses a filtering algorithm to filter the observed value to obtain a target tracking result.
The actual application scene and the target motion characteristic of the vehicle-mounted millimeter wave radar are complex, usually a tracker with an Interactive Multiple Model (IMM) structure is adopted for multi-target tracking, but because the millimeter wave radar adopts a doppler principle for target speed measurement, the speed detection precision of a transverse motion target and a turning target is reduced, so that the tracking precision is reduced, and track fracture occurs.
Therefore, it is urgently needed to provide a stable multi-target tracking method and system based on a vehicle-mounted millimeter wave radar, so as to realize accurate and stable multi-target tracking in a complex environment.
Disclosure of Invention
In view of this, a stable multi-target tracking method and system based on a vehicle-mounted millimeter wave radar are needed to be provided to solve the technical problem that multi-target tracking is prone to track terminal discontinuity under a complex environment, and therefore multi-target tracking accuracy and stability are low in the prior art.
On one hand, the invention provides a stable multi-target tracking method based on a vehicle-mounted millimeter wave radar, which comprises the following steps:
obtaining current point cloud data at the current moment based on a vehicle-mounted millimeter wave radar, and clustering the current point cloud data to obtain a target data set; the target data set comprises a plurality of target points and measurement information of the target points;
acquiring a track list corresponding to previous point cloud data at a previous moment; the track list comprises a plurality of tracks and last-time state information of the tracks;
acquiring the motion direction of the track and the state information of the previous moment, determining a motion model based on the motion direction of the track, and performing track prediction on the track based on the motion model and the state information of the previous moment to acquire the prediction state information of the current moment of the track;
determining the shape of an associated gate based on the motion direction of the track, and determining the threshold condition of the associated gate based on the measurement information and the current time prediction state information;
performing target association on the target data set and the track based on the association wave gate and the threshold condition, and judging whether the association is successful;
and when the association is successful, performing interactive multi-model filtering estimation on the track to obtain the current moment state information of the track, and updating the motion direction of the track based on the current moment state information and the last moment state information of the track.
In some possible implementations, the motion model includes a first motion sub-model and a second motion sub-model; the determining a motion model based on the motion direction of the trajectory comprises:
when the motion direction of the track is a straight motion track, a transverse motion track or an unknown motion track, the first motion sub-model is a constant speed motion model, and the second motion sub-model is a constant acceleration motion model;
and when the motion direction of the track is a turning motion track, the first motion sub-model is a constant speed motion model, and the second motion sub-model is a constant turning speed motion model.
In some possible implementation manners, the current-time prediction state information is:
Figure SMS_1
in the formula ,
Figure SMS_2
predicting state information for the current time;
Figure SMS_3
state information at the last moment;
Figure SMS_4
a matrix of a first motion sub-model;
Figure SMS_5
the model probability of the first motion sub-model at the current moment is obtained;
Figure SMS_6
a matrix of a second motion sub-model;
Figure SMS_7
the model probability of the second motion sub-model at the current moment.
In some possible implementations, the determining the shape of the associated wave gate based on the motion direction of the trajectory includes:
when the motion direction of the track is a straight motion track or a transverse motion track, the shape of the associated wave gate is rectangular;
when the motion direction of the track is a turning motion track, the shape of the associated wave gate is a positive direction;
and when the motion direction of the track is an unknown motion track, the shape of the associated wave gate is circular.
In some possible implementations, the measurement information includes a distance, a radial velocity, and an angle, and the current-time prediction state information includes a predicted lateral coordinate, a predicted longitudinal coordinate, a predicted lateral motion velocity, a predicted longitudinal motion velocity, a predicted distance, a predicted radial velocity, and a predicted angle; when the shape of the associated wave gate is rectangular or square, the threshold condition is:
Figure SMS_8
when the shape of the associated gate is circular, the threshold condition is:
Figure SMS_9
in the formula ,
Figure SMS_13
is a distance;
Figure SMS_14
is the radial velocity;
Figure SMS_20
is an angle;
Figure SMS_18
to predict the lateral coordinates;
Figure SMS_24
to predict the longitudinal coordinates;
Figure SMS_16
predicting the transverse movement speed;
Figure SMS_25
to predict the longitudinal movement velocity;
Figure SMS_17
is a predicted distance;
Figure SMS_26
to predict radial velocity;
Figure SMS_10
is a predicted angle;
Figure SMS_19
is a lateral distance threshold;
Figure SMS_11
is a longitudinal distance threshold;
Figure SMS_22
is a lateral velocity threshold;
Figure SMS_12
is a longitudinal speed threshold;
Figure SMS_23
is a distance threshold;
Figure SMS_15
is an angle threshold;
Figure SMS_21
is a speed threshold.
In some possible implementations, the interactive multi-model includes a plurality of kalman filters constructed based on the motion model; the interactive multi-model filtering estimation of the track to obtain the current moment state information of the track comprises the following steps:
acquiring last-time state estimation and last-time model probability of the Kalman filter;
obtaining model transition probabilities of the Kalman filters, and determining a mixing probability of the Kalman filters based on the model transition probabilities and the model probability at the last moment;
determining a last-time hybrid state estimate and a last-time hybrid covariance estimate matrix for the Kalman filter based on the last-time state estimate and the hybrid probability;
filtering the track based on the last-moment mixed state estimation and the last-moment mixed covariance estimation matrix to obtain a current-moment state estimation and a current-moment covariance matrix;
acquiring a likelihood function, and determining the current moment model probability of the Kalman filter based on the likelihood function;
determining a total state estimate and a total covariance matrix for the interactive multi-model based on the current-time model probability, the current-time state estimate, and the current-time covariance matrix.
In some possible implementations, the overall state estimate is:
Figure SMS_27
the total covariance matrix is:
Figure SMS_28
Figure SMS_29
wherein ,
Figure SMS_30
Figure SMS_31
Figure SMS_32
Figure SMS_33
Figure SMS_34
Figure SMS_35
Figure SMS_36
Figure SMS_37
Figure SMS_38
Figure SMS_39
Figure SMS_40
in the formula ,
Figure SMS_47
estimating the total state;
Figure SMS_44
is a total covariance matrix;
Figure SMS_56
estimating the state at the current moment;
Figure SMS_46
the model probability at the current moment;
Figure SMS_54
is a transposed symbol;
Figure SMS_49
is a covariance matrix at the current moment;
Figure SMS_55
is a likelihood function;
Figure SMS_45
is the mixing probability;
Figure SMS_59
the model transition probability;
Figure SMS_41
is the model probability of the last moment;
Figure SMS_60
is a normalized probability;
Figure SMS_43
is a normalization constant;
Figure SMS_53
measuring information;
Figure SMS_42
is a prediction error covariance matrix;
Figure SMS_52
estimating for a prediction state;
Figure SMS_50
is a measurement matrix;
Figure SMS_58
estimating the state of the last time;
Figure SMS_51
an observed noise covariance matrix at the current moment;
Figure SMS_57
estimating the mixing state at the last moment;
Figure SMS_48
the matrix is estimated for the hybrid covariance at the previous time.
In some possible implementations, the updating the motion direction of the track based on the current time state information and the last time state information of the track includes:
determining the current transverse displacement and the current longitudinal displacement of the track at the current moment based on the current moment state information and the last moment state information of the track;
acquiring a plurality of historical transverse displacement amounts and a plurality of historical longitudinal displacement amounts of the track at a plurality of historical moments;
when the existing frame number of the track is larger than a preset frame number, determining an average transverse displacement amount based on the current transverse displacement amount and the plurality of historical transverse displacement amounts, and determining an average longitudinal displacement amount based on the current longitudinal displacement amount and the plurality of historical longitudinal displacement amounts;
determining a displacement scaling factor based on the average longitudinal displacement amount and the average lateral displacement amount;
and updating the motion direction of the track based on the displacement scale factor.
In some possible implementations, the updating the motion direction of the trajectory based on the displacement scale factor includes:
when the displacement scale factor is smaller than a first threshold value, the motion direction of the track is a straight motion track;
when the displacement scale factor is larger than or equal to the first threshold and smaller than a second scale threshold, the motion direction of the track is a turning motion track;
and when the displacement scale factor is larger than or equal to the second threshold value, the track motion direction is a transverse moving track.
On the other hand, the invention also provides a stable multi-target tracking system based on the vehicle-mounted millimeter wave radar, which comprises the following components:
the system comprises a target data set acquisition unit, a target data set acquisition unit and a data processing unit, wherein the target data set acquisition unit is used for acquiring current point cloud data at the current moment based on a vehicle-mounted millimeter wave radar and clustering the current point cloud data to acquire a target data set; the target data set comprises a plurality of target points and measurement information of the target points;
the track list acquiring unit is used for acquiring a track list corresponding to previous point cloud data at a previous moment; the track list comprises a plurality of tracks and last-time state information of the tracks;
the state information prediction unit is used for acquiring the motion direction of the track, determining a motion model based on the motion direction of the track, and performing track prediction on the track based on the motion model and the state information of the previous moment to acquire the current moment prediction state information of the track;
a correlation gate determining unit, configured to determine a shape of a correlation gate based on a motion direction of the trajectory, and determine a threshold condition of the correlation gate based on the measurement information and the current time prediction state information;
the association unit is used for carrying out target association on the target data set and the track based on the association wave gate and the threshold condition and judging whether the association is successful or not;
and the track updating unit is used for carrying out interactive multi-model filtering estimation on the track when the association is successful, obtaining the current moment state information of the track, and updating the motion direction of the track based on the current moment state information of the track and the last moment state information of the track.
The beneficial effects of adopting the above embodiment are: according to the stable multi-target tracking method based on the vehicle-mounted millimeter wave radar, the shape of the associated wave gate is determined based on the motion direction of the track, the threshold condition of the associated wave gate is determined based on the measurement information and the current-time prediction state information, association errors or association failures caused by adoption of unified associated wave gates and threshold conditions are avoided, the problem of track interruption is avoided, and the stability and the accuracy of multi-target tracking are improved. Furthermore, the motion model is determined based on the motion direction of the track, so that the defect of reduced tracking precision caused by excessive competition of the motion model can be avoided, the multi-target tracking precision is further improved, the problem of track interruption caused by low tracking precision is further solved, and the stability of multi-target tracking is further improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a schematic flow chart of an embodiment of a stable multi-target tracking method based on a vehicle-mounted millimeter wave radar according to the present invention;
FIG. 2 is a flowchart illustrating an embodiment of determining a motion model based on a track-based motion direction in S103;
FIG. 3 is a flowchart illustrating an embodiment of determining the shape of the associated wave gate based on the track-based motion direction in S104;
fig. 4 is a schematic flowchart of an embodiment of performing interactive multi-model filtering estimation on a trajectory to obtain state information of the trajectory at the current time in S106;
fig. 5 is a flowchart illustrating an embodiment of updating the moving direction of the track based on the current time status information and the previous time status information of the track in S106;
FIG. 6 is a schematic diagram illustrating the structure of S505 in FIG. 5 according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of an embodiment of the stable multi-target tracking system based on the vehicle-mounted millimeter wave radar provided by the invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be understood that the schematic drawings are not necessarily to scale. The flowcharts used in this invention illustrate operations performed in accordance with some embodiments of the present invention. It should be understood that the operations of the flow diagrams may be performed out of order, and that steps without logical context may be reversed in order or performed concurrently. One skilled in the art, under the direction of this summary, may add one or more other operations to, or remove one or more operations from, the flowchart. Some of the block diagrams shown in the figures are functional entities and do not necessarily correspond to physically or logically separate entities. These functional entities may be implemented in the form of software, or in one or more hardware modules or integrated circuits, or in different networks and/or processor systems and/or microcontroller systems.
Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the invention. The appearances of the phrase in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. It is explicitly and implicitly understood by one skilled in the art that the embodiments described herein may be combined with other embodiments.
The invention provides a method and a system for stably tracking multiple targets based on a vehicle-mounted millimeter wave radar, which are respectively explained below.
Fig. 1 is a schematic flow chart of an embodiment of the method for stable multi-target tracking based on the vehicle-mounted millimeter wave radar, as shown in fig. 1, the method for stable multi-target tracking based on the vehicle-mounted millimeter wave radar includes:
s101, obtaining current point cloud data at the current moment based on a vehicle-mounted millimeter wave radar, and clustering the current point cloud data to obtain a target data set; the target data set comprises a plurality of target points and measurement information of each target point;
s102, acquiring a track list corresponding to previous point cloud data at a previous moment; the track list comprises a plurality of tracks and last-time state information of the tracks;
s103, acquiring the motion direction of the track, determining a motion model based on the motion direction of the track, and predicting the track based on the motion model and the state information of the previous moment to acquire the predicted state information of the current moment of the track;
s104, determining the shape of the associated wave gate based on the motion direction of the track, and determining the threshold condition of the associated wave gate based on the measurement information and the current time prediction state information;
s105, performing target association on the target data set and the track based on the associated wave gate and the threshold condition, and judging whether the association is successful;
and S106, when the association succeeds, performing interactive multi-Model (IMM) filtering estimation on the track to obtain the current-time state information of the track, and updating the motion direction of the track based on the current-time state information of the track and the state information of the last time.
Compared with the prior art, the stable multi-target tracking method based on the vehicle-mounted millimeter wave radar determines the shape of the associated wave gate based on the motion direction of the track, determines the threshold condition of the associated wave gate based on the measurement information and the current-time prediction state information, and avoids association errors or association failures caused by the adoption of unified associated wave gate and threshold condition, so that the problem of track interruption is avoided, and the stability and the accuracy of multi-target tracking are improved. Furthermore, the embodiment of the invention determines the motion model based on the motion direction of the track, and can avoid the defect of reduced tracking precision caused by excessive competition of the motion model, thereby further improving the multi-target tracking precision, further relieving the problem of track interruption caused by lower tracking precision, and further improving the stability of multi-target tracking.
The clustering of the current point cloud data in step S101 specifically includes: clustering is carried out on the current point cloud data Based on a Noise-Based Density Clustering method (DBSCAN). Among them, DBSCAN divides a region having sufficient density into clusters, and finds an arbitrarily shaped cluster in a spatial database having noise, which defines a cluster as the maximum set of density-connected points.
In a specific embodiment of the invention, the target data set
Figure SMS_61
，
Figure SMS_62
Is shown as
Figure SMS_63
And measuring information of each target point, wherein the measuring information comprises the distance, the radial speed and the angle of the target point.
In some embodiments of the invention, the motion model comprises a first motion sub-model and a second motion sub-model; then, as shown in fig. 2, the determining the motion model based on the motion direction of the track in step S103 includes:
s201, when the motion direction of the track is a straight motion track, a transverse motion track or an unknown motion track, the first motion sub-model is a constant velocity motion model (CV), and the second motion sub-model is a constant acceleration motion model (CA);
s202, when the motion direction of the track is the turning motion track, the first motion sub-model is a constant speed motion model (CV), and the second motion sub-model is a constant turning speed motion model (CT).
In some embodiments of the present invention, the current time prediction state information is:
Figure SMS_64
in the formula ,
Figure SMS_65
predicting state information for the current time;
Figure SMS_66
state information of the last moment;
Figure SMS_67
a matrix of a first motion sub-model;
Figure SMS_68
the model probability of the first motion submodel at the current moment is set;
Figure SMS_69
a matrix of a second motion sub-model;
Figure SMS_70
and the model probability of the second motion submodel at the current moment.
In some embodiments of the present invention, as shown in fig. 3, the determining the shape of the associated wave gate based on the trajectory-based motion direction in step S104 includes:
s301, when the motion direction of the track is a straight motion track or a transverse motion track, the shape of the associated wave gate is rectangular;
s302, when the motion direction of the track is a turning motion track, the shape of the associated wave gate is a positive direction;
and S303, when the motion direction of the track is the unknown motion track, the shape of the associated wave gate is circular.
According to the embodiment of the invention, the shape of the associated wave gate is selected according to the motion direction of the track, so that the adaptability of the shape of the associated wave gate and the track type can be improved, the problem of target track interruption caused by association error or association failure due to adoption of a unified associated wave gate shape is avoided, and the stability of multi-target tracking is favorably improved.
In some embodiments of the present invention, the current-time prediction state information includes a predicted lateral coordinate, a predicted longitudinal coordinate, a predicted lateral movement speed, a predicted longitudinal movement speed, a predicted distance, a predicted radial speed, and a predicted angle;
then when the shape of the associated wave gate is rectangular or square, the threshold condition is:
Figure SMS_71
when the shape of the associated gate is circular, the threshold condition is:
Figure SMS_72
in the formula ,
Figure SMS_75
is a distance;
Figure SMS_77
is the radial velocity;
Figure SMS_87
is an angle;
Figure SMS_78
to predict the lateral coordinates;
Figure SMS_89
to predict the longitudinal coordinates;
Figure SMS_80
predicting the transverse movement speed;
Figure SMS_84
to predict the longitudinal movement velocity;
Figure SMS_81
is a predicted distance;
Figure SMS_86
to predict radial velocity;
Figure SMS_73
is a predicted angle;
Figure SMS_82
is a lateral distance threshold;
Figure SMS_79
is a longitudinal distance threshold;
Figure SMS_85
is a lateral velocity threshold;
Figure SMS_76
is a longitudinal speed threshold;
Figure SMS_88
is a distance threshold;
Figure SMS_74
is an angle threshold;
Figure SMS_83
is a speed threshold.
According to the embodiment of the invention, the appropriate threshold condition of the associated wave gate is selected according to the measurement information and the shape of the associated wave gate, so that the adaptability between the threshold condition of the associated wave gate and the track type can be improved, the problem of target track interruption caused by association error or association failure due to adoption of a unified associated wave gate threshold condition is further avoided, and the stability of multi-target tracking is further improved.
In some embodiments of the invention, the interactive multi-model comprises a plurality of Kalman filters constructed based on the motion model; the filter algorithm in the Kalman filter is an extended Kalman filter algorithm (EKF), motion models in the Kalman filters are different, and when the motion direction of the track is a straight motion track, a transverse motion track or an unknown motion track, the motion models in the Kalman filter are a constant speed motion model and a constant acceleration motion model; and when the motion direction of the track is the turning motion track, the motion models are a constant speed motion model and a constant turning speed motion model.
The interactive multi-model is used for weighting the filtering result of each Kalman filter by utilizing a plurality of Kalman filters of different motion models to jointly estimate the state information of the target. The Markov chain transition probability of the first order among a plurality of motion models can be met, and various models can be selected according to targets in different motion states.
Then, as shown in fig. 4, the interactive multi-model filtering estimation is performed on the track in step S106, and the obtaining of the state information of the track at the current time includes:
s401, obtaining last-time state estimation and last-time model probability of a Kalman filter;
s402, obtaining model transition probabilities of a plurality of Kalman filters, and determining the mixing probability of the Kalman filters based on the model transition probabilities and the model probability at the last moment;
s403, determining a last-moment mixed state estimation matrix and a last-moment mixed covariance estimation matrix of the Kalman filter based on the last-moment state estimation matrix and the mixed probability;
s404, filtering the track based on the last mixed state estimation and the last mixed covariance estimation matrix to obtain a current state estimation and a current covariance matrix;
s405, a likelihood function is obtained, and the current moment model probability of the Kalman filter is determined based on the likelihood function;
s406, determining a total state estimation and a total covariance matrix of the interactive multi-model based on the current-time model probability, the current-time state estimation and the current-time covariance matrix.
Wherein, the model transition probability in step S402 is:
Figure SMS_90
in the formula ,
Figure SMS_91
and N is the total number of models in the interactive multi-model, wherein N is the model transition probability of the model i to the model j.
In a specific embodiment of the present invention, when the motion direction of the trajectory is an unknown motion direction trajectory, the interactive multi-model includes two kalman filters constructed by a constant velocity motion model and a constant acceleration motion model, and the initialized model transition probability is Ptij = [0.3,0.7, 0.3.
In some embodiments of the invention, the overall state estimate is:
Figure SMS_92
the total covariance matrix is:
Figure SMS_93
Figure SMS_94
wherein ,
Figure SMS_95
Figure SMS_96
Figure SMS_97
Figure SMS_98
Figure SMS_99
Figure SMS_100
Figure SMS_101
Figure SMS_102
Figure SMS_103
Figure SMS_104
Figure SMS_105
in the formula ,
Figure SMS_109
estimating the total state;
Figure SMS_113
is a total covariance matrix;
Figure SMS_120
estimating the state at the current moment;
Figure SMS_111
the model probability at the current moment;
Figure SMS_118
is a transposed symbol;
Figure SMS_114
is a covariance matrix at the current moment;
Figure SMS_122
is a likelihood function;
Figure SMS_110
is the mixing probability;
Figure SMS_119
the model transition probability;
Figure SMS_106
is the model probability of the last moment;
Figure SMS_117
is a normalized probability;
Figure SMS_112
is a normalization constant;
Figure SMS_124
is measurement information;
Figure SMS_115
is a prediction error covariance matrix;
Figure SMS_125
estimating for a prediction state;
Figure SMS_108
is a measurement matrix;
Figure SMS_121
estimating the state of the last moment;
Figure SMS_116
an observation noise covariance matrix at the current moment;
Figure SMS_123
estimating the mixing state at the last moment;
Figure SMS_107
the matrix is estimated for the hybrid covariance at the previous time.
In some embodiments of the present invention, as shown in fig. 5, the updating the moving direction of the track based on the current time status information and the last time status information of the track in step S106 includes:
s501, determining the current transverse displacement and the current longitudinal displacement of the track at the current moment based on the current moment state information and the last moment state information of the track;
s502, acquiring a plurality of historical transverse displacement amounts and a plurality of historical longitudinal displacement amounts of a track at a plurality of historical moments;
s503, when the existing frame number of the track is larger than the preset frame number, determining an average transverse displacement based on the current transverse displacement and a plurality of historical transverse displacements, and determining an average longitudinal displacement based on the current longitudinal displacement and a plurality of historical longitudinal displacements;
s504, determining a displacement scale factor based on the average longitudinal displacement and the average transverse displacement;
and S505, updating the motion direction of the track based on the displacement scale factor.
It should be noted that: and when the existing frame number of the track is less than or equal to the preset frame number, directly outputting the current moment state information of the track without updating the motion direction of the track.
In step S501, the track current-time status information includes a current-time track transverse position coordinate and a current-time track longitudinal position coordinate, the previous-time status information includes a previous-time track transverse position coordinate and a previous-time track longitudinal position coordinate, and then the current transverse displacement offset x is:
offsetX=X-lastX
the current longitudinal displacement amount offset is:
offsetY=Y-lastY
in the formula, X is the coordinate of the transverse position of the track at the current moment; y is the longitudinal position coordinate of the track at the current moment; lastX is the coordinate of the transverse position of the track at the last moment; lastY is the track longitudinal position coordinate at the last moment.
The preset frame number in step S503 may be adjusted or set according to an actual application scenario or an empirical value, and in a specific embodiment of the present invention, the preset frame number is 10.
Wherein the average lateral displacement amount meanX in step S503 is:
meanX=offsetX/α
the average longitudinal displacement amount meanY is:
meanY=offsetY/α
wherein α is a predetermined number of frames.
Further, the displacement scale factor is the ratio of the average lateral displacement to the average longitudinal displacement, i.e.: the displacement scale factor is:
factor=meanX/meanY。
in some embodiments of the present invention, as shown in fig. 6, step S505 comprises:
s601, when the displacement scale factor is smaller than a first threshold value, the motion direction of the track is a straight motion track;
s602, when the displacement scale factor is larger than or equal to a first threshold value and smaller than a second scale threshold value, the motion direction of the track is a turning motion track;
and S603, when the displacement scale factor is larger than or equal to a second threshold value, the motion direction of the track is a transverse running track.
It should be noted that: the first threshold and the second threshold may be set or adjusted according to an actual application scenario and an empirical value, and in a specific embodiment of the present invention, the first threshold is 0.5, and the second threshold is 1.
Since the velocity value estimated when the kalman filter has not converged may be inaccurate when the motion direction of the trajectory is a lateral motion trajectory or a turning motion trajectory, the model probability of the current time of the kalman filter for the trajectory needs to be corrected, that is: after step S505, the method further includes: and updating the model probability of the Kalman filter at the current moment.
According to the embodiment of the invention, the convergence of the Kalman filter is accelerated by updating the model probability of the Kalman filter at the current moment, so that the problem of target track interruption caused by low tracking precision is further relieved, and the stability of multi-target tracking is further improved.
In the specific embodiment of the invention, when the motion direction of the track is the turning motion track, the adopted motion models are CV and CT models, if the probability of the CT model is less than that of the CV model, the probability of the CT model is corrected to be a displacement scale factor, and the probability of the CV model is corrected to be 1-factor; when the motion direction of the track is a transverse motion track, the adopted motion models are CV and CA models, if the probability of the CA model is smaller than that of the CV model, the probability of the CA model is corrected to be 0.9, and the probability of the CV model is corrected to be 0.1, so that the Kalman filter is accelerated to converge.
In order to better implement the method for stable multi-target tracking based on the vehicle-mounted millimeter wave radar in the embodiment of the present invention, on the basis of the method for stable multi-target tracking based on the vehicle-mounted millimeter wave radar, as shown in fig. 7, correspondingly, the embodiment of the present invention further provides a system for stable multi-target tracking based on the vehicle-mounted millimeter wave radar, where the system 700 for stable multi-target tracking based on the vehicle-mounted millimeter wave radar includes:
a target data set obtaining unit 701, configured to obtain current point cloud data of a current time based on a vehicle-mounted millimeter wave radar, and perform clustering on the current point cloud data to obtain a target data set; the target data set comprises a plurality of target points and measurement information of each target point;
a track list obtaining unit 702, configured to obtain a track list corresponding to previous point cloud data at a previous time; the track list comprises a plurality of tracks and last-time state information of the tracks;
the state information prediction unit 703 is configured to obtain a motion direction of the trajectory, determine a motion model based on the motion direction of the trajectory, perform trajectory prediction on the trajectory based on the motion model and the state information of the previous time, and obtain current-time prediction state information of the trajectory;
a correlation gate determining unit 704, configured to determine a shape of a correlation gate based on the motion direction of the track, and determine a threshold condition of the correlation gate based on the measurement information and the current time prediction state information;
an association unit 705, configured to perform target association on the target data set and the trajectory based on an association wave gate and a threshold condition, and determine whether the association is successful;
and the track updating unit 706 is configured to perform interactive multi-model filtering estimation on the track when the association is successful, obtain state information of the track at the current moment, and update the motion direction of the track based on the state information of the track at the current moment and the state information of the track at the previous moment.
The stable multi-target tracking system 700 based on the vehicle-mounted millimeter wave radar provided in the above embodiment may implement the technical solutions described in the above stable multi-target tracking prediction method based on the vehicle-mounted millimeter wave radar, and the specific implementation principles of the above modules or units may refer to the corresponding contents in the above stable multi-target tracking method based on the vehicle-mounted millimeter wave radar, and are not described herein again.
Correspondingly, the embodiment of the invention also provides a computer-readable storage medium, which is used for storing a computer-readable program or instruction, and when the program or instruction is executed by a processor, the steps or functions of the stable multi-target tracking method based on the vehicle-mounted millimeter wave radar provided by the above method embodiments can be realized.
Those skilled in the art will appreciate that all or part of the processes of the methods of the above embodiments may be implemented by instructing relevant hardware (such as a processor, a controller, etc.) by a computer program, and the computer program may be stored in a computer readable storage medium. The computer readable storage medium is a magnetic disk, an optical disk, a read-only memory or a random access memory.
The method and the system for stably tracking multiple targets based on the vehicle-mounted millimeter wave radar are described in detail, specific examples are applied in the method to explain the principle and the implementation mode of the method, and the description of the embodiments is only used for helping to understand the method and the core idea of the method; meanwhile, for those skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims (10)

1. A stable multi-target tracking method based on a vehicle-mounted millimeter wave radar is characterized by comprising the following steps:
obtaining current point cloud data at the current moment based on a vehicle-mounted millimeter wave radar, and clustering the current point cloud data to obtain a target data set; the target data set comprises a plurality of target points and measurement information of each target point;
acquiring a track list corresponding to previous point cloud data at a previous moment; the track list comprises a plurality of tracks and last-time state information of the tracks;
acquiring the motion direction of the track, determining a motion model based on the motion direction of the track, and predicting the track based on the motion model and the state information of the previous moment to acquire the predicted state information of the current moment of the track;
determining the shape of an associated gate based on the motion direction of the track, and determining the threshold condition of the associated gate based on the measurement information and the current time prediction state information;
performing target association on the target data set and the track based on the association wave gate and the threshold condition, and judging whether the association is successful;
and when the association is successful, performing interactive multi-model filtering estimation on the track to obtain the current moment state information of the track, and updating the motion direction of the track based on the current moment state information and the last moment state information of the track.
2. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 1, wherein the motion model comprises a first motion sub-model and a second motion sub-model; the determining a motion model based on the motion direction of the trajectory comprises:
when the motion direction of the track is a straight motion track, a transverse motion track or an unknown motion track, the first motion sub-model is a constant speed motion model, and the second motion sub-model is a constant acceleration motion model;
and when the motion direction of the track is a turning motion track, the first motion sub-model is a constant speed motion model, and the second motion sub-model is a constant turning speed motion model.
3. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 2, wherein the current-time prediction state information is:
Figure QLYQS_1
in the formula ,
Figure QLYQS_2
predicting state information for the current time;
Figure QLYQS_3
state information at the last moment;
Figure QLYQS_4
a matrix of a first motion sub-model;
Figure QLYQS_5
the model probability of the first motion sub-model at the current moment is obtained;
Figure QLYQS_6
a matrix of a second motion sub-model;
Figure QLYQS_7
the model probability of the second motion sub-model at the current moment.
4. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 1, wherein the determining of the shape of the associated gate based on the motion direction of the trajectory comprises:
when the motion direction of the track is a straight motion track or a transverse motion track, the shape of the associated wave gate is rectangular;
when the motion direction of the track is a turning motion track, the shape of the associated wave gate is a positive direction;
and when the motion direction of the track is an unknown motion track, the shape of the associated wave gate is circular.
5. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 4, wherein the measurement information comprises distance, radial speed and angle, and the current-time prediction state information comprises predicted transverse coordinates, predicted longitudinal coordinates, predicted transverse movement speed, predicted longitudinal movement speed, predicted distance, predicted radial speed and predicted angle; when the shape of the associated wave gate is rectangular or square, the threshold condition is:
Figure QLYQS_8
when the shape of the associated gate is circular, the threshold condition is:
Figure QLYQS_9
in the formula ,
Figure QLYQS_17
is a distance;
Figure QLYQS_11
is the radial velocity;
Figure QLYQS_22
is an angle;
Figure QLYQS_15
to predict the lateral coordinates;
Figure QLYQS_25
to predict the longitudinal coordinates;
Figure QLYQS_18
predicting the transverse movement speed;
Figure QLYQS_24
to predict the longitudinal movement velocity;
Figure QLYQS_13
is a predicted distance;
Figure QLYQS_26
to predict radial velocity;
Figure QLYQS_10
is a predicted angle;
Figure QLYQS_19
is a lateral distance threshold;
Figure QLYQS_16
is a longitudinal distance threshold;
Figure QLYQS_21
is a lateral velocity threshold;
Figure QLYQS_14
is a longitudinal speed threshold;
Figure QLYQS_20
is a distance threshold;
Figure QLYQS_12
is an angle threshold;
Figure QLYQS_23
is a speed threshold.
6. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 1, wherein the interactive multi-model comprises a plurality of Kalman filters constructed based on the motion model; the interactive multi-model filtering estimation of the track to obtain the current-time state information of the track includes:
acquiring last-time state estimation and last-time model probability of the Kalman filter;
obtaining model transition probabilities of the Kalman filters, and determining a mixing probability of the Kalman filters based on the model transition probabilities and the model probability at the last moment;
determining a last-time hybrid state estimate and a last-time hybrid covariance estimate matrix for the Kalman filter based on the last-time state estimate and the hybrid probability;
filtering the track based on the last-moment mixed state estimation and the last-moment mixed covariance estimation matrix to obtain a current-moment state estimation and a current-moment covariance matrix;
acquiring a likelihood function, and determining the current moment model probability of the Kalman filter based on the likelihood function;
determining a total state estimate and a total covariance matrix for the interactive multi-model based on the current-time model probability, the current-time state estimate, and the current-time covariance matrix.
7. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 6, wherein the total state estimation is as follows:
Figure QLYQS_27
the total covariance matrix is:
Figure QLYQS_28
Figure QLYQS_29
wherein ,
Figure QLYQS_30
Figure QLYQS_31
Figure QLYQS_32
Figure QLYQS_33
Figure QLYQS_34
Figure QLYQS_35
Figure QLYQS_36
Figure QLYQS_37
Figure QLYQS_38
Figure QLYQS_39
Figure QLYQS_40
in the formula ,
Figure QLYQS_42
estimating the total state;
Figure QLYQS_43
is the total covariance matrix;
Figure QLYQS_55
estimating the state at the current moment;
Figure QLYQS_46
the model probability at the current moment;
Figure QLYQS_56
is a transposed symbol;
Figure QLYQS_51
is a covariance matrix at the current moment;
Figure QLYQS_59
is a likelihood function;
Figure QLYQS_45
is the mixing probability;
Figure QLYQS_54
a model transition probability;
Figure QLYQS_41
is the model probability of the last moment;
Figure QLYQS_52
is a normalized probability;
Figure QLYQS_47
is a normalization constant;
Figure QLYQS_58
is measurement information;
Figure QLYQS_48
is a prediction error covariance matrix;
Figure QLYQS_60
estimating for a predicted state;
Figure QLYQS_49
is a measurement matrix;
Figure QLYQS_53
an observation noise covariance matrix at the current moment;
Figure QLYQS_50
estimating the state of the last moment;
Figure QLYQS_57
estimating the mixing state at the last moment;
Figure QLYQS_44
the matrix is estimated for the hybrid covariance at the previous time.
8. The method for stable multi-target tracking based on the vehicle-mounted millimeter wave radar as claimed in claim 7, wherein the updating the motion direction of the track based on the current time state information and the last time state information of the track comprises:
determining the current transverse displacement and the current longitudinal displacement of the track at the current moment based on the current-moment state information and the last-moment state information of the track;
acquiring a plurality of historical transverse displacement amounts and a plurality of historical longitudinal displacement amounts of the track at a plurality of historical moments;
when the existing frame number of the track is larger than a preset frame number, determining an average transverse displacement amount based on the current transverse displacement amount and the plurality of historical transverse displacement amounts, and determining an average longitudinal displacement amount based on the current longitudinal displacement amount and the plurality of historical longitudinal displacement amounts;
determining a displacement scaling factor based on the average longitudinal displacement amount and the average transverse displacement amount;
and updating the motion direction of the track based on the displacement scale factor.
9. The vehicle-mounted millimeter wave radar-based stable multi-target tracking method according to claim 8, wherein the updating of the motion direction of the trajectory based on the displacement scale factor comprises:
when the displacement scale factor is smaller than a first threshold value, the motion direction of the track is a straight motion track;
when the displacement scale factor is larger than or equal to the first threshold and smaller than a second scale threshold, the motion direction of the track is a turning motion track;
and when the displacement scale factor is larger than or equal to the second threshold value, the track motion direction is a transverse moving track.
10. The utility model provides a stabilize multi-target tracking system based on-vehicle millimeter wave radar which characterized in that includes:
the system comprises a target data set acquisition unit, a target data set acquisition unit and a data processing unit, wherein the target data set acquisition unit is used for acquiring current point cloud data at the current moment based on a vehicle-mounted millimeter wave radar and clustering the current point cloud data to acquire a target data set; the target data set comprises a plurality of target points and measurement information of each target point;
the track list acquiring unit is used for acquiring a track list corresponding to previous point cloud data at a previous moment; the track list comprises a plurality of tracks and last-time state information of the tracks;
the state information prediction unit is used for acquiring the motion direction of the track, determining a motion model based on the motion direction of the track, and performing track prediction on the track based on the motion model and the state information of the previous moment to acquire the current moment prediction state information of the track;
a correlation gate determining unit, configured to determine a shape of a correlation gate based on a motion direction of the trajectory, and determine a threshold condition of the correlation gate based on the measurement information and the current time prediction state information;
the correlation unit is used for performing target correlation on the target data set and the track based on the correlation wave gate and the threshold condition and judging whether the correlation is successful or not;
and the track updating unit is used for carrying out interactive multi-model filtering estimation on the track when the association is successful, obtaining the current moment state information of the track, and updating the motion direction of the track based on the current moment state information of the track and the last moment state information of the track.
CN202310059397.0A 2023-01-17 2023-01-17 Stable multi-target tracking method and system based on vehicle millimeter wave radar Active CN115808682B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310059397.0A CN115808682B (en) 2023-01-17 2023-01-17 Stable multi-target tracking method and system based on vehicle millimeter wave radar

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310059397.0A CN115808682B (en) 2023-01-17 2023-01-17 Stable multi-target tracking method and system based on vehicle millimeter wave radar

Publications (2)

Publication Number Publication Date
CN115808682A true CN115808682A (en) 2023-03-17
CN115808682B CN115808682B (en) 2023-05-09

Family

ID=85487412

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310059397.0A Active CN115808682B (en) 2023-01-17 2023-01-17 Stable multi-target tracking method and system based on vehicle millimeter wave radar

Country Status (1)

Country Link
CN (1) CN115808682B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117687029A (en) * 2024-02-01 2024-03-12 深圳市佰誉达科技有限公司 Millimeter wave radar-based vehicle motion trail tracking method and system

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105044710A (en) * 2014-05-03 2015-11-11 袁俊泉 Tracking initiation method including radial velocity information for a circular motion target
CN108303692A (en) * 2018-01-30 2018-07-20 哈尔滨工业大学 A kind of multi-object tracking method of solution doppler ambiguity
CN109031279A (en) * 2018-06-23 2018-12-18 中国人民解放军海军航空大学青岛校区 Multiple multiple targets trackings with shape information
CN111007495A (en) * 2019-12-10 2020-04-14 西安电子科技大学 Target track optimization method based on double-fusion maximum entropy fuzzy clustering JPDA
CN111699406A (en) * 2019-03-29 2020-09-22 深圳市大疆创新科技有限公司 Tracking detection method of millimeter wave radar, millimeter wave radar and vehicle
CN112526521A (en) * 2020-11-25 2021-03-19 湖北工业大学 Multi-target tracking method for automobile millimeter wave anti-collision radar
CN113064155A (en) * 2021-03-18 2021-07-02 沈阳理工大学 Optimization method of track association under multi-target tracking of aerial radar
CN113866754A (en) * 2021-09-14 2021-12-31 西安电子科技大学 Moving target track correlation method based on Gaussian distribution wave gate

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105044710A (en) * 2014-05-03 2015-11-11 袁俊泉 Tracking initiation method including radial velocity information for a circular motion target
CN108303692A (en) * 2018-01-30 2018-07-20 哈尔滨工业大学 A kind of multi-object tracking method of solution doppler ambiguity
CN109031279A (en) * 2018-06-23 2018-12-18 中国人民解放军海军航空大学青岛校区 Multiple multiple targets trackings with shape information
CN111699406A (en) * 2019-03-29 2020-09-22 深圳市大疆创新科技有限公司 Tracking detection method of millimeter wave radar, millimeter wave radar and vehicle
CN111007495A (en) * 2019-12-10 2020-04-14 西安电子科技大学 Target track optimization method based on double-fusion maximum entropy fuzzy clustering JPDA
CN112526521A (en) * 2020-11-25 2021-03-19 湖北工业大学 Multi-target tracking method for automobile millimeter wave anti-collision radar
CN113064155A (en) * 2021-03-18 2021-07-02 沈阳理工大学 Optimization method of track association under multi-target tracking of aerial radar
CN113866754A (en) * 2021-09-14 2021-12-31 西安电子科技大学 Moving target track correlation method based on Gaussian distribution wave gate

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SARAH H 等: "Precise Ego-Motion Estimation with Millimeter-Wave Radar under Diverse and Challenging Conditions", 《2018 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 *
赵菡 等: "杂波环境下强机动目标自适应关联波门选择" *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117687029A (en) * 2024-02-01 2024-03-12 深圳市佰誉达科技有限公司 Millimeter wave radar-based vehicle motion trail tracking method and system
CN117687029B (en) * 2024-02-01 2024-05-03 深圳市佰誉达科技有限公司 Millimeter wave radar-based vehicle motion trail tracking method and system

Also Published As

Publication number Publication date
CN115808682B (en) 2023-05-09

Similar Documents

Publication Publication Date Title
CN113777600B (en) Multi-millimeter wave radar co-location tracking method
CN107132504B (en) Particle filter-based positioning and tracking device and method and electronic equipment
CN112198503A (en) Target track prediction optimization method and device and radar system
US20220180750A1 (en) Method for determining collision distance, storage medium and electronic equipment
CN115808682A (en) Stable multi-target tracking method and system based on vehicle-mounted millimeter wave radar
CN109885046B (en) Mobile robot positioning and accelerating method based on particle filtering
CN111259332B (en) Fuzzy data association method and multi-target tracking method in clutter environment
CN112328959B (en) Multi-target tracking method based on adaptive extended Kalman probability hypothesis density filter
CN111257865A (en) Maneuvering target multi-frame detection tracking method based on linear pseudo-measurement model
CN115840221A (en) Method for realizing target feature extraction and multi-target tracking based on 4D millimeter wave radar
CN111366900A (en) Tracking radar track quality evaluation method, system and medium based on residual error statistics
CN111679269B (en) Multi-radar fusion track state estimation method based on variation
CN109541541B (en) Indoor triangulation positioning precision correction method and device
CN106885567B (en) Inertial navigation cooperation positioning method and positioning equipment
CN111007880A (en) Extended target tracking method based on automobile radar
JP4196684B2 (en) Target tracking device
CN114740448B (en) Target state estimation method and device for vehicle-mounted radar and storage medium
CN115935133A (en) Importance Gaussian integral particle filtering method and device and readable storage medium
CN114608585A (en) Method and device for synchronous positioning and mapping of mobile robot
CN113281736B (en) Radar maneuvering intersection target tracking method based on multi-hypothesis singer model
CN112068121A (en) Formation target tracking method based on random finite set
CN117250606B (en) Track tracking method, device, equipment and storage medium
CN117214857B (en) Tracking method of Gaussian multi-hypothesis multi-expansion target in three-dimensional scene
WO2024004113A1 (en) Target tracking device, target tracking method, and target tracking program
WO2023178525A1 (en) Apparatus and method for tracking of extended objects

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant