Unmanned aerial vehicle cluster distributed cooperative positioning method assisted by ground laser radar point cloud

Abstract

The invention discloses an unmanned aerial vehicle cluster distributed cooperative positioning method assisted by ground laser radar point cloud. The method comprises the following steps: an unmanned aerial vehicle performs initial pose estimation through a monocular camera and an IMU (Inertial Measurement Unit); the unmanned vehicle constructs a dense point cloud map by adopting a Fast-LIO algorithm through laser radar and IMU data; constructing a factor graph optimization problem by combining sparse and dense point cloud data; a factor graph is formed, each unmanned aerial vehicle receives environment information, and six-degree-of-freedom pose estimation is achieved; the unmanned aerial vehicle independent operation pose estimation module uses a BA algorithm to accurately solve the pose; and information synchronization between the unmanned aerial vehicle and the unmanned vehicle is realized. According to the invention, unmanned aerial vehicle cluster-unmanned vehicle distributed non-prior real-time pose estimation in an unstructured environment is realized, the distributed architecture enables multiple unmanned aerial vehicles to respectively solve respective real-time poses on the basis of sharing the same laser radar map, and construction of a visual odometer factor graph fused with laser radar point cloud information is realized. The drifting problem of the vision-inertia odometer in a long track can be reduced.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a distributed collaborative positioning method for a cluster of unmanned aerial vehicles (UAVs) assisted by ground laser radar point clouds. Background Art

[0002] Monocular visual inertial odometry (VIO) technology is a comprehensive technology that combines monocular camera images with inertial measurement unit (IMU) data to achieve real-time positioning and attitude estimation. It can help drones and robots to move autonomously and complete tasks without human intervention in complex environments without GPS signals. Monocular VIO can significantly improve the autonomy and intelligence level of mobile devices. The construction of a monocular VIO system depends on efficient image processing algorithms and accurate inertial data fusion. It needs to complete functions such as environmental perception, data synchronization, and trajectory estimation, and achieve cross-platform versatility and real-time performance as much as possible.

[0003] Simultaneous Localization and Mapping (SLAM) is a technology that uses sensors (such as LiDAR, cameras, IMU, etc.) to estimate the position of a robot or device in an unknown environment in real time and build a map of the environment. SLAM technology enables robots to navigate autonomously without relying on pre-built maps, effectively respond to dynamic and complex environmental changes, and is widely used in unmanned driving, drones, service robots and other fields.

[0004] Factor graph optimization is a method that models multivariate problems as graph structures and solves them through global optimization. It effectively solves large-scale optimization problems in robot localization and mapping (SLAM) by representing poses and observations as nodes and constraints as edges to form a factor graph. Factor graph optimization can handle complex multi-dimensional data relationships, improve the accuracy and robustness of estimates, and is widely used in visual SLAM, laser SLAM and other fields.

[0005] Distributed pose estimation is a common method used in robot swarms, which aims to achieve pose estimation and data sharing between multiple robots without centralized control. In this method, each robot processes local sensor data independently and exchanges information with other robots through limited communication to achieve globally consistent pose estimation. This distributed architecture improves the robustness and scalability of the system, and the entire system can still work properly even if some robots fail. At the same time, this method reduces the dependence on centralized computing resources, making the system more flexible and adaptable in dynamic and complex environments.

[0006] A vision-based collaborative simultaneous localization and mapping (CSLAM) method realizes environmental perception and map construction by sharing visual information among multiple robots. The method uses the camera carried by each robot to obtain environmental images, and independently extracts image feature points and constructs local maps through the visual SLAM algorithm. Then, the robots share these feature points and map data through a wireless communication network, and collaborative map construction and pose estimation are performed through a global optimization algorithm. First, each robot extracts feature points from the image and generates a local map using a visual SLAM algorithm; then, these data are shared with other robots through wireless communication, and a global optimization algorithm is used to integrate multiple local maps to obtain a consistent global map. This method improves the environmental perception ability and map construction accuracy of the multi-robot system, significantly improves the computational efficiency and positioning accuracy, and has good adaptability and stability in complex and dynamic environments. However, since each robot independently constructs maps and extracts feature points, the initial error may be amplified during the global optimization process, and precise communication and synchronization mechanisms are required to ensure overall performance. The vision-based collaborative simultaneous localization and mapping (CSLAM) method usually faces the problems of error accumulation and drift when the trajectory is long, and it is difficult to obtain the absolute pose in the environment.

