Multi-unmanned aerial vehicle cooperative positioning method, system, device and medium

Abstract

The invention discloses a multi-unmanned aerial vehicle cooperative positioning method, system and device and a medium, and relates to the technical field of unmanned aerial vehicle navigation and group cooperative positioning, and the method comprises the steps: obtaining a visual image and IMU data of a multi-unmanned aerial vehicle; internal measurement and external measurement are constructed in the visual image and IMU data of each unmanned aerial vehicle; constructing a global pose map optimization objective function according to the internal measurement and the external measurement, performing maximum likelihood estimation on the pose of each frame of the unmanned aerial vehicle through the global pose map optimization objective function, and further determining the global pose of each unmanned aerial vehicle and the relative pose between every two unmanned aerial vehicles; error terms in global pose map optimization are constructed through internal measurement and external measurement, calculation burden is allocated to each unmanned aerial vehicle, an alternating direction multiplier method is adopted for solving, the global pose map is optimized and distributed, and the communication data size and calculation complexity are effectively reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) navigation and swarm cooperative positioning, and particularly relates to a multi-UAV cooperative positioning method, system, device and medium. Background Art

[0002] With the rapid development of robot technology, the working environment and tasks of robots are becoming more and more complex and diverse. Many complex tasks cannot be effectively completed by a single robot alone, and human-robot cooperation is required to jointly complete the tasks. Traditional industrial robots can only complete tasks according to pre-set programming instructions, cannot adapt to complex human-robot interaction environments, and lack dexterity, safety and compliance. Collaborative robots can combine the advantages of high precision of robots and high dexterity of human partners. Robots are responsible for tasks with high repeatability and high precision requirements, and humans are responsible for tasks that are more complex and have high uncertainty. Compared with traditional robots, they have advantages such as flexible deployment, simple operation, and good interaction performance, and are widely used in fields such as aerospace, industrial production and life services.

[0003] In a system with only a single UAV, it is usually necessary to estimate the pose of this UAV relative to the coordinate system of the operating environment or relative to the coordinate system of its starting position. Currently available solutions include GPS, ultra-wideband positioning, vision-based positioning, IMU (Inertial Measurement Unit)-based positioning, and the fusion between these sensors. Among them, Simultaneous Localization and Mapping (SLAM) has received great attention. It can achieve self-localization only relying on the sensors carried by itself without relying on external positioning systems or sensors. In the field of SLAM, scholars have proposed vision-based SLAM (ORB-SLAM) and visual inertial navigation system VINS; among them, VINS can estimate the pose of each frame of the UAV motion trajectory relative to the starting frame through the measurements of the camera and IMU.

[0004] However, in the currently proposed cooperative SLAM methods, it is necessary to rely on a central node outside the UAV cluster to complete communication relay, command and coordination, and computational burden. This centralized structure is difficult to implement in the case of limited remote and communication bandwidth, which limits the scalability of multi-UAV systems. Summary of the Invention

[0005] Aiming at the deficiency of the prior art that it is necessary to rely on a central node outside the cluster to complete communication relay, command and coordination, and computational burden, the present invention proposes a multi-UAV cooperative positioning method, system, device and medium, which uses the RGB camera and IMU data carried by the UAVs to output the global pose and relative pose of each UAV relative to the initial UAV, thus solving the problems existing in the prior art.

[0006] A multi-UAV collaborative positioning method, comprising the following steps:

[0007] Obtain the visual images and IMU data of multiple UAVs;

[0008] Construct internal measurements and external measurements based on the visual images and IMU data of each UAV; the internal measurement is to obtain the trajectory of the UAV by using a visual inertial odometer, and according to the pose node difference between two adjacent moments in the UAV trajectory, obtain the global pose of the same UAV at different moments; the external measurement is to convert the latest frame of the current UAV into a word vector, use the distributed bag-of-words method to search for the frame most similar to the word vector in the historical word vectors of the cluster, request the position of the ORB feature points of the frame from the owner of the frame, and after matching the requested ORB feature points with the feature points of the UAV itself, obtain the relative pose between the poses of two different UAVs;

[0009] Construct an optimization objective function for the global pose graph based on the internal measurement and the external measurement, and perform maximum likelihood estimation on it to determine the global pose of each UAV in the global coordinate system and the relative pose between each pair.

[0010] Further, the set of the multiple UAVs is defined as Ω={α,β,γ,...}, and the pose of UAV α relative to the global coordinate system at time i is expressed as:

[0011]

[0012] where, represents the attitude of the UAV; SO(3) represents the three-dimensional special orthogonal group, which is defined as represents the displacement of the UAV, The definition of the SE(3) group is

[0013] Further, the step of obtaining the global pose of the same UAV at different moments by using a visual inertial odometer to obtain the trajectory of the UAV and according to the pose node difference between two adjacent moments in the UAV trajectory includes the following steps:

[0014] Use a visual inertial odometer to obtain the trajectory of UAV α From the trajectory Obtain internal measurements:

[0015]

[0016] where denote as the (k + 1)-th image frame b respectivelyk+1 The position, velocity and attitude of the pose relative to the initialization coordinate system; among them, is a quaternion representation, Represents quaternion multiplication;

[0017] remember is the rotation matrix of the pose at time t relative to the initialization coordinate system, t k+1 For frame b k+1 The corresponding moment, Δt is t k to k+1 The time interval of IMU pre-integration is expressed as:

[0018]

[0019] in, Pre-integration for IMU; Indicates t k+1 The rotation matrix of the sensor coordinate system b relative to the initialization coordinate system w at the moment; Indicates that at t k+1 At time , the position vector of the sensor coordinate system b relative to the initialization coordinate system w; represents the velocity vector of b relative to w, g w is the gravitational acceleration vector; The quaternion representing the rotation of the sensor coordinate system b relative to the initialization coordinate system w; It means that the IMU sensor measures the angular velocity and obtains the value from t by integration. k to k+1 The posture increment;

