A UAV navigation method and device

Abstract

The present application discloses a method and device for unmanned aerial vehicle navigation, including: when the confidence of GNSS posture is in a high confidence interval, the GNSS posture at this time is used for positioning flight, and the visual map construction step is performed based on the posture of the image frame; when the confidence of GNSS posture is in a medium confidence interval, the credibility of the current GNSS posture is judged by the constructed visual map, if it is credible, the visual map is updated based on the GNSS posture to obtain the updated visual map, if it is untrustworthy, the GNSS posture is discarded, and the positioning flight is performed based on the posture of the current image frame obtained by the visual map; when the confidence of GNSS posture is in a failure interval, the visual map is used to match the feature points in the current image frame and the visual map to estimate the current posture of the unmanned aerial vehicle. The problem of how to achieve positioning and navigation by combining GNSS and visual information when the unmanned aerial vehicle cannot receive or trust GNSS signals is solved.

Description

Technical Field

[0001] The present application relates to the field of drone technology, and in particular to a drone navigation method and device. Background Art

[0002] With the rapid development of drone technology, drones are increasingly used in many fields such as aerial photography, environmental monitoring, logistics distribution, and agricultural plant protection. In these applications, the autonomous flight capability of drones is particularly critical, and the core of autonomous flight lies in real-time and accurate acquisition of the drone's position information. Traditional drone positioning and navigation solutions mainly rely on the Global Navigation Satellite System (GNSS), combined with inertial measurement units (IMUs) such as gyroscopes to achieve high-precision positioning. However, GNSS signals are susceptible to interference, obstruction, or multiple reflections in complex environments, resulting in reduced positioning accuracy or even failure. This situation is called a GNSS-denied environment.

[0003] In order to overcome the positioning problem in GNSS-denied environments, a variety of alternative solutions have been proposed, among which visual SLAM (Simultaneous Localization and Mapping) technology has attracted much attention because it does not rely on external signals and can autonomously build environmental maps and locate at the same time. Visual SLAM uses image information captured by the camera and combines it with IMU data to achieve autonomous positioning and navigation of drones in unknown environments. Although visual SLAM has shown great potential in solving the GNSS denial problem, it also has its own shortcomings such as high consumption of computing resources and increased cumulative errors after long-term operation. It is difficult to be used alone as a positioning and navigation solution for drones for long-term and large-scale flight missions. However, simply switching GNSS and visual SLAM as independent modules in the time domain will bring about problems such as inconsistent coordinate systems and missing positioning information within the switching time window.

[0004] Therefore, how to achieve positioning and navigation by combining GNSS and visual information when the drone cannot receive or trust GNSS signals becomes a problem to be solved. Summary of the invention

[0005] In an embodiment of the present application, a drone navigation method is provided to solve the problem of how to achieve positioning and navigation by combining GNSS and visual information when the drone cannot receive or trust GNSS signals.

[0006] In a first aspect, an embodiment of the present application provides a method for unmanned aerial vehicle navigation, comprising: obtaining a GNSS pose and determining its confidence interval; when the confidence of the GNSS pose is in a high confidence interval, the unmanned aerial vehicle adopts the GNSS pose at this time for positioning flight, the visual mapping module uses the GNSS pose to infer the pose of the image frame, and executes the step of constructing a visual map based on the pose of the image frame; when the confidence of the GNSS pose is in a medium confidence interval, the credibility of the current GNSS pose is judged by using the constructed visual map, if it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map, if it is unreliable, the GNSS pose is discarded, and the unmanned aerial vehicle performs positioning flight with the pose of the current image frame obtained based on the visual map; when the confidence of the GNSS pose is in an invalid interval, the constructed visual map or the updated visual map is used to match the feature points in the current image frame and the visual map to estimate the current pose of the unmanned aerial vehicle.

[0007] In one possible implementation, the visual mapping module uses the GNSS pose to infer the pose of the image frame, including: obtaining the pose of the drone in the world coordinate system as the drone pose, and using the drone pose and camera extrinsics to infer the pose of the image frame through matrix multiplication.

[0008] In a possible implementation, the steps of constructing the visual map include: taking the first frame image acquired by the visual mapping module as the initial key frame, and iteratively executing the key frame adding step to the visual map; selecting a part of the key frames in the set of existing key frames to form a key frame subset, and matching the new key frame with each key frame in the key frame subset to obtain a set of feature point matching pairs; iteratively executing the iteration step for each matching pair in the set of feature point matching pairs until each key frame in the key frame subset completes feature point matching with the new key frame, so as to obtain a set of all newly added map points and map points with newly added observation points as a set of map points to be updated; and using a multi-frame triangulation algorithm to calculate the coordinates of all map points in the set of map points to be updated to complete this visual map update.

[0009] In a possible implementation, the key frame adding step includes: calculating the distance metric between the current image frame and the image pose of the latest key frame, and adding the current image frame whose distance metric is greater than or equal to a preset threshold as a new key frame; wherein the distance metric is calculated by taking the weighted sum of the optical center translation distance and the camera rotation angle as a unified distance metric.