[0007] A collaborative simultaneous localization and mapping (CSLAM) method based on GPS or RTK achieves environmental perception and map construction by sharing high-precision positioning information among multiple robots. The GPS or RTK module equipped on each robot can obtain accurate location information, and then these positioning data and map information are shared among robots through a wireless communication network. Collaborative map construction and pose estimation rely on global optimization algorithms. Specifically, first, each robot uses its GPS or RTK module to obtain high-precision location information and generate a local map; then, these data are shared with other robots through wireless communication, and multiple local maps are integrated through a global optimization algorithm to construct a global map. The collaborative simultaneous localization and mapping (CSLAM) method based on GPS or RTK is highly dependent on GPS or RTK signals. This method cannot work properly in complex outdoor environments or indoor environments where GPS signals are poor.

[0008] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0009] The main purpose of the present invention is to solve the problems existing in the above-mentioned background technology and to provide a distributed collaborative positioning method for drone clusters assisted by ground laser radar point cloud.

[0010] To achieve the above object, the present invention adopts the following technical solutions:

[0011] In a first aspect of the present invention, a distributed collaborative positioning method for a drone cluster using a ground-based laser radar point cloud as an aid comprises the following steps:

[0012] Step 1: Use the monocular camera and IMU on the drone to extract and track visual feature points and combine the pre-integrated data of the IMU to estimate the initial pose of the drone.

[0013] Step 2: The ground unmanned vehicle obtains point cloud data of the surrounding environment in real time through the lidar, and combines it with the IMU data, using the Fast-LIO algorithm, calculating the point-to-surface residual as the system observation, and using the iterative error state Kalman filter IESKF to build the unmanned vehicle's pose estimation and dense 3D point cloud map in real time;

[0014] Step 3: Combining the sparse feature point cloud of the UAV and the dense point cloud data of the unmanned vehicle, a factor graph optimization problem is constructed to solve the variables to be optimized, including relative pose variables, camera-related variables, and visual feature variables, so as to achieve real-time relative pose estimation of each UAV in the UAV cluster;

[0015] Step 4: Use the point cloud residual factor based on point-to-surface matching, the visual feature point reprojection residual factor and the IMU pre-integration residual factor to form a factor graph of the UAV cluster system. Each UAV receives the environmental information constructed by the lidar and publishes the pose information to achieve real-time six-degree-of-freedom pose estimation based on the shared lidar map.

[0016] Step 5: In the UAV cluster, each UAV independently runs the distributed pose estimation module, uses hierarchical BA to align the input lidar dense point cloud and the camera sparse feature point cloud and calculate the residual. By minimizing the residual in the factor graph, the real-time relative pose between each UAV in the UAV cluster can be accurately solved;

[0017] Step 6: Real-time synchronization of the position information and environmental map data between the drone cluster and the unmanned vehicle.

[0018] In a second aspect of the present invention, a computer-readable storage medium stores a computer program, characterized in that when the computer program is executed by a processor, the distributed collaborative positioning method of a drone cluster assisted by a ground lidar point cloud is implemented.

[0019] In a third aspect of the present invention, a computer program product includes a computer program, which, when executed by a processor, implements the distributed collaborative positioning method of a drone cluster assisted by a ground lidar point cloud.

[0020] The present invention has the following beneficial effects:

[0021] The present invention provides an innovative distributed collaborative positioning method for drone clusters, which effectively combines ground-based laser radar point clouds with monocular cameras and IMU data carried by drones, and realizes robust and real-time pose estimation in unstructured environments. By constructing point cloud residual factors based on point-to-surface matching, visual feature point reprojection residual factors, and IMU pre-integration residual factors, the present invention optimizes the factor graph and improves the accuracy of pose estimation, reducing the drift problem in long trajectories. In addition, the distributed architecture of the present invention allows multiple drones to share the same laser radar map and independently solve the real-time pose, without relying on prior environmental map information, and can quickly and robustly estimate the initial pose, and use environmental information to correct the pose estimation drift error of the visual-inertial odometer in real time. This method is particularly suitable for scenarios such as post-disaster search and rescue, large-area map coverage exploration, and complex terrain collaborative navigation. It can significantly improve the efficiency and effectiveness of task execution, and achieve more comprehensive coverage of the environment and more efficient search. At the same time, the present invention solves the error accumulation and drift problems faced by traditional vision-based CSLAM methods when the trajectory is long, as well as the limitations of GPS- or RTK-based CSLAM methods in environments with poor GPS signals.

[0022] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 The factor graph of the drone cluster system according to the embodiment of the present invention is

[0024] Figure 2 This is a diagram showing an unmanned vehicle platform according to an embodiment of the present invention.

[0025] Figure 3 This is a diagram showing a UAV platform according to an embodiment of the present invention.

[0026] Figure 4 This is a diagram showing the effect of distributed pose estimation of a drone cluster according to an embodiment of the present invention. DETAILED DESCRIPTION

[0027] The following is a detailed description of the embodiments of the present invention. It should be emphasized that the following description is only exemplary and is not intended to limit the scope and application of the present invention.

