A collaborative positioning method for distributed drone swarms in complex environments
The method uses fisheye cameras and UWB modules for distributed collaborative positioning of UAVs, addressing deployment and accuracy issues by eliminating drift and reducing computational load, ensuring stable and high-precision positioning in complex environments.
Patent Information
- Application Number
- CN202311873606.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-12-29
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2043-12-29
AI Technical Summary
Existing multi-UAV systems face challenges in complex environments due to the reliance on external devices for state estimation, which are difficult to deploy and suffer from accuracy issues like drift errors, electromagnetic interference, and reduced observability, necessitating a method for distributed collaborative positioning without external equipment.
A collaborative positioning method using fisheye cameras and UWB modules on each drone for image preprocessing, loop detection, and relative distance measurement, combined with a distributed loop detection and outlier elimination to ensure accurate and stable positioning.
Enables accurate and stable positioning of drone swarms in complex environments by eliminating drift errors and reducing computational load, ensuring global consistency and high-precision positioning through active loop detection.
Smart Images

Figure CN117826141B_ABST
Abstract
Description
Technical Field
[0001] The present invention is applicable to the field of UAV technology, and in particular relates to a collaborative positioning method for a distributed UAV group in a complex environment. Background Art
[0002] In recent years, the research on multi-UAV systems has received more and more attention. Compared with a single UAV, a multi-UAV system can better complete some tasks, such as exploring an area faster and more accurately in other tasks such as autonomous exploration or search and rescue, and share individual information with other individuals to coordinate their respective tasks and plans.
[0003] Currently, most multi-UAV positioning relies on external devices to provide state estimation, such as anchor-based UWB (Ultra-Wideband), motion capture systems and GPS systems. However, these centralized systems require the deployment of a large number of external devices to estimate the state of the UAV swarm. When in some open places or complex outdoor environments, the difficulty of deploying external devices will be greatly increased, and it is not suitable for application in actual exploration tasks. Therefore, it is necessary to solve the problem of distributed multi-UAV collaborative positioning without deploying external devices.
[0004] In addition, the commonly used methods for relative state estimation of multiple UAVs also have certain problems: (1) The visual inertial odometry (VIO) of the UAV can get rid of the deployment problem of external equipment, but the state estimation in scenes with little texture or lack of features has drift errors. (2) The UWB module can provide the relative distance between UAVs, but the UWB module will be affected by non-line-of-sight or insufficient long-distance communication, resulting in reduced accuracy. (3) The lack of observability of UAVs makes it difficult for UAVs to recognize the same landmarks on the one hand, and difficult for UAVs to recognize adjacent UAVs on the other hand.
[0005] In summary, the visual autonomous positioning method can complete the position estimation with high accuracy and does not require the deployment of any external facilities, but it has strict requirements on lighting and texture, and the state estimation may drift. When multiple UAVs are used, there is also the problem of different reference systems for pose estimation. The UWB positioning method does not have the estimation drift problem and can provide a global reference system, but the estimation accuracy is lower than the visual method and is susceptible to electromagnetic interference. Therefore, it is very necessary to study the method of distributed collaborative positioning of multiple UAVs in complex environments to improve the accuracy and stability of collaborative positioning of multiple UAVs in complex environments. Summary of the invention
[0006] The present invention provides a collaborative positioning method for a distributed UAV group in a complex environment, aiming to solve the problem of distributed multi-UAV collaborative positioning without deploying external equipment.
[0007] The collaborative positioning method comprises the following steps:
[0008] S1. Install a fisheye lens camera and a UWB module on each drone in the drone group;
[0009] S2, obtaining a fisheye image through the fisheye camera, and preprocessing the fisheye image to obtain an optimized image;
[0010] S3, extracting the visual inertial odometer in the optimized image, estimating the motion of each of the drones according to the visual inertial odometer, and obtaining a measurement model of each of the drones;
[0011] S4, performing mutual loop detection on the optimized images by using a distributed loop detection method, estimating the relative position and posture of each of the drones by using the fisheye camera, and eliminating the drift error of the visual inertial odometer;
[0012] S5. Measuring the relative distance between each of the drones through the UWB module;
[0013] S6. Establishing a redundant key frame deletion mechanism, and deleting the key frame with the highest redundancy value among the key frames of the visual inertial odometer according to the redundant key frame deletion mechanism; wherein the key frame of the visual inertial odometer includes the optimized image and the external parameters of the fisheye camera;
[0014] S7. estimating the state of the drone swarm according to the measurement model;
[0015] S8. Detect each of the drones in the drone group to determine whether it can observe adjacent drones. If so, the distance between the currently detected drone and the other drones is relatively close, and a close-range high-precision distributed collaborative positioning strategy is executed; if not, it indicates that the currently detected drone cannot be successfully detected or is detected to be relatively far away from other drones, and the currently detected drone is set as an independent drone for autonomous positioning through an active loopback detection strategy.
[0016] Preferably, in step S2, the process of preprocessing the fisheye image includes the following steps:
[0017] S21, converting the fisheye image according to a cylindrical projection model to obtain the optimized image;
[0018] The cylindrical projection model satisfies the following calculation formula:
[0019]
[0020] in, represents the radial distance of the cylinder, φ=atan2(X,Z) represents the azimuth, and f φ and f Y represent the focal lengths along the X-axis and the Y-axis respectively, u0 and v0 represent the principal points; u and v represent the coordinates of the projection points on the fisheye image; X, Y and Z represent the coordinate values of the X-axis, the Y-axis and the Z-axis respectively.
[0021] Preferably, the measurement model satisfies the following relationship:
[0022]
[0023] in, represents the relative position of UAV i from time t-1 to time t, represents the four-degree-of-freedom position of UAV i at time t, n vio represents Gaussian noise.
[0024] Preferably, the distributed loop detection method includes:
[0025] A first drone is defined. When the first drone receives a key frame of the visual inertial odometer, the first drone extracts features of the key frame to obtain feature information; the feature information is integrated and packaged with the state information of the first drone to obtain a packaged key frame; and the packaged key frame is broadcasted to other drones for communication;
[0026] A second UAV is defined, and a local database and a common-view database are established, wherein the local database is used to store key frames generated by the UAV itself; and the common-view database is used to store key frames received by communicating with adjacent UAVs; when the second UAV receives the encapsulated key frame, it is determined whether the serial numbers of the first UAV and the second UAV are the same, and if so, the encapsulated key frame is stored in the local database; if not, the encapsulated key frame is stored in the common-view database;
[0027] Retrieving the key frame with the highest matching degree with the encapsulated key frame from the local database or the common view database as a return key frame, and outputting the return key frame to the first drone; wherein the return key frame includes a global descriptor, a landmark, and the speed and acceleration of the second drone;
[0028] Performing distributed loop closure detection on the loop between the first UAV and the second UAV according to the encapsulation key frame and the return key frame;
[0029] Determine whether the loops between the first UAV and the second UAV are consistent; if not, remove the loops as outliers.
[0030] Preferably, the UWB module measures the relative distance between each of the drones to satisfy the following calculation formula:
[0031]
[0032] in, represents the distance measurement of drone i to drone j at time t, Indicates the status of the drone The translation part, n D represents Gaussian noise, n nlos Indicates the measurement error under non-line-of-sight conditions.
[0033] Preferably, the drone further comprises an outlier elimination module, and the outlier elimination module is used to eliminate the measurement error generated when the UWB module measures the relative distance between each of the drones.
[0034] Preferably, the redundancy value of the key frame of the visual inertial odometer satisfies the following calculation formula:
[0035]
[0036] Where i represents the serial number of key frame F, ξ i It represents the key frame F i The set of all observed map points, j represents the key frame F i The number of the observed map point, ob(M j ) represents the key frame F i The observed map point with sequence number j.
[0037] Preferably, the active loop detection strategy is:
[0038] Maintaining the path points of the independent UAV through the KD tree, and scoring the fitness of each path point as an active closed-loop path point to obtain a fitness value result;
[0039] Select the path point with the smallest cost from the current position of the independent UAV according to the fitness value result and the KD tree, and select the optimal path point;
[0040] The cost between the path of the independent UAV active closed-loop detection and the optimal path point is calculated to correct the accumulated drift error of the independent UAV.
[0041] Compared with the prior art, the present invention uses a fisheye camera and a UWB module to make the drone swarm not limited by the field of view, and can still accurately estimate the relative state in the absence of rich common environmental features; adopts a distributed mutual loop detection method to avoid the traditional centralized dependence on the ground station, and each drone can independently run self-state estimation and relative state estimation, and will not crash due to the exit of a certain drone; the outlier removal module can effectively remove the outliers measured by the UWB module, thereby making the UWB measurement more accurate and avoiding the decrease in collaborative positioning accuracy or even estimation errors; the mechanism of key frame redundancy value calculation is used to limit the computing resources used by the drone. By calculating the redundancy value of each key frame, the key frame with the highest redundancy value can be deleted, and the earlier key frames can be effectively retained, making the map more sparse, which can also lead to less drift and better global consistency; in view of the problem that when the drone swarm degenerates into a single VIO, long-term operation will produce cumulative drift, and an active loop detection method is proposed to eliminate the drift error of the drone. This method can ensure that a single UAV can maintain a good positioning in a complex environment, which will provide a good initial pose value for relative state estimation after the UAV group meets, achieve a coarse-to-fine positioning effect, and effectively ensure the global consistency of multiple UAVs. BRIEF DESCRIPTION OF THE DRAWINGS
[0042] The present invention will be described in detail below in conjunction with the accompanying drawings. The above and other aspects of the present invention will become clearer and easier to understand through the detailed description made in conjunction with the following drawings. In the accompanying drawings:
[0043] Figure 1 is a flow chart of a method for collaborative positioning of a distributed UAV swarm in a complex environment provided by an embodiment of the present invention;
[0044] Figure 2 It is a schematic diagram of a distributed collaborative positioning strategy flow chart of a collaborative positioning method of a distributed UAV swarm in a complex environment provided by an embodiment of the present invention;
[0045] Figure 3 It is a schematic diagram of the framework of a method for collaborative positioning of a distributed UAV group in a complex environment provided by an embodiment of the present invention;
[0046] Figure 4 It is a schematic diagram of the transformation from virtual to actual fisheye image of the collaborative positioning method of a distributed UAV group in a complex environment provided by an embodiment of the present invention;
[0047] Figure 5 Schematic diagram of a distributed loop detection method of a collaborative positioning method of a distributed UAV group in a complex environment provided by an embodiment of the present invention;
[0048] Figure 6It is a schematic diagram of loop outlier elimination in distributed loop detection of a method for collaborative positioning of a distributed drone group in a complex environment provided by an embodiment of the present invention;
[0049] Figure 7 It is a schematic diagram of UWB module ranging of a method for collaborative positioning of a distributed UAV group in a complex environment provided by an embodiment of the present invention;
[0050] Figure 8 It is a schematic flow chart of a redundant key frame deletion mechanism of a collaborative positioning method of a distributed UAV swarm in a complex environment provided by an embodiment of the present invention;
[0051] Fig. 9 It is a schematic diagram of the number of active closed-loop candidate point paths of the collaborative positioning method of a distributed drone swarm in a complex environment provided by an embodiment of the present invention. DETAILED DESCRIPTION
[0052] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.
[0053] Please refer to Figure 1-Figure 9 The present invention provides a method for collaborative positioning of a distributed UAV group in a complex environment, and the collaborative positioning method comprises the following steps:
[0054] S1. Install a fisheye lens camera and a UWB module on each drone in the drone group;
[0055] In an embodiment of the present invention, each drone in the drone group is equipped with an onboard computer, a flight controller, a UWB module, two single-purpose fisheye lens cameras and a WiFI module for communication. The UWB module is installed on each drone as an airborne sensor. It does not need to deploy anchor points in the environment and is only responsible for periodically measuring the relative distance between drones. The two single-purpose fisheye cameras are important modules for omnidirectional perception. The viewing angle of the fisheye camera is up to 220 degrees. By deploying two fisheye cameras, one of which is installed on the top of the drone and the other is installed on the bottom of the drone, the surrounding environment can be observed in all directions. The framework of the distributed collaborative positioning system is as follows: Figure 3 As shown. At the same time, timestamp synchronization is an issue that needs attention in robot swarms. The UWB modules complete mutual timestamp synchronization during ranging and communication, and send the synchronized timestamp to the onboard computer through the serial port. Therefore, the UWB timestamp is selected as the time reference to obtain the timestamp between multiple drones.
[0056] S2, obtaining a fisheye image through the fisheye camera, and preprocessing the fisheye image to obtain an optimized image;
[0057] In an embodiment of the present invention, the process of preprocessing the fisheye image includes the following steps:
[0058] S21, converting the fisheye image according to a cylindrical projection model to obtain the optimized image;
[0059] The cylindrical projection model satisfies the following calculation formula:
[0060]
[0061] in, represents the radial distance of the cylinder, φ=atan2(X,Z) represents the azimuth, and f φ and f Y represent the focal lengths along the X-axis and the Y-axis respectively, u0 and v0 represent the principal points; u and v represent the coordinates of the projection points on the fisheye image; X, Y and Z represent the coordinate values of the X-axis, the Y-axis and the Z-axis respectively.
[0062] like Figure 4 As shown, assuming that the intrinsic matrices of the virtual pinhole camera and cylindrical projection are equal, the transformation between real coordinates [X, Y, Z] and virtual coordinates can be obtained
[0063]
[0064] The corresponding inverse transform is:
[0065]
[0066] The 3D size of an object and its allocentric orientation are inferred from its appearance, which in cylindrical projection is similar to its appearance in a perspective image. The azimuth and elevation of a virtual object are related to the real azimuth and elevation as:
[0067]
[0068] They can be used with isocentric rotations to convert between real and virtual yaw and pitch.
[0069] The cylindrically projected fisheye image and the object after the virtual-to-real transformation are closer to the object in the normal image, so the preprocessed fisheye image can be applied to a 3D vision neural network trained using a normal image dataset and obtain approximately the same effect.
[0070] S3, extracting the visual inertial odometer in the optimized image, estimating the motion of each of the drones according to the visual inertial odometer, and obtaining a measurement model of each of the drones;
[0071] In the embodiment of the present invention, the individual drones in the swarm are estimated by fisheye cameras and inertial measurement units (IMUs). VIO is extracted based on the preprocessed fisheye image to provide real-time local attitude and velocity estimation. Due to the long-term drift of VIO, the original odometer is not directly fused, but the four-degree-of-freedom relative posture extracted from VIO is fused at the back end. The measurement model satisfies the following relationship:
[0072]
[0073] in, represents the relative position of UAV i from time t-1 to time t, represents the four-degree-of-freedom position of UAV i at time t, n vio Denotes Gaussian noise. The key frames of the drone’s VIO include the preprocessed image and the camera’s external parameters, which are further processed using real-time attitude estimation to avoid redundant calculations.
[0074] S4, performing mutual loop detection on the optimized images by using a distributed loop detection method, estimating the relative position and posture of each of the drones by using the fisheye camera, and eliminating the drift error of the visual inertial odometer;
[0075] In an embodiment of the present invention, the distributed loop detection method includes:
[0076] A first drone is defined. When the first drone receives a key frame of the visual inertial odometer, the first drone extracts features of the key frame to obtain feature information; the feature information is integrated and packaged with the state information of the first drone to obtain a packaged key frame; and the packaged key frame is broadcasted to other drones for communication;
[0077] A second UAV is defined, and a local database and a common-view database are established, wherein the local database is used to store key frames generated by the UAV itself; and the common-view database is used to store key frames received by communicating with adjacent UAVs; when the second UAV receives the encapsulated key frame, it is determined whether the serial numbers of the first UAV and the second UAV are the same, and if so, the encapsulated key frame is stored in the local database; if not, the encapsulated key frame is stored in the common-view database;
[0078] Retrieving the key frame with the highest matching degree with the encapsulated key frame from the local database or the common view database as a return key frame, and outputting the return key frame to the first drone; wherein the return key frame includes a global descriptor, a landmark, and the speed and acceleration of the second drone;
[0079] Performing distributed loop closure detection on the loop between the first UAV and the second UAV according to the encapsulation key frame and the return key frame;
[0080] Determine whether the loops between the first UAV and the second UAV are consistent; if not, remove the loops as outliers.
[0081] Specifically, in order to solve the problem that the centralized collaborative positioning of multiple drones depends on the host, the present invention designs a distributed loop detection method for a drone group, which runs separately on each drone. The optimized image is subjected to mutual loop detection. The fisheye camera is used to identify the locations visited by all drones to achieve relative positioning estimation and eliminate the drift of VIO.
[0082] In order to store and retrieve the VIO keyframes generated locally by the drone and the keyframes broadcasted by neighboring drones, two databases are constructed based on the vector similarity retrieval database: a local database for storing the keyframes generated by the drone itself and a common view database for storing the keyframes received by communicating with neighboring drones.
[0083] like Figure 5 As shown in the figure, the core of distributed loop detection is divided into four modules, namely key frame encapsulation, database retrieval, pose estimation and outlier removal. Distributed loop detection is divided into four sub-modules:
[0084] Keyframe Packaging Submodule: When UAV i (i.e., the first UAV) receives a VIO keyframe, it uses the lightweight deep learning model mobileNetVALD to extract global features, and uses the deep learning model SuperPoint for image feature extraction to extract landmarks and corresponding descriptors. Feature matching is performed to establish the correspondence between landmarks from the forward fisheye camera and the backward fisheye camera, and the matched landmarks are triangulated to estimate their 3D positions in the local frame. Global descriptors and landmarks, as well as information such as the speed and acceleration of the UAV, are packaged in the keyframe. In the process of broadcasting to the entire drone swarm, since some features cannot establish corresponding relationships, only feature points with successfully estimated 3D positions are broadcast to reduce the amount of communication.
[0085] Database detection submodule: When UAV j (i.e. the second UAV) receives the key frame After that, the drone determines the source of the key frame. If i=j, it means that the key frame is generated locally, and it searches in the common view database. After the search, the key frame is added to the local database. Otherwise, if i≠j, it means that the key frame is broadcasted by a neighboring drone, and it searches in the local database. After the search, it is added to the common view database. The drone will retrieve the most similar key frame in the database, and then pack the global descriptor, landmark, and drone speed and acceleration information of the matching frame with the highest similarity score into a key frame. Return to UAVi (i.e. drone i).
[0086] Pose estimation submodule: When receiving the returned closed-loop keyframe, use brute force matching to create two keyframes (keyframe and keyframes ) 2d-3d matching, and then use RANSAC PnP (Perspective-n-Point with Random Sample Consensus) to estimate the relative transformation between the two matching keyframes. If enough inliers are found in RANSAC and pass the geometric test, the distributed loop closure detection between the two drones is considered to be a good loop, and its model is:
[0087]
[0088] Among them, n z is Gaussian noise, Keyframe To Keyframe The relative position of . is the state of UAV i in the local frame under the body coordinates. If UAV i completes the relative pose estimation, the relative pose Transmitted back to UAVj (i.e. drone j).
[0089] When i≠j, map-based measurement Sufficient observability of the relative poses of UAV i and UAV j is provided. This makes map-based measurements essential for observability verification and initialization of state estimation.
[0090] If t0≠t1, it indicates the relative posture of the UAVs visiting the same location at different times, eliminating the accumulated drift error of VIO.
[0091] Outlier removal submodule: In order to avoid introducing erroneous interloops, the present invention proposes an interloop outlier removal submodule, such as Figure 6As shown in the figure, this module is mainly used for mutual loop outlier suppression, mainly to check whether the closed loops between paired drones are consistent with each other. The following formula is used to determine whether the loops between two drones are consistent in pairs:
[0092]
[0093] The present invention believes that the closed loop and the odometer should be identical. When the Mahalanobis distance between the loop and the odometer of the drone is greater than the threshold σ L , it is considered to be an erroneous mutual loop and is removed.
[0094] S5. Measuring the relative distance between each of the drones through the UWB module;
[0095] In the embodiment of the present invention, since it is difficult to build a UWB base station in an unknown complex environment, the present invention does not deploy anchor points in the environment, but deploys a UWB module on each drone to periodically measure the relative distance between each drone, such as Figure 7 As shown, the UWB module measures the relative distance between each of the drones to satisfy the following calculation formula:
[0096]
[0097] in, represents the distance measurement of drone i to drone j at time t, Indicates the status of the drone The translation part, n D represents Gaussian noise, n nlos Indicates the measurement error under non-line-of-sight conditions.
[0098] In an embodiment of the present invention, the drone further includes an outlier elimination module, and the outlier elimination module is used to eliminate the measurement error generated when the UWB module measures the relative distance between each of the drones.
[0099] When the drones are not visible to each other, there will be multipath effects and other influences in the non-line-of-sight (NLOS) situation, causing the distance measurement error to deviate from the normal distribution. According to the principle of graph optimization, if the outliers are not eliminated and directly used for optimization, it is equivalent to adding an edge that should not exist to the pose graph, which is bound to lead to a decrease in the accuracy of collaborative positioning or even estimation errors.
[0100] Therefore, in view of the measurement error of the UWB module, the present invention proposes an outlier elimination module for eliminating the measurement error of the UWB module.
[0101] The steps for the outlier removal module to remove measurement errors are as follows:
[0102] When the relative elevation angle between the two drones is large, UWB produces significant outliers. This is due to the occlusion of the drone’s body, so the measurement is marked And satisfy the following relationship:
[0103]
[0104] in, and are the estimated heights of UAVs i and j, τ angle is the angle threshold, which can be set to 37° in practice. angle It is selected based on the angle at which the UWB module is blocked by the drone body.
[0105] Then, the displacement increments of the two UAVs can be calculated by using the speed and acceleration of the adjacent UAVs obtained through communication between the UAVs.
[0106]
[0107]
[0108]
[0109] in, is the instantaneous speed of UAV i at time t-1 and time t, is the acceleration of UAV i at time t-1, is the instantaneous speed of UAV j at time t-1 and time t, is the acceleration of UAV j at time t-1. is the displacement increment of UAV i from time t-1 to time t, is the displacement increment of UAV j from time t-1 to time t. It is the maximum displacement increment that UAV i and UAV j can produce from time t-1 to time t.
[0110]
[0111] If the difference between the two distance measurements is greater than the maximum relative displacement increment that the two drones can produce, it means that the current distance measurement value is an outlier.
[0112] Then, the distance measurement of the UWB module at a certain moment should be consistent with the relative posture calculated by the distributed interloop detection.
[0113]
[0114] When the drone swarm is located in a feature-rich scene, if the error between the UWB distance measurement value and the relative pose obtained by the interloop detection is large, the current UWB distance measurement value is considered to be an outlier.
[0115] Finally, the residual with Huber loss is used to evaluate whether the current relative distance measurement is consistent with the relative state estimate.
[0116]
[0117] The expected error of UWB measurement does not exceed 10 cm, and the present invention selects it as τ D to ensure that the correct values are not over-filtered. D When τ is too large, outliers will interfere with the estimation. D If it is too small, correct measurements will be rejected due to inaccurate initialization.
[0118] S6. Establishing a redundant key frame deletion mechanism, and deleting the key frame with the highest redundancy value among the key frames of the visual inertial odometer according to the redundant key frame deletion mechanism; wherein the key frame of the visual inertial odometer includes the optimized image and the external parameters of the fisheye camera;
[0119] In the embodiment of the present invention, in order to solve the problem of large amount of calculation in the back-end optimization of the drone, the present invention proposes a redundant key frame deletion mechanism to limit the computing resources used by the back-end; once the group key frame in the figure grows to more than m max , then delete a group keyframe according to the redundancy of the group keyframe. The intuition of this method is that during the operation of the drone, there are some landmarks that can be observed by multiple keyframes. If some keyframes corresponding to the landmarks are discarded, it will not affect the estimation of the landmarks. However, if some landmarks are only observed by a few keyframes, these keyframes will have an important impact on the location estimation of the map points and the drone. With this method, the earlier group keyframes have less impact on the relative estimation and will not be discarded immediately, but become more sparse. Therefore, the earlier group keyframes can be used for optimization without affecting the calculation speed due to too many keyframes.
[0120] As the number of times a landmark is observed increases, the redundancy value of a keyframe to a single map point also increases rapidly. In order to better quantify the relationship between keyframes and map points, a redundant keyframe evaluation function is defined to represent the redundancy value of a keyframe to a single map point. The redundant keyframe evaluation function is as follows:
[0121]
[0122] This nonlinear function can quantify the redundancy of different keyframes according to the number of times the landmark point is observed. In order to facilitate the adjustment of subsequent scale changes, the redundancy value of the redundant keyframe is normalized to between [0,1], and the formula is as follows:
[0123]
[0124] cost(x) is the normalized redundancy value of the keyframe for a single map point.
[0125] The redundancy value of the key frame of the visual inertial odometry satisfies the following calculation formula:
[0126]
[0127] Where i represents the serial number of key frame F, ξ i It represents the key frame F i The set of all observed map points, j represents the key frame F i The number of the observed map point, ob(M j ) represents the key frame F i The observed map point with sequence number j.
[0128] Through the above steps, the redundancy value of each key frame can be accurately obtained. By sorting the key frames in descending order, it is possible to determine which key frame to delete first when a key frame needs to be deleted. The process of deleting redundant key frames is as follows: Figure 8 shown.
[0129] S7. estimating the state of the drone swarm according to the measurement model;
[0130] In the embodiment of the present invention, a nonlinear least squares optimization method is used to solve the maximum a posteriori (MAP) inference of the factor graph, thereby performing group state estimation. For drone k, the full state vector X of the drone group state estimation problem is defined as k :
[0131]
[0132] in, is the state vector of the four-degree-of-freedom posture, n is the number of drones in the swarm system, and m is the number of key frames in the figure.
[0133] The nonlinear least squares optimization problem for MAP inference is expressed as the following formula:
[0134]
[0135] where ρ(·) is the Huber norm, ‖(·)‖ ∑is the Mahalanobis norm, S is the set of all mileage factors, U is the set of all distance factors, L is the set of map-based factors, is the residual of the distance factor, where r p (·,X k ) is the residual of the relative pose, applicable to the ego-motion factor and the map-based factor. Represents the residual of the self-motion factor, which ensures the local consistency of the state of UAV i. Representing the residual of the map-based factor, the global consistency and observability of the relative state are guaranteed. Since the distance measurement and the map-based factor measurement may produce some outliers, the Huber norm ρ(s) is used to reduce the influence of possible outlier factors.
[0136] According to the measurement model, the relative pose residual is defined as:
[0137]
[0138] in Represents relative pose measurement, including self-motion measurement and measurement based on interloop detection.
[0139] According to the measurement equation of the UWB module, the distance residual is:
[0140]
[0141] Finally, the open source C++ library Ceres solver developed by Google for modeling and solving nonlinear least squares optimization problems is used to solve the optimization problem, in which the trust region method using sparse normal Cholesky decomposition is selected as the optimization algorithm.
[0142] S8. Detect each of the drones in the drone group to determine whether it can observe adjacent drones. If so, the distance between the currently detected drone and the other drones is relatively close, and a close-range high-precision distributed collaborative positioning strategy is executed; if not, it indicates that the currently detected drone cannot be successfully detected or is detected to be relatively far away from other drones, and the currently detected drone is set as an independent drone for autonomous positioning through an active loopback detection strategy.
[0143] In an embodiment of the present invention, in order to avoid collisions between drone groups, when the drones are close to each other, a high-precision relative positioning is required between the drones. When the distance between the drones becomes farther or they cannot be observed from each other, accurate relative positioning accuracy is not required between the drones, but it is more necessary to ensure the precise positioning of a single drone, so that when subsequent drones meet and approach each other, an accurate initial position value can be provided to achieve a change in positioning accuracy from coarse to fine.
[0144] In response to the above problems, such as Figure 1 As shown, the present invention proposes a distributed collaborative positioning strategy for multiple UAVs at close range and long range, which uses the visibility of adjacent UAVs as a sign to distinguish between them.
[0145] Through a target detection module, this module is mainly used to detect other drones in the cluster and detect the relative distance. Using YOLOv4-tiniy, one of the visual target detection methods based on convolutional neural networks, the two-dimensional bounding box of the drone is detected. The pre-processed fisheye image can effectively detect the drone trained with custom data.
[0146] When adjacent drones are successfully detected, it means that the distance between the current drones is relatively close, and high-precision relative positioning is required between the drones. In this case, a close-range and high-precision distributed collaborative positioning strategy is implemented. If the drone cannot be successfully detected or is detected at a relatively long relative distance, it means that the drone will not be able to obtain relatively accurate relative positioning, and therefore it degenerates into a single VIO for autonomous positioning.
[0147] According to the actual scenario, the individual existence of the drone swarm is degraded to a single VIO for autonomous positioning. When the following situations occur, the individual drone swarm will degrade to a single VIO for autonomous positioning:
[0148] (1) When communication is limited or interrupted, drones cannot communicate with each other and will degenerate into a single VIO positioning system.
[0149] (2) When the distance between drones becomes longer, or a drone cannot be observed by adjacent drones for a long time, the drone swarm can no longer perform accurate relative positioning, so the drone swarm degenerates into a single VIO positioning system.
[0150] In an embodiment of the present invention, the active loop detection strategy is:
[0151] Maintaining the path points of the independent UAV through the KD tree, and scoring the fitness of each path point as an active closed-loop path point to obtain a fitness value result;
[0152] Select the path point with the smallest cost from the current position of the independent UAV according to the fitness value result and the KD tree, and select the optimal path point;
[0153] The cost between the path of the independent UAV active closed-loop detection and the optimal path point is calculated to correct the accumulated drift error of the independent UAV.
[0154] Specifically, when multiple drones perform tasks such as autonomous search, the distance between drones becomes longer and high-precision relative positioning estimation is no longer possible, and the drones will degenerate into single VIO positioning. And because drones rarely pass by the same place repeatedly during exploration, drones rarely perform loop detection, resulting in cumulative drift.
[0155] Aiming at the problem that multiple UAVs degenerate into a single VIO and cannot effectively perform loop closure detection, long-term operation will produce cumulative drift. This paper proposes an active loop closure detection method to correct the cumulative drift error and ensure the global consistency of UAVs.
[0156] The specific steps of the active loop detection strategy are as follows:
[0157] Use KD tree to maintain historical path points and score each path point, such as Figure 8 As shown, when the drone is at a multi-pathway path point, it is easier to form a closed loop, and there are many landmarks and rich feature points near the multi-pathway, so it is more suitable as a candidate path point for active closed loop. The fitness value function of the active closed loop path point proposed by the present invention is as follows:
[0158] F(p_loop i .x i )=ln(p_loop i .x i +1)*w_d
[0159] where p_loop i .x i For the path point p_loop i The corresponding number of paths, w_d is the weight value, F(x i ) is a fitness value of an active closed-loop path point, indicating the fitness of the current path point as an active closed-loop path point.
[0160] By finding the path point with the minimum cost from the current position in the KD tree as the active loop closure candidate point:
[0161]
[0162] Where c(·) is the path cost between two points, w_c is the weight of the path cost, and the optimal active loop closure candidate point is related to the fitness value of the active loop closure and the path cost between the two points. By weighing the two, the optimal path point is found as the active loop closure point.
[0163] At the same time, in order to balance the efficiency of active loop closure detection and exploration, it is necessary to calculate and compare the cost between the path containing active loop closure detection and the optimal path:
[0164] C_loop=c(p_current,p_loop best )+c(p_loop best ,p_goal)
[0165] C_plan=c(p_start,p_goal)
[0166] Where C_loop is the path cost including active loop closure detection, and C_plan is the cost without active loop closure detection.
[0167] C_loop≤(1+ω cost )*C_plan
[0168] Since active loop closure usually takes a detour and reduces exploration efficiency, ω cost Represents the trade-off between exploration efficiency and global consistency. ω cost A larger weight means that more loop closure cost needs to be tolerated to correct the accumulated drift error of the UAV and maintain a more globally consistent map.
[0169] The active loop detection strategy proposed in the present invention can ensure that a single UAV (i.e., an independent UAV) can achieve good positioning in a complex environment. This will provide a good initial value for the encounter of the UAV group, achieve a coarse-to-fine positioning effect, and effectively ensure the global consistency of multiple UAVs.
[0170] Compared with the prior art, the present invention uses a fisheye camera and a UWB module to make the drone swarm not limited by the field of view, and can still accurately estimate the relative state in the absence of rich common environmental features; adopts a distributed mutual loop detection method to avoid the traditional centralized dependence on the ground station, and each drone can independently run self-state estimation and relative state estimation, and will not crash due to the exit of a certain drone; the outlier removal module can effectively remove the outliers measured by the UWB module, thereby making the UWB measurement more accurate and avoiding the decrease in collaborative positioning accuracy or even estimation errors; the mechanism of key frame redundancy value calculation is used to limit the computing resources used by the drone. By calculating the redundancy value of each key frame, the key frame with the highest redundancy value can be deleted, and the earlier key frames can be effectively retained, making the map more sparse, which can also lead to less drift and better global consistency; in view of the problem that when the drone swarm degenerates into a single VIO, long-term operation will produce cumulative drift, and an active loop detection method is proposed to eliminate the drift error of the drone. This method can ensure that a single UAV can maintain a good positioning in a complex environment, which will provide a good initial pose value for relative state estimation after the UAV group meets, achieve a coarse-to-fine positioning effect, and effectively ensure the global consistency of multiple UAVs.
[0171] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.
[0172] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only the preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation manner. The above-mentioned specific implementation manner is only illustrative rather than restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, all of which are within the protection of the present invention.
Claims
1. A collaborative positioning method for a distributed drone swarm in a complex environment, characterized in that: The collaborative positioning method comprises the following steps: S1. Install a fisheye camera and a UWB module on each drone in the drone group; S2, obtaining a fisheye image through the fisheye camera, and preprocessing the fisheye image to obtain an optimized image; S3, extracting the visual inertial odometer in the optimized image, estimating the motion of each of the drones according to the visual inertial odometer, and obtaining a measurement model of each of the drones; S4, performing mutual loop detection on the optimized images by using a distributed loop detection method, estimating the relative position and posture of each of the drones by using the fisheye camera, and eliminating the drift error of the visual inertial odometer; S5. Measuring the relative distance between each of the drones through the UWB module; S6. Establishing a redundant key frame deletion mechanism, and deleting the key frame with the highest redundancy value among the key frames of the visual inertial odometer according to the redundant key frame deletion mechanism; wherein the key frame of the visual inertial odometer includes the optimized image and the external parameters of the fisheye camera; S7. estimating the state of the drone swarm according to the measurement model; S8, detecting each of the drones in the drone group to determine whether it can observe adjacent drones. If so, the distance between the currently detected drone and other drones is relatively close, and a close-range high-precision distributed collaborative positioning strategy is executed; if not, it indicates that the currently detected drone cannot be successfully detected or is detected to be relatively far away from other drones, and the currently detected drone is set as an independent drone for autonomous positioning through an active loopback detection strategy; The distributed loop detection method includes: A first drone is defined. When the first drone receives a key frame of the visual inertial odometer, the first drone extracts features of the key frame to obtain feature information; the feature information is integrated and packaged with the state information of the first drone to obtain a packaged key frame; and the packaged key frame is broadcasted to other drones for communication; A second UAV is defined, and a local database and a common-view database are established, wherein the local database is used to store key frames generated by the UAV itself; and the common-view database is used to store key frames received by communicating with adjacent UAVs; when the second UAV receives the encapsulated key frame, it is determined whether the serial numbers of the first UAV and the second UAV are the same, and if so, the encapsulated key frame is stored in the local database; if not, the encapsulated key frame is stored in the common-view database; Retrieving the key frame with the highest matching degree with the encapsulated key frame from the local database or the common view database as a return key frame, and outputting the return key frame to the first drone; wherein the return key frame includes a global descriptor, a landmark, and the speed and acceleration of the second drone; Performing distributed loop closure detection on the loop between the first UAV and the second UAV according to the encapsulation key frame and the return key frame; Determine whether the loops between the first UAV and the second UAV are consistent; if not, remove the loops as outliers; The active loop detection strategy is: Maintaining the path points of the independent UAV through the KD tree, and scoring the fitness of each path point as an active closed-loop path point to obtain a fitness value result; Select the path point with the smallest cost from the current position of the independent UAV according to the fitness value result and the KD tree, and select the optimal path point; The cost between the path of the independent UAV active closed-loop detection and the optimal path point is calculated to correct the accumulated drift error of the independent UAV.
2. The method for collaborative positioning of a distributed drone group in a complex environment as claimed in claim 1, characterized in that: In step S2, the process of preprocessing the fisheye image includes the following steps: S21, converting the fisheye image according to a cylindrical projection model to obtain the optimized image; The cylindrical projection model satisfies the following calculation formula: in, represents the radial distance of the cylinder, represents the azimuth, and represent the focal lengths along the X-axis and Y-axis respectively, and Indicates the main point; and represents the coordinates of the projection point on the fisheye image; X, Y and Z represent the coordinate values of the X-axis, Y-axis and Z-axis respectively.
3. The method for collaborative positioning of a distributed drone group in a complex environment as claimed in claim 1, characterized in that: The measurement model satisfies the following relationship: in, represents the relative position of UAV i from time t-1 to time t, represents the four-degree-of-freedom position of UAV i at time t, represents Gaussian noise.
4. The method for collaborative positioning of a distributed UAV swarm in a complex environment as claimed in claim 1, characterized in that: The UWB module measures the relative distance between each of the drones to satisfy the following calculation formula: in, represents the distance measurement of drone i to drone j at time t, Indicates the status of the drone The translation part of represents Gaussian noise, Indicates the measurement error under non-line-of-sight conditions.
5. The method for collaborative positioning of a distributed UAV swarm in a complex environment as claimed in claim 4, characterized in that: The drone also includes an outlier elimination module, which is used to eliminate measurement errors generated when the UWB module measures the relative distance between each of the drones.
6. The method for collaborative positioning of a distributed drone group in a complex environment as claimed in claim 1, characterized in that: The redundancy value of the key frame of the visual inertial odometry satisfies the following calculation formula: Where i represents the sequence number of key frame F, Indicates a key frame The set of all observed map points, j represents the key frame The ordinal number of the observed map point, Represents a key frame The observed map point with sequence number j.
Citation Information
Patent Citations
Unmanned aerial vehicle vision-inertia fusion indoor positioning method
CN111024066A
5G security rescue networked unmanned aerial vehicle cloud side cooperative control method
CN112581590A
