A target detection and tracking method based on sensor fusion

Abstract

The present invention discloses a target detection and tracking method based on sensor fusion, including: obtaining the state information of surrounding targets through vehicle-mounted sensing equipment, and performing target detection on image data and point cloud data respectively; calibrating the external parameters of the laser radar and camera included in the vehicle-mounted sensing equipment; using the Hungarian algorithm to perform target matching on the data collected by the laser radar and camera respectively; using the Kalman filter algorithm to track the target after the data matching, and obtain the motion trajectory of multiple targets around. This method enriches the target information through the fusion of laser radar and camera, solves the problem that the target information detected by a single sensor is not sufficient; solves the problem of missed detection and occlusion by implementing tracking technology, and facilitates multi-target analysis; the whole set of solutions has high detection accuracy and good real-time performance.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned vehicle environment perception and sensor technology, and in particular relates to a target detection and tracking method based on sensor fusion. Background Art

[0002] With the rapid development of intelligent driving, various sensors are installed on cars to provide accurate and real-time perception of the surrounding environment. The most common one is the camera. Since a single sensor has certain disadvantages, such as a single camera cannot obtain accurate depth information of the target and is greatly affected by light; radar has a huge advantage in obtaining depth information, but there is a huge gap with vision in target classification ability. Multi-sensor fusion methods have attracted the attention of scholars at home and abroad. Although some perception detection networks for intelligent driving have high accuracy, they have poor real-time performance and are not easy to deploy on embedded devices. In addition, they lack solutions to the problems of tracking global targets separately and managing tracks in a multi-target environment.

[0003] Therefore, for the environmental perception of unmanned vehicles, how to achieve accurate matching of camera and radar data with high real-time performance has become a hot topic of research for practitioners in this field. Summary of the invention

[0004] The main purpose of the present invention is to provide a sensor fusion-based target detection and tracking method that at least partially solves the above-mentioned technical problems. The method can ensure accuracy while meeting real-time requirements; and the method is compact and reasonable based on the step setting, has good verifiability and practicality, and the algorithm processing time is short, which can reserve sufficient time for subsequent decision-making, planning and other modules.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] A target detection and tracking method based on sensor fusion, comprising:

[0007] Obtain status information of surrounding targets through on-board sensor equipment, and perform target detection on image data and point cloud data respectively;

[0008] Calibrate the external parameters of the laser radar and camera included in the vehicle-mounted sensor equipment;

[0009] The Hungarian algorithm is used to perform target matching on the data collected by the laser radar and the camera respectively;

[0010] The Kalman filter algorithm is used to track the target that has completed data matching and obtain the motion trajectories of multiple surrounding targets.

[0011] Furthermore, target detection is performed on the image data and the point cloud data respectively, including:

[0012] Use Yolov3 algorithm to detect targets in image data and output the location, target category and confidence information of multiple targets in the image;

[0013] The cuboid cutting RANSAC algorithm is used to perform target detection on point cloud data using the ground Kdtree accelerated Euclidean clustering algorithm.

[0014] Furthermore, the cuboid cutting RANSAC algorithm is used to perform target detection on point cloud data using the ground Kdtree accelerated Euclidean clustering algorithm, including:

[0015] Use PCL library functions to set the boundary vertex values ​​of the cuboid, filter out the point cloud data outside the cuboid area, and obtain valid point cloud data;

[0016] A RANSAC algorithm is used to fit a large plane in the valid point cloud data to separate ground points;

[0017] Kdtree is used to search the point cloud data after separating the ground points, and the Euclidean clustering algorithm is used to realize target detection in the point cloud data.

[0018] Furthermore, the laser radar and camera included in the vehicle-mounted sensor equipment are calibrated with external parameters, including:

[0019] The improved lidar_camera_calibration algorithm is selected, and the initialization module and the automatic point cloud corner extraction module are added to realize the external parameter calibration of the lidar and camera.

[0020] Furthermore, the Hungarian algorithm is used to perform target matching on the data collected by the laser radar and the camera, including:

[0021] The laser radar and the camera are respectively collected for time synchronization through a time synchronizer and unified into one coordinate system;

[0022] Use the Hungarian algorithm to traverse the midpoints detected by the laser radar and the camera, and take the closest distance in the image coordinate system as the goal to find the matching image detection frame midpoint and point cloud cluster center;

[0023] The key point matching counts are used to match multiple frames and multiple targets to form a multi-target information sequence.

[0024] Furthermore, the key point matching counts are used to match multiple frames and multiple targets to form a multi-target information sequence, including:

[0025] Extract key points from the previous and next frame images respectively, and calculate the descriptor of each key point;

[0026] The key points of the previous and next frames are matched by calculating the descriptor distance to obtain the key point matching point pairs;

[0027] For each detection box, count the key points inside it in each detection box in the other frame;

[0028] If it contains corresponding matching key points, the count is increased by one, and the detection box with the largest matching key point count is used as the pairing object to form a multi-target information sequence.

[0029] Furthermore, the Kalman filter algorithm is used to track the target that has completed data matching, and the motion trajectories of multiple targets around it are obtained, including:

[0030] According to the multi-target information sequence, each target is tracked by Kalman filtering to achieve global multi-target track management, obtain the movement status of multiple surrounding targets, and smooth the trajectory.

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] A target detection and tracking method based on sensor fusion includes: obtaining the state information of surrounding targets through vehicle-mounted sensor equipment, and performing target detection on image data and point cloud data respectively; calibrating the external parameters of the laser radar and camera included in the vehicle-mounted sensor equipment; using the Hungarian algorithm to match the data collected by the laser radar and camera respectively; using the Kalman filter algorithm to track the targets that have completed data matching, and obtain the motion trajectories of multiple targets around. This method enriches the target information through the fusion of laser radar and camera, and solves the problem that a single sensor does not have sufficient target information; solves the problems of missed detection and occlusion by implementing tracking technology, and facilitates multi-target analysis; the whole solution has high detection accuracy and good real-time performance.

[0033] Furthermore, this method has the following advantages:

[0034] 1) With the goal of reducing the false matching rate, a solution for optimizing the extrinsic parameter solution is proposed. The matching accuracy is improved by using more accurate extrinsic parameters. The existing extrinsic parameter calibration algorithm is improved, and an initialization module and an automatic point cloud corner extraction module are added to enhance the automation performance of the processing process and optimize the accuracy of the results.

[0035] 2) When matching multiple frames, the key point matching count is used instead of directly position correspondence, which makes the algorithm more robust and reduces the mismatch caused by a single distance indicator.

[0036] 3) The adopted processing solution has low detection network component, high real-time performance, and is easy to deploy on embedded devices.

[0037] 4) Tracking processing is added after the detection module to help determine the identity of the target across frames, group target information, smooth the trajectory, and facilitate solving the target occlusion problem through prediction. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 A flow chart of a target detection and tracking method based on sensor fusion provided in an embodiment of the present invention;

[0039] Figure 2 A flow chart of an external parameter calibration method provided in an embodiment of the present invention;

[0040] Figure 3 A flow chart of a point cloud corner detection module provided by an embodiment of the present invention;

[0041] Figure 4 A schematic diagram of the effect of projecting a point cloud onto an image provided by an embodiment of the present invention;

[0042] Figure 5 is the target detection result, the box and label are the image detection results, and the point is the projection diagram of the point cloud cluster center point on the image. DETAILED DESCRIPTION

[0043] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below in conjunction with specific implementation methods.

[0044] In the description of the present invention, it should be noted that the terms "upper", "lower", "inner", "outer", "front end", "rear end", "two ends", "one end", "the other end" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance.

[0045] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "provided with", "connected", etc. should be understood in a broad sense. For example, "connected" can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0046] Reference Figure 1 As shown, the present invention provides a target detection and tracking method based on sensor fusion, comprising:

[0047] S10, obtaining status information of surrounding targets through on-board sensor equipment, and performing target detection on image data and point cloud data respectively;