[0020] To measure the IMU and The variance is passed to and The IMU measurement is used as the input of the linear time-varying system, and the state of the state is derived by using the error state Kalman filter transfer process. The dynamic equation is:

[0021]

[0022] Then we get:

[0023]

[0024] Among them, n ω represents the gyroscope measurement noise, which is used to describe the random error in angular velocity measurement; represents the accelerometer bias noise, reflecting the random drift of the accelerometer bias over time; represents the gyroscope bias noise, represents the random walk of the gyroscope bias; Indicates the position error state; Indicates the velocity error state; Indicates the attitude error state; δb a Indicates the accelerometer bias error; δb ω Indicates the gyroscope bias error; rotation matrix Indicates the rotation from the world coordinate system to the k-th b frame, is the position of the b-th frame relative to the world coordinate system, is the velocity of the b-th frame, is the attitude of the b-th frame, g w is the gravity vector in the world coordinate system, and Δt is the time interval;

[0025] Suppose the landmark l is first observed in the image frame i and then observed again in the image frame j. Then, the observation residual of the landmark l in the image frame j is defined as:

[0026]

[0027] Where:

[0028]

[0029] Among them, are the pixel coordinates where the landmark i is first observed in the image frame i, are the pixel coordinates of the same landmark in the image frame j, λ l is the inverse depth of the landmark l in i; π -1 is an inverse projection model that outputs a unit vector in three-dimensional space, equivalent to converting pixel coordinates to coordinates on a normalized sphere; Since the degree of freedom of the visual residual is 2, therefore, project the unit residual vector output by π -1 onto the tangent space of the vector b 1 and b 2 are randomly selected orthogonal bases in the tangent space; Indicates the three-dimensional position of the landmark l in the image frame c j ; is the rotation matrix from the camera coordinate system to the body coordinate system, Indicates the rotation matrix from the world coordinate system to the body coordinate system of the UAV j, is the rotation matrix from the body coordinate system of the UAV i to the world coordinate system, is the rotation matrix from the body coordinate system to the camera coordinate system; Indicates the position of the camera coordinate system in the body coordinate system, and respectively represent the positions of the body coordinate systems of UAV i and UAV j in the world coordinate system;

[0030] Then x VIO is included in the solution χ of the optimization problem ＊ According to the obtained obtain internal measurements It is expressed as:

[0031]

[0032] where is the set of all IMU measurement values, and C is the set of map points observed more than twice.

[0033] Furthermore, the global pose graph optimization objective function is constructed based on internal measurements and external measurements to determine the global pose of each UAV in the global coordinate system and the optimal estimate of the relative pose between each pair, specifically including the following steps:

[0034] Denote the set of all internal measurements of UAV α as Denote the internal set of all UAVs as Denote the set of all external measurements related to UAV α as Denote the set of all external measurements as ε S , denote all measurements in the UAV cluster as ε = ε I ∪ε S , and each UAV only stores the measurement information related to itself

[0035] The vector The maximum likelihood estimate value of x is expressed as:

[0036]

[0037] Assume that each measurement is independent, and use Yaw-Pitch-Roll Euler angles for rotation parameterization:

[0038]

[0039] where φ represents the roll angle, θ represents the pitch angle, and ψ represents the yaw angle;

[0040] Use to represent the pose Then the pose graph optimization is simplified to:

[0041]

[0042] where ρ t and ρ ψare the weight coefficients of the position error and the attitude error respectively, which are used to balance the influence of different types of errors in the optimization; and respectively represent the translation vectors of UAV α at key frame i and UAV β at key frame j; and respectively represent the rotation matrix calculated based on the heading angle pitch angle and roll angle ; represents the relative displacement of UAV β at key frame j with respect to UAV α at key frame i; and respectively represent the heading angles of UAV β at key frame j and UAV α at key frame i, is the relative heading angle of UAV β with respect to UAV α; represents the set of all UAVs in the UAV set Ω, represents the inertial measurement unit data of UAV α at key frame i;

[0043] By minimizing the weighted sum of the position and attitude errors, the optimal estimation of the poses of each UAV in the global coordinate system and their relative poses is achieved.

[0044] Furthermore, the EPNP algorithm is used to obtain the relative pose between the two poses of different UAVs, which specifically includes the following steps:

[0045] UAV α encodes the picture observed at time i into a word vector

[0046] The word vector is divided into n = |Ω| parts; each part corresponds to a UAV;

[0047] The word vector as well as the number α of the word vector in the cluster and the local time i corresponding to the word vector are sent to UAV η;

[0048] UAV η is responsible for storing the database where is all the historical word vectors of the UAV cluster; after UAV η receives , it searches for the word vector component most similar to the word vector component in the word vector component database, and assumes that the owner corresponding to the most similar word vector is γ, and its corresponding time is j;

[0049] UAV η sends the information (γ, j) back to UAV α;

[0050] UAV α counts the information (γ, j) sent back by all other UAVs and selects the information with the highest number of votes; among them, assuming that (ξ, k) has the most votes, then the frame (ξ, k) is the similar frame;

[0051] UAV α requests the feature point information of the frame (ξ, k) from UAV ξ for feature matching and PNP, and then solves the relative pose between the two poses of different UAVs.

[0052] The present invention also includes a multi-UAV collaborative positioning system, including:

[0053] An acquisition module for acquiring visual images and IMU data of multiple UAVs;

[0054] A construction module for constructing internal measurements and external measurements according to the visual images and IMU data of each UAV; the internal measurement is to obtain the trajectory of the UAV by using visual inertial odometry, and according to the pose node difference between two adjacent moments in the UAV trajectory, obtain the global pose of the same UAV at different times; the external measurement is to convert the latest frame of the current UAV into a word vector, use the distributed bag-of-words method to search for the most similar frame to the word vector in the historical word vectors of the cluster, request the position of the ORB feature points of the frame from the owner of the frame, and match the requested ORB feature points with the feature points of the UAV itself to obtain the relative pose between the two poses of different UAVs;

