Multi-vehicle positioning method, device and computer equipment based on multi-sensor fusion

Abstract

The above multi-vehicle positioning method, device and computer equipment based on multi-sensor fusion obtain the combined inertial navigation sensor data, perception data installed on the positioning unmanned vehicle in the formation, and the navigation positioning data of the unmanned vehicle to be positioned, correlate the navigation positioning data, perception data and the prediction data of the filter on the unmanned vehicle to be positioned at the current time step to obtain an associated data packet; according to the preset adaptive switching strategy, select one of the data in the associated data packet as the observation data of the unmanned vehicle to be positioned. Finally, input the observation data into the filter on the unmanned vehicle to be positioned to obtain the positioning result of the unmanned vehicle to be positioned in the formation. Comprehensive analysis of the data of multiple sensors can more accurately describe the external environment. Since there are differences in the effectiveness of the multi-vehicle communication system and various sensor data under different environmental conditions, data association is performed and the positioning source is autonomously switched through environmental conditions, improving the credibility of the positioning data.

Description

Technical Field

[0001] This application relates to the technical field of unmanned vehicle formation positioning, and particularly to a multi-vehicle positioning method, device, and computer device based on multi-sensor fusion. Background Art

[0002] To complete the formation mission, any unmanned vehicle in the formation needs to know the absolute positions between other unmanned vehicles to complete the corresponding path planning. Usually, the position information exchange of the multi-vehicle formation system is completed through a master-slave or distributed communication architecture for the interaction of positioning information between multiple vehicles. Any unmanned vehicle in the formation can complete the corresponding formation mission only after obtaining the absolute pose and speed information of any other unmanned vehicle.

[0003] However, the positioning method that simply relies on radio communication to exchange information such as GPS has limited application scenarios and cannot handle complex and changeable environments. When facing strong electromagnetic environments, false positioning information, and multiple environmental interferences such as sound, light, and electricity, communication refusal and the inability to obtain absolute positions often occur, and they exist concurrently. Therefore, the multi-sensor fusion technology has emerged. However, the effectiveness and credibility of different sensor data in complex environments are different, and how to adaptively switch to reliable data according to environmental conditions is a very challenging problem. At the same time, the abnormal origin of the communication time coordinate system between vehicles, the instability of the trigger periods of the multi-sensors of the positioning unmanned vehicle, and the inconsistency of the coordinate systems of different sensor data also pose challenges to the unification of data fusion results and the state design of filters. Summary of the Invention

[0004] Based on this, in view of the above technical problems, it is necessary to provide a multi-vehicle positioning method and device based on multi-sensor fusion that can autonomously switch the positioning source according to the communication environment.

[0005] A multi-vehicle positioning method based on multi-sensor fusion, the method includes:

[0006] Obtain sensor data of multiple sensors installed on the positioning unmanned vehicle in the formation; the sensor data includes: combined inertial navigation sensor data and target perception sensor data;

[0007] Determine the perception data of the unmanned vehicle to be positioned according to the target perception sensor data, and obtain the navigation and positioning data of the unmanned vehicle to be positioned through wireless communication;

[0008] Correlate the navigation and positioning data, the perception data, and the prediction data of the filter of the unmanned vehicle to be positioned for the current time step to obtain a correlation data packet;

[0009] According to a pre-set adaptive switching strategy, select one of the data in the correlation data packet as the observation data of the unmanned vehicle to be positioned;

[0010] Input the observation data into the filter on the unmanned vehicle to be located, and obtain the positioning result of the unmanned vehicle to be located in the formation.

[0011] In one embodiment, the combined inertial navigation sensor data includes: the navigation positioning data and the IMU integrated positioning data of the unmanned vehicle to be located; the navigation positioning data of the unmanned vehicle to be located includes positioning covariance; the target perception sensor data includes images, millimeter-wave radar point clouds, and lidar point clouds.

[0012] In one embodiment, the determining the perception data of the unmanned vehicle to be located according to the target perception sensor data includes:

[0013] Input the image into the YOLO network to obtain a first image with target boxes, project the lidar point cloud onto the first image, extract the point cloud located in the target boxes in the first image, process the point cloud in the target boxes respectively using the nearest neighbor clustering algorithm to obtain the clustering center points corresponding to the target boxes, and combine the first image and the clustering center points to obtain a second image.

[0014] Polarize the lidar point cloud, perform ground segmentation on the polarized point cloud to obtain a non-ground obstacle area, perform over-segmentation on the positive obstacle points in the non-ground obstacle area using the nearest neighbor clustering algorithm to obtain a positive obstacle area, and perform nearest neighbor clustering on the positive obstacle area to obtain a first lidar point cloud.

[0015] In one embodiment, before correlating the navigation positioning data, the perception data, and the prediction data of the filter on the unmanned vehicle to be located for the current time step to obtain a correlation data packet, it includes:

[0016] Obtain the perception data and navigation positioning data of the unmanned vehicle to be located in the previous time step;

[0017] Judge whether the wireless communication is normal by comparing the timestamps in the navigation positioning data of the previous frame and the current frame of the unmanned vehicle to be located, and judge whether the navigation positioning is normal according to the positioning covariance.

[0018] In one embodiment, correlating the navigation positioning data, the perception data, and the prediction data of the filter on the unmanned vehicle to be located for the current time step to obtain a correlation data packet includes:

[0019] When the wireless communication and the navigation positioning are normal:

[0020] Optimal match the navigation positioning data of the unmanned vehicle to be located in the current frame with the second image. If the match is successful, store the corresponding second image data;

[0021] Calculate the point cloud center based on the first lidar point cloud, and perform an optimal match between the navigation and positioning data of the currently located driverless vehicle and the point cloud center. If the match is successful, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the located driverless vehicle.

[0022] Perform an optimal match between the navigation and positioning data of the currently located driverless vehicle and the millimeter-wave radar point cloud. If the match is successful, store the corresponding millimeter-wave radar point cloud data.

[0023] In one embodiment, correlate the navigation and positioning data, the perception data, and the prediction data of the filter for the current time step on the located driverless vehicle to obtain a correlation data packet, further including:

[0024] When wireless communication or navigation and positioning are abnormal:

[0025] Calculate the point cloud center based on the first lidar point cloud, and perform an optimal match between the prediction data of the filter for the current time step and the point cloud center. If the match is successful, obtain the point cloud features corresponding to the located driverless vehicle, calculate the ratio of the point cloud features of the previous frame and the current frame of the located driverless vehicle. If the ratio is less than a preset threshold, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the located driverless vehicle.

