Method for estimating relative pose between unmanned aerial vehicles based on vision and distance measurement

Abstract

The invention provides an inter-unmanned aerial vehicle relative pose estimation method based on vision and distance measurement, which comprises the following steps that: firstly, each unmanned aerial vehicle in a cluster acquires image information and distance measurement information, the distance measurement information is transmitted to a central server, and the image information is transmitted to an unmanned aerial vehicle front-end SLAM module; the SLAM module combines the pose frames and transmits the pose frames to the central server; judging whether the current frame is a key frame or not, and sending all key frames and key frame map points in a local SLAM map of the unmanned aerial vehicle to a central server; the central server establishes a reference coordinate system and calculates a pose transformation matrix of other unmanned aerial vehicles relative to the reference unmanned aerial vehicle; the central server performs loopback detection on the key frame, performs map merging according to a detection result, establishes a relative pose optimization equation in a merged map, updates a pose frame of the front-end unmanned aerial vehicle to a pose transformation matrix under a server reference coordinate system, and sends the pose transformation matrix to the corresponding unmanned aerial vehicle; and the front-end unmanned aerial vehicle performs corresponding updating, and the observation information of other unmanned aerial vehicles acts on the current unmanned aerial vehicle, so that the relative positioning precision of the unmanned aerial vehicle cluster is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous positioning of unmanned aerial vehicle (UAV) clusters, and in particular to a method and system for estimating relative pose between UAVs based on vision and distance measurement. Background Art

[0002] In recent years, drone swarms have become a major development in drone technology. Accurately estimating the relative poses of drones within a swarm is essential for reliable operation, and real-time estimation of the relative poses of drones within a swarm is a key technology for drone swarms.

[0003] Relative positioning methods for drones within a cluster can be divided into two categories based on whether the absolute position of each individual is obtained. One category relies on external sensors to provide absolute position information for each individual in the cluster. Based on the absolute position information of the individual, the relative position between the target individual and the current individual is obtained by calculating the position transformation relationship from the absolute position coordinates to the current individual's body coordinate system. This type of method uses the absolute position information provided by the Global Navigation Satellite System (GNSS), optical capture system, or Ultra Wide Band (UWB) base station positioning system to achieve relative positioning within the cluster. However, these solutions are only applicable to open and interference-free outdoor environments or indoor scenarios covered by the positioning system, which limits the operating scenarios and mission capabilities of drones.

[0004] Another type of relative positioning method relies solely on its own sensors to acquire environmental information to achieve relative positioning in a swarm. This method primarily uses environmental imagery captured by drones, as well as inter-UAV distance and angle information, to estimate the relative position of each drone. Due to their ease of deployment and adaptability, these methods have become a hot topic in swarm relative positioning research. Current purely visual methods for estimating relative position between drones calculate relative poses by identifying markers attached to other drones. To address the difficulty and susceptibility of markers to fall off, Xian Bin of Tianjin University proposed a Darknet-based convolutional neural network method for estimating the relative position of drones within the camera's field of view. This method is limited by the field of view, resolution, and depth of field of the drone-mounted cameras, and can only identify drones within a limited range, making it inadequate for large-scale swarm operations. Wang Yong of Xidian University proposed a distributed relative positioning method for drone swarms based on distance measurement. This method divides a swarm into clusters, measures the distances between drones, and combines the real-time positioning information of drones in each cluster to establish a local relative positioning error fusion method for different clusters. The relative transformation matrix between all clusters is then iteratively calculated. This method requires real-time and accurate positioning information of each drone provided by the outside world, which limits the working scenarios of drone clusters. Summary of the Invention

[0005] In order to solve the problems existing in the prior art, the present invention proposes a relative pose estimation method between UAVs based on vision and distance measurement, which is applied in scenarios where clustered UAVs cannot obtain external global information to achieve high-precision relative positioning between UAVs.

[0006] The technical solution of the present invention is:

[0007] A method for estimating relative pose between drones based on vision and distance measurement is applied to a drone cluster consisting of at least three drones. The cluster has a central server that can communicate with all drones in the cluster. Each drone in the cluster is equipped with a camera and distance measurement equipment, as well as a communication module that can establish contact with the central server.

[0008] The method comprises the following steps:

[0009] Step 1: Each drone in the cluster receives the image information and ranging information collected by its own sensors, modifies the timestamp of the image information and ranging information to the time when the drone's onboard computing platform system receives the information, and sets an ID for each image frame and distance frame; transmits the ranging information to the central server through the communication module, and transmits the image information to the SLAM module on the front end of the drone;

[0010] Step 2: After receiving the image information, the SLAM module in the drone executes the monocular vision SLAM algorithm. After completing SLAM initialization, it tracks each frame and calculates its pose. It combines the drone ID, image frame ID and its timestamp, the image frame's reference keyframe ID, and the image frame's pose transformation matrix relative to its reference keyframe into a pose frame, which is then transmitted to the central server via the communication module.

[0011] Step 3: The SLAM module determines whether the received current frame image is a key frame. If this image frame is a key frame, it inserts this image frame into the SLAM map of the drone's local SLAM module; the communication module sends all key frames and map points observed by the key frames in the drone's local SLAM map to the central server;

[0012] Step 4: After the central server receives the pose frame and distance frame information transmitted by each drone's communication module, it sets the drone with the smallest ID as the reference drone and sets the SLAM positioning coordinate system of the reference drone as the reference coordinate system. Based on the ranging information of other drones and the reference drone, it establishes a relative pose optimization equation and solves the relative pose optimization equation to obtain the pose transformation matrix of the navigation coordinate system of other drones relative to the reference coordinate system.

[0013] Step 5: In the central server, based on the mutual ranging information of the UAVs in the cluster, the relative posture optimization equation between the UAVs is constructed. The equation is solved to obtain the posture transformation matrix of other UAVs relative to the reference UAV, and the posture transformation matrix is stored in the communication module;

[0014] Step 6: After the central server receives the key frame of the front-end UAV SLAM module sent by the communication module, it performs loop detection on the key frame; if there is a loop with the key frame in the server map, it determines whether the key frame and the loop frame belong to the same UAV map. If they belong to the same UAV map, the similarity transformation between the key frame and the loop frame is solved, and loop correction is performed to correct the posture of the map in the loop; if they are not in the same map, a new map is generated, and the map with the smaller ID in the two maps of the loop is completely copied to the new map. The relative posture transformation matrix between the map with the larger ID and the map with the smaller ID is used to convert the map with the larger ID to the new map, and map point matching is performed. The same map points are merged using the back projection and feature matching method;

[0015] Step 7: After the map is merged in step 6, the distance error between the pose frames of the two drones and the reprojection error between the keyframes and the map points are constructed, and the relative pose optimization equation in the merged map is established. The optimization equation is solved to obtain the relative pose transformation matrix in the merged map and the pose and map point position of each keyframe; based on the relative pose transformation matrix in the map merging process, the pose transformation matrix of the pose frame of the front-end drone SLAM module in the communication module is updated to the reference coordinate system in the server;

