Point cloud clustering and tracking method based on group tracking algorithm

Abstract

The invention discloses a point cloud clustering and tracking method based on a group tracking algorithm. The method comprises the steps of 1, determining a motion model; 2, performing extended Kalman filtering linearization; step 3, a prediction step; step 4, an association step; step 5, a distribution step; step 6, an updating step; step 7, maintaining; and step 8, defining a data interface. According to the method, clustering is carried out while tracking is carried out, two independent modules of traditional clustering and tracking are unified and fused together, multi-dimensional point cloud information can be processed, and a stable and reliable target motion trail can be output. And meanwhile, unstable and wrong tracking results caused by factors such as environment in a millimeter wave radar multi-target tracking process are solved by utilizing multi-dimensional point cloud information.

Description

Technical Field

[0001] The present invention relates to the field of radar signal processing, and in particular to a point cloud clustering and tracking method based on a group tracking algorithm. Background Art

[0002] In recent years, millimeter-wave radar has been widely studied in the field of multi-target tracking and perception. This is all thanks to the Doppler information and micro-Doppler carried in the radar echo. Micro-Doppler generally refers to the additional Doppler information generated by the movement of various parts of the target. The Doppler characteristics can be used to determine the characteristics of the target, so as to track and perceive the target. The identified characteristics can eventually be converted into the motion characteristics of the target, thanks to the high sensitivity of the millimeter-wave radar due to its extremely short wavelength.

[0003] Usually, radar works in a harsh signal propagation environment, and the multi-dimensional point cloud information it generates will be affected by noise, target occlusion, multipath effect, etc. This makes multi-target tracking and perception tasks quite difficult. For example, noise can cause unstable target tracking, target occlusion can make target tracking discontinuous, and multipath effect can cause false targets to appear. At present, there is no unified implementation framework for the research and technology used to achieve robust and reliable multi-target tracking and perception, especially millimeter wave radar. In view of this, the present invention proposes a group tracking algorithm that uses the multi-dimensional information of point clouds to solve the above difficulties and realizes a point cloud processing flow.

[0004] Existing technical solutions

[0005] The existing multi-target tracking scheme mainly consists of two parts. The first step is clustering, and the second step is tracking. Due to the high resolution of millimeter-wave radar, a single target will have multiple point cloud information. In this case, the original single target will evolve into an extended target. Before tracking the extended target, all point clouds in the radar field of view need to be divided into different clusters according to different targets. Each cluster corresponds to a unique extended target, and finally the target is tracked in clusters. Clustering can be regarded as spatial filtering, and tracking can be regarded as temporal filtering. Both are completed as separate modules.

[0006] 1. Existing clustering algorithms are:

[0007] (1) K-means clustering:

[0008] K-means clustering can be said to be the most common clustering algorithm. It is the most widely used of all clustering algorithms due to its simplicity and efficiency. The main idea of ​​the algorithm is to first give a set of data points and the number of clusters K that need to be clustered, where K is determined in advance by the user. Then K center points are randomly initialized, and each data point is classified by calculating the distance between the point and each center point. According to the minimum distance, the point is classified into the cluster corresponding to the center point. After multiple iterations of the algorithm, it can be divided into K groups of point clusters. The advantage of the K-means algorithm is that it is very fast because only the distance between the point and the cluster center needs to be calculated.

[0009] (2) DBSCAN algorithm:

[0010] The DBSCAN algorithm is a density-based clustering algorithm that starts with an arbitrary data point that has not been visited. The domain of this point is defined by distance. If there are a sufficient number of points in the domain, the clustering process begins and the current data point becomes the first point in the new cluster. Otherwise, the point is marked as noise. In both cases, the point is marked as "visited". For the first point in the new cluster, the points in its distance neighborhood also become part of the same cluster. This process makes all points in the domain belong to the same cluster. The above process is then repeated for all new points added to the cluster until all points have been visited and each point is marked as belonging to a cluster or noise. The advantage of the DBSCAN algorithm is that it does not require a predetermined number of clusters and can identify outliers as noise. In addition, it can find clusters of any size and shape very well.

[0011] 2. Existing Tracking Algorithms

[0012] The existing tracking algorithms mainly use Kalman filtering, which is a theoretically complete and widely used algorithm. It is a general technology that is widely used in various fields such as signal processing, robotics, and target tracking. The main idea of ​​the algorithm is to dynamically estimate the state of the next moment in an iterative manner, while taking into account the historical state and observation data. For target tracking, the Kalman filter models the motion model of the target, including the uniform velocity model and the uniform acceleration model, which can be used to estimate the position of the target in the next frame. According to the application scenario, the Kalman filter can also be divided into the extended Kalman filter and the unscented Kalman filter. The extended Kalman filter is mainly used to analyze nonlinear systems. When the dynamic or observation model of the system is not linear, the extended Kalman filter will linearize these models and then apply the Kalman filter step. The unscented Kalman filter provides a more accurate way to deal with nonlinear problems, that is, using unscented transformation to deal with the nonlinear transfer problem of mean and covariance. Compared with the extended Kalman, the unscented Kalman usually obtains better results because it uses higher-order statistical information. Disadvantages of existing technologies:

[0013] (1) The biggest disadvantage of K-means clustering is that the number of clusters must be determined in advance. However, since the number of targets detected by the millimeter-wave radar is uncertain, the number of point clouds corresponding to a target is also uncertain, so it is difficult to determine an accurate value in advance. The actual environment is complex, and there may be many different targets in the environment, such as pedestrians, motor vehicles, non-motor vehicles, etc. The number and spatial distribution range of point clouds obtained by reflections from different types of targets are different. When there are multiple targets in close proximity, the clustering effect will deteriorate due to the mean effect and false convergence problems of K-means clustering. In addition, another disadvantage of K-means clustering is that the center point of the cluster is randomly selected to start the operation, which means that running the algorithm multiple times on the same data may produce different clustering results.

