Industrial environment millimeter wave radar target detection and tracking method and system

Abstract

The invention discloses an industrial environment millimeter wave radar target detection and tracking method and system, and the method comprises the steps: collecting the polar coordinate parameters of a target in an industrial scene through a millimeter wave radar module, converting the polar coordinate parameters into the point cloud information of spatial distribution, and enabling the polar coordinate parameters to comprise the distance, the azimuth angle and the radial speed under a polar coordinate system; calculating a multi-dimensional distance between the point cloud information based on the radial speed constraint, and generating a target measurement value according to the multi-dimensional distance clustering; reconstructing polar coordinate parameters based on the target prediction state, and dynamically adjusting a Cartesian coordinate system to measure a noise covariance matrix according to the reconstructed polar coordinate parameters; and fusing the target measurement value and the noise covariance matrix to obtain target state estimation. According to the industrial environment millimeter wave radar target detection and tracking method and system, the precision of target tracking in the industrial environment is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial automation target perception, and in particular to an industrial environment millimeter wave radar target detection and tracking method and system. BACKGROUND

[0002] Currently, real-time monitoring of various targets in industrial environments is an important prerequisite for promoting intelligent production. By analyzing the monitored system data, dangerous hidden information and fault information are reminded to improve safety production efficiency and achieve the purpose of safety, high efficiency and staff reduction. However, the current target detection and tracking system in the industrial environment faces many challenges. Unlike mainstream environmental perception sensors such as lasers and cameras, millimeter wave radars have strong penetration, speed sensitivity and all-weather working characteristics, making them ideal perception solutions for dense environments (such as warehouse workshops and assembly lines). However, the inherent defects of millimeter wave radars will cause two major problems: first, the clustering distortion caused by point cloud sparsity. Millimeter wave radar point clouds have the "near dense and far sparse" feature. Traditional clustering algorithms are prone to target splitting or merging errors in sparse areas, resulting in serious measurement position deviation. Second, the error of polar coordinate conversion is nonlinear. The Jacobian matrix in the measurement noise covariance conversion formula in the Cartesian coordinate system contains a distance term, which makes the same angle error produce linear amplification of the horizontal error at a long distance. If not handled properly, it will cause many adverse effects, such as target misjudgment caused by clustering distortion, and trajectory jump caused by coordinate error nonlinear expansion.

[0003] Under the above background, the target detection and tracking technology based on millimeter wave radar is researched, and the technical framework of multi-millimeter wave radar collaborative perception, millimeter wave radar and vision fusion, and millimeter wave radar fusion deep neural network is preliminarily formed, realizing high-precision tracking.

[0004] When using millimeter wave radars for industrial site target detection and tracking tasks, three constraints of industrial environments on millimeter wave radars need to be considered: multipath interference constraint, dynamic noise modeling deficiency, and real-time constraint. Therefore, a point cloud similarity calculation method considering speed constraint is needed, and the noise model is adjusted in real time according to the characteristics of millimeter wave radars, and a millimeter wave radar target detection and tracking system suitable for industrial environments is invented. SUMMARY

[0005] To solve the technical problems in the background art, the present application proposes an industrial environment millimeter wave radar target detection and tracking method and system.

[0006] The industrial environment millimeter wave radar target detection and tracking method proposed by the present application comprises: The polar coordinate parameters of the target in the industrial scene are collected by the millimeter wave radar module, and the polar coordinate parameters are converted into spatially distributed point cloud information, and the polar coordinate parameters include distance, azimuth angle and radial velocity in the polar coordinate system; A multi-dimensional distance between the point cloud information is calculated based on the radial velocity constraint, and a target measurement value is generated according to the multi-dimensional distance clustering; The polar coordinate parameters are reconstructed based on the target predicted state, and the measurement noise covariance matrix of the Cartesian coordinate system is dynamically adjusted according to the reconstructed polar coordinate parameters; The target state estimation is obtained by fusing the target measurement value and the noise covariance matrix.