[0028] Abbreviations and key terms:

[0029] Simultaneous Localization And Mapping (SLAM)

[0030] Visual-Inertial Odometry (VIO)

[0031] Bundle Adjustment (BA)

[0032] Light Detection and Ranging (LiDAR)

[0033] Decentralized Pose Estimation

[0034] Robot Operating System (ROS)

[0035] The embodiment of the present invention provides a method for distributed collaborative positioning of a drone cluster using a ground laser radar point cloud, comprising the following steps:

[0036] Step 1: Initialize pose estimation: Use the monocular camera and IMU carried by the drone to extract and track visual feature points, combined with the pre-integrated data of the IMU, to achieve the initial pose estimation of the drone; in this step, each unit of the drone cluster operates independently, providing initial conditions for achieving the real-time and accuracy of the distributed collaborative positioning algorithm.

[0037] Step 2: Implementation of the LiDAR-Inertial Odometry (LIO) module: The ground unmanned vehicle obtains point cloud data of the surrounding environment in real time through LiDAR, and combines it with IMU data. It uses the Fast-LIO algorithm to calculate the point-to-surface residual as the system observation and uses the Iterative Error State Kalman Filter (IESKF) to complete the unmanned vehicle's pose estimation and real-time construction of a dense 3D point cloud map.

[0038] In a preferred embodiment, step two specifically includes: using the laser radar carried by the ground unmanned vehicle to perform real-time detection of the surrounding environment and obtain continuous point cloud data; combining IMU data, real-time processing and fusing the point cloud data obtained by the laser radar, and using the Fast-LIO algorithm to calculate the point-to-surface residual; using the iterative error state Kalman filter (IESKF) technology to estimate the system position and attitude and other states, thereby building and maintaining a global map; using the global map as the basis for point cloud registration, inputting it into the relative pose estimation module, and providing a shared and accurate environmental model for the drone cluster.

[0039] Step 3: Construct a distributed pose estimation module: Combine the sparse feature point cloud of the UAV and the dense point cloud data of the unmanned vehicle to construct a factor graph optimization problem to solve the variables to be optimized, including relative pose variables, camera-related variables, and visual feature variables, so as to realize real-time relative pose estimation of each UAV in the UAV cluster.

[0040] In a preferred embodiment, in step three, a factor graph optimization problem is constructed for each drone to solve the variables to be optimized, specifically including: Definition of relative pose variables: Define pose variables relative to the lidar coordinate system for each drone, including translation and rotation, to determine the direction and position of the drone in space. Definition of camera-related variables: Define variables related to the camera, including the rotation and translation of the drone's current coordinate system relative to the initial coordinate system, as well as the speed in this coordinate system and the bias parameters of the sensor. Definition of visual feature variables: Determine the inverse depth of visual feature points, which are used for matching and tracking features in camera images. Factor graph construction: Based on the variables defined above, a factor graph is constructed, which represents the observation relationship and constraints between the drones in the drone cluster. Solving the optimization problem: Using the factor graph, an optimization problem is constructed and solved for each drone to minimize the observation error and optimize the pose estimation.

[0041] Step 4: Construction of factor graph optimization problem and real-time pose estimation: Define the point cloud residual factor based on point-to-surface matching, the visual feature point reprojection residual factor and the IMU pre-integration residual factor to form a factor graph of the UAV cluster system; and use the Ceres open source library, each UAV receives the environmental information constructed by the lidar and publishes the pose information to achieve real-time six-degree-of-freedom pose estimation based on the shared lidar map.

[0042] In a preferred embodiment, in step 4, the factor graph optimization problem is constructed and solved, which specifically includes: the introduction of environmental point cloud information: using the environmental point cloud information obtained by the laser radar carried by the ground unmanned vehicle to provide accurate spatial scale information for the drone cluster; the construction of the factor graph optimization problem: for each drone in the cluster, a factor graph optimization problem containing point cloud residuals is constructed, which takes into account the geometric relationship between the drone and the environmental point cloud; constructing a point cloud optimization problem based on point-surface matching. The cloud residual factor is used to quantify the difference between the feature points observed by the drone and the lidar point cloud surface. difference; construct visual feature point reprojection residual factors to calculate the difference between feature points observed by the camera and the projection points obtained by UAV pose estimation; construct IMU pre-integration residual factors to evaluate the degree of match between IMU data and UAV pose estimation; solve the optimization problem: solve the pose estimation problem of each UAV by minimizing the weighted sum of the above residual factors to achieve accurate real-time pose estimation.

