Unmanned aerial vehicle autonomous navigation method based on depth image processing

Abstract

The invention discloses an unmanned aerial vehicle autonomous navigation method based on depth image processing, and the method comprises the steps: firstly obtaining a feasible region in a current search region as a candidate point set for a depth image and six-degree-of-freedom pose data collected in the flight process of an unmanned aerial vehicle; then generating a random single-step target point approaching the target point; performing reverse iterative search on the target point based on multi-frame depth and pose data; generating a local point cloud in each frame, and converting the point cloud and a target point coordinate to an unmanned aerial vehicle coordinate system through a coordinate conversion algorithm; performing feasibility analysis by adopting a target point judgment algorithm; judging whether the target point is in a view field or not according to the converted target point sphere coordinates; searching adjacent points of the target point according to the angle relation of the spherical coordinates; under the coordinate system of the unmanned aerial vehicle, forming a view cone by adjacent points and dividing the view cone into four sub view cones; and calculating the projection of a connecting line from the original point to the single-step target point on the bottom surface of the sub-view cone, and judging whether the line segment is completely located in the bottom surface or not, thereby determining the feasibility of the single-step target point.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV autonomous navigation, and particularly relates to a UAV autonomous navigation method based on depth image processing. Background Art

[0002] With the continuous development of UAV technology, micro UAVs have been widely used in many fields such as logistics distribution, environmental monitoring, and disaster relief due to their small size, high flexibility, and low cost. During the UAV autonomous navigation process, realizing real-time perception, positioning, and path planning of the environment is of great significance for improving flight safety and mission execution efficiency. Currently, the existing technologies mainly focus on the optimization of single modules or local algorithms, but there are still deficiencies in the systematic solutions for the global exploration algorithms of micro UAVs.

[0003] In terms of UAV autonomous navigation, the following main methods are included:

[0004] (1) UAV exploration algorithms based on computer vision and deep learning: This method uses an on-board camera to collect environmental images, and combines deep learning models such as convolutional neural networks for feature extraction and target recognition to achieve obstacle detection and path planning. Its advantage lies in being able to better handle complex scenarios and dynamic changes, but it has a strong dependence on large-scale training data and high-computing-power hardware, and it is difficult to achieve real-time processing on resource-constrained micro UAVs.

[0005] (2) UAV exploration algorithms based on visual mapping: This method uses a camera to collect data in real time, and adopts SLAM (Simultaneous Localization and Mapping) technology to construct an environmental map to achieve positioning and navigation. The advantage is that it can complete environmental perception without additional dependence on expensive sensors, but under the conditions of insufficient light or a single environmental texture, the accuracy and stability of map construction may be greatly affected.

[0006] (3) UAV exploration based on positioning methods such as GPS: This method mainly relies on satellite positioning systems such as GPS and Beidou, combined with inertial measurement unit (IMU) data to provide accurate position information and heading estimation. Although it has high positioning accuracy and a wide range of applications, in environments with weak satellite signals such as indoors, tunnels, or densely built urban areas, its navigation performance will drop significantly, limiting its application scenarios.

[0007] In summary, the current micro UAV autonomous navigation exploration algorithms mainly face the following challenges:

[0008] 1. Limited by payload and energy consumption, it is difficult for micro UAVs to deploy high-precision sensors and complex deep learning algorithms;

[0009] 2. The applicability and robustness of various exploration algorithms have limitations in different environments, especially in scenarios with insufficient lighting, single texture, or limited satellite signals;

[0010] 3. How to reduce system costs and energy consumption while ensuring navigation accuracy is a key issue that urgently needs to be solved in future research and applications.

[0011] To achieve efficient autonomous navigation on micro unmanned aerial vehicles (UAVs) with limited computing power and storage capacity, a feasible technical solution is to use a non-map-building scheme to avoid high requirements for storage capacity; at the same time, data reuse is realized to achieve efficient navigation with lower computing power. Existing technologies generally ignore the limitations of storage capacity, and the judgment of the feasibility of target points during the navigation process requires high computing power. Summary of the Invention

[0012] The technical problem to be solved by the present invention is: to propose an autonomous navigation method for UAVs based on depth image processing. Through data acquisition and storage, coordinate conversion, feasibility judgment, reverse search, and iterative loop, it realizes path feasibility analysis and planning, control and flight of UAVs through collaborative work. It fully considers the computing power and energy limitations of micro UAVs, adopts suitable lightweight algorithms, can effectively improve the flight performance of micro UAVs, and combines a target point planning algorithm and a feasibility judgment algorithm to greatly improve the accuracy of target point feasibility judgment and path planning. This method is lightweight and efficient, can improve navigation efficiency on platforms with limited computing power, and improve the accuracy of target point feasibility determination.

[0013] The present invention adopts the following technical solutions to solve the above technical problems:

[0014] An autonomous navigation method for UAVs based on depth image processing, including:

[0015] S1. Obtain the entire area scene and judge the feasible area in the scene.

[0016] S2. From the depth data, calculate the coordinate information of the lidar measurement points to generate point cloud data. Use the target point and the stored depth data to iteratively loop in reverse to judge whether the point is a feasible point. In each round of iteration, first use the point cloud generation algorithm and coordinate conversion algorithm given in S2 and S3 to obtain the relative coordinates of the obstacle point cloud and the target point in the past frame studied in this round. Then use the target point judgment algorithm given in S4 to judge whether the point is a feasible point in the past frame. If the result is true, end the loop and control the UAV to fly to this point; if the result is false, continue the loop, read the data of the previous past frame for judgment; if the feasibility of the target point still cannot be judged after reading all the stored past frames, mark this point as an infeasible point.