[0026] Perform an optimal match between the millimeter-wave radar point clouds of the current frame and the previous frame. If the match is successful, obtain the millimeter-wave radar point cloud of the corresponding located driverless vehicle, perform an optimal match between the millimeter-wave radar point cloud and the prediction data of the filter for the current time step. If the match is successful, store the corresponding millimeter-wave radar point cloud data. If the match fails, perform an optimal match between the millimeter-wave radar point cloud and the first lidar point cloud. If the match is successful, store the corresponding millimeter-wave radar point cloud data.

[0027] Perform an optimal match between the second images of the current frame and the previous frame. If the match is successful, obtain the second image of the corresponding located driverless vehicle, perform an optimal match between the second image and the prediction data of the filter for the current time step. If the match is successful, store the corresponding second image data.

[0028] In one embodiment, select one of the correlation data as the observation data according to a pre-set adaptive switching strategy, including:

[0029] When wireless communication and navigation and positioning are normal, select the navigation and positioning data as the observation data of the located driverless vehicle.

[0030] When the wireless communication or navigation positioning is abnormal, automatically switch to a model for positioning the driverless vehicle based on the target perception sensor. According to the data priority preset by the model and the actual operating state of the target perception sensor, select one of the second image data, the first lidar point cloud center data, or the millimeter wave radar data as the observation data at the current moment.

[0031] A multi-vehicle positioning device based on multi-sensor fusion, the device includes:

[0032] A data acquisition module for acquiring sensor data of a plurality of sensors installed on the positioning driverless vehicle in the formation; the sensor data includes: combined inertial navigation sensor data and target perception sensor data;

[0033] A data determination module for determining the perception data of the driverless vehicle to be positioned according to the target perception sensor data, and obtaining the navigation positioning data of the driverless vehicle to be positioned through wireless communication;

[0034] A data association module for associating the navigation positioning data, the perception data, and the prediction data of the filter on the driverless vehicle to be positioned for the current time step to obtain an associated data packet;

[0035] A data selection module for selecting one of the associated data packets as the observation data of the driverless vehicle to be positioned according to a preset adaptive switching strategy;

[0036] A result output module for inputting the observation data into the filter on the driverless vehicle to be positioned to obtain the positioning result of the driverless vehicle to be positioned in the formation.

[0037] A computer device includes a memory and a processor, the memory stores a computer program, and when the processor executes the computer program, the steps of the method in the above embodiment are implemented.

[0038] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method in the above embodiment are implemented.

[0039] The above multi-vehicle positioning method, device and computer equipment based on multi-sensor fusion obtain the data of the combined inertial navigation sensor, perception data installed on the positioning unmanned vehicle in the formation, and the navigation and positioning data of the unmanned vehicle to be positioned, correlate the navigation and positioning data, perception data, and the prediction data of the filter on the unmanned vehicle to be positioned for the current time step to obtain a correlation data packet; select one of the data in the correlation data packet as the observation data of the unmanned vehicle to be positioned according to the preset adaptive switching strategy, and finally, input the observation data into the filter on the unmanned vehicle to be positioned to obtain the positioning result of the unmanned vehicle to be positioned in the formation. Concentrating and comprehensively analyzing the data of multiple sensors can more accurately and reliably describe the external environment. Since there are differences in the effectiveness of the multi-vehicle communication system and various sensor data under different environmental conditions, data correlation is performed and the positioning source is autonomously switched according to the environmental conditions, improving the credibility of the positioning data. Description of the Drawings

[0040] Figure 1 It is a schematic flowchart of a multi-vehicle positioning method based on multi-sensor fusion in an embodiment;

[0041] Figure 2 It is a schematic diagram of the fusion of the first image and the lidar point cloud in an embodiment;

[0042] Figure 3 It is a schematic diagram of the ground segmentation and obstacle detection effects in an actual scene in an embodiment;

[0043] Figure 4 It is a data relationship diagram of millimeter-wave radar data between the polar coordinate system and the rectangular coordinate system in an embodiment;

[0044] Figure 5 It is a block diagram of a multi-vehicle positioning system based on multi-sensor fusion in an embodiment;

[0045] Figure 6 It is a block diagram of the structure of a multi-vehicle positioning device based on multi-sensor fusion in an embodiment;

[0046] Figure 7 It is an internal structure diagram of a computer device in an embodiment. Detailed Embodiments

[0047] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0048] In one embodiment, as Figure 1 shown, a multi-vehicle positioning method based on multi-sensor fusion is provided, including the following steps:

[0049] Step 102: Obtain the sensor data of multiple sensors installed on the positioning unmanned vehicle in the formation.

[0050] The sensor data includes: combined inertial navigation sensor data and target perception sensor data.

[0051] A formation refers to a team with a certain order or organizational form. Any unit in the formation can complete the corresponding formation task only after obtaining the absolute positions between any other units.

[0052] The positioning unmanned vehicle can be any unmanned vehicle in the formation, and the located unmanned vehicles can be any number of unmanned vehicles other than the positioning unmanned vehicle in the formation; the multiple sensors can be any combination of sensors such as lidar, millimeter-wave radar, cameras, and combined inertial navigation.

[0053] Combined inertial navigation refers to a navigation system that includes satellite positioning and an inertial measurement unit (IMU), where the IMU is not affected by external electromagnetic interference.

[0054] Step 104: Determine the perception data of the located unmanned vehicle based on the target perception sensor data, and obtain the navigation and positioning data of the located unmanned vehicle through wireless communication.

[0055] The navigation and positioning data is the positioning result of the located unmanned vehicle obtained by the positioning unmanned vehicle through vehicle-to-vehicle wireless communication. This positioning result is in the absolute coordinate system of the navigation and positioning system. Since the IMU integrated pose is not affected by electromagnetic interference, the pose optimization of this vehicle relative to other vehicles uses the IMU coordinate system, and it needs to be converted to the global coordinate system of the positioning unmanned vehicle's IMU to be used for positioning and planning the formation path of the located unmanned vehicle.

[0056] Similarly, the target perception sensor data also needs to be converted to the global coordinate system of the positioning unmanned vehicle's IMU to obtain the perception data.

[0057] Step 106: Correlate the navigation and positioning data, the perception data, and the prediction data of the filter on the located unmanned vehicle for the current time step to obtain a correlation data packet.

