Target track tracking method based on fusion of multi-millimeter wave radar and laser radar

Abstract

The present application relates to a kind of target track tracking method based on the fusion of multi-millimeter wave radar and laser radar, belong to intelligent mining area, solve the problem that the wide body mine card wide angle of view demand cannot be met in the prior art, and obstacle perception stability and accuracy are poor.The method comprises: initial target set detection is carried out by the multi-millimeter wave radar and laser radar on the mine card respectively, the initial target set detection information is fused to create first track set, and the initial target speed is obtained;According to the sampling interval of multi-millimeter wave radar and laser radar respectively, according to the initial target speed and first track set, predicted track set is obtained;When the multi-millimeter wave radar and / or laser radar returns target detection information set, fusion matching is carried out with the predicted track set;According to the fusion matching result, the first track set is updated.The target track tracking applied on the wide body mine card is realized.

Description

Technical Field

[0001] This invention relates to the field of intelligent mining technology, and in particular to a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar. Background Technology

[0002] Unmanned mining trucks represent a significant development direction for smart mining areas. The perception module of the unmanned driving system for wide-body mining trucks in mining areas needs to detect obstacles around the mining area, providing essential information for operations such as stopping upon encountering obstacles and detouring around them. Sensors used in unmanned driving perception systems typically include lidar, millimeter-wave radar, and optical cameras; ultrasonic radar is also used in some scenarios.

[0003] Compared to urban road conditions, mining area roads are much harsher. When mining trucks are running, the lidar is affected by a lot of dust in the environment. When the lidar is working in rain, fog, or dusty conditions, it will generate echoes from rain, fog, and dust particles in the air, causing false alarms for obstacles. In addition, due to the poor road conditions, millimeter-wave radar is prone to false alarms and missed detections when in bumpy conditions, which reduces the accuracy of the sensor itself. Furthermore, wide-body mining trucks have a large lateral width, and the field of view of a single millimeter-wave radar cannot meet the needs of a wide field of view. Therefore, it is impossible to meet the wide field of view requirements of wide-body mining trucks in mining areas.

[0004] Therefore, there is a lack of a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar that can perceive obstacles over a wider range and effectively ensure the stability and accuracy of obstacle perception. Summary of the Invention

[0005] Based on the above analysis, the embodiments of the present invention aim to provide a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar, in order to solve the problems that existing methods cannot meet the wide field of view requirements of wide-body mining trucks in mining areas, and have poor obstacle perception stability and accuracy.

[0006] On one hand, embodiments of the present invention provide a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar, including:

[0007] Initial target set detection is performed using multi-millimeter wave radar and lidar on the mining truck, the initial target set detection information is fused to create a first track set, and the initial target velocity is obtained;

[0008] Based on the sampling intervals of the multi-millimeter-wave radar and the lidar, and based on the initial target velocity and the first track set, the predicted track set is obtained respectively.

[0009] After the multi-millimeter-wave radar and / or lidar returns the target detection information set, it is fused and matched with the predicted trajectory set; based on the fusion and matching result, the first trajectory set is updated.

[0010] Furthermore, the multi-millimeter-wave radar includes at least two millimeter-wave radars, and the multiple millimeter-wave radars are fixed at a certain angle to the front bumper of the mining truck by tooling fixtures.

[0011] The lidar is installed at a high position on the top of the mining truck's cockpit.

[0012] Furthermore, the update of the first track set includes:

[0013] If there is target detection information in the target detection information set that successfully matches the track in the predicted track set, then the successfully matched target detection information is updated to the track corresponding to the first track set;

[0014] If none of the tracks in the predicted track set match the detection information of a certain target in the target detection information set, then the track corresponding to the target and the target detection frame number are created first. When the target is detected again and no match is found, the detection frame number is incremented by 1 until the detection frame number reaches a set threshold, and then the track corresponding to the target is published.

[0015] When no target detection information in the target detection information set matches a track in the predicted track set, and the number of consecutive unsuccessful detections of the track exceeds a set loss threshold, the track is deleted.

