Semi-distributed cross-modal relative positioning method and system

Abstract

The application provides a kind of semi-distributed cross-modal relative positioning method and system, including respectively through unmanned aerial vehicle and unmanned vehicle to the feature extraction of each frame image data collected, the feature point of current image frame, descriptor and the global descriptor of current image frame are obtained;Global descriptor is input to loop retrieval database, the frame data of the closest frame of current frame of unmanned vehicle and current frame of unmanned aerial vehicle is filtered and matched, and the key frame matching pair is obtained;Determine the initial value of the relative pose of key frame matching pair;According to the initial value of relative pose, the odometer information corresponding to the current key frame and the external parameter information, according to the Lie group coordinate transformation in three-dimensional Euclidean space, the initial value of the rotation and translation relationship between the world coordinate system of unmanned aerial vehicle and unmanned vehicle is calculated, and the relative pose of the world coordinate system of unmanned aerial vehicle and unmanned vehicle is optimized.It can realize real-time relative positioning in low bandwidth, high precision and strong robustness three-dimensional space, and improve the accuracy of relative positioning.

Description

Technical Field

[0001] This invention relates to the field of unmanned system positioning technology, and in particular to a semi-distributed cross-modal relative positioning method and system. Background Technology

[0002] With accelerated urbanization and frequent extreme weather events, the suddenness and complexity of disasters such as earthquakes, floods, and fires have increased significantly. Traditional manual search and rescue methods face bottlenecks in harsh environments (such as high temperatures, dense smoke, and structural collapses), including slow response times, limited coverage, and high personnel safety risks. Unmanned systems (such as drones and unmanned vehicles) are gradually becoming a core technological means for disaster search and rescue due to their advantages such as rapid response, strong disaster resistance, and the ability to carry specialized equipment.

[0003] However, most existing relative positioning methods for multi-unmanned systems cannot achieve real-time relative positioning in three-dimensional space with low bandwidth, high precision, and strong robustness in GNSS-denied scenarios. Summary of the Invention

[0004] This invention provides a semi-distributed cross-modal relative positioning method and system to address the shortcomings of existing relative positioning methods for unmanned systems, which are mostly unable to achieve real-time relative positioning in three-dimensional space with low bandwidth, high precision, and strong robustness in GNSS-denied scenarios.

[0005] In a first aspect, the present invention provides a semi-distributed cross-modal relative positioning method, comprising:

[0006] Each frame of image data collected is processed by drones and unmanned vehicles to extract features, thereby obtaining the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptors of the current image frame.

[0007] Input the global descriptor into the loop retrieval database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and match them to obtain key frame matching pairs;

[0008] Based on the keyframe matching pairs, the three-dimensional points corresponding to the feature points of the current unmanned vehicle keyframe are found, and 3D-2D matching point pairs are obtained.

[0009] Based on the 3D-2D information of the keyframe matching pair, the initial value for the relative pose calculation of the keyframe matching pair is determined.

[0010] Based on the initial value of the relative pose calculation, the odometer information corresponding to the current keyframe, and the extrinsic parameter information, the initial value of the rotation and translation relationship between the UAV and the unmanned vehicle world coordinate system is calculated according to the Lie group coordinate transformation in three-dimensional Euclidean space, and the relative pose of the UAV and the unmanned vehicle world coordinate system is optimized.

[0011] According to the distributed cross-modal relative positioning method provided by the present invention, the feature extraction of each frame of image data acquired by a UAV includes:

[0012] On the drone side, the current keyframe is selected based on the pose information of the current odometry.

[0013] Extract the feature points of the current keyframe, the descriptors corresponding to the feature points, and the global descriptor of the current keyframe;

[0014] The feature points, the descriptors, and the global descriptors are packaged into a vector set, which, together with the pose of the current keyframe, is sent as a data frame to the autonomous vehicle.

[0015] According to the distributed cross-modal relative positioning method provided by the present invention, the feature extraction of each frame of image data acquired by an unmanned vehicle includes:

[0016] On the autonomous vehicle side, keyframes are selected based on the pose information of the current odometer by the changes in translation and rotation.

[0017] After the unmanned vehicle obtains the keyframe image, it extracts the local feature points and descriptors of the keyframe image, as well as the global descriptor of the keyframe image.

[0018] According to a distributed cross-modal relative positioning method provided by the present invention, the step of finding the 3D points corresponding to the feature points of the current unmanned vehicle keyframe based on the keyframe matching pair to obtain 3D-2D matching point pairs includes:

[0019] The point cloud data in the world frame output by the laser odometry on the autonomous vehicle is transformed into the odometry coordinate system of the autonomous vehicle based on the current pose of the autonomous vehicle through coordinate transformation.

[0020] Based on the extrinsic parameters of the odometer coordinate system and the camera coordinate system, the point cloud data is transformed into the camera coordinate system;

[0021] Based on the camera intrinsic parameters, the point cloud data is projected onto the image plane, and the nearest neighbor match between the projected point and the feature point is found through the nearest neighbor search algorithm, which serves as a 3D-2D matching point pair.

[0022] According to a distributed cross-modal relative positioning method provided by the present invention, the optimization of the relative pose of the UAV and the unmanned vehicle in the world coordinate system includes:

[0023] Optimization of 3D-2D matching error in keyframes of autonomous vehicles;

[0024] Optimize the keyframe matching pairs in the pose optimization queue.

[0025] According to the distributed cross-modal relative positioning method provided by the present invention, the optimization of the 3D-2D matching error of the keyframe of the unmanned vehicle includes:

[0026] The essential matrix is ​​calculated based on the intrinsic parameters of the autonomous vehicle's camera. Mismatched points are eliminated, and feature point matching is performed between the current keyframe and the previous keyframe.

