Leader vehicle tracking method and system based on multi-sensor fusion
Through multi-sensor fusion technology, combined with 3D lidar, camera and millimeter wave radar, GPS and IMU information are used to solve the stability of vehicle detection and tracking in unstructured environments, and realize continuous vehicle tracking under harsh conditions and The goal distinction improves the real-time performance of the autonomous driving system.
Patent Information
- Application Number
- CN202310497025.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-05
- Publication Date
- 2024-11-05
AI Technical Summary
Under unstructured environments and harsh weather conditions, it is difficult for the prior art to achieve stable detection and continuous tracking of vehicles, especially in the case of complex road surfaces and light changes, the performance of 3D lidar and cameras is degraded, and millimeter wave radar is distinguished. Difficulties exist in static vehicles and obstacles, and limited vision leads to the easy loss of targets.
Using a multi-sensor fusion method, combining 3D lidar and camera for joint target detection, using millimeter-wave radar for dynamic tracking, and maintaining the vehicle position through the echo attitude information provided by GPS and IMU when the target is lost, ensuring that in extreme cases Track stability.
Vehicle detection and continuous tracking in severe weather such as non-structural roads and dust is realized, using the characteristics of each sensor to fully prevent target loss, and distinguish dynamic vehicles and obstacles through feedback information to ensure real-time performance and stable results.
Smart Images