[0017] S3. Based on the position information of each measurement point in S2 and the UAV world coordinate system information, by aligning the coordinate systems, calculate the transformation matrix and the calibration of the target point, and convert the coordinate information belonging to this frame to the coordinate information of the previous frame of the UAV.

[0018] S4. According to the coordinates of each measurement point and the target point obtained in S3, through the feasibility judgment algorithm, first unify the depth of the measurement points, calculate the measurement point closest to the target point according to the distance between the target point and each measurement point, and then obtain the other three closest measurement points through the relative position relationship between the target point and the closest measurement point, so that the projection of the target point is located in the closed figure composed of these four measurement points, and then perform graphic segmentation in the visual cone composed of adjacent points in the UAV coordinate system, obtain 4 sub-visual cones with the perpendicular bisector as the common edge and the side-angle bisector and the origin-adjacent point ray as the edges, and then calculate the projection of the origin-single-step target point connection line on the bottom surface of the sub-visual cone in each of the four sub-visual cones respectively, and judge whether the projection line segment is within the bottom surface, so as to judge whether the single-step target point is feasible.

[0019] S5. Use the target point planning algorithm to plan the target point to fly to in the next frame for feasibility judgment.

[0020] Further, in steps S1 - S4, there is a type of data entity. Since the characteristic of the reverse iterative loop is the reverse iterative query of the target point described in step S2, the premise is that during the flight of the UAV, a linked list storage structure has been constructed based on the UAV pose data and depth map data, a circular buffer with a length of N is created, the linked list storage structure is initialized, and the maximum recursion depth is set to K. The linked list node structure is as follows:

[0021]

[0022] Each linked list node unit stores the UAV pose data from the previous frame to this frame, including three-dimensional rectangular coordinates x, y, z; and three-dimensional angular coordinates yaw, pitch, roll; and the depth data of 8 * 8 measurement points in the previous frame. The depth image data is an 8×8 single-precision floating-point matrix, and each element corresponds to the depth value of the center point of the lidar field of view grid. The target point data is three-dimensional double-precision floating-point coordinates (x t , y t , z t ), which is generated by the feasible region search algorithm.

[0023] Further, in step S1, the feasible region search algorithm includes the following sub-steps:

[0024] S101. Obtain the regional point set. All candidate point sets within the current search area are obtained through sensors, that is, the positions and quantities of the points for which feasibility judgment is required are determined, providing basic data for subsequent judgments. The candidate point set refers to all candidate points within the same horizontal plane as the UAV, that is, the points to be queried.

[0025] S102. Current frame judgment. For the points in the point set determined in step S101, judge whether they are reachable in the current frame of the UAV. If feasible, perform the current frame judgment on the remaining target points in the point set; if not feasible, perform a reverse iterative query for this target point to judge whether it was feasible in the previous few frames of the UAV.

[0026] Furthermore, in step S2, the point cloud generation algorithm includes the following sub-steps:

[0027] S201. Depth map query. Extract the depth value of each data from the depth data, that is, the depth data ResultsData dist in the storage node FrameNode. ResultsData stores the depth (distance), status, and reflectance of the 8*8 target points obtained by the lidar in the form of a two-dimensional array as a structure.

[0028] S202. Angle table query. Consult the pre-calibrated angle mapping table to obtain the corresponding Pitch and Yaw values for each region.

[0029] S203. Coordinate calculation. Based on the depth and angle data, use the corresponding coordinate conversion formula to calculate the local coordinates of the 8*8 sampling points in the target area in the current frame. The depth map data is stored in a linked list, and the angle mapping table is a set of queryable constants, that is, the scanning area of the lidar is an 8*8 square matrix, and the side length is equal to the depth data of the point directly in front. For different depth regions, the angle information remains unchanged, but the size of the corresponding unit block increases with the increase in depth. The edge field of view angle of this scenario is 45 degrees, and the diagonal is 65 degrees. For each sampling point, its coordinate conversion, that is, the point cloud generation formula is:

[0030]

[0031] X rel =Res dis

[0032] where Res dis is the depth value, and Pitch and Yaw are the pitch angle and yaw angle calibrated by the light beam.

[0033] Furthermore, in step S3, the coordinate conversion algorithm mainly includes the following sub-steps:

[0034] S301. Align the coordinate systems. Considering the deviation between the position of the lidar and the origin of the UAV, accurately align the coordinate systems of the lidar and the UAV to ensure accurate data conversion.

[0035] S302. Calculate the transformation matrix. In order to convert the local coordinates of the point to be queried in the current frame into the local coordinates in the previous frame, it is necessary to calculate the transformation matrix according to the change in the UAV pose data from the current frame to the previous frame. The coordinate transformation formula is as follows:

[0036]

[0037] (x t ,y t ,z t )·R yaw ·R pitch ·R roll +(x pos ,y pos ,z pos )=(x t-1 ,y t-1 ,z t-1 )

[0038] Where (x t ,y t ,z t ) are the local coordinates of the point to be queried in the current frame, (x pos ,y pos ,z pos ) are the coordinate parts of the pose transformation vector from the previous frame to this frame, (x t-1 ,y t-1 ,z t-1 ) are the local coordinates of the point to be queried in the previous frame, and R yaw , R pitch , R roll are rotation transformation matrices.

[0039] After that, map the lidar coordinate system to the UAV local coordinate system through the transformation matrix:

[0040]

[0041] S303. Calibrate the coordinates of the target point, that is, convert the coordinates of the target point in the current frame into the coordinates in the previous frame. Obtain the change in the UAV pose data from the current frame to the previous frame through the linked list node, and the coordinate transformation matrix can be applied to complete it.