[0016] Furthermore, the predicted track set includes: predicted track multi-millimeter wave ID tuple, predicted track multi-millimeter wave position information, predicted track multi-millimeter wave velocity information, predicted track lidar position information, predicted track lidar velocity information, predicted track point cloud quantity, and point predicted track cloud bounding box size.

[0017] After the multi-millimeter-wave radar and / or lidar returns multiple target detection information sets, they are fused and matched with the predicted trajectory set, including:

[0018] First, perform source matching for multi-millimeter-wave radar or lidar;

[0019] Then perform heterogeneous matching between multi-millimeter wave radar and lidar.

[0020] Furthermore, the multi-millimeter-wave radar homogeneous matching includes: matching the multi-millimeter-wave radar detected target information id tuple and the predicted trajectory multi-millimeter-wave id tuple. When there is any id in the predicted trajectory multi-millimeter-wave id tuple that matches the id tuple in the detected target information, the matching is successful.

[0021] Furthermore, the lidar source matching includes: weighted averaging the distance results of the lidar-detected target information (position information, velocity information, number of point clouds, and size of the point cloud bounding box) with the corresponding position information, velocity information, number of point clouds, and size of the point cloud bounding box in the predicted trajectory to obtain a first similarity matrix between the predicted trajectory and the lidar-detected target information; using the first similarity matrix as the first input of the KM algorithm to determine whether a match is found.

[0022] Furthermore, the heterogeneous matching between the multi-millimeter-wave radar and the lidar includes:

[0023] The position and velocity information in the LiDAR / multi-millimeter wave target detection information are weighted and averaged with the corresponding data distance results of the predicted trajectory multi-millimeter wave / LiDAR position and velocity information to obtain a second similarity matrix between the predicted trajectory and the LiDAR / multi-millimeter wave target detection information. The second similarity matrix is ​​used as the second input of the KM algorithm to obtain the result of whether they match.

[0024] Furthermore, the trajectory information includes target position information, velocity information, and acceleration information;

[0025] The updated first track is a centralized track. The position information is directly updated by using the target detection information returned by multi-millimeter-wave radar and lidar, and the velocity and acceleration information are updated by Kalman filtering.

[0026] Furthermore, the id tuple includes three millimeter-wave radar IDs: left, center, and right. When detection information for a millimeter-wave radar at a certain location is missing, the ID for that location is set to None. The id tuple is expressed as follows:

[0027] (id 左 id 中 id 右 )

[0028] Among them, id 左 For the target ID information detected by the left-side millimeter-wave radar, ID 中 For mid-range millimeter-wave radar to detect target ID information, ID 右 The target ID information is detected by the millimeter-wave radar on the right.

[0029] Furthermore, prior to the initial target detection, the following steps are also included:

[0030] Multiple millimeter-wave radars are calibrated, and the data detected by the millimeter-wave radars on the left and right sides are projected onto the middle position of the multiple millimeter-wave radars according to the positional relationship of the tooling fixture through a rotation matrix, in order to obtain the target position information of the XY horizontal plane with the middle millimeter-wave radar as the first origin.

[0031] The LiDAR clusters the target point cloud based on Kd-Tree Euclidean clustering to obtain the length, width, height and centroid position information of the target point cloud in the vehicle coordinate system with the LiDAR as the second origin.

[0032] The processed millimeter-wave target location information is projected onto the coordinates of the lidar vehicle body.

[0033] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0034] 1. The fusion of multi-millimeter-wave radar and lidar can increase the field of view of environmental perception, so as to better meet the perception field requirements of wide-body trucks.

[0035] 2. This application, through lifecycle-based obstacle target trajectory management, matches and fuses sensors from the same source and different sources, which can reduce the impact of defects in a single sensor, improve the overall detection accuracy of the perception module, and provide stable obstacle detection.

[0036] 3. Using ID tuples generated by multiple millimeter waves for matching can reduce the impact of a single millimeter wave radar losing track of a target, effectively avoid the millimeter wave radar itself missing detection due to vibration, and improve the stability of target tracking.