Figure CN118898818A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving technology, and in particular to a leader vehicle tracking method and system based on multi-sensor fusion. Background Art
[0002] In structured scenes, the road is flat, unobstructed, and there are traffic rules. The leader vehicle and the following vehicle travel in the same lane, so target detection and continuous tracking are relatively simple. In contrast, vehicle detection and tracking in unstructured environments are much more difficult. For cameras, existing target tracking methods can achieve good performance on public datasets, but in unstructured scenes, complex road environments and changing light can cause images to lack stability. In addition, due to the limited field of view of the camera, the target is easily lost when the leader vehicle turns. For 3D lidar, compared with cameras, the position of the leader vehicle can be obtained directly from the point cloud space without perspective transformation, and the horizontal field of view is 360° without blind spots. Deep learning techniques widely used in image processing have also been applied to 3D target detection, and these methods are mainly used to solve tracking problems in urban environments. In the case of dusty bumpy roads in unstructured environments, the performance of these methods will drop sharply due to the poor anti-interference ability of 3D lidar to dust. In addition, as the distance increases, the point cloud becomes sparse, so that the detection accuracy drops rapidly in the distance. Millimeter-wave radar has strong anti-interference ability against dust, rain, fog and snow. At present, millimeter-wave radar is widely used in automobile safety and adaptive cruise control, but it cannot distinguish between static vehicles and surrounding obstacles. When the leader vehicle starts statically, it needs other sensors to provide the initial tracking target. In addition, it is easy to lose the tracking target due to the limited field of view when turning.
[0003] From the perspective of existing technologies, there are more studies on tracking in structured scenarios in the field of autonomous driving, but there are fewer studies on vehicle detection and continuous tracking on unstructured roads and / or in severe weather conditions such as dust, and it is difficult to achieve stable results. Summary of the invention
[0004] In view of the above analysis, an embodiment of the present invention aims to provide a leader vehicle tracking method and system based on multi-sensor fusion, so as to solve the problem of vehicle detection and continuous tracking in unstructured roads and / or severe weather such as dust in autonomous driving technology.
[0005] On the one hand, an embodiment of the present invention provides a leader vehicle tracking method based on multi-sensor fusion, comprising:
[0006] Use 3D laser radar and camera to perform joint target detection and determine the leader vehicle as the initial tracking target;
[0007] Initially tracking the leader vehicle using a 3D laser radar;
[0008] When the 3D laser radar tracking is stable, the leader vehicle is matched with the target generated by the millimeter wave radar, and the millimeter wave radar continuously and dynamically tracks the leader vehicle; wherein,
[0009] When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and the tracking result of the millimeter-wave radar is judged cyclically to see whether it matches the tracking result of the 3D laser radar; if they match, the millimeter-wave radar is used to continue tracking;
[0010] When neither the 3D lidar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to re-determine the initial tracking target.
[0011] Further, the 3D laser radar and the camera are used to perform joint target detection, and the leader vehicle is determined as the initial tracking target, including:
[0012] Utilize 3D lidar and cameras for object detection;
[0013] Extracting a point cloud of interest in the 3D laser radar point cloud space, and clustering the point cloud of interest to obtain a plurality of cluster blocks;
[0014] Obtain the 2D target detection frame of the camera and extract obstacle features; perform similarity measurement between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and mark the corresponding leader vehicle in the 2D target detection frame;
[0015] Use the IOU-based Hungarian matching algorithm to match the cluster blocks obtained by the 3D lidar with the leader vehicle in the 2D target detection box generated by the camera;
[0016] The target information of the matched target is used to initialize the tracking target, and the leader vehicle is determined as the initial tracking target; wherein the target information includes the position coordinates of the leader vehicle obtained by the 3D laser radar.
[0017] Furthermore, the point clouds of interest in the 3D LiDAR point cloud space are extracted, including:
[0018] Use pitch and roll information to correct the coordinates of the point cloud in 3D LiDAR point cloud space;
[0019] Project all point clouds vertically onto a two-dimensional grid map;
[0020] Based on the height difference of the point cloud projected to the same grid, whether it is an obstacle grid is determined; the point cloud projected to the obstacle grid is marked as an obstacle point cloud; and the obstacle point cloud is extracted as a point cloud of interest.
[0021] Furthermore, clustering the point cloud of interest to obtain a plurality of cluster blocks includes:
[0022] The voxel grid method is used for downsampling in the 3D laser radar coordinate system, and the point cloud coordinates obtained by downsampling are represented by the centroid of the sampled point cloud;
[0023] The KD tree method is used to cluster the downsampled point cloud of interest in three-dimensional space and generate a 3D object detection box; wherein the 3D object detection box is the outer boundary of the clustering block.
[0024] Furthermore, obtaining the 2D target detection frame of the camera and extracting obstacle features includes:
[0025] The YOLOX model is trained using a data enhancement method; wherein the training includes retraining using low-quality leader vehicle images in a dusty environment and with deformation and occlusion;
[0026] The target detection results of the camera are input into the trained YOLOX model to obtain the 2D target detection frame and extract the obstacle features.
[0027] Furthermore, the cluster blocks obtained by the 3D lidar and the leader vehicle in the 2D target detection frame generated by the camera are matched using the IOU-based Hungarian matching algorithm, including:
[0028] Converting the 3D target detection frame generated by 3D lidar clustering into a 2D projection in the camera frame, including: converting the 3D target detection frame in the 3D lidar coordinate system to the 3D target detection frame in the camera coordinate system; projecting the obtained 3D target detection frame in the camera coordinate system to the image coordinate system to generate a 2D target detection frame; converting the obtained 2D target detection frame in the image coordinate system to the 2D target detection frame in the pixel coordinate system;
[0029] The 2D target detection frame in the pixel coordinate system is matched with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm.
[0030] Furthermore, the 2D target detection frame in the pixel coordinate system is matched with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm, including:
[0031] The 3D lidar and the 2D target detection box generated by the camera are matched using the IOU-based Hungarian matching algorithm. The IOU loss is defined as follows:
[0032]
[0033] IOU loss =1-IOU
[0034] Where A is the 2D target detection frame corresponding to the 3D laser radar; B is the 2D target detection frame corresponding to the camera; S is the area of the target detection frame; if B is the marked leader vehicle, all target detection frames in A are matched with the target detection frame of the marked leader vehicle, and the IOU of all matching pairs is loss The lowest matching pair with IOU not less than 0.5 is considered to be matched with the leader vehicle successfully.
[0035] Furthermore, when the 3D laser radar tracking is stable, the leader vehicle is matched with the target generated by the millimeter wave radar, and the millimeter wave radar continuously and dynamically tracks the leader vehicle, including:
[0036] The millimeter-wave radar and 3D laser radar are matched in the 3D laser radar coordinate system; the Euclidean distance d in the top view is used to match the closest initialization tracking target, and the target detection frame with the smallest d among several target detection frames of the millimeter-wave radar is considered to be the leader vehicle;
[0037] in,
[0038]
[0039] x 初始化 ,y 初始化 x is the coordinate of the center point of the target detection frame of the leader vehicle in the top view of the laser radar 3D coordinate system; 毫米波 ,y 毫米波 It is the coordinate of the center point of the target detection frame of the millimeter wave radar in the top view perspective of the lidar 3D coordinate system.
[0040] Furthermore, in the continuous dynamic tracking process, the detected leader vehicle is placed in the local frame F V The position under is converted to the global frame F G The position under the vehicle is used to form a continuous trajectory for the leader vehicle's motion trend analysis; the historical tracking trajectory is associated with the current tracking result through coordinate transformation to extract the region of interest; the current frame and one or several adjacent frames are called local frames, and the entire motion process is called the global frame; the vehicle body local coordinate system F V Select the center of the rear axle of the vehicle projected onto the ground as the coordinate origin, the vehicle's forward direction is the positive direction of the y-axis, the right side of the vehicle is the positive direction of the x-axis, and the vertical upward direction is the positive direction of the z-axis; F G It is the geodetic coordinate system.
[0041] On the other hand, an embodiment of the present invention provides a leader vehicle tracking system based on multi-sensor fusion, comprising:
[0042] The camera-lidar initialization module is used for joint target detection by 3D laser radar and camera to determine the leader vehicle as the initial tracking target; it includes: ROI extraction module, cluster analysis module, camera target detection frame extraction module, camera-lidar fusion detection module;
[0043] ROI extraction module: used to extract the point cloud of interest in the 3D lidar point cloud space;
[0044] Cluster analysis module: used to cluster the extracted 3D lidar point cloud of interest to obtain several cluster blocks;
[0045] Camera target detection frame extraction module: used for camera target detection; obtains the camera's 2D target detection frame and extracts obstacle features; measures the similarity between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and marks the corresponding leader vehicle in the 2D target detection frame;
[0046] Camera-LiDAR fusion detection module: used to convert the 3D target detection frame generated by 3D LiDAR clustering into a 2D projection in the camera frame, and match the 3D LiDAR 2D target detection frame in the pixel coordinate system with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm;
[0047] Millimeter wave radar tracking module: used to continuously and dynamically track the leader vehicle after the 3D laser radar tracking is stable;
[0048] The GPS and IMU information module is used to provide echo attitude information to maintain the absolute position of the leader vehicle. When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and it is cyclically determined whether the tracking result of the millimeter-wave radar matches the tracking result of the 3D laser radar. If they match, the tracking based on the millimeter-wave radar continues. When neither the 3D laser radar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to redetermine the initial tracking target.
[0049] The beneficial effects of the above technical solution are as follows: In the leader vehicle tracking task, we use a multi-sensor fusion method to achieve vehicle detection and continuous tracking. The fusion process makes full use of the characteristics of each sensor. First, the 3D laser radar and camera joint detection are used to solve the problem of tracking target initialization. Since the horizontal field of view of the 3D laser radar and camera is 360 degrees, it can effectively prevent the target from being lost when the leader vehicle turns. At close range, it can also assist in target tracking. The millimeter wave radar is mainly used for dynamic target tracking during movement. It can continuously and stably track the same target with a constant exponent. In addition, according to the feedback relative speed information, it can effectively distinguish between dynamic vehicles and surrounding static obstacles. Finally, for extreme cases such as sharp turns and slopes, when the 3D laser radar and millimeter wave radar cannot detect the target, we use the echo attitude information provided by GPS / IMU to maintain the absolute position of the leader vehicle. At the same time, analyzing the movement trend of the leader vehicle based on the echo attitude information and relative position relationship helps to reduce the area of interest to ensure real-time performance. Thus, the detection and continuous tracking of the leader vehicle are achieved in bad weather such as unstructured roads and / or dust, and stable results are achieved.
[0050] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0051] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.
[0052] Figure 1 A processing flow chart of a leader vehicle tracking method based on multi-sensor fusion according to an embodiment of the present invention;
[0053] Figure 2 The present invention is an embodiment of a process flow chart of a leader vehicle tracking system based on multi-sensor fusion. DETAILED DESCRIPTION
[0054] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.
[0055] A specific embodiment of the present invention discloses a leader vehicle tracking method based on multi-sensor fusion, such as Figure 1As shown, the method comprises the following steps:
[0056] Step S1, using 3D laser radar and camera to perform joint target detection, and determining the leader vehicle as the initial tracking target, includes the following steps:
[0057] Step S11: Unify the target joint detection data of the 3D laser radar and the camera into the local coordinate system F of the vehicle body V , where the body local coordinate system F V Select the center of the rear axle of the vehicle projected onto the ground as the coordinate origin, the vehicle's forward direction is the positive direction of the y-axis, the right side of the vehicle is the positive direction of the x-axis, and the vertical upward direction is the positive direction of the z-axis;
[0058] Step S12, extracting the point cloud of interest in the 3D laser radar point cloud space, and clustering the point cloud of interest to obtain a plurality of cluster blocks; the steps include:
[0059] Step S121: In rough terrain, in order to eliminate the influence of road undulations, the pitch and roll information is used to correct the coordinates of the point cloud in the 3D lidar point cloud space, as shown in (1).
[0060] P c =R(γ,β,α).P v =R x (γ).R y (β).P v (1)
[0061] Where Pv represents the source point cloud in Fv, Pc represents the corrected point cloud, γ represents the pitch angle, β represents the roll angle, α represents the yaw angle, R represents the rotation matrix, and F V is the local coordinate system of the vehicle body. X Represents the rotation matrix around the x-axis, R y Represents the rotation matrix around the y-axis.
[0062] Step S122, extracting the obstacle point cloud in the corrected 3D laser radar point cloud space as the point cloud of interest, the steps include:
[0063] Project all point clouds vertically onto a two-dimensional grid map;
[0064] Whether it is an obstacle grid is determined based on the height difference of the point cloud projected to the same grid; the point cloud projected to the obstacle grid is marked as an obstacle point cloud.
[0065] Obstacles are detected based on the height difference information between neighbors.
[0066] The maximum height difference of each cell Δ x,y It can be calculated by formula (2) as follows
[0067]
[0068] Among them, |Z x,y is the set Z x,y The height of a point in the grid map is (x, y), and Gref is the reference height of the surrounding ground plane calculated by (3).
[0069]
[0070] Where N-{n}(x, y) represents the n neighboring points of the point with coordinates (x, y).
[0071] Step S123: using a downsampling method to reduce the number of point clouds, including:
[0072] The voxel grid method is used for downsampling in the 3D laser radar coordinate system, and the point clouds that have not been collected are deleted. The coordinates of the downsampled point clouds are represented by the centroid of the sampled point clouds;
[0073] Step S124: cluster the downsampled point cloud of interest in three-dimensional space using a KD tree method and generate a 3D object detection box; wherein the 3D object detection box is the outer boundary of the clustering block.
[0074] The Euclidean distance between point clouds is used in the clustering process, as shown in (4):
[0075]
[0076] Among them, (x i ,y i ,z i ) and (x j ,y j ,z j ) represents the coordinates of the x-point cloud and the y-point cloud, Δ R = 0.1m is the distance threshold. After clustering, the 3D lidar point cloud is divided into several cluster blocks and a 3D target detection box is generated based on the outer boundary of the cluster block.
[0077] Step S13, obtaining a 2D target detection frame of the camera and extracting obstacle features; measuring the similarity between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and marking the corresponding leader vehicle in the 2D target detection frame; comprising:
[0078] Step S131, use the camera to perform target detection based on deep learning training of target sample data, use the YOLOv5 model, train the model with 300 epochs on the COCO train2017 public dataset, warm-up for 5 epochs, use stochastic gradient descent (SGD) for training, use learning rate lr×BatchSize÷64 (linear scaling), initial lr=0.01 and cosine learning rate schedule. The weight decay is 0.0005, and the SGD momentum is 0.9. The default number of images per batch for 8GPU devices is 128, and the number of input images is evenly distributed between 448 and 832 with a step of 32. COCO train2017 is the name of a public dataset. Epoch is defined as a single training iteration of all batches in forward and backward propagation, which can be translated into rounds or cycles. Warm-up is a warm-up training mechanism, in which the learning rate is adjusted to a very low level to ensure model convergence. Batch-size is the number of data (samples) passed to the program for training at a single time. 8GPU is a graphics processor.
[0079] Step S132, add Mosaic and MixUp to the data enhancement strategy to improve the performance of YOLOX. Mosaic is a mosaic data enhancement mechanism. MixUp is an image and label mixed averaging data enhancement mechanism. In specific operation, Mosaic randomly scales 4 pictures and then splices them together to ensure that the entire picture is the original size after splicing. MixUp is an unconventional data enhancement method, a simple data enhancement principle that is independent of data. It constructs new training samples and labels in a linear interpolation manner, that is, two random pictures are mixed in proportion, and the corresponding labels are also allocated in proportion. This embodiment adopts a 1:1 mixing.
[0080] Step S133: For specific application scenarios, a large amount of real vehicle data in unstructured road environments and / or dusty environments is added during model training. In the added data, vehicles are often obscured by dust and / or the vehicle changes shape when turning, resulting in missing image information and image distortion. A large amount of low-quality data and special data improves the training generalization ability of the model and better meets the needs of application scenarios.
[0081] Step S134, obtain a 2D target detection frame and extract obstacle features, use the feature vector and the features of the template vehicle to measure similarity, and mark the corresponding leader vehicle;
[0082] The output result of the last layer of the network is used as a feature extractor to extract a 128-dimensional feature vector x for each detected obstacle; at the same time, several leader vehicle images stored in the database are input into the network with the same parameters to obtain 128-dimensional feature vectors, and the average of each dimension of the several leader vehicle images is obtained to obtain y 平均 .
[0083] Calculate the vector Euclidean distance,
[0084] Among them, n=128, and the obstacle corresponding to the smallest feature vector x is marked as the leader vehicle.
[0085] Step S14: Use the IOU-based Hungarian matching algorithm to match the cluster blocks obtained by the 3D laser radar with the leader vehicle in the 2D target detection frame generated by the camera;
[0086] Step S141, converting the 3D target detection frame generated by 3D laser radar clustering into a 2D projection in the camera frame, including: converting the 3D target detection frame in the 3D laser radar coordinate system to the 3D target detection frame in the camera coordinate system; projecting the obtained 3D target detection frame in the camera coordinate system to generate a 2D target detection frame in the image coordinate system; converting the obtained 2D target detection frame in the image coordinate system to the 2D target detection frame in the pixel coordinate system;
[0087] Step S142, matching the 2D target detection frame in the pixel coordinate system with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm, including:
[0088] The Iou loss is defined as follows:
[0089]
[0090] IOU loss =1-IOU
[0091] Among them, A is the 2D target detection frame corresponding to the 3D lidar; B is the target detection frame corresponding to the camera; S is the area of the target detection frame.
[0092] If B is the marked leader vehicle, all object detection frames in A are matched with the marked leader vehicle’s object detection frame, and the IOU of all matches is loss The lowest matching pair with IOU not less than 0.5 is considered to be matched with the leader vehicle successfully.
[0093] In the pixel coordinate system, IOU lossThe matching degree of the 3D lidar and the camera to the obstacles is measured. Using the IOU loss, we can complete the matching and associate the position information of the leader vehicle in the 3D lidar with the target detection in the camera, thereby completing the target detection of the leader vehicle.
[0094] Step S15, using the target information of the matched target to initialize the tracking target, and determining the leader vehicle as the initial tracking target; wherein the target information includes the position coordinates of the leader vehicle obtained by the 3D laser radar.
[0095] At this point, the initialization of the guided vehicle has been achieved using the relevant algorithms of the two sensors, 3D lidar and camera.
[0096] Step S2: After the 3D laser radar and camera generate a target detection frame, it is input into the Kalman filter; the target detection frame of the millimeter wave radar is provided by the device itself, and after the target detection frame is generated, it is also input into the Kalman filter.
[0097] Step S3: using a 3D laser radar to initially track the determined leader vehicle.
[0098] Step S4: When the 3D laser radar tracking is stable, the leader vehicle is matched with the target generated by the millimeter wave radar, and the millimeter wave radar continuously and dynamically tracks the leader vehicle; wherein,
[0099] When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and the tracking result of the millimeter-wave radar is judged cyclically to see whether it matches the tracking result of the 3D laser radar; if they match, the millimeter-wave radar is used to continue tracking;
[0100] When neither the 3D lidar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to re-determine the initial tracking target.
[0101] Specific:
[0102] Step S41, based on the target position detected by the 3D laser radar, the millimeter wave radar regards the dynamic target closest to the initialization result as the tracking target. The millimeter wave radar Delphi ESR used in the present invention has a built-in clustering function that can generate a 3D clustering frame. Since there is no distortion, and the default installation positions of the millimeter wave and 3D laser radar are in parallel (basically there will not be a large installation angle for one of them), the Euclidean distance d under the top view perspective is directly used to match the object closest to the initialization tracking, that is, the leader vehicle detected by the laser radar-camera. In one frame, the one with the smallest d among several target detection frames of the millimeter wave radar is considered to be the leader vehicle.
[0103] in,
[0104]
[0105] x 初始化 ,y 初始化 x is the coordinate of the center point of the target detection frame of the leader vehicle in the top view of the laser radar 3D coordinate system; 毫米波 ,y 毫米波 It is the coordinate of the center point of the target detection frame of the millimeter wave radar in the top view perspective of the lidar 3D coordinate system.
[0106] Step S42: For the vehicle tracking process based on millimeter wave radar, the main task is to keep tracking the same target and eliminate the interference of other dynamic obstacles.
[0107] Millimeter wave radar is the main sensor used for target tracking. It is an ideal sensor for moving target tracking because it can directly obtain the relative speed of the target. The relative speed is very useful for eliminating static obstacles in the tracking process.
[0108] In actual applications, the millimeter-wave radar returns multiple tracking results. For each tracking result of the millimeter-wave radar, when the 3D lidar sensor can detect the target, the millimeter-wave radar can automatically track the target with the same index. In the continuous tracking process, according to the last detection result, if the target with the same index is still valid and its position is within the region of interest ROI, the current tracking is valid. When the millimeter-wave radar tracking is valid, the speed information of the tracked target is input into the Kalman filter prediction. The position coordinates of the effective tracking result of the 3D lidar are input into the Kalman filter for prediction, and Kalman outputs the region of interest ROI for it, and then returns it to the millimeter-wave radar. Due to interference such as driving on bumpy roads, turning and occlusion, continuous tracking with the same index will be interrupted in unstructured environments. Sometimes, the sensor can detect targets in other detection directions with different indicators. In this case, if the candidate detection target closest to the predicted position is within the region of interest, it will be reselected as the tracking target. Then update the index of the continuously tracked target and continue.
[0109] Step S43: During the continuous tracking process, the detected leader vehicle needs to be V The position under is converted to the global frame F G The position of the leader vehicle is obtained by calculating the position of the leader vehicle under the control of the leader vehicle to form a continuous trajectory for the leader vehicle movement trend analysis; the historical tracking trajectory is associated with the current result through coordinate transformation to extract the region of interest (ROI); F G It is the geodetic coordinate system.
[0110] When the tracking target is partially or completely blocked by trees or walls, the global position of the last valid tracking target can be converted to the current frame to ensure that the follower can drive to the lost position and restart target detection for tracking initialization; among them, the current frame and one or several adjacent frames are called local frames, and the entire motion process is called the global frame.
[0111] Step 44, for the loss of tracking based on millimeter-wave radar, it switches to the auxiliary tracking based on 3D lidar as described above, which often occurs in corners because its horizontal field of view is narrow. If the 3D lidar-assisted tracking is effective tracking, the detected target is input into the Kalman filter, the Kalman filter predicts the output region of interest ROI, and tries to re-match the tracking result of the millimeter-wave radar with the tracking result of the Kalman filter predicting the output region of interest ROI. After re-matching, the target tracking based on the millimeter-wave radar continues. When the millimeter-wave radar re-tracks the target, the tracking based on the millimeter-wave radar switches to continue. If the 3D lidar-assisted tracking is not effective tracking, such as for extreme cases such as sharp turns and slopes, when the 3D lidar and millimeter-wave radar cannot detect the target, the echo attitude information provided by GPS and IMU is used to maintain the absolute position of the leader vehicle; or the leader vehicle is driven to a new valid position, and the 3D lidar and camera are re-detected starting from S1.
[0112] On the other hand, another embodiment of the present invention provides a leader vehicle tracking system based on multi-sensor fusion, such as Figure 2 As shown, including:
[0113] The camera-lidar initialization module is used for joint target detection by 3D laser radar and camera to determine the leader vehicle as the initial tracking target; it includes: ROI extraction module, cluster analysis module, camera target detection frame extraction module, camera-lidar fusion detection module;
[0114] ROI extraction module: used to extract the point cloud of interest in the 3D lidar point cloud space;
[0115] Cluster analysis module: used to cluster the extracted 3D lidar point cloud of interest to obtain several cluster blocks;
[0116] Camera target detection frame extraction module: used for camera target detection; obtains the camera's 2D target detection frame and extracts obstacle features; measures the similarity between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and marks the corresponding leader vehicle in the 2D target detection frame;
[0117] Camera-LiDAR fusion detection module: used to convert the 3D target detection frame generated by 3D LiDAR clustering into a 2D projection in the camera frame, and match the 3D LiDAR 2D target detection frame in the pixel coordinate system with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm;
[0118] Millimeter wave radar tracking module: used to continuously and dynamically track the leader vehicle after the 3D laser radar tracking is stable;
[0119] The GPS and IMU information module is used to provide echo attitude information to maintain the absolute position of the leader vehicle. When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and it is cyclically determined whether the tracking result of the millimeter-wave radar matches the tracking result of the 3D laser radar. If they match, the tracking based on the millimeter-wave radar continues. When neither the 3D laser radar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to redetermine the initial tracking target.
[0120] The above modules are implemented by installing the camera in front of the vehicle, the 3D laser radar on the roof of the driverless car, the millimeter wave radar at the front of the driverless car, and the integrated navigation device at the center of the rear axle of the driverless car. Pandar40 is selected as the 3D laser radar with a horizontal field of view of 360°; the millimeter wave radar uses Delphi ESR, with a horizontal field of view of ±45°, and outputs 64 relative positions and relative speeds to track the target; during continuous tracking, the index of each target remains unchanged until it disappears; the integrated navigation device INS / GPS uses XW-GI7660; the echo attitude information can be obtained from the combined results of the inertial navigation system INS / GPS, and the output includes heading, pitch, roll, global positioning, vehicle speed and other information.
[0121] Compared with the prior art, the present embodiment provides a leader vehicle tracking method and system based on multi-sensor fusion. First, 3D laser radar and camera are used to solve the tracking target initialization problem. Since the horizontal field of view of the 3D laser radar is 360°, it can effectively prevent the target from being lost when the seeker turns, and it can also assist in target tracking at close range. The camera has good vehicle recognition performance and strong robustness to detection. Millimeter wave radar is mainly used for dynamic target tracking during movement. It can track the same target continuously and stably, and the index is constant. In addition, according to the feedback relative speed information, dynamic vehicles and surrounding static obstacles can be effectively distinguished. Finally, for extreme situations such as sharp turns and tilts, the echo attitude information provided by GPS / IMU is used to maintain the absolute position of the leader vehicle when the 3D laser radar and millimeter wave radar cannot detect the target. When the short-term target is lost, it is useful to re-track the target. At the same time, analyzing the movement trend of the leader vehicle based on the echo attitude information and relative position relationship helps to reduce the area of interest and ensure real-time performance.
[0122] In the field of autonomous driving, it has achieved stable detection and continuous tracking of the leader vehicle in harsh environments such as non-institutional roads and dust.
[0123] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.
[0124] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.
Claims
1. A leader vehicle tracking method based on multi-sensor fusion, characterized in that: include: Use 3D laser radar and camera to perform joint target detection and determine the leader vehicle as the initial tracking target; Initially tracking the leader vehicle using a 3D laser radar; When the 3D laser radar tracking is stable, the leader vehicle is matched with the target generated by the millimeter wave radar, and the millimeter wave radar continuously and dynamically tracks the leader vehicle; wherein, When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and the tracking result of the millimeter-wave radar is judged cyclically to see whether it matches the tracking result of the 3D laser radar; if they match, the millimeter-wave radar is used to continue tracking; When neither the 3D lidar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to re-determine the initial tracking target.
2. The leader vehicle tracking method according to claim 1, characterized in that: Using 3D laser radar and camera to perform joint target detection, the leader vehicle is determined as the initial tracking target, including: Utilize 3D lidar and cameras for object detection; Extracting a point cloud of interest in the 3D laser radar point cloud space, and clustering the point cloud of interest to obtain a plurality of cluster blocks; Obtain the 2D target detection frame of the camera and extract obstacle features; perform similarity measurement between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and mark the corresponding leader vehicle in the 2D target detection frame; Use the IOU-based Hungarian matching algorithm to match the cluster blocks obtained by the 3D lidar with the leader vehicle in the 2D target detection box generated by the camera; The target information of the matched target is used to initialize the tracking target, and the leader vehicle is determined as the initial tracking target; wherein the target information includes the position coordinates of the leader vehicle obtained by the 3D laser radar.
3. The leader vehicle tracking method according to claim 2, characterized in that: Extract the point cloud of interest in the 3D LiDAR point cloud space, including: Use pitch and roll information to correct the coordinates of the point cloud in the 3D lidar point cloud space; Project all point clouds vertically onto a two-dimensional grid map; Based on the height difference of the point cloud projected to the same grid, whether it is an obstacle grid is determined; the point cloud projected to the obstacle grid is marked as an obstacle point cloud; and the obstacle point cloud is extracted as a point cloud of interest.
4. The leader vehicle tracking method according to claim 2, characterized in that: The clustering of the point cloud of interest to obtain a plurality of cluster blocks includes: The voxel grid method is used for downsampling in the 3D laser radar coordinate system, and the point cloud coordinates obtained by downsampling are represented by the centroid of the sampled point cloud; The KD tree method is used to cluster the downsampled point cloud of interest in three-dimensional space and generate a 3D object detection box; wherein the 3D object detection box is the outer boundary of the clustering block.
5. The leader vehicle tracking method according to claim 2, characterized in that: The step of obtaining the camera's 2D target detection frame and extracting obstacle features includes: The YOLOX model is trained using a data enhancement method; wherein the training includes retraining using low-quality leader vehicle images in a dusty environment and with deformation and occlusion; The target detection results of the camera are input into the trained YOLOX model to obtain the 2D target detection frame and extract the obstacle features.
6. The leader vehicle tracking method according to claim 4, characterized in that: Use the IOU-based Hungarian matching algorithm to match the cluster blocks obtained by the 3D lidar with the leader vehicle in the 2D target detection box generated by the camera. , include: Converting the 3D target detection frame generated by 3D lidar clustering into a 2D projection in the camera frame, including: converting the 3D target detection frame in the 3D lidar coordinate system to the 3D target detection frame in the camera coordinate system; projecting the obtained 3D target detection frame in the camera coordinate system to the image coordinate system to generate a 2D target detection frame; converting the obtained 2D target detection frame in the image coordinate system to the 2D target detection frame in the pixel coordinate system; The 2D target detection frame in the pixel coordinate system is matched with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm.
7. The leader vehicle tracking method according to claim 6, characterized in that: The 2D target detection frame in the pixel coordinate system is matched with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm , include: The 3D lidar and the 2D target detection box generated by the camera are matched using the IOU-based Hungarian matching algorithm. The IOU loss is defined as follows: IOU loss =1-IOU Where A is the 2D target detection frame corresponding to the 3D laser radar; B is the 2D target detection frame corresponding to the camera; S is the area of the target detection frame; if B is the marked leader vehicle, all target detection frames in A are matched with the target detection frame of the marked leader vehicle, and the IOU of all matching pairs is loss The lowest matching pair with IOU not less than 0.5 is considered to be successfully matched with the leader vehicle.
8. The leader vehicle tracking method according to claim 1, characterized in that: When the 3D laser radar tracking is stable, the leader vehicle is matched with the target generated by the millimeter wave radar, and the millimeter wave radar continuously and dynamically tracks the leader vehicle, including: The millimeter-wave radar and 3D laser radar are matched in the 3D laser radar coordinate system; the Euclidean distance d in the top view is used to match the closest initialization tracking target, and the target detection frame with the smallest d among several target detection frames of the millimeter-wave radar is considered to be the leader vehicle; in, x 初始化 ,y 初始化 x is the coordinate of the center point of the target detection frame of the leader vehicle in the top view of the laser radar 3D coordinate system; 毫米波 ,y 毫米波 It is the coordinate of the center point of the target detection frame of the millimeter wave radar in the top view perspective of the lidar 3D coordinate system.
9. The leader vehicle tracking method according to claim 1, characterized in that: In the continuous dynamic tracking process, the detected leader vehicle is placed in the local frame F V The position under is converted to the global frame F G The position under the vehicle is used to form a continuous trajectory for the leader vehicle's motion trend analysis; the historical tracking trajectory is associated with the current tracking result through coordinate transformation to extract the region of interest; the current frame and one or several adjacent frames are called local frames, and the entire motion process is called the global frame; the vehicle body local coordinate system F V Select the center of the rear axle of the vehicle projected onto the ground as the coordinate origin, the vehicle's forward direction is the positive direction of the y-axis, the right side of the vehicle is the positive direction of the x-axis, and the vertical upward direction is the positive direction of the z-axis; F G It is the geodetic coordinate system.
10. A leader vehicle tracking system based on multi-sensor fusion, characterized in that: include: The camera-lidar initialization module is used for joint target detection by 3D laser radar and camera to determine the leader vehicle as the initial tracking target; it includes: ROI extraction module, cluster analysis module, camera target detection frame extraction module, camera-lidar fusion detection module; ROI extraction module: used to extract the point cloud of interest in the 3D lidar point cloud space; Cluster analysis module: used to cluster the extracted 3D lidar point cloud of interest to obtain several cluster blocks; Camera target detection frame extraction module: used for camera target detection; obtains the camera's 2D target detection frame and extracts obstacle features; measures the similarity between the obstacle features and the features of the template vehicle corresponding to the leader vehicle, and marks the corresponding leader vehicle in the 2D target detection frame; Camera-LiDAR fusion detection module: used to convert the 3D target detection frame generated by 3D LiDAR clustering into a 2D projection in the camera frame, and match the 3D LiDAR 2D target detection frame in the pixel coordinate system with the leader vehicle in the 2D target detection frame generated by the camera using the IOU-based Hungarian matching algorithm; Millimeter wave radar tracking module: used to continuously and dynamically track the leader vehicle after the 3D laser radar tracking is stable; The GPS and IMU information module is used to provide echo attitude information to maintain the absolute position of the leader vehicle. When the millimeter-wave radar loses the tracking target, the 3D laser radar is used to assist in tracking, and it is cyclically determined whether the tracking result of the millimeter-wave radar matches the tracking result of the 3D laser radar. If they match, the tracking based on the millimeter-wave radar continues. When neither the 3D laser radar nor the millimeter-wave radar can find the leader vehicle, the echo attitude information provided by the GPS and IMU is used to maintain the absolute position of the leader vehicle, or the leader vehicle is moved to a new position to redetermine the initial tracking target.
Citation Information
Patent Citations
Vehicle-mounted multi-sensor tight coupling fusion positioning method and system, storage medium and vehicle
CN110906923A
Perception fusion method of automatic driving system
CN114035187A
Vehicle detection method and device based on multi-source data fusion
CN114118253A
Three-dimensional target detection method fusing monocular camera and laser radar
CN114120075A
Multi-sensor fusion sensing efficiency enhancement method
CN115034324A
Cited By
Target tracking method and device based on multi-sensor fusion
CN120254837A
