Method, device and equipment for optimizing visual inertial odometer of unmanned aerial vehicle and medium

Abstract

The invention provides an unmanned aerial vehicle visual inertial odometer optimization method and device, equipment and a medium, and the method comprises the steps: carrying out image enhancement and denoising preprocessing on an image based on image information collected by an RGB-D camera of an unmanned aerial vehicle; constructing an image pyramid for the preprocessed image for subsequent multi-scale feature extraction; extracting optical flow change characteristics of two adjacent frames of images by using positive and reflective flow methods, determining a motion relation between adjacent image frames, and eliminating mismatching points by using an RANSAC algorithm; and according to a screening result, based on a motion relation between adjacent image frames, pose estimation is completed through loopback detection and a rear end, and then the pose of the camera is optimized according to the estimated pose to obtain an optimal pose estimation result of the visual inertial odometer. According to the method, the unmanned aerial vehicle can effectively execute the visual inertial odometer and the visual SLAM task in the complex environment, and the robustness positioning of the unmanned aerial vehicle in the complex environment is realized.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology, and in particular to a method, device, equipment and medium for optimizing a visual inertial odometer of an unmanned aerial vehicle. Background Art

[0002] Visual Inertial Odometry (VIO) is the key to simultaneous localization and mapping (SLAM) and an important module in augmented reality (AR), autonomous driving, robotics and other fields. At the same time, for scenes with more complex environments such as forests, cities or buildings, using drones for exploration or reconnaissance will help liberate productivity and improve work efficiency.

[0003] However, when the satellite navigation signal is weak, it performs poorly in application scenarios that require real-time dynamic feedback (such as disaster relief at night) such as visual inertial odometer and visual SLAM tasks, and cannot perform accurate pose estimation in challenging environments such as fast motion, low light and high dynamic range. Summary of the invention

[0004] In order to overcome the problems existing in the related art, the present disclosure provides a method, device, equipment and medium for optimizing the visual inertial odometer of an unmanned aerial vehicle to solve the technical problem of inaccurate posture estimation of unmanned aerial vehicles in complex environments in the related art.

[0005] One or more embodiments of this specification provide a method for optimizing a drone visual inertial odometer, including the following steps:

[0006] Based on the image information collected by the UAV's RGB-D camera, the image is preprocessed by image enhancement and denoising;

[0007] Construct an image pyramid for the preprocessed image for subsequent multi-scale feature extraction;

[0008] The forward and reverse optical flow methods are used to extract the optical flow change characteristics of two adjacent frames of images, determine the motion relationship between adjacent image frames, and use the RANSAC algorithm to eliminate mismatched points; and

[0009] According to the screening results, based on the motion relationship between adjacent image frames, the pose estimation is completed through loop closure detection and the back end, and then the camera pose is optimized according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

[0010] One or more embodiments of this specification provide a drone visual inertial odometer optimization device, including:

[0011] Image processing module, used to collect image data based on the RGB-D camera of the drone and perform image enhancement and denoising preprocessing on the image;

[0012] Image pyramid construction module, used to construct image pyramid for preprocessed images for subsequent multi-scale feature extraction;

[0013] A matching module is used to extract the optical flow change features of two adjacent frames of images by forward and reverse extraction, determine the motion relationship between adjacent image frames, and eliminate mismatched points by using the RANSAC algorithm; and

[0014] The pose confirmation module is used to complete pose estimation through loop closure detection and the back end based on the screening results and the motion relationship between adjacent image frames, and then optimize the camera pose according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

[0015] One or more embodiments of the present specification provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-described UAV visual inertial odometer optimization method when executing the computer program.

[0016] One or more embodiments of the present specification provide a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the drone visual inertial odometer optimization method as described above is implemented.

[0017] The present disclosure provides a method, device, equipment and medium for optimizing visual inertial odometer of unmanned aerial vehicle, which has the advantages of improving contrast and brightness of images by preprocessing collected image information, and considering that the scale inconsistency of corner points may occur when extracting features from processed images, an image pyramid is constructed for subsequent multi-scale feature extraction, because pyramids of different levels represent images at different scales, local features are extracted for each scale to capture image feature information at different scales, and then the feature points extracted from each frame of image are tracked and matched by forward and directional optical flow methods, so as to determine the motion relationship between adjacent image frames, and quickly track each feature point, and introduce RANSAC algorithm after optical flow matching to reduce the problem of increased mismatch rate caused by increased number of feature points, so as to ensure the accuracy of matching, and finally, based on the motion relationship between adjacent image frames, the construction of visual inertial odometer is completed through loop detection and back-end optimization, so as to effectively solve the limitations of traditional image sensors in complex environments, and can effectively perform visual inertial odometer and visual SLAM tasks in complex environments such as fast motion, low light or high dynamic range, so as to realize robust positioning of unmanned aerial vehicles in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate one or more embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0019] Figure 1 A schematic diagram of a first scenario of a method for optimizing a visual inertial odometer of a drone provided in one or more embodiments of this specification;