[0043] The implementation of the point cloud residual factor based on point-to-plane matching specifically includes: calculation of point-to-plane residual: for each camera feature point, calculate the residual of its theoretical projection point to the corresponding laser radar local plane, which reflects the deviation between the camera observation and the laser radar data; construction of residual expression: construct a residual expression, which combines the observation of the camera feature point with the parameters of the laser radar local plane to form a residual term for point cloud matching; determination of the Jacobian matrix: determine the Jacobian matrix of the above residual expression, which describes the sensitivity of the residual to each optimization variable and is used to guide the optimization algorithm to adjust the posture parameters of the drone; adjustment of optimization variables: use the Jacobian matrix to adjust the relative posture of the camera to the laser radar during the optimization process, including rotation and translation, and the inverse depth of the feature point, to minimize the point cloud residual; iterative optimization process: by iteratively solving the optimization problem, gradually reduce the point cloud residual until the predetermined accuracy requirement is met to ensure the accuracy of the drone posture estimation.

[0044] The realization of the reprojection residual factor of the visual feature point specifically includes: calculation of the reprojection error: calculating the difference between the projection point from the world coordinate system to the camera sensor plane and the actually observed pixel point to obtain the reprojection error; application of the feature point projection model: using the camera model to project the feature point in the world coordinate system onto the camera sensor plane to form a predicted pixel position; comparison of observed pixels and predicted pixels: comparing the pixel point actually observed by the camera with the pixel position predicted according to the camera model to determine the deviation between the two; construction of the residual expression: constructing a residual expression to quantify the difference between the observed pixel position and the predicted pixel position, and the residual expression is used for subsequent posture optimization; optimization and adjustment of posture parameters: using the residual expression to adjust the posture parameters of the drone in the optimization algorithm, including rotation and translation, to minimize the reprojection error; iterative optimization process: by iteratively solving the residual minimization problem, the posture parameters are gradually optimized until the predetermined accuracy requirements are met, thereby ensuring the accuracy and reliability of the posture estimation.

[0045] The implementation of the IMU pre-integration residual factor specifically includes: calculation of pre-integration observation values: based on the measurement data of the IMU, calculate the pre-integration observation values ​​of the position, velocity, angle, acceleration and angular velocity bias; determination of actual observation values: determine the position, velocity, angle, acceleration and angular velocity bias values ​​actually observed by the IMU; residual calculation: calculate the difference between the pre-integration observation value and the actual observation value to obtain the IMU pre-integration residual; construction of the residual expression: construct a residual table DA This expression quantifies the deviation between the estimated value and the actual value during the IMU pre-integration process;Optimization of posture and bias parameters: Use residual expressions to adjust the posture parameters of the drone and the bias parameters of the IMU in the optimization algorithm to minimize the IMU pre-integration residual; Iterative optimization process: By iteratively solving the residual minimization problem, gradually optimize the posture and bias parameters until the predetermined accuracy requirements are met.

[0046] Step 5: Update and correct real-time pose estimation: In the UAV cluster, each UAV independently runs the distributed pose estimation module, uses hierarchical BA to register the input lidar dense point cloud and camera sparse feature point cloud and calculate the residual, and achieves accurate solution of the real-time relative pose between each UAV in the UAV cluster by minimizing the residual in the factor graph;

[0047] In a preferred embodiment, step five specifically includes: point cloud registration: receiving the dense point cloud data of the lidar provided by the ground unmanned vehicle and the sparse feature point cloud data captured by the camera mounted on the drone, and performing a registration operation to align the two sets of data sets; residual calculation: based on the registered point cloud data, calculating the residual between the camera sparse feature point cloud and the lidar dense point cloud; hierarchical BA application: using a hierarchical bundle adjustment (BA) algorithm to adjust the pose estimation of the drone according to the residual to minimize the observation error; distributed independent operation: ensuring that each drone independently runs its pose estimation module in the drone cluster, realizing distributed computing of the algorithm, and improving the robustness and efficiency of the system.

[0048] Step 6: Communication and data synchronization: Through the Topic communication mechanism of the ROS system, the real-time synchronization of the posture information and environmental map data between the drone cluster and the unmanned vehicle is achieved, ensuring the distributed independent operation of the algorithm and the accuracy of the real-time relative posture estimation.

[0049] The distributed collaborative positioning method of drone clusters assisted by ground-based laser radar point cloud provided in an embodiment of the present invention realizes distributed real-time pose estimation of drone clusters and unmanned vehicles without a priori in an unstructured environment. The distributed architecture enables multiple drones to solve their respective real-time poses based on sharing the same laser radar map. By constructing a visual odometry factor graph that integrates laser radar point cloud information, the drift problem of the visual-inertial odometry in long trajectories can be reduced.