[0055] A positioning estimation module for constructing a global pose graph optimization objective function based on internal measurements and external measurements and performing maximum likelihood estimation on it to determine the global pose of each UAV in the global coordinate system and the relative pose between each pair.

[0056] The present invention also proposes a multi-UAV collaborative positioning computer device, including: a memory, a processor, and a computer program stored in the memory, and the processor implements the steps of the multi-UAV collaborative positioning method when executing the computer program.

[0057] The present invention also proposes a readable storage medium, the readable storage medium stores a computer program, the computer program includes program instructions, and when the program instructions are executed by a processor, they are used to execute the steps of the multi-UAV collaborative positioning method.

[0058] The present invention provides a multi-UAV collaborative positioning method, system, device and medium, having the following

[0059] Beneficial effects:

[0060] The present invention processes the visual images and IMU data carried by each drone in a distributed manner, without relying on a central node outside the cluster. By constructing internal measurements and external measurements based on the visual images and IMU data of each drone, each drone only needs to communicate information with several adjacent drones, thereby obtaining local information and completing the positioning task, greatly improving the robustness and scalability of the system. At the same time, by constructing a global pose graph optimization objective function and performing maximum likelihood estimation, the likelihood of obtaining this series of internal measurements and external measurements is maximized by adjusting the pose of each frame of each drone. Each drone divides the error terms of the optimization variables and the optimization objective function into several categories from its own perspective and undertakes the optimization of its own part, effectively reducing the communication data volume and computational complexity, and improving the robustness and reliability of the multi-drone system. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 FIG. is a schematic diagram of the co-visibility relationship discovered by the bag-of-words method and DeepLCD in the embodiment of the present invention;

[0062] Figure 2 FIG. is the global pose graph from the perspective of drone α in the embodiment of the present invention;

[0063] Figure 3 FIG. is a schematic diagram of each module in the embodiment of the present invention;

[0064] Figure 4 FIG. is a schematic diagram of the nodes on ROS and their information exchange in the embodiment of the present invention;

[0065] Figure 5 FIG. is a display diagram of the hardware platform in the embodiment of the present invention;

[0066] Figure 6 FIG. is a drone trajectory diagram when using the CoVINS dataset in the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0067] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments.

[0068] The present invention proposes a multi-drone cooperative positioning method, which uses the RGB camera and IMU data carried by the drone to output the absolute pose and relative pose of each drone relative to the initial drone (numbered 0). By defining the initial pose of drone 0 as the world coordinate system, the position and direction of any drone can be calculated, and any coordinate system can be selected as the world coordinate system, thereby re-determining the absolute pose of the drone.

[0069] As Figure 3As shown in the figure, the present invention includes a part for calculating internal measurements, a part for calculating external measurements, and a part for performing distributed pose graph optimization; the part for calculating internal measurements includes feature point extraction and tracking (i.e., using visual measurement data: reprojection error), IMU pre-integration, and a tightly-coupled non-linear optimization part for estimating the trajectory of the UAV itself. This part mainly calls the code in the VINS-Mono framework for implementation; after calculating the trajectory of the UAV itself, the difference between adjacent pose nodes in the trajectory is calculated to obtain internal measurements; the part for calculating external measurements includes a similar frame detection module using the distributed bag-of-words method, feature point matching, and PNP part, as Figure 1 As shown in Figure 1 , the figure shows a schematic diagram of the co-visibility relationship discovered by the bag-of-words method and DeepLCD. (a) is a schematic diagram of the co-visibility relationship using the bag-of-words method, (b) is a schematic diagram of the co-visibility relationship discovered by DeepLCD with a threshold of 0.85; (c) is a schematic diagram of the co-visibility relationship discovered by DeepLCD with a threshold of 0.90; (d) is a schematic diagram of the co-visibility relationship discovered by DeepLCD with a threshold of 0.80; the centralized pose graph optimization part in the figure uses ADMM to solve the global pose graph optimization.

[0070] Regarding internal and external measurements, considering a multi-UAV system, the set of UAVs is represented as Ω = {α, β, γ,...} The pose of UAV α relative to the global coordinate system at time i is represented as:

[0071]

[0072] where represents the attitude of the UAV, SO(3) represents the three-dimensional special orthogonal group, which is defined as:

[0073]

[0074] represents the displacement of the UAV, The definition of the SE(3) group is as follows:

[0075]

[0076] The problem of obtaining the global pose of each UAV and the relative pose between pairs can be reduced to obtaining a set containing a finite number of poses: where T α is the set of all key frame corresponding times of UAV α.

[0077] An example of internal measurement is the measurement of the pose change between two adjacent times provided by the odometer. This measurement can be obtained between and A constraint is formed among them. External measurements are measurements between different UAVs. For example, during the flight of a UAV cluster, UAV α (the on-board computer time of α is i) observes the scene witnessed by UAV β at time j. Thus, the present invention can measure the relative pose of UAV β at time j with respect to UAV α at time i.

[0078] Denote the set of all internal measurements of UAV α as Denote the internal set of all UAVs as Denote the set of all external measurements related to UAV α as Denote the set of all external measurements as ε S 。

[0079] In order to obtain Use visual inertial odometry to obtain the trajectory of UAV α Use the following formula to obtain internal measurements from the trajectory Obtain internal measurements:

[0080]

[0081] Where In order to introduce internal measurements with different spans, in the above formula, find