[0020] Figure 2 A block diagram of a drone visual inertial odometer optimization device provided for one or more embodiments of this specification;

[0021] as well as

[0022] Figure 3 A schematic diagram of the structure of a computer device provided for one or more embodiments of this specification. DETAILED DESCRIPTION

[0023] In order to enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the technical solutions in one or more embodiments of this specification will be clearly and completely described below in conjunction with the drawings in one or more embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0024] The present invention is described in detail below in conjunction with specific implementation methods and the accompanying drawings.

[0025] Method Embodiment

[0026] According to an embodiment of the present invention, a method for optimizing a visual inertial odometer of a drone is provided. Figure 1 As shown, it is a flow chart of the UAV visual inertial odometer optimization method provided in this embodiment. According to the UAV visual inertial odometer optimization method of the embodiment of the present invention, the steps include:

[0027] Step S1, based on the RGB-D camera of the drone, image data is collected and image enhancement and denoising preprocessing is performed on the image; the image data includes a color image and a depth image, the color image is used to identify the x and y values ​​of the feature points, and the depth image is used to obtain the corresponding z value.

[0028] Step S2, constructing an image pyramid for the preprocessed image for subsequent multi-scale feature extraction;

[0029] Step S3, respectively extracting optical flow change features of two adjacent frames of images using forward and reverse optical flow methods, determining the motion relationship between adjacent image frames, and eliminating mismatching points using RANSAC (RANdom SAmple Consensus, random consistency check algorithm); and

[0030] Step S4, based on the screening results and the motion relationship between adjacent image frames, pose estimation is completed through loop closure detection and the back end, and then the camera pose is optimized according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

[0031] The optimization method of the visual inertial odometer of a UAV provided in this embodiment firstly performs a preprocessing operation on the collected image information to improve the contrast and brightness of the image. Then, considering that the scale inconsistency of the corner points may occur when extracting features from the processed image, an image pyramid is constructed for subsequent multi-scale feature extraction. Because pyramids of different levels represent images at different scales, local features are extracted for each scale to capture the image feature information at different scales. The feature points extracted from each frame of the image are then tracked and matched by the forward and directional optical flow methods, respectively, so as to determine the motion relationship between adjacent image frames and quickly track each feature point. After optical flow matching, the RANSAC algorithm is introduced to reduce the problem of increased mismatch rate caused by the increase in the number of feature points, so as to ensure the accuracy of matching. Finally, based on the motion relationship between adjacent image frames, the construction of the visual inertial odometer is completed through loop detection and back-end optimization, so as to effectively solve the limitations of traditional image sensors in complex environments, and can effectively perform visual inertial odometer and visual SLAM tasks in complex environments such as fast motion, low light or high dynamic range, so as to achieve robust positioning of the UAV in complex environments.

[0032] In this embodiment, the image data acquired by the RGB-D camera is converted into an image sequence, and the image contrast is enhanced by using AHE (Adaptive Histogram Equalization) algorithm, and then threshold denoising is performed to obtain pre-processed image data. The specific process of enhancing the image contrast by the adaptive histogram equalization algorithm is as follows:

[0033] The specific implementation process is as follows.

[0034] There are L gray levels in the grayscale range, and its histogram is defined as the following discrete function:

[0035]

[0036] In the formula, is the kth grayscale within the grayscale range, for The number of pixels is normalized after the histogram definition is obtained, and the probability of the k-th grayscale appearing can be obtained:

[0037]

[0038] In the formula, is the total number of pixels in the image. For the extracted local image, k = 0, 1, ..., L-1. Using the obtained probability of occurrence of each gray level, the gray range k is expanded to a suitable range to obtain the gray value of the processed image. It can be expressed as:

[0039]

[0040] Thus, the histogram equalization processing of the local image is completed, and then the image after equalization processing is processed through the constructed sliding window to obtain an image with enhanced contrast. The above processing can enhance the subsequent extraction effect of ORB feature points and improve the positioning accuracy of the drone.

[0041] This embodiment uses the AHE algorithm to calculate the local histogram of the image and then redistribute the gray image level to enhance the contrast and visual brightness of the image to improve the local contrast of the image and obtain more image details. This embodiment further processes the image through a sliding window, and can process each part of the image, retain the detailed features of the image during the processing, and improve the contrast of the image under different lighting or different shadows.

[0042] In this embodiment, when extracting features from an image processed by histogram equalization, inconsistent scales of corner points may occur. Therefore, an image pyramid is constructed using images of different scales, and feature points are evenly distributed to each layer of the pyramid image according to the area, so that each extracted feature point has scale invariance. The specific implementation principle is that the image pyramid is a multi-scale image structure composed of a series of images with different resolutions, and each image is a downsampled version of the previous image; in the pyramid, the image resolution decreases as the number of layers increases, thereby realizing multi-scale representation of the image, and a Gaussian pyramid type is adopted, which is constructed by stepwise downsampling and Gaussian filtering of the original image.