[0014] (2) The main disadvantage of the DBSCAN algorithm is that it performs poorly when the data point density is uneven. This is because when the data point density changes, the distance threshold used to identify neighboring points and the threshold of the minimum point will change with the point density. In actual operating environments, the density of data points will change with the environment and time, and it is impossible to accurately estimate the above thresholds based on the prior information of the point clusters. In addition, in multi-target scenarios, the point cluster density is large because the distance between the targets is too close, making it impossible to distinguish targets that are close to each other.

[0015] (3) Existing multi-target tracking methods all have a shortcoming, that is, clustering and tracking are separated and run as two separate modules, which will not process the feedback point cloud information in a timely manner, resulting in an impact on the continuity of tracking. Continuous tracking refers to tracking multiple targets in the environment only when they are within the current field of view of the radar. Discontinuous tracking is an extension of continuous tracking, that is, if the target disappears within the current field of view, it is assumed that the target will reappear in the future field of view of the radar and can also be associated with the previous trajectory.

[0016] In addition, radar signal and information processing can be simply divided into four processes, corresponding to the RF front end, detection, positioning and final classification. The tracking algorithm is located in the positioning process. It receives the point cloud information obtained by detection and processes it, and then provides positioning information to the classification. The technology of radar single target tracking is relatively mature, and commonly used methods include Kalman filtering, constant gain α~β filtering, etc. However, with the development of high-resolution radar sensors, especially millimeter-wave radars, the detection layer can perceive many reflections of one or more targets in space, thereby obtaining richer point cloud information, including the target's distance, direction, speed and signal-to-noise ratio. Usually, a target will consist of multiple point cloud information. At this time, the tracking algorithm cannot be used directly, but point cloud clustering operations must be performed first.

[0017] Faced with multi-dimensional point cloud information, traditional clustering algorithms for a single distance dimension are no longer applicable. For example, the K-means clustering algorithm only uses the location information of the point to calculate the distance to the cluster center, and the number of clusters must be determined first, which is contradictory to our desire to use a clustering algorithm to help us find out how many clusters there are. The DBSCAN algorithm (Density-Based Spatial Clustering of Applications with Noise) is a density-based clustering algorithm. Compared with the K-means algorithm, it does not require the number of clusters to be determined in advance. However, when the data cluster density is uneven, the distance threshold and minimum point number threshold for identifying similar points will change with the change of clusters. This phenomenon is particularly obvious when processing high-dimensional point cloud data.

[0018] In addition, the Kalman filter used in traditional tracking algorithms is only applicable to linear system models. Considering that there are many nonlinear system models in actual scenarios, it is necessary to use extended Kalman filters (EKF) to linearize the model and then apply the Kalman filter step. Summary of the invention

[0019] The present invention proposes a point cloud clustering and tracking method based on a group tracking algorithm, which performs clustering while tracking, unifies and integrates the two separate modules of traditional clustering and tracking, and can process multi-dimensional point cloud information and output stable and reliable target motion trajectories. At the same time, multi-dimensional point cloud information is used to solve the unstable and erroneous tracking results caused by environmental factors during millimeter wave radar multi-target tracking.

[0020] The present invention is a point cloud clustering and tracking method based on a group tracking algorithm, the method comprising the following steps:

[0021] Step 1. Determine the motion model;

[0022] Step 2. Extended Kalman filter linearization: Use the extended Kalman filter to linearize the state vector to the measurement vector;

[0023] Step 3. Prediction step: Use Kalman filter operation to predict the position of the trajectory at the next moment;

[0024] Step 4. Association step: The association step mainly associates the radar measurement value with an existing tracking trajectory and constructs a measurement boundary around each predicted centroid. The shape of this boundary is determined by the threshold function.

[0025] Step 5. Assignment step: For the measurement vectors that are not associated with any track, i.e., points outside the threshold function, a new group tracker will be initialized and assigned;

[0026] Step 6. Update step: predict the centroid G 1,apr (n) and the measured average The difference between them is called the update amount, which measures the difference between the predicted value and the observed measurement value; using the update amount, the trajectory center is updated to G in the update step 1 (n), by repeating this process, the point cloud trajectory can be clustered and tracked;

[0027] Step 7. Maintenance step: by adding three different states of the target trajectory and maintaining the transition between states through state transition parameters;

[0028] Step 8. Define data interface: A data interface needs to be defined between the point cloud data output by the radar and the input of the group tracking algorithm so that the algorithm can process the output point cloud data in a timely manner.

[0029] Furthermore, the motion model in step 1 is a uniform acceleration model. For the motion of an object, this assumption is basically valid in a short period of time.

[0030] Further, step 1 is as follows: for a moving object, we believe that its velocity change is reasonable, so we use a uniform acceleration model; the uniform acceleration model assumes that the target moves with uniform acceleration, and we use a Kalman filter to redefine the target's position estimate. The state of the Kalman filter at time n is defined as:

[0031] s(n)=Fs(n-1)+Γw(n)

[0032] Here the state vector s(n) is defined in Cartesian coordinates:

[0033]

[0034] in are the velocity components in the x and y directions at time n, are the acceleration components in the x-direction and y-direction at time n, respectively. In the actual motion process, the target acceleration may have random disturbances, which are called process noise and are represented by w(n); Γ is the noise gain, which is defined as:

[0035]

[0036] w(n) is the process noise vector defined in the rectangular coordinate system, and its covariance matrix Q(n) = E[Γw 2 (n)Γ T] is a 6x6 matrix for the two-dimensional uniform acceleration model; T is the radar frame time; in Kalman filter terminology, the process noise covariance matrix Q(n) represents the deviation between the actual state of the system and the motion model;

[0037] F is the state transfer matrix, which is used to specify the dynamic characteristics of the motion model. The state transfer matrix of the two-dimensional uniform acceleration model is:

[0038]

[0039] Expanding S(n) yields:

[0040]

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] From the expanded results, we can see that the current position estimate is the result of the previous position plus the speed of the previous position multiplied by the time T to get a small offset and finally add noise; the uniform acceleration model adds the acceleration component, resulting in the addition of new distance components and velocity components;

[0047] The input vector u(n) represents the measurement value of the nth point cloud acquired by the radar, including the radial distance, azimuth, and radial velocity of the nth point cloud:

[0048]

[0049] The relative relationship between the state quantity s(n) and the input vector u(n) of the Kalman filter is expressed as follows:

[0050] u(n)=H(s(n))+v(n)

[0051] Among them, H(·) represents the measurement matrix, which is used to convert points in the rectangular coordinate system to polar coordinates, v(n) is the measurement noise vector defined in polar coordinates, and its covariance matrix R(n) is a 3x3 matrix. The specific expression of the measurement matrix is:

[0052]

[0053] Further, step 2 is specifically as follows: In the above formula, there is a nonlinear relationship between the input vector u(n) and the state vector s(n), namely:

[0054]

[0055]

[0056]

[0057] Therefore, an extended Kalman filter is used to simplify the relationship between u(n) and s(n); the specific operation is to retain only the first-order Taylor expansion of H(·), that is, f(x) = f(x 0 )+f′(x 0 )(xx 0 ) is:

[0058] u(n)≈H(s apr (n))+J H (s apr (n))[s(n)-s apr (n)]

[0059] Among them, s apr (n) is the prior estimate of the state vector at time n based on the measured value at time n-1, is the predicted value, J H (s) is the Jacobian matrix of H(·):

[0060]

[0061] Calculating the partial derivatives of the above matrix gives the specific result:

[0062]

[0063] Since the measurement matrix H(·) does not contain acceleration variables, there will be two columns of zero vectors in the Jacobian matrix.

[0064] Specifically, step 3 is as follows: at time n-1, the centroid of the two trajectories is denoted by G 1 (n-1) and G 2 (n-1), in the prediction step, the trajectory center of mass at time n is predicted according to the motion model, and the predicted trajectory center of mass is marked as G 1,apr (n), G 2,apr (n);

[0065] The Kalman filter prediction step includes the state vector s(n-1) at time n-1 and the error covariance matrix P(n-1); the error covariance matrix is ​​defined as: P(n) = Cov(s(n)-s apr (n)); when the input measurement value is obtained at time n-1, the prior state s apr (n) and the prior error covariance estimate P apr(n) can be obtained by the following formula:

[0066] s apr (n) = Fs(n-1)

[0067] P apr (n) = FP(n-1)F T +Q(n-1)

[0068] The above equations reflect the prediction step of the Kalman filter. Q(n) is the process noise covariance matrix mentioned in the above step; in addition, in the prediction process we also calculate H(s apr (n)) Convert the predicted prior state from rectangular coordinates to polar coordinates, which will be used in subsequent update steps.

[0069] Specifically, step 4 is as follows: a set of measurements obtained from the detection layer at time n needs to be associated with a unique trajectory; then the distance metric between the measurement and the predicted trajectory centroid is calculated, which is determined by the scoring function, and the measurement is assigned to the closest trajectory; finally, for each trajectory, the average of all associated measurements is calculated. Specifically, it includes three steps: threshold, scoring and trajectory association allocation.

[0070] Further, step 5 is as follows: for the measurement vectors not associated with any track, i.e., the points outside the threshold function, a new group tracker will be initialized and assigned; this is an iterative process and is only performed on the remaining points;

[0071] First, an arbitrary point is selected as the starting point of the allocation, and a center point equal to it is set; then the remaining points are traversed to determine whether the remaining points and the center point meet the speed check and distance check; if the check is passed, the remaining points will be added to the cluster and the center point will be recalculated; after completion, the newly generated cluster will be further checked, including whether the number of point clouds in the cluster exceeds the minimum point cloud number threshold, a strong enough signal-to-noise ratio, and the minimum dynamics of the center of mass; if the above checks are passed, a new tracking object will be allocated and the discrete matrix will be initialized with the associated points; clusters that fail will be ignored.

[0072] Further, step 6 is specifically as follows: The updating process is as follows, and the updating step is performed only when there are enough dynamic points associated with the trajectory;

[0073] (1) For each trajectory, first calculate the average of all associated measurements If desired, this average can also be a signal-to-noise ratio weighted average;

[0074] (2) Using the average value obtained in (1), calculate the update amount, also called measurement error, H(s apr (n)) has been calculated in step 3;

[0075]

[0076] (3) Calculate the group measurement residual covariance matrix

[0077]

[0078] R c is the measurement error covariance matrix of the centroid, which is used for Kalman update and is defined as follows:

[0079]

[0080] is the group discrete matrix, R m is the measurement covariance matrix, N A is the number of measurement points associated with the trajectory, is the estimated number of points in a given trajectory;

[0081]

[0082] Note that when There is no false detection. All points are detected; when There is only a single associated measurement;

[0083] Estimated number of points By recursion we get:

[0084]

[0085] Group Discrete Matrix The estimate at time n describes the degree of discreteness of the target point cluster at this time. For the measurement vector The discrete matrix D is defined as follows:

[0086]

[0087] In the above matrix, From The covariance between all measurements and the centroid in the target point cluster is calculated in three aspects; N represents the total number of measurements in the target point cluster; is the centroid of a given set of measurements, then the group discrete matrix D n The discrete matrix D is updated as follows:

[0088]

[0089] is the weight factor, which describes the weight of the target discreteness calculated in the current frame in the target accumulated multi-frame discreteness. A and When they are close, D will be given a higher weight;

[0090] (4) Using the prediction error covariance P apr (n) and the group measurement residual covariance C(n) calculate the Kalman gain:

[0091]

[0092] (5) Calculate the posterior state vector s(n) based on the update amount y(n) and the Kalman gain K(n):

[0093] s(n)=s apr (n)+K(n)y(n)

[0094] (6) Update the posterior error covariance matrix P(n):

[0095] P(n)=P apr (n)-K(n)J H (s apr (n))P apr (n).

[0096] Further, the point cloud clustering and tracking method based on the group tracking algorithm according to claim 1 is characterized in that, in step 7: the tracked target switches between the following three states, the three states being detection, activity and idle;

[0097] When a track is assigned, the target being tracked is in the detection state; when a track is in the active state, it means that it has passed all checks and become an active target; when a track is in the idle state, it means that the target has disappeared.

[0098] The transitions between states are maintained by state transition parameters;

[0099] When the counter value exceeds the corresponding threshold, the following four state transitions will occur: idle to detect, detected activity, detected idle, and active to idle.

[0100] The present invention has the following beneficial effects:

[0101] 1. The present invention proposes a point cloud clustering and tracking method based on a group tracking algorithm, which performs tracking while clustering. At the same time, the multi-dimensional point cloud information of the millimeter wave radar is used to reduce the influence of noise, target occlusion, multipath effect, etc., and output a stable and reliable tracking trajectory.

[0102] 2. The present invention can improve the performance of continuous tracking and discontinuous tracking by increasing the states of the target trajectory and maintaining the transitions between the states through state transfer parameters.

[0103] 3. The present invention is highly portable and only needs to adapt the data interface. It is applicable to point cloud data output by various millimeter-wave radars. The parameter configuration is flexible and is suitable for tracking short, medium and long distance segments and various types of targets. This scheme uses a uniform acceleration model to reasonably describe the motion state of general objects. Since the uniform acceleration model is a nonlinear system and is not suitable for Kalman filtering, an extended Kalman filter is used to linearize the state vector to the measurement vector, which facilitates subsequent Kalman filtering operations. The prediction step uses the classic Kalman filter operation to predict the position of the trajectory at the next moment.

[0104] 4. The threshold function in the association, i.e. the defined group error covariance matrix, takes into account the mobility of the target, the discreteness of the group, and the measurement noise, limiting the scope of the association. The scoring function is the basis for determining which track a measurement point is assigned to when it falls within the threshold of multiple tracks. The measurement point will be assigned to the track with the highest score, so that the association process can distinguish different targets even when the point cloud is dense.

[0105] 5. The assignment process uses multi-dimensional point cloud information such as distance, speed, signal-to-noise ratio, etc. to assign unassociated points to a new trajectory in a reasonable way. The update operation is performed only when there are enough dynamic points associated with the trajectory. In the update step, the Kalman gain is calculated by calculating parameters such as the group error covariance matrix, the group discrete matrix, and the estimated number of points in the trajectory. Finally, the posterior state vector and the posterior error covariance matrix are updated.

[0106] 6. During the maintenance process, by adding three different states of the target trajectory and maintaining the transition between states through state transfer parameters, the performance of continuous tracking and discontinuous tracking can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] In order to more clearly and completely describe the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings used in the technical description.

[0108] Figure 1 is a flow chart of the algorithm of the present invention;

[0109] Figure 2 is the state transition diagram of the maintenance step;

[0110] Figure 3 is the radar output data format;

[0111] Figure 4 This is a road vehicle tracking simulation result diagram;

[0112] Figure 5 This is a road vehicle tracking effect diagram. DETAILED DESCRIPTION

[0113] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0114] Step 1. Determine the motion model;

[0115] For moving objects, we believe that their speed changes are reasonable, so we use the uniform acceleration model. The uniform acceleration model assumes that the target moves with uniform acceleration. We use the Kalman filter to redefine the target's position estimate. The state of the Kalman filter at time n is defined as:

[0116] s(n)=Fs(n-1)+Γw(n)

[0117] Here the state vector s(n) is defined in Cartesian coordinates:

[0118]

[0119] in are the velocity components in the x and y directions at time n, are the acceleration components in the x-direction and y-direction at time n, respectively. In the actual motion process, the target acceleration may have random disturbances, which are called process noise and are represented by w(n). Γ is the noise gain, which is defined as:

[0120]

[0121] w(n) is the process noise vector defined in the rectangular coordinate system, and its covariance matrix Q(n) = E[Γw 2 (n)Γ T ] is a 6x6 matrix for the two-dimensional uniform acceleration model. T is the radar frame time. In Kalman filter terminology, the process noise covariance matrix Q(n) represents the deviation between the actual state of the system and the motion model.

[0122] F is the state transfer matrix, which is used to specify the dynamic characteristics of the motion model. The state transfer matrix of the two-dimensional uniform acceleration model is:

[0123]

[0124] Expanding S(n) yields:

[0125]

[0126]

[0127]

[0128]

[0129]

[0130]

[0131] From the expanded results, we can see that the current position estimate is the result of the previous position plus the speed of the previous position multiplied by time T to get a small offset and finally add noise. The uniform acceleration model adds the acceleration component, resulting in the addition of new distance components and velocity components.

[0132] The input vector u(n) represents the measurement value of the nth point cloud acquired by the radar, including the radial distance, azimuth, and radial velocity of the nth point cloud:

[0133]

[0134] The relative relationship between the state quantity s(n) and the input vector u(n) of the Kalman filter is expressed as follows:

[0135] u(n)=H(s(n))+v(n)

[0136] Among them, H(·) represents the measurement matrix, which is used to convert points in the rectangular coordinate system to polar coordinates, v(n) is the measurement noise vector defined in polar coordinates, and its covariance matrix R(n) is a 3x3 matrix. The specific expression of the measurement matrix is:

[0137]

[0138] Step 2. Extended Kalman filter linearization: Use the extended Kalman filter to linearize the state vector to the measurement vector;

