Unmanned aerial vehicle real-time three-dimensional obstacle avoidance method and system based on Cubic Spline offline path library

Abstract

The invention discloses an unmanned aerial vehicle real-time three-dimensional obstacle avoidance method and system based on a Cubic Spline off-line path library, and the method comprises the following steps: generating a three-dimensional path group covering a forward sensing space in an off-line manner through a Cubic Spline interpolation method based on a course angle threshold value and a pitch angle threshold value of an unmanned aerial vehicle; dividing a three-dimensional space covered by the three-dimensional path group into voxel grids with dynamic density, dynamically adjusting the voxel density of near-end and far-end areas in the voxel grids according to the distance of the sensor, and establishing a collision index relation table of voxels and paths; the obstacle point cloud coordinates are converted to a machine system, a collision-free path set is rapidly screened by matching a voxel grid and a collision index relation table, and the collision-free path set is obtained based on the course angle difference and the pitch angle difference between the tail end of a path in the collision-free path set and a target point and the deviation degree between the path and the center line of a collision-free area. And selecting an optimal path through a weighted heuristic rule.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV autonomous navigation and obstacle avoidance, and specifically relates to a method and system for real-time three-dimensional obstacle avoidance of UAVs based on a Cubic Spline offline path library. Background Art

[0002] In complex tasks such as disaster rescue and boiler inspection, UAVs are increasingly widely used, but their autonomous exploration capabilities still face severe challenges. Limited by the performance of the on-board computing platform, the resolution of the navigation map is usually low, resulting in small obstacles being difficult to accurately detect, and dynamic obstacles or environmental changes may invalidate the pre-planned path. In the prior art, although the obstacle avoidance algorithm based on online optimization can adapt to environmental changes, its computational complexity is high and it is difficult to meet the real-time requirements; the fixed path library method reduces the computational load but cannot dynamically respond to obstacles. How to achieve efficient obstacle avoidance with limited computing power while taking into account path safety and environmental adaptability has become a technical problem to be solved urgently. Summary of the Invention

[0003] The present invention aims to solve the deficiencies of the prior art and provides the following solutions:

[0004] A method for real-time three-dimensional obstacle avoidance of UAVs based on a Cubic Spline offline path library, comprising the following steps:

[0005] Based on the heading angle threshold and pitch angle threshold of the UAV, a three-dimensional path group covering the forward sensing space is generated offline by the Cubic Spline interpolation method;

[0006] The three-dimensional space covered by the three-dimensional path group is divided into voxel grids with dynamic density, and the voxel density in the proximal and distal regions of the voxel network is dynamically adjusted according to the sensor distance, and a collision index relationship table between voxels and paths is established;

[0007] The obstacle point cloud coordinates are converted to the body coordinate system of the aircraft, and the collision-free path set is quickly screened by matching the voxel grid and the collision index relationship table, and the optimal path is selected based on the heading angle difference, pitch angle difference between the end of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area through a weighted heuristic rule.

[0008] Preferably, the method for generating the three-dimensional path group includes:

[0009] Given a set of interpolation points, a piecewise cubic polynomial is constructed based on the interpolation points:

[0010] S i (x)=a i (x - x i ) 3 +bi (x - x i ) 2 + c i (x - x i ) + d i

[0011] where a i , b i , c i , d i represent the polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point;

[0012] Based on the cubic polynomial, make S i (x i ) = y i , S i (x i+1 ) = y i+1 , S’ i (x i+1 ) = S’ i+1 (x i+1 ) and S” i (x i+1 ) = S” i+1 (x i+1 ), and complete the interpolation fitting of the path points;

[0013] Based on the drone heading angle, pitch angle constraints, and the maximum length dis of a single segment of the path, specify the path library generation range and calculate the initial non-offset control point R c :

[0014] R c = 0:0.1:dis

[0015] Based on the adjustable step size, the initial non-offset control point, the heading angle threshold, and the pitch angle threshold, calculate the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint:

[0016] Shift Hori = -horiAngle:del Hor :horiAngle

[0017] Shift Vert = -vertAngle:del Vert :vertAngle

[0018] where del Vert represents the adjustable step size of the heading angle, del Hor represents the adjustable step size of the pitch angle, horiAngle represents the heading angle threshold, vertAngle represents the pitch angle threshold, ShiftHori Indicates the horizontal offset of the heading angle, Shift Vert Indicates the vertical offset of the pitch angle constraint;

[0019] Calculate the horizontal and vertical offset angles based on the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint, and calculate the three-axis coordinates of the path point:

[0020] X i = R c * cos(Shif tHori * pi / 180)

[0021] Y i = R c * sin(Shift Hori * pi / 180)

[0022] Z i = R c * sin(Shift Vert * pi / 180)

[0023] Among them, X i Indicates the X-axis coordinate of the path point, Y i Indicates the Y-axis coordinate of the path point, Z i Indicates the Z-axis coordinate of the path point, pi represents the pi;

[0024] After completing the interpolation fitting of the first group of paths, based on the end point of the first group of path points as the starting point, iteratively perform the interpolation fitting of the second and third groups of paths to obtain the three-dimensional path group.

[0025] Preferably, the method for obtaining the collision index relationship table includes:

[0026] Based on the coverage space of the three-dimensional path group and the body size, obtain the three-dimensional segmentation ranges (startX, endX), (startY, endY), and (startZ, endZ);

[0027] Calculate the scaling factors on the Y-axis and Z-axis based on the three-dimensional segmentation range:

[0028]

[0029] Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, x t Represents the X-axis coordinate of the voxel, R Hor Represents the horizontal search radius during voxel neighbor search, R Ver Represents the vertical search radius during voxel neighborhood search;

[0030] Calculate the three-dimensional coordinates of a single voxel based on the scaling factor and the spatial division voxel step size:

[0031] x t = endX - voxelSize * indX

[0032] y t = scaleY + (endY - voxelSize * indY)

[0033] z t = scaleZ * (endZ - voxelSize * indZ)

[0034] where y t represents the Y-axis coordinate of the voxel, z t represents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the voxel index number on the X-axis, indY represents the voxel index number on the Y-axis, and indZ represents the voxel index number on the Z-axis;

[0035] For each voxel with determined three-dimensional coordinates, according to the search radius R during voxel neighbor search Hor and R Ver , find the matching relationship between each voxel and the path. When the path points are included within the voxel range, the path to which the path points belong is defined as the occluded path;

[0036] Record the number of each voxel point, the number of paths occluded by the voxel point, and the occluded path numbers in the text file to generate the collision index relationship table.

[0037] Preferably, the method for selecting the optimal path by the weighted heuristic rule includes:

[0038] Convert the obstacle point cloud coordinates to the body coordinate system, and quickly screen the collision-free path set by matching the voxel grid with the collision index relationship table;

[0039] Obtain that the pitch angle difference between the end point of the i-th path in the collision-free path set and the body is endPitch[i], and the yaw angle difference with the body is endYaw[i];

[0040] Obtain the pitch angle difference GoalPitch and yaw angle difference GoalYaw between the desired target point and the body, and calculate the angle difference between the path end point and the desired target point:

[0041] Yaw Diff = GoalYaw - endYaw[i]

[0042] Pitch Diff = GoalPitch - endPitch[i]

[0043] Among them, Yaw Diff represents the heading angle difference between the end point of the path and the desired target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the desired target point;

[0044] Based on the angle difference between the end point of the path and the desired target point, an optimal path is selected through a weighted heuristic rule:

[0045] score = (1 - Pitch Wei * Pitch Diff ) * (1 - Yaw Wei * Yaw Diff ) * (1 - Center Diff / 60)

[0046] Among them, score represents the heuristic function, and Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the heading angle difference weight, and Center Diff represents the free path center offset weight;

[0047] Sort the weighted synthesis of the heuristic functions of each path, and select the path with the smallest synthesis as the optimal path.

[0048] The present invention also provides a UAV real-time three-dimensional obstacle avoidance system based on a Cubic Spline offline path library. The system applies the method described in any one of the above, and includes: a path group construction module, a relationship table construction module, and an optimal path selection module;

[0049] The path group construction module generates a three-dimensional path group covering the forward sensing space offline through a Cubic Spline interpolation method based on the heading angle threshold and pitch angle threshold of the UAV;

[0050] The relationship table construction module is used to divide the three-dimensional space covered by the three-dimensional path group into voxel grids with dynamic density, dynamically adjust the voxel density of the proximal and distal regions in the voxel network according to the sensor distance, and establish a collision index relationship table between voxels and paths;

[0051] The optimal path selection module is used to convert the obstacle point cloud coordinates to the body frame, quickly screen the collision-free path set by matching the voxel grid and the collision index relationship table, and select the optimal path through a weighted heuristic rule based on the heading angle difference, pitch angle difference between the end of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area.

[0052] Preferably, the working process of the path group construction module includes:

[0053] Given a set of interpolation points, construct a piecewise cubic polynomial based on the interpolation points:

[0054] S i (x) = a i (x - x i ) 3 + b i (x - x i ) 2 + c i (x - x i ) + d i

[0055] where a i , b i , c i , d i represent the polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point;

[0056] Based on the cubic polynomial, make S i (x i ) = y i , S i (x i+1 ) = y i+1 , S’ i (x i+1 ) = S’ i+1 (x i+1 ) and S” i (x i+1 ) = S” i+1 (x i+1 ), and complete the interpolation fitting of the path points;

[0057] Based on the heading angle, pitch angle constraints of the UAV and the maximum length dis of a single segment of the path, specify the generation range of the path library and calculate the initial non-offset control point R c :

[0058] R c = 0:0.1:dis

[0059] Based on the adjustable step size, the initial non-offset control point, the heading angle threshold, and the pitch angle threshold, calculate the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint:

[0060] Shift Hori = -horiAngle:del Hor : horiAngle

[0061] Shifft Vert=-vertAngle:del Vert :vertAngle

[0062] where del Vert represents the adjustable step of the course angle, del Hor represents the adjustable step of the pitch angle, horiAngle represents the course angle threshold, vertAngle represents the pitch angle threshold, Shift Hori represents the horizontal offset of the course angle, Shift Vert represents the vertical offset constraint of the pitch angle;

[0063] Calculate the horizontal direction offset angle and the vertical direction offset angle based on the horizontal offset of the course angle and the vertical offset constraint of the pitch angle, and calculate the three-axis coordinates of the path point:

[0064] X i =R c *cos(Shift Hori *pi / 180)

[0065] Y i =R c *sin(Shift Hori *pi / 180)

[0066] Z i =R c *sin(Shift Vert *pi / 180)

[0067] where X i represents the X-axis coordinate of the path point, Y i represents the Y-axis coordinate of the path point, Z i represents the Z-axis coordinate of the path point, pi represents the pi;