[0016] Step 8: Send the key frame pose and map point position of each pose change in the central server to the SLAM module of the corresponding drone through the communication module according to its corresponding drone ID;

[0017] Step 9: After receiving the keyframe pose and map point position, the UAV SLAM module updates the keyframe pose and map point position, and uses the updated values in subsequent optimization to apply the observation information of other UAVs to the current UAV to improve the relative positioning accuracy of the UAV cluster.

[0018] Furthermore, the specific process of step 1 is:

[0019] Step 1.1: Each drone in the cluster establishes a communication relationship with the central server through the communication module. The central server assigns a drone ID to each drone, receives the registration information sent by each drone through the communication module, and establishes a database and server map for each drone in the central server;

[0020] Step 1.2: Each UAV in the cluster collects image information of the external environment in real time and measures the distance information between other UAVs. After each UAV receives the image captured by its own camera sensor, it sets the onboard computing platform time at the time of reception as the image timestamp, adds the UAV ID and image frame ID to the image, constructs an image frame and sends it to the onboard real-time positioning and mapping module (SLAM module). After each UAV receives the ranging information output by its own ranging device, it sets the onboard computing platform time at the time of reception as the ranging timestamp, adds the ranging timestamp to the distance frame constructed by the distance measurement value, the IDs of the two UAVs involved in the ranging, and the distance frame ID, and sends the distance frame to the central server through the communication module.

[0021] Furthermore, in step 1.2, each drone measures the distance information between itself and other drones by means of information transmission and feedback. For example, drone No. 1 sends a signal by broadcasting. When drone No. 2 receives the signal broadcast by drone No. 1, it immediately feeds back a signal with its own ID added. After drone No. 1 receives the feedback signal from drone No. 2, it calculates the measured distance based on the signal time difference.

[0022] Furthermore, the specific process of step 2 is:

[0023] Step 2.1: After receiving the image information, the SLAM module performs feature point matching on two consecutive image frames. When the number of matched feature points meets the set threshold, the relative pose between the two frames is calculated based on the matching relationship between the feature points between the two frames, and the position of the map points observed jointly by the two frames is calculated using the triangulation method. The two frames are set as key frames, and the key frames and their jointly observed map point positions are inserted into the SLAM map to complete the SLAM module initialization.

[0024] Step 2.2: After completing the SLAM module initialization, set the new image frame received by the SLAM module as the current frame, and set the newly generated keyframe as the reference keyframe of the current frame; establish a matching relationship between the current frame and its reference keyframe, use the feature points of the reference keyframe and the feature points of the current frame to establish a reprojection error, establish the pose optimization equation of the current frame based on the reprojection error, optimize the pose of the current frame, and correct the position of the map points in the current frame;

[0025] Step 2.3: Combine the image frame ID, current frame timestamp, drone ID, reference keyframe ID of the current frame, and the pose transformation matrix of the current frame relative to its reference keyframe to construct a pose frame, insert the pose frame into the SLAM map, and send the pose frame to the central server through the communication module.

[0026] Furthermore, the specific process of step 3 is:

[0027] Step 3.1: After receiving the current frame image, the SLAM module calculates the difference between the current frame and its reference key frame, and determines whether to set the current frame as a key frame based on the difference;

[0028] Step 3.2: If the current frame is set as a keyframe, insert the keyframe and its observed map points into the local SLAM map of the drone; construct the keyframe's image frame ID, feature points, feature point descriptors, and word vector information into keyframe information, where the feature point descriptors are numerical vectors representing the image information of the area around the feature points, and the word vectors are word vectors generated using the DBOW2 library; construct the descriptors, map point IDs, and keyframe information of at least two map points observed by the keyframe into map point information, and send the map point information to the central server via the communication module;

[0029] Step 3.3: After the central server receives the keyframe and map point information, it inserts the word vector information in the keyframe into the server's map database. The central server performs loop detection within the cluster based on the matching relationship between the keyframes in the map database and corrects the position of the keyframe. The corrected keyframe position is sent back to the SLAM module of the corresponding drone. The SLAM module modifies the position of the corresponding keyframe and the position of the map point to achieve loop correction.

[0030] Furthermore, in step 3.1, the relative pose difference and the number of common observation feature points are used as evaluation criteria. When the relative pose difference between the current frame and its reference key frame exceeds the set threshold, the current frame is set as the key frame. Otherwise, the number of common observation feature points is further judged. If the number of common observation feature points is less than the set threshold, the current frame is set as the key frame.

[0031] Furthermore, in step 3.3, the specific process of loop correction is as follows:

[0032] Step 3.3.1: Use the word vector of the current frame to search for similar frames with the same words as the current frame in the map database. Calculate the bag-of-words vector similarity score between the current key frame and similar frames based on TF-IDF weighting. Calculate the total similarity score of all similar frames to the current frame and the current frame. Select the similar frame with a similarity score greater than 0.75 times the total similarity score as the candidate frame.

[0033] Step 3.3.2: Check whether the previous frame and the next frame of the candidate frame are both loop frames of the current frame. If so, these three frames form a consistency group.

[0034] Step 3.3.3: Based on the observation results of the current frame and the loop frame for the same map point, establish the similarity transformation optimization equation:

[0035]

[0036] In the formula b p i and a p i is the position of the i-th map point in the key frame b coordinate system and the key frame a coordinate system, s is the scaling factor between the key frame a coordinate system and the key frame b coordinate system, R b,a is the rotation matrix from the key frame b coordinate system to the key frame a coordinate system, t b,a is the translation transformation matrix from the key frame b coordinate system to the key frame a coordinate system;

[0037] Solving the similarity transformation optimization equation to obtain a similarity transformation matrix, correcting the pose of the current frame according to the similarity transformation matrix, and correcting the poses of all key frames involved in the loop;

[0038] The correction process is as follows: the similarity transformation matrix obtained is multiplied by the pose of each key frame involved in the loop to obtain the corrected key frame pose, and the similarity transformation matrix is multiplied by the position of the map point observed in each key frame involved in the loop to correct the map point; after the correction is completed, the reprojection error is constructed based on the observation relationship between the key frames involved in the loop and the map points, and the pose optimization equation is established to optimize the key frame pose and map point position;

[0039] Step 3.3.4: After loop correction is completed, perform global optimization of the server map: After loop correction is completed on the map, construct the reprojection error based on the observation relationship between all key frames and map points in the map, and weight the reprojection error to establish a global optimization equation. Solve the optimization equation to achieve global optimization of the server map.

[0040] Furthermore, the loop frame is determined in the following way: the RANSAC method is used to perform feature matching between the frame to be determined and the current frame, the relative pose is calculated based on the matching results, and the relative pose is used for back projection and matching. When the number of matching points is greater than the set threshold, the frame is considered to be a loop frame of the current frame.

[0041] Furthermore, step 4 specifically includes the following process:

[0042] Step 4.1: The central server receives the pose frame and distance frame information transmitted by the communication module of each drone, sets the drone with the smallest ID as the reference drone, and sets the SLAM positioning coordinate system of the reference drone as the reference coordinate system; based on the bidirectionality of ranging, establishes the optimization equation of the system time difference between each drone and the reference drone, and optimizes the system time difference Δt between each drone and the reference drone. i :