[0027] If the number of matches is less than or equal to the threshold, then the optimization of the current keyframe is canceled.

[0028] If the number of matches is greater than the threshold, the reprojection error is minimized by the bundle adjustment method, and the inverse depth and pose of the 3D points in the current keyframe are optimized.

[0029] According to a distributed cross-modal relative positioning method provided by the present invention, the optimization of keyframe matching pairs in the pose optimization queue includes:

[0030] Traverse the keyframe matching pairs in the pose optimization queue and estimate the relative pose from the vehicle camera coordinate system to the drone camera coordinate system based on the 3D-2D mapping of the keyframe matching pairs.

[0031] Determine the coordinate transformation from the UAV's camera coordinate system to the unmanned vehicle's world coordinate system, and the coordinate transformation from the unmanned vehicle's camera coordinate system to the unmanned vehicle's world coordinate system;

[0032] Based on the relative pose from the unmanned vehicle camera coordinate system to the drone camera coordinate system, the coordinate transformation from the drone camera coordinate system to the unmanned vehicle world coordinate system, and the coordinate transformation from the unmanned vehicle camera coordinate system to the unmanned vehicle world coordinate system, the initial values ​​from the drone world coordinate system to the unmanned vehicle world coordinate system are determined.

[0033] Based on the initial value, nonlinear optimization is performed on the reprojection error of the keyframe matching pair composed of the UAV and the unmanned vehicle.

[0034] According to the distributed cross-modal relative positioning method provided by the present invention, the reprojection error of the keyframe matching pair composed of the UAV and the unmanned vehicle, which is nonlinearly optimized, includes:

[0035] Determine the camera intrinsic parameter matrix of the UAV and the three-dimensional spatial points in the camera coordinate system of the unmanned vehicle;

[0036] The three-dimensional spatial points in the camera coordinate system of the unmanned vehicle are transformed to the camera coordinate system of the UAV to obtain the three-dimensional spatial points in the camera coordinate system of the UAV.

[0037] Based on the intrinsic parameters of the drone camera, the three-dimensional spatial points in the drone camera coordinate system are projected onto the pixel plane. The relative relationships between the three-dimensional spatial points in the drone camera coordinate system, the drone pose, and the world coordinate system are optimized by the bundle adjustment method.

[0038] A distributed cross-modal relative positioning method provided by the present invention further includes:

[0039] The optimization of the 3D-2D matching error of the unmanned vehicle keyframes and the optimization of the keyframe matching pairs in the pose optimization queue are performed in a separate thread. After each optimization, the parameters are updated. With the unmanned vehicle as the main terminal, the unmanned vehicle receives odometry information from the drone and itself. Each time a new odometry information is received, the change in the current odometry pose is calculated based on the optimized keyframe pose. Combining the relative relationship of the world coordinate system, all odometry information is unified in a world coordinate system.

[0040] Secondly, the present invention provides a semi-distributed cross-modal relative positioning system, comprising:

[0041] The feature extraction module is used to extract features from each frame of image data collected by the UAV and the unmanned vehicle respectively, to obtain the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptors of the current image frame.

[0042] The loop closure detection module is used to input the global descriptor into the loop closure retrieval database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and match them to obtain key frame matching pairs; based on the key frame matching pairs, find the three-dimensional points corresponding to the feature points of the current key frame of the unmanned vehicle to obtain 3D-2D matching point pairs.

[0043] The pose optimization module is used to determine the initial value of the relative pose calculation of the keyframe matching pair based on the 3D-2D information of the keyframe matching pair; based on the initial value of the relative pose calculation, the odometry information corresponding to the current keyframe and the extrinsic parameter information, and according to the Lie group coordinate transformation in three-dimensional Euclidean space, calculate the initial value of the rotation and translation relationship between the UAV and the unmanned vehicle world coordinate system, and optimize the relative pose of the UAV and the unmanned vehicle world coordinate system.

[0044] Thirdly, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the semi-distributed cross-modal relative positioning method as described above.

[0045] Fourthly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the semi-distributed cross-modal relative positioning method as described above.

[0046] Fifthly, the present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the semi-distributed cross-modal relative positioning method as described above.

[0047] This invention provides a semi-distributed cross-modal relative positioning method and system, comprising: extracting features from each frame of image data acquired by a UAV and an unmanned vehicle (UAV) to obtain feature points, descriptors corresponding to the feature points, and global descriptors of the current image frame; inputting the global descriptors into a loopback retrieval database, filtering and matching the frame data most similar to the current frame of the UAV and the current frame of the UAV to obtain keyframe matching pairs; based on the keyframe matching pairs, finding the 3D points corresponding to the feature points of the current keyframe of the UAV to obtain 3D-2D matching point pairs; determining the initial value for relative pose calculation of the keyframe matching pairs based on the 3D-2D information of the keyframe matching pairs; and calculating the initial value for the rotation and translation relationship between the world coordinate systems of the UAV and the UAV based on the initial value for relative pose calculation, the odometer information corresponding to the current keyframe, and the extrinsic parameter information, according to the Lie group coordinate transformation in 3D Euclidean space, and optimizing the relative pose of the world coordinate systems of the UAV and the UAV. By acquiring multi-source data and detecting loopback databases, real-time relative positioning can be achieved in a low-bandwidth, high-precision, and robust three-dimensional space, thus improving the accuracy of relative positioning. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0049] Figure 1 This is a schematic diagram of the system architecture after the sensor configuration provided in this embodiment;

[0050] Figure 2 This is a schematic diagram of the sensor hardware configuration provided in this embodiment;

[0051] Figure 3 This is a flowchart illustrating the semi-distributed cross-modal relative positioning method provided in this embodiment;

[0052] Figure 4 This is a schematic diagram of the two-stage optimization provided in this embodiment;