[0007] Preferably, the multi-dimensional distance between the point cloud information is calculated based on the radial velocity constraint, specifically: ; Wherein, and are the distance dimension weight coefficient and the speed dimension weight coefficient respectively; represents the spatial distance of two point clouds, ; represents the absolute value of the difference of the radial velocity between two point clouds, ; is a direction penalty term coefficient, which is used to amplify the speed difference between two point clouds; is a conditional expression, when is less than 0, the value is 1, when is greater than or equal to 0, the value is 0; is a multi-dimensional distance.

[0008] Preferably, the target measurement value is generated according to the multi-dimensional distance clustering, specifically: The point cloud information belonging to the same target is classified into the same point cloud cluster, the center of the point cloud cluster is calculated, and the center is taken as the target measurement value.

[0009] Preferably, the state equation of the target predicted state is: The CV model is used to model the target in the industrial environment: ; Wherein, is the state vector of the target predicted state, which can be expressed as , respectively represent the position of the target, respectively represent the differential of the target in the two directions, i.e. speed; is a state transition matrix, ; is the sampling interval of the system, and is a process noise sequence; The covariance matrix of is the noise covariance matrix.

[0010] Preferably, fusing the target measurement value and the noise covariance matrix to obtain the target state estimate specifically includes: The measurement equation of the extended Kalman tracking filter is constructed based on the target measurement value. The measurement equation is specifically: ; in, is the measurement noise sequence, and its covariance matrix is ; is the target measurement value; The measurement equation is subjected to a first-order Taylor expansion to become a linear equation. The linear equation is specifically: ; in, yes exist The first-order term of the first-order Taylor expansion at ; The linear equation Kalman gain is: ; Among them, the prediction error covariance matrix for: ; The target state calculated by the extended Kalman filter is: ; In summary, the estimated state of the target at each observation moment is obtained, that is, the position of the target at each observation moment and its speed relative to the ground.

[0011] Preferably, reconstructing polar coordinate parameters based on the target prediction state specifically includes: The measurement noise covariance matrix in polar coordinates is defined as: ; According to the error propagation formula, the measurement noise covariance matrix in the Cartesian coordinate system of the position dimension is The Jacobian matrix To do the conversion:

[0012] The covariance of each position component in the Cartesian coordinate system can be obtained as follows: ; ; ; ; wherein, is the error variance of each measurement dimension in radar polar coordinates; is the error variance of each dimension in Cartesian coordinates; is the error variance of coupling of X and Y directions; is the angle value backstepped from the predicted state of the target; is the distance value backstepped from the predicted state of the target; At each iteration of the extended Kalman tracking filter, the polar coordinate parameters are reconstructed according to the target state predicted by the state equation.

[0013] The application provides an industrial environment millimeter wave radar target detection and tracking system, which comprises: a data acquisition module, configured to acquire polar coordinate parameters of targets in an industrial scene through a millimeter wave radar module, and convert the polar coordinate parameters into spatially distributed point cloud information, wherein the polar coordinate parameters comprise distance, azimuth angle and radial velocity in a polar coordinate system; a first processing module, configured to calculate multi-dimensional distances between the point cloud information based on radial velocity constraints, and generate target measurement values according to multi-dimensional distance clustering; a second processing module, configured to reconstruct polar coordinate parameters based on a target predicted state, and dynamically adjust a measurement noise covariance matrix of a Cartesian coordinate system based on the reconstructed polar coordinate parameters; a data fusion module, configured to fuse the target measurement values and the noise covariance matrix to obtain target state estimation.

[0014] Preferably, in the first processing module, the multi-dimensional distances between the point cloud information are calculated based on radial velocity constraints, and specifically, the multi-dimensional distances are calculated according to the following formula: ; wherein, and are a distance dimension weight coefficient and a velocity dimension weight coefficient respectively; represents a spatial distance between two point clouds, ; represents an absolute value of a difference in radial velocity between two point clouds, ; is a direction penalty term coefficient, used to amplify the difference in velocity between two point clouds; is a conditional expression, and when is less than 0, the value is 1, and when is greater than or equal to 0, the value is 0; is a multi-dimensional distance.