[0043]

[0044] In the formula is the measured distance between the reference UAV and the UAV with ID i at time t, = t+Δt i The measured distance between the drone with ID i and the reference drone at time, where Δt i The difference between the system time of the drone with ID i and the system time of the benchmark drone, where the system time is the time of the drone’s onboard computing platform;

[0045] Step 4.2: Based on the difference between each UAV system time and the benchmark UAV system time, the central server unifies the received pose frames and distance frames to the benchmark UAV system time;

[0046] The spherical coordinate interpolation method is used to interpolate the pose frame to obtain the pose of the i-th UAV unified to the benchmark UAV system time.

[0047]

[0048] cosθ=q m ·q n

[0049]

[0050] p k =p m +f·(p n -p m )

[0051] Where t k is the moment to be interpolated, q k With p k They are t k The rotation quaternion and position vector obtained by interpolating the pose frame at the moment, t m , t n t k The previous and next pose frame time, q m ,q n 、p m 、p n t m , t n The rotation quaternion and position vector corresponding to the moment;

[0052] Step 4.3: Construct the distance error between the pose frames, establish an optimization equation by weighting the bi-norm of the distance error, and solve it to obtain the similarity transformation matrix between the SLAM positioning coordinate system of each UAV and the reference coordinate system;

[0053] The optimization equation is:

[0054]

[0055] The distance error is:

[0056]

[0057] Where P i k is the position of the k-th pose frame of UAV i, is the distance measured between UAV i and the reference UAV at the kth frame, T 1,i is the position transformation matrix between the i-th UAV map and the reference map, s 1.i is the scale transformation between the i-th UAV map and the reference map, W k is the information matrix corresponding to the kth distance error.

[0058] Furthermore, step 5 specifically includes the following process:

[0059] Step 5.1: If the number of drones in the cluster is greater than 3 and the distance between drones can be measured, based on step 4, establish the distance error between the pose frames of the drones and construct the relative position optimization equation between the drones:

[0060]

[0061] Where T j,1 T 1,j The inverse transformation of the pose transformation matrix represented by ;

[0062] Step 5.2: For the key frames received subsequently, use the similarity transformation matrix obtained in step 5.1 to transform them into the reference coordinate system, and use the inverse operation of the similarity transformation matrix to process the corrected map to be transmitted back to the drone and send it to the SLAM module of the corresponding drone.

[0063] Furthermore, step 6 specifically includes the following process:

[0064] Step 6.1: After receiving the new keyframe sent by the front-end drone, insert the keyframe into the server map and perform loop detection;

[0065] Step 6.2: If the loop closure detects that the loop frame and the current key frame belong to the same map corresponding to the front-end drone, loop correction is used to optimize the key frame pose and map point position involved in the loop; after the loop correction is completed, the pose of the relevant pose frame is updated according to the pose transformation relationship between the pose frame and its reference key frame:

[0066] T j,0 =T j,ref ·T ref,0

[0067] Where Tj,0 is the pose of the j-th pose frame under its map, T j,ref is the pose transformation of the j-th pose frame relative to its reference key frame, T ref,0 The reference key frame's pose under its front-end map is used to recalculate the relative pose transformation matrix between the current UAV's corresponding map and the reference map;

[0068] Step 6.3: If the detected loop frame and the current key frame belong to the maps corresponding to different front-end drones, feature matching is performed on the associated frames of the current key frame and the loop frame, and a similarity transformation optimization equation is established based on the ranging relationship between the key frames in the two front-end drone maps:

[0069]

[0070] In the formula is the pixel coordinate of the i-th matching map point in the j-th key frame of the l-th UAV, π l is the projection matrix of the lth UAV, is the jth key frame pose of the lth UAV, T l,i is the pose transformation matrix between the i-th UAV map and the l-th UAV map, s l,i is the scale ratio between the i-th UAV and the l-th UAV; the key frame in the i-th UAV map is transformed into the new map using the transformation matrix between the i-th UAV map and the l-th UAV map.

[0071] Furthermore, the specific process of step 7 is as follows:

[0072] Step 7.1: Use the pose transformation matrix of the reference keyframe of the map point to transform the map point into the merged new map. Match each newly transformed map point with an existing map point in the current map. Project the matched map point into the observation frame of the newly transformed map point and determine the positional relationship between the projected point and the feature point corresponding to the newly transformed map point. If the pixel position of the feature point is close to that of the projected point of the matched map point, the two map points are considered to coincide, and all observation relationships of the newly transformed map point are transferred to the matched map point.

[0073] Step 7.2: After the map points are merged, a new observation relationship is established in the new map, and the two merged maps contain ranging information. Based on the observation relationship, a reprojection error is established, and based on the ranging information of the pose frame in the map, a position distance error is established. The two errors are weighted to establish a relative pose optimization equation. The optimization equation is solved to obtain the relative pose transformation matrix during the map merging process, and the pose of the keyframes and map point positions in the new map are optimized:

[0074]

[0075] In the formula is the pixel coordinate of the i-th matching map point in the k-th key frame, π k is the projection matrix of key frame k, i P k is the position of the i-th map point in the k-th key frame, P i , P j are the positions of the i-th pose frame and the j-th pose frame respectively;

[0076] Step 7.3: After the map merging is completed, based on the relationship between the new map and the reference map, the pose transformation relationship between the SLAM positioning coordinate system of all front-end drones in the new map and the reference coordinate system in the communication module is updated.

[0077] Furthermore, the specific process of step 8 is as follows:

[0078] Step 8.1: After the central server completes the map merging, it modifies the keyframe pose and map point positions of the merged map. The communication module publishes the modified keyframe pose and the modified map point position information to the corresponding front-end drone. After receiving the modified keyframe pose, the front-end drone modifies the position of the observable map point based on the modified keyframe:

[0079] T i =T i,1 ·T 1,j ·T j,k

[0080] p j =T i T ·T i ′·p j '

[0081] Where T j,k is the pose of the kth key frame in the jth map in the central server, T 1,j is the pose transformation between the jth map and the reference map, T i,1 is the pose transformation from the reference map to the front end, T i The pose of the server keyframe passed to the front-end map; j With p j ′ are the positions of the map points before and after modification, T i With T i ′ is the pose of the keyframe before and after modification;

[0082] Step 8.2: After the front-end drone receives the key frame pose and map point position information that have changed after global optimization from the server, it modifies the key frame pose and map point position according to the ID information. After the modification is completed, global optimization is performed:

[0083]

[0084] In the formula is the pixel coordinate of the kth map point in the i-th key frame, π is the projection matrix of the i-th key frame, T j is the pose of the jth key frame, P k is the position of the kth map point in the front-end map.

[0085] Beneficial effects

[0086] The proposed method relies solely on visual cameras and ranging equipment to achieve high-precision relative pose estimation between drones within a drone cluster. This method does not require changes to the structure of mobile devices or use shape-based mobile device recognition algorithms, and can estimate the relative pose between mobile devices over long distances. By utilizing a method that jointly optimizes visual and ranging information within a cluster, the present invention enables real-time autonomous positioning of mobile devices and real-time estimation of the relative pose between drones within a cluster in GNSS-denied environments.