[0058] Data correlation is a key technology for multi-sensor data fusion, which refers to establishing the corresponding relationships between the navigation and positioning data of a target, several perception data of the target by multiple sensors, etc. and a certain target, and obtaining a set of perception data of a specific target by different sensors.

[0059] Step 108: Select one of the data in the correlation data packet as the observation data of the located unmanned vehicle according to the pre-set adaptive switching strategy.

[0060] Among them, the observed data may be linear data or non-linear data. The linear data includes images, lidar point clouds, navigation and positioning data, or IM integrated positioning data, etc. The non-linear data includes millimeter-wave radar point clouds, etc.

[0061] Step 110: Input the observed data into the filter on the vehicle to be located, and obtain the positioning result of the vehicle to be located in the formation.

[0062] When the observed data is linear data, the Kalman filter (KF) can be selected as the filter. When the observed data is non-linear data, the Unscented Kalman filter (UKF) or the Extended Kalman filter (EKF) can be selected as the filter.

[0063] The above multi-vehicle positioning method, device, and computer equipment based on multi-sensor fusion obtain the combined inertial navigation sensor data, perception data, and navigation and positioning data of the vehicle to be located in the formation, correlate the navigation and positioning data, perception data, and the predicted data of the filter on the vehicle to be located at the current time step, and obtain a correlation data packet. According to the preset adaptive switching strategy, one of the data in the correlation data packet is selected as the observed data of the vehicle to be located. Finally, the observed data is input into the filter on the vehicle to be located, and the positioning result of the vehicle to be located in the formation is obtained. Gathering the data of multiple sensors together for comprehensive analysis can more accurately and reliably describe the external environment. Due to the differences in the effectiveness of the multi-vehicle communication system and various sensor data under different environmental conditions, data correlation and autonomous switching of the positioning source according to environmental conditions improve the credibility of the positioning data.

[0064] In one embodiment, the combined inertial navigation sensor data includes the navigation and positioning data of the vehicle to be located and the IMU integrated positioning data.

[0065] The navigation and positioning data includes the positioning covariance δ x , δ y , and it is possible to judge whether the navigation and positioning is affected by electromagnetic interference through this positioning covariance. When the condition: δ x > τ gps_x && δ y > τ gps_y is satisfied, it is determined that the navigation and positioning has been affected by electromagnetic interference, that is, the navigation and positioning is abnormal, the navigation and positioning data is not credible, and it is necessary to automatically switch to the model based on sensor detection. Otherwise, multi-vehicle positioning of the formation can be performed using vehicle-to-vehicle wireless communication.

[0066] The target perception sensor data includes images, millimeter-wave radar point clouds, and lidar point clouds.

[0067] In one embodiment, determining the perception data of the positioned driverless vehicle according to the target perception sensor data includes:

[0068] Input the image into the YOLO network to obtain the first image with target boxes, project the lidar point cloud onto the first image, extract the point cloud located in the target boxes in the first image, process the point cloud in the target boxes respectively using the nearest neighbor clustering algorithm to obtain the clustering center points corresponding to the target boxes, and combine the first image and the clustering center points to obtain the second image.

[0069] Polarize the lidar point cloud, perform ground segmentation on the polarized point cloud to obtain the non-ground obstacle area, perform over-segmentation on the positive obstacle points in the non-ground obstacle area using the nearest neighbor clustering algorithm to obtain the positive obstacle area, and perform nearest neighbor clustering on the positive obstacle area to obtain the first lidar point cloud.

[0070] In one embodiment, before associating the navigation and positioning data, the perception data, and the prediction data of the positioned driverless vehicle by the filter for the current time step to obtain the associated data packet, it includes:

[0071] Obtain the perception data and navigation and positioning data of the positioned driverless vehicle in the previous time step;

[0072] Judge whether the wireless communication is normal by comparing the timestamps in the navigation and positioning data of the previous frame and the current frame of the positioned driverless vehicle, and judge whether the navigation and positioning is normal according to the positioning covariance.

[0073] When the timestamp in the navigation and positioning data of the previous frame of the positioned driverless vehicle is earlier than the timestamp in the navigation and positioning data of the current frame of the positioned driverless vehicle and the two are within a reasonable time difference, the vehicle-to-vehicle wireless communication is normal.

[0074] In one embodiment, associating the navigation and positioning data, the perception data, and the prediction data of the positioned driverless vehicle by the filter for the current time step to obtain the associated data packet includes:

[0075] When the wireless communication and the navigation and positioning are normal:

[0076] Perform optimal matching between the navigation and positioning data of the current frame of the positioned driverless vehicle and the second image. If the matching is successful, store the corresponding second image data;

[0077] Calculate the point cloud center according to the first lidar point cloud, perform optimal matching between the navigation and positioning data of the current frame of the positioned driverless vehicle and the point cloud center. If the matching is successful, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned driverless vehicle;

[0078] Optimize the matching between the navigation and positioning data of the current frame of the positioned driverless vehicle and the millimeter-wave radar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data.

[0079] When wireless communication or navigation and positioning are abnormal:

[0080] Calculate the point cloud center based on the first lidar point cloud. Optimize the matching between the prediction data of the current time step by the filter and the point cloud center. If the matching is successful, obtain the point cloud features corresponding to the positioned driverless vehicle, and calculate the ratio of the point cloud features of the previous frame and the current frame of the positioned driverless vehicle. If the ratio is less than the preset threshold, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned driverless vehicle.

[0081] Optimize the matching between the millimeter-wave radar point clouds of the current frame and the previous frame. If the matching is successful, obtain the millimeter-wave radar point cloud of the corresponding positioned driverless vehicle. Optimize the matching between the millimeter-wave radar point cloud and the prediction data of the current time step by the filter. If the matching is successful, store the corresponding millimeter-wave radar point cloud data. If the matching fails, optimize the matching between the millimeter-wave radar point cloud and the first lidar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data.

[0082] Optimize the matching between the second images of the current frame and the previous frame. If the matching is successful, obtain the second image of the corresponding positioned driverless vehicle. Optimize the matching between the second image and the prediction data of the current time step by the filter. If the matching is successful, store the corresponding second image data.

[0083] In one embodiment, according to the pre-set adaptive switching strategy, select one of the associated data as the observation data, including:

[0084] When wireless communication and navigation and positioning are normal, select the navigation and positioning data as the observation data of the positioned driverless vehicle;

[0085] When wireless communication or navigation and positioning are abnormal, automatically switch to the model for positioning the driverless vehicle based on the target perception sensor. According to the pre-set data priority of the model and the actual operating state of the target perception sensor, select one of the second image data, the first lidar point cloud center data, or the millimeter-wave radar data as the observation data at the current moment.

[0086] Specifically, provide an embodiment to illustrate the method in detail. Assume that there are N vehicles in the vehicle fleet. We assume that the current program is located in vehicle i, and it is necessary to perform multi-sensor fusion positioning on the positions of the other N - 1 vehicles. In this embodiment, GPS is selected for navigation and positioning.

[0087] 1. Determine the perception data of the positioned driverless vehicle based on the target perception sensor data:

[0088] a. The first image is the detection result of the YOLO network, and the result is expressed as u and v represent the pixel coordinates of the detection result, w and h represent the length and width. There are a total of N - 1 detection results, corresponding to the other N - 1 unmanned vehicles, and the reception time of the image is T image , project the lidar point cloud onto the first image, and extract all the point clouds in the target box. Figure 2 A fusion schematic diagram of the first image and the lidar point cloud is provided. Using the region-based nearest neighbor clustering algorithm, the clustering data points of the foreground are obtained. Among them, m is the number of clusters of the clustering data points, and calculate the clustering center point:

[0089]

[0090] This is the final result of converting the image to the point cloud three-dimensional coordinate system, that is, the second image can be expressed as

[0091] b. The millimeter-wave radar point cloud is the data generated in the polar coordinate system. Each frame has 64 radar points, and each point protects the corresponding id number, radial distance ρ, and radial velocity of the located unmanned vehicle and the polar coordinate angle That is and the radar wave point cloud reception time T radar .

[0092] c. At T t_lidar moment, a three-dimensional point cloud set of obstacles containing C point clouds is collected First, polarize it, perform ground segmentation on it based on Gaussian process regression. Secondly, use the region-based nearest neighbor obstacle clustering algorithm to over-segment the positive obstacle points to obtain the positive obstacle region. Then, perform nearest neighbor clustering on the positive obstacle region to obtain the corresponding first lidar point cloud T lidar , where K is the number of clusters of the clustering point cloud, and m is the number of point clouds in a certain cluster of clustering point clouds. Figure 3 A schematic diagram of ground segmentation and obstacle detection effects in an actual scenario is provided.

[0093] 2. Obtain the GPS data, IMU integrated pose, and the pose data of the located unmanned vehicle obtained based on communication in the combined inertial navigation sensor data:

[0094] Obtain the GPS or IMU integrated positioning data of this vehicle (the positioning unmanned vehicle) for the conversion between global coordinates and local coordinates, and obtain the pose data of other vehicles through communication for the change and control of the formation. Since the IMU integrated pose is not affected by electromagnetic interference, the IMU coordinate system is used for the pose optimization of other vehicles relative to this vehicle, as follows:

[0095] Use the communication system to receive the GPS positioning data of the located unmanned vehicle, and compare the timestamp included in the GPS data of the located unmanned vehicle with the current time of this vehicle (the positioning unmanned vehicle). If the timestamp is earlier than the current time of this vehicle and within a reasonable time difference, it indicates that the wireless communication between vehicles is normal, and the GPS positioning data obtained through communication can be used; if the timestamp is later than the current time of this vehicle or even if it is earlier than the current time of this vehicle but the time difference is not within the reasonable range, it is determined that the communication is abnormal, and a communication abnormality flag is output. When the communication is abnormal, no association operation is performed on the GPS positioning data of the located unmanned vehicle in the communication system.

[0096] The positioning (x imu , y imu , z imu , Heading imu , T imu ) obtained by the IMU integration of this vehicle, and the GPS positioning data of this vehicle is (x gps , y gps , z gps , δ x , δ y , Heading gps , T gps ), where δ x , δ y is the positioning covariance of GPS, used to judge whether the GPS data is interfered. When δ x > τ gps_x && δ y > τ gps_y , the GPS has been affected by electromagnetic interference and needs to automatically switch to the model based on sensor detection. Otherwise, communication can be used for the multi-vehicle positioning of the formation. When the GPS positioning is normal, the GPS positioning data of the located unmanned vehicle is converted to the local coordinate system of this vehicle's MIU. When the GPS positioning is abnormal, the GPS data received by communication is discarded and not associated.

[0097] The rotation and translation matrices corresponding to IMU and GPS respectively are g_1, l_g represents the conversion order.

[0098] Based on the GPS positioning result (x j_c , y j_c , θ j_c ) of the located unmanned vehicle obtained through vehicle-to-vehicle communication, Tc , GPS is the absolute position. Under ideal circumstances, there will be no deviation in the GPS positioning data received by this vehicle from other vehicles. However, the IMU will have cumulative errors, and the initial relative pose errors between multiple vehicles are unknown. If IMU integrated positioning is used for vehicle-to-vehicle pose communication, inaccurate problems will occur. Therefore, vehicle-to-vehicle communication uses the GPS positioning results. When this vehicle estimates the positions of other unit vehicles in the formation, problems such as communication anomalies and inaccurate GPS will exist in the collected data. Therefore, for the self-motion estimation within this unit vehicle and the tracking of specific dynamic vehicles, we adopt the IMU integrated coordinate system.

[0099] Since the positioning results of other unit vehicles in communication are in the GPS absolute coordinate system, this result needs to be converted to the global coordinate system of this vehicle's IMU before it can be used by this vehicle's positioning and planning system. The conversion method is as follows:

[0100]

[0101] It should be noted that in this embodiment, the data obtained above are all current frame data.

[0102] Data association involves the association of the positioning results (x j_c , y j_c , θ j_c , T j_c ) obtained through communication with the second image, millimeter-wave radar point cloud, first lidar point cloud, the data association between historical data and current frame data, the data association between the heterogeneous sensor data of this vehicle, and the association between the sensor data and the predicted data of the filter. The final output is the result after updating all the effective sensor associations in the current frame. The overall data association process is as follows:

[0103] 1. Obtain the data of various sensors installed on the positioning unmanned vehicle in the previous time step:

[0104] a. The id and corresponding flag bit of the positioned unmanned vehicle matched in the detection result of the previous frame image, that is, id image_1 , flag_image_1;

