Real-time relative pose estimation method in unmanned aerial vehicle-unmanned vehicle cooperative system

Abstract

The invention discloses a real-time relative pose estimation method in an unmanned aerial vehicle-unmanned vehicle cooperative system, and the method comprises the steps: 1, achieving the real-time pose estimation of each platform through visual feature point tracking and IMU pre-integration through a monocular camera and IMU data, and constructing a sparse feature point cloud map; step 2, combining laser radar point cloud data with an IMU, and applying a Fast-LIO algorithm and an IESKF to complete dense three-dimensional point cloud map construction of each platform; and step 3, fusing the data in the step 1 and the step 2, constructing a factor graph optimization problem through point-surface matching residual error, visual reprojection residual error and IMU pre-integration residual error factors, and solving the relative pose in real time. According to the method, vision and laser radar information are integrated, the pose estimation precision and robustness are improved, the method is suitable for an unstructured environment, and the collaborative operation efficiency is enhanced.

Description

Technical Field

[0001] The present invention relates to robot and computer vision technology, and in particular to a real-time relative pose estimation method in a UAV-UAV collaborative system. 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] According to different implementation methods, pose estimation methods can be divided into filtering-based and optimization-based. The filtering-based pose estimation method processes the data of sensors (such as IMU, GPS, LiDAR) in real time, and uses technologies such as Kalman filtering or particle filtering to incrementally update and correct the pose; while the optimization-based pose estimation method uses global optimization techniques, such as factor graph optimization, to obtain higher-precision pose estimation by minimizing the reprojection error of the entire trajectory or map. These two methods have their own advantages and disadvantages. The filtering-based method is suitable for scenarios with high real-time requirements, while the optimization-based method performs better in terms of high accuracy and global consistency.

[0006] The learning-based method establishes the correspondence by learning the common feature descriptors between the image and the point cloud. This method uses an end-to-end deep network architecture to jointly learn the descriptors of 2D and 3D key points from the image and point cloud, respectively, and realizes relative pose estimation based on the matching relationship between the descriptors. First, the descriptors of 2D and 3D key points are extracted from the image and point cloud respectively using an end-to-end deep network architecture, and a matching relationship database is generated; then, the image feature points are matched with the point cloud feature points through projection and optimization algorithms to obtain relative pose estimation. This method improves the performance of image and point cloud data fusion, significantly improves the computational efficiency and accuracy, and has good adaptability and stability in complex environments. This type of method usually decomposes the image to point cloud problem into two modules, namely the 2D-3D association module and the pose estimation module. However, the errors in the 2D-3D association module will not be corrected in the subsequent pose estimation module because they are not jointly optimized for a unique goal. The learning-based method has limited generalization in unknown complex scenes, and the required computing resources are large, making it difficult to achieve real-time pose estimation. Furthermore, mismatching during 2D-3D association will cause the pose estimation results to diverge.

[0007] The matching-based relative pose estimation algorithm establishes a matching relationship between a 2D image and a 3D point cloud in a direct or indirect way, and then realizes relative pose estimation based on this matching relationship. First, in the direct method, the 3D point cloud is projected onto a 2D image, and a similarity index is used for comparison to generate a matching relationship; in the indirect method, 2D and 3D feature lines are extracted, and a matching relationship is established between the line features. Then, the relative pose of the camera is estimated by using a Kalman filter or a factor graph optimization algorithm; then, the point-to-surface residual term and Jacobian matrix are used to further optimize the pose estimation result; finally, the accuracy and robustness of matching and estimation are improved by filtering and error correction methods. This method improves the performance of image and point cloud data fusion, significantly improves the computational efficiency and accuracy, and has good adaptability and stability in complex environments. The method based on pre-extracted feature matching requires fine preprocessing of the environment map and is highly dependent on the structured information in the scene. In unstructured scenes such as the wild, problems such as insufficient features and mismatching are prone to occur.

[0008] For scenarios such as post-disaster search and rescue, large-area map coverage exploration, and complex terrain collaborative navigation, the coordinated cooperation of drones and unmanned vehicles can usually take advantage of their complementary characteristics, that is, drones have high-speed and flexible agile mobility but poor load capacity, and unmanned vehicles have large load capacity but very limited field of view and speed. In complex scenarios, the cooperation of drones and unmanned vehicles can take advantage of their complementary advantages and greatly improve the efficiency and effectiveness of mission execution. Therefore, the development of an intelligent drone-unmanned vehicle air-ground collaborative platform and the study of drone-unmanned vehicle relative positioning algorithms are of great value to practical application scenarios.

