A method for UAV localization that integrates visual features and multi-sensor information

Abstract

This invention belongs to the field of UAV remote sensing mapping and control, and relates to a UAV localization method that integrates visual features and multi-sensor information. The method includes acquiring binocular image frames, depth image frames, and sensor information; extracting point features and line features from the binocular image frames and performing preliminary pose estimation in combination with sensor information; determining keyframes from the binocular image frames and inserting the keyframes into a local map to establish a local map; optimizing each local map by minimizing the reprojection error between frames to obtain a localization map; constructing a three-dimensional spatial local gradient using depth image frames and sensor information, and constructing a global path in combination with the preliminary pose estimation; performing obstacle detection on the global path, updating the global path using the A* algorithm to obtain the control point path of the UAV after encountering an obstacle; and optimizing the control point path by minimizing the path cost to obtain the updated path, which is the planned path.

Description

Technical Field

[0001] This invention belongs to the field of UAV remote sensing mapping and control, specifically relating to a UAV positioning method that integrates visual features and multi-sensor information. Background Technology

[0002] With the rapid development of drone technology, it has been widely applied in various fields. In these fields, Simultaneous Localization and Mapping (SLAM) and path planning frameworks play crucial roles. Drones often face complex and changing environments during missions, such as urban areas, building complexes, and forests. Traditional positioning systems, especially those relying on the Global Positioning System (GPS), may encounter problems such as signal blockage and multipath effects in these environments, leading to inaccurate positioning.

[0003] In contrast, SLAM systems, by fusing data from multiple sources such as vision, LiDAR, and inertial sensors, can autonomously construct environmental maps in real time and simultaneously estimate the UAV's position within the map, thus achieving high-precision positioning in complex environments. Navigation planning is also a crucial step in the autonomous execution of UAV missions, directly impacting the safety and efficiency of flight paths. SLAM systems continuously update environmental maps, providing real-time environmental perception data for path planning. Based on this data, the path planning framework can intelligently select paths and identify and avoid obstacles in real time. This enables UAVs to respond quickly during flight, effectively ensuring flight safety and efficiency. The close integration of SLAM systems and path planning frameworks makes collaborative operations between multiple UAVs possible, acquiring rich environmental information through various sensors to provide reliable positioning information for path planning algorithms.

[0004] From the perspective of sensor type, SLAM technology can be broadly divided into two categories: (1) SLAM systems based on lidar sensors and (2) SLAM systems based on vision sensors. Since lidar sensors are typically expensive, this can increase the overall system cost, thus limiting the application of lidar-based systems. Furthermore, lidar suffers from insufficient accuracy in long-distance, small-scale environments. Therefore, most SLAM systems employ vision-based methods for localization. However, vision-based systems mostly rely on feature points, using key corner points to describe the environment and estimate camera pose. In low-texture indoor environments, with rapid movement, or in backlit, blurry images, insufficient feature points often lead to system degradation or even collapse, thus failing to support the UAV's autonomous task execution.

[0005] In UAV navigation applications, classical graph search algorithms are among the most widely used algorithms in path planning. Dijkstra's algorithm, while a classic graph search algorithm, has room for efficiency improvement. Subsequent algorithms, A* and D*, further optimized efficiency, and many researchers have proposed various improvement schemes. However, the actual task of UAVs is navigation planning in a three-dimensional environment. Previous algorithms required building a prior 3D global map for navigation planning, consuming significant additional computational resources. The large amount of redundant high-dimensional data increased the navigation planning time, making it difficult for UAVs to perceive the environment in real-time and meet real-time requirements. Summary of the Invention

[0006] To address the issues of insufficient positioning accuracy and low real-time performance of visual SLAM systems in autonomous localization and navigation of UAVs in low-texture environments, this invention proposes a UAV localization method that integrates visual features and multi-sensor information. It combines point and line segment features in low-texture environments with visual and inertial sensor information. By acquiring this information, the projection error of features between adjacent frames is calculated, and the error is minimized through an optimization formula. The SLAM system of this invention features a multi-map system, loop closure detection, and real-time tracking capabilities. Furthermore, an efficient and robust path planning algorithm is developed by incorporating a depth camera sensor, and a multivariate path planning cost function is designed to improve the real-time performance of path planning. Finally, the SLAM and navigation planning modules are integrated to form a complete framework.

[0007] The present invention provides a method for UAV localization that integrates visual features and multi-sensor information, comprising the following steps:

[0008] Binocular image frames are acquired using a stereo camera, depth image frames are acquired using a depth camera, and sensor information is acquired using sensor devices.