[0105] b. The id and corresponding flag bit of the positioned unmanned vehicle in the detection result of the previous frame millimeter-wave radar, that is, id radar_1 , flag_radar_1;

[0106] c. The time T when the previous frame positioning unmanned vehicle released the GPS positioning data j_c_1 ;

[0107] d. The pose prediction data of the filter for the positioned unmanned vehicle in the current frame in the previous frame.

[0108] 2. When the time when the previous frame of the located autonomous vehicle publishes GPS positioning data is earlier than the time in the current frame of GPS data of the located autonomous vehicle, i.e., T j_c_1 <T j_c , and the two are within a reasonable time difference, then the vehicle-to-vehicle communication is normal, and the flag bit of the corresponding wireless communication is True, flag_c_1 = True, indicating that the wireless communication result is credible. At the same time, if the positioning covariance in the current frame of GPS data of the located autonomous vehicle is less than the corresponding GPS credibility threshold parameter, i.e., satisfying δ x ≤τ gps_x &&δ y ≤τ gps_y , then the GPS positioning is normal.

[0109] When the vehicle-to-vehicle wireless communication and GPS positioning are normal, update the current frame of sensor data:

[0110] a. Match the GPS data of the current frame of the located autonomous vehicle with the image data, and calculate the first minimum distance min(dis((x j_c , y j_c ), (p ix_image , p iy_image ))). If the first minimum distance is less than the first distance threshold τ image , store the image data p x , p y = p iX_image , p iy_image and the id of the corresponding located autonomous vehicle for use in the next frame calculation. Otherwise, flag_image_1 = false, id image_1 = -1;

[0111] b. Calculate the point cloud center of the first lidar point cloud corresponding to the located autonomous vehicle:

[0112]

[0113] Match the current frame of GPS data of the located autonomous vehicle with the lidar point cloud center, and calculate the second minimum distance If the second minimum distance is less than the second distance threshold τ lidar , then store the lidar point cloud data x, y = x i , y i and the id of the corresponding located autonomous vehicle, and at the same time update the point cloud features of the corresponding located autonomous vehicle: the number of point clouds, the length of the point cloud, and the width of the point cloud N lidar_1 , W lidar_1 , H lidar_1 , otherwise flag_lidar_1 = false.

[0114] c. Match the current frame GPS data of the positioned driverless vehicle with the millimeter-wave radar data, and calculate the corresponding third minimum distance If the third minimum distance is less than the third distance threshold τ radar , then store the millimeter-wave radar point cloud data Otherwise, flag_radar_1 = false, id radar_1 = -1.

[0115] When vehicle-to-vehicle communication or GPS positioning is abnormal, switch to the sensor-based positioning model:

[0116] a. Match the predicted data of the filter with the corresponding laser point cloud center, and calculate the fourth minimum distance between the two If the fourth minimum distance is less than the second distance threshold τ lidar , then store the lidar point cloud data and the id of the corresponding positioned driverless vehicle, update the point cloud number, point cloud length, and point cloud width N lidar_1 , W lidar_1 , H lidar_1 , select the point cloud features of min_id corresponding to the optimal matching point cloud subset, and further compare them with the point cloud features of the current frame. If the following conditions are met:

[0117] abs(N lidar_1 / N lidar [min_id]) < ξ N

[0118] &abs(H lidar_1 / H lidar [min_id]) < ξH

[0119] &abs(W lidar_1 / W lidar [min_id]) < ξw

[0120] Then flag_lidar_1 = True, otherwise flag_lidar_1 = False.

[0121] Among them, ζ N , ζ H , ζ W are respectively the ratio threshold parameters of the point cloud number, point cloud length, and point cloud width during the matching between the front and rear frames of the Lidar point cloud detection results. Then flag_lidar_1 = True, otherwise flag_lidar_1 = False

[0122] b. If in the previous frame of millimeter-wave radar detection result, flag_radar_1 = True && idradar_1 ≥0, match the millimeter-wave radar detection result of the previous frame with that of the current frame, select the min_id corresponding to the optimal matching result, match the predicted data of the filter with the corresponding millimeter-wave radar detection result, and calculate the fifth minimum distance between the two. If the fifth minimum distance is less than the third distance threshold τ radar , then store the lidar point cloud data and the id of the corresponding positioned driverless vehicle, flag_radar_1 = True, otherwise flag_radar_1 = false, id radar_1 = -1;

[0123] If the millimeter-wave radar detection result of the previous frame does not satisfy flag_radar_1 = True && id radar_1 ≥0, then match the lidar point cloud data of the previous frame with the millimeter-wave point cloud, and calculate the sixth minimum distance between the lidar point cloud data of the previous frame and the millimeter-wave radar data of the current frame If the sixth minimum distance is less than the third distance threshold τ radar , then store the millimeter-wave radar point cloud data Otherwise flag_radar_1 = false, id radar_1 = -1.

[0124] c. If in the image detection result of the previous frame, flag_image_1 = True && id image_1 ≥0, match the image data of the previous frame with the image frame of the current frame, select the min_id corresponding to the optimal matching result, select the min_id corresponding to the optimal matching result, match the predicted data of the filter with the corresponding millimeter-wave radar detection result, and calculate the seventh minimum distance min_dis = min(dis((p x , p y ), (p ix_image , p iy_image ))), if the seventh minimum distance is less than the first distance threshold τ image , then store the lidar point cloud data and the id of the corresponding positioned driverless vehicle, flag_radar_1 = True, otherwise flag_radar_1 = false, id radar_1 = -1;

[0125] If the image detection result of the previous frame does not satisfy flag_image_1 = True && id image_1 ≥0, then match the predicted data of the filter with the corresponding image detection result, and calculate the eighth minimum distance If the eighth smallest distance is less than the first distance threshold τ image , store the image data p x , p y = p ix_image , p iy_image and the id of the corresponding located autonomous vehicle for use in the next frame calculation. Otherwise, flag_image_1 = false and id image_1 = -1.

[0126] The data association process is shown in Table 1:

[0127] Table 1 Data Association Algorithm Process

[0128]

[0129]

[0130]

[0131]

[0132] 3. According to the preset adaptive switching strategy, select one of the data in the associated data packet as the observation data of the located autonomous vehicle.

[0133] Obtain the data output through data association processing, as shown in Table 2

[0134]