[0037] 4. The lidar matching strategy in this application can improve the robustness of lidar matching and effectively avoid the influence of dust on environmental perception, thus erroneously filtering out the effective tracking of lidar targets.

[0038] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0039] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0040] Figure 1 This is a flowchart illustrating a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar according to an embodiment of this application;

[0041] Figure 2 This is a schematic diagram of the layout of a vehicle-mounted multi-millimeter-wave radar and lidar according to one embodiment of this application;

[0042] Figure 3This is a schematic diagram illustrating how two millimeter-wave radars are fixed on a tooling fixture according to one embodiment of this application;

[0043] Figure 4 This is a schematic diagram illustrating the method of fixing three millimeter-wave radars on a tooling fixture according to one embodiment of this application. Detailed Implementation

[0044] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form 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 intended to limit the scope of the present invention.

[0045] like Figure 1 As shown, a specific embodiment of the present invention discloses a target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar, comprising:

[0046] S10. Initial target set detection is performed using the multi-millimeter-wave radar and lidar on the mining truck, the initial target set detection information is fused to create a first track set, and the initial target velocity is obtained.

[0047] The first track set includes multiple tracks, each track being the track of a certain initial target relative to the mining card in the initial target set detection information; for example, the first track set has ten tracks, corresponding to the ten initial target detection information in the initial target set.

[0048] Specifically, before the initial target set detection, preparatory work such as the layout and calibration of multi-millimeter-wave radar and lidar needs to be carried out:

[0049] Specifically, such as Figure 2 As shown, this embodiment employs one lidar and multiple millimeter-wave radars. The lidar is installed high up on the top of the mining truck's cab, facilitating detection at greater distances. The multiple millimeter-wave radars, including at least two millimeter-wave radars, are fixed at a certain angle to a tooling fixture on the front bumper of the mining truck. The multiple millimeter-wave radars and the lidar sensor work together to detect obstacles in front of the wide-body mining truck. Optionally, as... Figure 3 and Figure 4 As shown, multiple millimeter-wave radars can be two or three millimeter-wave radars.

[0050] Specifically, the calibration of multi-millimeter-wave radar and lidar is the process of accurately calculating the relative positional relationship of sensors with different characteristics and different observation ranges. That is, the calibration process in this embodiment is divided into calibration of sensors of the same origin and calibration of sensors of different origins:

[0051] The same-source sensor calibration calibrates multiple millimeter-wave radars located in front of the mine truck bumper. Since the detection data of the millimeter-wave radar is two-dimensional data in the XY plane, with the middle position as the coordinate origin, for two millimeter-wave radars, the data detected by the left and right millimeter-wave radars is projected onto the middle position of multiple millimeter-wave radars according to the positional relationship of the tooling fixture through a rotation matrix; this is used to obtain the target position information in the XY horizontal plane with the middle millimeter-wave radar as the first origin. For three millimeter-wave radars, the millimeter-wave data from the left and right sides is projected onto the coordinates of the middle millimeter-wave radar according to the positional relationship of the tooling fixture through a rotation matrix, and the accurate positional relationship of multiple millimeter-wave radars is obtained through calibration.

[0052] The millimeter-wave radar returns target detection data in CAN frame format. The detection results are obtained by parsing the CAN frames received via the CAN port, including the target's position, velocity, and target ID in the XY plane. Based on the pre-defined positional relationships, the targets from the three millimeter-wave radars are first projected onto the coordinates of the middle millimeter-wave radar. All detected target IDs under this coordinate system are then grouped into an ID tuple containing the target IDs detected by the left, middle, and right millimeter-wave radars. 左 id 中 id 右 When a target cluster lacks detection by a certain millimeter-wave radar at a certain location, the location ID is set to None. Euclidean clustering is used to cluster the three millimeter-wave detected targets. For targets clustered into the same category, the detection location with the latest timestamp is used as the location of the millimeter-wave radar target. The average of all detected velocities of this target is used as the velocity of the millimeter-wave radar target. Finally, the location, velocity, and target ID tuple information of the multi-millimeter-wave radar target detection are obtained.