[0043] Therefore, the construction of the image pyramid is as follows:

[0044] Assume the image width is W, length is L, and scaling factor is S (0<S<1), such as Figure 2As shown, the 0th layer (bottom layer) is the original image, the 1st layer is the result after the first scaling factor S, and after superposition, the 3rd layer is the result after the second scaling factor S, and so on. The total area of ​​the entire image pyramid is:

[0045]

[0046] According to the specified total number of features to be extracted, the number of feature points per unit area is calculated according to the geometric progression relationship:

[0047]

[0048] The number of feature points to be allocated to the αth layer (α = 0 to 3) is:

[0049]

[0050] In this embodiment, based on the above image pyramid, the direction of the image feature point is determined by the ORB algorithm. First, the corner points (feature points) are detected on each layer of the pyramid image to obtain the feature points of each layer of the pyramid image. Then, the direction of the feature point is calculated by the grayscale centroid method. The specific steps are as follows:

[0051] a. Select pixel p in the image, assuming that the brightness of pixel p is I p ;

[0052] b. Set a brightness threshold t;

[0053] c. Select 16 pixels on a circle with a radius of 3 pixels and pixel p as the center, and label them from 0 to 15.

[0054] d. Compare the brightness of pixel p with the pixels numbered 1, 5, 9, and 13 on the domain circle. At least three pixels satisfy the brightness greater than I at the same time. p +t or at least 3 pixels have brightness less than I at the same time p -t, then the pixel point p is the corner point that is initially screened, so as to determine the position where the gray value in the image changes significantly;

[0055] e. Then compare the brightness of the pixel points of the initially selected corner points with the brightness of the pixels numbered 0 to 15. If there are 9 consecutive pixels with brightness greater than I p +t or the brightness of 9 consecutive pixels is less than I p -t, then point p is a corner point. In this embodiment, the first step uses 3 pixels to make a preliminary judgment, filtering out most of the impossible corner points, and the second step further refines and confirms whether the corner points filtered in the first step are real corner points, thereby reducing the amount of calculation and improving the algorithm accuracy.

[0056] f. Repeat steps a to e until every pixel in the image pyramid is traversed, and the corner points of each layer are screened out before executing step g.

[0057] g. In order to avoid too many corner points at a certain position on the image, it is necessary to select the best quality corner points within the preset minimum distance, that is, to use the non-maximum suppression corner point screening, as follows:

[0058] Calculate the corresponding response score of each corner point, then take the pixel point p as the center, take its field corner point, and compare to determine whether the pixel point p has the largest response value. If it is, it is retained, otherwise it is suppressed. The response score is determined by the intensity response function calculation, which is calculated as follows:

[0059]

[0060] Among them, V is the corner point with the largest response score in the field, and the remaining corner points will be discarded to obtain feature points.

[0061] h. After extracting the feature points on the pyramid of each frame of the image, the grayscale centroid method is used to calculate the direction of the feature points according to the position coordinates of the feature points and the attribute information of the number of layers of the pyramid. The main idea is to regard the domain range of the feature points as a patch and find the centroid of this patch, as follows:

[0062] In an image block B, the moment m of the image block pq Defined as:

[0063]

[0064] Where (x, y) represents the pixel coordinates; I(x, y) represents the grayscale value of the pixel coordinates of the image block, and the centroid of the image block in, Indicates the weight of the image pixel in the x direction, that is, the x coordinate of the center of gravity, Indicates the weight of the image pixel in the y direction, that is, the y coordinate of the center of gravity.

[0065] Then find the direction of vector OC, connect the geometric center of image block B with the centroid C, and construct the direction vector That is, the (FAST) feature point direction θ is:

[0066]

[0067] In this embodiment, the use of the optical flow method in step S3 to extract the optical flow change characteristics of two adjacent frames of images can be implemented by the LK forward optical flow method and the reverse optical flow method, respectively, which specifically includes two situations, namely, firstly determining the optical flow change characteristics of the two frames of images by the LK forward optical flow method, and then verifying whether the movement of the two frames of pixels from the next frame to the current frame is correct by the LK reverse optical flow method. This embodiment introduces the implementation process of these two methods in the first case as follows.

[0068] a.LK forward optical flow method:

[0069] Assume that the corner point position extracted at time t in the image is (x, y). Using the grayscale invariance assumption, the corner point position changes from t to t+td:

[0070] I(x+dx,y+dy,t+dt)=I(x,y,t);

[0071] The first-order Taylor expansion of the above equation yields:

[0072]

[0073] in, represents the speed of the corner point along the x-axis, denoted as u, Represents the velocity of the corner point along the y-axis, denoted as v.

[0074] At time t+td, let function T(x+dx,y+dy) be the optical flow matching to the gray value of the next frame corner point (x+dx,y+dy). Using nonlinear optimization, the matching relationship between the current frame corner point and the next frame corner point can be obtained as:

[0075] ||f(dx,dy)|| 2 =||I(x,y)-T(x+dx,y+dy)|| 2 ;

[0076] Where I(x, y) represents the grayscale value of the pixel coordinates of the image block; f(dx, dy) represents the grayscale difference between two frames.

[0077] According to the Gauss-Newton method, the Jacobian matrix of f(dx,dy) can be obtained The transform delta between the current frame and the next frame is:

[0078] (Δdx,Δdy) T =-(J T (dx,dy))J(dx,dy) -1 J T (dx,dy)f(dx,dy);

[0079] Dividing both sides of the above equation by the time difference Δt, we can find the moving speed of the corner point (x, y):

[0080]

[0081] However, between two frames of an image, each extracted corner point f(dx,dy) is estimated through nonlinear optimization, which will result in excessive computation and affect process efficiency. Considering that when the optical flow method matches corner points, the corner points extracted between image frames are reversible. Therefore, in order to verify and improve the accuracy of verifying the forward optical flow, this embodiment uses the reverse LK optical flow method to verify the movement of pixels from the next frame to the current frame as follows.

[0082] b.LK reverse optical flow method:

[0083] Assume that the optical flow matching change from the corner point a of the current frame to the corner point b of the next frame is p, then the optimization function is:

[0084]

[0085] Let q=0,q(b)=b, then the above formula can be simplified to:

[0086]

[0087] The reverse optical flow transformation increment Δq is:

[0088] Δq=-(J T J) -1 J T f(q);

[0089] Among them, f(q)=W(B)-W(p(a));

[0090] By inverse transformation (Δq) -1 It can be obtained whether the optical flow matching results from corner point a to corner point b in the forward optical flow are consistent; this embodiment improves the robustness of the positioning algorithm based on the forward and reverse LK optical flow tracking and matching algorithm of the feature points.

[0091] This embodiment uses the forward optical flow method to estimate the movement (position and posture) of the pixel from the current frame to the next frame, and then uses the reverse optical flow method to reversely verify the movement of the pixel from the next frame to the current frame. This can verify the accuracy of the forward optical flow. The ultimate goal of these two methods is to achieve feature matching between pixels (feature matching is generally the position and posture relationship between pixels. Only after feature matching can better motion estimation be achieved).

[0092] In this embodiment, after optical flow matching, the RANSAC basic matrix calculation method is used to remove mismatched feature points to reduce the probability of feature point mismatching and ensure the accuracy of feature point tracking. The specific idea of ​​RANSAC random consistency check algorithm to remove mismatched points is as follows.

[0093] Randomly select three frames of image observations, two feature points and two line features to construct a linear equation problem with minimum quadratic constraints. Use the Lagrange multiplier method to convert the problem into a quadratic eigenvalue solution problem to obtain the initial state vector. Traverse each observation of each feature point (i.e. the position or coordinates of the feature point in each frame), use the state vector to restore the feature point and calculate the reprojection error of the line feature, select observations with errors below a specific threshold and add them to the inlier set. If the number of inlier sets is greater than a specific threshold, reconstruct the linear equation problem with quadratic constraints based on the inlier set, solve to obtain new state variables, and then recalculate the reprojection error of the point and line features to obtain the state vector with the minimum error. Repeat until the iteration ends.

[0094] In a specific embodiment, the specific steps of removing mismatched points using the RANSAC random consistency check algorithm are as follows:

[0095] There is the same transformation relationship between the multiple feature points matched in each frame image. A pair of matched feature points p feat1 With p feat2 There is a transformation matrix H trans , the transformation relationship is:

[0096] p feat2 =H trans p feat1 ;

[0097] In the formula, H trans is a third-order matrix, and there is an element in the matrix that is 1. If we solve the matrix H trans , four pairs of matching feature points are required.

[0098] Using RANSAC random consistency test, four pairs of matching feature points are selected to solve the matrix H trans , define it as the interior point model M, and judge whether the remaining exterior point projection error meets the threshold, and decide whether to treat it as an interior point. The judgment criteria are:

[0099]

[0100] Where thres t is the screening threshold of the internal points, p featj With p feati are the matching feature points.

[0101] After the number of internal point sets meets the conditions, the next iteration will be carried out to solve the optimal transformation matrix H trans , and then the mismatched features are eliminated. The adaptive conditions of the number of iterations are as follows:

[0102]

[0103] Where p is the probability that at least one of the n feature points selected after k selections is an inlier point, the probability is set to 0.99, and w is the proportion of inliers.

[0104] In this embodiment, based on the motion relationship between adjacent image frames, pose estimation is completed through loop closure detection and back-end optimization, and the camera pose is optimized according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