[0053] Figure 5 This is a schematic diagram of the dataset test results provided in this embodiment;

[0054] Figure 6This is a schematic diagram of the semi-distributed cross-modal relative positioning system provided in this embodiment;

[0055] Figure 7 This is a schematic diagram of the structure of the electronic device provided in this embodiment. Detailed Implementation

[0056] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0057] The semi-distributed cross-modal relative positioning method provided in this invention, taking a single UAV and single unmanned vehicle collaborative scenario as an example, requires the unmanned system to meet the following sensor configuration:

[0058] Unmanned aerial vehicles (UAVs): Employing a minimal sensor combination of a visible light camera and an IMU (Inertial Measurement Unit), autonomous positioning is achieved through visual inertial odometry (such as Open-VINS or VINS-MONO). In complex scenarios (such as areas with strong lighting changes or dense dynamic obstacles), a lidar system can be added, replacing it with a laser inertial odometry (such as Direct LiDAR-Inertial Odometry). This improves positioning stability by fusing laser point cloud data with IMU data. Both visual and laser odometry must ensure accurate estimation of the gravity direction, aligning the odometry's world coordinate system Z-axis with the gravity direction to eliminate rotational degree-of-freedom errors in subsequent relative positioning.

[0059] Autonomous vehicles: Due to their greater payload capacity, they need to be equipped with LiDAR, visible light cameras, and IMUs. Their positioning relies on laser odometry (such as Direct LiDAR-Inertial Odometry), which uses high-precision laser point clouds and IMU data to construct local 3D maps, ensuring centimeter-level positioning accuracy in complex terrains (such as ruins or underground utility tunnels).

[0060] Communication Module: The UAV and unmanned vehicle transmit critical data packets via lightweight communication protocols (such as LoRa or Mesh ad hoc networks), with bandwidth requirements below 0.5Mbps. The data packets contain visual feature descriptors, keyframes of laser point clouds, and timestamps of raw IMU data, ensuring spatiotemporal synchronization accuracy better than 10ms.

[0061] Aligning the odometer with the direction of gravity is crucial to the constraints of the odometer.

[0062] Visual inertial odometry (VINS-MONO) inherently possesses the ability to estimate the direction of gravity, as its world coordinate system's Z-axis is parallel to the direction of gravity, allowing for direct use in relative positioning. However, under dynamic disturbances (such as strong winds or bumpy rides in autonomous vehicles), the loss of visual features may lead to short-term drift, necessitating correction using LiDAR data.

[0063] Laser inertial odometry (such as Direct LiDAR-Inertial Odometry) achieves localization independently of the visual environment by fusing laser point clouds with IMU data, making it particularly suitable for scenarios with dense smoke or low light. During its initialization phase, it forcibly aligns with the gravity direction to avoid nonlinear optimization errors caused by coordinate system rotation deviations. In contrast, some open-source solutions (such as Fast-LIO) fail to estimate the gravity direction, resulting in an angle deviation of more than 5° between the world coordinate system's Z-axis and the gravity direction, significantly reducing relative positioning accuracy. Therefore, this embodiment requires the laser odometry to meet the gravity direction alignment constraint, or to achieve coordinate system normalization through algorithmic compensation (such as introducing a gravity vector correction module).

[0064] System scalability and compatibility: This embodiment has universal applicability to sensor configurations.

[0065] Heterogeneous system compatibility: At least one unmanned system (such as an autonomous vehicle) needs to be equipped with both LiDAR and IMU, while other systems (such as drones) can rely solely on vision and IMU. By dynamically assigning sensor roles (e.g., an autonomous vehicle acting as the master node providing LiDAR data, and a drone acting as a slave node transmitting visual features), the system can adapt to different hardware configurations.

[0066] Algorithm scalability: The modular design of open-source solutions (such as Open-VINS and Direct LiDAR-Inertial Odometry) allows for the rapid integration of new sensors (such as UWB or event cameras).

[0067] Among them, such as Figure 1 The diagram shows the system architecture after sensor configuration. Taking an air-to-ground scenario consisting of unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) as an example, the overall process covers three core modules: data preprocessing, loop closure detection (incremental maintenance and retrieval of the common-view database), and pose optimization (phase one and phase two). The UGV is the main terminal, receiving data packets sent by the UAVs. It can be expanded to a scenario of "1 UGV + multiple UAVs". The UGV calculates and maintains the relationship with the world coordinate system of each UAV, using the UGV world coordinate system as the globally consistent reference coordinate system. The specific process is as follows:

[0068] 1. UAV end data preprocessing

[0069] UAVs are equipped with IMUs (Inertial Measurement Units) and cameras, and their core function is to generate local features, global descriptors, and basic positioning data.

[0070] Positioning data generation: IMU and camera data are input into VIO (Visual Inertial Odometry) to complete UAV autonomous positioning and output UAV end pose-related data;

[0071] Feature and Descriptor Extraction: The image frames acquired by the camera are input through XFeat (feature extraction model) to generate local features AFt (UAV feature points), corresponding descriptors AD (UAV descriptors), and parameters AP (UAV image pose); then the image frames are input into MixVPR (global descriptor model) to generate global descriptors AV (UAV global descriptors).

[0072] Data transmission: Pack the pose data output by AFt, AD, AP, AV and VIO into a data packet and send it to UGV.

[0073] 2. UGV end data preprocessing

[0074] The UGV is equipped with an IMU, camera, and radar. Its core functions include generating its own local features, global descriptors, processing radar data, and receiving UAV data.

[0075] Positioning data generation: IMU and radar data are input into LIO (laser inertial odometry) to complete the autonomous positioning of UGV and output UGV end pose-related data (including camera pose).

