Path planning method and system of unmanned aerial vehicle, unmanned aerial vehicle and storage medium

Abstract

The invention relates to the technical field of unmanned aerial vehicles. The invention discloses a path planning method and system of an unmanned aerial vehicle, the unmanned aerial vehicle and a storage medium, which can ensure stable pose output in an extreme environment so as to improve the accuracy of path planning of the unmanned aerial vehicle. The path planning method of the unmanned aerial vehicle comprises the steps that destination information, a first depth image and a second depth image are acquired, and the first depth image and the second depth image show the same scene at different angles; performing feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points and a plurality of second feature points; under the condition that the first matching result is successful matching, positioning the plurality of first feature points and the plurality of second feature points by adopting an EPnP algorithm in combination with an RANSAC algorithm to obtain first pose information; and planning a first flight path of the unmanned aerial vehicle based on the first pose information and the destination information.

Description

Technical Field

[0001] The present application relates to the field of drone technology. More specifically, the present application relates to a drone path planning method, system, drone and storage medium. Background Art

[0002] Traditional UAV path planning methods usually only use the EPnP algorithm to identify and process the images collected by the UAV to obtain the UAV's posture information. However, in extreme environments, image data may be more seriously interfered by noise, such as extreme lighting conditions, image blur, or sensor failure. These noises will affect the correspondence between 3D points and 2D projection points, affect the accuracy of the EPnP algorithm, and cause the posture output by the EPnP algorithm to be inaccurate, thereby affecting the accuracy of the UAV's path planning. Summary of the invention

[0003] The purpose of the embodiments of the present application is to provide a method, system, drone and storage medium for drone path planning, which can ensure stable posture output in extreme environments to improve the accuracy of drone path planning. The embodiments of the present application are mainly implemented through the following technical solutions:

[0004] A first aspect of an embodiment of the present application provides a path planning method for a drone, comprising:

[0005] Acquire destination information, a first depth image and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles;

[0006] Performing feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points;

[0007] When the first matching result is a successful match, the EPnP algorithm is combined with the RANSAC algorithm to perform positioning processing on the plurality of first feature points and the plurality of second feature points to obtain first pose information;

[0008] A first flight path of the UAV is planned based on the first posture information and the destination information.

[0009] According to an embodiment of the present application, the step of performing feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points includes:

[0010] Scaling the first depth image to obtain a first image to be processed;

[0011] Performing scaling processing on the second depth image to obtain a second image to be processed;

[0012] Extracting a plurality of first pixel points in the first image to be processed to obtain a plurality of third feature points, and performing region allocation on the plurality of third feature points to obtain the plurality of first feature points;

[0013] Extracting a plurality of second pixel points in the second image to be processed to obtain a plurality of fourth feature points, and performing region allocation on the plurality of fourth feature points to obtain the plurality of second feature points;

[0014] Setting a first preset threshold;

[0015] calculating the number of identical features between the plurality of first feature points and the plurality of second feature points;

[0016] When the number of identical items is greater than the first preset threshold, the first matching result is a successful match;

[0017] When the same number is equal to or less than the first preset threshold, the first matching result is a failed match.

[0018] According to one embodiment of the present application, after the step of planning the first flight path of the drone based on the first posture information and the destination information, the path planning method of the drone further includes:

[0019] Constructing a first map based on the first posture information;

[0020] Performing loop closure detection on the third feature point and the fourth feature point to obtain second pose information and a fusion map;

[0021] generating third posture information based on the first map and the second posture information;

[0022] A second flight path of the UAV is planned based on the third posture information and the destination information.

[0023] According to an embodiment of the present application, the step of performing loop detection on the third feature point and the fourth feature point to obtain second pose information and a fusion map includes:

[0024] Acquire a second map; set a second preset threshold;

[0025] Perform loop detection based on the third feature point and the fourth feature point to obtain a detection result;

[0026] Calculating a confidence parameter of the detection result, where the confidence parameter is used to characterize the confidence that the third feature point and the fourth feature point are historical loop positions passed by the drone;

[0027] When the confidence parameter is greater than the second preset threshold, generating a detection map;

[0028] Fusing the second map with the detection map to obtain a fused map;

[0029] Second pose information is generated based on the fused map.

[0030] According to an embodiment of the present application, after the step of performing loop detection on the third feature point and the fourth feature point to obtain the second pose information and the fusion map, the path planning method of the drone further includes:

[0031] Performing loop optimization on the fused map to obtain a first optimization result;

[0032] Outputting fourth posture information based on the first optimization result and the first map;

[0033] A third flight path of the UAV is planned based on the fourth posture information and the destination information.

[0034] According to an embodiment of the present application, after the step of performing loop optimization on the fused map to obtain a first optimization result, the path planning method of the UAV further includes:

[0035] Perform global optimization based on the first optimization result to obtain a second optimization result;

[0036] Outputting fifth pose information based on the second optimization result and the first map;

[0037] A fourth flight path of the UAV is planned based on the fifth posture information and the destination information.