[0015] The industrial environment millimeter wave radar target detection and tracking method and system provided in the application solve the problem of low tracking accuracy caused by nonlinear expansion of millimeter wave radar measurement noise in an industrial environment. First, the DBSCAN clustering algorithm is improved to propose a radial velocity-constrained multi-dimensional distance formula, which solves the near-dense and far-sparse point cloud distribution problem and improves the clustering accuracy. Then, the measurement covariance in the Cartesian coordinate system is dynamically adjusted based on the error propagation law, the measurement noise is dynamically adjusted, and the state equation of the system is constructed according to the CV model; finally, the state information of the target is obtained by using the extended Kalman filter algorithm. The application can effectively improve the accuracy of target tracking in an industrial environment, and the root mean square error of the traditional Kalman filter tracking is improved from 0.25m to 0.21m. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 The working process schematic diagram of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 2 The application process structure schematic diagram of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 3 The point cloud clustering process and the coupling relationship between the clustering process and the extended Kalman tracking filter of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 4 The error nonlinear expansion schematic diagram caused by the distance change in the industrial scene that the traditional fixed covariance model cannot adapt to under the fixed angle error of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 5 The implementation process structure schematic diagram of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 6 The effect comparison diagram of the traditional DBSCAN algorithm and the improved DBSCAN algorithm of the industrial environment millimeter wave radar target detection and tracking method provided in the application; Figure 7 The tracking effect comparison diagram of the KF algorithm and the AEKF algorithm of the industrial environment millimeter wave radar target detection and tracking method provided in the application. DETAILED DESCRIPTION

[0017] REFERENCE Figures 1-7 The industrial environment millimeter wave radar target detection and tracking method provided in the application includes the following steps: S1: Collect the polar coordinate parameters of the target in the industrial scene by the millimeter wave radar module, and convert the polar coordinate parameters into spatially distributed point cloud information, the polar coordinate parameters including the distance, azimuth angle and radial velocity in the polar coordinate system.

[0018] S2: calculating multi-dimensional distance between point cloud information based on radial velocity constraint, and generating target measurement value according to multi-dimensional distance clustering.

[0019] In the embodiment, the multi-dimensional distance between point cloud information is calculated based on radial velocity constraint, specifically: wherein, and are distance dimension weight coefficient and velocity dimension weight coefficient respectively; denotes the spatial distance of two point clouds, denotes the absolute value of the difference of radial velocity between two point clouds, is a direction penalty term coefficient, used to amplify the velocity difference between two point clouds; is a conditional expression, when is less than 0, the value is 1, when is greater than or equal to 0, the value is 0; is a multi-dimensional distance.

[0020] Specifically, as shown in Figure 2 , it is composed of: a single-chip low-cost millimeter wave radar module, which can provide millimeter wave radar point cloud information required for target detection and tracking in industrial field; a server module, the system software is built on the ROS system based on Linux system, ROS uses distributed network, and uses TCP / IP-based communication mode to realize the point-to-point loose coupling link between modules, which can perform several types of communication, including topic-based asynchronous data stream communication and service-based synchronous data stream communication. Here, we take the millimeter wave radar as a node of ROS, publish the point cloud information in the industrial environment, and take the millimeter wave radar target detection and tracking system as another node of the ROS system to subscribe to the point cloud information of the millimeter wave radar, detect and track the target, and the whole system can realize real-time operation under the framework of ROS. The target detection and tracking software is the core module for realizing the function of system target detection and tracking, including two components, which are respectively an improved DBSCAN point cloud clustering algorithm component based on radial velocity constraint and an extended Kalman filter component for automatically adjusting measurement error.