[0087] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments with reference to the accompanying drawings, in which:

[0089] Figure 1 This is a flowchart of a relative pose estimation method between mobile devices based on vision and distance measurement proposed by the present invention. DETAILED DESCRIPTION

[0090] The following describes in detail embodiments of the present invention. The embodiments are exemplary and intended to explain the present invention, but are not to be construed as limiting the present invention.

[0091] This invention is applicable in scenarios where swarm drones lack access to external global information. The entire application scenario consists of a drone swarm and a central server capable of communicating with all drones in the swarm. Each drone in the swarm is equipped with a camera and ranging equipment, and has a communication module capable of establishing a connection with the central server. Each drone runs a front-end program, while the central server runs a back-end program. By tightly coupling the image information collected by all drones in the swarm with the distance information between drones, using visual estimation of drone motion, and leveraging shared observations and real-time ranging information between drones, the system can determine the high-precision relative position between drones, thus achieving high-precision relative positioning of drones within the swarm.

[0092] The whole method includes the following steps:

[0093] Step 1: Each drone in the cluster receives image and distance information collected by its own sensors, modifies the timestamps of these images and distance information to the time the drone's onboard computing platform receives the information, and assigns an ID to each image frame and distance frame. The distance information is then transmitted to the central server via the communication module, and the image information is then transmitted to the SLAM module on the drone's front end.

[0094] The specific process includes the following:

[0095] Step 1.1: Each drone in the cluster establishes a communication relationship with the central server through the communication module. The central server assigns a drone ID to each drone. The central server receives the registration information sent by each drone through the communication module and establishes a database and server map for each drone in the central server.

[0096] Step 1.2: Each drone in the cluster collects image information of the external environment in real time and measures the distance information between other drones. After each drone receives the image captured by its own camera sensor, it sets the onboard computing platform time at the time of reception as the image timestamp, adds the drone ID and image frame ID to the image, constructs an image frame and sends it to the onboard real-time positioning and mapping module (SLAM module). After each drone receives the ranging information output by its own ranging device, it sets the onboard computing platform time at the time of reception as the ranging timestamp, adds the ranging timestamp to the distance frame constructed by the distance measurement value, the IDs of the two drones involved in the ranging, and the distance frame ID, and sends the distance frame to the central server through the communication module.

[0097] Each drone measures the distance information between itself and other drones through information transmission and feedback. For example, drone No. 1 broadcasts a signal. When drone No. 2 receives the signal broadcast by drone No. 1, it immediately feeds back a signal with its own ID added. After drone No. 1 receives the feedback signal from drone No. 2, it calculates the measured distance based on the signal time difference.

[0098] Because the distance measurement device and image acquisition device have different frequencies and different system times, and subsequent tight coupling requires a time-matched relationship between the image and ranging information, the onboard computing platform's time is used as the reference to timestamp the ranging and image frames, ensuring that the reference times of the two types of information are consistent. Due to the high acquisition frequency of the distance and image frames, the time delay between signal acquisition and the onboard computing platform is extremely small, and the drone's displacement during this delay is negligible.

[0099] Step 2: After receiving the image information, the SLAM module in the drone executes the monocular vision SLAM algorithm. After completing SLAM initialization, it tracks each image frame and calculates its pose. It combines the drone ID, the pose transformation matrix of the image frame relative to its reference keyframe, the image frame ID and its timestamp, and the image frame's reference keyframe ID into a pose frame. This pose frame is then transmitted to the central server via the communication module.

[0100] The specific process includes the following:

[0101] Step 2.1: After receiving the image information, the SLAM module matches the feature points of two consecutive frames of images. When the number of matched feature points meets the set threshold, the relative pose between the two frames of images is calculated based on the matching relationship of the feature points between the two consecutive frames of images, and the position of the map points observed jointly by the two frames of images is calculated using the triangulation method. The two frames of images are set as key frames, and the key frames and their jointly observed map point positions are inserted into the SLAM map to complete the SLAM module initialization.

[0102] Step 2.2: After completing the SLAM module initialization, set the new image frame received by the SLAM module as the current frame, and set the newly generated key frame as the reference key frame of the current frame; establish a matching relationship between the current frame and its reference key frame, use the feature points of the reference key frame and the feature points of the current frame to establish a reprojection error, establish the pose optimization equation of the current frame based on the reprojection error, optimize the pose of the current frame and correct the position of the map points in the current frame.

[0103] Step 2.3: Combine the image frame ID, current frame timestamp, drone ID, reference keyframe ID of the current frame, and the pose transformation matrix of the current frame relative to its reference keyframe to construct a pose frame, insert the pose frame into the SLAM map, and send the pose frame to the central server through the communication module.

[0104] The pose frame method can send all visual information to the central server. The pose frame only contains the relevant ID and a pose transformation matrix, greatly reducing the communication tasks between the drone and the central server. Because keyframe images are not collected at a fixed frequency, the pose frame method can better match the ranging information.

[0105] Step 3: The SLAM module determines whether the current frame received is a keyframe. If so, it inserts the image frame into the SLAM map of the drone's local SLAM module. The communication module sends all keyframes and map points observed by the keyframes in the drone's local SLAM map to the central server.

[0106] The specific process includes the following:

[0107] Step 3.1: After the SLAM module receives the current frame image, it calculates the difference between the current frame and its reference key frame, such as the difference in timestamps, the difference in relative posture, the number of common observation feature points, etc. This embodiment uses the relative posture difference and the number of common observation feature points as evaluation criteria. When the relative posture difference between the current frame and its reference key frame exceeds the set threshold, the current frame is set as a key frame. Otherwise, the number of common observation feature points is further judged. If the number of common observation feature points is less than the set threshold, which is 30 in this embodiment, the current frame is set as a key frame.

[0108] Step 3.2: If the current frame is set as a keyframe, insert the keyframe and its observed map points into the drone's local SLAM map. Construct the keyframe's image frame ID, feature points, feature point descriptors, and word vector information into keyframe information. The feature point descriptors are numerical vectors representing the image information of the area surrounding the feature points, and the word vectors are word vectors generated using the DBOW2 library. Construct map point information from the descriptors, map point IDs, and keyframe information of at least two map points observed by the keyframe. This map point information is then sent to the central server via the communication module.

[0109] Step 3.3: After the central server receives the keyframe and map point information, it inserts the word vector information in the keyframe into the server's map database. The central server performs loop detection within the cluster based on the matching relationship between the keyframes in the map database and corrects the keyframe pose. The corrected keyframe pose is sent back to the SLAM module of the corresponding drone. The SLAM module modifies the pose of the corresponding keyframe and the position of the map point to achieve loop correction. The specific process is as follows:

[0110] Step 3.3.1: Use the word vector of the current frame to search for similar frames with the same words as the current frame in the map database. Calculate the bag-of-words vector similarity score between the current key frame and similar frames based on TF-IDF weighting. Calculate the total similarity score of all similar frames to the current frame and the current frame. Select the similar frame with a similarity score greater than 0.75 times the total similarity score as the candidate frame.