[0038] According to one embodiment of the present application, after the step of performing global optimization based on the first optimization result to obtain a second optimization result, the path planning method of the UAV further includes:

[0039] Performing local optimization on the first map to obtain a third optimization result;

[0040] Jointly optimizing the third optimization result and the second optimization result to obtain a fourth optimization result;

[0041] Generate sixth posture information based on the fourth optimization result;

[0042] A fifth flight path of the UAV is planned based on the sixth posture information and the destination information.

[0043] A second aspect of the embodiments of the present application provides a path planning system for an unmanned aerial vehicle, including:

[0044] An acquisition module, configured to acquire destination information, a first depth image and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles;

[0045] A matching module, configured to perform feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points;

[0046] A positioning module, configured to, when the first matching result is a successful match, use an EPnP algorithm combined with a RANSAC algorithm to perform positioning processing on the plurality of first feature points and the plurality of second feature points to obtain first pose information;

[0047] A planning module is used to plan a first flight path of the UAV based on the first posture information and the destination information.

[0048] According to a third aspect of an embodiment of the present application, a drone is provided, comprising: a processor and a memory, the memory being used to store a computer program, the processor being used to call and run the computer program stored in the memory, and executing the steps of the drone path planning method described in the first aspect of the embodiment of the present application.

[0049] In a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium is used to store a computer program, and the computer program enables a computer to execute the steps of the path planning method for a drone as described in the first aspect of the embodiment of the present application.

[0050] The beneficial effects of the embodiments of the present application include:

[0051] In the embodiment of the present application, the RANSAC algorithm is added on the basis of the EPnP algorithm to improve the accuracy of the pose output. Specifically, in the embodiment of the present application, a first matching result is obtained by performing feature matching processing on the first depth image and the second depth image; when the first matching result is a successful match, the EPnP algorithm is combined with the RANSAC algorithm to perform positioning processing on the multiple first feature points and the multiple second feature points to obtain the first pose information; thus, the drone can plan the first flight path based on the first pose information and the destination information. The RANSAC algorithm can optimize the first pose information output by the EPnP algorithm during the iteration process. Therefore, compared with the prior art, the embodiment of the present application can ensure stable pose output in extreme environments to improve the accuracy of drone path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the conventional technology, the drawings required for use in the embodiments or the conventional technology descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0053] Figure 1 A flow chart of a path planning method for a drone of the present application in some embodiments;

[0054] Figure 2 A flow chart of a path planning method for a drone of the present application in other embodiments;

[0055] Figure 3 A reference block diagram of the process for analyzing the motion state of each independent obstacle in this application;

[0056] Figure 4 This is a principle block diagram of the path planning system of the UAV of the present application in some embodiments;

[0057] Figure 5 This is a principle block diagram of the drone of the present application in some embodiments;

[0058] Figure 6 A reference diagram of the state of the drone of this application during flight;

[0059] Figure 7 This is the hardware topology diagram of the drone of this application. DETAILED DESCRIPTION

[0060] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific implementation methods of the present application are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present application. However, the present application can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present application, so the present application is not limited by the specific embodiments disclosed below.

[0061] It should be noted that the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0062] The terms "exemplary" or "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "exemplary" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific way.

[0063] The terms "comprises," "includes," or any other variations thereof are intended to cover a non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or elements is not necessarily limited to those steps or elements explicitly listed but may include other steps or elements not explicitly listed or inherent to such process, method, product, or apparatus.

[0064] Unless otherwise defined, all technical and scientific terms used in the specification of this application have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application. The term "and / or" used in the specification of this application includes any and all combinations of one or more related listed items.

[0065] The specific implementation of the present application is further described below in conjunction with the accompanying drawings.

[0066] <Path planning method for drones>

[0067] refer to Figure 1 shown. Figure 1 A flowchart of a method for planning a path for a drone provided in the first aspect of the present application. Figure 1 In the method, the path planning method of the UAV includes:

[0068] S1. Acquire destination information, a first depth image, and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles.

[0069] The first depth image and the second depth image are both acquired by a camera of a drone, and the resolutions of the two images are both 640*480@30Hz.

[0070] The first depth image and the second depth image are Figure 2 Image Input 30Hz in the

[0071] S2. Perform feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points.

[0072] Furthermore, the S2 step includes:

[0073] S21. Scaling the first depth image to obtain a first image to be processed.

[0074] Specifically, the scaling ratio of the first depth image is 0.707 or 0.5. In other implementations, the scaling ratio may be set by those skilled in the art according to actual needs.

[0075] S22: Scaling the second depth image to obtain a second image to be processed.

[0076] Specifically, the scaling ratio of the second depth image is 0.707 or 0.5. The scaling ratio of the second depth image is the same as the scaling ratio of the first depth image.

[0077] The steps S21 and S22 can be understood as Figure 2 The "Image Preprocessing" step in .

[0078] S23. Extract a plurality of first pixel points in the first image to be processed to obtain a plurality of third feature points, and perform region allocation on the plurality of third feature points to obtain the plurality of first feature points.