[0076] Visual feature extraction: The image frames captured by the camera are used by XFeat to generate UGV local features GFt (UGV feature points), corresponding descriptors GD (UGV descriptors), and parameters GP (UGV image pose); then input into MixVPR to generate UGV global descriptors GV (UGV global descriptors).

[0077] Radar data processing: Radar data is input into iVox (radar data processing module), which generates UGV radar-related data GC (UGV radar data). At the same time, the camera pose is obtained through linear interpolation to ensure data temporal consistency.

[0078] 3. Loopback Detection (Incremental Maintenance and Retrieval of Common View Database)

[0079] UGV uses a shared-view database to match keyframes based on received UAV data and its own data.

[0080] Incremental maintenance: The global descriptor AV of UAV, the global descriptor GV of UGV, and their corresponding local features (AFt, GFt) are stored in the common vision database, and incremental maintenance is continuously performed to update the database data;

[0081] Retrieval and Matching: When UGV needs to be matched, the retrieval function searches the common-view database for the closest frame between the current UGV frame (including GFt and GV) and the UAV frame (including AFt and AV), completes the data association, and obtains key frame matching pairs (i.e., the matching of UAV end feature AFt:t'n and UGV end feature GF at ttn, corresponding to the feature matching relationship between UAV and UGV at different timestamps).

[0082] 4. Pose optimization (Phase 1 and Phase 2)

[0083] Using UGV as the main endpoint, keyframe matching is used to optimize pose and map points, thus implementing coordinate system one:

[0084] Phase 1 optimization: Based on the UGV data and fixed map points, perform preliminary optimization on the map points to be optimized and the map points to be corrected, and at the same time correct the camera pose of the UGV to provide accurate initial values ​​for Phase 2 optimization.

[0085] The second-stage optimization is the core optimization step. It fixes some basic poses of the UGV and UAV, and optimizes the remaining poses and map points based on the keyframe matching pairs obtained by loop closure detection, combined with the VIO output of the UAV and the LIO output of the UGV. In the process, the camera pose data of the UAV and UGV will also be used to unify all odometry information of the UAV and UGV into the world coordinate system of the UGV to achieve globally consistent relative positioning.

[0086] Hardware configuration such as Figure 2 As shown, the drone is equipped with a Realsense D435i, featuring a visible light camera and IMU, providing sensor information input for visual inertial odometry. It also includes a Livox Mid360 LiDAR, which can switch to laser inertial odometry when the visual inertial odometry is unstable, allowing the use of only the Realsense D435i's visible light camera. The autonomous vehicle is equipped with an Azure Kinect camera, providing visible light and depth information; here, only visible light information is used. It also features an OSI 128-line LiDAR with a built-in IMU, serving as the laser inertial odometry input for the vehicle, and includes a base station.

[0087] After the sensor configuration is complete, relative positioning can begin, such as... Figure 3 As shown, the specific steps include:

[0088] 101. Use drones and unmanned vehicles to extract features from each frame of image data to obtain the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptor of the current image frame.

[0089] Specifically, both drones and unmanned vehicles are equipped with visible light cameras. For each frame of input image data, feature extraction can be performed. To improve the stability of subsequent feature matching, a feature point extraction model based on a deep neural network, such as XFeat or SuperPoint, can be used. The image frame is taken as the model input, and the output is the feature points of the current image frame and the descriptor corresponding to each feature point. A feature point is a two-dimensional vector representing the pixel coordinates of certain key points in the image. The descriptor represents the feature information around the key point pixel, usually an N-dimensional vector. After extracting feature points and descriptors from two images under the condition of shared view, the correspondence between feature points in the two images can be found through feature matching. Both drones and unmanned vehicles need to extract feature points and descriptors from their respective visible light cameras as local features for image matching. After the unmanned vehicle extracts the image feature points, it also needs to fuse the feature points and point cloud data to find the correspondence between the feature points centered on the image coordinate system and the point cloud data under the current pose.

[0090] On the drone side, to reduce bandwidth load during communication, the drone selects keyframes based on its current odometry pose. For example, when the drone moves in three-dimensional space, odometry information can be used to calculate that if the current frame is translated more than 10cm or rotated more than 30° compared to the previous keyframe, it can be considered a keyframe. Once a keyframe is selected, feature points and corresponding descriptors, along with the global descriptor of the image, are extracted from the current keyframe. These are packaged into a vector set and sent to the drone as a data frame along with the pose of the current keyframe.

[0091] The unmanned vehicle and the drone maintain normal communication. The unmanned vehicle receives data frames sent by the drone. The data structure of the data frame contains a global descriptor of the image, which is used for loopback frame index retrieval. The unmanned vehicle adds the global descriptor in the data frame to the loopback frame retrieval database, which is used by the unmanned vehicle to retrieve the index of the drone keyframe that matches the image when it receives the image.

[0092] On the autonomous vehicle side, to reduce the computational load caused by too many keyframe matching pairs, the vehicle also needs to filter keyframes based on the pose information of the current odometry by the amount of translation and rotation changes. After the autonomous vehicle obtains a keyframe image, it first extracts local feature points and descriptors, as well as the global descriptor of the image.

[0093] 102. Input the global descriptor to the loop closure search database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and obtain the key frame matching pair.

[0094] Specifically, loop closure detection can employ bag-of-words models like DBow2, or neural network-based loop closure detection models like MixVPR. Its main function is to find the most similar frame(s) in the database by comparing global descriptors. The autonomous vehicle stores the data sent from the drone in the database, compares the vehicle's current frame with the drone's frames in the database, and finds the most similar frames. Since the global descriptor query method is based on image similarity, after obtaining the most similar frames, feature point matching is performed on the found image pairs to filter out the keyframe pairs with the largest number of matches.

[0095] The global descriptor of the image is fed into the loop closure retrieval database. The database returns the index numbers of the keyframes from the drones that are most similar to the current image from the autonomous vehicle. Since loop closure retrieval is based on image similarity comparison, incorrect loop closure frames may be found. In this case, feature point matching is used to match the current keyframe from the autonomous vehicle with the retrieved keyframes from the drones. During this process, the keyframe with the most matching feature points that exceeds a threshold is identified as the correct keyframe match.

[0096] 103. Based on keyframe matching pairs, find the 3D points corresponding to the feature points of the current unmanned vehicle keyframe, and obtain 3D-2D matching point pairs.

[0097] After finding the keyframe matching pair, it is also necessary to find the 3D points corresponding to the feature points of the current keyframe of the autonomous vehicle to obtain the 3D-2D matching point pair. The specific method is to transform the point cloud data of the current world frame output by the laser odometry of the autonomous vehicle into the odometry coordinate system of the autonomous vehicle through coordinate transformation according to the current pose of the autonomous vehicle. Then, according to the extrinsic information of the odometry coordinate system and the camera coordinate system, the point cloud data is transformed into the camera coordinate system. According to the camera intrinsic parameters, the point cloud data is projected onto the image plane, and the nearest neighbor matching between the projection point and the feature point is found through the nearest neighbor search algorithm.

[0098] At this point, a new keyframe matching pair is obtained. The data in the matching pair includes: the 3D-2D matching pair from the autonomous vehicle, the 2D-2D matching relationship of the feature points of the drone and the autonomous vehicle, and the pose information of the drone and the autonomous vehicle in their respective world coordinate systems. This matching pair is then added to the pose optimization data queue. In the current example, the scenario depicted has only one drone and one autonomous vehicle. If it were a scenario with multiple drones and a single autonomous vehicle, each drone would have a separate pose optimization data queue. By calculating the pose of the keyframe matching pair in each queue, a many-to-one relative positioning method can be achieved.

[0099] 104. Based on the 3D-2D information of the keyframe matching pair, determine the initial value for the relative pose calculation of the keyframe matching pair.

[0100] Specifically, the EPNP algorithm is used to calculate the initial value of the relative pose of the keyframe matching pair based on the 3D-2D information of the keyframe matching pair. Here, the relative pose refers to the rotation and translation relationship between the camera coordinate systems of the corresponding keyframe timestamps of the UAV and the unmanned vehicle.

[0101] 105. Based on the initial values ​​of relative pose, the odometer information corresponding to the current keyframe, and the extrinsic parameters, calculate the initial values ​​of the rotation and translation relationships between the UAV and the unmanned vehicle world coordinate systems according to the Lie group coordinate transformation in three-dimensional Euclidean space, and optimize the relative pose of the UAV and the unmanned vehicle world coordinate systems.

[0102] Specifically, the pose optimization can be divided into two stages. The first stage optimizes the 3D-2D matching error of the autonomous vehicle's keyframes. The second stage optimizes the keyframe matching pairs in the pose optimization queue.

[0103] The first stage focuses on optimizing the 3D-2D matching error of the keyframes of the autonomous vehicle. Since the matching is achieved by projecting the point cloud onto the image plane to find the nearest neighbor, and the point cloud is a point cloud in the world coordinate system output by the laser odometry, there may be errors caused by pose interpolation and errors of the lidar sensor. Therefore, the first stage of optimization mainly optimizes the three-dimensional points to obtain more accurate initial values ​​for 3D-2D matching.

[0104] The core idea of ​​the first-stage optimization is to use the matching relationship between the current keyframe and the previous keyframe of the autonomous vehicle to project the 3D points of the current frame onto the previous frame, minimizing the reprojection error and optimizing the 3D points corresponding to the feature points of the current keyframe. Specifically, feature point matching is performed between the current and previous keyframes. Due to the existence of mismatches, the essential matrix needs to be calculated based on the intrinsic parameters of the autonomous vehicle's camera to eliminate mismatched points and find the correct matching results. Furthermore, to ensure the optimization effect, if the number of matches between the current and previous frames is less than or equal to a threshold, the optimization of the current keyframe is canceled. If the condition is met, i.e., if the number of matches is greater than the threshold, the reprojection error is minimized using bundle adjustment, optimizing the inverse depth and pose of the 3D points in the current frame.

[0105] Failure of the first-stage optimization due to insufficient feature point matching will not have an impact, as the purpose of the first-stage optimization is to provide high-quality 3D points and poses for the second-stage optimization, thereby improving the overall system accuracy. Furthermore, the feature point matching in the first-stage optimization involves matching the features of the vehicle in two consecutive keyframes, while the second stage focuses on matching the features of the drone and the vehicle. Therefore, the 3D points optimized in the first stage are a subset and cannot cover the feature point matching results in the second stage.

[0106] The second stage optimizes the keyframe matching pairs in the pose optimization queue. This optimization primarily focuses on the keyframe matching pairs within the pose optimization queue and is the core part of the entire relative positioning method. First, the coordinate system labels are defined. We assign the UAV coordinate system 1, the vehicle coordinate system 2, the camera coordinate system C, the odometry coordinate system O, and the world coordinate system W, such as coordinate transformation T. w2w1 This refers to the transformation from world coordinate system W1 to W2.

[0107] The keyframe matching pairs in the pose optimization queue are traversed, and the relative pose from the vehicle camera coordinate system C2 to the UAV camera coordinate system C1 is estimated using the RANSAC algorithm based on the 3D-2D mapping of the keyframe matching pairs. In practice, you can use OpenCV's cv::solvePnPRansac. However, due to mismatches, the calculated result will be... There is still room for further refinement in the matching results, so after calculating... Then, the RANSAC estimation results can provide interior points, which can eliminate mismatches and re-evaluate the results. As initial values ​​for pose estimation, combined with interior points, the cv::solvePnPRefineLM function is used for further optimization. The estimation results are obtained The purpose is to provide more accurate initial values ​​for subsequent optimization.

[0108] Determine the coordinate transformation from the UAV's camera coordinate system to the UAV's world coordinate system as follows: And the coordinate transformation from the camera coordinate system of the autonomous vehicle to the world coordinate system of the autonomous vehicle. These two edges can be calculated using information from their respective odometry and the extrinsic parameters from the camera to the odometry. In addition, the odometry needs to estimate the direction of gravity to ensure that the Z-axis of the odometry's world coordinate system is parallel to the direction of gravity. Therefore, the Z-axis of the world coordinate systems of the UAV and the unmanned vehicle are parallel. Thus, it can be concluded that only the yaw angle of the UAV and the unmanned vehicle is uncertain in terms of rotation, while the roll angle and pitch angle are both 0. In addition, the translation direction is uncertain. From this, we can know that the relative pose relationship between the UAV and the unmanned vehicle in the world coordinate system only requires calculation of four variables: Yaw, tx, ty, and tz.

[0109] Therefore, based on the relative pose between the unmanned vehicle camera coordinate system and the UAV camera coordinate system, the coordinate transformation from the UAV camera coordinate system to the unmanned vehicle world coordinate system, and the coordinate transformation from the unmanned vehicle camera coordinate system to the unmanned vehicle world coordinate system, the initial values ​​from the UAV world coordinate system to the unmanned vehicle world coordinate system are determined. That is, according to the above definitions of coordinates, the initial values ​​from the UAV world coordinate system to the unmanned vehicle world coordinate system can be calculated according to formula (1). :

[0110] (1)

[0111] And because Since only the yaw angle component is involved in the rotation, it is necessary to extract the yaw angle from the rotation and set the roll and pitch angles to 0°, thus obtaining a new initial value. When the entire system has not yet been initialized, the initial value is assigned to the global variable. This global variable is continuously updated during system runtime.

[0112] After the initial value calculation is completed, the nonlinear optimization process begins. The two-stage optimization has two residual terms. The first residual term is the pose residual of four degrees of freedom. The initial value can be obtained from equation (1). According to the knowledge of Lie groups and Lie algebras in three-dimensional space, it can be known that the residuals are represented by Yaw, tx, ty, and tz. with initial value The inverse transformation multiplication is represented as an identity matrix on the Lie group and is zero on the Lie algebra. It should be noted that the rotation matrix is ​​constructed from the Yaw angle, as shown in equation (2).

[0113] (2)

[0114] The second residual term is the reprojection error of the keyframe matching pair formed by the drone and the unmanned vehicle, such as Figure 4 As shown, driverless cars and drones share some points of view in space, such as... Figure 4 The red hexagon in the image represents a 3D-2D mapping relationship between the matched feature points and the 3D points in space. Here, we assume the UAV's camera intrinsic parameter matrix is... The three-dimensional spatial points in the coordinate system of the unmanned vehicle camera are denoted as Transform the point to the camera coordinate system of the UAV to obtain... As shown in equation (3):

[0115] (3)

[0116] Then, based on the internal parameters of the drone camera... The projection is onto the pixel plane, and the 3D spatial points are optimized using the bundle adjustment (BA) method. The pose of the drone and its relative position in the world coordinate system. The pose of the autonomous vehicle has been optimized in the first stage, so the second stage optimization is fixed.

[0117] Once the two residual terms are determined, the optimization process will add the residual terms calculated from all keyframe matching pairs in the pose optimization queue to an optimization problem for calculation. After the two-stage optimization is completed, the optimized result will be obtained. Yaw, tx, ty、tz Used to represent And the optimized pose of the drone.

[0118] After completing the two-stage optimization, the optimized poses of the UAV and the autonomous vehicle on the keyframes, as well as their relative relationships within the world coordinate system, are obtained. To ensure system real-time performance, the optimization process is performed in a separate thread. Parameters are updated after each optimization. Using the autonomous vehicle as the primary endpoint, it receives odometry information from both the UAV and itself. Upon receiving new odometry information, the relative positioning method calculates the change in odometry pose based on the optimized keyframe poses. Combining this with the relative relationships within the world coordinate system, all odometry information is unified into a single world coordinate system. Down.

[0119] To verify the method of the present invention, tests were conducted on two indoor datasets. The accuracy of the relative positioning method proposed in this invention was evaluated using motion capture as the ground truth in an indoor environment, as shown in Table 1. The indoor space was approximately 100 square meters.

[0120] Table 1

[0121]

[0122] It can be seen that this method achieves high relative positioning accuracy, as shown in the experimental images. Figure 5 The image shows the test results for the dataset.

[0123] The relative positioning method proposed in this invention belongs to the back-end optimization process, realizing the unification of the coordinate system and continuous optimization of keyframe poses, resulting in a globally consistent trajectory and map. It not only achieves high precision but also ensures real-time performance. Specific advantages are as follows: Through a real-time, high-precision, low-bandwidth relative positioning method using multi-sensor fusion, centimeter-level accuracy can be achieved. The method is highly flexible, capable of integrating different visual odometry and laser odometry, and is not limited by the type of unmanned system, enabling relative positioning between air-to-air, ground-to-ground, and air-to-ground systems, achieving decoupling within the system. The semi-distributed architecture balances the computational load of each unmanned system and enables efficient data transmission, avoiding the high bandwidth requirements of multi-unmanned system communication.

[0124] Additionally, it should be noted that this invention primarily introduces and describes air-to-ground systems, but does not impose restrictions on the specific type of unmanned system. It can be used in combinations such as humanoid robot formations, robot dog formations, drone formations, unmanned vehicle formations, and drone-vehicle formations, as long as necessary constraints such as sensor configuration are met. Point cloud data is provided by LiDAR, or it can be from a depth camera, such as the Realsense D435i. In a drone-to-drone combination, one drone can acquire point cloud information through a depth camera. Both drones operate visual-inertial odometry, and the matched 2D feature points combined with depth information yield a 3D-2D matching pair.

[0125] The method of this invention belongs to the category of backend optimization. Although the backend optimization time may be relatively long, the frontend update is based on the results of the previous calculation and optimization, thus ensuring the real-time performance of the system. By unifying the world coordinate systems of each unmanned system into a globally consistent world coordinate system, to avoid the reduction in accuracy due to factors such as cumulative errors during the calculation process, the odometry used on the unmanned system is required to have gravity estimation capabilities, ensuring that the gravity direction is parallel to the Z-axis of the world coordinate system. Common vision is not required in the initial system state; as long as loop closure detection and subsequent calculation conditions are met, the relative pose can be calculated. For example, as long as the unmanned vehicle passes through a location previously visited by the drone and detects common vision, triggering the loop closure detection and calculation process, the relative pose can be calculated. Throughout the entire process, the drone and the unmanned vehicle can remain separate. The matching efficiency and accuracy based on deep neural networks are higher than traditional methods.

[0126] Based on the same general inventive concept, this invention also protects a semi-distributed cross-modal relative positioning system, which can be referred to in correspondence with the semi-distributed cross-modal relative positioning method described above.

[0127] like Figure 6 As shown, an embodiment of the present invention provides a semi-distributed cross-modal relative positioning system, comprising:

[0128] The feature extraction module 601 is used to extract features from each frame of image data collected by the UAV and the unmanned vehicle respectively, to obtain the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptors of the current image frame.

[0129] The loop closure detection module 602 is used to input the global descriptor into the loop closure retrieval database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and match them to obtain key frame matching pairs; based on the key frame matching pairs, find the three-dimensional points corresponding to the feature points of the current key frame of the unmanned vehicle to obtain 3D-2D matching point pairs.

[0130] The pose optimization module 603 is used to determine the initial value of the relative pose calculation of the keyframe matching pair based on the 3D-2D information of the keyframe matching pair; based on the initial value of the relative pose calculation, the odometry information corresponding to the current keyframe and the extrinsic parameter information, and according to the Lie group coordinate transformation in three-dimensional Euclidean space, the initial value of the rotation and translation relationship between the UAV and the unmanned vehicle world coordinate system is calculated, and the relative pose of the UAV and the unmanned vehicle world coordinate system is optimized.

[0131] Figure 7 This is a schematic diagram of the structure of the electronic device provided in this embodiment.

[0132] like Figure 7 As shown, the electronic device may include a processor 701, a communications interface 702, a memory 703, and a communication bus 704. The processor 701, communications interface 702, and memory 703 communicate with each other via the communication bus 704. The processor 701 can call logical instructions from the memory 703 to execute a semi-distributed cross-modal relative positioning method.

[0133] Furthermore, the logical instructions in the aforementioned memory 703 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0134] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer is able to execute the semi-distributed cross-modal relative positioning method provided by the above methods.