[0050] Drones have high-speed, flexible and agile mobility but poor load-bearing capacity, while unmanned vehicles have large load capacity but very limited field of view and speed. The method of the present invention can be used for complex scenarios such as post-disaster search and rescue, large-area map coverage exploration, and complex terrain collaborative navigation. It can utilize the coordinated cooperation of drone clusters composed of multiple drones and unmanned vehicles, and utilize the complementary advantages of the two to greatly improve the efficiency and effectiveness of task execution. A drone cluster composed of multiple drones can achieve more comprehensive coverage of the environment and more efficient search. For a drone cluster composed of multiple drones, a distributed and decentralized pose estimation algorithm is the basis for the system to perform tasks, that is, using the unmanned vehicle to carry a laser radar to build an environmental point cloud map and estimate the real-time trajectories of multiple drones in the cluster.

[0051] The present invention can realize robust and real-time relative positioning of drone clusters and unmanned vehicles without relying on prior environmental map information. Multiple drones can quickly and robustly estimate the initial pose in the location environment, and use environmental information to correct the pose estimation drift error of the visual-inertial odometer in real time, thereby realizing distributed and decentralized simultaneous multi-drone pose estimation.

[0052] The specific embodiments of the present invention and algorithm examples thereof are further described below.

[0053] System composition

[0054] The algorithm consists of two modules: real-time relative pose estimation of distributed drone clusters and lidar-inertial odometer (LIO). Each module runs in the form of a function package in the ubuntu20.04 (Linux operating system mainly for desktop applications) operating system and ROS-noetic (open source robot operating system) software environment. This is a general drone cluster relative pose estimation system based on lidar-vision fusion, so it can be used on various commonly used robot platforms, including: four-wheel drive, Ackerman wheel, crawler and other commonly used ground unmanned vehicle platforms, as well as common aerial robot platforms such as quadcopters and fixed-wing aircraft.

[0055] The ground unmanned vehicle uses LiDAR to detect the surrounding environment and return real-time point cloud data. The LiDAR-Inertial Odometer module will combine the data returned by the IMU and the point cloud data returned by the LiDAR at each moment to build a global map, and input it into the relative pose estimation module as the basis for point cloud registration.

[0056] The distributed UAV cluster real-time relative pose estimation module first aligns the input lidar dense point cloud and camera sparse feature point cloud and calculates the residual, and then uses hierarchical BA to estimate the relative pose of the two in real time. The algorithm can be distributed and run independently on each UAV in the cluster.

[0057] Actual machine experiments have demonstrated the effectiveness of real-time relative pose estimation of distributed UAV swarms.

[0058] LiDAR-Inertial Odometer Module

[0059] The internal implementation of the LiDAR-Inertial Odometer module uses the open source Fast-LIO algorithm, and is implemented with reference to the basic principles of the algorithm. The algorithm uses LiDAR and IMU as system inputs, calculates point-to-surface residuals as system observations, and uses the Iterative Error State Kalman Filter (IESKF) to estimate the system position and other states, and can obtain the robot's own posture and the dense three-dimensional point cloud map of the environment in real time.

[0060] Distributed UAV swarm real-time relative pose estimation module

[0061] In the real-time relative pose estimation problem between drone swarm and unmanned vehicle, a factor graph optimization problem is constructed for each drone to solve the following variables to be optimized:

[0062]

[0063]

[0064]

[0065]

[0066] Where i = 1:N Drone is the number of each drone in the cluster. The above variables to be optimized are mainly divided into three categories: one is the relative pose variable, including the relative translation t between the camera coordinate system and the lidar coordinate system LC and relative rotation R LC ; The second is camera-related variables, including the rotation between the current camera coordinate system and the initial camera coordinate system and pan The speed of the current camera coordinate system relative to the initial camera coordinate system Acceleration measurement bias b of the inertial measurement unit a and angular velocity measurement bias b g ; The third is the visual feature variable, including the inverse depth λ of all visual feature points.

[0067] The environmental point cloud information provided by the laser radar is introduced into the drone cluster, and the following factor graph optimization problem including the point cloud residual is constructed for each drone in the cluster:

[0068]

[0069] There are three factors: point cloud residual factor based on point-to-surface matching, visual feature point reprojection residual factor, and IMU pre-integration residual factor. Figure 1 As shown in the figure, multiple drones share the same lidar point cloud map to estimate their relative pose:

[0070] (1) Point cloud residual factor based on point-to-surface matching:

[0071] The point-to-plane residual from a specific camera feature point to the corresponding lidar local plane is expressed as:

[0072]

[0073] The Jacobian matrix of this residual is:

[0074]

[0075] (2) Visual feature point reprojection residual factor:

[0076] The residual term of the visual camera is the reprojection error, that is, the difference between the landmark point in the world coordinate system, which is projected onto the sensor plane through the camera model, and the point obtained and the actually observed pixel point.