[0021] Specifically, the point cloud clustering algorithm is: abandoning the method of calculating point cloud similarity by using Euclidean distance in traditional DBSCAN point cloud clustering algorithm, combining the characteristics of millimeter wave radar, such as near dense and far sparse, and Doppler information being measurable, the radial velocity constraint millimeter wave radar point cloud multi-dimensional distance is derived. As shown in Figure 3 ​​​As shown, using the above new point cloud similarity calculation method, the millimeter wave radar point cloud is clustered, and the dense point cloud cluster generated by the target from the chaotic point cloud returned by the millimeter wave radar is found, and the target is clustered into a class to obtain the unique measurement state of the target at this moment, and is applied to the extended Kalman tracking filter. As shown in Figure 4 As shown, the same angle error produces a linearly amplified lateral error at a long distance, so the measurement error matrix in the Cartesian coordinate system needs to be adjusted in real time according to the state of the target. The improved clustering performance of the system is compared as shown in Figure 6 As shown, the improved DBSCAN point cloud clustering algorithm: average error: 0.26m, maximum error: 0.72m, root mean square error: 0.31m; traditional DBSCAN point cloud clustering algorithm: average error: 0.34m, maximum error: 1.03m, root mean square error: 0.4m. The tracking performance is improved as shown in Figure 7 As shown, AEKF: root mean square error (RMSE): 0.21m; KF: root mean square error (RMSE): 0.25m.

[0022] In this embodiment, the target measurement value is generated according to multi-dimensional distance clustering, specifically: The point cloud information belonging to the same target is classified into the same point cloud cluster, the center of the point cloud cluster is calculated, and the center is taken as the target measurement value.

[0023] S3: Reconstruct the polar coordinate parameters based on the target predicted state, and dynamically adjust the measurement noise covariance matrix in the Cartesian coordinate system according to the reconstructed polar coordinate parameters.

[0024] In this embodiment, the state equation of the target predicted state is: The CV model is used to model the target in the industrial environment: ; Wherein, is the state vector of the target predicted state, which can be expressed as , respectively represent the position of the target, respectively represent the differential of the target in the two directions, i.e. the speed; is the state transition matrix, ; is the sampling interval of the system, and is a process noise sequence; is the covariance matrix of, i.e. the noise covariance matrix.

[0025] In this embodiment, the polar coordinate parameters are reconstructed based on the target predicted state, specifically including: The measurement noise covariance matrix in the polar coordinate system is defined as: ; By error propagation formula, the measurement noise covariance matrix in Cartesian coordinate system of position dimension can be converted by Jacobian matrix

[0026] The covariance of each position component in Cartesian coordinate system can be obtained as follows: where, is the error variance of each measurement dimension in radar polar coordinates; is the error variance of each dimension in Cartesian coordinate system; is the error variance of coupling of X and Y directions; is the angle value obtained by backstepping from the predicted state of the target; is the distance value obtained by backstepping from the predicted state of the target; At each iteration of the extended Kalman tracking filter, the polar coordinate parameters are reconstructed according to the predicted state of the target. Since the original data of the millimeter wave radar are all measurements in polar coordinates, the measurements in Cartesian coordinate system output by the radar are all converted measurements, and the measurement error covariance matrix in the extended Kalman tracking filter is based on Cartesian coordinate system. The traditional fixed covariance model cannot adapt to the nonlinear expansion of Cartesian coordinate system error caused by distance change in industrial scenarios.

[0027] Specifically, the following are and The specific derivation process is as follows: The first step of the extended Kalman filter is to estimate the target state at the current time according to the target state at the last time (the state is the predicted state): where, is a state vector which can be simply represented as: That is, = arctan( ); r= ; therefore, and r are strictly polar coordinate estimates obtained according to the predicted state of the extended Kalman.

[0028] Specifically, the Jacobian matrix J is the partial derivative matrix of the conversion function with respect to the polar coordinate parameters, that is:​​​​​​​ ; The radar measurement in the polar coordinate system can be expressed as: ; v is the radial velocity. The three measurement dimensions are independent of each other, so the measurement noise covariance matrix in polar coordinates can be defined. The measurement in Cartesian coordinates is , where v is also the radial velocity, then the conversion relationship between polar coordinates and Cartesian coordinates is: .