[0135] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform the semi-distributed cross-modal relative positioning methods provided by the methods described above.

[0136] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0137] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0138] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A semi-distributed cross-modal relative positioning method, characterized in that, include: Each frame of image data collected is processed by drones and unmanned vehicles to extract features, thereby obtaining the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptors of the current image frame. Input the global descriptor into the loop retrieval database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and match them to obtain key frame matching pairs; Based on the keyframe matching pairs, the three-dimensional points corresponding to the feature points of the current unmanned vehicle keyframe are found, and 3D-2D matching point pairs are obtained. Based on the 3D-2D information of the keyframe matching pair, the initial value for the relative pose calculation of the keyframe matching pair is determined. Based on the initial value of the relative pose, the odometer information corresponding to the current keyframe, and the extrinsic parameter information, the initial value of the rotation and translation relationship between the UAV and the unmanned vehicle world coordinate system is calculated according to the Lie group coordinate transformation in three-dimensional Euclidean space, and the relative pose of the UAV and the unmanned vehicle world coordinate system is optimized. The optimization of the relative pose of the UAV and the unmanned vehicle in the world coordinate system includes: optimizing the keyframe matching pairs in the pose optimization queue; The optimization of keyframe matching pairs in the pose optimization queue includes: Traverse the keyframe matching pairs in the pose optimization queue and estimate the relative pose from the vehicle camera coordinate system to the drone camera coordinate system based on the 3D-2D mapping of the keyframe matching pairs. Determine the coordinate transformation from the UAV's camera coordinate system to the unmanned vehicle's world coordinate system, and the coordinate transformation from the unmanned vehicle's camera coordinate system to the unmanned vehicle's world coordinate system; Based on the relative pose from the unmanned vehicle camera coordinate system to the drone camera coordinate system, the coordinate transformation from the drone camera coordinate system to the unmanned vehicle world coordinate system, and the coordinate transformation from the unmanned vehicle camera coordinate system to the unmanned vehicle world coordinate system, the initial values ​​from the drone world coordinate system to the unmanned vehicle world coordinate system are determined. Based on the initial value, nonlinear optimization is performed on the reprojection error of the keyframe matching pair composed of the UAV and the unmanned vehicle.

