A visual-based method and device for cooperative positioning of a UAV swarm

Abstract

The application discloses a visual-based unmanned aerial vehicle group cooperative positioning method and device. Key frame information of adjacent unmanned aerial vehicles is acquired, and historical matching key frame information matched with a first key frame of a new suspension point after loop closure occurs is acquired. Corresponding pose information is acquired according to the key frame information, relative observation residuals and loop residuals are generated based on the pose information, and the unmanned aerial vehicle state optimization model is constrained, so that the consistency and high-precision accuracy of the unmanned aerial vehicle group cooperative positioning are improved.

Description

Technical Field

[0001] This application relates to the field of computer vision technology, and in particular to a vision-based method for cooperative localization of UAV swarms and a vision-based device for cooperative localization of UAV swarms. Background Technology

[0002] In related technologies, UAV swarm cooperative positioning methods typically employ individual UAV autonomous positioning, which relies primarily on each UAV's own visual inertial odometry for independent positioning. This process mainly includes steps such as individual UAV data acquisition, individual UAV pose estimation, data communication, centralized or collaborative optimization, and positioning output. However, in scenarios such as mines, forests, and canyons where there is no Global Navigation Satellite System (GNSS) or weak GNSS signals, the individual autonomous positioning method suffers from poor consistency and inaccurate high-precision cooperative positioning due to the tendency of the visual inertial odometry in the UAVs to generate and accumulate drift. Summary of the Invention

[0003] To address the above issues, this application proposes a vision-based UAV swarm cooperative localization method and apparatus. This method acquires keyframe information from neighboring UAVs, as well as historical keyframe information matching the first keyframe of the new hovering point after a loop closure. Based on these keyframe information, corresponding pose information is obtained, and a relative observation residual is generated. and cyclic residual These are used to constrain the UAV state optimization model in order to improve the consistency and high-precision accuracy of UAV swarm cooperative positioning.

[0004] Firstly, this application proposes a vision-based cooperative localization method for UAV swarms, characterized in that the UAV swarm cooperative localization method includes: acquiring the location of any UAV in the UAV swarm. Multiple keyframes, based on the drone The first relative pose observation is determined by the key points in two adjacent keyframes. , For drones Relative rotation of the observation matrix, For drones Relative translation observation vector; obtaining the time interval between two adjacent keyframes. The drone mentioned inside Inertial measurement data set According to the measured angular velocity value Acceleration measurement value Determine the relative rotation pre-integral Relative velocity pre-integral and relative displacement pre-integral Acquisition and drones Related data packets, the data packets including the drone exist The drone was observed at all times. Second relative pose observations The drone For the drone Adjacent drones; under the condition of satisfying the loopback condition, the drones A loop occurs, and the drone is acquired. The third relative pose observation between the first keyframe of the new levitation point and the historical matching keyframes. The first relative pose observation value The relative rotation pre-integration The relative velocity pre-integration The relative displacement pre-integration The second relative pose observation value and the third relative pose observation value Input into the UAV state optimization model to obtain the UAV Status information The status information Including translation Rotation ,speed accelerometer zero bias gyroscope zero bias The UAV state optimization model is based on the UAV's... Visual residual IMU residuals Relative observation residuals and cyclic residual The sum of squares of the Mahalanobis norm is determined, and the visual residual is... The IMU residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent keyframes. The relative observation residual is used to constrain the difference between the IMU pre-integrated measurements and the relative motion predicted by the corresponding pose and velocity estimates. The loop closure residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent UAVs. This is used to constrain the difference between the relative pose observations and the corresponding relative pose estimates obtained by the UAV after a loop closure and related to the historical matching keyframes.

[0005] Optionally, in some embodiments of this application, the method further includes: when the UAV state optimization model reaches the convergence condition or iteration threshold, the UAV... To other drones in the drone swarm Exchange the status information; and place the drone Visual residual IMU residuals Relative observation residuals and cyclic residual Linearization is performed to obtain local Gaussian-Newton equations: The To and , , and The relevant first approximate Hessian matrix, For the increment of the state variable, Let be the gradient vector; if the first approximate Hessian matrix There is a drone in the local information sub-block. If the information is obtained, then the drone will be acquired. Sending with the drone Related local information sub-blocks To generate common local information sub-blocks; if multiple drones exist Then obtain multiple of the aforementioned drones. The sum of local information sub-blocks, or, obtaining multiple of the aforementioned drones. The weighted average of the local information sub-blocks is used to generate the common local information sub-block, wherein the UAV To be with the drone Unmanned aerial vehicles with constraints; replacing the first approximate Hessian matrix with the common local information sub-block. Local information sub-blocks are used to generate new Gauss-Newton equations. ,in, This is a second approximate Hessian matrix that contains the information of the common local information sub-blocks. The gradient vector containing the common local information sub-block information is used to obtain the UAV again by iterating the new Gauss-Newton equation. Status information.

[0006] Optionally, in some embodiments of this application, the step of acquiring any drone in the drone swarm is... Multiple keyframes, including: determining the drone Current image frame The feature points, and the Jacobian matrix of the visual reprojection error of the feature points with respect to the state variables; the current image frame is determined based on the sum of the L2 norms of each column vector of the Jacobian matrix. The first information scalar; if the relative rate of change between the first information scalar and the information scalar of the previous keyframe is greater than a first threshold, then the current image frame is... The current image frame is marked as a new keyframe, and the first informatics scalar is set as the new informatics scalar to determine the next keyframe. If the relative rate of change between the first informatics scalar and the informatics scalar of the previous keyframe is less than or equal to a first threshold, the current image frame is discarded. .