[0048] S20, calibrating external parameters of the laser radar and camera included in the vehicle-mounted sensor equipment;

[0049] S30, performing target matching on the data collected by the laser radar and the camera respectively using the Hungarian algorithm;

[0050] S40, using a Kalman filter algorithm to track the target that has completed data matching, and obtain the motion trajectories of multiple surrounding targets.

[0051] The following is a detailed description of each of the above steps:

[0052] In one embodiment, the above step S10, performing target detection on the image data and the point cloud data respectively, includes:

[0053] S11, use Yolov3 algorithm to detect targets on image data, and output the location, target category and confidence information of multiple targets in the image;

[0054] S12. Use the cuboid cutting RANSAC algorithm to remove the ground Kdtree accelerated Euclidean clustering algorithm to perform target detection on point cloud data

[0055] In this embodiment, the Yolov3 algorithm is used to detect the target of the image. Yolov3 can be implemented based on OpenCV::dnn or Darknet. If GPU acceleration is used, OpenCV::dnn only supports Intel's GPU. To support Nvidia GPU, the target detection information is output using the darknet_ros package during implementation. The data topic of the camera is used as the input of the package, and the position, target category, and confidence information of multiple targets in the image can be output. Detection can no longer be retrained, and the weight file provided by Yolov3 can be used directly, and the detection accuracy is good. Yolov3 uses Darknet-53 as the classification backbone of the entire network. There is only a convolution layer, and the size of the output feature map is controlled by adjusting the convolution step size. A total of three feature maps are output, corresponding to different downsampling multiples, and multiple feature maps are spliced ​​to fuse features of different scales.

[0056] Point cloud target detection uses the strategy of "cuboid cutting-RANSAC algorithm to remove ground-Kdtree accelerated Euclidean clustering". The random sampling consensus algorithm (RANSAC) can be used to fit large planes in the point cloud and then separate ground points; Kdtree is a k-dimensional search tree that divides the space up and down or left and right to find neighboring points as quickly as possible without traversal; the Euclidean clustering (abbreviated as Euclidean clustering) algorithm is a method that searches the Euclidean distance between each point cloud and its neighboring points by establishing a topological relationship.

[0057] First, use the PCL library function to set the boundary vertex values ​​of the cuboid and filter out the point cloud points outside the cuboid area, which is equivalent to defining a preliminary ROI for point cloud processing. This can reduce the impact of surrounding environments such as buildings and trees on target detection.

[0058] The RANSAC algorithm is used to process three-dimensional point clouds in the following steps: 3 seed points are randomly selected from the point cloud signal, and a plane equation is fitted from these three points; the distance from each point in a small neighborhood to the plane is calculated, and a distance threshold k is set, which is the distance range from the point to the plane that a point should satisfy if it belongs to the plane; a point number threshold n is set, which is the minimum number of points that constitute a plane. The parameter size is estimated based on the plane situation in the environment and the density of the point cloud. If it is less than the threshold k, the point is considered to be an inner point of the plane, otherwise it is an outer point; if the number of inner points of the same plane exceeds the set threshold n, the plane is saved.

[0059] Set the distance threshold k, the upper limit of a certain type of points m, and the lower limit of points n. The parameter size is estimated based on the density of the point cloud. Perform a nearest neighbor search for each point cloud point, add the points around it whose distance is less than the distance threshold k to the set, and then repeat the above operation for the points in the set until no new points are added to the point cloud set. Determine whether the number of points of this type is between the upper limit m and the lower limit n. If it is satisfied, it is regarded as a result class.

[0060] When performing point cloud search, the point cloud is managed using Kdtree to accelerate the search.

[0061] Among them, the full name of the RANSAC algorithm is random sample consensus (RANSAC). The basic assumption of the RANSAC algorithm is that the sample contains correct data (inliers, data that can be described by the model) and abnormal data (outliers, data that deviates far from the normal range and cannot adapt to the mathematical model), that is, the data set contains noise. These abnormal data may be caused by incorrect measurements, incorrect assumptions, incorrect calculations, etc. At the same time, RANSAC also assumes that, given a set of correct data, there is a method to calculate the model parameters that conform to these data. There are two types of data: valid data (inliers) and invalid data (outliers). Data with small deviations are called valid data, and data with large deviations are invalid data.