[0053] The calibration of non-homogeneous sensors involves determining the relative position between the center position of the multi-millimeter-wave radar and the roof-mounted LiDAR, using the roof-mounted LiDAR as the origin of the vehicle's coordinate system. Since the multi-millimeter-wave radar data is two-dimensional data in the XY plane, the centroid coordinates of the object's point cloud obtained by the LiDAR need to be projected onto the XY plane. Based on the manually measured relative position between the center position of the multi-millimeter-wave radar and the roof-mounted LiDAR, the processed target information detected by the multi-millimeter-wave radar is projected onto the LiDAR's vehicle coordinate system. This calibration process yields the precise positional relationship.

[0054] The LiDAR data is obtained in the form of point clouds via Ethernet. Voxelized mesh is used to downsample the point cloud. Ground point clouds are filtered out using height information, and Euclidean clustering based on Kd-Tree is used to cluster the point cloud to filter out noise and obtain effective object point cloud coordinates. By calculating the envelope of the point cloud, the length, width, height, and centroid of the vehicle body are obtained in the coordinates with the LiDAR as the second origin. The target velocity is calculated by managing the LiDAR target trajectory, and the processed target position and velocity information are obtained.

[0055] Specifically, the processed millimeter-wave detection target position is projected onto the coordinate system of the lidar vehicle.

[0056] Specifically, after the layout and calibration are completed, the multi-millimeter-wave radar and lidar begin to detect the initial target set. The multi-millimeter-wave radar and lidar detection information of the same target at different times in the initial target set detection information are fused to create the first track set. The track information includes target position information, velocity information and acceleration information.

[0057] S20. Based on the sampling intervals of the multi-millimeter-wave radar and the lidar, and based on the initial target velocity and the first track set, obtain the predicted track set;

[0058] Specifically, based on the acquired target position information, velocity information, acceleration information, and sampling time of the multi-millimeter-wave radar and lidar, a predicted track set is predicted for the next sampling time of the multi-millimeter-wave radar or lidar. The predicted track set includes: the predicted track multi-millimeter-wave ID tuple, the predicted track multi-millimeter-wave position information, the predicted track multi-millimeter-wave velocity information, the predicted track lidar position information, the predicted track lidar velocity information, the predicted track point cloud number, and the bounding box size of the point predicted track cloud.

[0059] S30. After the multi-millimeter-wave radar and / or lidar returns the target detection information set, it is fused and matched with the predicted trajectory set; based on the fusion and matching result, the first trajectory set is updated.

[0060] When the wide-body mining truck is traveling in the mining area, it uses multi-millimeter-wave radar and lidar for real-time target detection. The target detection information set returned by the multi-millimeter-wave radar and / or lidar represents the set of all target detection information in front of the mining truck at that moment. The number of targets detected in the target detection information set may be the same as or different from the number of targets in the initial target set. The matching of the target detection information set with the predicted trajectory is checked one by one to determine whether the target at the current moment is the same as the target at the previous moment, and whether there has been an increase or decrease.

[0061] After the multi-millimeter-wave radar and / or lidar returns multiple target detection information sets, they are fused and matched with the predicted track set. This includes: first, performing homogeneous matching between the multi-millimeter-wave radar and lidar; if the matching is successful, the first track set is updated; if the matching fails, then performing heterogeneous matching between the multi-millimeter-wave radar and lidar; if the heterogeneous matching is successful, the first track set is updated; if the heterogeneous matching fails, then the addition or deletion of tracks is considered.

[0062] Specifically, the update of the first track is achieved by directly updating the position information using the target detection information returned by multi-millimeter-wave radar and lidar, and updating the velocity and acceleration information using Kalman filtering.

[0063] More specifically, when updating the first track set, considering that the lidar cannot obtain accurate speed and acceleration, and that using incorrect acceleration and speed filtering will affect the target's position accuracy, this embodiment decouples the target states in the updated target detection information and updates the speed, acceleration and position information in the target states respectively.