[0010] In a possible implementation, the iteration step includes: if a feature point of a key frame in the feature point matching pair set does not find a corresponding observed map point in the visual map, adding a new map point in the visual map, and using the feature point matched with the feature point as the initial observation point of the map point; if a feature point of a key frame in the feature point matching pair set corresponds to an observed map point in the visual map, adding a new observation point to the observed map point; wherein the newly added observation point is a feature point of a new key frame.

[0011] In a possible implementation, when the confidence of the GNSS pose is in a medium confidence interval, the constructed visual map is used to judge the credibility of the current GNSS pose. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. If it is unreliable, the GNSS pose is discarded, and the UAV performs positioning flight with the pose of the current image frame obtained based on the visual map, including: extracting feature points from the new image frame and matching them to map points in the visual map; estimating the pose of the current image frame using the P3P algorithm and the method of minimizing the reprojection error; calculating the mean reprojection error of the matching pairs to judge the credibility of the GNSS pose and obtain an evaluation result; if the evaluation result is credible, using it as a pose constraint to update the visual map to obtain an updated visual map; if the evaluation result is unreliable, discarding the GNSS pose, and the UAV performs positioning flight with the pose of the current image frame.

[0012] In a possible implementation, the constructed visual map or the updated visual map is also optimized, and the optimization step specifically includes: evaluating the distance metric between the current image frame and the latest key frame and the confidence of the GNSS pose to determine whether to add it as a new key frame; matching feature points of the new key frame, updating map points and observation points; calculating and minimizing the reprojection error to adjust the coordinates of the map points and the key frame pose; when the confidence of the GNSS pose is insufficient, performing a bundle adjustment operation to simultaneously optimize the key frame pose and the coordinates of the map points by optimizing the cost function.

[0013] In one possible implementation, the distance metric between the current image frame and the latest key frame and the confidence of the GNSS pose are evaluated to determine whether to add it as a new key frame, including: if the confidence of the GNSS pose is in a high confidence interval and the distance metric is greater than or equal to a first distance metric threshold, adding the current image frame as a new key frame; if the confidence of the GNSS pose is in a medium confidence interval or a failure interval and the distance metric is greater than or equal to a second distance metric threshold, adding the current image frame as a new key frame.

[0014] In one possible implementation, the method for determining the GNSS pose confidence interval includes: when the confidence of the GNSS pose is greater than a first threshold, judging that the confidence of the GNSS pose is in a high confidence interval; when the confidence of the GNSS pose is between the first threshold and the second threshold for a first preset time, judging that the confidence of the GNSS pose is in a medium confidence interval; when the confidence of the GNSS pose is less than or equal to the second threshold, judging that the confidence of the GNSS pose is in a failure interval; when the confidence of the GNSS pose increases to greater than the first threshold and remains for a second preset time, judging that the confidence of the GNSS pose is again in the high confidence interval.

[0015] In a second aspect, an embodiment of the present application provides a drone navigation device, which includes: an acquisition module for acquiring a GNSS pose and determining its confidence interval; a construction step execution module for, when the confidence of the GNSS pose is in a high confidence interval, the drone adopts the GNSS pose at this time for positioning flight, the visual mapping module uses the GNSS pose to infer the pose of the image frame, and executes the visual map construction step based on the pose of the image frame; an updated visual map acquisition module for, when the confidence of the GNSS pose is in a medium confidence interval, using the constructed visual map to determine the credibility of the current GNSS pose, if it is credible, updating the visual map based on the GNSS pose to obtain an updated visual map, if it is unreliable, discarding the GNSS pose, and the drone performs positioning flight with the pose of the current image frame obtained based on the visual map; a matching module for, when the confidence of the GNSS pose is in a failure interval, using the constructed visual map or the updated visual map to match the current image frame with feature points in the visual map to estimate the current pose of the drone.

[0016] One or more technical solutions provided in the embodiments of the present application have at least the following technical effects:

[0017] The embodiment of the present application provides a method for navigation of a drone. When the confidence of the GNSS pose is in a high confidence interval, the drone can directly use GNSS data for high-precision positioning flight. This not only ensures the stable flight of the drone in an open, unobstructed area, but also uses these precise GNSS poses to infer the pose of the image frame through the visual mapping module, further improving the accuracy of visual map construction. When the quality of the GNSS signal decreases and the confidence enters the medium confidence interval, the current pose of the drone is estimated by the constructed visual map, and the credibility of the current GNSS pose is evaluated. When the GNSS pose is evaluated as credible, these poses are used to update the visual map, so that the visual map can dynamically reflect changes in the environment and maintain its timeliness and accuracy. And it can identify and eliminate potential erroneous GNSS data, thereby avoiding the accumulation of positioning errors. When the GNSS signal fails completely and the confidence falls into the failure interval, it completely relies on the visual map for positioning. By matching the feature points in the current image frame and the visual map, the system can estimate the pose of the drone in real time and realize autonomous navigation without GNSS conditions. This capability greatly enhances the survivability and mission execution capabilities of drones in complex environments. In addition, by continuously updating the visual map, the need for repeated calculations is reduced, further improving computing efficiency. This application not only solves the positioning problem in GNSS-denied environments, but also enhances the drone's perception and understanding of the environment through the construction and updating of visual maps. This enables drones to cope with complex and changing flight environments more intelligently, improving their autonomous flight capabilities and mission execution capabilities. This solves the problem of how to achieve positioning and navigation for drones by combining GNSS and visual information when they cannot receive or trust GNSS signals. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0019] Figure 1 A flowchart of a drone navigation method provided in an embodiment of the present application;