[0009] In the UAV-UAV air-ground collaboration scenario, real-time estimation of the relative pose of the UAV and the UAV without a priori environmental map and without a priori initial relative pose estimation is the basis for the collaboration between the two. Relying solely on visual-inertial odometers cannot estimate the relative pose of the UAV and the UAV, and there are problems such as trajectory estimation error accumulation and drift during operation, making it difficult to operate stably in large-scale environments or scenarios with visual feature degradation. Methods based on pre-extraction feature matching require sophisticated preprocessing of the environmental map and are highly dependent on structured information in the scene. In unstructured scenes such as the wild, problems such as insufficient features and mismatching are prone to occur. Methods based on deep learning usually lack generalization and are difficult to run in real time on robots.

[0010] Therefore, robust and real-time UAV-DRV relative positioning requires:

[0011] 1. Fast and stable solution algorithm.

[0012] 2. Does not rely on prior environmental map information.

[0013] 3. Use environmental information to correct the drift error of the visual-inertial odometry pose estimation in real time.

[0014] 4. Effectively integrate heterogeneous information from vision and lidar.

[0015] 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

[0016] The main purpose of the present invention is to solve the problems existing in the above-mentioned background technology and to provide a real-time relative pose estimation method in a UAV-UAV collaborative system.

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

[0018] A real-time relative pose estimation method in a UAV-UAV collaborative system comprises the following steps:

[0019] Step 1: Visual-Inertial Odometry (VIO): Using monocular camera images and inertial measurement unit (IMU) data, visual feature point extraction and tracking and IMU pre-integration as the front end, and factor graph optimization as the back end, the real-time pose estimation of the UAV and the unmanned vehicle is realized, and the sparse feature point cloud maps of each are constructed;

[0020] Step 2: LiDAR-Inertial Odometry (LIO): The point cloud data acquired by the LiDAR in real time is combined with the IMU data. The Fast-LIO algorithm is used to calculate the point-to-surface residual and use the Iterative Error State Kalman Filter (IESKF) to complete the real-time construction of dense 3D point cloud maps of the UAV and unmanned vehicle postures and their respective environments.

[0021] Step 3: Real-time relative pose estimation: Based on steps 1 and 2, a factor graph optimization problem is constructed, including the point-to-surface matching residual factors between the sparse feature point cloud obtained in step 1 and the dense point cloud obtained in step 2, as well as the visual feature point reprojection residual factors and the IMU pre-integration residual factors. The relative pose between the UAV and the unmanned vehicle is solved in real time by minimizing the residual.

[0022] Among them, step one provides initial pose estimation and sparse feature point cloud for step two and step three, step two provides dense point cloud data for step three, and step three integrates the data of step one and step two to achieve accurate relative pose estimation through point cloud registration and optimization algorithm.

[0023] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the real-time relative pose estimation method in the unmanned aerial vehicle-unmanned vehicle collaborative system.

[0024] A computer program product comprises a computer program, wherein when the computer program is executed by a processor, the real-time relative pose estimation method in the unmanned aerial vehicle-unmanned vehicle collaborative system is implemented.

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

[0026] The real-time relative pose estimation method in the UAV-UAV collaborative system of the present invention innovatively integrates visual-inertial odometer (VIO) and lidar-inertial odometer (LIO) technology, as well as factor graph optimization strategy, to achieve efficient collaborative operation in complex environments. This method not only improves the autonomy and intelligence level of the system, but also ensures cross-platform versatility and real-time performance by closely combining monocular camera and IMU data and utilizing efficient image processing algorithms and accurate inertial data fusion. At the same time, by introducing lidar point cloud data and combining the Fast-LIO algorithm, the present invention can generate an accurate three-dimensional environmental map, and significantly improve the accuracy and robustness of pose estimation through point cloud registration technology.

[0027] The pose estimation method of the present invention realizes the real-time pose estimation function of drone-unmanned vehicle without prior knowledge in unstructured environment, and does not rely on prior environmental map information; it realizes the construction of the optimization problem of visual odometer factor graph integrating laser radar point cloud information, solves the drift problem inherent in visual-inertial odometer, can correct the pose estimation drift error in visual-inertial odometer in real time, and effectively solves the problem of stable operation in large-scale environment or visual feature degradation scene. Its deep fusion of heterogeneous information of vision and laser radar enhances the adaptability of the system in unstructured scenes such as the wild, reduces the dependence on scene structured information, and reduces the problems caused by insufficient features or mismatching. Finally, the method provides strong technical support for the collaborative operation of drones and unmanned vehicles in scenes such as post-disaster search and rescue, large-area map coverage exploration, and complex terrain collaborative navigation through a fast and stable solution algorithm, and significantly improves the efficiency and effect of task execution.

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

[0029] Figure 1 is a factor graph including point cloud residuals according to an embodiment of the present invention. DETAILED DESCRIPTION

[0030] 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.

[0031] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0032] The embodiment of the present invention provides a real-time relative pose estimation method in a UAV-UAV collaborative system. The method is applicable to a variety of robot platforms, including but not limited to four-wheel drive, Ackerman wheel, crawler ground robot platforms, and four-rotor UAV aerial robot platforms. The method specifically comprises the following steps:

[0033] Step 1: Visual-Inertial Odometry (VIO): Using monocular camera images and inertial measurement unit (IMU) data, visual feature point extraction and tracking with IMU pre-integration as the front end, and factor graph optimization as the back end, the real-time pose estimation of the UAV and unmanned vehicle is achieved, and their respective sparse feature point cloud maps are constructed.

[0034] In step one, preprocessing of the monocular camera image and IMU data includes but is not limited to feature point extraction, tracking, and time synchronization with the IMU data.

[0035] In a preferred embodiment, step one specifically includes:

[0036] The visual feature point extraction and tracking technology is used, combined with IMU pre-integration as the front-end processing, to estimate the initial pose of UAVs and unmanned vehicles; factor graph optimization is used as the back-end processing to further optimize the pose estimation obtained by the front end to improve the accuracy and robustness of pose estimation; in the initialization stage, the structure from motion method (SfM) and sliding window method are implemented to select continuous image frames for initialization, identify stable feature tracking and sufficient parallax, and set the reference frame; epipolar geometry and 2D-2D feature point matching relationship are used to solve the pose transformation of the initial frame relative to the reference frame, and the triangulation method is used to construct a scaled 3D point cloud; the PnP method and 3D point cloud are applied to the initialization stage. The poses of other frames in the D-2D matching relationship estimate window are used to construct a 3D feature point cloud of the environment, while dealing with the problem of the monocular camera lacking absolute scale information. The point cloud scale factor is accurately estimated by combining IMU pre-integration and 3D lidar data to achieve scale correction. The angular velocity drift is corrected for the IMU pre-integration, and the velocity, gravity and scale factor are jointly optimized to construct the pre-integration residual and minimize it to obtain the optimal estimate. In the real-time pose estimation stage, a factor graph optimization problem is constructed, including variables such as camera coordinate system transformation, velocity, IMU bias parameters and inverse depth of visual feature points. The Ceres open source library is used to solve the optimization problem and obtain the real-time pose.

[0037] The implementation of the VIO module refers to open source visual-inertial odometry frameworks such as VINS to construct a sparse feature point three-dimensional point cloud map, and accurately estimates the point cloud scale through the results of IMU pre-integration.

[0038] Step 2: LiDAR-Inertial Odometry (LIO): The point cloud data acquired by the LiDAR in real time is combined with the IMU data. The Fast-LIO algorithm is used to calculate the point-to-surface residual and use the iterative error state Kalman filter (IESKF) to complete the real-time construction of dense 3D point cloud maps of the UAV and unmanned vehicle positions and their respective environments.

[0039] In step 2, the point cloud data acquired by the lidar is fused with the IMU data by calculating the point-to-surface residual and using the iterative error state Kalman filter (IESKF) for pose estimation to achieve real-time construction of a dense three-dimensional point cloud map of the environment around the drone and unmanned vehicle.

[0040] In a preferred embodiment, step 2 specifically includes:

[0041] Using LiDAR and IMU as input, the movement of the robot in the environment and the point cloud data of the surrounding environment are captured synchronously; using the point-to-surface residual calculation method, each point in the LiDAR point cloud data is matched with the local plane model to form a residual term observed by the system; using the Iterative Error State Kalman Filter (IESKF) algorithm, the system state is recursively updated, where the state includes but is not limited to the robot's position, speed, posture and IMU deviation parameters; processing the point-to-surface residual through the IESKF algorithm, the robot's posture is accurately estimated, and a dense three-dimensional point cloud map of the environment is generated in real time; the dense three-dimensional point cloud map is combined with the robot's posture information to provide an accurate environment model and posture prior for the real-time relative posture estimation in step three.

[0042] Step 3: Real-time relative pose estimation: Based on steps 1 and 2, a factor graph optimization problem is constructed, including the point-to-surface matching residual factors between the sparse feature point cloud obtained in step 1 and the dense point cloud obtained in step 2, as well as the visual feature point reprojection residual factors and the IMU pre-integration residual factors. The relative pose between the UAV and the unmanned vehicle is solved in real time by minimizing the residual.

[0043] In step three, the constructed factor graph optimization problem may include using a hierarchical Bundle Adjustment algorithm to accurately estimate the relative pose between the UAV and the unmanned vehicle, where the point-to-surface matching residual factor may be calculated using a local plane represented in the form of a Hesse normal vector.

[0044] In a preferred embodiment, step three specifically includes:

[0045] The factor graph optimization problem is constructed, involving three types of variables to be optimized: relative pose variables, camera-related variables and visual feature variables. In the relative pose variables, the relative translation and rotation between the camera coordinate system and the lidar coordinate system are defined to determine the spatial relationship between the two. The camera-related variables include the rotation, translation, velocity of the camera coordinate system relative to the initial coordinate system, as well as the acceleration and angular velocity measurement bias of the IMU. In the visual feature variables, the inverse depth of all visual feature points is involved, which is used to optimize the camera pose and the spatial position of the feature points. The point cloud information provided by the lidar is introduced to construct a point cloud residual factor based on point-to-surface matching, which is used to accurately align the lidar point cloud and the camera coordinate system. The visual feature point reprojection residual factor is defined to ensure the consistency between the observation of the feature points and the prediction of the camera model. The IMU pre-integration residual factor is defined to integrate the IMU data with the pose estimation to improve the robustness and accuracy of the system. By minimizing the weighted sum of the above residual factors, the optimization algorithm is used to solve the variables to be optimized to achieve accurate estimation of the relative pose between the drone and the unmanned vehicle.

[0046] Among them, the real-time relative pose estimation module utilizes the fusion of lidar and camera data, and through the factor graph optimization algorithm, realizes real-time and accurate estimation of the relative pose between the drone and the unmanned vehicle in a dynamic environment.

[0047] In a preferred embodiment, the construction and use of the point cloud residual factor based on point-to-plane matching described in step three include: using a point-to-plane matching method to establish a correspondence between the feature points extracted by the camera and the local planes in the laser radar point cloud; using Hesse normal vectors to represent the local planes in the point cloud, including the unit normal vector and the distance from the plane to the origin; identifying the local planes related to the feature points through the KD tree search algorithm, and fitting the normal vectors of the planes; calculating the distances from the feature points to the corresponding local planes for constructing the point cloud residual factors for quantifying the plane consistency of the feature points in the laser radar coordinate system; initializing the inverse depth of the feature points using the triangulation method, and converting the feature point coordinates to the world coordinate system of the laser radar according to the estimated relative pose; defining the point-to-plane residual for quantifying the deviation of the feature points in the laser radar coordinate system; constructing a Jacobian matrix to describe the gradient information of the point-to-plane residual relative to the pose variables and the inverse depth parameters; fusing the point-to-plane residual and the Jacobian matrix into the factor graph optimization problem for accurately adjusting the relative pose estimation.

[0048] Among them, the processing of the point cloud residual factor based on point-to-plane matching enhances the accuracy and robustness of pose estimation, especially in the presence of large amounts of data and high noise.

[0049] In a preferred embodiment, the processing steps of the visual feature point reprojection residual factor described in step three specifically include: front-end processing of image data captured by the visual camera sensor, including feature point extraction, matching and inter-frame pose estimation; using feature point method, optical flow method, direct method or deep learning-based algorithm to estimate the movement of the camera between two adjacent frames; using key point detection and descriptor algorithms, such as ORB, SIFT, SURF, BRIEF, BRISK, etc., to extract and match feature points; using algorithms such as RANSAC to remove mismatched noise data points to improve the accuracy of feature point matching; according to the camera type and scene, selecting epipolar geometry, PnP or ICP algorithm to solve the relative pose change of the camera between two frames; calculating the reprojection error, that is, the difference between the position of the landmark point in the world coordinate system after being projected by the camera model and the actual observed pixel point; defining the reprojection residual factor to quantify the consistency between the feature point observation and the camera model prediction; integrating the reprojection residual factor into the factor graph optimization problem for adjusting and optimizing the camera pose estimation.

[0050] In a preferred embodiment, the processing steps of the IMU pre-integration residual factor in step 3 specifically include: implementing the IMU motion model recursion and pre-integration to construct front-end processing, wherein the IMU provides angular velocity and acceleration data as an inertial sensor; using the IMU pre-integration technology, taking a certain frame as a reference system, integrating the IMU measurement value to avoid repeated calculations associated with the optimized state quantity; using numerical integration methods, such as Euler integration or median integration, to achieve discrete-time pre-integration of IMU data; constructing pre-integration quantities by recursion to integrate the IMU data with the visual The fusion of data provides accurate relative pose information; in the back end, IMU pre-integration factors are constructed, including the difference between the observed and actual values ​​of position, velocity, angle, acceleration, and angular velocity bias; IMU pre-integration residual terms are defined to quantify the difference between the estimated relative pre-integration values ​​and the actual system observations; IMU pre-integration residuals are integrated into the factor graph optimization problem to improve the accuracy of pose estimation and the robustness of the system; IMU pre-integration residual terms are used to correct the pose estimation to ensure the stability of the relative pose estimation of UAVs and unmanned vehicles in dynamic environments.

[0051] The specific embodiments of the present invention are further described below.

[0052] System composition

[0053] This algorithm consists of three modules: visual-inertial odometry (VIO), lidar-inertial odometry (LIO), and real-time relative pose estimation. 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 lidar-vision fusion relative pose estimation system, so it can be used on various commonly used robot platforms, including: four-wheel drive, Ackerman wheel, crawler and other commonly used ground robot platforms, as well as common aerial robot platforms such as quadcopters.

[0054] The ground robot 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. See 3.2.2 for details.