[0007] Optionally, in some embodiments of this application, the UAV state optimization model is: ; To match the visual residual The inverse of the relevant covariance matrix, To the IMU residual The inverse of the relevant covariance matrix, To the relative observation residual The inverse of the relevant covariance matrix, It is related to the loop residual. The inverse of the relevant covariance matrix; For the drone With all the drones mentioned Related relative observation sets; The drone The The drone was observed in the frame. The The relative pose constraints generated by the frame; For drones The set of all successfully triggered active loops; The drone The k-th frame and the drone The effective closure constraint formed in the m-th frame.

[0008] Optionally, in some embodiments of this application, the visual residual , wherein This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The first pose estimate at time 1, Time Drone The second pose estimate.

[0009] Optionally, in some embodiments of this application, the IMU residual for: ; in, express Drones at the Moment Estimated value of the rotation matrix in the world coordinate system express Drones at the Moment Estimated value of the rotation matrix in the world coordinate system express Drones at the Moment Translation estimate in world coordinate system express Drones at the Moment Translation estimate in world coordinate system express Drones at the Moment Velocity estimates in the world coordinate system express Drones at the Moment Velocity estimates in the world coordinate system This represents the gravity vector in the world coordinate system. Indicates the time interval between two adjacent keyframes , This represents the change in accelerometer bias between two adjacent keyframes. This represents the change in gyroscope zero bias between two adjacent keyframes.

[0010] Optionally, in some embodiments of this application, the relative observation residual for: ; Among them, the This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The second pose estimate at time t. Indicates drone exist The estimated third pose at time t; The cyclic residual for: ; Among them, the This represents mapping elements of a Lie group to its Lie algebra; The drone The fourth pose estimation value of the new suspension point, Indicates drone In the historical matching keyframes The fifth pose estimate at that time.

[0011] Optionally, in some embodiments of this application, the drone In the absence of a loopback, the loopback condition is based on The aforementioned visual residuals The first mean of the Mahalanobis norm, The IMU residuals The second mean of the Mahalanobis norm The relative observation residuals The relationship between the weighted sum of the third average of the Mahalanobis norm and the second threshold; in the drone In the event of a loop closure, the loop closure condition is the first average value, the second average value, the third average value, and... The aforementioned loop residuals The relationship between the weighted sum of the fourth average and the second threshold; the closure condition is that there are multiple consecutive weighted sums that are all greater than the second threshold.

[0012] Optionally, in some embodiments of this application, historical keyframes include any one of the drones in the drone swarm. The UAV swarm cooperative localization method further includes: selecting multiple first candidate keyframes from the historical keyframes whose time interval is greater than or equal to a third threshold, or whose spatial distance is greater than or equal to a fourth threshold; determining the similarity between each first candidate keyframe and the first keyframe, and arranging them from high to low according to the similarity, and filtering the first candidate keyframes. The first candidate keyframe is the second candidate keyframe. Greater than 1; from Among the second candidate keyframes, the candidate frame with a number of matching interior points greater than or equal to the fifth threshold and satisfying the consistency verification is selected as the third candidate keyframe; when there is only one third candidate keyframe, the third candidate keyframe is determined as the historical matching keyframe; when there are multiple third candidate keyframes, the number of interior points, pose estimation uncertainty, and information content in each third candidate keyframe are scored, and the third candidate keyframe with the highest score is determined as the historical matching keyframe.

[0013] Secondly, this application proposes a vision-based UAV swarm cooperative localization device, a processor, and a memory communicatively connected to the processor, wherein the memory stores program instructions executable by the processor, and the processor can execute the vision-based UAV swarm cooperative localization method as described in the first aspect by calling the program instructions.

[0014] Based on the above scheme, keyframe information of adjacent UAVs and historical matching keyframe information of the first keyframe of the new hovering point after loop closure are obtained. Corresponding pose information is then obtained based on this keyframe information, and relative observation residuals are generated based on this pose information. and cyclic residual These are used to constrain the UAV state optimization model in order to improve the consistency and high-precision accuracy of UAV swarm cooperative positioning.

[0015] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0016] Figure 1 This is a schematic block diagram illustrating a vision-based collaborative localization method for unmanned aerial vehicle (UAV) swarms, according to an embodiment of this application. Figure 2 This is a schematic block diagram of a neural network visual odometry according to an embodiment of this application; Figure 3 This is a schematic diagram of a factor in an embodiment of this application; Figure 4 This is a block diagram illustrating a vision-based UAV swarm cooperative localization method according to another embodiment of this application. Detailed Implementation

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

[0018] In related technologies, drone swarm cooperative positioning schemes mainly rely on centralized or single-drone autonomous positioning technologies. Centralized schemes typically require a central server to collect sensor data from all drones for joint processing and optimization, while single-drone schemes primarily rely on each drone's own visual-inertial odometry for independent positioning. Their workflow generally includes the following core steps: a. Single-unit data acquisition: Each UAV acquires image sequences through its onboard camera and obtains angular velocity and acceleration information through its inertial measurement unit (IMU).