[0062] In one embodiment, in the above step S20, the improved lidar_camera_calibration algorithm is used for the external parameter calibration. An initialization module is added to it, and the point cloud corner extraction method is improved, from manual to automatic fitting of the calibration plate edge and calculation of vertices. Adding an initial value module is very effective in preventing the optimization from falling into the local optimal value problem. The overall calibration process is as follows: Figure 2 shown.

[0063] First, make a calibration board, which is a square cardboard with an ArUco code on the upper left corner for subsequent image calibration board detection. The calibration board is hung at an angle, and the number of ArUco marks on the calibration board and the size of the calibration board are recorded in the configuration file of the external reference calibration code.

[0064] The image is detected by the OpenCV library function to detect the corner points of the calibration plate. The point cloud is obtained by detecting the left and right edge points of the multi-line point cloud. Straight lines are fitted to the edge points to obtain the equations of the four edge lines of each calibration plate. The intersection points are calculated to obtain the position of the calibration plate corner points in the point cloud, which correspond to the points in the image to obtain multiple pairs of feature points. The external parameter matrix is ​​calculated by processing the data of the corresponding points of the point cloud and image. Minimizing the reprojection error is used as the optimization goal for nonlinear optimization solution. The Ceres library can be used for calculation to obtain the rotation and translation matrices when the reprojection error is minimized as the required external parameter matrix. The above steps constitute a flow chart Figure 2 .

[0065] The process of the point cloud corner detection module is as follows Figure 3For the Velodyne VLP-16 mechanical laser radar used, the point cloud on the calibration plate is a number of parallel lines, and the left and right edge points of each line are candidate edge points of the calibration plate. The direction of the line connecting the two candidate points is calculated, and the candidate points are grouped according to whether they are on the same edge. Multiple edge equations are obtained, and then the intersection points are calculated and corresponded to the corner points detected in the image.

[0066] The solution principle of the initial value module is as follows:

[0067] There are two constraint equations: Among them, R cl is the external parameter rotation matrix between the lidar cameras, t cl is the external parameter translation vector, n l is the normal vector of the calibration plate in the laser radar coordinate system, n c is the normal vector of the calibration plate in the camera coordinate system, P l is the coordinate of the point in the laser radar coordinate system, d c is the vertical distance from the origin of the camera coordinate system to the calibration plate. The normal vector of the calibration plate detected by the point cloud and the image satisfies constraint (1), and the point on the calibration plate satisfies constraint (2). N is the total number of point cloud frames used for solving, i is the index of each frame, and the external parameter rotation matrix R is calculated. cl , translation vector t cl The appropriate initial value is the parameter corresponding to the minimum C. After simplification, minimizing C is maximizing F. make but When RH is a positive definite matrix, the trace of the matrix is ​​the largest. Perform SVD decomposition on H. when hour, RH is a positive definite matrix, at this time F is the largest and C is the smallest, so we get R cl Substituting the estimated value of into constraint (2) yields t cl This value is used as the initial value to input into the nonlinear optimization module, providing the starting point of gradient descent, preventing the optimization from falling into the local optimal value, and facilitating the acquisition of the global optimal external parameter value. When solving the initial value, some pairs of measurement data are used, and it is not necessary to substitute all the measurements.

[0068] In one embodiment, in the above step S30, the Hungarian algorithm is used to perform target matching on the data collected by the laser radar and the camera, including:

[0069] S31, synchronizing the time of the laser radar and the camera respectively collected by a time synchronizer to unify them into one coordinate system;

[0070] S32, using the Hungarian algorithm, traversing the midpoints detected by the laser radar and the camera, taking the closest distance in the image coordinate system as the target, and finding the matching midpoint of the image detection frame and the point cloud cluster center;

[0071] S33, key point matching counting is used to match multiple frames and multiple targets to form a multi-target information sequence.