[0055] The visual-inertial odometry module uses the monocular camera image as input, combines it with the IMU data to complete real-time pose estimation and sparse feature point cloud map construction, and inputs the sparse feature point cloud into the relative pose estimation module. See 3.2.3 for details.

[0056] The real-time relative pose estimation module first registers the input lidar dense point cloud and the 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. See 3.2.4 for details.

[0057] LiDAR-Inertial Odometer Module

[0058] 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.

[0059] Visual-Inertial Odometry Module

[0060] The VIO module refers to classic open source visual-inertial odometry frameworks such as VINS, uses visual feature point extraction and tracking and IMU pre-integration as the front end, and factor graph optimization as the back end to achieve complete initialization and real-time pose estimation. Different from common visual-inertial odometry, this module focuses more on building a better sparse feature point 3D point cloud map, and a very important algorithm is to accurately estimate the point cloud scale through the results of IMU pre-integration.

[0061] In the initialization stage, the present invention implements the structure from motion method (SfM) based only on visual estimation of the initial pose and three-dimensional point cloud, adopts a sliding window method, and selects several consecutive frames of images as initialization frames for initialization. First, all initialization frames are traversed in the sliding window. When an image frame with stable feature tracking (more than 30 tracking features) and sufficient parallax (more than 20 pixels of rotation compensation) is first identified, this frame is set as the reference frame for initialization. Subsequently, the epipolar geometry is used to solve the pose transformation of all initial frames relative to the reference frame through the 2D-2D feature point matching relationship, and a scaled three-dimensional point cloud is obtained using a triangulation method (at this time, due to the inherent problems of the monocular camera, the absolute scale cannot be obtained, and the unified scale makes the relative translation between the reference frame and the last initialization frame unit length). Afterwards, the PnP method and 3D-2D matching relationship are used to estimate the pose of all other frames in the window. Triangulation and PnP algorithms can obtain the three-dimensional coordinates of the features in all image reference frames used for initialization in the reference frame, and construct a three-dimensional feature point cloud of the relevant environment. Through the above algorithm, a scaled feature point cloud and the poses of a series of cameras in the sliding window can be obtained. Given the lack of absolute scale information in the monocular visual odometer, the scale factor s of the point cloud will be combined with the IMU pre-integration and the dense point cloud of the 3D lidar for a more accurate estimate.

[0062] Perform IMU pre-integration, angular velocity drift correction, and joint optimization of vision and IMU pre-integration in the initialization phase. Set the initial camera coordinate system to: Set as the reference coordinate system for SfM and IMU pre-integration. The k-th frame pose can be expressed as in, is the relative position of the scale, is a rotation variable expressed in quaternion form. The camera coordinate system within the sliding window in the inertial (IMU) coordinate system can be described as:

[0063]

[0064] in is the external parameter between the camera and IMU, and the rotation matrix From quaternions Consider two consecutive frames b in the window k and b k+1 , the terms α, β, and γ represent the pre-integrations related to position, velocity, and angular velocity, respectively. k Under this condition, the pre-integration of the IMU measurement value at discrete time i and i+1 can be expressed as:

[0065]

[0066] After obtaining the pre-integrated values ​​between frames, the gyroscope drift correction and the joint estimation of velocity, gravity and scale factor are performed in sequence. The core is to construct the residual related to the pre-integration, and the residual is defined as the increment obtained by the IMU pre-integration between two adjacent frames. and The error from the predicted value:

[0067]

[0068] By minimizing the above residual function, the optimal estimate of angular velocity drift, velocity, gravity and scale factor can be obtained.

[0069] In real-time pose estimation, a factor graph optimization problem is constructed to solve the following variables to be optimized:

[0070] x c ={x c ,x f}

[0071]

[0072]

[0073] The above variables to be optimized mainly include camera-related variables and visual feature variables. Camera-related variables include 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 visual feature variable includes the inverse depth λ of all visual feature points.

[0074] The following factor graph optimization problem is constructed:

[0075]

[0076] The real-time pose of the current camera can be obtained by solving the above optimization problem using the Ceres open source library.

[0077] Real-time relative pose estimation module

[0078] In the real-time relative pose estimation problem between drones and unmanned vehicles, a factor graph optimization problem is constructed to solve the following variables to be optimized:

[0079]

[0080] x r = {R LC ,t LC}

[0081]

[0082]

[0083] 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.

[0084] Introducing the environmental point cloud information provided by the lidar, the following factor graph optimization problem including the point cloud residual is constructed:

[0085]

[0086] 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. The factor diagram of the system is shown in Figure 2. Figure 1 shown.

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

[0088] It is assumed that all feature points extracted by the camera correspond to specific local planes in the LiDAR point cloud map. Since surface features are usually composed of a large number of points and are less affected by single-point noise, the results of surface feature extraction are usually more stable when the data volume is large and the noise is high. The plane in the LiDAR point cloud is represented in the form of a Hesse normal vector, which is expressed as a unit normal vector and a scalar representing the distance from the plane to the origin. Given a point, its distance to the plane is calculated as follows:

[0089]

[0090] To identify the local plane associated with a specific point, a KD tree is used to find nearby points and fit the local plane. The calculated normal vector is used to construct the residual term and the Jacobian matrix. In the current camera coordinate system, the coordinates of the feature points are initialized by triangulation to initialize their inverse depth. These points are then transformed to the world coordinate system of the lidar using the estimated relative pose:

[0091]

[0092] The corresponding local plane is parameterized as a unit normal vector and a distance scalar. Then, the point-to-plane residual from a specific camera feature point to the corresponding lidar local plane is expressed as:

[0093]

[0094] The Jacobian matrix of this residual is:

[0095]

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

[0097] First, the image data of the visual camera sensor is processed at the front end, including feature point extraction, feature point matching and inter-frame pose estimation. The function of the front-end visual odometer is to estimate the camera motion based on the data of two adjacent frames collected. Common algorithms include feature point method, optical flow method, direct method and inter-frame estimation based on deep learning. The feature point method extracts and matches the feature points of each image frame between adjacent frames, and then uses algorithms such as RANSAC to remove the mismatched noise data points as much as possible. Feature points are composed of key points and descriptors. Considering the accuracy and calculation time, key point detection and descriptors should be used together. Among them, the key point detection algorithms mainly include ORB, SIFT and SURF, and the descriptor algorithms include BRIEF, BRISK, etc. In order to cope with the problem of difficult feature point extraction and simple scene texture, the optical flow method has also been widely used as a supplement to feature points. It is based on the idea that the brightness of pixels at a certain position in the image will not change in a short time, so some pixels in the image are selected for tracking, and finally the pose of the camera is solved. The direct method evolved from optical flow. It is based on the grayscale invariance assumption and uses the changes in pixel grayscale values ​​in the image to estimate the direction of camera movement. It can be calculated without key points and descriptors.

[0098] After extracting feature points from the image frames captured by the camera and matching them, that is, determining the correspondence between the current feature points and the previously obtained feature points, the matched feature points are used to estimate the change in the camera's posture between the two frames. Different solutions are used in different scenarios based on the different principles of the camera. The main cases are as follows: when using a monocular camera and obtaining two sets of matched 2D pixel coordinates, epipolar geometry is used for solution; when using a binocular camera or RGB-D camera to obtain the spatial coordinates of some 3D points and their two-dimensional projection positions, the PnP algorithm is used for solution; when using a binocular camera or RGB-D camera to obtain two sets of matched 3D spatial coordinates, the ICP algorithm is used for solution. These algorithms can solve the relative posture change of the camera between two frames.

[0099] 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.

[0100]

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

[0102] The front-end processing of IMU pre-integration construction includes IMU motion model recursion and pre-integration. As an inertial sensor, IMU can obtain the angular velocity and acceleration of an object in an inertial coordinate system. Usually in a visual inertial odometer system, when the IMU is fused with the visual image, its sampling frequency is much higher than the frequency of the image frame obtained by the camera. In this way, there will be a lot of IMU sampling data between adjacent frames. The IMU information between two adjacent image frames needs to be integrated to obtain the posture information at this time, so as to align it with the visual information. In order to avoid repeated integration, the IMU pre-integration technology is adopted. By integrating the bth k The frame is regarded as the reference system. The content of the integral is only related to the measurement value of the IMU, and has nothing to do with the state quantity being optimized. The following is the integral result obtained by pre-integration:

[0103]

[0104] This strategy saves a huge amount of computing resources for optimization-based algorithms in SLAM, without having to repeatedly calculate IMU measurements. For the IMU pre-integration form at discrete moments between two frames, numerical integration methods such as Euler integration or median integration can be used. In implementation, the pre-integration quantity can be constructed recursively:

[0105]

[0106] The pre-integrated quantity constructed recursively will be used in the backend to construct the residual about the IMU.

[0107] 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:

[0108]

[0109] 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.

[0110] In general, the present invention designs and constructs a point cloud residual factor based on point-surface matching, and completes the real-time solution of the relative posture of the UAV-unmanned vehicle by constructing a radar-visual-inertial factor graph optimization problem that adds the point cloud residual factor based on point-surface matching; and proposes an overall system architecture design for real-time relative posture estimation of UAV-unmanned vehicle in unstructured complex environments. The present invention realizes the real-time posture estimation function of UAV-unmanned vehicle without prior knowledge in unstructured environments, realizes the construction of the visual odometer factor graph optimization problem that integrates the laser radar point cloud information, and solves the drift problem inherent in the visual-inertial odometer.