[0064] In order to ensure that the target's position information can immediately reflect the position detected by the radar sensor, a direct update strategy is adopted to update the position;

[0065] For acceleration and velocity, a single-state Kalman filter is used for updating, that is, the acceleration information is updated separately and the velocity information is updated separately. The Kalman filter consists of two steps: prediction and update.

[0066] Track status prediction:

[0067]

[0068]

[0069] in, The current value is the predicted state of the target; H is the target state transition matrix; x k-1 This is the estimated target state value from the previous moment; P is the covariance matrix predicted at the current time. k-1 Let Q be the system covariance matrix predicted at the previous time step. k This is system noise;

[0070] Track status update:

[0071]

[0072]

[0073]

[0074] Where, x k K represents the optimal target state value after Kalman filtering update. k The Kalman gain at the current moment; Let be the covariance matrix predicted at the current time; H be the target state transition matrix, which is the identity matrix for single-state filtering; R be the observation noise, z k Acceleration or velocity information in the target detection information returned by multi-millimeter-wave radar or lidar; P is the predicted value of the target state at the current moment; k Let I be the predicted current system covariance matrix; I is the identity matrix. This is the predicted covariance matrix at the current time.

[0075] Specifically, with x k This represents the optimal target state value after Kalman filtering.

[0076] More specifically, because millimeter-wave sensors can track targets based on the waveform returned by the target, matching accuracy is prioritized for millimeter-wave radar homogeneous matching. More specifically, matching is performed using the tracking ID tuples of the millimeter-wave radar. This homogeneous matching across multiple millimeter-wave radars includes matching the target information ID tuple detected by the multi-millimeter-wave radar with the predicted trajectory multi-millimeter-wave ID tuple. A successful match is achieved when any ID in the predicted trajectory multi-millimeter-wave ID tuple matches an ID tuple in the detected target information. If a successful match occurs, the tracked target is considered to be moving along its original trajectory, and therefore the target detection information in the target detection information set needs to be updated on the corresponding trajectory in the first trajectory set. If a match fails, heterogeneous matching is performed with the lidar to reconfirm whether the initial target is moving along its original trajectory.

[0077] Compared to ID matching of a single millimeter-wave radar, ID tuple matching using multiple millimeter-wave radars can effectively solve the problem of millimeter-wave radar losing track and improve the accuracy of tracking and matching; for targets that are not matched, a matching method using non-homogeneous sensors is used for matching.

[0078] The lidar-based target matching process includes: weighted averaging the distance results of the lidar's detected target information (position, velocity, number of points, and bounding box size) with the corresponding information in the predicted trajectory to obtain a first similarity matrix between the predicted trajectory and the lidar-detected target information. This first similarity matrix is ​​used as the first input to the KM algorithm to determine whether a match has occurred. If a match is successful, the initially tracked target is considered to be moving along its original trajectory, and therefore the target detection information in the target detection set needs to be updated on the corresponding trajectory in the first trajectory set. If a match fails, heterogeneous matching is performed with a multi-millimeter-wave radar to reconfirm whether the initial target is moving along its original trajectory.

[0079] For non-homogeneous sensors, based on the current state of the target detection frame in the predicted trajectory state, a similarity matrix is ​​calculated based on the distance and velocity difference. The KM algorithm is then used for optimal matching of the bipartite graph. Specifically, the heterogeneous matching between the multi-millimeter-wave radar and the lidar includes:

[0080] The position and velocity information in the LiDAR / multi-millimeter wave target detection information are weighted and averaged with the corresponding data distance results of the predicted trajectory multi-millimeter wave / LiDAR position and velocity information to obtain a second similarity matrix between the predicted trajectory and the LiDAR / multi-millimeter wave target detection information. The second similarity matrix is ​​used as the second input of the KM algorithm to obtain the result of whether they match.

[0081] If the heterogeneous source matching is successful, it is assumed that the initial target being tracked is still moving along the original track. Therefore, the target detection information needs to be updated on the corresponding track in the first track set. If the heterogeneous source matching is unsuccessful, it is determined whether the target has been added or disappeared. If the target has been added, a new track is created. If the target has disappeared, the corresponding track is deleted.