[0139] In the above formula, there is a nonlinear relationship between the input vector u(n) and the state vector s(n), namely:

[0140]

[0141]

[0142]

[0143] Therefore, we use the extended Kalman filter to simplify the relationship between u(n) and s(n). The specific operation is to only retain the first-order Taylor expansion of H(·), that is, f(x) = f(x 0 )+f′(x 0 )(xx0 ) is:

[0144] u(n)≈H(s apr (n))+J H (s apr (n))[s(n)-s apr (n)]

[0145] Among them, s apr (n) is the prior estimate of the state vector at time n based on the measured value at time n-1, is the predicted value, J H (s) is the Jacobian matrix of H(·):

[0146]

[0147] Calculating the partial derivatives of the above matrix gives the specific result:

[0148]

[0149] Since the measurement matrix H(·) does not contain acceleration variables, there will be two columns of zero vectors in the Jacobian matrix.

[0150] Step 3. Prediction step: Use Kalman filter operation to predict the position of the trajectory at the next moment;

[0151] At time n-1, the centroid of the two trajectories is denoted by G 1 (n-1) and G 2 (n-1), in the prediction step, the trajectory center of mass at time n is predicted according to the motion model, and the predicted trajectory center of mass is marked as G 1,apr (n), G 2,apr (n). Figure 1 As shown, the Kalman filter prediction step includes the state vector s(n-1) at time n-1 and the error covariance matrix P(n-1). The error covariance matrix is ​​defined as: P(n) = Cov(s(n) - s apr (n)). When the input measurement value is obtained at time n-1, the prior state s apr (n) and the prior error covariance estimate P apr (n) can be obtained by the following formula:

[0152] s apr (n) = Fs(n-1)

[0153] P apr (n) = FP(n-1)F T +Q(n-1)

[0154] The above equations reflect the prediction step of the Kalman filter. Q(n) is the process noise covariance matrix mentioned in the above step. In addition, we also calculate H(s apr (n)) Convert the predicted prior state from rectangular coordinates to polar coordinates, which will be used in subsequent update steps.

[0155] Step 4. Association step: The association step mainly associates the radar measurements with an existing tracking trajectory around each predicted centroid (i.e., G 1,apr (n) and G 2,apr (n)) Construct a measurement boundary whose shape is determined by the threshold function.

[0156] A set of measurements obtained from the detection layer at time n needs to be associated with a unique track. Then the distance metric between the measurement and the predicted track centroid is calculated. This metric is determined by the scoring function, which assigns the measurement to the track that is closest to it. Finally, for each track, the average of all associated measurements is calculated. The above is Figure 1 There are three steps in ,thresholding, scoring and trajectory association assignment.

[0157] (1) Threshold function

[0158] The threshold function represents the amount of update we are willing to accept given the uncertainty in the current track. As defined above, the update is a measure of the difference between our prediction and the measurement we observe. For a track i and a measurement vector u j The update amount is defined as:

[0159]

[0160] To quantify the uncertainty, we define the group residual covariance matrix C G :

[0161]

[0162] Group residual covariance matrix C G It is relative to a member of the observation group and the group centroid, and describes the deviation between each measurement value and the group centroid. Comparing the above group residual covariance calculation formula with the corresponding covariance formula for single target tracking, we can find that the formula represents the uncertainty of the group centroid due to the target mobility, which is consistent with the prior covariance formula P in single target tracking. apr (n) = FP(n-1)F T +Q(n-1) is similar. Mis the diagonal matrix of measurement variance, describing the error caused by the radar in collecting data. It represents the estimation of the group discrete matrix, which describes the discrete degree of all measurement points in the point cluster formed by the target, and refers to the discrete degree accumulated over multiple frames. The above values ​​are calculated in the update step.

[0163] For each existing tracking trajectory i, for all measurement vectors j acquired at time n, we define the distance function Indicates the size of the incremental update when new measurements are added to an existing track:

[0164]

[0165]

[0166] Define a chi-squared test to bound the size of the update error we are willing to accept:

[0167]

[0168] G represents the maximum normalized distance we are willing to accept from a measurement vector in the group.

[0169] (2) Scoring steps

[0170] In the scoring step, we assign a score to each measurement that falls within the trajectory threshold using the Mahalanobis distance metric and the likelihood function, and then the assignment process assigns one measurement at a time to the closest trajectory. This creates a set of measurements associated with each trajectory.

[0171] According to the literature, the likelihood function for assigning observation j to trajectory i is as follows:

[0172]

[0173] Among them, |C Gi | is the determinant of the group error covariance matrix of trajectory i. In order to maximize g ij , the maximum value of the likelihood function is derived by taking the logarithm:

[0174]

[0175] The reference in this step is "Multiple-Target Tracking with Radar Applications" (Artech House, Samuel S. Blackman. 1986).

[0176] (3) Track association assignment: Assign the measurement value j to the track i with the highest score.

[0177] Step 5. Assignment step: For the measurement vectors that are not associated with any track, i.e., points outside the threshold function, a new group tracker will be initialized and assigned;

[0178] This is an iterative process, similar to the DBSCAN algorithm, but simpler than DBSCAN because it only operates on the remaining points.

[0179] First, an arbitrary point is selected as the starting point for the allocation, and a center point equal to it is set. Then the remaining points are traversed to determine whether the remaining points and the center point meet the speed check and distance check. If the check passes, the remaining points will be added to the cluster and the center point will be recalculated. After completion, the newly generated cluster will be further checked, including whether the number of point clouds in the cluster exceeds the minimum point cloud number threshold, a strong enough signal-to-noise ratio, and the minimum dynamics of the center of mass. If all the above checks pass, a new tracking object will be allocated and the discrete matrix will be initialized with the associated points. Clusters that fail will be ignored.