2. The semi-distributed cross-modal relative positioning method according to claim 1, characterized in that, The feature extraction of each frame of image data acquired by the drone includes: On the drone side, the current keyframe is selected based on the pose information of the current odometry. Extract the feature points of the current keyframe, the descriptors corresponding to the feature points, and the global descriptor of the current keyframe; The feature points, the descriptors, and the global descriptors are packaged into a vector set, which, together with the pose of the current keyframe, is sent as a data frame to the autonomous vehicle.

3. The semi-distributed cross-modal relative positioning method according to claim 2, characterized in that, The process of extracting features from each frame of image data collected by the unmanned vehicle includes: On the autonomous vehicle side, keyframes are selected based on the pose information of the current odometer by the changes in translation and rotation. After the unmanned vehicle obtains the keyframe image, it extracts the local feature points and descriptors of the keyframe image, as well as the global descriptor of the keyframe image.

4. The semi-distributed cross-modal relative positioning method according to claim 1, characterized in that, The step of finding the 3D points corresponding to the feature points of the current autonomous vehicle keyframe based on the keyframe matching pair, and obtaining the 3D-2D matching point pair, includes: The point cloud data in the world frame output by the laser odometry on the autonomous vehicle is transformed into the odometry coordinate system of the autonomous vehicle based on the current pose of the autonomous vehicle through coordinate transformation. Based on the extrinsic parameters of the odometer coordinate system and the camera coordinate system, the point cloud data is transformed into the camera coordinate system; Based on the camera intrinsic parameters, the point cloud data is projected onto the image plane, and the nearest neighbor match between the projected point and the feature point is found through the nearest neighbor search algorithm, which serves as a 3D-2D matching point pair.