[0072] Among them, step S31 aligns the point cloud and image data in time and space. Time synchronization uses the synchronizer class Synchronizer in C++, sets an approximate time synchronization strategy, and realizes the alignment of the two data. All synchronized messages are triggered in a callback manner, and the synchronized data is processed in the callback function.

[0073] Spatial synchronization is achieved by projecting the point cloud coordinates onto the image. Then, the points in the point cloud coordinate system are multiplied by the external parameter matrix and the internal parameter matrix to obtain their points in the image coordinate system, as shown in formula (10). This realizes the projection of the point cloud to the image and spatially aligns the data of the two sensors. uv is the coordinate in the image coordinate system, u and v are the horizontal and vertical pixel coordinate values ​​of the point in the image coordinate system, P l is the coordinate of the point in the laser radar coordinate system, K is the camera internal parameter matrix, Z is the z coordinate value of the three-dimensional coordinate in the laser radar coordinate system, R cl is the external parameter rotation matrix between the lidar cameras, t cl is the external parameter translation vector, u and v are the horizontal and vertical coordinates of the point in the pixel coordinate system.

[0074]

[0075] The data of the two sensors are synchronized by the time synchronizer. After the rigid body transformation matrix between the coordinate systems of the two sensors is obtained by external parameter calibration, the coordinate information of the two sensors is unified into one coordinate system. This is achieved by projecting the point cloud coordinates onto the image. The effect after projection is as follows: Figure 4 shown.

[0076] Step S32: Use the Hungarian algorithm to perform target matching on the two sensors.

[0077] In the image detection frame and the point cloud detection frame, find the closest center point as a pair of matches to enrich the target information. And introduce the "one-to-one" idea of ​​the Hungarian algorithm to prevent many-to-one matching and reduce the mismatch rate. First, the coordinates of the point cloud cluster center in the image coordinate system are calculated through the operation of the external parameter matrix and the internal parameter matrix. The visualization effect is as follows: Figure 5 Then, the Hungarian algorithm is used to traverse the midpoints detected by the two sensors, and the closest distance in the image coordinate system is taken as the goal to find the matching midpoint of the image detection frame and the point cloud cluster center.

[0078] Step S33, key point matching counts are used to perform multi-frame multi-target matching.

[0079] Since the object moves continuously and there are multiple targets in each frame, if you want to obtain the continuous motion information of each target separately, you need to perform multi-frame matching and group the motion information of multiple frames according to the object ID. Using the key point matching counting method, multiple targets detected in each frame are matched between two frames, and the information is added to each target label to form a multi-frame information sequence for each target. The specific operation is to extract key points from the previous and next frame images respectively, and calculate the descriptor of each key point. Then, the key points of the previous and next frames are matched by calculating the descriptor distance to obtain key point matching point pairs. After that, for each detection frame, the key points inside it are counted in each detection frame in the other frame. If there is a corresponding matching key point, the count is increased by one, and the detection frame with the largest matching key point count is used as the matching object. When matching multiple frames, the key point matching count is used instead of the position to correspond, which makes the algorithm more robust, more suitable for scenes with dense objects, and reduces the mismatch caused by a single distance indicator.

[0080] In one embodiment, in the above step S40, a Kalman filter algorithm is used to track the target to obtain the motion trajectories of multiple surrounding targets.

[0081] After obtaining the target detection results, a tracking module is added. When multiple objects are detected in the data frame, tracking helps to determine the identity of the object across frames, and can be supplemented with tracking prediction results when the detection fails and the object is lost. In addition, when the vehicle is driving on complex roads, in multi-target problems, target tracking is helpful to determine the identity and trajectory of the object, obtain the motion distribution status of surrounding vehicles, and enhance perception reliability and real-time performance.

[0082] Using the target information sequence, each target is tracked by Kalman filtering to achieve global multi-target track management, obtain the motion of multiple surrounding targets, and smooth the trajectory. In this scheme, the target motion equation is taken as a linear equation, and the linear Kalman filter is initially selected as the tracking scheme.