[0082] each pose corresponds to a frame of image. The frame rate of the image is about 30Hz, while the frequency of the IMU is usually as high as over 100Hz. A large number of IMU measurements occur between two frames of image data. The purpose of IMU pre-integration is to integrate multiple IMU measurements between two image frames into one measurement and use this measurement to constrain the state to be estimated.

[0083] Denote as the position, velocity, and attitude of the pose of the (k + 1)-th image frame b k+1 relative to the initial coordinate system respectively. is represented by a quaternion, represents quaternion multiplication. Denote as the rotation matrix of the pose at time t relative to the initial coordinate system. t k+1 is the time corresponding to frame b k+1 Δt is the time interval from t k to t k+1 Finally, the following pre-integration form can be obtained, where is called IMU pre-integration.

[0084]

[0085] The method of using Euler's method for calculation and is as follows:

[0086]

[0087] where δt is the time interval between two frames of IMU data, and R(·) represents the conversion from quaternion to rotation matrix.

[0088] To transfer the variance of IMU measurements and to and regard the measurement model as a linear time-varying system, regard the IMU measurement as the input of this linear time-varying system, and utilize the propagation process of the Error State Kalman Filter (ESKF). During the error Kalman filtering process, the true state is equal to the nominal state plus the error state, and the estimation target of ESKF is the error state rather than the entire true state. If the nominal state is numerically equal to the actual measurement value, the uncertainty of the true state can be included in the error state.

[0089] From the true state of ESKF:

[0090]

[0091] The nominal state of ESKF:

[0092]

[0093] Therefore, the dynamic equation of the state can be derived as follows: The dynamic equation is as follows:

[0094]

[0095] It can be obtained that:

[0096]

[0097] Using visual measurement data, the reprojection error can be calculated. Suppose the landmark l is first observed in the image frame i and is observed again in the image frame j. Its observation residual in the image frame j is defined as:

[0098]

[0099] where

[0100]

[0101] is the pixel coordinate at which the road punctuation i is first observed in the image frame i, is the pixel coordinate of the same road punctuation in the image frame j, λ l is the inverse depth of the road punctuation l in i. π -1 is an inverse projection model that outputs a unit vector in three-dimensional space, which is equivalent to converting the pixel coordinate into the coordinate on the normalized spherical surface. Since the degree of freedom of the visual residual is 2, so π -1 projects the output unit residual vector onto the vector 's tangent space. b 1 , b 2 is an arbitrarily selected orthogonal basis in the tangent space.

[0102] x VIO is included in the solution χ ＊ of the optimization problem. Among them is the set of all IMU measurements (referring to pre-integrated values), and C is the set of map points observed more than twice.

[0103]

[0104] After obtaining , a series of internal measurements can be obtained through the above formula, that is

[0105] And the external measurement, that is the relative pose between two poses from different UAVs. The external measurement is divided into three parts: co-visible frame search, co-visible frame confirmation, and relative pose estimation.

[0106] The specific steps for the invention to obtain external measurement are as follows:

[0107] The UAV converts the current latest frame into a word vector, uses the distributed bag-of-words method to search for the most similar frame in the historical word vectors of the cluster, requests the position (3d) and its descriptor of the ORB feature points of this frame from the owner of this frame, uses these feature points to match with its own feature points (2d), and uses the EPNP algorithm to obtain the relative pose between these two frames; the specific steps are:

[0108] (1) The UAV α encodes the picture it observes at time i into a word vector

[0109] (2) Divide the word vector into n = |Ω| parts, and each part corresponds to a UAV.

[0110] (3) Send its own number α in the cluster and the local time i corresponding to this word vector to the UAV η for the word vector .

[0111] (4) The drone η is responsible for storing the database where are all the historical word vectors of the drone swarm. The drone η receives Search for the word vector component most similar to this word vector component in this word vector component database. Let the owner corresponding to the most similar vector be γ, and the corresponding time be j.

[0112] (5) The drone η sends the information (γ, j) back to the drone α.

[0113] The drone α statistically analyzes all the (owner, time) information sent back by other drones and selects the (owner, time) information with the highest number of votes. Let (ξ, k) have the most votes, then the frame (ξ, k) is the similar frame to be found. Subsequently, the drone α will request the feature point information of this frame from the drone ξ for subsequent feature matching and PNP.

[0114] After obtaining the internal measurements and external measurements, the present invention uses them to construct a global pose graph optimization objective function. The significance of this global pose graph optimization problem is maximum likelihood estimation, and by adjusting the pose of each frame of each drone, the possibility of obtaining this series of internal measurements and external measurements is maximized. The global pose graph optimization has a high-dimensional optimization variable and a large amount of optimization calculation. In order to distribute the calculation amount to each drone, the present invention uses the alternating direction multiplier method to divide the original problem into several sub-problems. In order to make the global pose graph optimization meet the segmentation conditions, the present invention adds redundant optimization variables, constructs a separable and physically meaningful global pose graph optimization objective function. Each drone divides the optimization variables and the error terms of the optimization objective function into several categories from its own perspective and receives its own part for optimization. In order to solve the incorrect external measurements introduced by the front end during the optimization stage, the present invention introduces the Huber robust kernel function into the objective function.

[0115] The convergence speed of the alternating direction multiplier method is relatively slow. In order to accelerate the convergence speed of the distributed global pose graph optimization, the present invention compares the influence of different optimization processes and optimization parameters on the convergence speed and proposes a coarse-to-fine optimization method, which can greatly accelerate the convergence of the optimization.

[0116] Denote the set of all internal measurements of the drone α as Denote the internal set of all drones as Denote the set of all external measurements related to the drone α as Denote the set of all external measurements as ε S , denote all the measurements in the drone swarm as ε = ε I ∪ε S , and each drone only stores the measurement information related to itself

[0117] Write the measurement result into the vector x. The maximum likelihood estimate is the value of x that maximizes the likelihood of obtaining the current measurement:

[0118]