[0111] The present invention provides a real-time relative pose estimation solution for a UAV-UAV collaborative system. By integrating visual-inertial odometry (VIO), lidar-inertial odometry (LIO) and factor graph optimization technology, the present invention not only enhances the autonomy and intelligence of the system, but also ensures its cross-platform versatility and real-time performance. In particular, the present invention effectively integrates visual and lidar information through the innovative design of point cloud residual factors, significantly improves the accuracy and robustness of pose estimation, and solves the drift problem in visual-inertial odometry. In addition, the system architecture design of the present invention realizes real-time pose estimation without the need for prior maps for unstructured complex environments, thereby improving the efficiency and effectiveness of collaborative operations between UAVs and unmanned vehicles in a variety of mission scenarios.

[0112] 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.

[0113] 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.

[0114] 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.

[0115] 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.

[0116] 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.

[0117] 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.

[0118] 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.

[0119] A person skilled in the art can understand that: all or part of the steps of implementing the above method embodiment can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above method embodiment; and the aforementioned storage medium includes: mobile storage devices, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), disks or optical disks, etc. Various media that can store program codes.

[0120] 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.

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

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

[0123] 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.

[0124] 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 real-time relative pose estimation method in a UAV-UAV collaborative system, characterized in that: The following steps are involved: Step 1: Using monocular camera images and inertial measurement unit (IMU) data, we use visual feature point extraction and tracking and IMU pre-integration as the front end, and factor graph optimization as the back end to achieve real-time pose estimation of the UAV and the unmanned vehicle, and construct their own sparse feature point cloud maps; Step 2: Combine the point cloud data acquired by the lidar in real time with the IMU data, use the Fast-LIO algorithm, calculate the point-to-surface residual and use the iterative error state Kalman filter IESKF to complete the real-time construction of dense 3D point cloud maps of the UAV and unmanned vehicle postures and their respective environments; Step 3. Based on steps 1 and 2, a factor graph optimization problem is constructed, including the point-to-surface matching residual factors between the sparse feature point cloud obtained in step 1 and the dense point cloud obtained in step 2, as well as the visual feature point reprojection residual factors and the IMU pre-integration residual factors. The relative position between the UAV and the unmanned vehicle is solved in real time by minimizing the residual.

2. The real-time relative pose estimation method in the UAV-UAV collaborative system according to claim 1, characterized in that: In step 1, the preprocessing of the monocular camera image and IMU data includes feature point extraction, tracking, and time synchronization with the IMU data; In step 2, the point cloud data acquired by the lidar is fused with the IMU data by calculating the point-to-surface residual and using the iterative error state Kalman filter (IESKF) for pose estimation to achieve real-time construction of a dense 3D point cloud map of the environment around the UAV and the unmanned vehicle. In step three, the constructed factor graph optimization problem includes using the hierarchical Bundle Adjustment algorithm to accurately estimate the relative pose between the UAV and the unmanned vehicle, where the point-to-surface matching residual factor is calculated using a local plane represented by a Hesse normal vector.

3. The real-time relative pose estimation method in the UAV-UAV collaborative system according to claim 1 or 2, characterized in that: Step 1 specifically includes: The visual feature point extraction and tracking technology is used, combined with IMU pre-integration as the front-end processing, to estimate the initial posture of drones and unmanned vehicles; Using factor graph optimization as the backend processing to further optimize the pose estimation obtained by the frontend; In the initialization phase, the structure-from-motion method (SfM) and sliding window method are implemented to initialize by selecting consecutive image frames, identifying stable feature tracking and sufficient parallax, and setting the reference frame; Using epipolar geometry and 2D-2D feature point matching, the pose transformation of the initial frame relative to the reference frame is solved, and a scaled 3D point cloud is constructed using a triangulation method. Apply the PnP method and 3D-2D matching relationship to estimate the poses of other frames in the window and construct a 3D feature point cloud of the environment, while dealing with the problem of the monocular camera lacking absolute scale information; Combine IMU pre-integration and 3D lidar data to accurately estimate the point cloud scale factor and achieve scale correction; The angular velocity drift correction is performed on the IMU pre-integration, and the velocity, gravity and scale factor are jointly optimized to construct the pre-integration residual and minimize it to obtain the optimal estimate; In the real-time pose estimation stage, a factor graph optimization problem is constructed. The variables include camera coordinate system transformation, speed, IMU bias parameters, and inverse depth of visual feature points. The Ceres open source library is used to solve the optimization problem and obtain the real-time pose.

4. The real-time relative pose estimation method in a UAV-UAV collaborative system according to any one of claims 1 to 3, characterized in that: Step 2 specifically includes: Using LiDAR and IMU as input, the robot’s motion in the environment and point cloud data of the surrounding environment are captured synchronously; Using the point-to-plane residual calculation method, each point in the lidar point cloud data is matched with the local plane model to form the residual term of the system observation; The Iterative Error State Kalman Filter (IESKF) algorithm is used to recursively update the system state, where the state includes the robot's position, velocity, posture and IMU deviation parameters; The IESKF algorithm is used to process point-to-surface residuals to estimate the robot's posture and generate a dense 3D point cloud map of the environment in real time. The dense three-dimensional point cloud map is combined with the robot posture information to provide an environment model and posture prior for the real-time relative posture estimation in step three.