[0020] Figure 2 A specific flow chart of the steps for constructing a visual map provided in an embodiment of the present application;

[0021] Figure 3A specific flow chart provided in an embodiment of the present application for determining the credibility of a current GNSS posture using a constructed visual map when the confidence of the GNSS posture is in a medium confidence interval, and updating the visual map based on the GNSS posture to obtain an updated visual map if the confidence is credible, and discarding the GNSS posture if the confidence is uncredible, and for the UAV to perform positioning flight with the posture of the current image frame obtained based on the visual map;

[0022] Figure 4 A specific flow chart of the steps for optimizing the constructed visual map or the updated visual map provided in the embodiment of the present application;

[0023] Figure 5 A schematic diagram of a drone navigation device provided in an embodiment of the present application;

[0024] Figure 6 A schematic diagram of a drone navigation server provided in an embodiment of the present application. DETAILED DESCRIPTION

[0025] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0026] The following describes some of the techniques involved in the embodiments of the present application to facilitate understanding, and they should be considered as merely exemplary. Therefore, it should be appreciated by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope and spirit of the present application. Similarly, for the sake of clarity and conciseness, some descriptions of well-known functions and structures are omitted in the following description.

[0027] The embodiment of the present application provides a method for drone navigation, which is applied to the transition of drones from an environment with stable GNSS signals to an environment with limited or completely ineffective GNSS signals. When the drone takes off, it is generally in a state of good GNSS signals, which provides conditions for the visual mapping module to construct a visual map using GNSS poses. When the drone fails during flight due to interference, occlusion, or multiple reflections in a complex environment, the constructed visual map can be used for positioning seamlessly for drone navigation. Figure 1 As shown, the method includes steps S101 to S104. Among them, Figure 1 This is only an execution order shown in the embodiment of the present application, and does not represent the only execution order of a drone navigation method. If the final result can be achieved, Figure 1The steps shown may be performed in parallel or reversed.

[0028] S101: Obtain GNSS pose and determine its confidence interval.

[0029] It should be noted that the GNSS pose includes position and attitude information, that is, the 6-DOF pose of the drone. The visual mapping module uses the GNSS pose to infer the pose of the image frame, including: obtaining the pose of the drone in the world coordinate system as the pose of the drone, and using the drone pose and camera external parameters to infer the pose of the image frame through matrix multiplication.

[0030] Specifically, according to the formula Estimate the pose of the image frame. is the estimated pose of the image frame in the world coordinate system. It is the pose of the UAV carrier coordinate system relative to the world coordinate system, that is, the GNSS pose. The GNSS pose can be obtained through the global navigation satellite system, including 3 translational degrees of freedom and 3 rotational degrees of freedom. is the camera external parameter. The inverse matrix of the image frame's pose is: .

[0031] It should be noted that the camera extrinsic parameters are the extrinsic parameters of the camera relative to the drone, which refers to a transformation matrix that describes the relative position between the camera coordinate system and the drone. The camera extrinsic parameters are determined after the drone and camera are installed together and do not need to be updated in real time during flight.

[0032] The method for determining the confidence interval of the GNSS posture includes: when the confidence of the GNSS posture is greater than a first threshold, the confidence of the GNSS posture is judged to be in a high confidence interval. When the confidence of the GNSS posture is between the first threshold and the second threshold for a first preset time, the confidence of the GNSS posture is judged to be in a medium confidence interval. The value range of the first preset time is [50ms, 500ms]. When the confidence of the GNSS posture is less than or equal to the second threshold, the confidence of the GNSS posture is judged to be in a failure interval. When the confidence of the GNSS posture increases to greater than the first threshold and remains for a second preset time, the confidence of the GNSS posture is judged to be in a high confidence interval again. The value range of the second preset time is [500ms, 2000ms].

[0033] Specifically, when the confidence of the GNSS posture is between the first threshold and the second threshold, the confidence of the GNSS posture will not be immediately judged to be in the medium confidence interval, but a judgment period of the first preset time will be started. For example, when the value range of the first preset time is [50ms, 500ms], it can be judged that the confidence of the GNSS posture is in the medium confidence interval when the confidence of the GNSS posture is 200ms between the first threshold and the second threshold. Of course, it can also be other values ​​as long as it is within the value range of the first preset time.

[0034] Specifically, the selection of the first threshold and the second threshold needs to be tested and adjusted according to the performance and operation effect of the specific GNSS sensor, and the setting of the first threshold and the second threshold mainly depends on experimental evaluation.