[0180] Step 6. Update step: predict the centroid G 1,apr (n) and the measured average The difference between is called the update amount, which measures how different our predictions are from the observed measurements; using the update amount, in the update step the trajectory center is updated to G 1 (n), and the point cloud trajectory can be clustered and tracked by repeating this process.

[0181] The updating process is shown below. The updating step is performed when there are enough dynamic points associated with the trajectory.

[0182] (1) For each trajectory, first calculate the average of all associated measurements If desired, this average can also be a signal-to-noise ratio weighted average.

[0183] (2) Using the average value obtained in (1), calculate the update amount, also called measurement error, H(s apr (n)) has been calculated in step 3.

[0184]

[0185] (3) Calculate the group measurement residual covariance matrix

[0186]

[0187] R c is the measurement error covariance matrix of the centroid, which is used for Kalman update and is defined as follows:

[0188]

[0189] is the group discrete matrix, R m is the measurement covariance matrix, N A is the number of measurement points associated with the trajectory, is the estimated number of points in a given trajectory.

[0190]

[0191] Note that when There is no false detection. All points are detected. There is only a single associated measurement.

[0192] Estimated number of points By recursion we get:

[0193]

[0194] Group Discrete Matrix The estimate at time n describes the degree of discreteness of the target point cluster at this time. For the measurement vector The discrete matrix D is defined as follows:

[0195]

[0196] In the above matrix, From The covariance between all measurements and the centroid in the target point cluster is calculated from three aspects. N represents the total number of measurements in the target point cluster. is the centroid of a given set of measurements, then the group discrete matrix D n The discrete matrix D is updated as follows:

[0197]

[0198] is the weight factor, which describes the weight of the target discreteness calculated in the current frame in the target accumulated multi-frame discreteness. A and When they are close, D is given a higher weight.

[0199] (4) Using the prediction error covariance P apr (n) and the group measurement residual covariance C(n) calculate the Kalman gain:

[0200]

[0201] (5) Calculate the posterior state vector s(n) based on the update amount y(n) and the Kalman gain K(n):

[0202] s(n)=s apr (n)+K(n)y(n)

[0203] (6) Update the posterior error covariance matrix P(n):

[0204] P(n)=P apr (n)-K(n)J H (s apr (n))P apr (n)

[0205] Step 7. Maintenance step: by adding three different states of the target trajectory and maintaining the transitions between states through state transition parameters.

[0206] The maintenance step plays an important role in the discontinuous tracking of the target. The tracked target is in one of the following three states, namely, detection, activity, and idle.

[0207] When a track is assigned, it is in the detection state. It is worth noting that assigning a track does not mean that it is a target. As mentioned in step 5, it must pass all checks to become a target.

[0208] When a track is in the active state, it means that it has passed all checks and become an active target. According to the state and position of the target, the active target can be subdivided into four situations, namely normal, static, exited, and dormant. The normal state means that the target is moving and in the static bounding box, the static state means that the target is stationary and in the static bounding box, the exit state means that the target track is not associated with the point cloud outside the static bounding box, and the dormant state means that the target track is not associated with the point cloud inside the static bounding box.

[0209] When a track is in the idle state, it means that the target has disappeared.

[0210] The transition between states is maintained by the state transfer parameters. The state transfer flow chart is as follows: Figure 2 As shown. A hit event is defined as any point cloud associated with the target track, and a miss event is defined as no point cloud associated with the target track. For each frame of data, the tracking track either obtains a hit event or a miss event. For each state transition, a separate counter is maintained to count the number of hit or miss events. When the counter value exceeds the corresponding threshold, the following state transitions occur:

[0211] (1) Idle to Detect

[0212] Once a track is in the idle state, step 5 will be executed to transfer to the detection state.

[0213] (2) Activity detected

[0214] When a trajectory in the detection state encounters a hit event, the detected activity counter will increase, and when it exceeds the threshold, it will transition from the detection state to the active state.

[0215] (3) Idle detected

[0216] When a trajectory in the detection state encounters a miss event, the detected idle counter will increase, and when it exceeds the threshold, it will transition from the detection state to the idle state.

[0217] (4) Active to Idle

[0218] When the trajectory is in the active state, the target can be in the normal, exit, sleep or static state. When the active state trajectory encounters a miss event, the active to idle counter increases. When the thresholds corresponding to the above four states are exceeded, it will transition from the active state to the idle state. When a hit event occurs, the active to idle counter is cleared.

[0219] Step 8. Define data interface: A data interface needs to be defined between the point cloud data output by the radar and the input of the group tracking algorithm so that the algorithm can process the output point cloud data in a timely manner.

[0220] The data output by the radar consists of frames, each of which includes a frame header and point cloud data, such as Figure 3 The specific formats of the frame header and point cloud data are shown in Table 1 and Table 2. The frame header size is 16 bytes, and the size of each point cloud data packet is 8 bytes. Note that the total amount of data in a frame is an integer multiple of 4 bytes. If the data length is not an integer multiple of 4 bytes, random numbers will be filled to make the data size an integer multiple of 4 bytes.

[0221] Table 1: Specific contents of Frame Header

[0222]

[0223] Table 2: Point Data details

[0224] name Type, number of bytes describe Range 'uint16',2B Target radial distance, unit: cm Angle 'int16',2B Target angle, unit: 0.0001rad Velocity 'int16',2B Target radial velocity, unit: 0.01m / s SNR 'int16',2B Target signal-to-noise ratio

[0225] In the present invention, the uniform acceleration model used in step 1 is not unique, and a simpler uniform velocity model or a more complex high-order motion model can also be used. The extended Kalman filter linearization in step 2 can also use the unscented Kalman filter method using higher-order statistics. The distance metric in the scoring function in step 4 can also use Manhattan distance, Euclidean distance metric, etc. The design method of the likelihood function is also not unique. The use of other clustering methods in the allocation process in step 5 can also achieve the purpose of allocation. Setting multiple trajectory states and state transition parameters in the maintenance step in step 7 can also achieve the purpose of maintaining the trajectory state.