[0111] Step 3.3.2: Check whether the previous and next frames of the candidate frame are both loop frames of the current frame. If so, the three frames are considered a consistent group. The loop frame is determined by using the RANSAC method to perform feature matching on the candidate frame and the current frame, calculating the relative pose based on the matching results, and using the relative pose backprojection and matching. When the number of matching points exceeds a set threshold, the frame is considered a loop frame of the current frame.

[0112] Step 3.3.3: Based on the observation results of the current frame and the loop frame for the same map point, establish the similarity transformation optimization equation:

[0113]

[0114] In the formula b p i and a p i is the position of the i-th map point in the key frame b coordinate system and the key frame a coordinate system, s is the scaling factor between the key frame a coordinate system and the key frame b coordinate system, R b,a is the rotation matrix from the key frame b coordinate system to the key frame a coordinate system, t b,a It is the translation transformation matrix from the key frame b coordinate system to the key frame a coordinate system.

[0115] The similarity transformation optimization equation is solved to obtain a similarity transformation matrix, and the posture of the current frame is corrected according to the similarity transformation matrix, and the postures of all key frames involved in the loop are corrected.

[0116] The correction process involves multiplying the solved similarity transformation matrix by the pose of each keyframe involved in the loop closure to obtain the corrected keyframe pose. The map points are then corrected by multiplying the similarity transformation matrix by the observed map point positions in each keyframe involved in the loop closure. After the correction is complete, a reprojection error is constructed based on the observational relationship between the keyframes involved in the loop and the map points. A pose optimization equation is then established to optimize the keyframe poses and map point positions.

[0117] Step 3.3.4: After loop correction is completed, perform global optimization of the server map: After loop correction is completed on the map, construct the reprojection error based on the observation relationship between all key frames and map points in the map, and weight the reprojection error to establish a global optimization equation. Solve the optimization equation to achieve global optimization of the server map.

[0118] Step 4: After receiving the pose and range frames from each drone's communication module, the central server sets the drone with the smallest ID as the reference drone and the reference drone's SLAM positioning coordinate system as the reference coordinate system. Using the ranging information between the other drones and the reference drone, a relative pose optimization equation is established. Solving this relative pose optimization equation yields the pose transformation matrix of the other drones' navigation coordinate systems relative to the reference coordinate system.

[0119] The specific process includes the following:

[0120] Step 4.1: The central server receives the pose frame and distance frame information transmitted by the communication module of each drone, sets the drone with the smallest ID as the reference drone, and sets the SLAM positioning coordinate system of the reference drone as the reference coordinate system; based on the bidirectionality of ranging, establishes the optimization equation of the system time difference between each drone and the reference drone, and optimizes the system time difference Δt between each drone and the reference drone.i :

[0121]

[0122] In the formula is the measured distance between the reference UAV and the UAV with ID i at time t, = t+Δt i The measured distance between the drone with ID i and the reference drone at time, where Δt i The time difference between the UAV system time with ID i and the benchmark UAV system time. The system time is the time of the UAV onboard computing platform.

[0123] Step 4.2: Based on the difference between each UAV system time and the benchmark UAV system time, the central server unifies the received pose frames and distance frames to the benchmark UAV system time:

[0124] The spherical coordinate interpolation method is used to interpolate the pose frame to obtain the pose of the i-th UAV unified to the benchmark UAV system time.

[0125]

[0126] cosθ=q m ·q n

[0127]

[0128] p k =p m +f·(p n -p m )

[0129] Where t k is the moment to be interpolated, q k With p k They are t k The rotation quaternion and position vector obtained by interpolating the pose frame at the moment, t m , t n t k The previous and next pose frame time, q m ,q n 、p m 、p n t m , t n The rotation quaternion and position vector corresponding to the moment.

[0130] Step 4.3: Construct the distance error between the pose frames, establish an optimization equation by weighting the bi-norm of the distance error, and solve it to obtain the similarity transformation matrix between the SLAM positioning coordinate system of each drone and the reference coordinate system.

[0131] The optimization equation is:

[0132]

[0133] The distance error is:

[0134]

[0135] Where P i k is the position of the k-th pose frame of UAV i, is the distance measured between UAV i and the reference UAV at the kth frame, T 1,i is the position transformation matrix between the i-th UAV map and the reference map, s 1.i is the scale transformation between the i-th UAV map and the reference map, W k is the information matrix corresponding to the kth distance error.

[0136] Step 5: In the central server, based on the mutual ranging information of the UAVs in the cluster, the relative posture optimization equation between the UAVs is constructed. The equation is solved to obtain the posture transformation matrix of other UAVs relative to the reference UAV, and the posture transformation matrix is stored in the communication module.

[0137] The specific process includes the following:

[0138] Step 5.1: If the number of drones in the cluster is greater than 3 and the distance between drones can be measured, based on step 4, establish the distance error between the pose frames of the drones and construct the relative position optimization equation between the drones:

[0139]

[0140] Where T j,1 T 1,j The above formula fully utilizes the mutual ranging information between UAVs and uses the similarity transformation matrix between the UAV SLAM positioning coordinate system and the reference coordinate system calculated in step 4 as the initial value for optimization and solution. This further optimizes the relative pose transformation between the UAV SLAM positioning coordinate system and the reference coordinate system, and makes the calculation of the relative pose transformation between any UAVs through the reference coordinate system more accurate.

[0141] Step 5.2: For the key frames received subsequently, use the similarity transformation matrix obtained in step 5.1 to transform them into the reference coordinate system, and use the inverse operation of the similarity transformation matrix to process the corrected map to be transmitted back to the drone and send it to the SLAM module of the corresponding drone.

[0142] Step 6: After the central server receives the keyframe from the front-end UAV SLAM module via the communication module, it performs loop detection on the keyframe. If there is a loop with the keyframe in the server map, it determines whether the keyframe and the loop frame belong to the same UAV map. If they belong to the same UAV map, the similarity transformation between the keyframe and the loop frame is solved, and loop correction is performed to correct the position of the map in the loop. If they are not in the same map, a new map is generated, and the map with the smaller ID of the two maps in the loop is completely copied to the new map. The relative pose transformation matrix between the map with the larger ID and the map with the smaller ID is used to convert the map with the larger ID to the new map, and map point matching is performed. The same map points are merged using the back projection and feature matching method.

[0143] The specific process includes the following:

[0144] Step 6.1: After receiving the new key frame from the front-end drone, insert the key frame into the server map and perform loop detection using the method in step 3.

[0145] Step 6.2: If the loop closure detects that the loop frame and the current key frame belong to the same map corresponding to the front-end drone, the loop correction method in step 3 is used to optimize the key frame pose and map point position involved in the loop. After the loop correction is completed, the pose of the relevant pose frame is updated according to the pose transformation relationship between the pose frame and its reference key frame:

[0146] T j,0 =T j,ref ·T ref,0