[0077]

[0078]

[0079] (3) IMU pre-integration residual factor:

[0080] The pre-integration factors of the IMU include the difference between the observed and actual values ​​of position, velocity, angle, acceleration, and angular velocity bias:

[0081]

[0082] In the above formula, the left side of the difference is the estimated relative pre-integration value, and the right side of the difference is the pre-integration value observed by the actual system. The difference between the two constitutes the residual term of IMU pre-integration.

[0083] After the factor graph model is built, the Ceres open source library is used to solve the optimization problem. Each drone receives the environmental information constructed by the lidar in the format of point cloud ( / PointCloud2), and publishes the robot's pose information ( / tf). The pose estimation of the drone cluster is based on the multi-drone factor graph model built above, and the real-time six-degree-of-freedom pose (Global Pose) of each drone in the environmental point cloud map is calculated. The unmanned vehicle and the drone communicate using Topic (a communication mechanism in the ROS operating system) in the ROS system, and use WIFI as the physical communication medium to run synchronously in real time.

[0084] Experimental platform and experimental data

[0085] The drone cluster-unmanned vehicle system adopts Figure 2 and Figure 3 The ground unmanned vehicle is equipped with an IMU and a lidar, and is equipped with an Intel Core i7-4790K CPU, 32GB RAM-based industrial PC as computing unit. The machine is installed with the Ubuntu 20.04 operating system and ROS Noetic. The unmanned vehicle acquires the measurement data of the LiDAR and IMU in real time to build an incremental point cloud map through the LiDAR-Inertial Odometer. The drone uses the Intel RealSense D435i camera to collect synchronized image and IMU data, processes the image and IMU data through the onboard Nvidia Jetson NX, and transmits them to the unmanned vehicle in real time through WiFi. The marker ball of the motion capture system is installed on the drone for visual tracking of the motion capture system to provide the true value data of the drone's trajectory and posture.

[0086] In order to verify the effect of the real-time pose estimation algorithm of the drone cluster-unmanned vehicle of the present invention, the unmanned vehicle and the drone are operated simultaneously in a room (35×25×12m) equipped with a motion capture system (FZMotion). The dynamic lidar point cloud map is constructed by lidar-inertial odometer increments. In this scenario, multiple drones are controlled by operators to operate with different trajectories. A rough initial pose is provided for each drone entering the point cloud map, and then the accurate initial value is estimated by the initialization algorithm. Each drone enters and exits the LiDAR map at different times, which are unknown in advance. The pose estimation of all drones is performed in the same lidar point cloud map.

[0087] For each drone trajectory, the proposed method is compared with the results estimated by the open source algorithm VINS and the ground truth data. Figure 4As shown in the lower part of , since VINS cannot be positioned in the lidar point cloud map and provide it with an accurate initial pose, only the trajectory drift and error accumulation caused by the visual odometer recursion are considered. In the case of severe image jitter and blur, the monocular VIO algorithm may produce serious scale estimation errors and trajectory drift. These scale estimation errors and drift error accumulation can be corrected in real time through the absolute spatial scale information provided by the lidar point cloud. Experiments show that the present invention can still work in real time and ensure effectiveness in the collaborative scenario of "3 drones + 1 unmanned vehicle".

[0088] The present invention proposes a distributed positioning algorithm for a swarm of unmanned aerial vehicles (UAVs) in a three-dimensional point cloud map of the environment, and proposes a factor graph design and construction strategy for the positioning of a swarm of unmanned aerial vehicles (UAVs).

[0089] The main technical advantages of the present invention compared with the traditional method are:

[0090] 1. The distributed real-time pose estimation of drone swarms and unmanned vehicles in unstructured environments is realized. The distributed architecture enables multiple drones to solve their own real-time poses based on sharing the same lidar map.

[0091] 2. By integrating the visual odometry factor graph with lidar point cloud information, the drift problem of visual-inertial odometry in long trajectories can be reduced.

[0092] An embodiment of the present invention further provides a storage medium for storing a computer program, which at least performs the above method when executed.

[0093] An embodiment of the present invention further provides a control device, comprising a processor and a storage medium for storing a computer program; wherein the processor is configured to execute at least the method described above when executing the computer program.

[0094] An embodiment of the present invention further provides a processor, wherein the processor executes a computer program and at least executes the method described above.

[0095] The storage medium can be implemented by any type of non-volatile storage device, or a combination thereof. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a magnetic random access memory (FRAM), a ferromagnetic random access memory (Flash Memory), a magnetic surface memory, an optical disc, or a compact disc read-only memory (CD-ROM); the magnetic surface memory can be a disk memory or a tape memory. The storage medium described in the embodiments of the present invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0096] In the several embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as: multiple units or components can be combined, or can be integrated into another system, or some features can be ignored or not executed. In addition, the coupling, direct coupling, or communication connection between the components shown or discussed can be through some interfaces, and the indirect coupling or communication connection of the devices or units can be electrical, mechanical or other forms.