[0079] The first pixel points may be pixel points with higher grayscale changes or edge changes. These first pixel points can remain relatively stable under different viewing angles, illuminations and scales, which helps the UAV to accurately locate itself in a complex environment.

[0080] The area allocation method can be understood as dividing according to the density of the third feature point. Exemplarily, the plurality of third feature points are divided into a low-density area, a medium-density area, and a high-density area. The third feature point in the high-density area is used as the first feature point. Of course, in other embodiments, the area allocation method can be set by those skilled in the art according to actual needs.

[0081] S24. Extract a plurality of second pixel points in the second image to be processed to obtain a plurality of fourth feature points, and perform region allocation on the plurality of fourth feature points to obtain the plurality of second feature points.

[0082] The second pixel points may be pixel points with higher grayscale changes or edge changes. These second pixel points can remain relatively stable under different viewing angles, illuminations, and scales, which helps the UAV to accurately locate itself in a complex environment.

[0083] The area allocation method in the step S24 can be understood as dividing according to the density of the fourth feature point. Exemplarily, the plurality of fourth feature points are divided into a low-density area, a medium-density area, and a high-density area. The fourth feature point in the high-density area is used as the second feature point. Of course, in other embodiments, the area allocation method can be set by those skilled in the art according to actual needs.

[0084] Steps S23 and S24 can be understood as Figure 2 The "feature point extraction and region allocation" step in .

[0085] S25. Set a first preset threshold.

[0086] The first preset threshold is used to determine whether the first feature point and the second feature point are matched successfully. The first preset threshold is a positive integer, and its specific value can be set by those skilled in the art according to actual needs.

[0087] S26, calculating the number of identical features between the plurality of first feature points and the plurality of second feature points. Figure 2 The "Feature Matching" step in .

[0088] S27: When the same number is greater than the first preset threshold, the first matching result is a successful match.

[0089] S28. When the same number is equal to or less than the first preset threshold, the first matching result is a failed match.

[0090] S3. When the first matching result is a successful match, use the EPnP (Perspective-n-Point) algorithm combined with the RANSAC algorithm to perform positioning processing on the multiple first feature points and the multiple second feature points to obtain the first pose information.

[0091] Exemplarily, the S3 step may include the following steps:

[0092] S31. Set the number of iterations and the inlier threshold of the RANSAC algorithm.

[0093] S32. Loop through the following steps S33-S38 based on the number of iterations.

[0094] S33: Match the multiple first feature points with the multiple second feature points to obtain multiple pairs of matching points.

[0095] S34. Randomly select 4 matching point pairs from all matching point pairs as target point pairs.

[0096] S35. Calculate a first rough pose using the target point pair using the EPnP algorithm, and add the first rough pose to a rough pose set.

[0097] S36. According to the first rough posture, reproject all the second feature points and calculate the reprojection error.

[0098] S37. Divide the points into internal points and external points according to the reprojection error. When the number of internal points is greater than the internal point threshold, the calculation is considered valid.

[0099] S38. Utilize all the internal points and use the EPnP algorithm again to calculate a second rough pose, and add the second rough pose to the rough pose set.

[0100] S39. Select an element with the smallest reprojection error from the rough pose set as the first pose information.

[0101] S4. Planning a first flight path of the UAV based on the first posture information and the destination information.

[0102] The embodiment of the present application adopts the A* (A Star) algorithm to plan the first posture information and the destination information.

[0103] The drone can perform a flight mission according to the first flight path.

[0104] It should be understood that the A* algorithm maintains a heuristic evaluation function: f(n) = g(n) + h(n); where g(n) is the cost from the starting node to the current node, h(n) is the heuristic function, and f(n) represents the cost from the current node to the end point. The heuristic evaluation function takes the shortest path as the optimization goal.

[0105] Furthermore, considering the calculation speed and path length, combined with the grid model used, the heuristic function is set to the diagonal distance. The specific formula is as follows:

[0106]

[0107] Where D is the cost of moving along the coordinate axis, and the cost of moving along the diagonal is set to nx is the horizontal coordinate of the current point, goal.x is the vertical coordinate of the current point, ny is the horizontal coordinate of the target point, and goal.y is the vertical coordinate of the target point. The embodiment of the present application selects the smallest (highest priority) node as the next node to be traversed. By maintaining an optimal path tree, real-time optimal path planning from the current position to the end point is achieved.

[0108] In the embodiment of the present application, the RANSAC algorithm is added on the basis of the EPnP algorithm to improve the accuracy of the pose output. Specifically, in the embodiment of the present application, a first matching result is obtained by performing feature matching processing on the first depth image and the second depth image; when the first matching result is a successful match, the EPnP algorithm is combined with the RANSAC algorithm to perform positioning processing on the multiple first feature points and the multiple second feature points to obtain the first pose information; thus, the drone can plan the first flight path based on the first pose information and the destination information. The RANSAC algorithm can optimize the first pose information output by the EPnP algorithm during the iteration process. Therefore, compared with the prior art, the embodiment of the present application can ensure stable pose output in extreme environments to improve the accuracy of drone path planning.