[0082] By matching and fusing homogeneous and heterogeneous sensors through lifecycle-based trajectory management, the impact of defects in a single sensor can be reduced, the overall detection accuracy of the perception module can be improved, and stable obstacle detection can be provided.

[0083] Specifically, the update of the first track set includes:

[0084] (1) If the target detection information in the target detection information set matches the track in the predicted track set, it is considered that the initial target being tracked is still moving along the original track. Therefore, the target detection information in the target detection information set needs to be updated on the track corresponding to the first track set to ensure the continued tracking of the detected target track.

[0085] (2) If any track in the predicted track set does not match any target detection information in the target detection information set, it means that the target is a new target, i.e., the first target. Then, a new track needs to be added. First, create the track corresponding to the first target and the number of detection frames for the target. When the target is detected again in the subsequent fusion matching process and the match is unsuccessful, the number of detection frames for the track corresponding to the first target is incremented by 1 until the number of detection frames reaches the set threshold, and the track corresponding to the first target is published.

[0086] The first target detection information refers to the information of the first target that has increased in the target detection information set at the current moment compared to the target information at the previous moment.

[0087] More specifically, in the lifecycle-based track management, for a detected target that does not match a predicted track, a new track is created first, but the track is not published immediately. A detection frame count parameter is set for this detected target. In each subsequent fusion matching process, when the target is detected again and fails to match, the detection frame count is incremented by 1. When the detection frame count reaches the detection frame count threshold, the track of the target is published. Optionally, the detection frame count threshold is set to 5.

[0088] By setting the number of detection frames and releasing the alarm when the threshold is reached, it is possible to effectively prevent the generation of echoes from rain, fog, and dust particles in the air when working in rainy, foggy, or dusty conditions, thus avoiding false alarms for obstacles.

[0089] (3) When no target detection information in the target detection information set matches a certain track in the predicted track set, it means that no target matches the track. Therefore, the target detected by the track is considered to have disappeared, and tracking of the track (i.e., the first track) is not required. First, a consecutive loss count is created for the track. When the consecutive loss count of unsuccessful detections of the track exceeds a set loss count threshold, the track is deleted. Optionally, the loss count threshold is set to 10.

[0090] By setting a consecutive loss count and deleting the data when the threshold is reached, false detections or missed detections caused by sensors or external environmental factors can be reduced.

[0091] In this embodiment, the fusion of multi-millimeter-wave radar and lidar can increase the field of view for environmental perception, better meeting the needs of wide-body trucks for perception. Simultaneously, by using lifecycle-based obstacle target trajectory management and matching fused homogeneous and heterogeneous sensors, the impact of individual sensor defects can be reduced, improving the overall detection accuracy of the perception module and providing stable obstacle detection. During trajectory tracking, using ID tuples generated by multi-millimeter-wave radar for matching can reduce the impact of a single millimeter-wave radar losing track of the target, effectively avoiding missed detections by the millimeter-wave radar itself due to vibration, and improving the stability of target tracking. Based on the characteristics of lidar, the corresponding matching strategy can also improve the robustness of lidar matching, effectively avoiding the influence of dust on environmental perception and erroneously filtering out effective tracking of lidar targets.