[0083] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A target detection and tracking method based on sensor fusion, characterized in that: The following steps are involved: Step 1: Obtain the status information of surrounding targets through on-board sensor equipment, and perform target detection on image data and point cloud data respectively; including: (1) Use the Yolov3 algorithm to detect targets in image data and output the location, target category and confidence information of multiple targets in the image; (2) Use the cuboid cutting RANSAC algorithm to remove the ground Kdtree accelerated Euclidean clustering algorithm to perform target detection on point cloud data; including: Use PCL library functions to set the boundary vertex values ​​of the cuboid, filter out the point cloud data outside the cuboid area, and obtain valid point cloud data; A RANSAC algorithm is used to fit a large plane in the valid point cloud data to separate ground points; Kdtree is used to search the point cloud data after separating the ground points, and the Euclidean clustering algorithm is used to detect targets in the point cloud data; Step 2: Calibrate the external parameters of the laser radar and camera included in the vehicle-mounted sensor equipment; including: The improved lidar_camera_calibration algorithm is used, and the initialization module and the automatic point cloud corner extraction module are added. The initialization module calculates the closed-form solution as the initial value to realize the external parameter calibration of the lidar and camera. The solution principle of the initial value module in the improved lidar_camera_calibration algorithm is as follows: There are two constraint equations: R cl n l =n c (1) Among them, R cl is the external parameter rotation matrix between the lidar cameras, t cl is the external parameter translation vector, n l is the normal vector of the calibration plate in the laser radar coordinate system, n c is the normal vector of the calibration plate in the camera coordinate system, P l is the coordinate of the point in the laser radar coordinate system, d c is the vertical distance from the origin of the camera coordinate system to the calibration plate; the normal vector of the calibration plate detected by the point cloud and the image satisfies constraint (1), and the point on the calibration plate satisfies constraint (2); make N is the total number of point cloud frames used for solving, i is the index of each frame, and the external parameter rotation matrix R is calculated. cl , translation vector t cl The appropriate initial value is the parameter corresponding to the minimum C; after simplification, minimizing C means maximizing F. make but When RH is a positive definite matrix, the trace of the matrix is ​​the largest. Perform SVD decomposition on H. when hour, RH is a positive definite matrix, at this time F is the largest and C is the smallest, so we get R cl Substituting the estimated value of into constraint (2) yields t cl The estimated value of ; this value is input into the nonlinear optimization module as the initial value to provide the starting point of gradient descent, prevent the optimization from falling into the local optimal value, and facilitate the acquisition of the global optimal external parameter value; When solving the initial value, some pairs of measurement data are used instead of all the measurements; Step 3: Using the Hungarian algorithm to perform target matching on the data collected by the laser radar and the camera respectively; including: (1) The data collected by the laser radar and the camera are synchronized by a time synchronizer and unified into one coordinate system; (2) Using the Hungarian algorithm, traverse the midpoints detected by the laser radar and the camera, and take the closest distance in the image coordinate system as the target to find the matching image detection frame midpoint and point cloud cluster center; (3) Key point matching counting performs multi-frame multi-target matching to form a multi-target information sequence; including: Extract key points from the previous and next frame images respectively, and calculate the descriptor of each key point; The key points of the previous and next frames are matched by calculating the descriptor distance to obtain the key point matching point pairs; For each detection box, count the key points inside it in each detection box in the other frame; If there is a corresponding matching key point, the count is increased by one, and the detection box with the largest matching key point count is used as the pairing object to form a multi-target information sequence; Step 4: Use the Kalman filter algorithm to track the target that has completed data matching and obtain the motion trajectories of multiple surrounding targets.

2. The target detection and tracking method based on sensor fusion according to claim 1, characterized in that: Use the Kalman filter algorithm to track the target that has completed data matching and obtain the motion trajectories of multiple targets around it, including: According to the multi-target information sequence, each target is tracked by Kalman filtering to achieve global multi-target track management, obtain the movement status of multiple surrounding targets, and smooth the trajectory.

Images