[0068] After completing the interpolation fitting of the first group of paths, based on the end point of the first group of path points as the starting point, iteratively perform the interpolation fitting of the second group and the third group of paths to obtain the three-dimensional path group.

[0069] Preferably, the working process of the relationship table construction module includes:

[0070] Based on the coverage space of the three-dimensional path group and the body size, obtain the three-dimensional segmentation ranges (startX, endX), (startY, endY) and (startZ, endZ);

[0071] Calculate the scaling factors on the Y-axis and Z-axis based on the three-dimensional segmentation ranges:

[0072]

[0073] Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, and x t represents the X-axis coordinate of the voxel, and R Hor represents the horizontal search radius during the voxel neighborhood search, and R Ver represents the vertical search radius during the voxel neighborhood search;

[0074] Based on the scaling factor and the spatial segmentation voxel step size, calculate the three-dimensional coordinates of a single voxel:

[0075] x t = endX - voxelSize * indX

[0076] y t = scaleY + (endY - voxelSize * indY)

[0077] z t = scaleZ * (endZ - voxelSize * indZ)

[0078] Among them, y t represents the Y-axis coordinate of the voxel, z t represents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the voxel index number of the X-axis, indY represents the voxel index number of the Y-axis, and indZ represents the voxel index number of the Z-axis;

[0079] For each voxel with determined three-dimensional coordinates, according to the search radii R Hor and R Ver during the voxel neighborhood search, find the matching relationship between each voxel and the path. When the path points are included in the voxel range, the path to which the path points belong is defined as the occluded path;

[0080] Record the number of each voxel point, the number of paths occluded by the voxel point, and the occluded path numbers in the text file to generate the collision index relationship table.

[0081] Preferably, the working process of the optimal path selection module includes:

[0082] Convert the obstacle point cloud coordinates to the body system, and quickly screen the collision-free path set by matching the voxel grid with the collision index relationship table;

[0083] Obtain that the pitch angle difference between the end point of the i-th path in the collision-free path set and the body is endPitch[i], and the yaw angle difference from the body is endYaw[i];

[0084] Obtain the pitch angle difference GoalPitch and yaw angle difference GoalYaw between the desired target point and the airframe, and calculate the angle difference between the end point of the path and the desired target point:

[0085] Yaw Diff = GoalYaw - endYaw[i]

[0086] Pitch Diff = GoalPitch - endPitch[i]

[0087] Among them, Yaw Diff represents the yaw angle difference between the end point of the path and the desired target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the desired target point;

[0088] Based on the angle difference between the end point of the path and the desired target point, select the optimal path through the weighted heuristic rule:

[0089] score = (1 - Pitch Wei * Pitch Diff ) * (1 - Yaw Wei * Yaw Diff ) * (1 - Center Diff / 60)

[0090] Among them, score represents the heuristic function, Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the yaw angle difference weight, and Center Diff represents the free path center offset weight;

[0091] Sort the weighted synthesis of the heuristic functions of each path, and select the path with the smallest synthesis as the optimal path.

[0092] Compared with the prior art, the beneficial effects of the present invention are:

[0093] The present invention transfers the high-complexity calculation in the traditional online path planning to the offline stage through the offline path pre-generation and dynamic voxel indexing technology. In the online stage, only table lookup and simple weighted calculation are required, significantly reducing the computational load and improving the real-time performance. The voxel grid with dynamic density takes into account both the proximal obstacle detection accuracy and the global computational efficiency, while the heuristic rule avoids the critical path risk by comprehensively considering the target point directivity and the path safety margin. This method is applicable to small UAV platforms with limited computing resources and can effectively meet the real-time obstacle avoidance requirements in complex dynamic environments. Description of the Drawings

[0094] To more clearly illustrate the technical solution of the present invention, the accompanying drawings required for use in the embodiments will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other accompanying drawings can be obtained based on these drawings without creative efforts.

[0095] Figure 1 It is a schematic flowchart of the method of the embodiment of the present invention;

[0096] Figure 2 It is a schematic diagram of generating a three-dimensional path group of the embodiment of the present invention. Specific embodiments

[0097] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0098] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0099] Embodiment 1

[0100] In this embodiment, as Figure 1 shown, a real-time three-dimensional obstacle avoidance method for an unmanned aerial vehicle based on a Cubic Spline offline path library includes the following steps:

[0101] S1. Based on the heading angle threshold and pitch angle threshold of the unmanned aerial vehicle, a three-dimensional path group covering the forward sensing space is generated offline by the Cubic Spline interpolation method.

[0102] The method for generating the three-dimensional path group includes: given a set of interpolation points (x0, y0), (x1, y1),... (x n , y n ), a piecewise cubic polynomial is constructed based on the interpolation points:

[0103] S i (x) = a i (x - x i ) 3 + b i (x - x i ) 2 + c i (x - x i ) + d i