[0147] Where T j,0 is the pose of the j-th pose frame under its map, T j,ref is the pose transformation of the j-th pose frame relative to its reference key frame, T ref,0 The reference key frame is used to find the pose of the front-end map, and the relative pose transformation matrix between the current UAV map and the reference map is recalculated.

[0148] Step 6.3: If the detected loop frame and the current key frame belong to the maps corresponding to different front-end drones, feature matching is performed on the associated frames of the current key frame and the loop frame, and a similarity transformation optimization equation is established based on the ranging relationship between the key frames in the two front-end drone maps:

[0149]

[0150] In the formula is the pixel coordinate of the i-th matching map point in the j-th key frame of the l-th UAV, π l is the projection matrix of the lth UAV, is the jth key frame pose of the lth UAV, T l,i is the pose transformation matrix between the i-th UAV map and the l-th UAV map, s l,i is the scale ratio between the i-th UAV and the l-th UAV; the key frame in the i-th UAV map is transformed into the new map using the transformation matrix between the i-th UAV map and the l-th UAV map.

[0151] Step 7: After the map is merged in step 6, the distance error between the pose frames of the two drones and the reprojection error between the keyframes and the map points are constructed, and the relative pose optimization equation in the merged map is established. The optimization equation is solved to obtain the relative pose transformation matrix in the merged map and the pose and map point position of each keyframe; based on the relative pose transformation matrix in the map merging process, the pose transformation matrix of the pose frame of the front-end drone SLAM module in the communication module is updated to the reference coordinate system in the server;

[0152] The specific process is:

[0153] Step 7.1: Use the pose transformation matrix of the reference keyframe of the map point to convert the map point to the merged new map. Match each newly converted map point with an existing map point in the current map. Project the matched map point to the observation frame of the newly converted map point and determine the positional relationship between the projected point and the feature point corresponding to the newly converted map point. If the pixel position of the feature point and the projected point of the matching map point are close, for example, less than 3 pixels, the two map points are considered to overlap, and all observation relationships of the newly converted map point are transferred to the matching map point.

[0154] Step 7.2: After the map points are merged, a new observation relationship is established in the new map, and the two merged maps contain ranging information. A reprojection error is established based on the observation relationship, and a position distance error is established based on the ranging information of the pose frame in the map. These two errors are weighted to establish a relative pose optimization equation. The optimization equation is solved to obtain the relative pose transformation matrix during the map merging process, and the pose of the keyframes and map point positions in the new map are optimized.

[0155]

[0156] In the formula is the pixel coordinate of the i-th matching map point in the k-th key frame, π k is the projection matrix of key frame k, i P k is the position of the i-th map point in the k-th key frame. i , P j are the positions of the i-th pose frame and the j-th pose frame respectively.

[0157] Step 7.3: After the map merging is completed, based on the relationship between the new map and the reference map, the pose transformation relationship between the SLAM positioning coordinate system of all front-end drones in the new map and the reference coordinate system in the communication module is updated.

[0158] Step 8: The key frame pose and map point position of each pose change in the central server after optimization are sent to the SLAM module of the corresponding drone through the communication module according to its corresponding drone ID.

[0159] The specific process is:

[0160] Step 8.1: After the central server completes the map merging, it modifies the keyframe pose and map point positions of the merged map. The communication module publishes the modified keyframe pose and the modified map point position information to the corresponding front-end drone. After the front-end drone receives the modified keyframe pose, it modifies the position of the observable map point based on the modified keyframe:

[0161] T i =T i,1 ·T 1,j ·T j,k

[0162] p j =T i T ·T i ′·p j '

[0163] Where T j,k is the pose of the kth key frame in the jth map in the central server, T 1,j is the pose transformation between the jth map and the reference map, T i,1 is the pose transformation from the reference map to the front end, T i The pose of the server keyframe passed to the front-end map. j With p j ′ are the positions of the map points before and after modification, T i With T i ′ is the pose of the keyframe before and after modification.

[0164] Step 8.2: After the front-end drone receives the key frame pose and map point position information that have changed after global optimization from the server, it modifies the key frame pose and map point position according to the ID information. After the modification is completed, global optimization is performed:

[0165]

[0166] In the formula is the pixel coordinate of the kth map point in the i-th key frame, π is the projection matrix of the i-th key frame, T j is the pose of the jth key frame, P k is the position of the kth map point in the front-end map.

[0167] Step 9: After receiving the pose transformation of the key frame and map point, the UAV SLAM module updates the pose of the key frame and the position of the map point, and uses the updated values in subsequent optimization to apply the observation information of other UAVs to the current UAV, thereby improving the relative positioning accuracy of the UAV cluster.

[0168] Based on the above method, this embodiment also proposes a relative pose estimation system between mobile devices based on vision and distance measurement, including: a drone front-end module, a central map module, and a front-end and back-end communication module;

[0169] The drone's front-end module uses environmental information collected by the camera to execute an independent visual SLAM algorithm, calculating the pose of each frame in real time and selecting keyframes. It saves the pose frames and keyframes in a local map, awaiting transmission from the communication module. It receives corrections to the keyframe poses and map point positions from the central map module and performs global optimization based on these modified keyframe poses and map point positions. It also receives ranging information and sets the timestamp of the distance measurement frame to the front-end system time.

[0170] The central map module receives pose frames and keyframe information published by the communication module. Based on the bidirectional nature of distance measurement, it achieves temporal alignment between different front-ends. Pose frames and measured distances are used to achieve coarse registration between maps from different front-ends. Loop detection and correction algorithms are applied to newly added keyframes, enabling pose optimization within the same map and map fusion between different maps. The fused map undergoes joint pose optimization, tightly coupled with vision and distance, to optimize the pose information of keyframes and map points.

[0171] The front-end and back-end communication module reads local data from the front-end, organizes pose frames and keyframes into a specific information format, and publishes it to the back-end map. It records the pose transformation matrix between the front-end map and the central server map, reads the keyframes and map points of pose changes in the central server map, transforms them into poses in the front-end SLAM map, and organizes them into information for publication to the front-end.

[0172] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention.

Claims