[0226] A specific application example of the algorithm of the present invention is to collect the traffic flow data of the three-lane 0-500m road condition and input it into the algorithm simulation program, and the result obtained is as follows: Figure 4 As shown in the figure, the colored dots represent the point cloud data of the vehicle, and the blue continuous track represents the vehicle tracking result. From the simulation results, we can find that for discrete and sparse point clouds, we consider them to be clutter or noise, and do not track them. For continuous and dense points, we consider them to be a moving vehicle and cluster and track them. The tracking results are continuous and clear, reflecting the actual driving status of the vehicle.

[0227] The traffic flow data is collected on the actual one-way three-lane 0-500m road condition. After completing the above steps, the output results are uploaded to the host computer to obtain the display results as follows Figure 5 As shown in the figure, the red circle represents the position of the tracked target in the Cartesian coordinate system (in meters), and also contains the target's ID, distance, and speed information. The yellow vertical line represents the lane line, which can be set in the host computer. Here, three lanes are set. Most of the vehicles in the figure fall into the set lanes, and the vehicles in adjacent lanes can be well distinguished. Vehicles with similar distances in the same lane can also be distinguished, indicating that the algorithm is effective and feasible.

[0228] Millimeter waves have the characteristics of wide bandwidth and short wavelength, and have the advantages of all-day, all-weather, and high resolution. Millimeter wave radar is considered to be one of the most promising sensor technologies besides millimeter wave communication. At present, millimeter wave radar is widely used in self-driving cars, smart homes and industrial applications. Its functions include but are not limited to traffic flow detection, activity recognition, posture estimation, gesture recognition, etc. The data output by millimeter wave radar mostly contains multi-dimensional point cloud information such as distance, azimuth, pitch, speed, signal-to-noise ratio, etc. How to obtain effective and correct information from massive amounts of information is a major problem in millimeter wave technology.

[0229] The present invention is aimed at the application of millimeter wave radar in multi-target tracking scenarios. It uses the multi-dimensional point cloud information of millimeter wave radar to reduce the influence of noise, target occlusion, multipath effect, etc., and outputs stable and reliable tracking trajectories. It is applicable to point cloud data output by various millimeter wave radars. The parameter configuration is flexible and it is suitable for tracking short, medium and long distance segments and various types of targets. It has good performance in scenes with high target density and complex motion trajectories such as pedestrian monitoring and traffic flow monitoring.

Claims

1. A point cloud clustering and tracking method based on a group tracking algorithm, characterized in that: The method comprises the following steps: Step 1. Determine the motion model; Step 2. Extended Kalman filter linearization: Use the extended Kalman filter to linearize the state vector to the measurement vector; Step 3. Prediction step: Use Kalman filter operation to predict the position of the trajectory at the next moment; Step 4. Association step: The association step mainly associates the radar measurement value with an existing tracking trajectory and constructs a measurement boundary around each predicted centroid. The shape of this boundary is determined by the threshold function. Step 5. Assignment step: For the measurement vectors that are not associated with any track, i.e., points outside the threshold function, a new group tracker will be initialized and assigned; Step 6. Update step: predict the centroid G 1,apr (n) and the measured average The difference between them is called the update amount, which measures the degree of difference between the predicted value and the observed measurement value. Using the update amount, the trajectory center will be updated to G1(n) in the update step, and this can be repeated to achieve clustering and tracking of point cloud trajectories. Step 7. Maintenance step: by adding three different states of the target trajectory and maintaining the transition between states through state transition parameters; Step 8. Define data interface: A data interface needs to be defined between the point cloud data output by the radar and the input of the group tracking algorithm so that the algorithm can process the output point cloud data in a timely manner.

2. The point cloud clustering and tracking method based on the group tracking algorithm according to claim 1, characterized in that: Step 1 is as follows: For a moving object, we believe that its velocity change is reasonable, so we use a uniform acceleration model; the uniform acceleration model assumes that the target moves with uniform acceleration, and we use a Kalman filter to redefine the target's position estimate. The state of the Kalman filter at time n is defined as: s(n)=Fs(n-1)+Γw(n) Here the state vector s(n) is defined in Cartesian coordinates: in are the velocity components in the x and y directions at time n, are the acceleration components in the x-direction and y-direction at time n, respectively. In the actual motion process, the target acceleration may have random disturbances, which are called process noise and are represented by w(n); Γ is the noise gain, which is defined as: w(n) is the process noise vector defined in the rectangular coordinate system, and its covariance matrix Q(n) = E[Γw 2 (n)Γ T ] is a 6x6 matrix for the two-dimensional uniform acceleration model; T is the radar frame time; In Kalman filter terminology, the process noise covariance matrix Q(n) represents the deviation between the actual state of the system and the motion model; F is the state transfer matrix, which is used to specify the dynamic characteristics of the motion model. The state transfer matrix of the two-dimensional uniform acceleration model is: Expanding S(n) yields: From the expanded result, we can see that the current position estimate is the result of the previous position plus the speed of the previous position multiplied by the time T to get a small offset and finally add noise; The uniform acceleration model adds the acceleration component, which results in the addition of new distance components and velocity components; The input vector u(n) represents the measurement value of the nth point cloud acquired by the radar, including the radial distance, azimuth, and radial velocity of the nth point cloud: The relative relationship between the state quantity s(n) and the input vector u(n) of the Kalman filter is expressed as follows: u(n)=H(s(n))+v(n) Among them, H(·) represents the measurement matrix, which is used to convert points in the rectangular coordinate system to polar coordinates, v(n) is the measurement noise vector defined in polar coordinates, and its covariance matrix R(n) is a 3x3 matrix. The specific expression of the measurement matrix is:

3. The point cloud clustering and tracking method based on the group tracking algorithm according to claim 2, characterized in that: Step 2 is specifically as follows: In the above formula, there is a nonlinear relationship between the input vector u(n) and the state vector s(n), namely: Therefore, an extended Kalman filter is used to simplify the relationship between u(n) and s(n); the specific operation is to retain only the first-order Taylor expansion of H(·), that is, the formula of the form f(x) = f(x0) + f′(x0)(x-x0): u(n)≈H(s apr (n))+J H (s apr (n))[s(n)-s apr (n)] Among them, s apr (n) is the prior estimate of the state vector at time n based on the measured value at time n-1, is the predicted value, J H (s) is the Jacobian matrix of H(·): Calculating the partial derivatives of the above matrix gives the specific result: Since the measurement matrix H(·) does not contain acceleration variables, there will be two columns of zero vectors in the Jacobian matrix.

4. The point cloud clustering and tracking method based on the group tracking algorithm according to claim 1, characterized in that: Step 3 is as follows: at time n-1, the centroids of the two trajectories are recorded as G1(n-1) and G2(n-1). In the prediction step, the centroid of the trajectory at time n is predicted based on the motion model, and the predicted centroid of the trajectory is marked as G 1,apr (n), G 2,apr (n); The Kalman filter prediction step includes the state vector s(n-1) at time n-1 and the error covariance matrix P(n-1); the error covariance matrix is ​​defined as: P(n) = Cov(s(n)-s apr (n)); when the input measurement value is obtained at time n-1, the prior state s apr (n) and the prior error covariance estimate P apr (n) can be obtained by the following formula: s apr (n)=Fs(n-1) P apr (n)=FP(n-1)F T +Q(n-1) The above equations reflect the prediction step of the Kalman filter; Q(n) is the process noise covariance matrix mentioned in the above steps; in addition, during the prediction process we also calculate H(s apr (n)) Convert the predicted prior state from rectangular coordinates to polar coordinates, which will be used in subsequent update steps.

5. The point cloud clustering and tracking method based on group tracking algorithm according to claim 1, characterized in that: Step 4 is as follows: a set of measurements obtained from the detection layer at time n needs to be associated with a unique trajectory; then the distance metric between the measurement value and the predicted trajectory centroid is calculated, which is determined by the scoring function, and the measurement value is assigned to the closest trajectory; finally, for each trajectory, the average value u(n) of all associated measurements is calculated; specifically, it includes three steps: threshold, scoring, and trajectory association assignment.

6. The point cloud clustering and tracking method based on group tracking algorithm according to claim 1, characterized in that: Step 5 is as follows: for the measurement vectors that are not associated with any track, i.e., the points outside the threshold function, a new group tracker will be initialized and assigned; this is an iterative process and is only performed on the remaining points; First, an arbitrary point is selected as the starting point of the allocation, and a center point equal to it is set; then the remaining points are traversed to determine whether the remaining points and the center point meet the speed check and distance check; if the check is passed, the remaining points will be added to the cluster and the center point will be recalculated; after completion, the newly generated cluster will be further checked, including whether the number of point clouds in the cluster exceeds the minimum point cloud number threshold, a strong enough signal-to-noise ratio, and the minimum dynamics of the center of mass; if the above checks are passed, a new tracking object will be allocated and the discrete matrix will be initialized with the associated points; clusters that fail will be ignored.

7. The point cloud clustering and tracking method based on group tracking algorithm according to claim 4, characterized in that: Step 6 is specifically as follows: The update process is as follows, and the update step is performed only when there are enough dynamic points associated with the trajectory; (1) For each trajectory, first calculate the average of all associated measurements If desired, this average can also be a signal-to-noise ratio weighted average; (2) Using the average value obtained in (1), calculate the update amount, also called measurement error, H(s apr (n)) has been calculated in step 3; (3) Calculate the group measurement residual covariance matrix R c is the measurement error covariance matrix of the centroid, which is used for Kalman update and is defined as follows: is the group discrete matrix, R m is the measurement covariance matrix, N A is the number of measurement points associated with the trajectory, is the estimated number of points in a given trajectory; Note that when There is no false detection. All points are detected; when There is only a single associated measurement; Estimated number of points By recursion we get: Group Discrete Matrix The estimate at time n describes the degree of discreteness of the target point cluster at this time. For the measurement vector The discrete matrix D is defined as follows: In the above matrix, From r, The covariance between all measurements and the centroid in the target point cluster is calculated in three aspects; N represents the total number of measurements in the target point cluster; is the centroid of a given set of measurements, then the group discrete matrix D n The discrete matrix D is updated as follows: is the weight factor, which describes the weight of the target discreteness calculated in the current frame in the target accumulated multi-frame discreteness. A and When they are close, D will be given a higher weight; (4) Using the prediction error covariance P apr (n) and the group measurement residual covariance C(n) calculate the Kalman gain: (5) Calculate the posterior state vector s(n) based on the update amount y(n) and the Kalman gain K(n): s(n)=s apr (n)+K(n)y(n) (6) Update the posterior error covariance matrix P(n): P(n)=P apr (n)-K(n)J H (s apr (n))P apr (n)。 8. The point cloud clustering and tracking method based on group tracking algorithm according to claim 1, characterized in that: In step 7: the tracked target switches between the following three states: detection, activity, and idle; When a track is assigned, the target being tracked is in the detection state; when a track is in the active state, it means that it has passed all checks and become an active target; when a track is in the idle state, it means that the target has disappeared. The transitions between states are maintained by state transition parameters; When the counter value exceeds the corresponding threshold, the following four state transitions will occur: idle to detect, detected activity, detected idle, and active to idle.

9. The point cloud clustering and tracking method based on group tracking algorithm according to claim 1, characterized in that: The motion model in step 1 is a uniform acceleration model. For the motion of objects, this assumption is basically valid in a short period of time.

Images