[0009] Point features and line features are extracted from the binocular image frames, and preliminary pose estimation is performed in combination with the sensor information;

[0010] Keyframes are determined from the stereo image frames and inserted into the local map to create a local map;

[0011] The reprojection error between frames is minimized in each local map to obtain the updated map, which is the localization map.

[0012] Using the depth image frames and the sensor information, a local gradient in three-dimensional space is constructed, and a global path is constructed in conjunction with the preliminary pose estimation;

[0013] Obstacle detection is performed on the global path, and the global path is updated using the A* algorithm to obtain the control point path of the UAV after encountering an obstacle.

[0014] The path to the control point is optimized by minimizing the path cost, and the updated path is the planned path.

[0015] The beneficial effects of this invention are:

[0016] This invention optimizes the local maps by minimizing inter-frame reprojection errors to obtain a localization map, thereby optimizing the estimated camera pose and environmental structure. Obstacle detection is performed on the global path, and the A* algorithm is used to update the global path, obtaining the control point path for the UAV after encountering obstacles. This update considers the location and characteristics of obstacles in the environment to ensure safe and efficient navigation for the UAV, enabling it to follow a new obstacle avoidance path to reach the predetermined destination. The control point path is then optimized using minimum path cost to obtain a planned path. By comprehensively considering these factors, the cost function provides a comprehensive evaluation criterion for path planning, thus achieving smooth, safe, and feasible navigation during path optimization. This invention reduces navigation planning time, meets real-time requirements, and improves the accuracy of optimized maps and paths. Attached Figure Description

[0017] Figure 1 This is a framework diagram of a UAV positioning system that integrates visual features and multi-sensor information according to an embodiment of the present invention;

[0018] Figure 2 This is a flowchart of a UAV positioning method that integrates visual features and multi-sensor information according to an embodiment of the present invention;

[0019] Figure 3 This is a demonstration diagram of drone positioning and navigation according to an embodiment of the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] Figure 1 This is a framework diagram of a UAV positioning and navigation system that integrates visual features and multi-sensor information according to an embodiment of the present invention, as shown below. Figure 1As shown, the system includes the following modules and units: a data input module, a frame feature extraction module, a local map construction module, a planned path update module, and a multi-map update and storage module. The data input module can input data including stereo image frames, depth image frames, and sensor information, where the sensor information can be inertial sensor information. The frame feature extraction module includes a point feature extraction unit, a line feature extraction unit, a preliminary pose estimation unit, and a keyframe detection unit. The point feature extraction unit is used to extract point features from the stereo image frames, and the line feature extraction unit is used to extract line features from the stereo image frames. The preliminary pose estimation unit is used to perform preliminary pose estimation based on sensor information, point features, and line features; the keyframe detection unit is used to detect keyframes from numerous stereo image frames; the local map construction module includes a keyframe insertion unit, a local map construction unit, and a reprojection error optimization unit; the keyframe insertion unit is used to determine whether a keyframe needs to be inserted based on the application of the keyframe determination function in the tracking process, and forms a queue of keyframes to be inserted into the map; the local map construction unit is used to insert the current keyframe into the map to construct the local map; the reprojection error optimization unit is used to optimize the local map based on various keyframes. Point and line features in keyframes are reprojected to optimize inter-frame reprojection errors. The planned path update module includes a 3D local map unit, a global path establishment unit, an obstacle detection unit, an A* control update unit, a cost function optimization unit, and a path update unit. The 3D local map unit constructs a 3D spatial local gradient based on depth image frames and sensor information. The global path establishment unit constructs a global path based on preliminary pose estimation. The obstacle detection unit detects obstacles on the global path. The A* control update unit updates the global path to obtain the path update result when the UAV encounters obstacles. The control point path after the obstacle is optimized by the cost function optimization unit, which performs minimum path cost optimization on the control point path. The path update unit obtains the updated path, which is the planned path. The multi-map update storage module includes a map insertion unit, a point matching unit, and a line matching unit. The map insertion unit inserts multiple local maps, the point matching unit matches point features of different local maps, and the line matching unit matches line features of different local maps, thereby updating the map. The positioning and navigation module updates the positioning and navigation data based on the data from the planned path update module and the multi-map update storage module.

[0022] Figure 2 This invention provides a method for UAV localization that integrates visual features and multi-sensor information. The method includes:

[0023] 101. Acquire stereo image frames using a stereo camera, acquire depth image frames using a depth camera, and acquire sensor information using sensor devices;

