CN115808682B - Stable multi-target tracking method and system based on vehicle millimeter wave radar - Google Patents
Stable multi-target tracking method and system based on vehicle millimeter wave radar Download PDFInfo
- Publication number
- CN115808682B CN115808682B CN202310059397.0A CN202310059397A CN115808682B CN 115808682 B CN115808682 B CN 115808682B CN 202310059397 A CN202310059397 A CN 202310059397A CN 115808682 B CN115808682 B CN 115808682B
- Authority
- CN
- China
- Prior art keywords
- track
- motion
- model
- state information
- current
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 claims abstract description 24
- 230000002452 interceptive effect Effects 0.000 claims abstract description 21
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 238000006073 displacement reaction Methods 0.000 claims description 62
- 239000011159 matrix material Substances 0.000 claims description 42
- 230000007704 transition Effects 0.000 claims description 13
- 238000010606 normalization Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention provides a stable multi-target tracking method and system based on a vehicle millimeter wave radar, wherein the method comprises the following steps: obtaining a target data set based on the vehicle 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 the state information of the tracks at the last moment; acquiring a motion direction of a track, determining a motion model based on the motion direction of the track, and acquiring current time prediction state information of the track; determining the shape of the associated wave gate and the threshold condition of the associated wave gate based on the motion direction of the track; 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 or not; and when the association is successful, performing interactive multi-model filtering estimation on the track, obtaining the state information of the current moment of the track, and updating the movement direction of the track. The invention realizes accurate and stable multi-target tracking in complex environments.
Description
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 detection carriers, accurately measures the distance, speed and angle of a target vehicle on the road to determine the position and movement information of the target to form a target point, clusters the target point and aggregates the mass center to obtain an observed value of the target, establishes a tracker, and filters the observed value by using a filtering algorithm to obtain a target tracking result.
The vehicle millimeter wave radar has complex actual application scene and target motion characteristics, and usually adopts an interactive multi-model (Interacting Multiple Model, IMM) tracker to track multiple targets, but because the millimeter wave radar adopts a Doppler principle to speed the targets, the speed detection precision of transverse movement and turning targets is reduced, so that the tracking precision is reduced, and track breakage occurs.
Therefore, there is an urgent need to provide a stable multi-target tracking method and system based on a vehicle millimeter wave radar, so as to realize accurate and stable multi-target tracking in a complex environment.
Disclosure of Invention
In view of the above, it is necessary to provide a stable multi-target tracking method and system based on a vehicle millimeter wave radar, so as to solve the technical problems in the prior art that the multi-target tracking is easy to occur in a discontinuous track terminal under a complex environment, resulting in lower accuracy and stability of the multi-target tracking.
In one aspect, the invention provides a stable multi-target tracking method based on a vehicle millimeter wave radar, comprising the following steps:
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;
acquiring a track list corresponding to the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and state information of the tracks at the last moment;
acquiring the motion direction of the track and the state information of the last time, determining a motion model based on the motion direction of the track, and carrying out track prediction on the track based on the motion model and the state information of the last time to acquire the current time prediction state information of the track;
determining the shape of an 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;
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 or not;
and when the association is successful, performing interactive multi-model filtering estimation on the track to obtain the state information of the current moment of the track, and updating the movement direction of the track based on the state information of the current moment of the track and the state information of the last moment.
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 track 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;
when the movement direction of the track is a turning movement track, the first movement sub-model is a constant speed movement model, and the second movement sub-model is a constant turning speed movement model.
In some possible implementations, the current time prediction state information is:
in the formula ,
predicting state information for the current moment; />
To last oneTime status information; />
A matrix that is a first motion sub-model; />
The model probability of the current moment of the first motion sub-model is given; />
A matrix for a second motion sub-model;
the model probability at the current moment of the second motion sub-model.
In some possible implementations, the determining the shape of the associated wave gate based on the direction of motion 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;
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 metrology information includes distance, radial velocity, and angle, and the current time predicted state information includes predicted lateral coordinates, predicted longitudinal coordinates, predicted lateral motion velocity, predicted longitudinal motion velocity, predicted distance, predicted radial velocity, and predicted angle; when the shape of the associated waveguide is rectangular or square, the threshold condition is:
when the shape of the associated waveguide is circular, the threshold condition is:
in the formula ,
is the distance; />
Is radial velocity; />
Is an angle; />
To predict lateral coordinates; />
To predict longitudinal coordinates; />
Predicting the transverse movement speed; />
To predict the longitudinal movement speed; />
Is the predicted distance; />
To predict radial velocity; />
Is the predicted angle;
is a lateral distance threshold; />
Is a longitudinal distance threshold; />
Is a transverse speed threshold; />
Is longitudinalA directional velocity threshold; />
Is a distance threshold; />
Is an angle threshold; />
Is a speed threshold.
In some possible implementations, the interactive multi-model includes a plurality of kalman filters built based on the motion model; the interactive multi-model filtering estimation is carried out on the track to obtain the state information of the current moment of the track, which comprises the following steps:
acquiring a last time state estimation and a 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 last moment model probability;
determining a last time hybrid state estimate and a last time hybrid covariance estimate matrix of the kalman filter based on the last time state estimate and the hybrid probability;
filtering the track based on the last time mixed state estimation and the last time mixed covariance estimation matrix to obtain a current time state estimation and a current time covariance matrix;
obtaining a likelihood function, and determining the current moment model probability of the Kalman filter based on the likelihood function;
and 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.
In some possible implementations, the total state estimate is:
the total covariance matrix is:
wherein ,
in the formula ,
estimating the total state; />
Is the total covariance matrix; />
Estimating the state of the current moment; />
The probability is the current moment model probability; />
Transpose the symbol; />
The covariance matrix is the current moment; />
Is a likelihood function; />
Is a mixing probability; />
The model transition probability; />
Model probability for the last moment; />
Is normalized probability; />
Is a normalization constant; />
Is measurement information; />
A covariance matrix of the prediction error; />
Estimating for a predicted state; />
Is a measurement matrix; />
Estimating the state for the last moment; />
The observed noise covariance matrix at the current moment;
estimating the mixing state at the last moment; />
The matrix is estimated for the hybrid covariance at the last time instant.
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 state information of the track at the current moment and the state information of the track at the last moment;
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 number of existing frames of the track is greater than a preset number of frames, determining an average lateral displacement amount based on the current lateral displacement amount and the plurality of historical lateral 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 movement 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 value and smaller than the second scale threshold value, the movement direction of the track is a turning movement track;
and when the displacement scale factor is greater than or equal to the second threshold value, the track movement direction is a transverse movement track.
On the other hand, the invention also provides a stable multi-target tracking system based on the vehicle millimeter wave radar, which comprises the following components:
the target data set acquisition unit is used for acquiring current point cloud data at the current moment based on the 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 acquisition unit is used for acquiring a track list corresponding to the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and state information of the tracks at the last moment;
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 at the last moment to acquire the current time prediction state information of the track;
an associated wave gate determining unit, configured to determine a shape of an associated wave gate based on a motion direction of the track, and determine a threshold condition of the associated wave 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 state information of the current moment of the track, and updating the movement direction of the track based on the state information of the current moment of the track and the state information of the last moment.
The beneficial effects of adopting the embodiment are as follows: 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, and the occurrence of association errors or association failures due to the adoption of the unified associated wave gate and the threshold condition is avoided, so that the problem of track interruption is avoided, and the stability and the accuracy of multi-target tracking are improved. Furthermore, the method and the device can avoid the defect of reduced tracking precision caused by excessive competition of the motion model by determining the motion model based on the motion direction of the track, 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.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of one embodiment of a stable multi-target tracking method based on a vehicle millimeter wave radar provided by the invention;
FIG. 2 is a flow chart of one embodiment of determining a motion model based on the motion direction of the track in S103;
FIG. 3 is a flow chart of one embodiment of determining the shape of an associated gate based on the direction of motion of the track in S104;
FIG. 4 is a flowchart of an embodiment of performing interactive multi-model filtering estimation on the track in S106 to obtain the state information of the current moment of the track;
FIG. 5 is a flowchart of an embodiment of updating the motion direction of the track based on the current state information and the previous state information of the track in S106;
FIG. 6 is a schematic structural diagram of an embodiment of the present invention in S505 of FIG. 5;
fig. 7 is a schematic structural diagram of an embodiment of a stable multi-target tracking system based on a vehicle millimeter wave radar according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It will be apparent that the described embodiments are only some, but not all, embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
It should be understood that the schematic drawings are not drawn to scale. A flowchart, as used in this disclosure, illustrates operations implemented according to some embodiments of the present invention. It should be appreciated that the operations of the flow diagrams may be implemented out of order and that steps without logical context may be performed in reverse order or concurrently. Moreover, one or more other operations may be added to or removed from the flow diagrams by those skilled in the art under the direction of the present disclosure. 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 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 may be included in at least one embodiment of the invention. The appearances of such phrases 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. Those of skill in the art will explicitly and implicitly appreciate that the embodiments described herein may be combined with other embodiments.
The invention provides a stable multi-target tracking method and system based on a vehicle millimeter wave radar, which are respectively described below.
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, and as shown in fig. 1, the stable multi-target tracking method based on the vehicle-mounted millimeter wave radar includes:
s101, 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;
s102, acquiring a track list corresponding to the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and the state information of the tracks at the last moment;
s103, acquiring a 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 at the last moment to obtain the predicted state information at 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, 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;
and S106, when the association is successful, performing interactive multi-model (Interacting Multiple Model, IMM) filtering estimation on the track, obtaining the state information of the current moment of the track, and updating the movement direction of the track based on the state information of the current moment of the track and the state information of the last moment.
Compared with the prior art, the stable multi-target tracking method based on the vehicle-mounted millimeter wave radar provided by the embodiment of the invention has the advantages that 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, and the occurrence of association errors or association failures caused by the adoption of the unified associated wave gate and the threshold condition is avoided, so that the problem of track interruption is avoided, and the stability and the accuracy of multi-target tracking are improved. Further, the embodiment of the invention can avoid the defect of reduced tracking precision caused by excessive competition of the motion model by determining the motion model based on the motion direction of the track, 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.
In step S101, clustering the current point cloud data specifically includes: the current point cloud data is clustered based on a Density-based clustering method with noise (Density-Based Spatial Clustering of Applications with Noise, DBSCAN). Wherein the DBSCAN divides an area having a sufficient density into clusters, and discovers arbitrarily shaped clusters in a noisy spatial database, which defines the clusters as the largest set of densely connected points.
In a particular embodiment of the invention, the target data set
，/>
Indicate->
And measuring information of the target points, wherein the measuring information comprises the distance, the radial speed and the angle of the target points.
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, determining a motion model based on the motion direction of the trajectory 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 movement direction of the track is the turning movement track, the first movement sub-model is a constant speed movement model (CV), and the second movement sub-model is a constant turning speed movement model (CT).
In some embodiments of the present invention, the current time prediction state information is:
in the formula ,
predicting state information for the current moment; />
Status information is the last moment; />
A matrix that is a first motion sub-model; />
The model probability of the current moment of the first motion sub-model is given; />
A matrix for a second motion sub-model;
the model probability at the current moment of the second motion sub-model.
In some embodiments of the present invention, as shown in fig. 3, determining the shape of the associated gate based on the track-based movement 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;
s303, when the motion direction of the track is an unknown motion track, the shape of the associated wave gate is circular.
According to the method and the device, the shape of the associated wave gate is selected according to the movement direction of the track, so that the adaptability of the shape of the associated wave gate to the track type can be improved, the problem that the target track is interrupted due to the fact that the associated error or the associated failure occurs due to the fact that the unified associated wave gate shape is adopted is avoided, and the stability of multi-target tracking is improved.
In some embodiments of the present invention, the current time predicted state information includes predicted lateral coordinates, predicted longitudinal coordinates, predicted lateral motion speed, predicted longitudinal motion speed, predicted distance, predicted radial speed, and predicted angle;
then when the associated wave gate is rectangular or square in shape, the threshold condition is:
when the shape of the associated wave gate is circular, the threshold condition is:
in the formula ,
is the distance; />
Is radial velocity; />
Is an angle; />
To predict lateral coordinates; />
To predict longitudinal coordinates; />
Predicting the transverse movement speed; />
To predict the longitudinal movement speed; />
Is the predicted distance; />
To predict radial velocity; />
Is the predicted angle;
is a lateral distance threshold; />
Is a longitudinal distance threshold; />
Is a transverse speed threshold; />
Is a longitudinal speed threshold; />
Is a distance threshold; />
Is an angle threshold; />
Is a speed threshold.
According to the method and the device, the suitability of the associated wave gate threshold condition and the track type can be improved by selecting the appropriate associated wave gate threshold condition according to the measurement information and the associated wave gate shape, so that the problem of target track interruption caused by association errors or association failures due to the adoption of the 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, a plurality of Kalman filters constructed based on a motion model are included in the interactive multi-model; the filtering algorithm in the Kalman filters is an extended Kalman filtering algorithm (EKF), the 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 filters are a constant-speed motion model and a constant-acceleration motion model; when the movement direction of the track is the turning movement track, the movement model is a constant speed movement model and a constant turning speed movement model.
The interactive multi-model is to utilize Kalman filters of a plurality of different motion models to weight the filtering results of the Kalman filters so as to jointly estimate the state information of the target. The first-order Markov chain transition probability is satisfied among a plurality of motion models, and a plurality of models can be selected aiming at targets in different motion states.
Then, as shown in fig. 4, in step S106, the interactive multi-model filtering estimation is performed on the track, to obtain the state information of the current moment of the track, including:
s401, acquiring last-moment state estimation and last-moment model probability of a Kalman filter;
s402, obtaining model transition probabilities of a plurality of 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;
s403, determining a last-time mixed state estimation and a last-time mixed covariance estimation matrix of the Kalman filter based on the last-time state estimation and the mixed probability;
s404, filtering the track based on the previous time mixed state estimation and the previous time mixed covariance estimation matrix to obtain the current time state estimation and the current time covariance matrix;
s405, acquiring a likelihood function, and determining the probability of a model at the current moment of the Kalman filter 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.
The model transition probability in step S402 is:
in the formula ,
for the model transition probability from model i to model j, N is the total number of models in the interactive multi-model.
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.7,0.3].
In some embodiments of the invention, the overall state estimate is:
the total covariance matrix is:
wherein ,
in the formula ,
estimating the total state; />
Is the total covariance matrix; />
Estimating the state of the current moment; />
The probability is the current moment model probability; />
Transpose the symbol; />
The covariance matrix is the current moment; />
Is a likelihood function; />
Is a mixing probability; />
The model transition probability; />
Model probability for the last moment; />
Is normalized probability; />
Is a normalization constant; />
Is measurement information; />
A covariance matrix of the prediction error; />
Estimating for a predicted state; />
Is a measurement matrix; />
Estimating the state for the last moment; />
The observed noise covariance matrix at the current moment;
estimating the mixing state at the last moment; />
The matrix is estimated for the hybrid covariance at the last time instant.
In some embodiments of the present invention, as shown in fig. 5, updating the movement direction of the track based on the current time state information and the last time state 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 the track at a plurality of historical moments;
s503, when the number of existing frames of the track is larger than a preset number of frames, determining an average transverse displacement amount based on the current transverse displacement amount and a plurality of historical transverse displacement amounts, and determining an average longitudinal displacement amount based on the current longitudinal displacement amount and a plurality of historical longitudinal displacement amounts;
s504, determining a displacement scale factor based on the average longitudinal displacement and the average transverse displacement;
s505, updating the movement direction of the track based on the displacement scale factor.
It should be noted that: when the number of existing frames of the track is smaller than or equal to the preset number of frames, the state information of the current moment of the track is directly output, and the moving direction of the track is not updated.
In step S501, the track current time state information includes a current time track lateral position coordinate and a current time track longitudinal position coordinate, and the previous time state information includes a previous time track lateral position coordinate and a previous time track longitudinal position coordinate, then the current lateral displacement offsetX is:
offsetX=X-lastX
the current longitudinal displacement offsetY is:
offsetY=Y-lastY
wherein X is the transverse position coordinate of the track at the current moment; y is the longitudinal position coordinate of the track at the current moment; lastX is the last moment track transverse position coordinate; lastY is the longitudinal position coordinate of the track at the previous time.
The preset frame number in step S503 may be adjusted or set according to the actual application scenario or the empirical value, and in the embodiment of the present invention, the preset frame number is 10.
Wherein, the average lateral displacement meanX in step S503 is:
meanX=offsetX/α
the average longitudinal displacement meanY is:
meanY=offsetY/α
wherein, alpha is a preset frame number.
Further, the displacement scaling factor is the ratio of the average lateral displacement to the average longitudinal displacement, namely: the displacement scaling factor is:
factor=meanX/meanY。
in some embodiments of the present invention, as shown in fig. 6, step S505 includes:
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 movement direction of the track is a turning movement track;
and S603, when the displacement scale factor is larger than or equal to a second threshold value, the track movement direction is a transverse movement track.
It should be noted that: the first threshold and the second threshold can be set or adjusted according to the actual application scene and the experience value, and in the specific embodiment of the invention, the first threshold is 0.5, and the second threshold is 1.
Since the estimated velocity value 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 kalman filter of the trajectory at the current moment needs to be corrected, that is: after step S505, further comprising: and updating the model probability of the current moment of the Kalman filter.
According to the embodiment of the invention, the convergence of the Kalman filter is accelerated according to the model probability at the current moment of updating the Kalman filter, so that the problem of target track interruption caused by low tracking precision is further solved, 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 a turning motion track, the motion models adopted are CV and CT models, if the CT model probability is smaller than the CV model probability, the CT model probability is corrected to be a displacement scale factor, and the model probability of the CV model is corrected to be 1-factor; when the motion direction of the track is the transverse motion track, the motion models are CV and CA models, if the CA model probability is smaller than the CV model probability, the CA model probability is corrected to 0.9, and the model probability of the CV model is corrected to 0.1, so that the Kalman filter is accelerated to converge.
In order to better implement the stable multi-target tracking method based on the vehicle millimeter wave radar in the embodiment of the present invention, correspondingly, as shown in fig. 7, the embodiment of the present invention further provides a stable multi-target tracking system based on the vehicle millimeter wave radar, where the stable multi-target tracking system 700 based on the vehicle millimeter wave radar includes:
the target data set acquisition unit 701 is configured to obtain current point cloud data at a current moment based on the vehicle millimeter wave radar, and cluster 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 the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and the state information of the tracks at the last moment;
a state information prediction unit 703, configured to obtain a motion direction of the track, determine a motion model based on the motion direction of the track, and predict the track based on the motion model and the state information at the previous time to obtain predicted state information at the current time of the track;
an associated wave gate determining unit 704, configured to determine a shape of an associated wave gate based on a motion direction of the track, and determine a threshold condition of the associated wave gate based on the measurement information and the current time prediction state information;
the association unit 705 is configured to perform target association on the target data set and the track based on the association gate and the threshold condition, and determine whether the association is successful;
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 current moment of the track, and update a motion direction of the track based on the state information of the current moment of the track and the state information of the last moment.
The stable multi-target tracking system 700 based on the vehicle millimeter wave radar provided in the foregoing embodiment may implement the technical solution described in the foregoing embodiment of the stable multi-target tracking prediction method based on the vehicle millimeter wave radar, and the specific implementation principle of each module or unit may refer to the corresponding content in the foregoing embodiment of the stable multi-target tracking method based on the vehicle millimeter wave radar, which is not described herein again.
Correspondingly, the embodiment of the invention also provides a computer readable storage medium, and the computer readable storage medium 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 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 flow of the methods of the embodiments described above may be accomplished by way of a computer program stored in a computer readable storage medium to instruct related hardware (e.g., a processor, a controller, etc.). The computer readable storage medium is a magnetic disk, an optical disk, a read-only memory or a random access memory.
The stable multi-target tracking method and system based on the vehicle millimeter wave radar provided by the invention are described in detail, and specific examples are applied to the description of the principle and the implementation mode of the invention, and the description of the above examples is only used for helping to understand the method and the core idea of the invention; meanwhile, as those skilled in the art will have variations in the specific embodiments and application scope in light of the ideas of the present invention, the present description should not be construed as limiting the present invention.
Claims (8)
1. A stable multi-target tracking method based on a vehicle-mounted millimeter wave radar is characterized by comprising the following steps:
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;
acquiring a track list corresponding to the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and state information of the tracks at the last moment;
acquiring a 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 at the last moment to obtain the predicted state information at the current moment of the track;
determining the shape of an 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;
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 or not;
when the association is successful, carrying out interactive multi-model filtering estimation on the track to obtain the state information of the current moment of the track, and updating the movement direction of the track based on the state information of the current moment of the track and the state information of the last moment;
the determining the shape of the associated waveguide door based on the motion direction of the track comprises the following steps:
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 square;
when the motion direction of the track is an unknown motion track, the shape of the associated wave gate is circular;
the measurement information comprises a distance, a radial speed and an angle, and the current time prediction state information comprises a prediction transverse coordinate, a prediction longitudinal coordinate, a prediction transverse movement speed, a prediction longitudinal movement speed, a prediction distance, a prediction radial speed and a prediction angle; when the shape of the associated waveguide is rectangular or square, the threshold condition is:
when the shape of the associated waveguide is circular, the threshold condition is:
in the formula ,
is the distance; />
Is radial velocity; />
Is an angle; />
To predict lateral coordinates; />
To predict longitudinal coordinates; />
Predicting the transverse movement speed; />
To predict the longitudinal movement speed; />
Is the predicted distance; />
To predict radial velocity; />
Is the predicted angle; />
Is a lateral distance threshold; />
Is a longitudinal distance threshold; />
Is a transverse speed threshold; />
Is a longitudinal speed threshold; />
Is a distance threshold; />
Is an angle threshold; />
Is a speed threshold.
2. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar 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 track 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;
when the movement direction of the track is a turning movement track, the first movement sub-model is a constant speed movement model, and the second movement sub-model is a constant turning speed movement model.
3. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar according to claim 2, wherein the current time prediction state information is:
in the formula ,
predicting state information for the current moment; />
Status information is the last moment; />
A matrix that is a first motion sub-model; />
The model probability of the current moment of the first motion sub-model is given;/>
a matrix for a second motion sub-model; />
The model probability at the current moment of the second motion sub-model.
4. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar 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 is carried out on the track to obtain the state information of the current moment of the track, which comprises the following steps:
acquiring a last time state estimation and a 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 last moment model probability;
determining a last time hybrid state estimate and a last time hybrid covariance estimate matrix of the kalman filter based on the last time state estimate and the hybrid probability;
filtering the track based on the last time mixed state estimation and the last time mixed covariance estimation matrix to obtain a current time state estimation and a current time covariance matrix;
obtaining a likelihood function, and determining the current moment model probability of the Kalman filter based on the likelihood function;
and 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.
5. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar according to claim 4, wherein the total state estimation is:
the total covariance matrix is:
wherein ,
in the formula ,
estimating the total state; />
Is the total covariance matrix; />
Estimating the state of the current moment; />
The probability is the current moment model probability; />
Transpose the symbol; />
The covariance matrix is the current moment; />
Is a likelihood function;
is a mixing probability; />
The model transition probability; />
Model probability for the last moment; />
Is normalized probability;
is a normalization constant; />
Is measurement information; />
A covariance matrix of the prediction error; />
Estimating for a predicted state; />
Is a measurement matrix; />
The observed noise covariance matrix at the current moment; />
Estimating the state for the last moment; />
Estimating the mixing state at the last moment; />
The matrix is estimated for the hybrid covariance at the last time instant.
6. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar according to claim 5, wherein the updating the moving direction of the track based on the track current time state information and the last time state information includes:
determining the current transverse displacement and the current longitudinal displacement of the track at the current moment based on the state information of the track at the current moment and the state information of the track at the last moment;
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 number of existing frames of the track is greater than a preset number of frames, determining an average lateral displacement amount based on the current lateral displacement amount and the plurality of historical lateral 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 movement direction of the track based on the displacement scale factor.
7. The stable multi-target tracking method based on the vehicle-mounted millimeter wave radar according to claim 6, wherein the updating the moving 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 value and smaller than the second scale threshold value, the movement direction of the track is a turning movement track;
and when the displacement scale factor is greater than or equal to the second threshold value, the track movement direction is a transverse movement track.
8. A stable multi-target tracking system based on a vehicle millimeter wave radar, comprising:
the target data set acquisition unit is used for acquiring current point cloud data at the current moment based on the 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 acquisition unit is used for acquiring a track list corresponding to the previous point cloud data at the previous moment; the track list comprises a plurality of tracks and state information of the tracks at the last moment;
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 at the last moment to acquire the current time prediction state information of the track;
an associated wave gate determining unit, configured to determine a shape of an associated wave gate based on a motion direction of the track, and determine a threshold condition of the associated wave 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;
the track updating unit is used for carrying out interactive multi-model filtering estimation on the track when the association is successful, obtaining the state information of the current moment of the track, and updating the movement direction of the track based on the state information of the current moment of the track and the state information of the last moment;
the determining the shape of the associated waveguide door based on the motion direction of the track comprises the following steps:
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;
when the motion direction of the track is an unknown motion track, the shape of the associated wave gate is circular;
the measurement information comprises a distance, a radial speed and an angle, and the current time prediction state information comprises a prediction transverse coordinate, a prediction longitudinal coordinate, a prediction transverse movement speed, a prediction longitudinal movement speed, a prediction distance, a prediction radial speed and a prediction angle; when the shape of the associated waveguide is rectangular or square, the threshold condition is:
when the shape of the associated waveguide is circular, the threshold condition is:
in the formula ,
is the distance; />
Is radial velocity; />
Is an angle; />
To predict lateral coordinates; />
To predict longitudinal coordinates; />
Predicting the transverse movement speed; />
To predict the longitudinal movement speed; />
Is the predicted distance; />
To predict radial velocity; />
Is the predicted angle; />
Is a lateral distance threshold; />
Is a longitudinal distance threshold; />
Is a transverse speed threshold; />
Is a longitudinal speed threshold; />
Is a distance threshold; />
Is an angle threshold; />
Is a speed threshold. />
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 CN115808682A (en) | 2023-03-17 |
| CN115808682B true 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) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117687029B (en) * | 2024-02-01 | 2024-05-03 | 深圳市佰誉达科技有限公司 | Millimeter wave radar-based vehicle motion trail tracking method and system |
Family Cites Families (8)
| 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 |
| CN108303692B (en) * | 2018-01-30 | 2021-02-05 | 哈尔滨工业大学 | Multi-target tracking method for solving Doppler ambiguity |
| CN109031279A (en) * | 2018-06-23 | 2018-12-18 | 中国人民解放军海军航空大学青岛校区 | Multiple multiple targets trackings with shape information |
| CN111699406B (en) * | 2019-03-29 | 2024-04-30 | 深圳市大疆创新科技有限公司 | Millimeter wave radar tracking detection method, millimeter wave radar and vehicle |
| CN111007495B (en) * | 2019-12-10 | 2023-06-13 | 西安电子科技大学 | Target track optimization method based on double fusion maximum entropy fuzzy clustering JPDA |
| CN112526521B (en) * | 2020-11-25 | 2022-08-19 | 湖北工业大学 | Multi-target tracking method for automobile millimeter wave anti-collision radar |
| CN113064155B (en) * | 2021-03-18 | 2023-09-15 | 沈阳理工大学 | Optimization method for track association under multi-target tracking of air radar |
| CN113866754B (en) * | 2021-09-14 | 2024-05-14 | 西安电子科技大学 | Moving target track association method based on Gaussian distribution wave gate |
-
2023
- 2023-01-17 CN CN202310059397.0A patent/CN115808682B/en active Active
Non-Patent Citations (1)
| Title |
|---|
| Precise Ego-Motion Estimation with Millimeter-Wave Radar under Diverse and Challenging Conditions;Sarah H 等;《2018 IEEE International Conference on Robotics and Automation》;6045-6052 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115808682A (en) | 2023-03-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20160139239A1 (en) | Determining device locations using movement, signal strength | |
| CN115808682B (en) | Stable multi-target tracking method and system based on vehicle millimeter wave radar | |
| CN108638062B (en) | Robot positioning method, device, positioning equipment and storage medium | |
| CN113587933B (en) | Indoor mobile robot positioning method based on branch-and-bound algorithm | |
| US11941989B2 (en) | Method for determining collision distance, storage medium and electronic equipment | |
| CN113777600B (en) | Multi-millimeter wave radar co-location tracking method | |
| CN109885046B (en) | Mobile robot positioning and accelerating method based on particle filtering | |
| US11910351B2 (en) | Systems and methods for determining locations of access points | |
| US20230417931A1 (en) | Method for real-time position estimate correction of a movable object | |
| CN111259332B (en) | Fuzzy data association method and multi-target tracking method in clutter environment | |
| CN111279215A (en) | Target detection method and device, track management method and device and unmanned aerial vehicle | |
| CN117214857B (en) | Tracking method of Gaussian multi-hypothesis multi-expansion target in three-dimensional scene | |
| Peterson et al. | Simulation-based examination of the limits of performance for decentralized multi-agent surveillance and tracking of undersea targets | |
| CN114325577A (en) | Non-line-of-sight positioning error correction method and device | |
| CN114608585A (en) | Method and device for synchronous positioning and mapping of mobile robot | |
| CN111679269B (en) | Multi-radar fusion track state estimation method based on variation | |
| CN106885567B (en) | Inertial navigation cooperation positioning method and positioning equipment | |
| CN109541541A (en) | A kind of interior triangulation location precision correcting method and device | |
| Zhang et al. | An adaptive road-constrained IMM estimator for ground target tracking in GSM networks | |
| CN115935133A (en) | Importance Gaussian integral particle filtering method and device and readable storage medium | |
| CN112130137B (en) | Method, device and storage medium for determining lane-level track | |
| CN113269301A (en) | Method and system for estimating parameters of multi-target tracking system based on neural network | |
| CN115865965B (en) | Method, system and equipment for detecting moving target based on hierarchical perception | |
| CN113281736B (en) | Radar maneuvering intersection target tracking method based on multi-hypothesis singer model | |
| CN114372237B (en) | Distributed state estimation method |
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 |