[0035] S102: When the confidence of the GNSS pose is in the high confidence interval, the UAV adopts the GNSS pose at this time for positioning flight, the visual mapping module uses the GNSS pose to infer the pose of the image frame, and executes the visual map construction step based on the pose of the image frame.

[0036] Figure 2 A specific flow chart of the steps for constructing a visual map provided in the embodiment of the present application, such as Figure 2 As shown, it includes steps S201 to S204.

[0037] S201: Use the first frame of image acquired by the visual mapping module as the initial key frame, and iteratively perform the key frame adding step to the visual map.

[0038] Specifically, when the UAV starts flying and successfully acquires the first frame of image, the image is added to the visual map as an initial keyframe.

[0039] The key frame adding step includes: calculating the distance metric between the current image frame and the image pose of the latest key frame, and adding the current image frame whose distance metric is greater than or equal to a preset threshold as a new key frame. The distance metric is calculated by taking the weighted sum of the optical center translation distance and the camera rotation angle as a unified distance metric. It should be noted that the preset threshold is an empirical value.

[0040] Specifically, the distance metric is calculated as: .in, is the distance metric, is the ratio of angle to distance, is the optical center translation distance between the current image frame and the latest key frame, is the camera rotation angle between the current image frame and the latest key frame, is a unified distance metric. A preset threshold is set to determine whether the current image frame should be added as a new keyframe. When the calculated distance metric is greater than or equal to the preset threshold, it is considered that there is a significant pose difference between the current image frame and the latest keyframe, so the current image frame is selected as a new keyframe and added to the visual map. When the calculated distance metric is less than the preset threshold, the current image frame is directly discarded and continues to wait for the next input image to be processed.

[0041] S202: Select a part of key frames from the existing key frame set to form a key frame subset, perform feature point matching between the new key frame and each key frame in the key frame subset, and obtain a feature point matching pair set.

[0042] Specifically, the new keyframe When it is added to the visual map, the set of key frames already in the visual map is In order to balance the amount of computation and the mapping effect, a subset is selected from the existing set of key frames for feature point matching. This subset consists of the latest added key frames, namely .in, is a parameter selected based on the compromise between computational effort and mapping effect, satisfying , m can be adjusted according to the actual situation. Match feature points with each keyframe in the keyframe subset. You can choose feature point extraction methods such as ORB, SURF, SIFT, SuperPoint, etc. These feature points have descriptors to facilitate subsequent matching. You can also choose feature points such as GFTT and obtain matching pairs through methods such as optical flow. It should be noted that the new keyframe By matching feature points with each keyframe in the keyframe subset, the new keyframe and Perform feature point matching. For a subset of keyframes For any key frame in the frame, the obtained feature point matching pair set is ,These matching pairs represent the corresponding feature points between the two key frames.

[0043] S203: Iteratively perform the iteration step for each matching pair in the feature point matching pair set until each key frame in the key frame subset completes feature point matching with the new key frame, so as to obtain a set of all newly added map points and map points with newly added observation points as the set of map points to be updated.

[0044] The iteration step includes: if the feature point of the key frame in the feature point matching pair set does not find a corresponding observed map point in the visual map, add a new map point in the visual map, and use the feature point matched with the feature point as the initial observation point of the map point. If the feature point of the key frame in the feature point matching pair set corresponds to an observed map point in the visual map, add an observation point to the observed map point. The newly added observation point is the feature point of the new key frame.

[0045] Specifically, for each matching pair in the feature point matching pair set Iterate the iteration step, if the feature point That is, if the feature point in the feature point matching pair set does not find a corresponding observed map point in the visual map, a new map point is added to the visual map. Then, the feature points and As a map point The initial observation points are added. This means that the map points The position and attributes of can be initialized and estimated based on these two observation points. Already corresponds to an observed map point in the visual map , for the observed map points Add a new observation point . Observed map points The increase in the number of observations of helps to improve the accuracy and stability of its position estimation. Each keyframe in the new keyframe After completing feature point matching and visual map updating, we can obtain the set of all newly added map points and map points with newly added observation points, i.e. the set of map points to be updated. In this set, each map point has more than or equal to 2 observation points, which come from different keyframes, which helps to enhance the reliability and accuracy of the map point.

[0046] S204: Use a multi-frame triangulation algorithm to calculate the coordinates of all map points in the set of map points to be updated, and complete this visual map update.

[0047] It should be noted that the coordinates of the map points in this application are the three-dimensional coordinates of the map points.