[0135] If flag_c_1 = True, use the GPS data of the located autonomous vehicle received through wireless communication as the observation data of the corresponding located autonomous vehicle in the current frame, that is, (p x , p y ) T = (x j_c , y j_c ) T , and the corresponding time difference Δt = T j_c_1_ T p_1 , where T p_1 is the update time of the previous frame of EKF;

[0136] If flag_c_1 = False & flag_radar_1 = True, use the millimeter-wave radar data of the located autonomous vehicle as the observation data of the current frame, that is the corresponding time difference T radar_1 -T p_1 ;

[0137] If flag_c_1 = False & flag_radar_1 = False & flag_image_1 = True, the image data of the positioned driverless vehicle is used as the observation data of the current frame, that is The corresponding time difference Δt = T image_1_ T p_1 ;

[0138] If flag_c_1 = False & flag_radar_1 = False & flag_image_1 = false & flag_lidar_1 = True, the lidar point cloud data of the positioned driverless vehicle is used as the observation data of the current frame, that is The corresponding time difference Δt = T Lidar_1 _T p_1 ;

[0139] If flag_c_1 = False & flag_radar_1 = False & flag_image_1 = false & flag_lidar_1 = false, the predicted data estimated by the filter is used as the observation data of the current frame, that is The corresponding time difference Δt = T current _T p_1 ;

[0140] As shown in Table 3, a data adaptive switching algorithm flow is provided for selecting one of the data in the associated data packet as the observation data of the positioned driverless vehicle according to the preset adaptive switching strategy in an embodiment.

[0141] For this unit vehicle, the cycle of the trigger time of various sensors is not fixed. Therefore, when the filter performs noise estimation and speed prediction, it cannot use a fixed time step for update, but needs to use a variable time step operation. If the time is abnormal, it will cause the prediction, especially the speed prediction, to become extremely unstable. For the positioning results of other vehicles received during communication transmission, generally, the GPS time is used to calibrate the clock of each vehicle, so that the time coordinate systems of all vehicles are unified under the GPS coordinate axis. Usually, the positioning moment of other vehicles received by a vehicle should not be earlier than the clock of this vehicle at this moment, but this situation occurs from time to time. Generally, due to the limitation of the GPS synchronization by the GPS receiving system, and at the same time, each unit vehicle contains multiple industrial computers, various factors cause the time coordinate systems among vehicles to be not fixed, which will cause problems such as inaccurate calculation or even invalid values in the time step of the filter update. The inconsistent time coordinate system will ultimately lead to abnormal update problems of the prediction matrix and noise covariance matrix caused by the filter.

[0142] This method solves the problem of non-fixed time coordinates by comparing the vehicle's time and the communication result receiving time during data association and data selection. Specifically, the problem of non-fixed time coordinates only occurs when GNSS positioning obtained from the communication of other vehicles is selected as the observation data for filter update during data association. The reason is that the origin of the time coordinates of other vehicles and the vehicle itself are not unified.

[0143] Table 3 Data Adaptive Switching Algorithm Process

[0144]

[0145] To solve this problem, the timestamps included in the positioning results of other vehicles are compared with the vehicle's own timestamp. If the time difference is positive, it proves that the GNSS result obtained from this communication is credible. The time difference Δt of the vehicle's sensor data is calculated using the moment of the vehicle's own time axis. When the time of the communication GPS and the vehicle's sensor time are exchanged, a strategy of only predicting one step is adopted to prevent the occurrence of time inconsistency problems.

[0146] 4. Input the observation data into the filter on the unmanned vehicle to be positioned, and obtain the positioning result of the unmanned vehicle to be positioned in the formation. Here, a specific process of inputting the observation data into the filter on the unmanned vehicle to be positioned and obtaining the positioning result of the unmanned vehicle to be positioned in the formation in an embodiment is provided:

[0147] The filter on any unmanned vehicle to be positioned includes a state prediction equation and two observation equations (linear and non-linear).

[0148] 1. For the state prediction equation, a linear state prediction equation is adopted:

[0149] x · = Fx + v

[0150] P · = FPF T + Q

[0151] That is:

[0152]

[0153]

[0154]

[0155]

[0156] Converted to matrix form

[0157]

[0158] where the output vector and the input vector are respectively

[0159]

[0160] transition matrix

[0161]

[0162] random noise matrix v ~ N(0, Q)

[0163]

[0164] Assume that the acceleration follows a distribution Then the predicted noise matrix is

[0165]

[0166] In summary, the state prediction equation and the noise covariance prediction equation are respectively

[0167] x · = Fx + v

[0168] P · = FPF T + Q

[0169] 2. State observation equations mainly include two equations. One is a linear observation equation for a two-dimensional plane rectangular coordinate system and a non-linear observation equation for a polar coordinate system. For the non-linear equation, a first-order approximation is used to convert the non-linear equation into a linear equation form, as follows:

[0170] y = z - Hx · - ω

[0171] S = HP'H T + R

[0172] K = P'H T S -1

[0173] x = x · + Ky

[0174] P = (I - KH)P'

[0175] 2.1 Observation equation for data in a two-dimensional plane rectangular coordinate system:

[0176]

[0177]

[0178]

[0179] The observation noise is a two-dimensional vector, px , p y will be affected by random noise, so

[0180]

[0181] So

[0182] S = cov(z - Hx`)

[0183] = cov(Hx + ω - Hx`)