[0019] b. Single-unit pose estimation: Visual odometry using the traditional feature point method or direct method, combined with IMU pre-integration, estimates the motion trajectory of the unit through tight coupling or loose coupling.

[0020] c. Data communication (in a collaborative scheme): Each UAV sends its pose estimate, keyframes, or map points to a central server or neighboring UAVs.

[0021] d. Centralized or collaborative optimization: The central server constructs a global map or pose graph based on all received data and performs optimization (centralized); or each UAV performs a certain degree of consistency optimization locally based on the received neighbor information (distributed prototype).

[0022] e. Positioning output: The optimized global pose or consistent pose is sent to each UAV for navigation and formation.

[0023] In scenarios with no or little GNSS signal, such as mines, forests, and canyons, the single-unit autonomous positioning method is prone to drift due to the visual inertial odometry in the UAV, which can easily generate and accumulate drift. This leads to poor consistency in the cooperative positioning of the UAV swarm and inaccurate high-precision cooperative positioning.

[0024] To address the above issues, this application proposes a vision-based UAV swarm cooperative localization method. This method can acquire keyframe information from neighboring UAVs, as well as historical keyframe information matching the first keyframe of the new hovering point after a loop closure, and obtain corresponding pose information based on these keyframe information. Based on this pose information, a relative observation residual is generated. and cyclic residual These are used to constrain the UAV state optimization model in order to improve the consistency and high-precision accuracy of UAV swarm cooperative positioning.

[0025] Figure 1 A schematic block diagram illustrating a vision-based cooperative localization method for unmanned aerial vehicle (UAV) swarms, according to an embodiment of this application, is shown. Figure 1 As shown, the UAV swarm cooperative localization method includes: S110, acquire any drone in the drone swarm Multiple keyframes, based on the drone The first relative pose observation is determined by the key points in two adjacent keyframes. , For drones Relative rotation of the observation matrix, For drones Relative translation of the observation vector.

[0026] Multiple keyframes can be selected based on time or distance intervals. However, this selection strategy fails to adequately consider changes in image information, which can easily lead to data redundancy or information loss.

[0027] Preferably, in some embodiments of this application, the step of acquiring any drone in the drone swarm... Multiple keyframes, including: determining the drone Current image frame The feature points, and the Jacobian matrix of the visual reprojection error of the feature points with respect to the state variables; the current image frame is determined based on the sum of the L2 norms of each column vector of the Jacobian matrix. The first information scalar; if the relative rate of change between the first information scalar and the information scalar of the previous keyframe is greater than a first threshold, then the current image frame is... The current image frame is marked as a new keyframe, and the first informatics scalar is set as the new informatics scalar to determine the next keyframe. If the relative rate of change between the first informatics scalar and the informatics scalar of the previous keyframe is less than or equal to a first threshold, the current image frame is discarded. .

[0028] For example, Indicates each drone At any moment Visual images acquired from monocular or binocular cameras, first information scalar , The larger the value, the greater the contribution of the current frame to the state estimation, and the richer the information; relative rate of change , This represents the information scalar of the previous keyframe; if the relative rate of change is greater than the first threshold, then the current image frame... Mark as a new keyframe and update. Otherwise, discard the current image frame. Then, process the next frame. It should be noted that when the relative rate of change is greater than the first threshold, it is determined that the scene information has changed significantly. Keyframes obtained in this way can effectively solve the problems of data redundancy and data loss.

[0029] Optionally, in some embodiments of this application, such as Figure 2 As shown, the first relative pose observation is determined by inputting two adjacent keyframes into the neural network visual odometry 200. The neural network visual odometry 200 includes a feature matching module 210 and a pose transformation module 220. The feature matching module 210 includes a convolutional neural network 211 and a graph neural network 212. For example, the feature matching module 210 uses a multimodal feature detector to extract multiple keypoints (e.g., 512 keypoints) and their descriptors. This multimodal feature detector supports multiple input modalities such as grayscale, RGB, RGB-D, and stereo vision. It adapts to different sensors by copying pre-trained weights to the input layer, ensuring stable feature extraction in complex environments (such as varying lighting and repetitive textures).

[0030] A matcher trained on a dataset is used for feature matching via graph neural networks and attention mechanisms, exhibiting stronger robustness to dynamic elements in the environment. Output Coordinates of successfully matched points Each action , in This represents the number of successful matches in the current frame. Where, ( ) is the first Feature points on keyframes, ( ) is the first Feature points on keyframes.

[0031] It should be noted that, to balance computational efficiency and accuracy, only 512 optimal key points are extracted from each keyframe, and feature matching is performed based on these. This is only 25% of the number of key points required by traditional methods (2048), significantly reducing computational and communication overhead.

[0032] The key point coordinates of the successful match output by the feature matching module 210 Combining into dimensions ( The sequence , 4) is taken as input, where The pose transformation module 220 includes a lightweight encoder. This model understands the overall motion pattern of all keypoints through a self-attention mechanism, thereby regressing the relative motion of the camera. The model outputs a 3D translation vector and a 6D rotation representation (the first two columns of the rotation matrix), which can be recovered into a complete 3x3 rotation matrix R through a Gram-Schmidt process. Finally, the visual odometry module 200 outputs the relative pose transformation between two adjacent keyframes. .