[0048] Furthermore, for each map point in the set of map points to be updated , which is observed by multiple points Observed in different image frames. Each observation point The corresponding image frame has a pose , and the pixel coordinates of the observation point in the image frame are For each observation point First, use the camera's intrinsic parameter matrix K (a 3×3 matrix) to convert its pixel coordinates Convert to normalized coordinates The transformation expression of normalized coordinates is: .in, represents the inverse matrix of the camera's intrinsic parameter matrix K, The pixel coordinates to which the homogeneous coordinate 1 is added to form a 3×1 vector. Normalized coordinates are coordinates that are scaled relative to the camera optical center (image center) and focal length. They are in the camera coordinate system but are still two-dimensional. Normalized coordinates eliminate the influence of the camera's intrinsic parameters on the image coordinates, making it more straightforward to use the relative pose between image frames to recover the three-dimensional position of the map point. For each observation point , the corresponding image frame pose It can be decomposed into a rotation matrix and translation vectors According to the principle of multi-frame triangulation, map points can be The coordinates of construct linear equations. Let , , They are Lines 1, 2, and 3 of , , They are The first, second and third elements of , then the linear equation constructed about the coordinates of the map point is: Since only two of the three lines of the linear equation above are linearly independent, select any two of them to construct a part of the overdetermined equation Ax=b. All n observation points can construct a 2n×3 matrix A and a 2n×1 vector b. By solving the least squares solution of the overdetermined system of equations Ax=b, that is, , you can get the map point The three-dimensional coordinates of . Where x is the three-dimensional coordinate of the map point to be found, is the transposed matrix of matrix A. Repeat the above steps to traverse the set of map points to be updated For each map point in , calculate its three-dimensional coordinates.

[0049] It should be noted that since each observation point is observed by n other observation points, because at least three non-collinear points are required to determine a point in three-dimensional space, n linear equations will be obtained. However, since each linear equation actually contains only three independent terms (corresponding to the map points 3D coordinates), these equations are not completely independent, so this system of equations cannot be solved directly. To solve this problem, we are actually constructing an overdetermined system of equations. Overdetermined systems of equations usually have no exact solution, but the least squares method can be used to find an optimal solution that minimizes the sum of squared residuals among all possible solutions.

[0050] S103: When the confidence of the GNSS pose is in the medium confidence interval, the constructed visual map is used to determine the credibility of the current GNSS pose. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. If it is unreliable, the GNSS pose is discarded, and the UAV performs positioning flight based on the pose of the current image frame obtained based on the visual map.

[0051] It should be noted that when the visual mapping module uses the GNSS pose to infer the pose of the image frame, that is, when the visual map is constructed or updated based on the GNSS pose, it is necessary to first determine the confidence threshold of the GNSS pose. When the threshold is greater than the first threshold, the GNSS pose is used to infer the pose of the image frame. Otherwise, the pose is discarded and not used for constructing or updating the visual map.

[0052] Figure 3 The embodiment of the present application provides a specific flow chart of when the confidence of the GNSS pose is in the medium confidence interval, using the constructed visual map to judge the credibility of the current GNSS pose, if it is credible, updating the visual map based on the GNSS pose, obtaining an updated visual map, and if it is unreliable, discarding the GNSS pose, and the UAV performing positioning flight with the pose of the current image frame obtained based on the visual map, as shown in the figure. Figure 3 As shown, it includes steps S301 to S305.

[0053] S301: Extract feature points from the new image frame and match them to map points in the visual map.

[0054] Specifically, the new image frame The feature points of the visual map are matched with the descriptors of all map points to obtain a set of matching pairs. .in, Corresponding to the set of map points to be updated The map point in Corresponding to image frames The feature point set in The feature points in .

[0055] S302: Estimate the pose of the current image frame using the P3P algorithm and the method of minimizing the reprojection error.

[0056] Specifically, when the number of matching pairs is less than 3, the current image positioning fails because sufficient geometric constraints cannot be constructed. When the number of matching pairs is greater than or equal to 3, the P3P (English: Perspective-Three-Point) algorithm is used to preliminarily estimate the initial pose of the current image frame. Then, the pose estimation is further optimized by minimizing the reprojection error. , is the feature point in the image frame that successfully matches the map point. is a map point, and its coordinates in the world coordinate system are This coordinate needs to be transformed into the current camera coordinate system, that is, calculated .in, It is a map point Coordinates in the camera coordinate system. Due to the camera imaging model, 3D points are usually projected onto the normalized plane of the camera to simplify subsequent calculations. Normalized coordinates . Among them, z is the third dimension of the coordinates in the camera coordinate system. With the normalized coordinates, the camera's intrinsic parameter matrix K can be used to calculate the map point In the image frame The reprojected coordinates in the camera. The camera's intrinsic parameter matrix K usually contains parameters such as focal length and optical center, which is used to project points in three-dimensional space onto the two-dimensional image plane. The reprojected coordinates are calculated by the following formula: .in, is the homogeneous form of the normalized coordinates, used to multiply with the intrinsic parameter matrix, is the reprojected coordinate. Calculate the matching pair The reprojection error is: .in, is the reprojection error of the jth matching pair, is the actual observed position of the jth feature point on the image, i.e., the pixel coordinate, is the predicted position of the jth feature point on the image, i.e. the reprojected position. Construct a least squares problem for all r matching pairs and solve the optimal camera pose by optimizing the objective function , so that the sum of squares of the reprojection error is minimized, the objective function is: .in, To solve the optimal camera pose, is a mathematical operator used to find the parameter value that minimizes the function. In this formula, it is used to find the optimal camera pose.

[0057] S303: Calculate the mean reprojection error of the matching pair to determine the credibility of the GNSS pose and obtain an evaluation result.