1. A method for estimating relative pose between drones based on vision and distance measurement, characterized by: Applicable to drone swarms consisting of at least three drones, with a central server capable of communicating with all drones in the swarm. Each drone in the swarm is equipped with a camera and ranging equipment, and a communication module capable of establishing contact with the central server. The method comprises the following steps: Step 1: Each drone in the cluster receives the image information and ranging information collected by its own sensors, modifies the timestamp of the image information and ranging information to the time when the drone's onboard computing platform system receives the information, and sets an ID for each image frame and distance frame; transmits the ranging information to the central server through the communication module, and transmits the image information to the SLAM module on the front end of the drone; Step 2: After receiving the image information, the SLAM module in the drone executes the monocular vision SLAM algorithm. After completing SLAM initialization, it tracks each frame and calculates its pose. It combines the drone ID, image frame ID and its timestamp, the image frame's reference keyframe ID, and the image frame's pose transformation matrix relative to its reference keyframe into a pose frame, which is then transmitted to the central server via the communication module. Step 3: The SLAM module determines whether the received current frame image is a key frame. If this image frame is a key frame, it inserts this image frame into the SLAM map of the drone's local SLAM module; the communication module sends all key frames and map points observed by the key frames in the drone's local SLAM map to the central server; Step 4: After the central server receives the pose frame and distance frame information transmitted by each drone's communication module, it sets the drone with the smallest ID as the reference drone and sets the SLAM positioning coordinate system of the reference drone as the reference coordinate system. Based on the ranging information of other drones and the reference drone, it establishes a relative pose optimization equation and solves the relative pose optimization equation to obtain the pose transformation matrix of the navigation coordinate system of other drones relative to the reference coordinate system. Step 5: In the central server, based on the mutual ranging information of the UAVs in the cluster, the relative posture optimization equation between the UAVs is constructed. The equation is solved to obtain the posture transformation matrix of other UAVs relative to the reference UAV, and the posture transformation matrix is stored in the communication module; Step 6: After the central server receives the key frame of the front-end UAV SLAM module sent by the communication module, it performs loop detection on the key frame; if there is a loop with the key frame in the server map, it determines whether the key frame and the loop frame belong to the same UAV map. If they belong to the same UAV map, the similarity transformation between the key frame and the loop frame is solved, and loop correction is performed to correct the posture of the map in the loop; if they are not in the same map, a new map is generated, and the map with the smaller ID in the two maps of the loop is completely copied to the new map. The relative posture transformation matrix between the map with the larger ID and the map with the smaller ID is used to convert the map with the larger ID to the new map, and map point matching is performed. The same map points are merged using the back projection and feature matching method; Step 7: After the map is merged in step 6, the distance error between the pose frames of the two drones and the reprojection error between the keyframes and the map points are constructed, and the relative pose optimization equation in the merged map is established. The optimization equation is solved to obtain the relative pose transformation matrix in the merged map and the pose and map point position of each keyframe; based on the relative pose transformation matrix in the map merging process, the pose transformation matrix of the pose frame of the front-end drone SLAM module in the communication module is updated to the reference coordinate system in the server; Step 8: Send the key frame pose and map point position of each pose change in the central server to the SLAM module of the corresponding drone through the communication module according to its corresponding drone ID; Step 9: After receiving the keyframe pose and map point position, the UAV SLAM module updates the keyframe pose and map point position, and uses the updated values in subsequent optimization to apply the observation information of other UAVs to the current UAV to improve the relative positioning accuracy of the UAV cluster.

2. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: The specific process of step 1 is: Step 1.1: Each drone in the cluster establishes a communication relationship with the central server through the communication module. The central server assigns a drone ID to each drone, receives the registration information sent by each drone through the communication module, and establishes a database and server map for each drone in the central server; Step 1.2: Each UAV in the cluster collects image information of the external environment in real time and measures the distance information between other UAVs. After each UAV receives the image captured by its own camera sensor, it sets the onboard computing platform time at the time of reception as the image timestamp, adds the UAV ID and image frame ID to the image, constructs an image frame and sends it to the onboard real-time positioning and mapping module (SLAM module). After each UAV receives the ranging information output by its own ranging device, it sets the onboard computing platform time at the time of reception as the ranging timestamp, adds the ranging timestamp to the distance frame constructed by the distance measurement value, the IDs of the two UAVs involved in the ranging, and the distance frame ID, and sends the distance frame to the central server through the communication module.

3. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: The specific process of step 2 is: Step 2.1: After receiving the image information, the SLAM module performs feature point matching on two consecutive image frames. When the number of matched feature points meets the set threshold, the relative pose between the two frames is calculated based on the matching relationship between the feature points between the two frames, and the position of the map points observed jointly by the two frames is calculated using the triangulation method. The two frames are set as key frames, and the key frames and their jointly observed map point positions are inserted into the SLAM map to complete the SLAM module initialization. Step 2.2: After completing the SLAM module initialization, set the new image frame received by the SLAM module as the current frame, and set the newly generated keyframe as the reference keyframe of the current frame; establish a matching relationship between the current frame and its reference keyframe, use the feature points of the reference keyframe and the feature points of the current frame to establish a reprojection error, establish the pose optimization equation of the current frame based on the reprojection error, optimize the pose of the current frame, and correct the position of the map points in the current frame; Step 2.3: Combine the image frame ID, current frame timestamp, drone ID, reference keyframe ID of the current frame, and the pose transformation matrix of the current frame relative to its reference keyframe to construct a pose frame, insert the pose frame into the SLAM map, and send the pose frame to the central server through the communication module.

4. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: The specific process of step 3 is: Step 3.1: After receiving the current frame image, the SLAM module calculates the difference between the current frame and its reference key frame, and determines whether to set the current frame as a key frame based on the difference; Step 3.2: If the current frame is set as a keyframe, insert the keyframe and its observed map points into the local SLAM map of the drone; construct the keyframe's image frame ID, feature points, feature point descriptors, and word vector information into keyframe information, where the feature point descriptors are numerical vectors representing the image information of the area around the feature points, and the word vectors are word vectors generated using the DBOW2 library; construct the descriptors, map point IDs, and keyframe information of at least two map points observed by the keyframe into map point information, and send the map point information to the central server via the communication module; Step 3.3: After the central server receives the keyframe and map point information, it inserts the word vector information in the keyframe into the server's map database. The central server performs loop detection within the cluster based on the matching relationship between the keyframes in the map database and corrects the position of the keyframe. The corrected keyframe position is sent back to the SLAM module of the corresponding drone. The SLAM module modifies the position of the corresponding keyframe and the position of the map point to achieve loop correction.

5. The method for relative pose estimation between drones based on vision and distance measurement according to claim 4, characterized in that: In step 3.3, the specific process of loop correction is as follows: Step 3.3.1: Use the word vector of the current frame to search for similar frames with the same words as the current frame in the map database. Calculate the bag-of-words vector similarity score between the current key frame and similar frames based on TF-IDF weighting. Calculate the total similarity score of all similar frames to the current frame and the current frame. Select the similar frame with a similarity score greater than 0.75 times the total similarity score as the candidate frame. Step 3.3.2: Check whether the previous frame and the next frame of the candidate frame are both loop frames of the current frame. If so, these three frames form a consistency group. Step 3.3.3: Based on the observation results of the current frame and the loop frame for the same map point, establish the similarity transformation optimization equation: In the formula b p i and a p i is the position of the i-th map point in the key frame b coordinate system and the key frame a coordinate system, s is the scaling factor between the key frame a coordinate system and the key frame b coordinate system, R b,a is the rotation matrix from the key frame b coordinate system to the key frame a coordinate system, t b,a is the translation transformation matrix from the key frame b coordinate system to the key frame a coordinate system; Solving the similarity transformation optimization equation to obtain a similarity transformation matrix, correcting the pose of the current frame according to the similarity transformation matrix, and correcting the poses of all key frames involved in the loop; The correction process is as follows: the similarity transformation matrix obtained is multiplied by the pose of each key frame involved in the loop to obtain the corrected key frame pose, and the similarity transformation matrix is multiplied by the position of the map point observed in each key frame involved in the loop to correct the map point; after the correction is completed, the reprojection error is constructed based on the observation relationship between the key frames involved in the loop and the map points, and the pose optimization equation is established to optimize the key frame pose and map point position; Step 3.3.4: After loop correction is completed, perform global optimization of the server map: After loop correction is completed on the map, construct the reprojection error based on the observation relationship between all key frames and map points in the map, and weight the reprojection error to establish a global optimization equation. Solve the optimization equation to achieve global optimization of the server map.

6. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: Step 4 specifically includes the following processes: Step 4.1: The central server receives the pose frame and distance frame information transmitted by the communication module of each drone, sets the drone with the smallest ID as the reference drone, and sets the SLAM positioning coordinate system of the reference drone as the reference coordinate system; based on the bidirectionality of ranging, establishes the optimization equation of the system time difference between each drone and the reference drone, and optimizes the system time difference Δt between each drone and the reference drone. i : In the formula is the measured distance between the reference UAV and the UAV with ID i at time t, = t+Δt i The measured distance between the drone with ID i and the reference drone at time, where Δt i The difference between the system time of the drone with ID i and the system time of the benchmark drone, where the system time is the time of the drone’s onboard computing platform; Step 4.2: Based on the difference between each UAV system time and the benchmark UAV system time, the central server unifies the received pose frames and distance frames to the benchmark UAV system time; The spherical coordinate interpolation method is used to interpolate the pose frame to obtain the pose of the i-th UAV unified to the benchmark UAV system time. cosθ=q m ·q n p k =p m +f·(p n -p m ) Where t k is the moment to be interpolated, q k With p k They are t k The rotation quaternion and position vector obtained by interpolating the pose frame at the moment, t m , t n t k The previous and next pose frame time, q m ,q n 、p m 、p n t m , t n The rotation quaternion and position vector corresponding to the moment; Step 4.3: Construct the distance error between the pose frames, establish an optimization equation by weighting the bi-norm of the distance error, and solve it to obtain the similarity transformation matrix between the SLAM positioning coordinate system of each UAV and the reference coordinate system; The optimization equation is: The distance error is: Where P i k is the position of the k-th pose frame of UAV i, is the distance measured between UAV i and the reference UAV at the kth frame, T 1,i is the position transformation matrix between the i-th UAV map and the reference map, s 1.i is the scale transformation between the i-th UAV map and the reference map, W k is the information matrix corresponding to the kth distance error.

7. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: Step 5 specifically includes the following processes: Step 5.1: If the number of drones in the cluster is greater than 3 and the distance between drones can be measured, based on step 4, establish the distance error between the pose frames of the drones and construct the relative position optimization equation between the drones: Where T j,1 T 1,j The inverse transformation of the pose transformation matrix represented by ; Step 5.2: For the key frames received subsequently, use the similarity transformation matrix obtained in step 5.1 to transform them into the reference coordinate system, and use the inverse operation of the similarity transformation matrix to process the corrected map to be transmitted back to the drone and send it to the SLAM module of the corresponding drone.

8. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: Step 6 specifically includes the following processes: Step 6.1: After receiving the new keyframe sent by the front-end drone, insert the keyframe into the server map and perform loop detection; Step 6.2: If the loop closure detects that the loop frame and the current key frame belong to the same map corresponding to the front-end drone, loop correction is used to optimize the key frame pose and map point position involved in the loop; after the loop correction is completed, the pose of the relevant pose frame is updated according to the pose transformation relationship between the pose frame and its reference key frame: T j,0 =T j,ref ·T ref,0 Where T j,0 is the pose of the j-th pose frame under its map, T j,ref is the pose transformation of the j-th pose frame relative to its reference key frame, T ref,0 The reference key frame's pose under its front-end map is used to recalculate the relative pose transformation matrix between the current UAV's corresponding map and the reference map; Step 6.3: If the detected loop frame and the current key frame belong to the maps corresponding to different front-end drones, feature matching is performed on the associated frames of the current key frame and the loop frame, and a similarity transformation optimization equation is established based on the ranging relationship between the key frames in the two front-end drone maps: In the formula is the pixel coordinate of the i-th matching map point in the j-th key frame of the l-th UAV, π l is the projection matrix of the lth UAV, is the jth key frame pose of the lth UAV, T l,i is the pose transformation matrix between the i-th UAV map and the l-th UAV map, s l,i is the scale ratio between the i-th UAV and the l-th UAV; the key frame in the i-th UAV map is transformed into the new map using the transformation matrix between the i-th UAV map and the l-th UAV map.

9. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: The specific process of step 7 is: Step 7.1: Use the pose transformation matrix of the reference keyframe of the map point to transform the map point into the merged new map. Match each newly transformed map point with an existing map point in the current map. Project the matched map point into the observation frame of the newly transformed map point and determine the positional relationship between the projected point and the feature point corresponding to the newly transformed map point. If the pixel position of the feature point is close to that of the projected point of the matched map point, the two map points are considered to coincide, and all observation relationships of the newly transformed map point are transferred to the matched map point. Step 7.2: After completing the merging of map points within the map, a new observation relationship is established within the new map, and ranging information exists within the two merged maps; The reprojection error is established based on the observation relationship, and the position distance error is established based on the ranging information of the pose frame in the map. The two errors are weighted to establish the relative pose optimization equation. The optimization equation is solved to obtain the relative pose transformation matrix in the process of merging the maps, and the pose of the key frames and the map point positions in the new map are optimized: In the formula is the pixel coordinate of the i-th matching map point in the k-th key frame, π k is the projection matrix of key frame k, i P k is the position of the i-th map point in the k-th key frame, P i , P j are the positions of the i-th pose frame and the j-th pose frame respectively; Step 7.3: After the map merging is completed, based on the relationship between the new map and the reference map, the pose transformation relationship between the SLAM positioning coordinate system of all front-end drones in the new map and the reference coordinate system in the communication module is updated.

10. The method for relative pose estimation between drones based on vision and distance measurement according to claim 1, characterized in that: The specific process of step 8 is: Step 8.1: After the central server completes the map merging, it modifies the keyframe pose and map point positions of the merged map. The communication module publishes the modified keyframe pose and the modified map point position information to the corresponding front-end drone. After receiving the modified keyframe pose, the front-end drone modifies the position of the observable map point based on the modified keyframe: T i =T i,1 ·T 1,j ·T j,k p j =T i T ·T i ′·p j ′ Where T j,k is the pose of the kth key frame in the jth map in the central server, T 1,j is the pose transformation between the jth map and the reference map, T i,1 is the pose transformation from the reference map to the front end, T i The pose of the server keyframe passed to the front-end map; j With p j ′ are the positions of the map points before and after modification, T i With T i ′ is the pose of the keyframe before and after modification; Step 8.2: After the front-end drone receives the key frame pose and map point position information that have changed after global optimization from the server, it modifies the key frame pose and map point position according to the ID information. After the modification is completed, global optimization is performed: In the formula is the pixel coordinate of the kth map point in the i-th key frame, π is the projection matrix of the i-th key frame, T j is the pose of the jth key frame, P k is the position of the kth map point in the front-end map.

Images