[0097] The units described above as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units; some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0098] In addition, all functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may be separately used as a unit, or two or more units may be integrated into one unit; the above-mentioned integrated units may be implemented in the form of hardware or in the form of hardware plus software functional units.

[0099] It will be understood by those skilled in the art that all or part of the steps of the above method embodiments can be completed by hardware associated with program instructions, and the above program can be stored in a computer. The program can read the storage medium When executed, the steps including the above method embodiment are performed; The aforementioned storage media include: mobile storage devices, read-only memory (ROM), random access memory (RAM), magnetic disks or optical disks, and other media that can store program codes.

[0100] Alternatively, if the above-mentioned integrated unit of the present invention is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as mobile storage devices, ROM, RAM, magnetic disks or optical disks.

[0101] The methods disclosed in the several method embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments.

[0102] The features disclosed in several product embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new product embodiments.

[0103] The features disclosed in several method or device embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments or device embodiments.

[0104] The above contents are further detailed descriptions of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art of the present invention, several equivalent substitutions or obvious variations can be made without departing from the concept of the present invention, and the performance or use is the same, which should be regarded as belonging to the protection scope of the present invention.

Claims

1. A distributed collaborative positioning method for UAV clusters assisted by ground-based laser radar point cloud. It is characterized in that The following steps are involved: Step 1: Use the monocular camera and IMU on the drone to extract and track visual feature points and combine the pre-integrated data of the IMU to estimate the initial pose of the drone. Step 2: The ground unmanned vehicle obtains point cloud data of the surrounding environment in real time through the lidar, and combines it with the IMU data, using the Fast-LIO algorithm, calculating the point-to-surface residual as the system observation, and using the iterative error state Kalman filter IESKF to build the unmanned vehicle's pose estimation and dense 3D point cloud map in real time; Step 3: Combining the sparse feature point cloud of the UAV and the dense point cloud data of the unmanned vehicle, a factor graph optimization problem is constructed to solve the variables to be optimized, including relative pose variables, camera-related variables, and visual feature variables, so as to achieve real-time relative pose estimation of each UAV in the UAV cluster; Step 4: Use the point cloud residual factor based on point-to-surface matching, the visual feature point reprojection residual factor and the IMU pre-integration residual factor to form a factor graph of the UAV cluster system. Each UAV receives the environmental information constructed by the lidar and publishes the pose information to achieve real-time six-degree-of-freedom pose estimation based on the shared lidar map. Step 5: In the UAV cluster, each UAV independently runs the distributed pose estimation module, uses hierarchical BA to align the input lidar dense point cloud and the camera sparse feature point cloud and calculate the residual. By minimizing the residual in the factor graph, the real-time relative pose between each UAV in the UAV cluster can be accurately solved; Step 6: Real-time synchronization of the position information and environmental map data between the drone cluster and the unmanned vehicle.

2. The distributed collaborative positioning method for drone clusters using ground-based laser radar point cloud as claimed in claim 1, It is characterized in that Step 2 specifically includes: Use the laser radar carried by the ground unmanned vehicle to detect the surrounding environment in real time and obtain continuous point cloud data; Combined with IMU data, the point cloud data obtained by the lidar is processed and fused in real time, and the Fast-LIO algorithm is used to calculate the point-to-surface residual; The Iterative Error State Kalman Filter (IESKF) technique is used to estimate the system position and attitude to build and maintain a global map. The global map is used as the basis for point cloud registration and input into the relative pose estimation module to provide a shared environment model for the UAV cluster.

3. The distributed collaborative positioning method for drone clusters using ground-based laser radar point cloud as claimed in claim 1 or 2, It is characterized in that In step 3, a factor graph optimization problem is constructed for each drone to solve the variables to be optimized, including: Define the pose variables for each drone relative to the lidar coordinate system, including translation and rotation, to determine the orientation and position of the drone in space; Determine camera-related variables, including the rotation and translation of the drone's current coordinate system relative to the initial coordinate system, as well as the velocity and sensor bias parameters in that coordinate system; Determining visual feature variables, including determining inverse depths of visual feature points used for matching and tracking features in camera images; Based on the above variables, a factor graph is constructed to represent the observation relationship and constraints between the drones in the drone cluster; Using the factor graph, an optimization problem is constructed and solved for each UAV to minimize the observation error and optimize the pose estimate.