[0105] In this embodiment, the corresponding target pose transformation matrix is ​​optimized and updated according to the result of loop detection to obtain an updated target pose transformation matrix. The estimation of pose is often a recursive process, that is, the pose of the current video frame is solved by the pose of the previous video frame, and the error is passed down frame by frame to obtain the cumulative error. In fact, the pose of the following video frames can also be directly inferred from the frontmost video frame. Such pose detection is called "loop". For example, the pose error of the fifth frame accumulates the errors of the previous four video frames, but the pose of the fifth frame does not necessarily have to be derived from the fourth frame, and can also be inferred from the second frame. Obviously, such calculation error will be much smaller. However, not any of the previous frames can be used to infer the pose of the following video frame. The two need to meet a certain relationship. Generally speaking, the poses of the two frames need to be particularly close, so it is necessary to find a video frame that meets this requirement, and then perform pose optimization to reduce the error. This method is called "loop detection".

[0106] Then, according to the result of loop detection, the corresponding target posture transformation matrix is ​​optimized and updated to obtain the updated target posture transformation matrix, including: determining the posture corresponding to each video frame image according to the target transformation matrix; obtaining the estimated posture transformation matrix corresponding to the loop frame relative to the previously matched key frame according to the cumulative calculation of the posture corresponding to each video frame image; obtaining the corresponding observation posture transformation matrix according to the loop frame; optimizing the target posture transformation matrix between the video frame images according to the estimated posture transformation matrix and the observation posture transformation matrix to obtain the updated target posture transformation matrix. Determine the three-dimensional coordinates corresponding to each video frame image according to the updated target posture transformation matrix.

[0107] In addition, the back-end optimization in this embodiment is to improve the accuracy and robustness of the system through graph optimization technology. For example, an optimization algorithm (such as g2o or Ceres Solver) is used to adjust the pose estimation of the camera and IMU and the position of the map points to minimize the residual of the visual and inertial observation data. These optimization problems are usually constructed as a graph, where the nodes represent the camera pose and map points, and the edges represent sensor observations or motion constraints. The optimization process minimizes the overall error through iterative solution, thereby improving the accuracy and consistency of the global trajectory, ensuring high-precision positioning and mapping even in long-term operation or complex environments.

[0108] Device Embodiment

[0109] According to an embodiment of the present invention, a device for optimizing a visual inertial odometer of a drone is provided. Figure 2 As shown, it is a block diagram of the unmanned aerial vehicle visual inertial odometer optimization device provided in this embodiment. The unmanned aerial vehicle visual inertial odometer optimization device according to the embodiment of the present invention includes:

[0110] The image processing module 10 is used to collect image data based on the RGB-D camera of the drone and perform image enhancement and noise removal preprocessing on the image; the image data includes a color image and a depth image, the color image is used to identify the x and y values ​​of the feature points, and the depth image is used to obtain the corresponding z value.

[0111] An image pyramid construction module 20 is used to construct an image pyramid for the preprocessed image for subsequent multi-scale feature extraction;

[0112] A matching module 30 is used to extract the optical flow change characteristics of two adjacent frames of images by using the forward and reverse optical flow method, determine the motion relationship between adjacent image frames, and eliminate mismatching points by using RANSAC (RANdom SAmple Consensus, random consistency check algorithm); and

[0113] The pose confirmation module 40 is used to complete the pose estimation through loop closure detection and the back end according to the screening results and the motion relationship between adjacent image frames, and then optimize the camera pose according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

[0114] The optimization method of the visual inertial odometer of a drone provided in this embodiment first performs a preprocessing operation on the collected image information through the image processing module 10 to improve the contrast and brightness of the image. Considering that the scale inconsistency of the corner points may occur when extracting features from the processed image, the image pyramid construction module 20 constructs an image pyramid for subsequent multi-scale feature extraction. Because pyramids at different levels represent images at different scales, local features are extracted for each scale to capture image feature information at different scales. Then, the matching module 30 tracks and matches the feature points extracted from each frame of the image through the forward and directional optical flow methods, thereby confirming the feature points. The motion relationship between adjacent image frames is determined and corrected, and each feature point is quickly tracked. After optical flow matching, the RANSAC algorithm is introduced to reduce the problem of increased mismatch rate caused by the increase in the number of feature points to ensure the accuracy of matching. Finally, the posture confirmation module 40 is based on the motion relationship between adjacent image frames, and completes the construction of the visual inertial odometer through loop detection and back-end optimization. The device provided in this embodiment effectively solves the limitations of traditional image sensors in complex environments, and can effectively perform visual inertial odometer and visual SLAM tasks in complex environments such as fast motion, low light or high dynamic range, so as to achieve robust positioning of drones in complex environments.

[0115] In this embodiment, the image data acquired by the RGB-D camera is converted into an image sequence, the AHE algorithm is used to enhance the image contrast, and then threshold denoising is performed to obtain pre-processed image data. The specific process of enhancing the image contrast by the adaptive histogram equalization algorithm is as follows:

[0116] The specific implementation process is as follows.

[0117] There are L gray levels in the grayscale range, and its histogram is defined as the following discrete function:

[0118]

[0119] In the formula, is the kth grayscale within the grayscale range, for The number of pixels is normalized after the histogram definition is obtained, and the probability of the k-th grayscale appearing can be obtained:

[0120]

[0121] In the formula, is the total number of pixels in the image. For the extracted local image, k = 0, 1, 2, ..., L-1. Using the obtained probability of occurrence of each gray level, the gray range k is expanded to a suitable range to obtain the gray value of the processed image. It can be expressed as:

[0122]

[0123] Thus, the histogram equalization processing of the local image is completed, and then the image after equalization processing is processed through the constructed sliding window to obtain an image with enhanced contrast. The above processing can enhance the subsequent extraction effect of ORB feature points and improve the positioning accuracy of the drone.

[0124] In this embodiment, a feature point direction confirmation module 50 is further provided, which is used to determine the direction of the image feature points based on the image pyramid constructed by the image pyramid construction module 20 by using the ORB algorithm. First, corner points are detected on each layer of the pyramid image to obtain the feature points of each layer of the pyramid image, and then the direction of the feature points is calculated by the grayscale centroid method. The specific steps are as follows:

[0125] a. Select pixel p in the image, assuming that the brightness of pixel p is I p ;

[0126] b. Set a brightness threshold t;

[0127] c. Select 16 pixels on a circle with a radius of 3 pixels and pixel p as the center, and label them from 0 to 15.

[0128] d. Compare the brightness of pixel p with the pixels numbered 1, 5, 9, and 13 on the domain circle. At least three pixels satisfy the brightness greater than I at the same time. p +t or at least 3 pixels have brightness less than I at the same time p -t, then the pixel point p is the corner point to be initially screened;

[0129] e. Then compare the brightness of the pixel points of the initially selected corner points with the brightness of the pixels numbered 0 to 15. If there are 9 consecutive pixels with brightness greater than I p +t or less than I p -t, then point p is a corner point.

[0130] f. Repeat steps a to e until every pixel in the image pyramid is traversed, and the corner points of each layer are screened out before executing step g.

[0131] g. In order to avoid too many corner points at a certain position on the image, it is necessary to select the best quality corner points within the preset minimum distance, that is, to use the non-maximum suppression corner point screening, as follows:

[0132] Calculate the corresponding response score of each corner point, then take the pixel point p as the center, take its field corner point, and determine whether p has the largest response value. If so, keep it, otherwise suppress it. The response score is determined by the intensity response function, which is calculated as follows:

[0133]

[0134] Among them, V is the corner point with the largest response score in the field, and the remaining corner points will be discarded to obtain feature points.

[0135] h. After extracting the feature points on the pyramid of each frame of the image, the grayscale centroid method is used to calculate the direction of the feature points according to the position coordinates of the feature points and the attribute information of the number of layers of the pyramid. The main idea is to regard the domain range of the feature points as a patch and find the centroid of this patch, as follows:

[0136] In an image block B, the moment m of the image block pq Defined as:

[0137]

[0138] Where (x, y) represents the pixel coordinates; I(x, y) represents the grayscale value of the pixel coordinates of the image block, and the centroid of the image block in, Indicates the weight of the image pixel in the x direction, that is, the x coordinate of the center of gravity, Indicates the weight of the image pixel in the y direction, that is, the y coordinate of the center of gravity.

[0139] Then find the direction of vector OC, connect the geometric center of image block B with the centroid C, and construct the direction vector That is, the (FAST) feature point direction θ is:

[0140]

[0141] In this embodiment, the matching module 30 uses the optical flow method to extract the optical flow change characteristics of two adjacent frames of images, which can be achieved through the LK forward optical flow method and the reverse optical flow method. The implementation process of these two methods is as follows.

[0142] a.LK forward optical flow method:

[0143] Assume that the corner point position extracted at time t in the image is (x, y). Using the grayscale invariance assumption, the corner point position changes from t to t+td:

[0144] I(x+dx,y+dy,t+dt)=I(x,y,t);

[0145] The first-order Taylor expansion of the above equation yields:

[0146]

[0147] in, represents the speed of the corner point along the x-axis, denoted as u, Represents the velocity of the corner point along the y-axis, denoted as v.

[0148] At time t+td, let function T(x+dx,y+dy) be the optical flow matching to the gray value of the next frame corner point (x+dx,y+dy). Using nonlinear optimization, the matching relationship between the current frame corner point and the next frame corner point can be obtained as:

[0149] ||f(dx,dy)|| 2 =||I(x,y)-T(x+dx,y+dy)|| 2 ;

[0150] Where I(x, y) represents the grayscale value of the pixel coordinates of the image block; f(dx, dy) represents the grayscale difference between two frames.

[0151] According to the Gauss-Newton method, the Jacobian matrix of f(dx,dy) can be obtained The transform delta between the current frame and the next frame is:

[0152] (Δdx,Δdy) T =-(J T (dx,dy))J(dx,dy) -1 J T (dx,dy)f(dx,dy);

[0153] Dividing both sides of the above equation by the time difference Δt, we can find the moving speed of the corner point (x, y):

[0154]