[0119] Assume that each measurement is independent. Under these assumptions, it can be shown that x is the solution to the following optimization problem.

[0120]

[0121] When a drone relies solely on its own visual-inertial odometer, cumulative errors will occur. The purpose of pose graph optimization is to obtain a consistent pose that suppresses cumulative errors. Using the rotation matrix and displacement to parameterize each pose, the rotation can be parameterized using Yaw-Pitch-Roll Euler angles instead:

[0122]

[0123] where φ represents the roll angle, θ represents the pitch angle, and ψ represents the yaw angle.

[0124] The attitude of a drone generally does not encounter singularities of Euler angles. Using Euler angles instead of the rotation matrix reduces the number of parameters for each pose from 12 to 6. However, 6D parameterization is also unnecessary when there is an IMU because gravity makes R(θ a i) and R(φ a i) do not produce cumulative errors. Therefore, in pose graph optimization, only can be used to represent the pose The pose graph optimization is simplified to:

[0125]

[0126] The present invention also uses ADMM to solve the global pose graph optimization. The idea of the present invention is to first define the optimization objective function and its iterative method for each drone, and then prove that when using ADMM as the optimization algorithm, the process of each drone optimizing its own optimization objective function is equivalent to the optimization of a well-defined global pose graph optimization objective function with clear physical meaning.

[0127] The classical ADMM algorithm is applicable to solving the following optimization problem:

[0128] minimize f(x)+g(z)

[0129] subject to Ax + Bz = c

[0130] ADMM defines the objective function by using the penalty and augmented Lagrangian method.

[0131]

[0132] Each iteration of using ADMM to solve the optimization problem in Equation (3-7) is as follows:

[0133]

[0134] y k+1 := y k + ρ(Ax k+1 + Bz k+1 - c)

[0135] Denote the optimization variable of the pose graph optimization formula as s. Let each UAV view this optimization variable from its own perspective. Denote the optimization variable in the view of UAV α as s α . Each UAV divides the optimization variable in its own view into the following four parts:

[0136] egoNAdjs α : The poses of UAV α that are not connected to the poses of other UAVs through loop edges.

[0137] egoAdjs α : The poses of UAV α that are connected to the poses of other UAVs through loop edges.

[0138] Adjs α : The poses of other UAVs connected to UAV α.

[0139] ress α : The poses of other UAVs not connected to UAV α.

[0140] After dividing the state into the above four parts, we can get:

[0141]

[0142] Among them, ego f α is the set of constraints formed by all internal measurements related to UAV α, Adj f α is the set of constraints formed by all external measurements related to UAV α, res f α is the other set except the above two parts, including internal measurements occurring in other UAVs and external measurements between some two of them, as Figure 2 shown.

[0143] To make the local pose graph optimization objective function optimized for each UAV easy to be solved by ADMM and achieve full distribution of the algorithm (i.e., each UAV only needs local information during operation), define the local pose graph optimization objective function that does not contain res f α of :

[0144]

[0145] Take the optimization of this objective function as the sub-problem for each UAV to solve. Denote the sum of these sub-problems as:

[0146]

[0147] Denote the global pose graph optimization objective function as:

[0148]

[0149] The following relationship exists between them:

[0150]

[0151] That is, the global pose graph optimization objective function obtained by summing the given local pose graph optimization objective function (the sub-problem for each UAV to solve) is equivalent to adding the error terms from all loop edges to the global pose graph optimization objective function of. The newly defined global pose graph optimization objective function calculates each loop edge twice and each edge from internal measurement once, which is equivalent to giving loop edges twice the weight and has a clear and reasonable physical meaning. More importantly, the newly defined objective function satisfies the separable condition and can be solved using ADMM.

[0152] Denote the set of a certain pose as all UAVs that contain γ in their own optimization variable s . Denote the consensus of the UAV cluster on the pose as The present invention hopes that in the optimization, the estimated value of each UAV for a pose is close to its consensus.

[0153] The transformed global pose graph optimization can be expressed in the following form:

[0154]

[0155] Write all the consensuses in the cluster into the vector Then the constraints of the above optimization problem can be denoted as where matrices A and B are matrices composed of 0, 1, and -1. Denote Then the optimization problem can be written in the same form as ADMM:

[0156]

[0157] The iterative method for solving the optimization problem using ADMM is as follows:

[0158]

[0159] For each pose in there is:

[0160]

[0161] For the augmented state the dual variables corresponding to each pose in are:

[0162]

[0163] Substitute into it to get:

[0164]

[0165] Note that the above formula can be obtained distributively and in parallel, and each term in the summation can be assigned to a drone. For example, drone α is responsible for solving the unconstrained optimization problem:

[0166]

[0167] In addition, drone α is also responsible for collecting poses in the cluster and then calculating the consensus of the cluster on it:

[0168]

[0169] The pose constraints across UAVs in global pose graph optimization are similar to loop closure edges in single - robot SLAM. However, it is easier to get incorrect loop closure edges in multi - robot SLAM because in single - robot SLAM, after passing the loop closure detection (place recognition), the node is not directly adopted as a loop closure. Instead, a further geometric check is carried out using the prior of the odometer. For multi - robot SLAM, there is no prior on the relative pose between two UAVs before the loop closure edge connects the pose graphs of the two UAVs, so geometric checks cannot be performed. In addition, if there are two locations with extremely similar appearances in the operating environment, the SLAM front - end will consider these two locations as the same location and then add an incorrect loop closure edge. This problem cannot be solved by simply improving the accuracy of the loop closure detection in the front - end because some environments contain such repetitive structures. For the above two reasons, it is very necessary to process incorrect loop closures at the back - end (after the global pose graph is constructed) to reduce the impact of incorrect loop closures on the optimization of the global pose graph.