[0033] S120, Obtain the time interval between two adjacent keyframes. The drone mentioned inside Inertial measurement data set According to the measured angular velocity value Acceleration measurement value Determine the relative rotation pre-integral Relative velocity pre-integral and relative displacement pre-integral .

[0034] For example, the time interval between two adjacent keyframes Inside, the inertial measurement data set output by the IMU at high frequency is: .exist In the body coordinate system at a given time, the time window All IMU sampled data, including angular velocity measurements. Acceleration measurement value With sampling interval Discrete integration is performed using the reference as a basis to determine the relative rotation pre-integral. Relative velocity pre-integral and relative displacement pre-integral These pre-integral quantities are only related to IMU measurements and It depends on the time offset: Relative rotation pre-integration : ; Relative velocity pre-integration : ; Relative displacement pre-integration : ; Optionally, in some embodiments of this application, a first-order Taylor expansion is used to pre-integrate the relative rotation. Relative velocity pre-integral and relative displacement pre-integral Perform linear approximation correction: ; ; ; in, , , This represents the basic pre-integration result calculated based on the old zero bias (including gyroscope zero bias and accelerometer zero bias) in the previous keyframe; , , This represents the latest pre-integral estimate obtained after linear correction following the current iteration update to zero bias; Represents relative rotation pre-integration Regarding the Jacobian matrix for zero bias of a gyroscope Represents the pre-integration of relative velocity Regarding the Jacobian matrix for zero bias of a gyroscope Represents relative rotation pre-integration Regarding the Jacobian matrix for zero bias of the gyroscope; Represents the pre-integration of relative velocity Regarding the Jacobian matrix for zero bias in accelerometers Represents relative rotation pre-integration Regarding the Jacobian matrix for zero bias of the accelerometer; and These represent the update amounts of the gyroscope zero bias and accelerometer zero bias generated in a single optimization iteration of the UAV state optimization model, respectively. They can also be described as the difference between the temporary new zero bias and the old baseline zero bias obtained by the UAV state optimization model in the current iteration.

[0035] Understandably, by using a first-order Taylor expansion to linearly approximate and correct each pre-integral quantity, the problem of high computational cost caused by the need for multiple integrations due to the continuous updating of gyroscope and accelerometer zero bias can be reduced.

[0036] S130, Acquiring information from drones Related data packets, the data packets including the drone exist The drone was observed at all times. Second relative pose observations The drone For the drone Adjacent drones.

[0037] For example, any drone in a drone swarm Start independently and load the pre-trained visual odometry model. Set the world coordinate system. The first drone (e.g., ) its initial pose Set as ,in Let be the initial rotation matrix for drone number 1 in the drone swarm. Let be the initial translation vector for drone number 1 in the drone swarm. It is the identity matrix. This is the zero vector. Other UAV initial poses are initialized through the first relative observation. For example, if the UAV... The drone was observed at the initial moment. Obtain the second relative pose observation value Then drone The initial pose can be initialized as follows: ; Determine the drone After the initial pose is determined, the drone... The onboard visual sensor acquires data from the drone. and Relative pose transformation between .

[0038] Any drone in a drone swarm Keyframe information is periodically encapsulated into data packets and broadcast to other drones. The data packet content includes: timestamps. Drone ID Current pose estimation Information such as image descriptors used for loop closure detection (e.g., NetVLAD descriptors). Drones It also periodically receives drones. The data packet sent includes the second relative pose observation. Optionally, in some embodiments, the second relative pose observation value It can also be done via drones Receive drone Send pose information and calculate it automatically.

[0039] S140, under the condition of satisfying the loopback condition, the drone A loop occurs, and the drone is acquired. The third relative pose observation between the keyframe of the new levitation point and the historical matching keyframes. .

[0040] Optionally, in some embodiments of this application, historical keyframes include any one of the drones in the drone swarm. The UAV swarm cooperative localization method further includes: selecting multiple first candidate keyframes from the historical keyframes whose time interval is greater than or equal to a third threshold, or whose spatial distance is greater than or equal to a fourth threshold; determining the similarity between each first candidate keyframe and the first keyframe, and arranging them from high to low according to the similarity, and filtering the first candidate keyframes. The first candidate keyframe is the second candidate keyframe. Greater than 1; from Among the second candidate keyframes, the candidate frame with a number of matching interior points greater than or equal to the fifth threshold and satisfying the consistency verification is selected as the third candidate keyframe; when there is only one third candidate keyframe, the third candidate keyframe is determined as the historical matching keyframe; when there are multiple third candidate keyframes, the number of interior points, pose estimation uncertainty, and information content in each third candidate keyframe are scored, and the third candidate keyframe with the highest score is determined as the historical matching keyframe.

[0041] The third threshold can be 5 seconds, and the fourth threshold can be 2 meters; the specific values ​​can be set according to the actual situation. The similarity between the first candidate keyframe and the first keyframe can be cosine similarity, and the specific calculation formula is as follows: ; This is the descriptor vector for the first keyframe. This is the descriptor vector of the first candidate keyframe. Multiple first candidate frames are sorted from high to low according to their cosine similarity, and the top ones are selected. One (for example, =5) is the second candidate keyframe.