[0058] Specifically, the formula for calculating the mean reprojection error of the matching pair is: .in, is the mean reprojection error of all matching pairs under a given camera pose, The smaller the value, the more the camera pose matches the actual observed data, that is, the higher the credibility of the evaluation result. r is the total number of matching pairs, and j is the index variable.

[0059] S304: If the evaluation result is credible, use it as a pose constraint to update the visual map and obtain an updated visual map.

[0060] S305: If the evaluation result is unreliable, the GNSS posture is discarded, and the UAV performs positioning flight at the posture of the current image frame.

[0061] Set judgment threshold , when the mean projection error of the matching pair When , the evaluation result is considered unreliable, the GNSS pose is discarded, and the UAV is positioned and flies at the pose of the current image frame. When , the evaluation result is considered to be credible, and it is used as the pose constraint to update the visual map to obtain the updated visual map.

[0062] S104: When the confidence of the GNSS pose is in the failure interval, the constructed visual map or the updated visual map is used to match the current image frame with feature points in the visual map to estimate the current pose of the UAV.

[0063] Specifically, when the confidence of the GNSS position is again in the high confidence interval, the UAV adopts the GNSS position at this time for flight.

[0064] Figure 4 A specific flow chart of the steps for optimizing the constructed visual map or the updated visual map provided in the embodiment of the present application, such as Figure 4 As shown, it includes steps S401 to S404.

[0065] S401: Evaluate the distance metric between the current image frame and the latest key frame and the confidence of the GNSS pose to determine whether to add it as a new key frame.

[0066] Evaluate the distance metric between the current image frame and the latest key frame and the confidence of the GNSS pose to determine whether to add it as a new key frame, including: if the confidence of the GNSS pose is in a high confidence interval and the distance metric is greater than or equal to a first distance metric threshold, add the current image frame as a new key frame. If the confidence of the GNSS pose is in a medium confidence interval or an invalid interval and the distance metric is greater than or equal to a second distance metric threshold, add the current image frame as a new key frame.

[0067] It should be noted that the first distance metric threshold is smaller than the second distance metric threshold.

[0068] S402: Perform feature point matching on the new key frame and update the map points and observation points.

[0069] Specifically, each time a new key frame is added, the feature point matching process is first performed. This process aims to obtain the set of all newly added map points and map points with newly added observation points, that is, the set of map points to be updated. .

[0070] S403: Calculate and minimize the reprojection error to adjust the coordinates of the map points and the key frame poses.

[0071] Specifically, for each map point in the set , whose corresponding observation set contains multiple observation feature points , set the feature point The pose of the corresponding key frame is , map point The three-dimensional coordinates of According to the definition of reprojection error, the map point can be calculated In keyframe The reprojection error in This error depends on the position of the key frame , which also depends on the 3D coordinates of the map points .

[0072] S404: When the confidence of the GNSS pose is insufficient, a bundle adjustment operation is performed to simultaneously optimize the pose of the key frame and the coordinates of the map points by optimizing the cost function.

[0073] Specifically, for each key frame in the visual map If the corresponding GNSS pose is credible (i.e. the confidence is high enough), the pose of the keyframe can be defined The error relative to its GNSS pose is: This error is obtained by calculating the logarithmic mapping difference between the two poses. represents the logarithmic mapping operation of the matrix Lie group, The pose of the key frame The error relative to its GNSS pose, is the pose of the key frame, The pose provided by the global navigation satellite system for the keyframe.

[0074] Specifically, when the confidence of the GNSS pose is insufficient, a bundle adjustment operation is performed for each new keyframe. The bundle adjustment operation (BA operation) adjusts the pose of the keyframe and the 3D coordinates of the map point simultaneously by optimizing a cost function. The cost function consists of two parts: 1. For all map points and their observation points, calculate the sum of the squares of the reprojection errors, and multiply by 2. For the key frame set U whose GNSS pose confidence falls within the medium confidence interval and is determined to be credible, calculate the sum of the squares of the GNSS pose errors of these key frames and multiply them by a weight coefficient α for weighting.

[0075] The expression of this cost function J is: .in, is the cost function used to quantify the difference between the pose and map point of the current keyframe and the ideal value. represents the poses of all keyframes involving the set of map points and whose confidence lies in the medium confidence interval or the failure interval, are the 3D coordinates of s map points, r is the number of observation points of the map points, i is the index variable, s is the total number of map points, j is the index variable, r is the number of observation points of each map point, U is the set of keyframes whose GNSS pose confidence is in the medium confidence interval and the evaluation result is credible, is the weight coefficient, is the reprojection error of the map point in the keyframe where its jth observation point is located. By minimizing the cost function, the pose of the keyframe and the 3D coordinates of the map point can be optimized, thereby improving the accuracy and consistency of the visual map.

[0076] The present application also provides a drone navigation device 500, such as Figure 5 As shown, the device includes: an acquisition module 501, a construction step execution module 502, an updated visual map acquisition module 503 and a matching module 504.

[0077] The acquisition module 501 is used to acquire the GNSS position and determine its confidence interval.