[0170] The intuitive idea of the present invention turning to the classic Huber robust kernel function is to add the Huber robust kernel function to the error term corresponding to the loop closure edge in the formula. However, it is found that the optimization is difficult to converge after using ADMM. The present invention adds the Huber robust kernel function to the consensus term in the formula:

[0171]

[0172] The optimization can converge smoothly, the incorrect loop closures are significantly suppressed, and it is not sensitive to the initial values. Using the Huber robust kernel function also enables the present invention to improve the problem of incorrect loop closures under the original continuous optimization framework, avoiding additional complexity.

[0173] The ADMM step after adding the Huber robust kernel function is iteratively updated as follows: Each UAV optimizes its own sub - problem. For UAV α:

[0174]

[0175] After the sub - problem optimization is completed, the owner of the pose is responsible for calculating the consensus:

[0176]

[0177] The user of the pose calculates the dual variable corresponding to this pose:

[0178]

[0179] To verify the running effect of the multi - UAV cooperative localization algorithm, the present invention verifies this algorithm in data sets and physical experiments. The information transfer between each module and among all modules of the algorithm is as Figure 3As shown. The input of the algorithm module is the IMU data stream and the video stream, and the output is the global pose of the UAV (with the initialization coordinate system of the UAV with the smallest number as the world coordinate system). According to the tasks completed by different algorithm modules, the algorithm can be generally divided into three parts: the part for calculating internal measurements, the part for external measurements, and the part for performing distributed pose graph optimization. The part for calculating internal measurements includes feature point extraction and tracking, IMU pre-integration, and the tightly coupled non-linear optimization part for estimating the UAV's own trajectory. This part mainly calls the code in the VINS-Mono framework for implementation. After calculating the UAV's own trajectory, the difference is taken between adjacent pose nodes in the trajectory to obtain internal measurements. The part for calculating external measurements includes the similar frame detection module using the distributed bag-of-words method and the feature point matching and PNP part. The centralized pose graph optimization part in the figure uses the Gauss-Newton method to optimize the same objective function and uses its result as a comparison for the distributed optimization method.

[0180] During the operation of the multi-UAV cooperative localization algorithm, for each UAV, the construction and addition of loop edges and pose graph optimization are carried out in two different threads in parallel. The consequent problem is how to ensure that each UAV in the cluster optimizes the same pose graph. Assume that an external measurement is generated between UAV α and UAV β According to the process of constructing relative measurements, this external measurement is first generated at α through a series of steps and then through EPNP. After α confirms this external measurement, it is sent to β. If the distributed optimization starts after all the above processes are completed, both the sub-pose graphs optimized by UAV α and β contain If the distributed optimization starts before this external measurement reaches β, then the sub-pose graph of β will not include The result of the distributed optimization will be unpredictable.

[0181] When a loop is generated in the ddbow node, its transfer path is as Figure 4 shown.

[0182] The following approach can ensure that the multi-UAV cooperative optimization is of the same pose graph.

[0183] When a loop is generated in the ddbow node of a certain UAV, use this moment as the timestamp of the loop.

[0184] When the distributed pose graph optimization starts at the admm_node node, only use the loops with timestamps earlier than the current moment by more than t ∈ seconds to construct the sub-pose graph.

[0185] Suppose the upper bound of the time required for the loop to be transmitted between ROS nodes in the UAV is t inter and the upper bound of the time required for the loop to be transmitted between UAVs is tcross The upper bound of the time difference caused by the time asynchronization of each UAV in the cluster is t shift When t inter +t cross +t shift <t∈, it can be ensured that the same pose graph is used for the collaborative optimization of multiple UAVs. In the experiment, t ∈ = 0.2 seconds is taken.

[0186] During the synchronization process of ADMM, each UAV needs to send requests: collect the poses of itself optimized by other UAVs, calculate the consensus of this pose using these poses, and collect the consensus of the poses of other UAVs that it is responsible for optimizing. Correspondingly, the UAV also needs to respond to the above requests initiated by other UAVs. In this process, deadlocks may occur. One situation where a deadlock occurs: UAV α initiates a request for UAV β, while UAV β initiates a request for UAV γ, and at the same time UAV γ initiates a request for UAV α, which causes these three UAVs to all fall into a waiting state, and a deadlock appears. The method to avoid deadlocks is as follows:

[0187] Each UAV initiates a service request to the UAV with a larger serial number than itself, gives the consensus of its own pose to others, and retrieves the consensus of the poses of others.

[0188] This step can be carried out in parallel and will not cause deadlocks. Even if UAV α initiates a request for UAV γ at the same time as UAV β issues a request for UAV γ, UAV γ can respond to these two requests in any order. Even if the communication topology graph of the cluster is a non-complete graph, this method also ensures that no deadlocks will occur. Proof: By contradiction, assume that a deadlock exists. There must be a circular call in a deadlock, but since each UAV only sends requests to UAVs with a larger serial number than itself, the circular call contradicts this premise.

[0189] The pose obtained by the admm_node node through distributed global pose graph optimization has a low frequency (about 0.5Hz). It is obviously not enough to directly use the latest pose of itself in admm_node as its current pose and as the input of the planning and control module. The present invention adopts the following method to obtain a high-frequency positioning result:

[0190] Let the latest pose in the admm_node node of UAV α be The pose corresponding to this pose in VIO is The current pose of VIO is (This pose is the superposition of the latest frame pose in VIO and the current IMU pre-integration value, and the publishing frequency is the same as the IMU frequency). Let the pose of the UAV α currently relative to the starting coordinate system of UAV 0 be Let the back-end optimization only provide a pose transformation It is defined by the following relationship:

[0191]

[0192] It includes the transformation between the starting coordinate system of UAV α and the starting coordinate system of UAV 0, as well as the VIO cumulative error of UAV α. It can be assumed that the cumulative error of the trajectory from the corresponding moment to the current moment has not changed. Then, at the corresponding moment, there is the following relationship:

[0193]

[0194] where The update frequency of is equal to the frequency of distributed pose graph optimization, The update frequency of is equal to the frequency of VIO key frames, The update frequency of is equal to the frequency of the IMU. Therefore, is the high-frequency positioning result required by the present invention.

[0195] The present invention uses the DJI Matrice M100 as the UAV and selects the NVIDIA Xavier NX as the computing platform of the present invention. 24 volts of direct current is led out from the M100, and a voltage conversion module is used to convert it to 19 volts to supply the Xavier NX. The present invention uses the Intel RealSense D435i, and a 3D printed part is designed to extend the camera to the front lower part of the fuselage. The Xavier NX is also installed on the fuselage of the M100 through a 3D printed part. The assembled hardware platform is as shown in Figure 5 : The verification of the algorithm is completed using the CoVINS dataset. The CoVINS dataset contains IMU data and camera images collected by 4 UAVs flying in the same laboratory. The camera images are as shown in Figure 5 : Four instances of the algorithm are run on the Robot Operating System ROS. Using the CoVINS dataset as the input of the algorithm, the trajectory as shown in Figure 6 is obtained. The Absolute Trajectory Error (ATE) is the direct difference between the estimated pose and the true pose, which can very intuitively reflect the algorithm accuracy and the global consistency of the trajectory. The position in the absolute trajectory error.

[0196] Based on the same inventive concept, the present invention also proposes a multi-UAV collaborative positioning system, including:

[0197] An acquisition module, configured to acquire visual images and IMU data of multiple UAVs.

[0198] A construction module is used to construct internal measurements and external measurements based on the visual images and IMU data of each drone. The internal measurement is to obtain the trajectory of the drone by using visual inertial odometry, and based on the pose node difference between two adjacent moments in the drone trajectory, obtain the global poses of the same drone at different moments. The external measurement is to convert the latest frame of the current drone into a word vector, use the distributed bag-of-words method to search for the frame most similar to this word vector in the historical word vectors of the cluster, request the position of the ORB feature points of this frame from the owner of this frame, and after matching the requested ORB feature points with the feature points of the drone itself, obtain the relative pose between the poses of two different drones.

[0199] A positioning estimation module is used to construct a global pose graph optimization objective function based on the internal measurement and external measurement, and perform maximum likelihood estimation on it to determine the global pose of each drone in the global coordinate system and the relative poses between each pair.

[0200] The present invention also proposes a multi-drone collaborative positioning computer device, including: a memory, a processor, and a computer program stored in the memory. When the processor executes the computer program, it performs the steps of the multi-drone collaborative positioning method.

[0201] The present invention also proposes a readable storage medium. The readable storage medium stores a computer program. The computer program includes program instructions. When the program instructions are executed by the processor, they are used to perform the steps of the multi-drone collaborative positioning method.

[0202] As described above, only the preferred specific embodiments of the present invention are provided, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and all should be covered by the protection scope of the present invention.

Claims

1. A multi-UAV collaborative positioning method, characterized in that: The following steps are involved: Acquire visual images and IMU data from multiple drones; Construct internal and external measurements based on the visual image and IMU data of each drone; the internal measurement is to obtain the trajectory of the drone by using a visual inertial odometer, and obtain the global pose of the same drone at different times based on the pose node difference between two adjacent moments in the drone trajectory; the external measurement is to convert the latest frame image of the current drone into a word vector, use the distributed bag-of-words method to search for a frame most similar to the word vector in the historical word vector of the cluster, request the ORB feature point position of the frame from the owner of the frame, match the requested ORB feature point with the feature point of the drone itself, and obtain the relative pose between two poses of different drones; Based on internal and external measurements, a global pose graph optimization objective function is constructed and maximum likelihood estimation is performed to determine the global pose of each UAV in the global coordinate system and the relative poses between them.

2. A multi-UAV collaborative positioning method according to claim 1, characterized in that: The set of the plurality of drones is defined as Ω = {α, β, γ, ...}, and the position of drone α at time i relative to the global coordinate system is expressed as: in, Indicates the attitude of the drone; SO(3) represents the three-dimensional special orthogonal group, which is defined as represents the displacement of the drone, The SE(3) group is defined as 3. The multi-UAV collaborative positioning method according to claim 2, characterized in that: The method of obtaining the trajectory of the drone by using a visual inertial odometer and obtaining the global posture of the same drone at different times according to the posture node difference between two adjacent times in the trajectory of the drone includes the following steps: Use visual inertial odometry to obtain the trajectory of the drone α From the track Get internal measurements: in remember are the k+1th image frame b k+1 The position, velocity and attitude of the pose relative to the initialization coordinate system; among them, is a quaternion representation, Represents quaternion multiplication; remember is the rotation matrix of the pose at time t relative to the initialization coordinate system, t k+1 For frame b k+1 The corresponding moment, Δt is t k to k+1 The time interval of IMU pre-integration is expressed as: in, Pre-integration for IMU; Indicates t k+1 The rotation matrix of the sensor coordinate system b relative to the initialization coordinate system w at the moment; Indicates that at t k+1 At time , the position vector of the sensor coordinate system b relative to the initialization coordinate system w; represents the velocity vector of b relative to w, g w is the gravitational acceleration vector; The quaternion representing the rotation of the sensor coordinate system b relative to the initialization coordinate system w; It means that the IMU sensor measures the angular velocity and obtains the value from t by integration. k to k+1 The posture increment; To measure the IMU and The variance is passed to and The IMU measurement is used as the input of the linear time-varying system, and the state of the state is derived by using the error state Kalman filter transfer process. The dynamic equation is: Then we get: Among them, n ω represents the gyroscope measurement noise, which is used to describe the random error in angular velocity measurement; represents the accelerometer bias noise, reflecting the random drift of the accelerometer bias over time; represents the gyroscope bias noise, represents the random walk of the gyroscope bias; Indicates the position error status; Indicates the speed error status; Indicates the attitude error state; δb a represents the accelerometer bias error; δb ω Represents the gyroscope bias error; rotation matrix Represents the world coordinate system to the kth b Frame rotation, is the position of the b-th frame relative to the world coordinate system, is the speed of frame b, is the pose of frame b, g w is the gravity vector in the world coordinate system, Δt is the time interval; Assume that landmark point l is observed for the first time in image frame i, and landmark point l is observed again in image frame j, then the observation residual of landmark point l in image frame j is defined as: in: in, is the pixel coordinate of the landmark point i observed for the first time in image frame i, is the pixel coordinate of the same landmark point in image frame j, λ l is the inverse depth of landmark point l in i; π -1 It is a back-projection model that outputs a unit vector in a three-dimensional space, which is equivalent to converting pixel coordinates into coordinates on a normalized sphere. Since the degree of freedom of the visual residual is 2, π -1 The output unit residual vector is projected onto the vector The tangent space of , b1, b2 are orthogonal bases randomly selected in the tangent space; Indicates that in image frame c j The three-dimensional position of the middle road mark l; is the rotation matrix from the camera coordinate system to the body coordinate system, represents the rotation matrix from the world coordinate system to the body coordinate system of drone j, is the rotation matrix from the body coordinate system of drone i to the world coordinate system, is the rotation matrix from the body coordinate system to the camera coordinate system; Indicates the position of the camera coordinate system in the body coordinate system. and Respectively represent the positions of the body coordinate systems of UAV i and UAV j in the world coordinate system; Then x VIO The solution to the optimization problem is contained in ★ According to the obtained Get internal measurements It is expressed as: in is the set of all IMU measurements, and C is the set of map points that are observed more than twice.