[0024] 102. Extract point features and line features from the binocular image frames, and perform preliminary pose estimation in combination with the sensor information;

[0025] In this embodiment of the invention, key point information in an image can be extracted using the Oriented Fast and Rotated BRIEF (ORB) algorithm, and line segment feature information in an image can be extracted using the Line Segment Detector (LSD) algorithm.

[0026] Specifically, this embodiment uses a simple yet effective method for measuring angular direction, namely, intensity centroid. The intensity centroid assumes that the intensity of an angle deviates from its center, and this vector can be used to estimate direction. Replacing the intensity centroid with a grayscale centroid, ORB keypoint detection can be expressed as:

[0027]

[0028]

[0029] Where, m pq The grayscale intensity is defined, pq represents the order of pixel calculation, which is the product of its coordinates raised to the power of p and the power of q, x, y are the current point, I(x,y) is the grayscale value calculation function of the point, and C is the defined center point.

[0030] In this embodiment, the LSD algorithm first calculates the horizontal angle of each pixel to form a horizontal field. This field is divided into several connected parts with approximately the same direction and within a tolerance τ, thus obtaining a series of regions called support regions. Each support region is essentially a set of pixels that are candidates for line segments. Whether a segment is a line segment is determined by observing its minimum bounding rectangle. Therefore, the LSD line segment detection process can be represented as:

[0031]

[0032]

[0033]

[0034]

[0035] Where G(x,y) represents the gradient of the gray value of a point, and arctan is the trigonometric function formula for finding the angle of a point. x c yIt is the center point of the rectangle you want to find. You can set the thickness of the lines by setting the width of the rectangle.

[0036] In this embodiment, by matching the extracted point and line features, the displacement of the point and line features in each frame of the image is calculated to obtain cumulative mileage information, thereby obtaining the UAV's positioning data. Throughout the process, Bundle Adjustment (BA) is used to optimize the camera pose (extrinsic parameters) and the coordinates of the 3D scene points (intrinsic parameters) so that the points projected onto the image plane are as consistent as possible with the actual observations, thereby improving the consistency and accuracy of the entire system.

[0037] 103. Determine keyframes from the binocular image frames and insert the keyframes into the local map to create a local map;

[0038] In this embodiment of the invention, based on the application of the keyframe decision function in the tracking process, it is determined whether a keyframe needs to be inserted, and a queue of keyframes to be inserted into the map is formed. Then, the Bag of Words (BoW) vector, co-view relationships, and other information of the current keyframe are calculated, and it is inserted into the map. This is a feature representation method for describing image content, as well as information such as co-view relationships with other keyframes. The aforementioned BoW / co-view relationship information is used to construct the association between the current keyframe and other keyframes in the map. By inserting the current keyframe into the map, not only is the camera pose estimation updated, but the structural information of the map is also enriched. This process is a key step in realizing local map creation and updating, providing a foundation for subsequent pose optimization and the robustness of the overall SLAM system.

[0039] 104. Minimize the inter-frame reprojection error optimization for each local map to obtain the updated map, which is the localization map;

[0040] In this embodiment of the invention, local (Bundle Adjustment, BA) is used to improve the accuracy of camera pose estimation. Point and line features in each keyframe are reprojected; adaptive weights are determined based on the number of point and line features in the current keyframe; the products of the reprojection error matrix of point features and the inverse of the corresponding covariance weight matrix, and the products of the reprojection error matrix of line features and the inverse of the corresponding covariance weight matrix, are weighted according to the adaptive weights; the reprojection error between frames is optimized by minimizing the weighted sum.

[0041] Specifically, as the number of feature points increases, the participation of line features decreases. When the number of point features is small, the weights of line features and point features are equal. To adapt more flexibly to different scenarios and data conditions, this invention introduces an adaptive factor, which is adjusted according to the number of point features and line features in the current frame. When there are many point features, the adaptive factor increases the weight of line features, making them more significant during the optimization process; while when there are few point features, the weights of line features and point features remain equal. This dynamic adjustment of weights helps improve the robustness and adaptability of the optimization method, where the adaptive weights are expressed as:

[0042]

[0043] Among them, w base The base weights are Fn, which represents the number of point features, N is the threshold value, and k is a user-defined parameter.

[0044] The entire optimization process employs the Levenberg-Marquardt (LM) method for least squares estimation of regression parameters in nonlinear regression to minimize reprojection error, thereby optimizing the estimated camera pose and environmental structure. The optimization function for minimizing inter-frame reprojection error is expressed as:

[0045]