[0109] In some embodiments, reference Figure 2 As shown, after the step S4, the path planning method of the UAV further includes:

[0110] S51: construct a first map based on the first posture information. Figure 2 The first map is a point cloud map.

[0111] S52: Perform loop detection on the third feature point and the fourth feature point to obtain second pose information and a fusion map.

[0112] Furthermore, the step S52 includes:

[0113] S521. Obtain a second map.

[0114] The second map is an initial map, which may be a map of the flight area pre-constructed by the UAV before the flight through satellite remote sensing, ground mapping and other technical means.

[0115] S522: Set a second preset threshold.

[0116] The second preset threshold is used to verify the reliability of the confidence parameter. The specific value of the second preset threshold can be determined by those skilled in the art according to actual needs.

[0117] S523: Perform loop detection based on the third feature point and the fourth feature point to obtain a detection result. Figure 2 The "Loop Closure Detection" step in .

[0118] The detection results include loops and non-loops. The drone will store images corresponding to all the locations that have been visited to obtain a historical image set. When the third feature point and the fourth feature point coincide with one or more images in the historical image set, it indicates that the drone has returned to the origin, that is, it has returned to the location that the drone has visited. At this time, the detection result is a loop; when the third feature point and the fourth feature point do not coincide with any image in the historical image set, it indicates that the drone has not returned to the origin, that is, the drone has not returned to the location that has been visited. At this time, the detection result is a non-loop.

[0119] S524. Calculate a confidence parameter of the detection result, where the confidence parameter is used to characterize the confidence that the third feature point and the fourth feature point are historical loop positions passed by the drone.

[0120] The historical loop position is the position that the drone has visited.

[0121] S525: When the confidence parameter is greater than the second preset threshold, generate a detection map. Figure 2 The "confidence judgment" step in .

[0122] S526: Fuse the second map with the detection map to obtain a fused map. Figure 2 The "Map Fusion" step in .

[0123] S527: Generate second position information based on the fused map. Figure 2 The "Pose Calculation" step in .

[0124] S53: Generate third posture information based on the first map and the second posture information.

[0125] S54: planning a second flight path for the UAV based on the third position information and the destination information. The UAV performs a flight mission based on the second flight path.

[0126] In some embodiments, reference Figure 2 As shown, after the step S52, the path planning method of the UAV further includes:

[0127] S61, loop optimization is performed on the fused map to obtain a first optimization result. Figure 2 The "loop closure optimization" step in .

[0128] Exemplarily, a graph optimization algorithm may be used to optimize the fused map to eliminate or reduce errors. The graph optimization algorithm may be g2o (General Graph Optimization) or CeresSolver.

[0129] It should be understood that the first optimization result is a map, a map optimized by the fused map through a graph optimization algorithm.

[0130] S62: Output fourth posture information based on the first optimization result and the first map.

[0131] Exemplarily, feature matching (such as ORB (Oriented FAST and Rotated BRIEF) feature matching) can be performed on the first optimization result and the first map to obtain a second matching result; the current position of the drone, that is, the fourth posture information, is obtained through the second matching result.

[0132] S63: planning a third flight path of the UAV based on the fourth position information and the destination information, and the UAV performing a flight mission according to the third flight path.

[0133] In some embodiments, after the step S61, the path planning method of the drone further includes:

[0134] S71: Perform global optimization based on the first optimization result to obtain a second optimization result. Figure 2 The Global Optimization step in .

[0135] Exemplarily, the present application may use SLAM technology, PrePass optimization technology, ShadowDepth optimization technology or BasePass optimization technology to optimize the first optimization result to obtain the second optimization result.

[0136] It should be understood that the second optimization result is a map.

[0137] S72. Output fifth posture information based on the second optimization result and the first map.

[0138] Exemplarily, feature matching (such as ORB (Oriented FAST and Rotated BRIEF) feature matching) can be performed on the second optimization result and the first map to obtain a third matching result; the current position of the drone, that is, the fifth posture information, is obtained through the third matching result.

[0139] S73. Planning a fourth flight path of the UAV based on the fifth posture information and the destination information.

[0140] In some embodiments, after the step S71, the path planning method of the drone further includes:

[0141] S81: locally optimize the first map to obtain a third optimization result. Figure 2 The "local optimization" step in .

[0142] Exemplarily, the present application may use SLAM technology to locally optimize the first map to obtain the third optimization result.

[0143] S82, jointly optimize the third optimization result and the second optimization result to obtain a fourth optimization result. Figure 2 The “Joint Optimization of Pose Map” step in .

[0144] Exemplarily, it can be understood that the third optimization result and the second optimization result are fused to obtain the fourth optimization result.

[0145] S83. Generate sixth posture information based on the fourth optimization result.

[0146] Exemplarily, the sixth position information can be obtained from the fourth optimization result by using SLAM technology. Figure 2 "Pose output 30Hz" in the

[0147] S84: planning a fifth flight path of the UAV based on the sixth position information and the destination information, and the UAV performing a flight mission according to the fifth flight path.