5. The real-time relative pose estimation method in a UAV-UAV collaborative system according to any one of claims 1 to 4, characterized in that: Step three specifically includes: Construct a factor graph optimization problem, involving three types of variables to be optimized: relative pose variables, camera-related variables, and visual feature variables; In the relative pose variable, the relative translation and rotation between the camera coordinate system and the lidar coordinate system are defined to determine the spatial relationship between the two. The camera-related variables include the rotation, translation, and velocity of the camera coordinate system relative to the initial coordinate system, as well as the acceleration and angular velocity measurement bias of the IMU; Among the visual feature variables, the inverse depth of all visual feature points is involved, which is used to optimize the camera pose and the spatial position of the feature points; The point cloud information provided by the LiDAR is introduced to construct a point cloud residual factor based on point-to-surface matching, which is used to align the LiDAR point cloud with the camera coordinate system. Define the visual feature point reprojection residual factor to ensure the consistency between the feature point observation and the camera model prediction; Define the IMU pre-integration residual factor to integrate IMU data with pose estimation; By minimizing the weighted sum of the residual factors, an optimization algorithm is used to solve the variables to be optimized, thereby realizing the estimation of the relative posture between the UAV and the unmanned vehicle.

6. The real-time relative pose estimation method in the UAV-UAV collaborative system according to claim 5, characterized in that: In step 3, the construction and use of the point cloud residual factor based on point-to-surface matching includes: A point-to-plane matching method is used to establish a correspondence between the feature points extracted by the camera and the local planes in the lidar point cloud; Use Hesse normal vectors to represent local planes in point clouds, including unit normal vectors and the distance from the plane to the origin; Through the KD tree search algorithm, the local plane related to the feature point is identified and the normal vector of the plane is fitted; Calculate the distance from the feature point to the corresponding local plane to construct the point cloud residual factor, which is used to quantify the plane consistency of the feature point in the lidar coordinate system; Initialize the inverse depth of the feature points using the triangulation method, and transform the feature point coordinates to the world coordinate system of the lidar based on the estimated relative pose; Define the point-to-plane residual to quantify the deviation of the feature point in the lidar coordinate system; Construct the Jacobian matrix to describe the gradient information of the point-to-plane residual with respect to the pose variables and inverse depth parameters; The point-to-plane residual and Jacobian matrix are fused into a factor graph optimization problem for fine-tuning relative pose estimates.

7. The real-time relative pose estimation method in the UAV-UAV collaborative system according to claim 5, characterized in that: In step 3, the processing step of the visual feature point reprojection residual factor specifically includes: Perform front-end processing on image data captured by visual camera sensors, including feature point extraction, matching, and inter-frame pose estimation; Use feature point method, optical flow method, direct method or deep learning-based algorithm to estimate the camera motion between two adjacent frames; Use key point detection and descriptor algorithms to extract and match feature points; Use RANSAC and other algorithms to remove mismatched noise data points and improve the accuracy of feature point matching; Depending on the camera type and scene, select epipolar geometry, PnP or ICP algorithm to solve the relative pose change of the camera between two frames; Calculate the reprojection error, which is the difference between the position of the landmark point in the world coordinate system after being projected through the camera model and the actual observed pixel point; Define the reprojection residual factor to quantify the consistency between feature point observations and camera model predictions; The reprojection residual factors are integrated into the factor graph optimization problem to adjust and optimize the camera pose estimation.

8. The real-time relative pose estimation method in the UAV-UAV collaborative system according to claim 5, characterized in that: In step 3, the processing steps of the IMU pre-integration residual factor include: Implement motion model recursion and pre-integration of IMU to build front-end processing, where IMU is an inertial sensor providing angular velocity and acceleration data; The IMU pre-integration technology is used to integrate the IMU measurement values ​​using a certain frame as the reference system to avoid repeated calculations associated with the optimized state quantity; Use numerical integration methods to achieve discrete-time pre-integration of IMU data; The pre-integrated components are constructed by recursion to provide relative pose information for the fusion of IMU data and visual data. In the backend, the IMU pre-integration factors are constructed, including the difference between the observed and actual values ​​of position, velocity, angle, acceleration, and angular velocity bias; Define the IMU pre-integration residual term to quantify the difference between the estimated relative pre-integration value and the actual system observation value; Integrate the IMU pre-integration residual into the factor graph optimization problem to improve the accuracy of pose estimation and the robustness of the system; The IMU pre-integration residual term is used to correct the pose estimation to ensure the stability of the relative pose estimation between the UAV and the unmanned vehicle in a dynamic environment.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the real-time relative pose estimation method in the UAV-UAV collaborative system is implemented as described in any one of claims 1 to 8.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the real-time relative pose estimation method in the UAV-UAV collaborative system is implemented as described in any one of claims 1 to 8.

Images