[0046] Where, ψ * This represents a set of keyframe vector parameters that minimize the projection error. Indicates the number of keyframes. Represents the set of point features in the keyframe. e represents the set of features along the center line in the keyframe. ij Let e ​​represent the reprojection error matrix of feature j at point i in keyframe i. ik This represents the reprojection error matrix of line feature k in keyframe i. This represents the inverse of the covariance weight matrix of feature j in keyframe i. W represents the inverse of the covariance weight matrix of linear feature k in keyframe i, with the superscript T indicating matrix transpose. adaptive This indicates adaptive weights.

[0047] 105. Using the depth image frame and the sensor information, construct a local gradient in three-dimensional space, and combine it with the preliminary pose estimation to construct a global path;

[0048] In this embodiment of the invention, the positioning data of the SLAM system is used as the coordinate input for navigation planning; depth image frames are acquired by sensing the environment through a depth camera, and a three-dimensional spatial local gradient field is established based on the depth image frames. A collision-free path from the starting point to the ending point is first established based on the set target point information.

[0049] 106. Perform obstacle detection on the global path, update the global path using the A* algorithm, and obtain the control point path of the UAV after encountering an obstacle;

[0050] In this embodiment of the invention, when using the A* algorithm for local obstacle path updating, the main steps include: creating an open list and a closed list based on the start and end points; iteratively selecting the optimal node and exploring its neighbors until the end point is found or the process cannot continue; backtracking the optimal path, and rerunning the A* search to obtain a new path when the environment changes. Since updating paths using the A* algorithm is not the focus of this invention, it will not be described in detail here.

[0051] 107. Perform minimum path cost optimization on the control point path to obtain the updated path, which is the planned path.

[0052] In this embodiment of the invention, when designing the cost function, the smoothness, safety, and flight feasibility of the navigation obstacle avoidance path are considered. The path is optimized by solving for the path control points that minimize the overall cost. In the cost function, smoothness can be measured by the curvature or derivative of the path, safety can be considered by the distance between the path and obstacles and the obstacle avoidance probability, and flight feasibility considers the dynamic limitations of the aircraft and environmental constraints. By comprehensively considering these factors, the cost function can provide a comprehensive evaluation standard for path planning, thereby achieving smooth, safe, and feasible navigation when optimizing the path. This optimization process can use the optimized LBGFS method to meet the various requirements of navigation tasks.

[0053] The optimization function for minimum path cost is expressed as:

[0054]

[0055] Among them, J s It is the smoothness cost of the planned path, λ s J is a weighting factor for the smoothness cost. c It is the safety cost of path planning, λ c It is a weighting factor for security costs, J d λ represents the cost of the drone's flight feasibility. d These are the weighting factors in the cost of flight feasibility. The calculation formulas for each cost are expressed as follows:

[0056]

[0057]

[0058]

[0059] Among them, A iJ represents acceleration. i N represents the derivative of acceleration. c Indicates the number of control points. Let c denote the L2 norm. ij c represents the distance to the obstacle. ij =s f -d ij s f Indicates the safe distance threshold range, d ij W represents the distance between the current control point and the obstacle surface, F() represents a second-order continuously differentiable metric function of the higher-order derivatives of the control point, and W represents the distance between the current control point and the obstacle surface. v W a W j These represent the weight information for each item.

[0060] Figure 3 This is a demonstration example image. Figure 3 The document contains images showing the operation of the present invention, demonstrating its effectiveness.

[0061] In this embodiment of the invention, absolute trajectory error (ATE) is used as the evaluation index for SLAM, and success rate, planning time, and flight time are used as indicators for navigation planning.

[0062] Table 1 ATE Positioning Accuracy Table

[0063]

[0064] Table 1 shows the absolute trajectory error (ATE) positioning accuracy table. Table 1 records the accuracy comparison between the method of this invention and different visual binocular SLAM systems. It can be seen that the present invention achieves more accurate results on more complex video datasets. Among them, ORB-SLAM is a 3D localization and mapping algorithm based on ORB features, published in IEEE Transactions on Robotics in 2015 by Raul Mur-Artal, JMM Montiel, and Juan D. Tardos; VINSFusion is an optimized multi-sensor state estimator that enables accurate self-localization for autonomous applications (drones, cars, and AR / VR); SVO (Semi-Direct Monocular Visual Odometry) is a visual odometry method published in 2014 by Professor Scaramuzza's laboratory at the University of Zurich; PL-SLAM is a SLAM based on the ORB-SLAM2 framework and LSD (Line Segment Detector), mainly combining point and line features to improve the reliability of SLAM operation in the environment.