4. The multi-UAV collaborative positioning method according to claim 2, characterized in that: The global pose graph optimization objective function is constructed based on internal and external measurements to determine the global pose of each UAV in the global coordinate system and the optimal estimate of the relative pose between each UAV, specifically including the following steps: The set of all internal measurements of drone α is The internal set of all drones is The set of all external measurements related to UAV α is Let the set of all external measurements be ε S , let all measurements in the drone cluster be ε = ε I ∪ε S , each drone only saves the measurement information related to itself vector The maximum likelihood estimate of x is expressed as: Assuming that each measurement is independent, the Yaw-Pitch-Roll Euler angle is used for rotation parameterization: Among them, φ represents the roll angle, θ represents the pitch angle, and ψ represents the heading angle; use Express posture Then the pose graph optimization is simplified to: Among them, ρ t and ρ ψ are the weight coefficients of position error and attitude error, respectively, which are used to balance the impact of different types of errors in optimization; and They represent the translation vectors of UAV α at key frame i and UAV β at key frame j respectively; and Respectively represent the heading angle Pitch Angle and roll angle The calculated rotation matrix; represents the relative displacement of UAV β at key frame j relative to UAV α at key frame i; and They represent the heading angles of UAV β at key frame j and UAV α at key frame i, respectively. is the relative heading angle of UAV β relative to UAV α; represents the set of all drones in the drone set Ω, Represents the inertial measurement unit data of drone α at key frame i; By minimizing the weighted sum of position and attitude errors, the optimal estimation of the position and attitude of each UAV in the global coordinate system and their relative positions is achieved.

5. The multi-UAV collaborative positioning method according to claim 4, characterized in that: The EPNP algorithm is used to obtain the relative posture between two postures of different UAVs, which specifically includes the following steps: Drone α encodes the image it observes at time i into a word vector The word vector Divide into n = |Ω| parts; each part corresponds to a drone; The word vector The number α of the word vector in the cluster and the local time i corresponding to the word vector are sent to the drone η; Drone η is responsible for storing the database in is the historical word vector of all drone clusters; drone η receives Then search the word vector component most similar to the word vector component in the word vector component database, and let the owner of the most similar word vector be γ, and its corresponding time be j; UAV η sends the information (γ, j) back to UAV α; Drone α counts the information (γ, j) sent back by all other drones and selects the information with the highest votes. If (ξ, k) has the most votes, then frame (ξ, k) is a similar frame. UAV α requests the feature point information of the frame (ξ, k) from UAV ξ for feature matching and PNP, and then solves the relative pose between two poses of different UAVs.

6. A multi-UAV collaborative positioning system, characterized in that: include: Acquisition module, used to acquire visual images and IMU data of multiple drones; A construction module is used to construct internal and external measurements based on the visual image and IMU data of each drone; the internal measurement is to obtain the trajectory of the drone by using a visual inertial odometer, and obtain the global pose of the same drone at different times based on the pose node difference between two adjacent moments in the drone trajectory; the external measurement is to convert the latest frame image of the current drone into a word vector, use the distributed bag of words method to search for a frame most similar to the word vector in the historical word vector of the cluster, request the ORB feature point position of the frame from the owner of the frame, match the requested ORB feature point with the feature point of the drone itself, and obtain the relative pose between two poses of different drones; The positioning estimation module is used to construct the global pose graph optimization objective function based on internal and external measurements, and perform maximum likelihood estimation on it to determine the global pose of each drone in the global coordinate system and the relative pose between them.

7. A multi-UAV collaborative positioning computer device, characterized in that: include: A memory, a processor, and a computer program stored in the memory, wherein the processor implements the steps of the multi-UAV collaborative positioning method described in any one of claims 1 to 5 when executing the computer program.

8. A readable storage medium, characterized in that: The readable storage medium stores a computer program, which includes program instructions. When the program instructions are executed by a processor, they are used to execute the steps of the multi-UAV collaborative positioning method described in any one of claims 1 to 5.

Images