[0042] Furthermore, in step S4, the feasibility judgment algorithm includes the following sub-steps:

[0043] S401. Query the surrounding points. That is, based on the position of the point to be queried, determine the four rays in the ray grid formed by the origin of the drone and the 8×8 measurement points that are closest to the point to be queried, and output the indices of these four rays in the ray grid.

[0044] First, uniformly depth the 8×8 sampling points and the point to be queried to the same x value of 100. According to the point cloud generation formula, let Res dis = 100, and we can get

[0045]

[0046] X = Res dis

[0047] XYZRay i, j = (X, Y, Z) i,j

[0048] We use the y - coordinate and z - coordinate values at a depth (x - coordinate) of 100 as a reference, and use the cross - product of vectors to find the index of the nearest measurement point. Taking the drone lidar as the coordinate origin, 64 vectors are formed by the origin and the reference points at a depth of 100. Calculate the cross - product of the vector formed by the origin and the point to be queried with each of these 64 vectors respectively, that is

[0049]

[0050] For a=(a1, a2, a3) and b=(b1, b2, b3), the cross - product calculation formula is:

[0051]

[0052] The magnitude of the resulting vector is |a||b|sinθ (θ is the angle between the two vectors), which geometrically corresponds to the area of the parallelogram formed by the two vectors. Divide the result by the magnitude of the depth vector to obtain the product of the magnitude of the target point vector and the angle. By traversing the 64 depth measurement points and recording the ray index with the minimum distance during the traversal, that is res i and res j , we can obtain the ray closest to the query point in the current grid.

[0053] Since we need to find the indices of the four depth measurement points closest to the target point, we need to complete the other three rays. First, extend the vector of the point to be queried to the fixed reference plane at a depth (x - coordinate) of 100, that is, according to the three components of the point to be queried, scale the y - coordinate and z - coordinate of the point to be queried according to the ratio of the x - coordinate. The formula is as follows:

[0054]

[0055] At this time, the depth of the query point is the same as the depth of the nearest measurement point. Then, by comparing the extended query point and the nearest ray , the relative position of the query point in the grid cell can be determined by comparing the y and z coordinate values, and then the four adjacent rays can be selected.

[0056] The comparison process is divided into the following four cases:

[0057] Case 1: When and , it indicates that the query point is in the "lower right" area of the ray. Then the four ray indices selected are:

[0058] (res i , res j ), (res i , res j + 1), (res i + 1, res j ), (res i + 1, res j + 1).

[0059] Case 2: When and , it indicates that the query point is in the area with a smaller y value but a larger z value. Then the following are selected:

[0060] (res i - 1, res j ), (res i - 1, res j + 1), (res i , res j ), (res i , res j + 1).

[0061] Case 3: When and , it indicates that the query point is in the area with a larger y value but a smaller z value. Then the four ray indices selected are:

[0062] (res i , res j - 1), (res i , res j ), (res i + 1, res j - 1), (res i + 1, res j ).

[0063] Case 4: When and If the area where the query point is larger in both directions is described, then select:

[0064] (res i -1,res j -1),(res i -1,res j ),(res i ,res j -1),(res i ,res j ).

[0065] In this way, the algorithm actually determines a conical open area composed of four rays, which surrounds the position where the query point is located, ensuring that the query point is inside the area formed by these four rays.

[0066] S402. Sub - frustum graphics segmentation: For the frustum area determined in step S401, judge whether the target point is in the field of view by dividing the quadrangular pyramid into four small quadrangular pyramids.

[0067] In the figure formed by the field - of - view rays, the four field - of - view rays closest to the target point form a large quadrangular pyramid, and the central axis of this large quadrangular pyramid divides it into four small quadrangular pyramids with different heights. The four edges of a small quadrangular pyramid are one of the four rays, the angular bisectors of this ray and the adjacent two rays, and the central axis of the large quadrangular pyramid. Therefore, we need to obtain the angular bisectors of the frustum and the central axis of the frustum according to the four rays around the query point to be obtained, and obtain the length of the bottom surface of the sub - frustum according to the angular relationship.

[0068] Calculate the central axis vector of the frustum and the side length of the bottom surface of the sub - frustum according to the coordinates of the target point, the indices of the four field - of - view rays around the target point, and the coordinates of the four sampling points corresponding to the four field - of - view rays.

[0069] Let the four field - of - view rays be OA1, OA2, OA3, OA4, then the four angular bisectors are Then the central axis is The side length of the bottom surface of the sub - frustum is (taking the upper - left sub - frustum as an example):

[0070] S403. Sub - frustum loop judgment: After we divide the large quadrangular pyramid into four small quadrangular pyramids, we then need to loop - judge whether the query point is within the areas of the four small quadrangular pyramids, that is, judge whether the target point is within the sub - frustum ray area and whether the target point and the origin are on the same side of the sub - frustum bottom plane. This includes the following sub - steps:

[0071] 1. Judge whether the target point and the origin are on the same side of the sub - frustum bottom plane

[0072] By calculating and comparing, the projected lengths of the target point and the sampling point on the central axis are OP·e center and OA i ·e center , to determine whether the target point and the origin are on the same side of the bottom plane of the sub-view frustum. If it is greater, it means that the target point and the origin are on the same side of the bottom plane of the sub-view frustum, otherwise they are not.

[0073] 2. Calculate the projection of the target point relative to the bottom surface

[0074] For the target point, it is necessary to project it onto the bottom plane of the small quadrangular pyramid, that is, calculate its projection position on the bottom plane. The method is to calculate the projection of the target point on this plane according to the relative position of the target point and the center of the bottom surface.