[0065] Table 2 Comparison of Navigation Methods

[0066]

[0067] Table 2 shows the success rate and efficiency of path planning in the positioning method proposed in this invention, recording the accuracy comparison between this invention and different gradient navigation systems. It can be seen that this invention has the best success rate. Among them, EGO is a local path planning method, whose main purpose is to generate short-term path plans suitable for current traffic conditions in real time during UAV flight to ensure the safety and flight efficiency of the UAV; FAST is an efficient global path planning method, mainly used to generate long-term planned paths for UAVs to minimize the time and distance of the entire path; EWOK is a local replanning method, which extends the original planner with a local replanning algorithm, and can solve unmodeled obstacles while getting as close as possible to the globally planned trajectory.

[0068] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, which may include ROM, RAM, disk, or optical disk, etc.

[0069] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for UAV localization that integrates visual features and multi-sensor information, characterized in that, The method includes: Binocular image frames are acquired using a stereo camera, depth image frames are acquired using a depth camera, and sensor information is acquired using sensor devices. Point features and line features are extracted from the binocular image frames, and preliminary pose estimation is performed in combination with the sensor information; Keyframes are determined from the stereo image frames and inserted into the local map to create a local map; The reprojection error between frames is minimized in each local map to obtain the updated map, which is the localization map. Using the depth image frames and the sensor information, a local gradient in three-dimensional space is constructed, and a global path is constructed in conjunction with the preliminary pose estimation; Obstacle detection is performed on the global path, and the global path is updated using the A* algorithm to obtain the control point path of the UAV after encountering an obstacle. The path to the control point is optimized by minimum path cost, and the updated path is the planned path. The optimization of inter-frame reprojection error minimization for each local map includes reprojecting point and line features in each keyframe; determining adaptive weights based on the number of point and line features in the current keyframe; weighting the product of the reprojection error matrix of the point features and the inverse of the corresponding covariance weight matrix, and the product of the reprojection error matrix of the line features and the inverse of the corresponding covariance weight matrix, according to the adaptive weights; and optimizing the inter-frame reprojection error by minimizing the weighted sum. The optimization function that minimizes the inter-frame reprojection error is expressed as: Where, ψ * This represents a set of keyframe vector parameters that minimize the projection error. Indicates the number of keyframes. Represents the set of point features in the keyframe. e represents the set of features along the center line in the keyframe. ij Let e ​​represent the reprojection error matrix of feature j at point i in keyframe i. ik This represents the reprojection error matrix of line feature k in keyframe i. This represents the inverse of the covariance weight matrix of feature j in keyframe i. W represents the inverse of the covariance weight matrix of linear feature k in keyframe i, with the superscript T indicating matrix transpose. adaptive Indicates adaptive weights; The formula for calculating the adaptive weights is as follows: Among them, w base Based on the weights, F n This represents the number of keypoint features detected, where N is the set threshold and k is a user-defined parameter.

2. The UAV positioning method fusing visual features and multi-sensor information according to claim 1, characterized in that, Minimum path cost optimization for the control point path includes constructing the smoothness cost of the planned path based on the curvature or derivative of the path, constructing the safety cost of the planned path based on the distance between the path and obstacles and the obstacle avoidance probability, and constructing the flight feasibility cost of the UAV based on the dynamic constraints of the aircraft and environmental constraints. The path cost is obtained by weighted summing of the smoothness cost of the planned path, the safety cost of the planned path, and the flight feasibility cost of the UAV.

3. The UAV positioning method fusing visual features and multi-sensor information according to claim 2, characterized in that, The optimization function for minimum path cost is expressed as: Among them, J s It is the smoothness cost of the planned path, λ s J is a weighting factor for the smoothness cost. c It is the safety cost of path planning, λ c It is a weighting factor for security costs, J d λ represents the cost of the drone's flight feasibility. d It is a weighting factor in the cost of flight feasibility.

4. The UAV positioning method fusing visual features and multi-sensor information according to claim 3, characterized in that, The formulas for calculating each cost are expressed as follows: Among them, A i J represents acceleration. i N represents the derivative of acceleration. c Indicates the number of control points. Let c denote the L2 norm. ij c represents the difference between the current control point's distance from the obstacle surface and the safe distance. ij =s f -d ij s f Indicates the safe distance threshold range, d ij The distance from the current control point to the obstacle surface is represented by F(), which is a second-order continuously differentiable metric function of the higher-order derivatives of the control point. v w a w j These represent the weight information for each item.

Images