[0184] = cov(H(x - x`)) + cov(ω)

[0185] = HP`H T + R

[0186] 2.2 Observation equation for polar coordinate-based data:

[0187] The data observed by Radar is in the polar coordinate system. And the state data output by the above prediction equation is in the global coordinate system of the IMU. Therefore, this data needs to be converted to the local coordinate system of the vehicle:

[0188]

[0189] Figure 4 provides a data relationship diagram between millimeter-wave radar data in polar coordinates and rectangular coordinates. From Figure 4 it can be obtained that the observation equation of Radar data is a non-linear equation:

[0190]

[0191] We linearize it through first-order Taylor expansion:

[0192]

[0193]

[0194] Use the Hessian matrix H as the observation matrix of Radar. Therefore, the observation deviation of Radar data is:

[0195]

[0196] Finally, the gain update and position output are:

[0197] K = P`H T S -1

[0198] x = x · + Ky

[0199] P = (I - KH)P'

[0200] Convert the local lower - state update to the global in the IMU coordinate system as the final output of the state It is:

[0201]

[0202] As Figure 5 shown, a block diagram of a multi - vehicle positioning system based on multi - sensor fusion is provided.

[0203] In one embodiment, as Figure 6 shown, a multi - vehicle positioning device based on multi - sensor fusion is provided. The device includes: a data acquisition module, a data determination module, a data association module, a data selection module, and a result output module, where:

[0204] The data acquisition module is used to acquire sensor data of multiple sensors installed on the positioning unmanned vehicle in the formation.

[0205] The sensor data includes: combined inertial navigation sensor data and target perception sensor data.

[0206] The data determination module is used to determine the perception data of the unmanned vehicle to be positioned according to the target perception sensor data, and obtain the navigation and positioning data of the unmanned vehicle to be positioned through wireless communication.

[0207] The data association module associates the navigation and positioning data, the perception data, and the prediction data of the filter on the unmanned vehicle to be positioned for the current time step to obtain an associated data packet.

[0208] The data selection module is used to select one of the data in the associated data packet as the observation data of the unmanned vehicle to be positioned according to a preset adaptive switching strategy.

[0209] The result output module is used to input the observation data into the filter on the unmanned vehicle to be positioned to obtain the positioning result of the unmanned vehicle to be positioned in the formation.

[0210] In one of the embodiments, the data determination module is further used to input an image into the YOLO network to obtain a first image with target boxes, project the lidar point cloud onto the first image, extract the point cloud located in the target boxes in the first image, process the point cloud in the target boxes respectively using the nearest neighbor clustering algorithm to obtain the clustering center points corresponding to the target boxes, and combine the first image and the clustering center points to obtain a second image.

[0211] Polarize the lidar point cloud, perform ground segmentation on the polarized point cloud to obtain a non-ground obstacle area, use the nearest neighbor clustering algorithm to over-segment the positive obstacle points in the non-ground obstacle area to obtain a positive obstacle area, and perform nearest neighbor clustering on the positive obstacle area to obtain the first lidar point cloud.

[0212] In one embodiment, the data association module is further configured to, when wireless communication and navigation positioning are normal:

[0213] Optimize the matching between the navigation and positioning data of the current frame of the positioned autonomous vehicle and the second image. If the matching is successful, store the corresponding second image data;

[0214] Calculate the point cloud center based on the first lidar point cloud, optimize the matching between the navigation and positioning data of the current frame of the positioned autonomous vehicle and the point cloud center. If the matching is successful, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned autonomous vehicle;

[0215] Optimize the matching between the navigation and positioning data of the current frame of the positioned autonomous vehicle and the millimeter-wave radar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data.

[0216] When wireless communication or navigation positioning is abnormal: Calculate the point cloud center based on the first lidar point cloud, optimize the matching between the prediction data of the filter for the current time step and the point cloud center. If the matching is successful, obtain the point cloud features corresponding to the positioned autonomous vehicle, calculate the ratio of the point cloud features of the previous frame and the current frame of the positioned autonomous vehicle. If the ratio is less than the preset threshold, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned autonomous vehicle;

[0217] Optimize the matching between the current frame and the previous frame of the millimeter-wave radar point cloud. If the matching is successful, obtain the millimeter-wave radar point cloud of the corresponding positioned autonomous vehicle, optimize the matching between the millimeter-wave radar point cloud and the prediction data of the filter for the current time step. If the matching is successful, store the corresponding millimeter-wave radar point cloud data. If the matching fails, optimize the matching between the millimeter-wave radar point cloud and the first lidar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data;

[0218] Optimize the matching between the current frame and the previous frame of the second image. If the matching is successful, obtain the second image of the corresponding positioned autonomous vehicle, optimize the matching between the second image and the prediction data of the filter for the current time step. If the matching is successful, store the corresponding second image data.

[0219] In one embodiment, the data selection module is further configured to select the navigation and positioning data as the observation data of the unmanned vehicle to be located when the wireless communication and navigation and positioning are normal. When the wireless communication or navigation and positioning is abnormal, it automatically switches to the model for positioning the unmanned vehicle based on the target perception sensor, and according to the data priority preset in the model and the actual operating state of the target perception sensor, selects one of the second image data, the first lidar point cloud center data, or the millimeter-wave radar data as the observation data at the current moment.

[0220] For the specific limitations of the multi-vehicle positioning device based on multi-sensor fusion, reference can be made to the limitations of the multi-vehicle positioning method based on multi-sensor fusion in the above text, which will not be elaborated here. Each module in the above multi-vehicle positioning device based on multi-sensor fusion can be implemented in whole or in part by software, hardware, and their combination. The above-mentioned modules can be embedded in the processor of the computer device in hardware form or be independent of it, or be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0221] In one embodiment, a computer device is provided. The computer device can be a terminal, and its internal structure diagram can be as Figure 7 shown. The computer device includes a processor, a memory, a network interface, a display screen, and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, it implements a multi-vehicle positioning method based on multi-sensor fusion. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be multiple sensors, etc. Those skilled in the art can understand that Figure 7 the structure shown is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have a different component layout.

[0222] In one embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the steps of the method in the above embodiment.

[0223] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the method in the above embodiment are implemented.

[0224] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium used in the various embodiments provided in this application can include non-volatile and / or volatile memories. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or an external cache. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), memory bus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0225] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered to be within the scope described in this specification.

[0226] The above-described embodiments merely represent several implementation manners of this application. The description is relatively specific and detailed, but it should not be construed as a limitation on the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the concept of this application, several modifications and improvements can still be made, and these all belong to the protection scope of this application. Therefore, the protection scope of the patent of this application should be subject to the appended claims.

Claims

1. A multi-vehicle positioning method based on multi-sensor fusion, characterized in that The method includes: Obtaining sensor data of multiple sensors installed on the positioning unmanned vehicle in the formation; the sensor data includes: combined inertial navigation sensor data and target perception sensor data; Determining the perception data of the positioned unmanned vehicle according to the target perception sensor data, and obtaining the navigation and positioning data of the positioned unmanned vehicle through wireless communication; Associating the navigation and positioning data, the perception data, and the prediction data of the filter on the positioned unmanned vehicle for the current time step to obtain an associated data packet; Selecting one of the data in the associated data packet as the observation data of the positioned unmanned vehicle according to a preset adaptive switching strategy; Inputting the observation data into the filter on the positioned unmanned vehicle to obtain the positioning result of the positioned unmanned vehicle in the formation; The combined inertial navigation sensor data includes: the navigation and positioning data of the positioning unmanned vehicle and IMU integrated positioning data; the navigation and positioning data of the positioning unmanned vehicle includes positioning covariance; The target perception sensor data includes images, millimeter wave radar point clouds, and lidar point clouds; Determining the perception data of the positioned unmanned vehicle according to the target perception sensor data includes: Inputting the image into the YOLO network to obtain a first image with target boxes, projecting the lidar point cloud onto the first image, extracting the point cloud located in the target boxes in the first image, processing the point cloud in the target boxes respectively using the nearest neighbor clustering algorithm to obtain the clustering center points corresponding to the target boxes, and combining the first image and the clustering center points to obtain a second image; Polarizing the lidar point cloud, performing ground segmentation on the polarized point cloud to obtain a non-ground obstacle area, performing over-segmentation on the positive obstacle points in the non-ground obstacle area using the nearest neighbor clustering algorithm to obtain a positive obstacle area, and performing nearest neighbor clustering on the positive obstacle area to obtain a first lidar point cloud.

2. The method according to claim 1, wherein Before associating the navigation and positioning data, the perception data, and the prediction data of the filter on the positioned unmanned vehicle for the current time step to obtain an associated data packet, it includes: Obtaining the perception data and navigation and positioning data of the positioned unmanned vehicle in the previous time step; Judging whether the wireless communication is normal by comparing the timestamps in the navigation and positioning data of the previous frame and the current frame of the positioned unmanned vehicle, and judging whether the navigation and positioning is normal according to the positioning covariance.

3. The method according to claim 2, wherein Associating the navigation and positioning data, the perception data, and the prediction data of the filter on the positioned unmanned vehicle for the current time step to obtain an associated data packet, includes: When the wireless communication and the navigation and positioning are normal: Performing optimal matching between the navigation and positioning data of the current frame of the positioned unmanned vehicle and the second image, and if the matching is successful, storing the corresponding second image; Calculating the point cloud center according to the first lidar point cloud, performing optimal matching between the navigation and positioning data of the current frame of the positioned unmanned vehicle and the point cloud center, and if the matching is successful, storing the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned unmanned vehicle; Optimize the matching between the navigation and positioning data of the current frame of the positioned unmanned vehicle and the millimeter-wave radar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data.

4. The method according to claim 2, wherein Correlate the navigation and positioning data, the perception data, and the prediction data of the filter on the positioned unmanned vehicle for the current time step to obtain a correlation data packet, further including: When wireless communication or navigation and positioning are abnormal: Calculate the point cloud center based on the first lidar point cloud. Optimize the matching between the prediction data of the filter for the current time step and the point cloud center. If the matching is successful, obtain the point cloud features corresponding to the positioned unmanned vehicle. Calculate the ratio of the point cloud features of the previous frame and the current frame of the positioned unmanned vehicle. If the ratio is less than the preset threshold, store the corresponding first lidar point cloud center data and the point cloud features corresponding to the positioned unmanned vehicle. Optimize the matching between the millimeter-wave radar point clouds of the current frame and the previous frame. If the matching is successful, obtain the millimeter-wave radar point cloud of the corresponding positioned unmanned vehicle. Optimize the matching between the millimeter-wave radar point cloud and the prediction data of the filter for the current time step. If the matching is successful, store the corresponding millimeter-wave radar point cloud data. If the matching fails, optimize the matching between the millimeter-wave radar point cloud and the first lidar point cloud. If the matching is successful, store the corresponding millimeter-wave radar point cloud data. Optimize the matching between the second images of the current frame and the previous frame. If the matching is successful, obtain the second image of the corresponding positioned unmanned vehicle. Optimize the matching between the second image and the prediction data of the filter for the current time step. If the matching is successful, store the corresponding second image.

5. The method according to any one of claims 1 to 4, characterized in that Select one of the correlation data as the observation data according to the preset adaptive switching strategy, including: When wireless communication and navigation and positioning are normal, select the navigation and positioning data as the observation data of the positioned unmanned vehicle. When wireless communication or navigation and positioning are abnormal, automatically switch to the model for positioning the unmanned vehicle based on the target perception sensor. According to the data priority preset by the model and the actual operating status of the target perception sensor, select one of the second image, the first lidar point cloud center data, or the millimeter-wave radar data as the observation data at the current moment.

6. A multi-vehicle positioning device based on multi-sensor fusion, characterized in that, The device includes: A data acquisition module for acquiring sensor data of multiple sensors installed on the positioned unmanned vehicle in the formation; the sensor data includes: combined inertial navigation sensor data and target perception sensor data; the combined inertial navigation sensor data includes: the navigation and positioning data and IMU integration positioning data of the positioned unmanned vehicle; the navigation and positioning data of the positioned unmanned vehicle includes positioning covariance; the target perception sensor data includes images, millimeter-wave radar point clouds, and lidar point clouds. A data determination module for determining the perception data of the positioned unmanned vehicle based on the target perception sensor data and acquiring the navigation and positioning data of the positioned unmanned vehicle through wireless communication. A data correlation module for correlating the navigation and positioning data, the perception data, and the prediction data of the filter on the positioned unmanned vehicle for the current time step to obtain a correlation data packet. A data selection module, configured to select one of the associated data packets as the observation data of the unmanned vehicle to be located according to a preset adaptive switching strategy; A result output module, configured to input the observation data into a filter on the unmanned vehicle to be located, and obtain a positioning result of the unmanned vehicle to be located in the formation; The data determination module is further configured to input the image into the YOLO network to obtain a first image with target frames, project the lidar point cloud onto the first image, extract the point cloud located in the target frames in the first image, and respectively process the point cloud in the target frames by using a nearest neighbor clustering algorithm to obtain the clustering center points corresponding to the target frames, and combine the first image and the clustering center points to obtain a second image; polarize the lidar point cloud, perform ground segmentation on the polarized point cloud to obtain a non-ground obstacle area, perform over-segmentation on the positive obstacle points in the non-ground obstacle area by using a nearest neighbor clustering algorithm to obtain a positive obstacle area, and perform nearest neighbor clustering on the positive obstacle area to obtain a first lidar point cloud.

7. A computer device, comprising a memory and a processor, the memory storing a computer program, characterized in that, When the processor executes the computer program, the steps of the method according to any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, the steps of the method according to any one of claims 1 to 5 are implemented.

Images