5. The semi-distributed cross-modal relative positioning method according to claim 1, characterized in that, The optimization of the relative pose of the UAV and the unmanned vehicle in the world coordinate system also includes: Optimization of 3D-2D matching error in keyframes of autonomous vehicles.

6. The semi-distributed cross-modal relative positioning method according to claim 5, characterized in that, The optimization for the 3D-2D matching error of keyframes in autonomous vehicles includes: The essential matrix is ​​calculated based on the intrinsic parameters of the autonomous vehicle's camera. Mismatched points are eliminated, and feature point matching is performed between the current keyframe and the previous keyframe. If the number of matches is less than or equal to the threshold, then the optimization of the current keyframe is canceled. If the number of matches is greater than the threshold, the reprojection error is minimized by the bundle adjustment method, and the inverse depth and pose of the 3D points in the current keyframe are optimized.

7. The semi-distributed cross-modal relative positioning method according to claim 1, characterized in that, The reprojection error of the keyframe matching pair consisting of the UAV and the unmanned vehicle, which is nonlinearly optimized, includes: Determine the camera intrinsic parameter matrix of the UAV and the three-dimensional spatial points in the camera coordinate system of the unmanned vehicle; The three-dimensional spatial points in the camera coordinate system of the unmanned vehicle are transformed to the camera coordinate system of the UAV to obtain the three-dimensional spatial points in the camera coordinate system of the UAV. Based on the intrinsic parameters of the drone camera, the three-dimensional spatial points in the drone camera coordinate system are projected onto the pixel plane. The relative relationships between the three-dimensional spatial points in the drone camera coordinate system, the drone pose, and the world coordinate system are optimized by the bundle adjustment method.