[0075] Assume that the point to be queried is P, the projection of the point to be queried on the central axis is K, and the projection of the point to be queried on the bottom plane is P ′ and the projection of the origin on the bottom plane is O ′ . Assume that the target point is P according to the formula. According to the formula

[0076] O′P′ = KP = OP - OK = OP - OP·e center

[0077] The projection of the target point P on the bottom surface of the sub-view frustum can be calculated. The origin is the intersection point of the central axis and this plane. Then, by comparing the relationship between the x and y values of the vector and the coordinates of the sub-view frustum, it can be determined whether the ray from the origin to the target point is within the small quadrangular pyramid.

[0078] 3. Determine whether the target point is inside the view frustum

[0079] The judgment basis here is that the target point is within the area surrounded by the four side surfaces and the respective bottom surfaces of the four sub-view frustums. Specifically, the judgment steps are as follows:

[0080] Loop through each sub-view frustum to determine whether the target point and the origin are on the same side of the bottom plane of the sub-view frustum and whether the ray from the origin to the target point is within the sub-view frustum. If the target point and the origin are not on the same side of the bottom plane of the sub-view frustum and the ray from the origin to the target point is within the sub-view frustum, it means that the target point is not inside the view frustum. If the above negative judgment condition does not occur after the loop ends, it means that the target point is inside the view frustum.

[0081] S501. Heuristic search algorithm (A*)

[0082] The system traverses all candidate target points within the search area and plans the optimal path based on the A* heuristic search algorithm. During this process, each candidate point is regarded as a search node, and the actual cost G from the starting point to the current node and the heuristic estimated cost H from the current node to the target end point are calculated for it. The sum of them constitutes the total cost F. The formula is as follows:

[0083] F = G + H

[0084] Where G is the actual cost from the starting point to the current node, and H is the estimated cost from the current node to the target end point.

[0085] The algorithm adds the starting point node to the open list, repeatedly selects the node with the smallest F value as the current node, and expands its adjacent nodes. During the expansion process, the path information is continuously updated. Finally, when the target node is expanded and its total cost is confirmed to be the lowest, the algorithm terminates and outputs the planned optimal path. This method can efficiently balance the search efficiency and path optimality, ensuring that the drone quickly and safely selects the appropriate target point during autonomous navigation.

[0086] Compared with the existing technology, the advantages of this solution are as follows:

[0087] 1. Lightweight data processing and historical frame reuse mechanism: Aiming at the storage and computing power bottlenecks of micro drones, a circular buffer reverse iteration query mechanism based on a linked list structure is created. Compared with the traditional slam vision method that needs to store a dense map, this solution constructs an inter-frame pose transformation linked list (FrameNode), dynamically stores the depth image and pose data of historical frames as a lightweight data structure that can be traced back (8×8 depth matrix + 6-degree-of-freedom pose parameters), and only requires O(N) storage space (N is the buffer length) to realize the spatio-temporal correlation analysis of multi-frame data.

[0088] 2. Dynamic coordinate system alignment and lightweight point cloud generation: Design a fast point cloud generation algorithm based on a pre-calibrated angle mapping table, and combine it with a non-iterative coordinate transformation matrix to realize the real-time and accurate mapping of lidar to the drone coordinate system.

[0089] 3. Feasibility judgment algorithm based on frustum segmentation: A dynamic frustum segmentation and projection criterion model is proposed, which breaks through the limitations of traditional methods based on Euclidean distance or simple geometric criteria, and the accuracy is significantly improved compared with the traditional ray casting method. By dividing the quadrangular pyramid formed by the adjacent rays of the target point into four sub-frusta and introducing the central axis projection and plane lateral criterion, an accurate model of the occlusion relationship of obstacles in three-dimensional space is realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 is the overall implementation flowchart of the present invention,

[0091] Figure 2Schematic diagram of target point projection and judgment in S503

[0092] Figure 3 Marking diagram of the feasible region of the example frame in S503 Detailed implementation manner

[0093] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented on the premise of the technical solution of the present invention, and gives detailed implementation manners and specific operation processes, but the protection scope of the present invention is not limited to the following embodiments.

[0094] Embodiment: The present invention proposes an unmanned aerial vehicle autonomous navigation method based on depth image processing, as Figure 1 shown, the method includes the following steps:

[0095] S1. Feasible region search algorithm, using the depth map read by the unmanned aerial vehicle in real time and the target point obtained by the target point planning algorithm to judge whether the point is in the feasible region in the current frame. The specific content is as follows:

[0096] S101. Read the depth map of the current frame from an 8×8 depth camera, and use the point cloud generation algorithm and coordinate transformation algorithm given in S3 and S4 to obtain the obstacle point cloud. Use the feasibility judgment algorithm given in S5 to densely scan the point set within the viewing cone, so as to obtain the feasible region point set.

[0097] S102. Perform the current frame judgment to judge whether the target point is within the closed region depicted by the feasible region point set. If it is, perform unmanned aerial vehicle control, fly directly to this point, and then use the target point generation algorithm to obtain a new target point; if not, enter the reverse iteration loop given in S2. In this embodiment, in a certain cycle R i+4 the target point in is (2000, 750, 0) (unit: mm, the same below). After judgment, this point is not within the feasible region and enters the judgment of frame R i .