[0078] The execution construction step module 502 is used for when the confidence of the GNSS posture is in the high confidence interval, the UAV adopts the GNSS posture at this time for positioning flight, the visual mapping module uses the GNSS posture to infer the posture of the image frame, and executes the visual map construction step based on the posture of the image frame.

[0079] The module 503 for obtaining an updated visual map is used to determine the credibility of the current GNSS posture using the constructed visual map when the confidence of the GNSS posture is in the medium confidence interval. If it is credible, the visual map is updated based on the GNSS posture to obtain an updated visual map. If it is unreliable, the GNSS posture is discarded, and the UAV performs positioning flight based on the posture of the current image frame obtained based on the visual map.

[0080] The matching module 504 is used to estimate the current posture of the UAV by matching the current image frame with feature points in the visual map using the constructed visual map or the updated visual map when the confidence of the GNSS posture is in the failure interval.

[0081] Some modules in the apparatus described in the present application can be described in the general context of computer executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, classes, etc. that perform specific tasks or implement specific abstract data types. The present application can also be practiced in distributed computing environments, in which tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules can be located in local and remote computer storage media including storage devices.

[0082] The devices or modules described in the above application embodiments can be implemented by computer chips or entities, or by products with certain functions. For the convenience of description, the above devices are described in various modules according to their functions. When implementing the embodiments of the present application, the functions of each module can be implemented in the same or multiple software and / or hardware. Of course, the module that implements a certain function can also be implemented by combining multiple sub-modules or sub-units.

[0083] The methods, devices or modules described in this application can be implemented in the form of computer-readable program codes. The controller can be implemented in any appropriate manner. For example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing computer-readable program codes (such as software or firmware) that can be executed by the (micro)processor, logic gates, switches, application-specific integrated circuits (English: Application Specific Integrated Circuit, referred to as: ASIC), programmable logic controllers and embedded microcontrollers. Examples of controllers include but are not limited to the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20 and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art also know that in addition to implementing the controller in the form of pure computer-readable program codes, the controller can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, this controller can be considered as a hardware component, and the devices included in it for implementing various functions can also be regarded as structures within the hardware component. Or even, the means for realizing various functions may be regarded as both a software module for realizing the method and a structure within a hardware component.

[0084] like Figure 6 As shown, an embodiment of the present application also provides a drone navigation server, including a memory 601 and a processor 602; the memory 601 is used to store computer executable instructions; the processor 602 is used to execute computer executable instructions to implement a drone navigation method described above in an embodiment of the present application.

[0085] An embodiment of the present application also provides a computer-readable storage medium, which stores executable instructions. When a computer executes the executable instructions, it can implement the drone navigation method described above in the embodiment of the present application.

[0086] It can be seen from the description of the above implementation methods that those skilled in the art can clearly understand that the present application can be implemented by means of software plus necessary hardware. Based on this understanding, the technical solution of the present application can be essentially or partly contributed to the prior art in the form of a software product, or it can be reflected in the implementation process of data migration. The computer software product can be stored in a storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a mobile terminal, a server, or a network device, etc.) to execute the method described in the embodiment of the present application.

[0087] The various embodiments in this specification are described in a progressive manner, and the same or similar parts between the various embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. All or part of this application can be used in many general or special computer system environments or configurations.

[0088] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit the present application. Although the present application has been described in detail with reference to the aforementioned embodiments, a person of ordinary skill in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the present application.

Claims

1. A drone navigation method, characterized in that: include: Get GNSS pose and determine its confidence interval; When the confidence of the GNSS pose is in the high confidence interval, the UAV uses the GNSS pose at this time for positioning flight, and the visual mapping module uses the GNSS pose to infer the pose of the image frame, and executes the visual map construction step based on the pose of the image frame; When the confidence of the GNSS pose is in the medium confidence interval, the credibility of the current GNSS pose is determined by using the constructed visual map. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. If it is uncredible, the GNSS pose is discarded, and the UAV performs positioning flight with the pose of the current image frame obtained based on the visual map. When the confidence of the GNSS pose is in the medium confidence interval, the credibility of the current GNSS pose is judged by using the constructed visual map. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. If it is unreliable, the GNSS pose is discarded, and the UAV performs positioning flight with the pose of the current image frame obtained based on the visual map, including: extracting feature points from the new image frame and matching them to map points in the visual map; estimating the pose of the current image frame by using the P3P algorithm and the method of minimizing the reprojection error; calculating the mean reprojection error of the matching pairs to judge the credibility of the GNSS pose and obtain an evaluation result; if the evaluation result is credible, using it as a pose constraint to update the visual map to obtain an updated visual map; if the evaluation result is unreliable, discarding the GNSS pose, and the UAV performs positioning flight with the pose of the current image frame; When the confidence of the GNSS pose is in the failure interval, the constructed visual map or the updated visual map is used to match the feature points in the current image frame and the visual map to estimate the current pose of the UAV.

2. The drone navigation method according to claim 1, characterized in that: The visual mapping module uses the GNSS pose to infer the pose of the image frame, including: The pose of the UAV in the world coordinate system is obtained as the UAV pose, and the pose of the image frame is calculated by matrix multiplication using the UAV pose and camera extrinsics.