[0148] It should be understood that in the embodiment of the present application, the first posture information, the second posture information, the third posture information, the fourth posture information, the fifth posture information and the sixth posture information are all published via messages in the nav_msgs::Odometry format in ROS.

[0149] In some embodiments, the path planning method of the drone is also loosely coupled with the IMU positioning to form a fusion positioning algorithm, which adjusts the IMU integral, reduces zero drift, and estimates gravity, thereby ensuring the accuracy of posture estimation.

[0150] Furthermore, the present application can obtain two pose information through binocular SLAM and IMU positioning systems under EKF. These two information have their own characteristics. Among them, binocular SLAM pose is accurate, but its frequency is low and its robustness is poor; while IMU positioning has serious drift, but its frequency is high and its robustness is good. Therefore, the present application obtains a visual inertial loosely coupled positioning system with accurate positioning, high frequency, good performance and continuous pose through a loosely coupled positioning method. First, visual SLAM is used to perform pose feedback on IMU positioning. The IMU positioning system has the characteristics of large drift error. Therefore, when the visual positioning is normal, the observation equation of the EKF filter is used to add pose feedback to the positioning system, thereby reducing the drift error of IMU positioning. Secondly, IMU positioning under EKF performs position prior on binocular SLAM. Due to the high maneuverability of the drone, the original constant speed model is no longer applicable. When the viewing angle changes rapidly, the image feature points will not track the map points, resulting in positioning failure. Therefore, the integral prediction of the IMU under EKF is used as one of the inputs of the SLAM system, and the pose prior is used for feature matching.

[0151] The fusion positioning algorithm can achieve centimeter-level positioning accuracy and ensure stable posture output in extreme sports scenes and challenging environments. In some embodiments, the path planning method of the drone uses the bundle adjustment method to solve the least squares problem, and uses the G2O (General Graphic Optimization) optimization library to optimize and obtain accurate posture information.

[0152] In some implementations, the path planning method for the drone further includes calculating the first posture information using an epipolar constraint algorithm. In this embodiment, the first posture information can be understood as an initial posture at the current moment.

[0153] Specifically, the calculation formula of the epipolar constraint algorithm is:

[0154]

[0155] Among them, p1 is the pixel coordinate of the image of the spatial point at the previous moment, and p2 is the pixel coordinate of the image of the spatial point at the current moment. is the transpose of p2, K is the intrinsic parameter matrix of the camera in the drone, K -1 is the inverse matrix of the internal parameter matrix, K -T is the transpose of the inverse matrix of the intrinsic matrix, R is the rotation matrix between the image at the previous moment and the image at the current moment, and t is the translation vector between the image at the previous moment and the image at the current moment. By solving the epipolar constraint algorithm, rough pose information can be obtained, and the location information of the map point can be obtained by triangulation.

[0156] In some embodiments, during the flight of the drone, several images collected by the drone will contain the same map points, that is, the several images will observe the same map points, thereby establishing a connection between the current image and more postures, so as to construct a least squares problem to optimize the posture and map and obtain more accurate posture information. Specifically, the least squares problem is defined as the following formula in this application:

[0157]

[0158] Where T is the pose set of the UAV on its flight trajectory, p is the three-dimensional spatial coordinate of the landmark point, m is the number of horizontal pixels in the image, n is the number of vertical pixels in the image, h is the function of predicting pixels based on pose and landmark points, and z ij is the actual pixel coordinate observation value of the landmark point, T i is the posture, p j In the embodiment of the present application, a rough pose estimation can be used as the initial value of the iterative optimization algorithm.

[0159] The use of the least squares problem can make up for the defect of the epipolar constraint algorithm that endangers low estimation accuracy due to the fact that only the relationship between the image at the previous moment and the image at the current moment is used.

[0160] Furthermore, the drone in this application is a quad-rotor drone. Since the quad-rotor drone has a differential flatness characteristic, this application is planned in the quad-rotor flat space {x, y, z, yaw}, and the final solved control instructions are mapped to the full state space to realize the real-time attitude control of the drone. This application uses B-spline curves as motion primitives to generate reference trajectories, and uses the B-spline curve control points as optimization variables. In order to ensure that the boundary state does not change, the three control points at the start and end points are ignored, and only {C3, C2, …, C N-3} to optimize, thus, the optimization function is constructed into the following expression: minJ total =λ s J s +λ c J c +λ f J f Among them, J s is the smoothness cost term, λ s is the weight of the smoothness cost term, J c is the collision cost term, λ c is the weight of the collision cost term, J f is the feasibility cost term, λ fis the weight of the feasibility cost term. The smoothness cost is defined as the minimum jerk term, the collision cost is defined as the sum of the current control point set on the Euclidean distance field map (ESDF), and the feasibility cost is defined as the degree of violation of the trajectory to the upper and lower bounds of velocity and acceleration. The expressions of each cost and the gradient function are all analytical functions, and the initial feasible solution is given by the A* algorithm. On this basis, the gradient descent method L-BFGS can be used to achieve a fast solution.