[0042] Will The second candidate keyframe is input into the feature matching module 210 for fine feature matching with the first keyframe, and geometric verification (using RANSAC to calculate the fundamental matrix or essential matrix). If the number of matched inliers exceeds or equals the fifth threshold (e.g., 60% of the total number of matches), then this second candidate keyframe is determined as the third candidate keyframe.

[0043] When there are multiple third candidate keyframes, the third candidate keyframes are comprehensively scored based on indicators such as the number of interior points, pose estimation uncertainty, and information content, and the third candidate keyframe with the highest score is selected as the historical matching keyframe. The scoring formula is as follows: ; in, The number of interior points. To determine the uncertainty in pose estimation, For information content, The time interval is specified. Each indicator is standardized to eliminate the influence of dimensions. Weighting coefficients are also specified. , , , It can be adjusted according to the scenario, such as setting it to [1.0, 0.8, 0.5, 0.3].

[0044] At this point, the loop closure detection is considered successful, and the relative pose transformation between the first keyframe and the historical matching keyframes is obtained. .

[0045] S150, the first relative pose observation value The relative rotation pre-integration The relative velocity pre-integration The relative displacement pre-integration The second relative pose observation value and the third relative pose observation value Input into the UAV state optimization model to obtain the UAV Status information The status information Including translation Rotation ,speed accelerometer zero bias gyroscope zero bias .

[0046] The UAV state optimization model is based on the UAV Visual residual IMU residuals Relative observation residuals and cyclic residual The sum of squares of the Mahalanobis norm is determined, and the visual residual is... The IMU residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent keyframes. The relative observation residual is used to constrain the difference between the IMU pre-integrated measurements and the relative motion predicted by the corresponding pose and velocity estimates. The loop closure residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent UAVs. This is used to constrain the difference between the relative pose observations and the corresponding relative pose estimates obtained by the UAV after a loop closure and related to the historical matching keyframes.

[0047] It should be noted that the state vector It is the set of poses of the drone at all keyframe moments. For the first... A drone at the key frame moment The pose is used in the world coordinate system. The transformation matrix under This is represented as a state vector for easier optimization. .

[0048] By acquiring keyframe information from adjacent UAVs and historical keyframe information matching the first keyframe of the new hover point after a loop closure, and obtaining corresponding pose information based on this keyframe information, a relative observation residual is generated. and cyclic residual These are used to constrain the UAV state optimization model in order to improve the consistency and high-precision accuracy of UAV swarm cooperative positioning.

[0049] Optionally, in some embodiments of this application, the UAV state optimization model is: ; To match the visual residual The inverse of the relevant covariance matrix, To the IMU residual The inverse of the relevant covariance matrix, To the relative observation residual The inverse of the relevant covariance matrix, It is related to the loop residual. The inverse of the relevant covariance matrix; For the drone With all the drones mentioned Related relative observation sets; The drone The The drone was observed in the frame. The The relative pose constraints generated by the frame; For drones The set of all successfully triggered active loops; The drone The k-th frame and the drone The effective closure constraint formed in the m-th frame.

[0050] Optionally, in some embodiments of this application, visual residual , wherein This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The first pose estimate at time 1, Time Drone The second pose estimate.

[0051] Optionally, in some embodiments of this application, visual residual , wherein This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The first pose estimate at time 1, Time Drone The second pose estimate.

[0052] Optionally, in some embodiments of this application, the IMU residual for: ; in, express Drones at the Moment Estimated value of the rotation matrix in the world coordinate system express Drones at the Moment Estimated value of the rotation matrix in the world coordinate system express Drones at the Moment Translation estimate in world coordinate system express Drones at the Moment Translation estimate in world coordinate system express Drones at the Moment Velocity estimates in the world coordinate system express Drones at the Moment Velocity estimates in the world coordinate system This represents the gravity vector in the world coordinate system. Indicates the time interval between two adjacent keyframes , This represents the change in accelerometer bias between two adjacent keyframes. This represents the change in gyroscope zero bias between two adjacent keyframes.

[0053] Optionally, in some embodiments of this application, relative observation residuals for: ; Among them, the This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The second pose estimate at time t. Indicates drone exist The estimated third pose at time t; The cyclic residual for: ; Among them, the This represents mapping elements of a Lie group to its Lie algebra; The drone The fourth pose estimation value of the new suspension point, Indicates drone In the historical matching keyframes The fifth pose estimate at that time.

[0054] For example, such as Figure 3The relationships between various residuals of different UAVs are shown, and a UAV state optimization model is constructed based on these relationships. Taking the Gauss-Newton method as an example, the UAV state optimization model is iteratively solved. The core steps are as follows: Initialization: Set initial state estimation (Estimated by preliminary estimation from front-end visual odometry and IMU pre-integration (velocity, accelerometer bias, gyroscope bias), number of iterations) .