3. The drone navigation method according to claim 1, characterized in that: The steps of constructing the visual map include: The first frame of image acquired by the visual mapping module is used as the initial key frame, and the key frame adding step is iteratively performed to the visual map; In the set of existing key frames, a part of key frames is selected to form a key frame subset, and feature point matching is performed between the new key frame and each key frame in the key frame subset to obtain a feature point matching pair set; Iteratively perform the iteration step for each matching pair in the feature point matching pair set until each key frame in the key frame subset completes feature point matching with the new key frame, so as to obtain a set of all newly added map points and map points with newly added observation points as a set of map points to be updated; The coordinates of all map points in the set of map points to be updated are calculated using a multi-frame triangulation algorithm to complete this visual map update.

4. The drone navigation method according to claim 3, characterized in that: The key frame adding step comprises: Calculate the distance metric between the current image frame and the image pose of the latest key frame, and add the current image frame whose distance metric is greater than or equal to a preset threshold as a new key frame; wherein the distance metric is calculated by taking the weighted sum of the optical center translation distance and the camera rotation angle as a unified distance metric.

5. The drone navigation method according to claim 3, characterized in that: The iterative steps include: If a feature point of a key frame in the feature point matching pair set does not find a corresponding observed map point in the visual map, a new map point is added to the visual map, and the feature point that matches the feature point is used as the initial observation point of the map point; If a feature point of a key frame in the feature point matching pair set corresponds to an observed map point in the visual map, a new observation point is added to the observed map point; wherein the newly added observation point is a feature point of a new key frame.

6. The drone navigation method according to claim 1, characterized in that: The method further includes optimizing the constructed visual map or the updated visual map, wherein the optimization step specifically includes: Evaluate the distance metric between the current image frame and the latest keyframe and the confidence of the GNSS pose to determine whether to add it as a new keyframe; Match feature points of new keyframes and update map points and observation points; Calculate and minimize the reprojection error to adjust the coordinates of map points and keyframe poses; When the confidence of the GNSS pose is insufficient, a bundle adjustment operation is performed to simultaneously optimize the pose of the keyframe and the coordinates of the map points by optimizing the cost function.

7. The drone navigation method according to claim 6, characterized in that: The step of evaluating the distance measurement between the current image frame and the latest key frame and the confidence of the GNSS pose to determine whether to add it as a new key frame includes: If the confidence of the GNSS pose is in a high confidence interval and the distance metric is greater than or equal to a first distance metric threshold, adding the current image frame as a new key frame; If the confidence of the GNSS pose is in a medium confidence interval or a failure interval and the distance metric is greater than or equal to a second distance metric threshold, the current image frame is added as a new key frame.

8. The drone navigation method according to claim 1, characterized in that: Methods for determining GNSS pose confidence intervals include: When the confidence of the GNSS posture is greater than the first threshold, it is determined that the confidence of the GNSS posture is in a high confidence interval; When the confidence of the GNSS posture is between the first threshold and the second threshold for a first preset time, determining that the confidence of the GNSS posture is in a medium confidence interval; When the confidence level of the GNSS posture is less than or equal to the second threshold, determining that the confidence level of the GNSS posture is in a failure interval; When the confidence level of the GNSS position increases to be greater than the first threshold and is maintained for a second preset time, it is determined that the confidence level of the GNSS position is again in the high confidence interval.

9. A drone navigation device, characterized in that: include: The acquisition module is used to obtain the GNSS pose and determine its confidence interval; The execution construction step module is used for when the confidence of the GNSS posture is in the high confidence interval, the UAV adopts the GNSS posture at this time for positioning flight, the visual mapping module uses the GNSS posture to infer the posture of the image frame, and executes the visual map construction step based on the posture of the image frame; The updated visual map module is used to determine the credibility of the current GNSS pose by using the constructed visual map when the confidence of the GNSS pose is in the medium confidence interval. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. If it is not credible, the GNSS pose is discarded. The UAV performs positioning flight with the pose of the current image frame obtained based on the visual map. When the confidence of the GNSS pose is in the medium confidence interval, the credibility of the current GNSS pose is determined by using the constructed visual map. If it is credible, the visual map is updated based on the GNSS pose to obtain an updated visual map. map, if it is unreliable, the GNSS pose is discarded, and the UAV performs positioning flight with the pose of the current image frame obtained based on the visual map, including: extracting feature points from the new image frame and matching them to map points in the visual map; estimating the pose of the current image frame by using the P3P algorithm and the method of minimizing the reprojection error; calculating the mean reprojection error of the matching pair to judge the credibility of the GNSS pose and obtain an evaluation result; if the evaluation result is reliable, using it as a pose constraint to update the visual map to obtain an updated visual map; if the evaluation result is unreliable, discarding the GNSS pose, and the UAV performs positioning flight with the pose of the current image frame; The matching module is used to estimate the current posture of the UAV by matching the current image frame with the feature points in the visual map using the constructed visual map or the updated visual map when the confidence of the GNSS posture is in the failure interval.

Images