8. The semi-distributed cross-modal relative positioning method according to claim 5, characterized in that, Also includes: The optimization of the 3D-2D matching error of the unmanned vehicle keyframes and the optimization of the keyframe matching pairs in the pose optimization queue are performed in a separate thread. After each optimization, the parameters are updated. With the unmanned vehicle as the main terminal, the unmanned vehicle receives odometry information from the drone and itself. Each time a new odometry information is received, the change in the current odometry pose is calculated based on the optimized keyframe pose. Combining the relative relationship of the world coordinate system, all odometry information is unified in a world coordinate system.

9. A semi-distributed cross-modal relative positioning system, characterized in that, include: The feature extraction module is used to extract features from each frame of image data collected by the UAV and the unmanned vehicle respectively, to obtain the feature points of the current image frame, the descriptors corresponding to the feature points, and the global descriptors of the current image frame. The loop closure detection module is used to input the global descriptor into the loop closure retrieval database, filter the frame data that is most similar to the current frame of the unmanned vehicle and the current frame of the drone, and match them to obtain key frame matching pairs; based on the key frame matching pairs, find the three-dimensional points corresponding to the feature points of the current key frame of the unmanned vehicle to obtain 3D-2D matching point pairs. The pose optimization module is used to determine the initial value of the relative pose calculation of the keyframe matching pair based on the 3D-2D information of the keyframe matching pair; and to calculate the initial value of the rotation and translation relationship between the UAV and the unmanned vehicle world coordinate system based on the initial value of the relative pose calculation, the odometry information corresponding to the current keyframe and the extrinsic parameter information, and the Lie group coordinate transformation in three-dimensional Euclidean space, thereby optimizing the relative pose of the UAV and the unmanned vehicle world coordinate system. The optimization of the relative pose of the UAV and the unmanned vehicle in the world coordinate system includes: optimizing the keyframe matching pairs in the pose optimization queue; The optimization of keyframe matching pairs in the pose optimization queue includes: Traverse the keyframe matching pairs in the pose optimization queue and estimate the relative pose from the vehicle camera coordinate system to the drone camera coordinate system based on the 3D-2D mapping of the keyframe matching pairs. Determine the coordinate transformation from the UAV's camera coordinate system to the unmanned vehicle's world coordinate system, and the coordinate transformation from the unmanned vehicle's camera coordinate system to the unmanned vehicle's world coordinate system; Based on the relative pose from the unmanned vehicle camera coordinate system to the drone camera coordinate system, the coordinate transformation from the drone camera coordinate system to the unmanned vehicle world coordinate system, and the coordinate transformation from the unmanned vehicle camera coordinate system to the unmanned vehicle world coordinate system, the initial values ​​from the drone world coordinate system to the unmanned vehicle world coordinate system are determined. Based on the initial value, nonlinear optimization is performed on the reprojection error of the keyframe matching pair composed of the UAV and the unmanned vehicle.

Images