[0161] In some embodiments, reference Figure 3 As shown, based on the first depth image and the second depth image (ie Figure 3 The depth map in (in) and the first pose information (i.e. Figure 3 The pose information in the image) constructs a DSP grid map (i.e. Figure 3 occupancy grid map in ).

[0162] refer to Figure 3 Based on the DSP grid map, the motion state of each independent obstacle is further analyzed through DBSCAN clustering, correlation between previous and next frames, and motion speed estimation. At the same time, based on Kalman filtering and bounding box algorithm, dynamic obstacles in the field of view are observed in real time to obtain their position and speed information, and nonlinear filtering is used to reduce the impact of sensor errors.

[0163] Furthermore, the embodiment of the present application uses a Bayesian estimation method to estimate the voxel grid m in the occupancy grid map (ie, the DSP grid map). i The probability of being occupied P(m i ). When the observation sequence is known to be z 1:t When the logarithmic confidence is defined as l t,i , the inversion observation is l inv,i , and the prior probability is l0. Specifically, the calculation formula of the logarithmic confidence is as follows:

[0164]

[0165] Among them, P is the probability of being occupied, m i is the voxel grid, z 1：t is a known observation sequence, is a priori estimate.

[0166] The calculation formula of the inversion observation is:

[0167]

[0168] Among them, P is the probability of being occupied, m i is the voxel grid, Z t is the observed value at time T, is a priori estimate.

[0169] The calculation formula of the prior probability is:

[0170]

[0171] Among them, P(m i ) is the voxel grid m i The probability of being occupied, is a priori estimate.

[0172] In some embodiments, the calculation formula of the logarithmic confidence can also be shown as follows:

[0173] l t,i = l t-1,i +l inv,i -l0;

[0174] Among them, l t-1，i is the occupancy probability at time t-1, l inv，i is the inversion observation, and l0 is the prior probability.

[0175] It should be understood that in each observation, the ray tracing method is used to process the grids within the observation range. At regular intervals, all grid occupancy probability estimates are restored to the initial state to avoid the influence of probability estimation time delay.

[0176] Furthermore, the present application uses the density-based DBSCAN clustering method to classify the occupancy grids belonging to an independent obstacle into a cluster, and processes the cluster as a unit in detection and motion estimation. The geometric center of each point cloud cluster bounding box in the current frame is associated with the geometric center of the point cloud cluster bounding box representing the same obstacle in the previous frame using the global nearest neighbor algorithm. The motion state of the obstacle in the short future time is estimated using a conservative motion model. The obstacle is within t k The position x at the moment k and speed The state equation is as follows:

[0177]

[0178] Where Δt is the time step, x k-1 is the state at time k-1, x k-1 is the derivative of the state at time k-1.

[0179] Furthermore, in t k The movement status of obstacles at all times Estimated value of With the measured value z k The calculation formula is:

[0180] in,

[0181]

[0182] H=[I 3×3 0 3×3 ];

[0183] in, is the state estimate at time k-1, v k is the process noise belonging to Gaussian distribution Ν(0,Q), Q is the variance, w k is the measurement noise belonging to Gaussian distribution N(0,R), R is the variance, I 3×3 is the identity matrix.

[0184] In some embodiments, based on the position information obtained by the fusion positioning algorithm and the grid map obtained by the environment perception algorithm, a path generation method based on A* is designed, and the obtained grid map is expanded to ensure the safety of the initial feasible solution. According to the design of the dynamic obstacle avoidance speed generation method based on GVO and the trajectory optimization method based on B-spline, the two logics of timing trigger and collision trigger are combined to realize the autonomous generation of continuous, smooth, safe and dynamic feasibility trajectories of the drone; in order to solve the problem of autonomous navigation of drones with limited perception range in unknown environments, a Receding Horizon Planning (RHP) framework is constructed to perform path planning within a credible map range, thereby improving the robustness of the trajectory generated by the planning module. The entire planning can be solved within 3ms, meeting the requirements of speed and real-time performance.

[0185] <UAV path planning system>

[0186] like Figure 4 FIG. 1 is a block diagram of a path planning system for a drone provided in the second aspect of the present application. Figure 4 In the embodiment, the path planning system 100 of the drone includes:

[0187] An acquisition module 101 is used to acquire destination information, a first depth image and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles;

[0188] A matching module 102, configured to perform feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points;

[0189] A positioning module 103 is used to, when the first matching result is a successful match, use an EPnP algorithm combined with a RANSAC algorithm to perform positioning processing on the plurality of first feature points and the plurality of second feature points to obtain first pose information;

[0190] The planning module 104 is used to plan a first flight path of the UAV based on the first posture information and the destination information.

[0191] <Drone>

[0192] like Figure 5 , which is a principle block diagram of a drone provided by the third aspect of the embodiment of the present application. Figure 5 In the embodiment, the drone 200 includes: a processor 201 and a memory 202, the memory 202 is used to store a computer program, and the processor 201 is used to call and run the computer program stored in the memory 202 to execute the steps of the drone path planning method described in the first aspect of the embodiment of the present application.

[0193] In some embodiments, the drone further includes a finite state machine, and the finite state machine is electrically connected to the processor. The finite state machine can comprehensively utilize the current observation information (such as point cloud information, position information, and current state of the drone, etc.) to give a corresponding "event" that triggers state transition, prompting the drone to reach the next state, thereby completing the corresponding behavior. The basic actions ultimately performed by the drone are translation and rotation. Other complex behaviors are essentially a combination of these two zero-level behaviors.

[0194] This application lists the various "states" of the drone during flight in a preset way, and then determines the "events" that cause state changes according to actual needs, prompting the state machine to switch from one state to another. Draw a complete state diagram and state table based on all states and events, and finally program according to the state diagram and state table to complete the construction of the basic framework of the state machine. Figure 6 As shown, the drone successively experienced stationary (i.e. Figure 6 "Idle" in the "Idle", idling, takeoff, hovering, flight mission, adjusting posture, abnormality, landing, "emergency braking" state (i.e. Figure 6"Brake stop" in it). Therefore, the actively triggered events are defined as "idle" event, "takeoff" event, "hover" event, "flight mission" event, "adjust posture" event, "abnormal" event, "landing" event, "emergency braking" event (also known as "brake stop" event) and "end program" event. When the finite state machine receives an idle command, the "idle" event is triggered; when the finite state machine receives a takeoff command, the "takeoff" event is triggered; when the UAV reaches a specified height or the finite state machine receives a hovering command, the finite state machine triggers a "hover" event; when the finite state machine receives an adjust posture command, the "adjust posture" event is triggered; when the UAV is abnormal, the finite state machine triggers an "abnormal" event; when the finite state machine receives an algorithm execution command, the "flight mission" event is triggered; when the finite state machine receives a landing command, the "landing" event is triggered; when the finite state machine receives a brake stop command, the "brake stop" event is triggered.

[0195] After establishing the basic behavior events of decision-making, any action state of the drone can be obtained by combining the basic behavior events of decision-making. Considering the convenience and safety of operation, the flight rules of PX4 flight control are drawn. Figure 6 The state diagram shown is composed of various basic decision-making behaviors. If an abnormal state occurs during flight, such as low battery, drifting posture, etc., the finite state machine will release the "abnormal" state, and the drone will land autonomously. This application uses a finite state machine to achieve autonomous decision-making of drones, simplifies the debugging process, effectively reduces the possibility of risks, and improves flight efficiency.

[0196] In some embodiments, the main frame of the drone of the present application is based on a 250 wheelbase quad-rotor frame, which is composed of four nylon fiber shafts and two layers of gold-plated PCB power distribution and fixing plates. The drone is equipped with multiple power supply interfaces to facilitate the fixing of electrical components and welding circuits. A lithium battery is placed between the two layers of gold-plated PCB power distribution and fixing plates to power the entire machine. The flight control is placed on the upper layer of the frame and supported to a certain height by aluminum columns, and the onboard computer and depth camera sensor are fixed on the top layer. The onboard computer and depth camera are fixed in a box-shaped fixing groove made of carbon fiber plate material, which makes the overall center of gravity of the drone centered, has good anti-fall performance, and is easy to install and debug. The structural rendering of the overall drone is shown in the figure. Figure 7 shown.

[0197] Furthermore, the drone system can be divided into a three-layer structure: the upper layer is the Intel NUC (11TNHi7) airborne computing platform, and is equipped with the Intel Realsense D435i as an external visual sensor, wherein the Intel NUC airborne computing platform is connected to the first voltage regulator module via the power supply interface (19V), and is connected to the USB-TypeC of the Intel Realsense D435i and the microUSB interface of the flight controller via USB. The middle layer is the CUAV V5+ flight controller (i.e. Figure 7 The Pixhawk 4mini flight control in the figure is located at the geometric center of the aircraft as the control core of the quadcopter position and attitude. The flight control is connected to four T-motor F45A electronic speed controllers through PWM, connected to the second voltage regulator module through the power supply interface (5V), and connected to the third voltage regulator module through a three-pin interface (i.e. Figure 7 The lower layer is the frame body with a wheelbase of 250mm (i.e. Figure 7 The Q250 quad-rotor frame in the figure is connected to the first voltage stabilizing module and the second voltage stabilizing module respectively, and connected to four motors, four electric speed controllers and propellers as actuators. Among them, the four motors are all T-motor F40Pro motors, the four electric speed controllers are all T-motor F45A electric speed controllers, and each electric speed controller is connected to the corresponding motor. Figure 7 In the diagram, the 4S on the T-motor F45A ESC frame indicates that the T-motor F45A ESC can be connected to a lithium battery.

[0198] Considering the limited loading capacity of the USB interface of the airborne computing platform, the power supply and signal transmission of the camera and flight control are separated to meet the functional requirements of each sensor unit. At the same time, two voltage regulator modules are used to power the flight control and the airborne computing platform respectively, so that they can work at the rated voltage of 5V and 19V respectively. Figure 7 , showing the hardware topology of the drone, where the black lines represent the power supply relationship, the dark gray lines represent the signal transmission, and the light gray lines represent the PWM control signal.

[0199] <Computer Readable Storage Medium>

[0200] In a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium is used to store a computer program, and the computer program enables a computer to execute the steps of the path planning method for a drone as described in the first aspect of the embodiment of the present application.

[0201] 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 by the present invention 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).

[0202] The technical features of the above embodiments can be combined without changing the basic principles of the present application. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0203] The above embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the patent application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the scope of patent protection of the present application shall be subject to the attached claims.

Claims

1. A path planning method for an unmanned aerial vehicle, characterized in that: include: Acquire destination information, a first depth image and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles; Performing feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points; When the first matching result is a successful match, the EPnP algorithm is combined with the RANSAC algorithm to perform positioning processing on the multiple first feature points and the multiple second feature points to obtain the first pose information; A first flight path of the UAV is planned based on the first posture information and the destination information.

2. The path planning method for an unmanned aerial vehicle according to claim 1, characterized in that: The step of performing feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points comprises: Scaling the first depth image to obtain a first image to be processed; Performing scaling processing on the second depth image to obtain a second image to be processed; Extracting a plurality of first pixel points in the first image to be processed to obtain a plurality of third feature points, and performing region allocation on the plurality of third feature points to obtain the plurality of first feature points; Extracting a plurality of second pixel points in the second image to be processed to obtain a plurality of fourth feature points, and performing region allocation on the plurality of fourth feature points to obtain the plurality of second feature points; Setting a first preset threshold; calculating the number of identical features between the plurality of first feature points and the plurality of second feature points; When the number of identical items is greater than the first preset threshold, the first matching result is a successful match; When the same number is equal to or less than the first preset threshold, the first matching result is a failed match.

3. The path planning method for an unmanned aerial vehicle according to claim 2, characterized in that: After the step of planning a first flight path of the UAV based on the first posture information and the destination information, the path planning method of the UAV further includes: Constructing a first map based on the first posture information; Performing loop closure detection on the third feature point and the fourth feature point to obtain second pose information and a fusion map; generating third posture information based on the first map and the second posture information; A second flight path of the UAV is planned based on the third posture information and the destination information.

4. The path planning method for an unmanned aerial vehicle according to claim 3, characterized in that: The step of performing loop detection on the third feature point and the fourth feature point to obtain second pose information and a fusion map comprises: Get the second map; Setting a second preset threshold; Perform loop detection based on the third feature point and the fourth feature point to obtain a detection result; Calculating a confidence parameter of the detection result, where the confidence parameter is used to characterize the confidence that the third feature point and the fourth feature point are historical loop positions passed by the drone; When the confidence parameter is greater than the second preset threshold, generating a detection map; Fusing the second map with the detection map to obtain a fused map; Second pose information is generated based on the fused map.

5. The path planning method for an unmanned aerial vehicle according to claim 3, characterized in that: After the step of performing loop detection on the third feature point and the fourth feature point to obtain the second pose information and the fusion map, the path planning method of the UAV further includes: Performing loop optimization on the fused map to obtain a first optimization result; Outputting fourth posture information based on the first optimization result and the first map; A third flight path of the UAV is planned based on the fourth posture information and the destination information.

6. The path planning method for an unmanned aerial vehicle according to claim 5, characterized in that: After the step of performing loop optimization on the fused map to obtain a first optimization result, the path planning method for the UAV further includes: Perform global optimization based on the first optimization result to obtain a second optimization result; Outputting fifth pose information based on the second optimization result and the first map; A fourth flight path of the UAV is planned based on the fifth posture information and the destination information.

7. The path planning method for an unmanned aerial vehicle according to claim 6, characterized in that: After the step of performing global optimization based on the first optimization result to obtain a second optimization result, the path planning method for the UAV further includes: Performing local optimization on the first map to obtain a third optimization result; Jointly optimizing the third optimization result and the second optimization result to obtain a fourth optimization result; Generate sixth posture information based on the fourth optimization result; A fifth flight path of the UAV is planned based on the sixth posture information and the destination information.

8. A path planning system for an unmanned aerial vehicle, characterized in that: include: An acquisition module, configured to acquire destination information, a first depth image and a second depth image, wherein the first depth image and the second depth image show the same scene at different angles; A matching module, configured to perform feature matching processing on the first depth image and the second depth image to obtain a first matching result, a plurality of first feature points, and a plurality of second feature points; A positioning module, configured to, when the first matching result is a successful match, use an EPnP algorithm combined with a RANSAC algorithm to perform positioning processing on the plurality of first feature points and the plurality of second feature points to obtain first pose information; A planning module is used to plan a first flight path of the UAV based on the first posture information and the destination information.

9. A drone, characterized in that: include: A processor and a memory, the memory being used to store a computer program, the processor being used to call and run the computer program stored in the memory to execute the steps of the path planning method for a drone as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: Used to store a computer program, wherein the computer program enables a computer to execute the steps of the path planning method for a drone as described in any one of claims 1 to 7.

Images