[0155] However, between two frames of an image, each extracted corner point f(dx,dy) is estimated through nonlinear optimization, which will result in excessive computation and affect process efficiency. Considering that when the optical flow method matches corner points, the corner points extracted between image frames are reversible. In this embodiment, the LK reverse optical flow method is used to verify whether the movement of two frames of pixels from the next frame to the current frame is correct.

[0156] b.LK reverse optical flow method:

[0157] Assume that the optical flow matching change from the corner point a of the current frame to the corner point b of the next frame is p, then the optimization function is:

[0158]

[0159] Let q=0,q(b)=b, then the above formula can be simplified to:

[0160]

[0161] The reverse optical flow transformation increment Δq is:

[0162] Δq=-(J T J) -1 J T f(q);

[0163] Among them, f(q)=W(B)-W(p(a));

[0164] By inverse transformation (Δq) -1 It can be obtained whether the optical flow matching results from corner point a to corner point b in the forward optical flow are consistent; this embodiment improves the robustness of the positioning algorithm based on the forward and reverse LK optical flow tracking and matching algorithm of the feature points.

[0165] In this embodiment, after optical flow matching, the matching module 30 is further configured to use the RANSAC calculation basic matrix method to remove mismatched feature points to reduce the probability of feature point mismatching and ensure the accuracy of feature point tracking. The specific idea of ​​the RANSAC random consistency check algorithm to remove mismatched points is as follows.

[0166] Randomly select three frames of image observations, two feature points and two line features to construct a linear equation problem with minimum quadratic constraints. Use the Lagrange multiplier method to convert the problem into a quadratic eigenvalue solution problem to obtain the initial state vector. Traverse each observation of each feature point, use the state vector to restore the feature point and calculate the reprojection error of the line feature. Select observations with errors below a specific threshold and add them to the inlier set. If the number of inlier sets is greater than a specific threshold, reconstruct the linear equation problem with quadratic constraints based on the inlier set, solve to obtain new state variables, and then recalculate the reprojection error of the point and line features to obtain the state vector with the minimum error. Repeat until the iteration ends.

[0167] The embodiment of the present invention is an apparatus embodiment corresponding to the above-mentioned method embodiment. The specific operations of the processing steps of each module can be understood by referring to the description of the method embodiment, which will not be repeated here.

[0168] like Figure 3 As shown, the present invention also provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the method for optimizing the visual inertial odometer of the unmanned aerial vehicle in the above-mentioned embodiment is implemented, or when the computer program is executed by a processor, the method for optimizing the visual inertial odometer of the unmanned aerial vehicle in the above-mentioned embodiment is implemented.

[0169] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0170] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The device and system embodiments described above are merely schematic, wherein the units described 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 they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0171] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and the contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A method for optimizing the visual inertial odometer of an unmanned aerial vehicle, characterized in that The following steps are involved: Based on the image information collected by the UAV's RGB-D camera, the image is preprocessed by image enhancement and denoising; Construct an image pyramid for the preprocessed image for subsequent multi-scale feature extraction; The forward and reverse optical flow methods are used to extract the optical flow change characteristics of two adjacent frames of images, determine the motion relationship between adjacent image frames, and use the RANSAC algorithm to eliminate mismatched points; as well as According to the screening results, based on the motion relationship between adjacent image frames, the pose estimation is completed through loop closure detection and the back end, and then the camera pose is optimized according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

2. The method for optimizing the visual inertial odometer of a UAV according to claim 1, characterized in that: The image is enhanced by using the adaptive histogram equalization algorithm. The specific steps are as follows: There are L gray levels in the grayscale range, and its histogram is defined as the following discrete function: In the formula, is the kth grayscale within the grayscale range, for The number of pixels is normalized after the histogram definition is obtained to obtain the probability of the k-th grayscale appearing: In the formula, is the total number of pixels in the image; For the extracted local image k=0, 1, ..., L-1, the grayscale range k is expanded to a suitable range using the obtained probability of occurrence of each grayscale level, and the processed image grayscale value is obtained, thereby completing the histogram equalization processing of the local image. The image grayscale value is calculated as follows: The equalized image is then processed through the constructed sliding window to obtain an image with enhanced contrast.

3. The method for optimizing the visual inertial odometer of a UAV according to claim 1, characterized in that: Also includes the steps: Based on the constructed image pyramid, the direction of the image feature points is determined by the ORB algorithm. The specific steps are as follows: a. Select pixel p in the image, assuming that the brightness of pixel p is I p ; b. Set a brightness threshold t; c. Select 16 pixels on a circle with a radius of 3 pixels and a center of pixel p, and label them from 0 to 15; d. Compare the brightness of pixel p with the pixels numbered 1, 5, 9, and 13 on the domain circle. At least three pixels satisfy the brightness greater than I at the same time. p +t or at least 3 pixels have brightness less than I at the same time p -t, then the pixel point p is the corner point to be initially screened; e. Then compare the brightness of the pixel points of the initially selected corner points with the brightness of the pixels numbered 0 to 15. If there are 9 consecutive pixels with brightness greater than I p +t or the brightness of 9 consecutive pixels is less than I p -t, then point p is a corner point; f. Loop steps a to e until every pixel in the image pyramid is traversed, and the corner points of each layer are screened out before executing step g; g. Use non-maximum suppression corner point screening; h. After screening, the direction of the feature point is calculated using the grayscale centroid method based on the location coordinates of the feature point and the attribute information of the pyramid layer.