[0104] wherein, a i , b i , c i , d i represent polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point; based on the cubic polynomial, for each interval [x i , x i+1 , make S i (x i ) = y i and S i (x i+1 ) = y i+1 , that is, d i = y i ; then, according to the continuity of the first derivative of the spline curve, there is S’ i (x i+1 ) = S’ i+1 (x i+1 ); according to the continuity of the second derivative of the spline curve, there is S” i (x i+1 ) = S” i+1 (x i+1 ), and the interpolation fitting of the path points is completed; for the generation of the interpolation points, based on the heading angle, pitch angle constraints of the UAV and the maximum length dis of a single segment of the path, the generation range of the path library is specified, and dis / 0.1 data points are generated as interpolation points with the center of the UAV as the starting point, with a step size of 0.1 and dis as the initial path length, and the initial non-offset control point R c is calculated as follows:

[0105] R c = 0:0,1:dis

[0106] Based on the adjustable step size, the initial non-offset control point, the heading angle threshold, and the pitch angle threshold, calculate the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint:

[0107] Shift Hori = -horiAngle:del Hor :horiAngle

[0108] Shift Vert = -vertAngle:del Vert :vertAngle

[0109] wherein, del Vert represents the adjustable step size of the heading angle, del Hor represents the adjustable step size of the pitch angle, horiAngle represents the heading angle threshold, vertAngle represents the pitch angle threshold, ShiftHori Indicates the horizontal offset of the heading angle, Shift Vert Indicates the vertical offset of the pitch angle constraint; using the Cubic Spline interpolation method, calculate the horizontal and vertical offset angles based on the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint, and calculate the three-axis coordinates of the path points:

[0110] X i = R c * cos(Shift Hori * pi / 180)

[0111] Y i = R c * sin(Shif Hori * pi / 180)

[0112] Z i = R c * sin(Shift Vert * pi / 180)

[0113] Among them, X i Indicates the X-axis coordinate of the path point, Y i Indicates the Y-axis coordinate of the path point, Z i Indicates the Z-axis coordinate of the path point, pi represents the circumference ratio; after completing the interpolation fitting of the first group of paths, set the heading angle threshold of the UAV to 30° and the pitch angle threshold to 10°. Taking the UAV body as the center, generate initial control points at a step of 10° in the horizontal direction and generate pitch angle offsets at a step of 10° in the vertical direction. Based on the end point of the first group of path points as the starting point, iteratively perform the interpolation fitting of the second and third groups of paths to obtain a three-dimensional path group.

[0114] Thus, the algorithm generated a path group in 35 directions in the first round of sampling. Subsequently, the algorithm iteratively performs the second and third rounds of sampling based on the above 35 directions. Finally, after three rounds of sampling, the algorithm generated 42,875 paths in the specified space, as Figure 2 shown.

[0115] S2. Divide the three-dimensional space covered by the three-dimensional path group into voxel grids with dynamic density, dynamically adjust the voxel density of the proximal and distal regions in the voxel network according to the sensor distance, and establish a collision index relationship table between the voxels and the paths.

[0116] The method for obtaining the collision index relationship table includes: based on the covered space of the three-dimensional path group and the body size, obtain the three-dimensional segmentation ranges (startX, endX), (startY, endY), and (startZ, endZ); calculate the scaling factors on the Y-axis and Z-axis based on the three-dimensional segmentation ranges:

[0117]

[0118] Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, x t represents the X-axis coordinate of the voxel, and R Hor represents the horizontal search radius during the voxel neighbor search, and R Ver represents the vertical search radius during the voxel neighborhood search; based on the scaling factor and the spatial segmentation voxel step size, the three-dimensional coordinates of a single voxel are calculated:

[0119] x t = endX - voxelSize * indX

[0120] y t = scaleY + (endY - voxelSize * indY)

[0121] z t = scaleZ * (endZ - voxelSize * indZ)

[0122] Among them, y t represents the Y-axis coordinate of the voxel, z t represents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the voxel index number on the X-axis, indY represents the voxel index number on the Y-axis, and indZ represents the voxel index number on the Z-axis; for each voxel whose three-dimensional coordinates are determined, according to the search radii R Hor and R Ver , the matching relationship between each voxel and the path is found. When the path points are included in the voxel range, the path to which the path points belong is defined as the occluded path; in the text file, the number gridID of each voxel point, the number of occluded paths collisionNum of the voxel point, and the occluded path number collisionPathID are recorded to generate a collision index relationship table.

[0123] In this embodiment, the dynamic density adjustment of the three-dimensional voxel grid is achieved through the horizontal scaling factor and the vertical scaling factor. The scaling factor is dynamically calculated according to the coordinate of the voxel on the X-axis and the preset search radius. The voxel step size in the proximal region is set to 0.05 meters, and the step size in the distal region is set to 0.1 meters; the calculation formula for the horizontal scaling factor of each voxel is the reciprocal of the sum of the sensor horizontal search radius and the voxel X-axis coordinate, and the vertical scaling factor is the reciprocal of the sum of the sensor vertical search radius and the voxel X-axis coordinate, so that the voxel density in the proximal region is higher than that in the distal region.

[0124] The construction of the collision index relationship table includes the following steps: For each voxel point, based on a preset horizontal search radius and vertical search radius, find all paths passing through the voxel space range; if a path point falls within the voxel space, mark the path as an occluded path, and record the voxel number, the number of occluded paths, and the corresponding path numbers in the index table; the index table is stored in the form of a hash table, with the voxel number as the key and the set of occluded path numbers as the value.

[0125] S3. Convert the obstacle point cloud coordinates to the body frame, quickly screen the collision-free path set by matching the voxel grid with the collision index relationship table, and select the optimal path based on the heading angle difference, pitch angle difference between the end of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area through a weighted heuristic rule.

[0126] The method for selecting the optimal path through a weighted heuristic rule includes: Convert the obstacle point cloud coordinates to the body frame, quickly screen the collision-free path set by matching the voxel grid with the collision index relationship table; obtain the pitch angle difference endPitch[i] between the end point of the i-th path in the collision-free path set and the body, and the heading angle difference endYaw[i] between the end point of the i-th path in the collision-free path set and the body; obtain the pitch angle difference GoalPitch and heading angle difference GoalYaw between the desired target point and the body, and calculate the angle difference between the end point of the path and the desired target point:

[0127] Yaw Diff = GoalYaw - endYaw[i]

[0128] Pitch Diff = GoalPitch - endPitch[i]

[0129] Among them, Yaw Diff represents the heading angle difference between the end point of the path and the desired target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the desired target point; based on the angle difference between the end point of the path and the desired target point, select the optimal path through a weighted heuristic rule:

[0130] score = (1 - Pitch Wei * Pitch Diff ) * (1 - Yaw Wei * Yaw Diff ) * (1 - Center Diff / 60)

[0131] Among them, score represents the heuristic function, Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the heading angle difference weight, Center DiffRepresents the free path center offset weight; sorts the weighted synthesis of the heuristic function for each path, and selects the path with the smallest synthesis as the optimal path.

[0132] In this embodiment, the calculation of the path center deviation is achieved through the following steps: according to the pitch angle offsets of all paths in the current collision-free path set, calculate their average value as the center line reference value; take the absolute difference between the pitch angle offset of each path and the center line reference value as the deviation degree. The greater the deviation degree, the closer the path is to the collision boundary, and its selection priority is reduced by increasing the weight.

[0133] The real-time matching of the obstacle point cloud includes the following steps: convert the obstacle point cloud data obtained by the lidar or depth camera to the body coordinate system of the UAV, and remove the noise through voxel filtering; for the filtered point cloud data, match it to the pre-generated voxel grid according to its three-dimensional coordinates, query the collision index table through the hash table, obtain all the blocked path numbers, and delete the corresponding paths from the path library to generate a collision-free path set.

[0134] Embodiment 2

[0135] In this embodiment, a real-time three-dimensional obstacle avoidance system for UAVs based on a Cubic Spline offline path library includes: a path group construction module, a relationship table construction module, and an optimal path selection module.

[0136] The path group construction module generates a three-dimensional path group covering the forward sensing space offline through the Cubic Spline interpolation method based on the heading angle threshold and pitch angle threshold of the UAV.

[0137] The workflow of the path group construction module includes: given a set of interpolation points, construct a piecewise cubic polynomial based on the interpolation points:

[0138] S i (x) = a i (x - x i ) 3 + b i (x - x i ) 2 + c i (x - x i ) + d i

[0139] Where a i , b i , c i , d i represent the polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point; based on the cubic polynomial, make S i (x i) = y i , S i (x i+1 ) = y i+1 , S’ i (x i+1 ) = S’ i+1 (x i+1 ) and S” i (x i+1 ) = S” i+1 (x i+1 ), complete the interpolation fitting of the path points; stipulate the generation range of the path library based on the heading angle, pitch angle constraints of the UAV and the maximum length dis of a single segment of the path, and calculate the initial non-offset control point R c :

[0140] R c = 0:0.1:dis

[0141] Based on the adjustable step size, the initial non-offset control point, the heading angle threshold and the pitch angle threshold, calculate the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint:

[0142] Shift Hori = -horiAngle:del Hor : horiAngle

[0143] Shift Vert = -vertAngle:del Vert : vertAngle

[0144] where del Vert represents the adjustable step size of the heading angle, del Hor represents the adjustable step size of the pitch angle, horiAngle represents the heading angle threshold, vertAngle represents the pitch angle threshold, Shift Hori represents the horizontal offset of the heading angle, Shift Vert represents the vertical offset of the pitch angle constraint; calculate the horizontal offset angle and the vertical offset angle based on the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint, and calculate the three-axis coordinates of the path points:

[0145] X i = R c * cos(Shift Hori * pi / 180)

[0146] Y i = R c * sin(Shift Hori * pi / 180)

[0147] Z i = Rc *sin(Shift Vert *pi / 180)

[0148] Among them, X i represents the X-axis coordinate of the path point, Y i represents the Y-axis coordinate of the path point, Z i represents the Z-axis coordinate of the path point, and pi represents the pi; after the interpolation fitting of the first group of paths is completed, based on the end point of the first group of path points as the starting point, the interpolation fitting of the second group and the third group of paths is iteratively performed to obtain a three-dimensional path group.

[0149] The relationship table construction module is used to divide the three-dimensional space covered by the three-dimensional path group into voxel grids with dynamic density, dynamically adjust the voxel density of the proximal and distal regions in the voxel network according to the sensor distance, and establish a collision index relationship table between the voxels and the paths.

[0150] The working process of the relationship table construction module includes: based on the covered space of the three-dimensional path group and the body size, obtaining the three-dimensional segmentation ranges (startX, endX), (startY, endY), and (startZ, endZ); calculating the scaling factors on the Y-axis and Z-axis based on the three-dimensional segmentation ranges:

[0151]

[0152] Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, x t represents the X-axis coordinate of the voxel, R Hor represents the horizontal search radius during voxel neighbor search, R Ver represents the vertical search radius during voxel proximity search; based on the scaling factors and the voxel step size of the space segmentation, calculate the three-dimensional coordinates of a single voxel:

[0153] x t = endX - voxelSize * indX

[0154] y t = scaleY + (endY - voxelSize * indY)

[0155] z t = scaleZ * (endZ - voxelSize * indZ)

[0156] Among them, y t represents the Y-axis coordinate of the voxel, z trepresents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the voxel index number on the X-axis, indY represents the voxel index number on the Y-axis, and indZ represents the voxel index number on the Z-axis; for each voxel with a determined three-dimensional coordinate, according to the search radius R during voxel neighborhood search Hor and R Ver , find the matching relationship between each voxel and the path. When the path points are included within the voxel range, the path to which the path points belong is defined as the occluded path; record the number of each voxel point, the number of paths occluded by the voxel point, and the occluded path numbers in a text file to generate a collision index relationship table.

[0157] The optimal path selection module is used to convert the coordinates of the obstacle point cloud to the body coordinate system, quickly screen the collision-free path set by matching the voxel grid with the collision index relationship table, and select the optimal path based on the heading angle difference, pitch angle difference between the end of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area through a weighted heuristic rule.

[0158] The working process of the optimal path selection module includes: converting the coordinates of the obstacle point cloud to the body coordinate system, and quickly screening the collision-free path set by matching the voxel grid with the collision index relationship table; obtaining the pitch angle difference between the end point of the i-th path in the collision-free path set and the body as endPitch[i], and the heading angle difference with the body as endYaw[i]; obtaining the pitch angle difference GoalPitch and heading angle difference GoalYaw between the expected target point and the body, and calculating the angle difference between the end point of the path and the expected target point:

[0159] Yaw Diff = GoalYaw - endYaw[i]

[0160] Pitch Diff = GoalPitch - endPitch[i]

[0161] where, Yaw Diff represents the heading angle difference between the end point of the path and the expected target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the expected target point; based on the angle difference between the end point of the path and the expected target point, select the optimal path through a weighted heuristic rule:

[0162] score = (1 - Pitch Wei * Pitch Diff ) * (1 - Yaw Wei * Yaw Diff ) * (1 - Center Diff / 60)

[0163] Among them, score represents the heuristic function, Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the yaw angle difference weight, Center Diff represents the free path center offset weight; the weighted synthesis of the heuristic functions of each path is sorted, and the path with the smallest synthesis is selected as the optimal path.

[0164] Embodiment III

[0165] In this embodiment, specific examples of the present application will be introduced.

[0166] (1) Offline path library construction: Taking the UAV body as the center, setting the heading angle threshold to 30° and the pitch angle threshold to 10°, and generating initial control points at a step of 10° in both the horizontal and vertical directions. After fitting the initial path by Cubic Spline interpolation, three-level iterative expansion is performed: 35 paths are generated in the first level, expanded to 1,225 in the second level, and 42,875 paths are formed in the third level, covering the forward 180° field of view. The three-dimensional coordinates of the path points are determined by polar coordinate conversion, the horizontal direction is calculated by the cosine component of the heading angle offset, the vertical direction is calculated by the sine component of the pitch angle offset, and the path length is controlled by the preset maximum flight distance. The coordinate sequences of all paths are stored as an offline path library.

[0167] (2) Voxel index table generation: According to the path coverage space, define the three-dimensional voxel segmentation range (X-axis 0 - 5 meters, Y-axis ±3 meters, Z-axis ±1 meter), with the voxel step size of 0.05 meters in the proximal area and 0.1 meters in the distal area. The horizontal scaling factor α y and the vertical scaling factor α z are calculated respectively by the formulas α y = 1 / (x + r y ), α z = 1 / (x + r z ), where x is the voxel X-axis coordinate, r y , r z is the search radius. By detecting the spatial inclusion relationship between the path points and the voxels, a collision index table in the form of a hash table is generated, with the voxel number as the key and the set of occluded paths as the value.

[0168] (3) Online obstacle avoidance process: After obtaining the obstacle point cloud data in real time, it is converted to the body coordinate system and filtered. Each point cloud data is matched with the voxel grid, and the occluded path numbers are obtained by looking up the table to generate a collision-free path set. Calculate the heading angle difference Δθ and pitch angle difference between the end of each path and the target point, as well as the deviation degree of the path from the center line of the collision-free area. Through the weighted formula Calculate the comprehensive score and select the path with the lowest score for output to the flight controller.

[0169] The embodiments described above are only descriptions of the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A real-time three-dimensional obstacle avoidance method for unmanned aerial vehicles based on a Cubic Spline offline path library, characterized in that, It includes the following steps: Based on the heading angle threshold and pitch angle threshold of the unmanned aerial vehicle, a three-dimensional path group covering the forward sensing space is generated offline by the Cubic Spline interpolation method; The three-dimensional space covered by the three-dimensional path group is divided into voxel grids with dynamic density, and the voxel density in the proximal and distal regions of the voxel network is dynamically adjusted according to the sensor distance, and a collision index relationship table between voxels and paths is established; The obstacle point cloud coordinates are converted to the body coordinate system, and a collision-free path set is quickly screened by matching the voxel grid and the collision index relationship table. Based on the heading angle difference, pitch angle difference between the end point of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area, the optimal path is selected by the weighted heuristic rule.

2. The real-time three-dimensional obstacle avoidance method for unmanned aerial vehicles based on a Cubic Spline offline path library according to claim 1, wherein The method for generating the three-dimensional path group includes: Given a set of interpolation points, a piecewise cubic polynomial is constructed based on the interpolation points: S i f(x)=a i (x - x i ) 3 + b i (x - x i ) 2 + c i (x - x i ) + d i Among them, a i , b i , c i , d i represent polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point; Based on the cubic polynomial, make S i (x i ) = y i , S i (x i+1 ) = y i+1 , S’ i (x i+1 ) = S’ i+1 (x i+1 ) and S” i (x i+1 ) = S” i+1 (x i+1 ), and complete the interpolation fitting of path points; Specify the path library generation range based on the heading angle and pitch angle constraints of the drone and the maximum length dis of a single-segment path, and calculate the initial non-offset control point R c : R c = 0:0.1:dis Based on the adjustable step size, the initial non-offset control points, the heading angle threshold, and the pitch angle threshold, the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint are calculated: Shift Hori = -horiAngle:del Hor :horiAngle Shift Vert =-vertAngle:del Vert :vertAngle Among them, del Vert represents the adjustable step size of the heading angle, del Hor represents the adjustable step size of the pitch angle, horiAngle represents the heading angle threshold, vertAngle represents the pitch angle threshold, Shift Hori represents the horizontal offset of the heading angle, Shift Vert represents the vertical offset constraint of the pitch angle; Based on the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint, the horizontal offset angle and the vertical offset angle are calculated, and the three-axis coordinates of the path points are calculated: X i = R c * cos(Shift Hori * pi / 180) Y t = R c * sin(Shift Hori * pi / 180) Z i = R c * sin(Shift Vert * pi / 180) Among them, X i represents the X-axis coordinate of the path point, Y i represents the Y-axis coordinate of the path point, Z i represents the Z-axis coordinate of the path point, and pi represents the pi; After the interpolation fitting of the first group of paths is completed, based on the end point of the first group of path points as the starting point, the interpolation fitting of the second group and the third group of paths is iteratively performed to obtain the three-dimensional path group.

3. The real-time three-dimensional obstacle avoidance method for an unmanned aerial vehicle based on a Cubic Spline offline path library according to claim 1, characterized in that, The method for obtaining the collision index relationship table includes: Based on the coverage space of the three-dimensional path group and the body size, the three-dimensional segmentation ranges (startX, endX), (startY, endY), and (startZ, endZ) are obtained; Based on the three-dimensional segmentation range, the scaling factors on the Y-axis and Z-axis are calculated: Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, x t represents the X-axis coordinate of the voxel, R Hor represents the horizontal search radius during the voxel neighborhood search, R Ver represents the vertical search radius during the voxel proximity search; Based on the scaling factor and the voxel step size of the space segmentation, the three-dimensional coordinates of a single voxel are calculated: x t = endX - voxelSize * indX y t = scaleY + (endY - voxelSize * indY) z t = scaleZ * (endZ - voxelSize * indZ) where y t represents the Y-axis coordinate of the voxel, z t represents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the voxel index number on the X-axis, indY represents the voxel index number on the Y-axis, and indZ represents the voxel index number on the Z-axis; For each voxel with determined three-dimensional coordinates, according to the search radius R during voxel neighborhood search Hor and R Ver , find the matching relationship between each voxel and the path. When the path points are included within the voxel range, the path to which the path points belong is defined as the occluded path; In the text file, the number of the voxel point, the number of paths blocked by the voxel point, and the blocked path numbers are recorded to generate the collision index relationship table.

4. The real-time three-dimensional obstacle avoidance method for an unmanned aerial vehicle based on a Cubic Spline offline path library according to claim 1, wherein The method for selecting the optimal path by the weighted heuristic rule includes: The obstacle point cloud coordinates are converted to the body coordinate system, and a collision-free path set is quickly screened by matching the voxel grid and the collision index relationship table; Obtain the pitch angle difference endPitch[i] between the end point of the i-th path in the collision-free path set and the body, and the heading angle difference endYaw[i] with the body; Obtain the pitch angle difference GoalPitch and heading angle difference GoalYaw between the expected target point and the body, and calculate the angle difference between the path end point and the expected target point: Yaw Diff = GoalYaw - endYaw[i] Pitch Diff = GoalPitch - endPitch[i] Among them, Yaw Diff represents the heading angle difference between the end point of the path and the desired target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the desired target point; Based on the angle difference between the path end point and the expected target point, the optimal path is selected by the weighted heuristic rule: score=(1 - Pitch Wei *Pitch Diff )*(1 - Yaw Wei *Yaw Diff )*(1 - Center Diff / 60) Among them, score represents the heuristic function, Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the yaw angle difference weight, Center Diff represents the free path center offset weight; Sort the weighted synthesis of the heuristic functions of each path, and select the path with the smallest synthesis as the optimal path.

5. A real-time three-dimensional obstacle avoidance system for an unmanned aerial vehicle based on a Cubic Spline offline path library, the system applying the method according to any one of claims 1-4, characterized in that, It includes: A path group construction module, a relationship table construction module, and an optimal path selection module; The path group construction module generates a three-dimensional path group covering the forward sensing space offline through the Cubic Spline interpolation method based on the heading angle threshold and pitch angle threshold of the UAV; The relationship table construction module is used to divide the three-dimensional space covered by the three-dimensional path group into voxel grids with dynamic density, dynamically adjust the voxel density in the proximal and distal regions of the voxel network according to the sensor distance, and establish a collision index relationship table between voxels and paths; The optimal path selection module is used to convert the obstacle point cloud coordinates to the body coordinate system, quickly screen the collision-free path set by matching the voxel grid and the collision index relationship table, and select the optimal path based on the heading angle difference, pitch angle difference between the end point of the path in the collision-free path set and the target point, and the deviation degree of the path from the center line of the collision-free area through the weighted heuristic rule.

6. The real-time three-dimensional obstacle avoidance system for unmanned aerial vehicles based on a Cubic Spline offline path library according to claim 5, wherein The working process of the path group construction module includes: Given a set of interpolation points, construct a piecewise cubic polynomial based on the interpolation points: S i f(x) = a i (x - x i ) 3 + b i (x - x i ) 2 + c i (x - x i ) + d i Among them, a i , b i , c i , d i represent polynomial coefficients, x represents the abscissa of the interpolation point, and x i represents the abscissa of the i-th interpolation point; Based on the cubic polynomial, make S i (x i ) = y i , S i (x i+1 ) = y i+1 , S’ i (x i+1 ) = S’ i+1 (x i+1 ), and S” i (x i+1 ) = S” i+1 (x i+1 ), to complete the interpolation fitting of path points; Specify the path library generation range based on the heading angle and pitch angle constraints of the drone and the maximum length dis of a single-segment path, and calculate the initial non-offset control point R c : R c = 0:0.1:dis Based on the adjustable step size, the initial non-offset control points, the heading angle threshold and the pitch angle threshold, calculate the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint: Shift Hori = -horiAngle:del Hor : horiAngle Shift Vert = -vertAngle:del Vert :vertAngle Among them, del Vert represents the adjustable step size of the heading angle, del Hor represents the adjustable step size of the pitch angle, horiAngle represents the heading angle threshold, vertAngle represents the pitch angle threshold, Shift Hori represents the horizontal offset of the heading angle, Shift Vert represents the constrained vertical offset of the pitch angle; Calculate the horizontal offset angle and vertical offset angle based on the horizontal offset of the heading angle and the vertical offset of the pitch angle constraint, and calculate the three-axis coordinates of the path points: X i = R c * cos(Shift Hori * pi / 180) Y i = R c * sin(Shift Hori * pi / 180) Z i = R c * sin(Shift Vert * pi / 180) Among them, X i represents the X-axis coordinate of the path point, Y i represents the Y-axis coordinate of the path point, Z i represents the Z-axis coordinate of the path point, and pi represents the pi; After completing the interpolation fitting of the first group of paths, use the end point of the first group of path points as the starting point, and iteratively perform the interpolation fitting of the second group and the third group of paths to obtain the three-dimensional path group.

7. The real-time three-dimensional obstacle avoidance system for unmanned aerial vehicles based on a Cubic Spline offline path library according to claim 5, wherein The working process of the relationship table construction module includes: Based on the coverage space of the three-dimensional path group and the body size, obtain the three-dimensional segmentation ranges (startX, endX), (startY, endY), and (startZ, endZ); Calculate the scaling factors on the Y-axis and Z-axis based on the three-dimensional segmentation range: Among them, scaleY represents the scaling factor of the Y-axis, scaleZ represents the scaling factor of the Z-axis, x t represents the X-axis coordinate of the voxel, R Hor represents the horizontal search radius during voxel neighbor search, R Ver represents the vertical search radius during voxel neighborhood search; Calculate the three-dimensional coordinates of a single voxel based on the scaling factor and the voxel step size of the space segmentation: x t = endX - voxelSize * indX y t = scaleY + (endY - voxelSize * indY) z t = scaleZ * (endZ - voxelSize * indZ) where y t represents the Y-axis coordinate of the voxel, z t represents the Z-axis coordinate of the voxel, voxelSize represents the voxel resolution, indX represents the X-axis voxel index number, indY represents the Y-axis voxel index number, and indZ represents the Z-axis voxel index number; For each voxel whose three-dimensional coordinates are determined, according to the search radius R during voxel neighborhood search Hor and R Ver , find the matching relationship between each voxel and the path. When the path points are included within the voxel range, the path to which the path points belong is defined as the occluded path; Record the number of the voxel point, the number of paths blocked by the voxel point, and the blocked path number in a text file to generate the collision index relationship table.

8. The real-time three-dimensional obstacle avoidance system for unmanned aerial vehicles based on a Cubic Spline offline path library according to claim 5, characterized in that, The working process of the optimal path selection module includes: Convert the obstacle point cloud coordinates to the body coordinate system, and quickly screen the collision-free path set by matching the voxel grid and the collision index relationship table; Obtain the pitch angle difference endPitch[i] between the end point of the i-th path in the collision-free path set and the body, and the heading angle difference endYaw[i] with the body; Obtain the pitch angle difference GoalPitch and heading angle difference GoalYaw between the desired target point and the body, and calculate the angle difference between the end point of the path and the desired target point: Yaw Diff = GoalYaw - endYaw[i] Pitch Diff = GoalPitch - endPitch[i] Among them, Yaw Diff represents the yaw angle difference between the end point of the path and the desired target point, and Pitch Diff represents the pitch angle difference between the end point of the path and the desired target point; Select the optimal path based on the angle difference between the end point of the path and the desired target point through the weighted heuristic rule: score=(1 - Pitch Wei *Pitch Diff )*(1 - Yaw Wei *Yaw Diff )*(1 - Center Diff / 60) Among them, score represents the heuristic function, Pitch Wei represents the pitch angle difference weight, Yaw Wei represents the yaw angle difference weight, Center Diff represents the free path center offset weight; Sort the weighted synthesis of the heuristic functions of each path, and select the path with the smallest synthesis as the optimal path.

Images