[0098] S2. Use the target point and the stored depth data to perform a reverse iteration loop to judge whether this point is a feasible point. This part is the overall coordination of S3, S4, and S5. In each round of iteration, first use the point cloud generation algorithm and coordinate transformation algorithm given in S3 and S4 to obtain the obstacle point cloud and the relative coordinates of the target point in the past frame studied in this round. Then use the target point judgment algorithm given in S5 to judge whether this point is a feasible point in this past frame. If the result is true, end the loop and control the unmanned aerial vehicle to fly to this point; if the result is false, continue the loop, read the data of the previous past frame for judgment. Read the data of the past frame studied to obtain the obstacle point cloud captured by the depth camera in this past frame. The specific content is as follows:

[0099] S201. Read out the 8×8 depth map of the past frame and the six - degree - of - freedom displacement of this frame pointing to the next frame from the storage unit:

[0100]

[0101] where X is a rectangular - coordinate vector, θ is an angular vector (in degree system, unit: °). (i, j, k) is the coordinate basis vector of the past frame, pointing to the positive direction of the coordinate axis; (α, β, γ) is an angular vector, following the right - hand screw rule. These data are stored in single - precision floating - point format. In this embodiment, the displacement read from R i is as follows:

[0102]

[0103] The depth - map data is as follows:

[0104] 1408 1386 1416 1390 2458 2483 846 2420 1407 1419 1424 1455 2490 2493 841 2586 1410 1422 1431 1435 2509 2496 855 837 1416 1426 1429 1460 2535 2536 871 2651 1426 1433 1447 1454 2561 2540 880 842 1412 1436 1435 1441 1312 1234 887 857 929 946 982 949 963 912 879 867 721 752 757 754 757 745 741 713

[0105] S202. Query the angle table, which is stored in the form of a static variable and describes the pitch angle Pitch and yaw angle Yaw of 8×8 points in spherical coordinates. In this embodiment, the angle table is:

[0106] Table 1 Angle Look - up Table (unit: degree)

[0107] Pitch 59.00 64.00 67.50 70.00 70.00 67.50 64.00 59.00 Yaw 135.00 125.40 113.20 98.13 81.87 66.80 54.60 45.00 Pitch 64.00 70.00 72.90 74.90 74.90 72.90 70.00 64.00 Yaw 144.60 135.00 120.96 101.31 78.69 59.04 45.00 35.40 Pitch 67.50 72.90 77.40 80.50 80.50 77.40 72.90 67.50 Yaw 156.80 149.04 135.00 108.45 71.55 45.00 30.96 23.20 Pitch 70.00 74.90 80.50 85.75 85.75 80.50 74.90 70.00 Yaw 171.87 168.69 161.55 135.00 45.00 18.45 11.31 8.13 Pitch 70.00 74.90 80.50 85.75 85.75 80.50 74.90 70.00 Yaw 188.13 191.31 198.45 225.00 315.00 341.55 348.69 351.87 Pitch 67.50 72.90 77.40 80.50 80.50 77.40 72.90 67.50 Yaw 203.20 210.96 225.00 251.55 288.45 315.00 329.04 336.80 Pitch 64.00 70.00 72.90 74.90 74.90 72.90 70.00 64.00 Yaw 203.20 223.00 239.04 258.69 381.31 300.96 315.00 324.60 Pitch 59.00 64.00 67.50 70.00 70.00 67.50 64.00 59.00 Yaw 225.00 234.60 246.80 261.87 278.13 293.20 305.40 315.00

[0108] S203. Calculate and generate the obstacle point cloud according to the angle look - up table and the depth data. The specific calculation formula is:

[0109]

[0110] X rel = Res dis

[0111] where Res dis is the depth value, Pitch and Yaw are the pitch angle and yaw angle calibrated by the beam.

[0112] S3. Coordinate - transformation algorithm. According to the target points in the next - past frame of this past frame, obtain the coordinates of the target points in this past frame through the transformation matrix. The specific content is as follows:

[0113] S301. Align the coordinate systems. This step is because the coordinate - system directions of the UAV and the depth camera may be different. In this embodiment, the transformation method is as follows:

[0114]