[0029] S4: Fuse the target measurement value and the noise covariance matrix to obtain the target state estimate.

[0030] In this embodiment, step S4 specifically includes: The measurement equation of the extended Kalman tracking filter is constructed based on the target measurement value. The measurement equation is specifically: ; in, is the measurement noise sequence, and its covariance matrix is ; is the target measurement value; Perform a first-order Taylor expansion on the measurement equation to make it a linear equation. The linear equation is as follows: ; in, yes exist The first-order term of the first-order Taylor expansion at ; The linear equation Kalman gain is: ; Among them, the prediction error covariance matrix for: ; The target state calculated by the extended Kalman filter is: ; In summary, the estimated state of the target at each observation moment is obtained, that is, the position of the target at each observation moment and its speed relative to the ground.

[0031] In this embodiment, the filtering effect of the extended Kalman tracking filter is related to the various parameters of the filter. Due to the characteristics of millimeter-wave radar, the measurement error is nonlinearly distributed in the Cartesian coordinate system. Therefore, the filtering parameters need to be adjusted before filtering to make the parameters closer to the true value and achieve the ideal filtering effect.

[0032] Reference Figures 1-7The application provides an industrial environment millimeter wave radar target detection and tracking system, which comprises: A data acquisition module is configured to acquire polar coordinate parameters of targets in an industrial scene through a millimeter wave radar module and convert the polar coordinate parameters into spatially distributed point cloud information, wherein the polar coordinate parameters comprise distance, azimuth angle and radial velocity in a polar coordinate system. A first processing module is configured to calculate multi-dimensional distance between point cloud information based on radial velocity constraint and generate target measurement values according to multi-dimensional distance clustering. A second processing module is configured to reconstruct polar coordinate parameters based on target predicted state and dynamically adjust a measurement noise covariance matrix of a Cartesian coordinate system according to the reconstructed polar coordinate parameters. A data fusion module is configured to fuse target measurement values and noise covariance matrix to obtain target state estimation.

[0033] In the first processing module, the multi-dimensional distance between point cloud information is calculated based on radial velocity constraint, and specifically, ; wherein, and are distance dimension weight coefficient and velocity dimension weight coefficient respectively. represents the spatial distance of two point clouds, ; represents the absolute value of the difference of radial velocity between two point clouds, ; is a direction penalty term coefficient, which is used to amplify the velocity difference between two point clouds. is a conditional expression, which is 1 when is less than 0, and is 0 when is greater than or equal to 0. is multi-dimensional distance.

[0034] The above merely describes the preferred embodiments of the application, but the protection scope of the application is not limited to this. Any person skilled in the art can make equivalent replacements or changes to the technical scheme and the inventive concept of the application within the technical range disclosed by the application, which should be covered in the protection scope of the application.

Claims

1. A method for detecting and tracking millimeter-wave radar targets in an industrial environment, characterized in that: include: The millimeter-wave radar module collects polar coordinate parameters of targets in industrial scenes and converts them into spatially distributed point cloud information. The polar coordinate parameters include distance, azimuth, and radial velocity in the polar coordinate system. Calculate the multidimensional distance between point cloud information based on radial velocity constraints, and generate target measurement values ​​based on multidimensional distance clustering; Reconstruct polar coordinate parameters based on the target prediction state, and dynamically adjust the Cartesian coordinate system measurement noise covariance matrix according to the reconstructed polar coordinate parameters; The target measurement value and the noise covariance matrix are fused to obtain the target state estimate.

2. The industrial environment millimeter wave radar target detection and tracking method according to claim 1 is characterized in that: The multi-dimensional distance between point cloud information is calculated based on radial velocity constraints, specifically: ； in, and are the distance dimension weight coefficient and the speed dimension weight coefficient respectively; Represents the spatial distance between two point clouds, ; Represents the absolute value of the difference in radial velocity between two point clouds, ; is the direction penalty coefficient, which is used to amplify the velocity difference between the two point clouds; Is a conditional expression. When it is less than 0, the value is 1. When it is greater than or equal to 0, the value is 0; is the multidimensional distance.