[0055] Linearization: Estimating the current state At this point, calculate the residuals corresponding to each factor. ( , and (any one of the following) and Jacobian matrix Perform a first-order Taylor expansion on all residual functions: ; in It is any one of the four residual vectors mentioned above in the current state. It is the first Each residual vector is paired with the state vector. The Jacobian matrix.

[0056] Constructing incremental equations: Substituting the linearized residuals into the UAV state optimization model, the minimization problem is approximated as: ; ; right Differentiate and set it to zero: ; The Gauss-Newton incremental equation is obtained: ; make , Then the equation simplifies to: ; in, It is called the approximate Hessian matrix. It consists of the sum of contributions from all factor Jacobian matrices and the information matrix, and is a large-scale sparse, symmetric, positive definite matrix. Its sparse structure is entirely determined by the topology of the factor graph. This is the gradient vector.

[0057] State update: Solve the linear equations using Cholesky decomposition or the conjugate gradient method to obtain the state increment. and update the status: ; in, The specific operators for the operation are as follows: translation, velocity, and zero-bias update use Euclidean addition, i.e. The rotation update employs an exponential mapping from Lie algebras to Lie groups, i.e. , This is the increment of the rotation vector.

[0058] Iteration and convergence: Repeat the above steps of linearization, constructing incremental equations, and state updates until the state increment is reached. The convergence condition or iteration threshold is reached. The convergence condition is... ,For example, = The iteration threshold can be 10.

[0059] It should be noted that the above-mentioned different types of covariance matrices can be obtained in the following ways: Covariance matrix of IMU factors The prior noise parameters (such as Gaussian white noise of angular velocity / acceleration and zero-bias random walk) obtained from the offline calibration of the IMU hardware are derived by linear error propagation based on the first-order Taylor expansion during the IMU pre-integration process.

[0060] Covariance matrix of visual / relative / laparoscopic factors The estimation is directly derived from the intrinsic estimation of neural network visual odometry. While regressing the relative pose, the model simultaneously outputs the measurement uncertainty of the current pose estimate based on the confidence distribution of feature matching and geometric consistency (such as the proportion of interior points), and uses this as the covariance matrix.

[0061] Understandable ; ; ; .

[0062] Optionally, in some embodiments of this application, the method further includes: when the UAV state optimization model reaches the convergence condition or iteration threshold, the UAV... To other drones in the drone swarm Exchange the status information; and place the drone Visual residual IMU residuals Relative observation residuals and cyclic residual Linearization is performed to obtain local Gaussian-Newton equations: The To and , , and The relevant first approximate Hessian matrix, For the increment of the state variable, The gradient vector; If the first approximate Hessian matrix There is a drone in the local information sub-block. If the information is obtained, then the drone will be acquired. Sending with the drone Related local information sub-blocks To generate common local information sub-blocks; if multiple drones exist Then obtain multiple of the aforementioned drones. The sum of local information sub-blocks, or, obtaining multiple of the aforementioned drones. The weighted average of the local information sub-blocks is used to generate the common local information sub-block, wherein the UAV To be with the drone Unmanned aerial vehicles with constraints; replacing the first approximate Hessian matrix with the common local information sub-block. Local information sub-blocks are used to generate new Gauss-Newton equations. ,in, This is a second approximate Hessian matrix that contains the information of the common local information sub-blocks. The gradient vector containing the common local information sub-block information is used to obtain the UAV again by iterating the new Gauss-Newton equation. Status information.

[0063] Among them, drones To be with the drone The drones with a constraint relationship include those related to drones Adjacent drones, drones that acquire historical matching keyframes, etc. It should be noted that after generating common local information sub-blocks, a consensus equation is formed, for example, Then, the drone u replaces the corresponding part of its local equations with this consensus equation to generate new Gauss-Newton equations. .

[0064] In this way, each drone uses a new local equation that incorporates information from neighboring drones. , Solve for the state increment. The system updates its state estimate. After multiple iterations of this "local computation-communication coordination-local update" process, all UAVs' estimates of the pose of the first keyframe that produces the loop will converge to the same value. Furthermore, private variables are synchronized through IMU residuals. Closely linked to public variables, the trajectory of the entire cluster achieves consistent optimization in a global sense. Public variables are the relevant variables within the common local information sub-blocks, while private scalars are the relevant variables excluding those within the common local information sub-blocks.

[0065] Optionally, in some embodiments of this application, the drone In the absence of a loopback, the loopback condition is based on The aforementioned visual residuals The first mean of the Mahalanobis norm, The IMU residuals The second mean of the Mahalanobis norm The relative observation residuals The relationship between the weighted sum of the third average of the Mahalanobis norm and the second threshold; in the drone In the event of a loop closure, the loop closure condition is the first average value, the second average value, the third average value, and... The aforementioned loop residuals The relationship between the weighted sum of the fourth average and the second threshold; the closure condition is that there are multiple consecutive weighted sums that are all greater than the second threshold.

[0066] For example, after each optimization, the UAV state optimization model calculates the above four types of residual statistics. For the... Class factors, statistical analysis of their most recent and corresponding information matrix The normalized residual statistic is defined as: ; in, Let be the residual vector of the i-th factor in the m-th class. This is the corresponding information matrix (the inverse of the covariance matrix). This formula represents the mean of the squared Mahalanobis distance between the observed and estimated values.

[0067] The normalized residuals of various factors are weighted and fused to obtain the global error index. : ; in These are the weight coefficients for various factors, whose magnitudes are related to the information content or reliability of the factors and satisfy normalization constraints. . The higher the value, the greater the inconsistency between the current state estimate and the observed data, and the larger the positioning error.

[0068] Decision-making and triggering: To avoid misjudgments caused by short-term noise or transient observation degradation, the system introduces a time consistency criterion. Only when the global error index... When the preset anomaly threshold is exceeded in z consecutive optimizations, the following condition is met: ; The system only then determined that the current pose estimation had a continuously accumulating error, and the drone... The trajectory has low reliability and triggers an active loop control process.

[0069] Among them, threshold Based on offline calibration data statistics, the mean values ​​of the positioning health indicators were determined under normal flight conditions. With variance And set the threshold as follows: = +λ λ = 2 or 3, where λ is the confidence factor.

[0070] After the loopback condition is met: Upon receiving the trigger signal, the UAV plans a safe retreat path based on its current heading angle and environmental information provided by onboard obstacle avoidance sensors (such as LiDAR or depth cameras). A typical strategy is to command the UAV to fly a preset distance in the opposite direction of its current heading. (For example (meters). After avoiding obstacles, hover in the new location and capture new keyframes. .

[0071] Thus, in this embodiment, when various residuals are detected to continuously exceed limits, the system automatically triggers a backflight and data resampling process, proactively creating loopback conditions and correcting errors by introducing a loopback factor. This approach enables the system to possess an intelligent closed-loop capability of "perception-decision-action," achieving a leap from passive optimization to proactive fault tolerance, and significantly improving the system's long-term positioning stability in complex environments.

[0072] For ease of understanding by those skilled in the art, such as Figure 4 As shown, this application presents a detailed embodiment illustrating a vision-based collaborative localization method for unmanned aerial vehicle (UAV) swarms.

[0073] S410, System Initialization and Data Acquisition.

[0074] S420, intelligent keyframe selection and local factor construction.

[0075] Among them, local factors are various types of residuals.

[0076] S430 allows UAVs to exchange keyframes and constraint information.

[0077] S440, Distributed Factor Graph Construction and Constraint Information.

[0078] S450, Residual Analysis and Error Detection.

[0079] S460, determine whether the residual exceeds the limit.

[0080] S470, if the limit is exceeded, active loop control will be activated.

[0081] S480, if not exceeding the limit, output the optimized global pose.

[0082] It should be noted that in the above embodiments, the acquisition of various residuals can be performed locally on the UAV, while the calculation of the UAV state optimization model and the judgment of loop closure conditions can be performed on the server.

[0083] Based on the same idea, this application also provides a drone swarm cooperative positioning device, which includes a memory for storing computer program instructions and a processor for executing the program instructions, wherein when the program instructions are executed by the processor, the drone swarm cooperative positioning device performs the various method embodiments described above.

[0084] Furthermore, the terms "first," "second," etc., used in the embodiments of this application are for descriptive purposes only and should not be construed as indicating or implying relative importance, or implicitly specifying the number of technical features indicated in this embodiment. Therefore, features defined with terms such as "first" and "second" in the embodiments of this application can explicitly or implicitly indicate that the embodiment includes at least one of those features. In the description of this application, the word "multiple" means at least two or more, such as two, three, four, etc., unless otherwise explicitly and specifically defined in the embodiments.

[0085] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. A vision-based cooperative localization method for unmanned aerial vehicle (UAV) swarms, characterized in that, The drone swarm cooperative localization method includes: Obtain any drone in the drone swarm Multiple keyframes, based on the drone The first relative pose observation is determined by the key points in two adjacent keyframes. , For drones Relative rotation of the observation matrix, For drones Relative translation of the observation vector; Obtain the time interval between two adjacent keyframes The drone mentioned inside Inertial measurement data set According to the measured angular velocity value Acceleration measurement value Determine the relative rotation pre-integral Relative velocity pre-integral and relative displacement pre-integral ; Acquisition and Drones Related data packets, the data packets including the drone exist The drone was observed at all times. Second relative pose observations The drone For the drone Adjacent drones; Under the condition of satisfying the loopback condition, the drone A loop occurs, and the drone is acquired. The third relative pose observation between the first keyframe of the new levitation point and the historical matching keyframes. ; The first relative pose observation value The relative rotation pre-integration The relative velocity pre-integration The relative displacement pre-integration The second relative pose observation value and the third relative pose observation value Input into the UAV state optimization model to obtain the UAV Status information The status information Including translation Rotation ,speed accelerometer zero bias gyroscope zero bias ; The UAV state optimization model is based on the UAV Visual residual IMU residuals Relative observation residuals and cyclic residual The sum of squares of the Mahalanobis norm is determined, and the visual residual is... The IMU residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent keyframes. The relative observation residual is used to constrain the difference between the IMU pre-integrated measurements and the relative motion predicted by the corresponding pose and velocity estimates. The loop closure residual is used to constrain the difference between the pose observations and corresponding pose estimates of two adjacent UAVs. This is used to constrain the difference between the relative pose observations and the corresponding relative pose estimates obtained by the UAV after a loop closure and related to the historical matching keyframes.

2. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The method further includes: When the UAV state optimization model reaches the convergence condition or iteration threshold, the UAV To other drones in the drone swarm Exchange the status information; The drone Visual residual IMU residuals Relative observation residuals and cyclic residual Linearization is performed to obtain local Gaussian-Newton equations: The To and , , and The relevant first approximate Hessian matrix, For the increment of the state variable, The gradient vector; If the first approximate Hessian matrix There is a drone in the local information sub-block. If the information is obtained, then the drone will be acquired. Sending with the drone Related local information sub-blocks To generate common local information sub-blocks; If there are multiple drones Then obtain multiple of the aforementioned drones. The sum of local information sub-blocks, or, obtaining multiple of the aforementioned drones. The weighted average of the local information sub-blocks is used to generate the common local information sub-block, wherein the UAV To be with the drone Unmanned aerial vehicles with constraints; Replace the first approximate Hessian matrix with the common local information sub-block. Local information sub-blocks are used to generate new Gauss-Newton equations. ,in, This is a second approximate Hessian matrix that contains the information of the common local information sub-blocks. The gradient vector containing the common local information sub-block information is used to obtain the UAV again by iterating the new Gauss-Newton equation. Status information.

3. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The acquisition of any drone in the drone swarm Multiple keyframes, including: Determine the drone Current image frame The feature points, and the Jacobian matrix of the visual reprojection error of the feature points with respect to the state variables; The current image frame is determined based on the sum of the L2 norms of each column vector of the Jacobian matrix. The first information scalar; If the relative rate of change between the first information scalar and the information scalar of the previous keyframe is greater than the first threshold, then the current image frame is... Mark it as a new keyframe and set the first informatics scalar as the new informatics scalar to determine the next keyframe; If the relative rate of change between the first information scalar and the information scalar of the previous keyframe is less than or equal to the first threshold, then the current image frame is discarded. .

4. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The UAV state optimization model is as follows: ； To match the visual residual The inverse of the relevant covariance matrix, To the IMU residual The inverse of the relevant covariance matrix, To the relative observation residual The inverse of the relevant covariance matrix, It is related to the loop residual. The inverse of the relevant covariance matrix; For the drone With all the drones mentioned Related relative observation sets; The drone The The drone was observed in the frame. The The relative pose constraints generated by the frame; For drones The set of all successfully triggered active loops; The drone The k-th frame and the drone The effective closure constraint formed in the m-th frame.

5. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The visual residual , wherein This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The first pose estimate at time 1, Time Drone The second pose estimate.

6. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The IMU residual for: ； in, express Current drones Estimated value of the rotation matrix in the world coordinate system express Current drones Estimated value of the rotation matrix in the world coordinate system express Current drones Translation estimate in world coordinate system express Current drones Translation estimate in world coordinate system express Current drones Velocity estimates in the world coordinate system express Current drones Velocity estimates in the world coordinate system Represents the gravity vector in the world coordinate system. Indicates the time interval between two adjacent keyframes , This represents the change in accelerometer bias between two adjacent keyframes. This represents the change in gyroscope zero bias between two adjacent keyframes.

7. The UAV swarm cooperative positioning method according to claim 1, characterized in that, The relative observation residual for: ； Among them, the This represents mapping elements of a Lie group to its Lie algebra. Indicates drone exist The second pose estimate at time t. Indicates drone exist The estimated third pose at time t; The cyclic residual for: ； Among them, the This represents mapping an element of a Lie group to its Lie algebra; The drone The fourth pose estimation value of the new suspension point, Indicates drone In the historical matching keyframes The fifth pose estimate at that time.

8. The UAV swarm cooperative positioning method according to claim 1, characterized in that, In the drone In the absence of a loopback, the loopback condition is based on The aforementioned visual residuals The first mean of the Mahalanobis norm, The IMU residuals The second mean of the Mahalanobis norm The relative observation residuals The relationship between the weighted sum of the third mean of the Mahalanobis norm and the second threshold; In the drone In the event of a loop closure, the loop closure condition is the first average value, the second average value, the third average value, and... The aforementioned loop residuals The relationship between the weighted sum of the fourth average and the second threshold; The condition for satisfying the loop closure is that there exist multiple consecutive weighted sums that are all greater than the second threshold.

9. The UAV swarm cooperative positioning method according to any one of claims 1-8, characterized in that, Historical keyframes include any drone in the drone swarm. The UAV swarm cooperative localization method further includes all keyframes and: Select multiple first candidate keyframes from the historical keyframes whose time interval is greater than or equal to a third threshold, or whose spatial distance is greater than or equal to a fourth threshold; Determine the similarity between each of the first candidate keyframes and the first keyframe, and sort them from high to low according to the similarity, before filtering. The first candidate keyframe is the second candidate keyframe. Greater than 1; from Among the second candidate keyframes, the candidate frame whose number of matching points with the first keyframe is greater than or equal to the fifth threshold and which satisfies the consistency verification is selected as the third candidate keyframe. When there is only one third candidate keyframe, the third candidate keyframe is determined to be the historical matching keyframe. When there are multiple third candidate keyframes, the number of interior points, pose estimation uncertainty, and information content in each third candidate keyframe are scored, and the third candidate keyframe with the highest score is determined as the historical matching keyframe.

10. A vision-based unmanned aerial vehicle (UAV) swarm cooperative positioning device, characterized in that, The processor, and a memory communicatively connected to the processor, wherein the memory stores program instructions executable by the processor, and the processor invokes the program instructions to execute the vision-based UAV swarm cooperative localization method as described in claim 1.

Images