4. The distributed collaborative positioning method for drone clusters using ground-based laser radar point cloud as described in any one of claims 1 to 3, It is characterized in that In step 4, the factor graph optimization problem is constructed and solved, including: The environmental point cloud information obtained by the laser radar carried by the ground unmanned vehicle provides accurate spatial scale information for the drone cluster; For each UAV in the cluster, a factor graph optimization problem containing point cloud residuals is constructed, which takes into account the geometric relationship between the UAV and the environment point cloud; Construct a point cloud residual factor based on point-to-surface matching to quantify the difference between the feature points observed by the drone and the lidar point cloud surface; Construct the visual feature point reprojection residual factor to calculate the difference between the feature points observed by the camera and the projected points estimated by the drone pose; Construct the IMU pre-integration residual factor to evaluate the matching degree between the IMU data and the UAV pose estimation; By minimizing the weighted sum of the above residual factors, the pose estimation problem of each UAV is solved to achieve accurate real-time pose estimation.

5. The distributed collaborative positioning method for drone clusters using ground laser radar point cloud as claimed in claim 4, It is characterized in that In step 4, the implementation of the point cloud residual factor based on point-to-surface matching specifically includes: For each camera feature point, calculate the residual of its theoretical projection point to the corresponding lidar local plane. This residual reflects the deviation between the camera observation and the lidar data. Construct a residual expression that combines the observations of camera feature points with the parameters of the lidar local plane to form a residual term for point cloud matching; Determine the Jacobian matrix of the residual expression, which describes the sensitivity of the residual to each optimization variable and is used to guide the optimization algorithm to adjust the posture parameters of the UAV; The Jacobian matrix is ​​used to adjust the relative pose of the camera to the lidar during the optimization process, including rotation and translation, as well as the inverse depth of the feature points to minimize the point cloud residual; By iteratively solving the optimization problem, the point cloud residual is gradually reduced until the predetermined accuracy requirement is met.

6. The distributed collaborative positioning method for drone clusters using ground laser radar point cloud as claimed in claim 4, It is characterized in that In step 4, the realization of the visual feature point reprojection residual factor specifically includes: Calculate the difference between the projection point from the world coordinate system to the camera sensor plane and the actual observed pixel point to obtain the reprojection error; Use the camera model to project feature points in the world coordinate system onto the camera's sensor plane to form predicted pixel positions; Compare the pixel positions actually observed by the camera with the pixel positions predicted by the camera model to determine the deviation between the two; Construct a residual expression to quantify the difference between the observed pixel position and the predicted pixel position for pose optimization; The residual expression is used to adjust the UAV's pose parameters, including rotation and translation, in the optimization algorithm to minimize the reprojection error. By iteratively solving the residual minimization problem, the pose parameters are gradually optimized until the predetermined accuracy requirements are met.

7. The distributed collaborative positioning method for drone clusters using ground laser radar point cloud as claimed in claim 4, It is characterized in that In step 4, the implementation of the IMU pre-integration residual factor specifically includes: Based on the IMU measurement data, calculate the pre-integrated observation values ​​of position, velocity, angle, acceleration and angular velocity bias; Determine the position, velocity, angle, acceleration and angular velocity bias value actually observed by the IMU; Calculate the difference between the pre-integrated observation value and the actual observation value to obtain the IMU pre-integrated residual; Construct a residual expression that quantifies the deviation between the estimated value and the actual value during the IMU pre-integration process; The residual expression is used to adjust the UAV's posture parameters and IMU's bias parameters in the optimization algorithm to minimize the IMU pre-integration residual; By iteratively solving the residual minimization problem, the pose and deviation parameters are gradually optimized until the predetermined accuracy requirements are met.

8. The method for distributed collaborative positioning of a drone cluster using ground-based laser radar point cloud as claimed in any one of claims 1 to 7, wherein step 5 specifically include: Receive the dense point cloud data of the lidar provided by the ground unmanned vehicle and the sparse feature point cloud data captured by the camera on the drone, and perform a registration operation to align the two sets of data sets; Based on the registered point cloud data, calculate the residual between the camera's sparse feature point cloud and the lidar's dense point cloud; A hierarchical bundle adjustment BA algorithm is used to adjust the UAV's pose estimation according to the residual to minimize the observation error. Each UAV runs its pose estimation module independently in the UAV cluster to achieve distributed computing of the algorithm.

9. A computer-readable storage medium storing a computer program, It is characterized in that When the computer program is executed by a processor, the distributed collaborative positioning method of a drone cluster using ground-based laser radar point cloud as described in any one of claims 1 to 8 is implemented.

10. A computer program product comprising a computer program, It is characterized in that When the computer program is executed by a processor, the distributed collaborative positioning method of a drone cluster using ground-based laser radar point cloud as described in any one of claims 1 to 8 is implemented.

Images