3. The industrial environment millimeter wave radar target detection and tracking method according to claim 1, characterized in that: The target measurement value is generated according to multidimensional distance clustering, specifically: The point cloud information belonging to the same target is grouped into the same point cloud cluster, the center of the point cloud cluster is calculated, and the center is used as the target measurement value.

4. The industrial environment millimeter wave radar target detection and tracking method according to claim 1, characterized in that: The state equation of the target predicted state is: The CV model is used to model targets in industrial environments: ； in, The state vector for the target prediction state can be expressed as , Represent the target location, Respectively represent the differential of the target in these two directions, i.e., the speed; is the state transition matrix, ; is the sampling interval of the system, Process noise series, The covariance matrix of is the noise covariance matrix.

5. The industrial environment millimeter wave radar target detection and tracking method according to claim 1, characterized in that: The fusing of the target measurement value and the noise covariance matrix to obtain the target state estimation specifically includes: The measurement equation of the extended Kalman tracking filter is constructed based on the target measurement value. The measurement equation is specifically: ； in, is the measurement noise sequence, and its covariance matrix is ; is the target measurement value; The measurement equation is subjected to a first-order Taylor expansion to become a linear equation. The linear equation is specifically: ； in, yes exist The first-order term of the first-order Taylor expansion at ; The linear equation Kalman gain is: ； Among them, the prediction error covariance matrix for: ； The target state calculated by the extended Kalman filter is: ； In summary, the estimated state of the target at each observation moment is obtained, that is, the position of the target at each observation moment and its speed relative to the ground.

6. The industrial environment millimeter wave radar target detection and tracking method according to claim 4, characterized in that: The polar coordinate parameters are reconstructed based on the target prediction state, specifically including: The measurement noise covariance matrix in polar coordinates is defined as: ； According to the error propagation formula, the measurement noise covariance matrix in the Cartesian coordinate system of the position dimension is The Jacobian matrix To do the conversion:

7. The covariance of each position component in the Cartesian coordinate system can be obtained as follows: ； ； ； ； in, is the error variance of each measurement dimension in the radar polar coordinates; That is, the error variance of each dimension in the Cartesian coordinate system; is the error variance of the coupling in the X and Y directions; is the angle value inferred from the predicted state of the target; is the distance value inferred based on the predicted state of the target; In each extended Kalman tracking filter iteration, the polar coordinate parameters are first reconstructed based on the target state predicted by the state equation.

8. An industrial environment millimeter wave radar target detection and tracking system, characterized in that: include: The data acquisition module is used to collect polar coordinate parameters of targets in industrial scenes through the millimeter-wave radar module and convert the polar coordinate parameters into spatially distributed point cloud information. The polar coordinate parameters include distance, azimuth, and radial velocity in the polar coordinate system; A first processing module is used to calculate the multidimensional distance between point cloud information based on radial velocity constraints, and generate target measurement values ​​according to multidimensional distance clustering; A second processing module is used to reconstruct polar coordinate parameters based on the target prediction state and dynamically adjust the Cartesian coordinate system measurement noise covariance matrix according to the reconstructed polar coordinate parameters; The data fusion module is used to fuse the target measurement value and the noise covariance matrix to obtain the target state estimation.

9. The industrial environment millimeter wave radar target detection and tracking system according to claim 7, characterized in that: In the first processing module, the multi-dimensional distance between point cloud information is calculated based on the radial velocity constraint, specifically: ； in, and are the distance dimension weight coefficient and the speed dimension weight coefficient respectively; Represents the spatial distance between two point clouds, ; Represents the absolute value of the difference in radial velocity between two point clouds, ; is the direction penalty coefficient, which is used to amplify the velocity difference between the two point clouds; Is a conditional expression. When it is less than 0, the value is 1. When it is greater than or equal to 0, the value is 0; is the multidimensional distance.

Images