4. The method for optimizing the visual inertial odometer of a UAV according to claim 3, characterized in that: The screening of non-maximum suppressed corner points is specifically as follows: calculate the corresponding response score of each corner point, then take the pixel point p as the center, take its field corner point, and compare and determine whether the pixel point p has the maximum response value. If so, retain it, otherwise suppress it. The response score is calculated as follows: Among them, V is the corner point with the largest response score in the field, and the remaining corner points will be discarded to obtain feature points.

5. The method for optimizing the visual inertial odometer of a UAV according to claim 1, characterized in that: The specific process of removing mismatched points using the RANSAC algorithm is as follows: Randomly select three frames of image observations, two feature points and two line features to construct a linear equation problem with minimum quadratic constraints. Use the Lagrange multiplier method to convert the problem into a quadratic eigenvalue solution problem to obtain the initial state vector. Iterate through each observation of each feature point, use the state vector to restore the feature points and calculate the reprojection error of the line feature. Select observations with errors below a specific threshold and add them to the internal point set. When the number of inlier points is greater than a certain threshold, the linear equation problem of quadratic constraints is reconstructed based on the inlier points, and the new state variables are obtained by solving them. Then, the reprojection error of the point and line features is recalculated to obtain the state vector with the minimum error. Repeat the above steps until the iteration ends.

6. A UAV visual inertial odometer optimization device, characterized in that include: Image processing module, used to collect image data based on the RGB-D camera of the drone and perform image enhancement and denoising preprocessing on the image; Image pyramid construction module, used to construct image pyramid for preprocessed images for subsequent multi-scale feature extraction; The matching module is used to extract the optical flow change characteristics of two adjacent frames of images using the forward and reverse optical flow methods, determine the motion relationship between adjacent image frames, and eliminate mismatched points using the RANSAC algorithm; as well as The pose confirmation module is used to complete pose estimation through loop closure detection and the back end based on the screening results and the motion relationship between adjacent image frames, and then optimize the camera pose according to the estimated pose to obtain the optimal pose estimation result of the visual inertial odometer.

7. The unmanned aerial vehicle visual inertial odometer optimization device according to claim 6, characterized in that: The image processing module is configured to perform image enhancement processing on the image through an adaptive histogram equalization algorithm, and the specific steps are as follows: There are L gray levels in the grayscale range, and its histogram is defined as the following discrete function: In the formula, is the kth grayscale within the grayscale range, for The number of pixels is normalized after the histogram definition is obtained to obtain the probability of the k-th grayscale appearing: In the formula, is the total number of pixels in the image; For the extracted local image k=0, 1, ..., L-1, the grayscale range k is expanded to a suitable range using the obtained probability of occurrence of each grayscale level, and the processed image grayscale value is obtained, thereby completing the histogram equalization processing of the local image. The image grayscale value is calculated as follows: The equalized image is then processed through the constructed sliding window to obtain an image with enhanced contrast.

8. The unmanned aerial vehicle visual inertial odometer optimization device according to claim 6, characterized in that: A feature point direction confirmation module is also set up to determine the direction of image feature points through the ORB algorithm based on the constructed image pyramid. The specific configuration performs the following steps: a. Select pixel p in the image, assuming that the brightness of pixel p is I p ; b. Set a brightness threshold t; c. Select 16 pixels on a circle with a radius of 3 pixels and a center of pixel p, and label them from 0 to 15; d. Compare the brightness of pixel p with the pixels numbered 1, 5, 9, and 13 on the domain circle. At least three pixels satisfy the brightness greater than I at the same time. p +t or at least 3 pixels have brightness less than I at the same time p -t, then the pixel point p is the corner point to be initially screened; e. Then compare the brightness of the pixel points of the initially selected corner points with the brightness of the pixels numbered 0 to 15. If there are 9 consecutive pixels with brightness greater than I p +t or the brightness of 9 consecutive pixels is less than I p -t, then point p is a corner point; f. Loop steps a to e until every pixel in the image pyramid is traversed, and the corner points of each layer are screened out before executing step g; g. Use non-maximum suppression corner point screening; h. After screening, the direction of the feature point is calculated using the grayscale centroid method based on the location coordinates of the feature point and the attribute information of the pyramid layer.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the unmanned aerial vehicle visual inertial odometer optimization method according to any one of claims 1 to 5 is implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the unmanned aerial vehicle visual inertial odometer optimization method according to any one of claims 1 to 5 is implemented.

Images