[0115] where (x′ t-1 , y′ t-1, z' t-1 ) T , (x t-1 , y t-1 , z t-1 ) T are the coordinate axes of the drone and the depth camera respectively.

[0116] S302. Calculate the transformation matrix based on the displacement of the previous frame of the past frame. The coordinate transformation matrix and formula are:

[0117]

[0118] (x t , y t , z t ) · R yaw · R pitch · R roll + (x pos , y pos , z pos ) = (x t-1 , y t-1 , z t-1 )

[0119] In this embodiment, the transformation matrix from R i+1 to R i is:

[0120]

[0121] S303. According to the transformation matrix, transform the target point to the past frame. The specific calculation formula is:

[0122] (x t , y t , z t ) · R yaw · R pitch · R roll + (x pos , y pos , z pos ) = (x t-1 , y t-1 , z t-1 )

[0123] where (x t , y t , z t ) is the local coordinate of the point to be queried in the current frame, (x pos , y pos , z pos ) is the coordinate part of the pose transformation vector from the previous frame to this frame, (x t-1 , y t-1 , z t-1)R is the local coordinate of the point to be queried in the previous frame yaw , R pitch , R roll is the rotation transformation matrix. In this embodiment, R i The coordinates of the target point in the frame are (1998.94, 740.42, 9.96).

[0124] S4. Perform a feasibility judgment based on the obstacle point cloud and the target point coordinates, detect the feasible region, and confirm whether the target point is within the feasible region of the past frame. The specific content is as follows:

[0125] S401. Loop to detect the feasibility of all points in the perspective of the past frame. Perform the first frustum segmentation according to the position of the point to be queried, and obtain the indexes of the points around the point to be queried. Equalize the depth of the 8×8 sampling points and the point to be queried, judge the ray closest to the point to be queried, and complete the other three rays, which are the sub-frustums (quadrangular pyramids) surrounding the point to be queried. The specific calculation formula is:

[0126]

[0127] X = Res dis

[0128] XYZRat i,j = (X, Y, Z) i,j

[0129]

[0130] In this embodiment, R i The coordinates of the points around the target point in are:

[0131]

[0132] S402. Based on the frustum obtained in the previous step, perform sub-frustum segmentation. First, obtain the angular bisectors of the apex angles on the 4 side faces of the frustum, and then take the angular bisectors of the two groups of opposite angular bisectors to obtain the central axis. A sub-frustum is formed by a side edge of the frustum, two adjacent angular bisectors on the faces, and the central axis, and the frustum is divided into 4 sub-frustums. The specific formula is as follows:

[0133]

[0134]

[0135] where e i (i = 1, 2, 3, 4) is the angular bisector of the side face, e centerTaking the central axis as the reference, let \(L\) be the side length of the bottom surface of the upper left sub-view frustum (the calculation methods for the other 3 sub-view frustums are similar). In this embodiment, the side lengths of the bottom surfaces of the 4 sub-view frustums are respectively \((112.55, 62.33, 113.71, 62.69)\).

[0136] S403. Cyclically judge the feasibility of the query point among the 4 sub-view frustums. First, obtain the sampling point on the central axis with the same depth as the query point. By calculating and comparing the projection lengths \(OP\cdot e\) center and \(OA_i\cdot e\) center on the central axis, determine whether the query point and the origin are on the same side of the plane of the bottom surface of the sub-view frustum. If it is greater, it means that the point and the origin are on the same side of the plane of the bottom surface of the sub-view frustum; otherwise, they are not. Based on this, determine in which sub-view frustum the query point is located. Then project it onto the plane of the bottom surface of the sub-view frustum, that is, calculate its projection position on the plane of the bottom surface. The method is to calculate the projection of the query point on this plane according to the relative positions of the query point and the center of the bottom surface. Assume that the query point is \(P\), the projection of the query point on the central axis is \(K\), the projection of the query point on the plane of the bottom surface is \(P'\) ′ and the projection of the origin on the plane of the bottom surface is \(O'\) ′ . According to the formula

[0137] \(O'P' = KP = OP - OK = OP - OP\cdot e\) center

[0138] Since the origin is the intersection point of the central axis and this plane, by comparing the \(x\) and \(y\) values of the vector with the coordinates of the sub-view frustum, it can be determined whether the ray from the origin to the query point is within this small quadrangular pyramid. Then determine whether the query point is inside the view frustum, based on the fact that the query point is within the region surrounded by the four side surfaces and the respective bottom surfaces of the four sub-view frustums. If the query point and the origin are not on the same side of the plane of the bottom surface of the sub-view frustum and the ray from the origin to the query point is within this sub-view frustum, it means that the query point is not inside the view frustum. If no such negative judgment condition appears after the loop ends, it means that the query point is inside the view frustum, that is, the query point is within the feasible region. Based on this, mark all the feasible regions in this past frame. In this embodiment, the target point in \(R\) i is inside the lower left sub-view frustum.

[0139] After that, determine whether the target point is within this region, so as to confirm whether the target point is a feasible point in this past frame. In \(R\) i , it is determined that \((1998.94, 740.42, 9.96)\) is feasible.

[0140] In this embodiment, the calibrated feasible region of \(R\) i+4 is as shown in Figure 3

[0141] ​S5. The target point planning algorithm outputs the planned optimal path.

[0142] S501. The heuristic search algorithm (A*)

[0143] The system traverses all candidate target points within the search area and plans the optimal path based on the A* heuristic search algorithm. During this process, each candidate point is regarded as a search node, and the actual cost G from the starting point to the current node and the heuristic estimated cost H from the current node to the target end point are calculated for it. The sum of them constitutes the total cost F. The formula is as follows:

[0144] F = G + H

[0145] Where G is the actual cost from the starting point to the current node, and H is the estimated cost from the current node to the target end point.

[0146] The algorithm adds the starting point node to the open list, repeatedly selects the node with the smallest F value as the current node, and expands its adjacent nodes. During the expansion process, the path information is continuously updated. Finally, when the target node is expanded and its total cost is confirmed to be the lowest, the algorithm terminates and outputs the planned optimal path. This method can efficiently balance the search efficiency and path optimality, ensuring that the drone quickly and safely selects the appropriate target point during autonomous navigation.

[0147] The above has described in detail the preferred specific embodiments of the present invention. It should be understood that those of ordinary skill in the art can make many modifications and variations according to the concept of the present invention without creative labor. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field of the present invention based on the concept of the present invention through logical analysis, reasoning or limited experiments should be within the protection scope determined by the claims.

Claims

1. An autonomous navigation method for an unmanned aerial vehicle based on deep image processing, characterized in that, The method includes the following steps: S1. Feasible region search S101. Region point set acquisition: All candidate point sets within the current search region are acquired through sensors, providing basic data for subsequent judgments. S102. Current frame judgment: For the selected target point, the current frame is judged for feasibility. If feasible, subsequent target point judgments are carried out; if not, reverse iterative queries are performed for this target point. S2. Point cloud generation: Based on depth map data and the angle table, the local coordinates of 8*8 sampling points in the target region in the current frame are calculated using the corresponding coordinate conversion formula. S201. Depth table query: The depth value of each region is extracted from the depth map data. S202. Angle table query: The pre-calibrated angle mapping table is consulted to obtain the Pitch and Yaw values corresponding to each region. S203. Coordinate calculation: Based on the depth and angle data, the local coordinates of the target region in the current frame are calculated using the corresponding coordinate conversion formula. S3. Coordinate conversion S301. Coordinate system alignment: The coordinate systems of the lidar and the drone are precisely aligned to ensure accurate data conversion. S302. Calculation of the transformation matrix S303. Target point coordinate calibration: The transformation matrix is used to convert the point to be queried from the coordinate system of this frame to the local coordinate system of the previous frame, realizing coordinate calibration. S4. Feasibility judgment algorithm S401. Query of surrounding points The 8*8 sampling points and the point to be queried are unified to the same x value in depth, the ray closest to the point to be queried is judged, and the other three rays are complemented. S402. Sub-cone graphic segmentation: According to the four rays around the point to be queried obtained, the angular bisector of the cone and the central axis of the cone are obtained, and the length of the bottom surface of the sub-cone is obtained according to the angle relationship. S403. Sub-cone loop judgment: The four sub-cones of the cone are loop-judged to determine whether the target point is within the ray region of the sub-cone and whether the target point and the origin are on the same plane as the bottom surface of the sub-cone. S5. Target point planning, and the planned optimal path is output, specifically as follows S501. Heuristic search algorithm (A*): The system traverses all candidate target points within the search region and plans the optimal path based on the A* heuristic search algorithm. During this process, each candidate point is regarded as a search node, and the actual cost G from the starting point to the current node and the heuristic estimated cost H from the current node to the target end point are calculated for it. Their sum constitutes the total cost F, and the formula is as follows: F = G + H where G is the actual cost from the starting point to the current node, and H is the estimated cost from the current node to the target end point. The algorithm adds the starting point node to the open list, repeatedly selects the node with the smallest F value as the current node, and expands its adjacent nodes. During the expansion process, the path information is continuously updated. Finally, when the target node is expanded and its total cost is confirmed to be the lowest, the algorithm terminates and outputs the planned optimal path.

2. The UAV autonomous navigation method based on deep image processing according to claim 1, wherein In step S101, all candidate point sets within the current search region refer to all candidate points searched within the same horizontal plane as the drone, that is, the points to be queried. In step S102, for the reverse iterative query, the precondition is that during the flight of the drone, a linked list storage structure has been constructed based on the drone pose data and depth map data, a circular buffer with a length of N is created, the linked list storage structure is initialized, the maximum recursion depth is set to K, and each node in the linked list contains inter-frame coordinate transformation information (pose transformation information pos from the previous frame to this frame), depth image data of the previous frame (dist), and a pointer to the next node in the linked list (preFrame).

3. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 1, wherein In step S102, a reverse iterative query is performed on the target point. The input parameter is the local coordinates of the target point to be queried in this frame. At this time, the head node of the linked list is the depth image of the previous frame and the pose transformation vector from the previous frame to this frame. The pose transformation vector includes three-dimensional Cartesian coordinates (X, Y, Z) and three-dimensional angular coordinates (Yaw, Pitch, Roll). The depth image data is an 8×8 single-precision floating-point matrix, and each element corresponds to the depth value of the center point of the lidar field of view grid. The target point data is three-dimensional double-precision floating-point coordinates (x t , y t , z t ), which is generated by the feasible region search algorithm.

4. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 1, wherein In step S2, based on the depth map data and the angle table, the local coordinates of 8×8 sampling points in the target area in the current frame are calculated using the corresponding coordinate transformation formula. The depth map data is stored in the linked list, and the angle mapping table is a set of queryable constants. For each sampling point, its coordinate transformation, i.e., the point cloud generation formula, is as follows: X rel = Res dis Among them, Res dis is the depth value, Pitch and Yaw are the pitch angle and yaw angle of beam calibration, (X rel , Y rel , Z rel ) is the coordinate of the sampling point converted from the depth value.

5. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 1, characterized in that In step S3, for the coordinate transformation algorithm, the work done is to transform the local coordinates of the point to be queried in this frame into the local coordinates in the previous frame. The coordinate transformation matrix and formula are as follows: (x t ,y t ,z t )·R yaw ·R pitch ·R roll +(x pos ,y pos ,z pos )=(x t-1 ,y t-1 ,z t-1 ) where (x t , y t , z t ) are the local coordinates of the point to be queried in this frame, (x pos , y pos , z pos ) are the coordinate parts of the pose transformation vector from the previous frame to this frame, (x t-1 , y t-1 , z t-1 ) are the local coordinates of the point to be queried in the previous frame, R yaw , R pitch , R roll are rotation transformation matrices, and Pitch and Yaw are the pitch angle and yaw angle of beam calibration; After that, the lidar coordinate system is mapped to the drone local coordinate system through the transformation matrix: where is the coordinate of the point to be queried in the local coordinate system of the UAV, is the local coordinate of the point to be queried in the lidar coordinate system.

6. The method for autonomous navigation of a drone based on depth image processing according to claim 1, characterized in that In step S401, the 8×8 sampling points and the point to be queried are unified to the same x value, the ray closest to the point to be queried is judged, and the other three rays are complemented. The main objective of this algorithm is to determine which four rays in the 8×8 ray grid are the most "proximate" to the point to be queried according to the point to be queried, and output the indices of these four rays in the grid. First, uniformly depth the 8*8 sampling points and the query point to the same x, that is, 100. According to the point cloud generation formula, let Res dis = 100, and we can get X = Res dis XYZRay i,j =(X, Y, Z) i,j XYZRay i,j The ray vector after unifying the depth for the sampling point with index (i, j). When traversing all rays, for each ray, the distance between it and the query point is calculated by the following formula: points is the ray vector from the origin to the point to be queried. Record the ray index with the minimum distance during traversal, i.e., res i and res j , which represents the ray with the shortest distance to the query point in the current grid.

7. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 6, wherein In step S401, when complementing the other three rays, according to the three components of the query point, the y and z coordinates of the query point are scaled according to the ratio of the x coordinate. The formula is as follows: It is to map the query point to a fixed reference plane (x = 100) so as to compare with the direction data of the ray to find the nearest ray With reference to the y and z components of the ray, based on the comparison between the y and z values of the query point and the corresponding values of the ray, the relative position of the query point in the grid cell is determined, and then four adjacent rays are selected The algorithm is divided into four cases: Case 1: When and It indicates that the query point is in the "lower right" area of the ray, then the four ray indexes selected are: (res i ,res j ),(res i ,res j +1),(res i +1,res j ),(res i +1,res j +1); Case 2: When and It indicates that the query point is in the area with a smaller yy direction but a larger z direction, then select: (res i -1, res j ), (res i -1, res j +1), (res i , res j ), (res i , res j +1); Case 3: When and It indicates that the query point is in the area with a larger y-direction but a smaller z-direction. Then the four ray indices selected are: (res i , res j -1), (res i , res j ), (res i +1, res j -1), (res i +1, res j ); Case 4: When and It indicates that the query point is in a region where it is larger in both directions, then select: (res i -1, res j -1), (res i -1, res j ), (res i , res j -1), (res i , res j ); In this way, the algorithm actually determines a rectangular area composed of four rays, which surrounds the position where the query point is located, ensuring that the query point is inside the area formed by these four rays.

8. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 7, wherein In step S402, for the sub-frustum graphic segmentation, the angular bisector of the frustum and the central axis of the frustum are obtained according to the four rays around the point to be queried, and the length of the bottom surface of the sub-frustum is obtained according to the angular relationship. The formula is as follows: According to the target point coordinates, the indices of the four field-of-view rays around the target point, the coordinates of the four sampling points corresponding to the four field-of-view rays, the central axis vector of the frustum and the side length of the bottom surface of the sub-frustum are calculated. Assume that the four field rays are OA1, OA2, OA3, and OA4, then the four angle bisectors are The central axis is The length of the bottom side of the sub-view cone is (taking the sub-view cone in the upper left corner as an example):

9. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 7, wherein In step S403, for the sub-frustum loop judgment, the four sub-frustums of the frustum are loop judged to determine whether the point to be queried is within the sub-frustum ray area and whether the point to be queried and the origin are on the same plane as the bottom surface of the sub-frustum; (1) Judge whether the point to be queried and the origin are on the same side of the sub-frustum bottom surface plane By calculating and comparing, the projected lengths of the point to be queried and the sampling point on the central axis are OP·e center and OA i ·e center , to determine whether the point to be queried is on the same side of the bottom plane of the sub-view frustum as the origin. If it is greater, it means that the point to be queried is on the same side of the bottom plane of the sub-view frustum as the origin, otherwise it is not; (2) Calculate the projection of the point to be queried relative to the bottom surface. For the point to be queried, it is necessary to project it onto the bottom plane of the sub-view frustum, that is, calculate its projection position on the bottom plane. The method is to calculate the projection of the point to be queried on this plane according to the relative position between the point to be queried and the center of the bottom surface. Assume that the point to be queried is P, the projection of the point to be queried on the central axis is K, the projection of the point to be queried on the bottom plane is P′, and the projection of the origin on the bottom plane is O′. According to the formula O′P′ = KP = OP - OK = OP - OP·e center Calculate the projection of the point P to be queried on the bottom surface of the sub-view frustum. The origin is the intersection of the central axis and this plane. Then, by comparing the x and y values of the vector with the coordinates of the sub-view frustum, it can be determined whether the ray from the origin to the point to be queried is within this small quadrangular pyramid. (3) Determine whether the point to be queried is inside the view frustum. The basis for judgment here is that the point to be queried is within the area surrounded by the four side faces and the bottom surfaces of the four sub-view frustums respectively. Specifically, the judgment steps are as follows: Loop through each sub-view frustum to determine whether the point to be queried is on the same side of the sub-view frustum bottom plane as the origin and whether the ray from the origin to the point to be queried is within this sub-view frustum. If the point to be queried and the origin are not on the same side of the sub-view frustum bottom plane and the ray from the origin to the point to be queried is within this sub-view frustum, it means that the point to be queried is not inside the view frustum. If the above negative judgment condition does not occur after the loop ends, it means that the point to be queried is inside the view frustum.

10. The method for autonomous navigation of an unmanned aerial vehicle based on depth image processing according to claim 7, wherein In step S501, traverse the points to be queried within the search area, obtain the feasible area through reverse iterative loop query, and plan the path through the breadth-first search algorithm. Determination method for whether the target point inside the view frustum is on the same side of the sub-view frustum bottom as the origin. Its implementation includes: 1). Sampling point coordinates A of the known sub-cone i ; 2). The secondary parameter calculated according to claim 8, given the central axis e of the visual cone center 3). Given the coordinates P of the target point 4). Calculate vectors OP and OA i In vector e center Magnitude relationship of the projection 5). If $\overrightarrow{OP} \cdot \overrightarrow{e}$ center <$ \overrightarrow{OA} $ i $\cdot \overrightarrow{e}$ center Then the target point and the origin are on the same side of the plane of the bottom surface of the sub-view frustum, otherwise they are not.

Images