[0092] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0093] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar, characterized in that, include: Initial target set detection is performed using multi-millimeter wave radar and lidar on the mining truck, the initial target set detection information is fused to create a first track set, and the initial target velocity is obtained; Based on the sampling intervals of the multi-millimeter-wave radar and the lidar, and according to the initial target velocity and the first track set, a predicted track set is obtained. The predicted track set includes: the predicted track multi-millimeter-wave ID tuple, the predicted track multi-millimeter-wave position information, the predicted track multi-millimeter-wave velocity information, the predicted track lidar position information, the predicted track lidar velocity information, the predicted track point cloud number, and the bounding box size of the point predicted track cloud. After the multi-millimeter-wave radar and / or lidar return the target detection information set, it is fused and matched with the predicted trajectory set to provide stable obstacle detection, including: First, perform source matching between multi-millimeter-wave radar and lidar; if the matching is successful, update the first track set; if the matching fails, perform source matching between multi-millimeter-wave radar and lidar; if the source matching is successful, update the first track set; if the source matching fails, consider adding or deleting tracks. The multi-millimeter-wave radar homogeneous matching includes: matching the multi-millimeter-wave radar detected target information id tuple and the predicted trajectory multi-millimeter-wave id tuple. When there is any id in the predicted trajectory multi-millimeter-wave id tuple that matches the id tuple in the detected target information, the matching is successful. The lidar source matching includes: the lidar detection target information, lidar position information, velocity information, number of point clouds, and point cloud bounding box size information, and the distance results of the corresponding position information, velocity information, number of point clouds, and point cloud bounding box size information in the predicted trajectory are weighted and averaged to obtain a first similarity matrix between the predicted trajectory and the lidar detection target information. The first similarity matrix is ​​used as the first input of the KM algorithm to obtain the result of whether they match. The heterogeneous matching between the multi-millimeter-wave radar and the lidar includes: The position and velocity information in the LiDAR / multi-millimeter wave target detection information are weighted and averaged with the corresponding data distance results of the predicted trajectory multi-millimeter wave / LiDAR position and velocity information to obtain a second similarity matrix between the predicted trajectory and the LiDAR / multi-millimeter wave target detection information. The second similarity matrix is ​​used as the second input of the KM algorithm to obtain the result of whether they match. Based on the fusion matching results, the first track set is updated; The update of the first track set includes: If there is target detection information in the target detection information set that successfully matches the track in the predicted track set, then the successfully matched target detection information is updated to the track corresponding to the first track set; If none of the tracks in the predicted track set match the detection information of a certain target in the target detection information set, then the track corresponding to the target and the target detection frame number are created first. When the target is detected again and no match is found, the detection frame number is incremented by 1 until the detection frame number reaches a set threshold, and then the track corresponding to the target is published. When no target detection information in the target detection information set matches a track in the predicted track set, and the number of consecutive unsuccessful detections of the track exceeds a set loss threshold, the track is deleted.

2. The target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar according to claim 1, characterized in that, The multi-millimeter-wave radar includes at least two millimeter-wave radars, and the multiple millimeter-wave radars are fixed at a certain angle to the front bumper of the mining truck by tooling fixtures. The lidar is installed at a high position on the top of the mining truck's cockpit.

3. The target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar according to claim 1, characterized in that, The trajectory information includes target position information, velocity information, and acceleration information; The updated first track is a centralized track. The position information is directly updated by using the target detection information returned by multi-millimeter-wave radar and lidar, and the velocity and acceleration information are updated by Kalman filtering.

4. The target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar according to claim 1, characterized in that, The `id` tuple includes three millimeter-wave radar IDs: left, center, and right. When detection information for a millimeter-wave radar at a certain location is missing, the ID for that location is set to `None`. The `id` tuple is expressed as follows: in, For target ID information detected by the left-side millimeter-wave radar, For mid-range millimeter-wave radar to detect target ID information, The target ID information is detected by the millimeter-wave radar on the right.

5. The target trajectory tracking method based on the fusion of multi-millimeter-wave radar and lidar according to claim 2, characterized in that, Prior to the initial target detection, the following is also included: Multiple millimeter-wave radars are calibrated, and the data detected by the millimeter-wave radars on the left and right sides are projected onto the middle position of the multiple millimeter-wave radars according to the positional relationship of the tooling fixture through a rotation matrix, in order to obtain the target position information of the XY horizontal plane with the middle millimeter-wave radar as the first origin. The LiDAR clusters the target point cloud based on Kd-Tree Euclidean clustering to obtain the length, width, height and centroid position information of the target point cloud in the vehicle coordinate system with the LiDAR as the second origin. The processed millimeter-wave target location information is projected onto the coordinates of the lidar vehicle body.

Images