A method for six-dimensional pose trajectory planning of a vector hexacopter aircraft

Abstract

This invention relates to a method for six-dimensional pose trajectory planning of a vector hexacopter aircraft. The invention belongs to the field of aircraft trajectory planning, specifically involving a method for six-dimensional pose trajectory planning of a vector hexacopter aircraft. The purpose of this invention is to solve the problem of low computational efficiency in existing six-dimensional pose trajectory planning methods for vector hexacopter aircraft. The process is as follows: Discrete point cloud data is constructed using Blender software; a new three-dimensional voxel grid map is obtained based on the discrete point cloud data; a full-field Euclidean symbolic distance field is generated based on the new three-dimensional voxel grid map; the new three-dimensional voxel grid map is processed using an algorithm to obtain the path from the starting point to the target point; the Euclidean distance value corresponding to each voxel on the path is queried in the full-field Euclidean symbolic distance field; the path from the starting point to the target point is divided into safe segments and narrow segments based on the Euclidean distance values; the final path is obtained; the final path is processed to obtain a spatiotemporally optimal trajectory.

Description

Technical Field

[0001] This invention belongs to the field of aircraft trajectory planning, specifically relating to a six-dimensional pose trajectory planning method for a vector hexacopter aircraft. Background Technology

[0002] With the rapid development of drone technology, vector hexacopter drones have gradually become important tools in various application scenarios. Compared with traditional fixed-tilt rotorcraft, vector hexacopter drones allow the aircraft to flexibly change its attitude and propulsion direction in three-dimensional space, possessing better maneuverability and higher flexibility. Therefore, they have significant advantages in aerobatic flight, complex environment traversal, and precision control tasks (such as grasping, pipeline inspection, and narrow environment inspection). However, this advantage also brings great challenges: since the aircraft not only needs to plan its position trajectory in three-dimensional space but also its attitude, its trajectory planning is essentially a "six-dimensional pose planning problem." Compared with traditional three-dimensional path planning that only considers position, six-dimensional pose planning has a higher search space dimension and greater complexity. Especially when the attitude changes significantly, the environment is complex, obstacles are dense, or passages are narrow, the planning difficulty increases rapidly. How to efficiently plan the position and attitude of the aircraft while ensuring its safety remains a technical problem that urgently needs to be solved. Summary of the Invention

[0003] The purpose of this invention is to address the problem of low computational efficiency in the six-dimensional pose trajectory planning of vector hexacopter aircraft under conditions of large attitude changes, complex environment, dense obstacles, or narrow passages, and to propose a six-dimensional pose trajectory planning method for vector hexacopter aircraft.

[0004] The specific process of a six-dimensional pose trajectory planning method for a vector hexacopter aircraft is as follows:

[0005] Step 1: Construct discrete point cloud data; obtain a new 3D voxel raster map based on the discrete point cloud data; generate the Euclidean symbolic distance field for the entire field based on the new 3D voxel raster map;

[0006] Step 2: Process the new 3D voxel grid map to obtain the path from the starting point to the target point;

[0007] Query the Euclidean distance value corresponding to each voxel on the path in the Euclidean symbolic distance field of the entire field;

[0008] The path from the starting point to the target point is divided into safe segments and narrow segments based on the Euclidean distance value;

[0009] Step 3: Expand the start and end points of each narrow segment to obtain each new narrow segment; process each new narrow segment to obtain the path from the start to the end point of each new narrow segment; and obtain the final path based on the paths of all safe segments and all narrow segments.

[0010] Step 4: The final path obtained in Step 3 is processed using a spatiotemporal joint trajectory optimization method based on MINCO parameterization to obtain a spatiotemporally optimal trajectory.

[0011] The beneficial effects of this invention are as follows:

[0012] This invention constructs a complete autonomous navigation framework, including static map building, hierarchical path planning, and backend spatiotemporal joint optimization, to create an autonomous flight system for unmanned aerial vehicles (UAVs) in complex obstacle environments. This system enables UAVs to perform safe and high-speed autonomous flight in known environments. Without relying on manual remote control, the system loads high-precision point cloud maps and combines them with efficient algorithms to achieve accurate representation of environmental obstacles, intelligent passage through narrow spaces, and globally optimal control of the flight trajectory. This significantly improves the operational efficiency and survivability of UAVs in complex scenarios.

[0013] This invention proposes a method based on The algorithm's hierarchical six-dimensional pose trajectory planning method employs low precision. The algorithm performs an initial path search and then performs a hierarchical search. Performing attitude planning significantly reduces the computational complexity of six-dimensional trajectory planning. It only performs detailed attitude planning for dangerous segments, while directly executing along the path for safe segments, avoiding unnecessary calculations and thus greatly improving the computational efficiency of trajectory planning.

[0014] This invention addresses the mission requirements of UAVs flying at high speeds in environments with dense obstacles by designing a MINCO parameterized spatiotemporal joint trajectory optimizer based on ESDF gradients for use in UAV autonomous navigation systems. This optimizer incorporates hierarchical... Attitude planning and Euclidean symbolic distance field gradient information can optimize the spatial shape and time allocation of the flight trajectory while ensuring dynamic feasibility. This allows for precise control of the aircraft to achieve smooth and rapid maneuvering while avoiding obstacles, thus completing a highly safe and dynamically responsive autonomous navigation mission. Attached Figure Description

[0015] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0016] Specific Implementation Method 1: The specific process of a six-dimensional pose trajectory planning method for a vector hexacopter aircraft in this implementation method is as follows:

[0017] The goal of this invention is to provide an efficient and safe six-dimensional pose spatiotemporal trajectory planning system that can achieve efficient navigation and flight control of UAVs by combining position and attitude planning requirements in complex and obstacle-dense environments.

[0018] Step 1: Construct discrete point cloud data; obtain a new 3D voxel raster map based on the discrete point cloud data; generate the Euclidean symbolic distance field for the entire field based on the new 3D voxel raster map;

[0019] Step 2: Process the new 3D voxel grid map obtained in Step 1 to obtain the path from the starting point to the target point;

[0020] In the Euclidean symbolic distance field of the entire field generated in step 1, query the Euclidean distance value corresponding to each voxel on the path;

[0021] The path from the starting point to the target point is divided into safe segments and narrow segments based on the Euclidean distance value;

[0022] Step 3: Expand the start and end points of each narrow segment to obtain each new narrow segment; process each new narrow segment to obtain the path from the start to the end point of each new narrow segment; and obtain the final path based on the paths of all safe segments and all narrow segments (the start and end points of the paths of each safe segment and each narrow segment are known).

[0023] Step 4: The final path obtained in Step 3 is processed using a spatiotemporal joint trajectory optimization method based on MINCO parameterization to obtain a spatiotemporally optimal trajectory.

[0024] Specific Implementation Method Two: This implementation method differs from Specific Implementation Method One in that: in step 1, discrete point cloud data is constructed; a new three-dimensional voxel raster map is obtained based on the discrete point cloud data; and a full-field Euclidean symbolic distance field is generated based on the new three-dimensional voxel raster map. The specific process is as follows:

[0025] Step 11: Construct discrete point cloud data using Blender software; convert the discrete point cloud data into PCD format files;

[0026] Step 12: Construct a 3D voxel grid map based on discrete point cloud data; the specific process is as follows:

[0027] A 3D voxel raster map consists of multiple rasters, each of which is labeled with a raster index, and the raster index corresponds to a voxel.

[0028] For each point in the discrete point cloud data, convert the coordinates of each point into a raster index, and mark the voxel corresponding to the raster index as occupied.

[0029] Step 13: In order to ensure the flight safety of the UAV, considering the fuselage radius, all voxels marked as occupied are subjected to spatial expansion processing to obtain the expanded voxels marked as occupied.

[0030] That is, within a certain range around the occupying voxel, the originally empty voxels are also marked as occupied, thereby constructing configuration space obstacles;

[0031] The expansion size is adjusted according to the drone radius. If the drone radius occupies 1 grid, it expands by 1 grid to the 26 surrounding neighbors centered on each voxel marked as "occupied". If the drone radius occupies 2 grids, it expands by 2 grids to the surrounding neighbors centered on each voxel marked as "occupied".

[0032] Step 14: Mark the three-dimensional voxel grid map constructed in step 12 based on the voxels marked as occupied after dilation, and obtain a new three-dimensional voxel grid map.

[0033] Step 15: The new 3D voxel grid map obtained in Step 14 is processed using the Euclidean Distance Transform (EDT) algorithm to obtain the Euclidean distance value (positive value) from each free voxel to the nearest occupied voxel and the Euclidean distance value (negative value) from each occupied voxel to the nearest free voxel.

[0034] Mark the Euclidean distance (positive value) from each free voxel to the nearest occupied voxel on each free voxel;

[0035] Mark the Euclidean distance (negative value) from each occupied voxel to the nearest free voxel on each occupied voxel;

[0036] Step 16: Generate the Euclidean signed distance field for the entire field based on the Euclidean distance value (positive value) from each idle voxel to the nearest occupied voxel and the Euclidean distance value (negative value) from each occupied voxel to the nearest idle voxel.

[0037] Step 17: For the Euclidean symbol of the entire field, the distance from any point in the field... Query point The Euclidean distance values ​​of the surrounding 26 voxels;

[0038] Using trilinear interpolation technique for the query point The query point is obtained by interpolating the Euclidean distance values ​​of the surrounding 26 voxels. smooth distance value and gradient with respect to position ;

[0039] Represented as:

[0040] ;

[0041] Among them, superscript This indicates the matrix transpose.

[0042] The gradient vector indicates the fastest direction away from the nearest obstacle and is directly used to calculate the collision penalty term in subsequent trajectory optimization.

[0043] This invention implements a method for constructing a Euclidean symbolic distance field. Based on an occupied grid map, an incremental Euclidean distance transformation algorithm is used to generate a distance field containing distance information and gradient information.

[0044] The other steps and parameters are the same as in Specific Implementation Method 1.

[0045] Specific Implementation Method 3: This implementation method differs from Specific Implementation Method 1 or 2 in that: in step 2, the new three-dimensional voxel grid map obtained in step 1 is processed to obtain the path from the starting point to the target point;

[0046] In the Euclidean symbolic distance field of the entire field generated in step 1, query the Euclidean distance value corresponding to each voxel on the path;

[0047] The path from the starting point to the target point is divided into safe segments and narrow segments based on the Euclidean distance value;

[0048] The specific process is as follows:

[0049] Step 21, based on The algorithm performs path search on the new 3D voxel grid map obtained in step 14 to obtain the path from the starting point to the target point;

[0050] Step 22: Query the Euclidean distance value corresponding to each voxel on the path obtained in step 21 in the Euclidean symbolic distance field of the whole field generated in step 16;

[0051] Step 23: Set the safe distance threshold;

[0052] Step 24: If the Euclidean distance value corresponding to a voxel on the path obtained in Step 21 is less than the safe distance threshold, then use the gradient direction obtained in Step 17 (the gradient direction can be adjusted up and down within a range of 30 degrees to improve the success rate of the push) to push the coordinate point of the voxel corresponding to the path point in the direction away from the obstacle; if it can be pushed to the point where the Euclidean distance value corresponding to the voxel is greater than or equal to the distance threshold, then the push is successful, and the path between the pushed path point and the previous path point is called the safe segment; if it cannot be pushed to the point where the Euclidean distance value corresponding to the voxel is greater than or equal to the distance threshold, then the push fails, and the path between the path point before the push and the previous path point is called the narrow segment.

[0053] If the Euclidean distance values ​​of all voxels in a certain segment of the path obtained in step 21 are greater than or equal to the distance threshold, then the corresponding segment is called a safe segment.

[0054] Step 2 considers only whether the drone, as the smallest bounding body (i.e., only its minimum passability), can traverse the obstacle environment, thereby quickly generating a rough spatial path from the starting position to the target position. For To address the issue of paths being too close to obstacles, this invention introduces a path shifting mechanism based on the Euclidean signed distance field (ESDF) gradient across the entire path. For each node on the path, if its distance to an obstacle is too close (less than a safe distance threshold), the node is shifted away from the obstacle using the gradient direction provided by the ESDF until the safe distance requirement is met. This step significantly improves the safety of the initial path and provides higher-quality initial values ​​for backend optimization.

[0055] right The generated path undergoes safety analysis. Based on the distance to obstacles around waypoints, the entire path is automatically divided into safe segments and narrow segments. Safe segments are path sections with open surrounding space and no risk of collision, through which the UAV can traverse in any attitude. Narrow segments are path sections closer to obstacles, requiring attitude adjustments to pass through. This approach significantly reduces the computational load of subsequent high-dimensional attitude planning, requiring only attitude search and trajectory optimization for dangerous segments, while safe segments do not require attitude search and can be directly executed along the optimized position path.

[0056] Other steps and parameters are the same as in specific implementation method one or two.

[0057] Specific Implementation Method Four: This implementation method differs from Specific Implementation Methods One to Three in that: in step 3, the start and end points of each narrow segment are expanded to obtain each new narrow segment; each new narrow segment is processed to obtain the path from the start to the end point of each new narrow segment; and the final path is obtained based on the paths of all safe segments and all narrow segments (the start and end points of the paths of each safe segment are known, and the start and end points of the paths of each narrow segment are known).

[0058] The specific process is as follows:

[0059] Step 31: Expand the start and end points of each narrow segment by extending 1 / 3 of the length of the adjacent safe segment into the narrow segment. Use the 1 / 3 position of the safe segment as the new start and end points of the narrow segment to obtain each new narrow segment.

[0060] Step 32, adopt The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the path of each safe segment are known, and the start and end points of the path of each narrow segment are known).

[0061] The other steps and parameters are the same as those in one of the specific implementation methods one to three.

[0062] Specific Implementation Method Five: This implementation method differs from Specific Implementation Methods One to Four in that: step 32 employs... The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the path of each safe segment are known, and the start and end points of the path of each narrow segment are known).

[0063] The specific process is as follows:

[0064] Step 321: Initialize the first search tree based on the UAV's initial attitude (no tilt attitude) and the index of the starting point of the current narrow segment being processed (the three-dimensional coordinates of the starting point are transformed into the index value of the starting point in one dimension to achieve dimensionality reduction calculation) as the root node;

[0065] Step 322: Initialize the second search tree based on the UAV's final attitude (no tilt attitude) and the index of the current processing narrow segment's endpoint (the three-dimensional coordinates of the starting point are transformed into the one-dimensional index value of the starting point to achieve dimensionality reduction calculation) as the root node;

[0066] Step 323: Set the algorithm start time ;

[0067] Step 324: Collect an index and a random UAV pose in the current processing narrow segment (each new narrow segment obtained in step 31 has no UAV pose and is randomly set) as a new node (the new node is 4-dimensional, the index is 1-dimensional and the UAV pose is 3-dimensional).

[0068] Step 325: Find the node A in the first search tree that is closest to the new node obtained in step 324;

[0069] The distance is calculated as follows:

[0070] The index of a node in the first search tree minus the index of the new node obtained in step 324 equals the index difference;

[0071] The difference between the pose of a node in the first search tree and the pose of the new node obtained in step 324 is equal to the pose difference.

[0072] The distance is obtained by multiplying the index difference by the scaling factor and then adding the attitude difference by the scaling factor.

[0073] Step 326: Calculate the minimum rotation increment between the pose of the nearest node A and the pose of the new node obtained in step 324;

[0074] Set the incremental threshold;

[0075] If the increment is greater than the increment threshold, the increment will be reduced to be equal to the threshold.

[0076] If the increment is less than or equal to the increment threshold, the increment remains unchanged;

[0077] Step 327: Based on the nearest node A found in step 325 and the increment in step 326, obtain a new 4-dimensional node;

[0078] The new 4D node consists of an index (1D) and a pose (3D);

[0079] The index of the new 4D node is taken as the index following the index of the nearest node A found in step 325;

[0080] The pose of the new 4D node is the pose of the nearest node A found in step 325 plus the pose after the increment in step 326.

[0081] Step 328: Set constraints. If the constraints are met, add the new 4D node obtained in step 327 to the first search tree and proceed to step 329; otherwise, check if the connection timed out. If the connection timed out, the connection failed, and the traditional six-dimensional whole-frame approach was used. The algorithm obtains the final path and executes step 4; if the connection does not time out, steps 324 to 328 are repeated until the constraints are met. Then, the new 4-dimensional node obtained in step 327 is added to the first search tree, and step 329 is executed. The specific process is as follows:

[0082] Step 3281: Check whether the nearest node A found in step 325 and the new 4D node obtained in step 327 collide with the obstacle;

[0083] Step 3282: Verify whether the UAV's journey from the nearest node A found in step 325 to the new 4D node obtained in step 327 satisfies the UAV's dynamic constraints; the UAV's dynamic constraints include UAV angular velocity constraints and UAV angular acceleration constraints.

[0084] Step 3283: If both steps 3281 and 3282 are satisfied, add the new 4-dimensional node obtained in step 327 to the first search tree and execute step 329; otherwise, determine whether the connection timeout has occurred (start timing from step 323).

[0085] If the connection times out, the connection fails, and the traditional six-dimensional holistic approach is used. The algorithm processes each new narrow segment obtained in step 31 to obtain a path from the start point to the end point. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the paths of each safe segment and the path of each narrow segment are known). Then, step 4 is executed.

[0086] If the connection does not time out, repeat steps 324 to 328 until steps 3281 and 3282 are satisfied simultaneously. Then, add the new 4-dimensional node obtained in step 327 to the first search tree and execute step 329.

[0087] When repeating steps 324 to 328, the first search tree and the second search tree are replaced.

[0088] Step 329: Find the node B in the second search tree that is closest to the new 4D node obtained in step 327;

[0089] The distance is calculated as follows:

[0090] The index of a node in the second search tree minus the index of the new 4-dimensional node obtained in step 327 equals the index difference;

[0091] The difference between the pose of a node in the second search tree and the pose of the new 4D node obtained in step 327 is equal to the pose difference.

[0092] The distance is obtained by multiplying the index difference by the scaling factor and then adding the attitude difference by the scaling factor.

[0093] Step 3210: Set the distance threshold;

[0094] Step 3211: If the distance between the nearest node B found in step 329 and the new 4D node obtained in step 327 is less than the distance threshold, then attempt to connect node B in the second search tree with the new 4D node obtained in step 327. If the connection is successful, the connection between the first and second search trees is completed, and the path search for the currently processed narrow segment is completed. Step 32 is repeated to complete the path search for all narrow segments, and finally the path from the start to the end of each new narrow segment is obtained. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the path for each safe segment are known, and the start and end points of the path for each narrow segment are known). Step 4 is then executed. If the connection fails, it is determined whether the connection timed out (the timer starts from step 323). If the connection timed out, the connection failed, and the traditional six-dimensional whole body is used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the paths of each safe segment and each narrow segment are known). Step 4 is then executed. If the connection does not time out, steps 324 to 3211 are repeated. When steps 324 to 3211 are repeated, the first search tree and the second search tree are replaced.

[0095] Step 3212: If the distance between the nearest node B found in step 329 and the new 4D node obtained in step 327 is greater than or equal to the distance threshold, determine whether the connection timed out (starting from step 323); if the connection timed out, the connection failed, and the traditional six-dimensional whole body was used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the paths of each safe segment and each narrow segment are known). Step 4 is then executed. If the connection does not time out, steps 324 to 3212 are repeated. When steps 324 to 3212 are repeated, the first search tree and the second search tree are replaced.

[0096] The other steps and parameters are the same as those in one of the specific implementation methods one to four.

[0097] Specific Implementation Method Six: This implementation method differs from Specific Implementation Methods One to Five in that: in step 3211, if the distance between the nearest node B found in step 329 and the new 4D node obtained in step 327 is less than the distance threshold, then node B in the second search tree is attempted to be connected to the new 4D node obtained in step 327; if the connection is successful, the connection between the first and second search trees is completed, the path search for the currently processed narrow segment is completed, step 32 is repeated to complete the path search for all narrow segments, and finally the path from the start to the end of each new narrow segment is obtained. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the path for each safe segment are known, and the start and end points of the path for each narrow segment are known), and step 4 is executed; if the connection fails, it is determined whether the connection timeout has occurred (the timer starts from step 323); if the connection timeout occurs, the connection fails, and the traditional six-dimensional whole body is used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the paths of each safe segment and each narrow segment are known), and step 4 is executed. If the connection does not time out, steps 324 to 3211 are repeated. When repeating steps 324 to 3211, the first search tree and the second search tree are replaced. The specific process is as follows:

[0098] Step 32111: Using the Spherical Linear Interpolation (Slerp) algorithm, calculate the pose transition sequence between the pose of node B in the second search tree and the pose of the new 4D node obtained in step 327.

[0099] Step 32112: Assign a corresponding index to each interpolated attitude in the attitude transition sequence (the index here refers to the order of the interpolated attitude transition sequence), and form a node based on each interpolated attitude and the corresponding index;

[0100] Each attitude and its corresponding index constitute one node in the attitude transition sequence;

[0101] Step 32113: Check whether the path formed by all nodes corresponding to all interpolations in step 32112 collides with the obstacle;

[0102] Step 32114: Verify whether all nodes corresponding to all interpolations in step 32112 satisfy the UAV dynamics constraints; UAV dynamics constraints include UAV angular velocity constraints and UAV angular acceleration constraints.

[0103] Step 32115: Determine whether all nodes corresponding to all interpolations in step 32112 simultaneously satisfy steps 32113 and 32114;

[0104] If so, connect all nodes corresponding to all interpolations in step 32112 into the second search tree to complete the connection between the first and second search trees, complete the path search for the currently processed narrow segment, repeat step 32 to complete the path search for all narrow segments, and finally obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, obtain the final path (the start and end points of the path of each safe segment are known, and the start and end points of the path of each narrow segment are known), and execute step 4.

[0105] If not, in step 32112, all nodes corresponding to all interpolations will not be connected to the second search tree, and the connection will fail.

[0106] Determine if the connection timeout has occurred (start timing from step 323);

[0107] If the connection times out, the connection fails, and the traditional six-dimensional holistic approach is used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained (the start and end points of the path of each safe segment are known, and the start and end points of the path of each narrow segment are known).

[0108] If the connection does not time out, repeat steps 324 to 3211;

[0109] When repeating steps 324 to 3211, the first search tree and the second search tree are replaced.

[0110] The other steps and parameters are the same as those in one of the specific implementation methods one to five.

[0111] Specific Implementation Method Seven: This implementation method differs from Specific Implementation Methods One to Six in that: in step 4, a spatiotemporal joint trajectory optimization method based on MINCO parameterization is used to process the final path obtained in step 3 to obtain a spatiotemporally optimal trajectory.

[0112] The specific process is as follows:

[0113] This invention employs a spatiotemporal joint trajectory optimization method based on MINCO parameterization. Instead of the traditional approach of directly optimizing polynomial coefficients, this invention utilizes MINCO parameterization.

[0114] Step 41, for a segment composed of Spacetime trajectory composed of segment polynomial trajectories , , Represent real numbers, The dimension representing the trajectory. The 6-dimensional representation includes the 3-dimensional attitude of the UAV and the 3-dimensional position of the UAV.

[0115] ;

[0116] in, Represents the first polynomial locus; Represent the second polynomial locus; Indicates the first Segmented polynomial locus; Indicates the first Segmented polynomial locus; Represents a global time variable;

[0117] MINCO parameterization of spatiotemporal trajectory Equivalent land is represented as intermediate waypoints and Time allocation ;

[0118] in, Representing spacetime trajectory of A set of intermediate waypoints; Representing spacetime trajectory The first intermediate waypoint; Representing spacetime trajectory The Middle One intermediate waypoint; Representing spacetime trajectory The Middle One intermediate waypoint; Representing spacetime trajectory of Time allocation; Representing spacetime trajectory The time corresponding to the first paragraph; Representing spacetime trajectory The Middle The time corresponding to the segment; Representing spacetime trajectory The Middle The time corresponding to the segment;

[0119] Step 42: Set the order of MINCO parameterization (Known, set) The optimal solution to the MINCO parameterization problem is derived from part Composed of polynomial loci of order 1, the polynomial locus can be expressed as follows:

[0120] (1)

[0121] in, Indicates the first Segmented polynomial locus; Represents a global time variable; superscript This indicates finding the transpose of a matrix. Indicates the number of trajectory segments; The coefficient matrix is ​​a polynomial (unknown, to be determined later). ; The dimension representing the trajectory; These are basis functions of the polynomial locus. Represents variables, ; Representing spacetime trajectory The Middle The time corresponding to the segment; , Represents the set of real numbers;

[0122] Step 43: In MINCO parameterization, according to intermediate waypoints and Time allocation Construct the constraint equations for the transformation between the MINCO trajectory and the polynomial trajectory; expressed as:

[0123] (2)

[0124] The MINCO trajectory is composed of intermediate waypoints and Time allocation Uniquely certain;

[0125] in, Indicates about Time allocation The banded constraint coefficient matrix (known) is related to the time allocation. A sparse banded block matrix is ​​used to carry the boundary constraints, waypoint constraints and continuity constraints of the trajectory; Indicates about intermediate waypoints and Time allocation The global polynomial coefficient matrix is ​​obtained from equation (1). polynomial coefficient matrix of segment polynomial locus get; Indicates about intermediate waypoints The waypoint constraint target value matrix (known);

[0126] During MINCO trajectory parsing It is a sparse banded matrix. Utilizing the sparse structure of a banded matrix, its banded PLU decomposition can be used to solve equations in O(N) time complexity, thereby... This set of low-dimensional parameters is analytically mapped to polynomial coefficients. This representation method not only significantly reduces the dimensionality of the optimization variables, but also naturally guarantees the continuity of the trajectory at the connection points (continuity of higher-order derivatives such as position, velocity, and acceleration).

[0127] To optimize the MINCO trajectory to meet the needs of various drone missions (such as following and obstacle avoidance), it is necessary to obtain the optimization parameters for various cost terms. The gradient;

[0128] Step 44: Construct the cost function ;

[0129] Step 45: Adjust the cost function Solve the equations and use the constraint equations obtained in step 43 to convert the MINCO trajectory into a polynomial trajectory, thus obtaining a spatiotemporally optimal polynomial trajectory.

[0130] The other steps and parameters are the same as those in any of the specific implementation methods one to six.

[0131] Specific Implementation Method Eight: This implementation method differs from Specific Implementation Methods One through Seven in that: in step 44, the cost function is constructed. Cost function Represented as:

[0132] (3)

[0133] in, The price of full-body obstacle avoidance, As a consequence of angular velocity constraints, The cost of angular acceleration constraints, As a cost of dynamic feasibility; The cost of full-body obstacle avoidance Weighting coefficients; Cost of angular velocity constraints Weighting coefficients; Cost of angular acceleration constraints Weighting coefficients; Cost of dynamic feasibility Weighting coefficients;

[0134] The other steps and parameters are the same as those in any of the specific implementation methods one to seven.

[0135] Specific Implementation Method Nine: This implementation method differs from Specific Implementation Methods One through Eight in that: the whole-body obstacle avoidance cost... Angular velocity constraint cost Angular acceleration constraint cost Dynamic feasibility cost The solution process is as follows:

[0136] 1) Define the rigid body dynamics equations for the UAV; expressed as:

[0137] (4)

[0138] (5)

[0139] in, Represents the resultant torque vector; Indicates the moment of inertia; Indicates angular velocity. express The first derivative; Waypoints posture, express The first derivative, express The second derivative; Indicates the resultant force vector of translational motion; Indicates the quality of the drone. Represents gravitational acceleration; express The second derivative, Representing spacetime trajectory The position in the middle;

[0140] 2) To ensure the differentiability and numerical stability of the optimization problem, this invention employs a smoothing penalty function. ;

[0141] Define a smoothing penalty function The expression is:

[0142] (6)

[0143] in, These are the original variables that need to be smoothed. It is the cutoff threshold for the ReLU smoothing approximation;

[0144] 3) Construct a multi-sphere collision envelope model for the aircraft based on a vector hexacopter;

[0145] The multi-sphere is a 7-sphere body, consisting of one central sphere and six terminal spheres;

[0146] A central sphere encloses the main body of the drone, as well as the flight controller, power battery pack, airborne navigation unit, and mission payload installed on the main body of the drone;

[0147] The six end spheres respectively enclose the rotors and motors at the ends of the six arms, completely covering the maximum shape range of the drone, with no blind spots for collisions;

[0148] Based on smoothing penalty function Calculate the cost of whole-body obstacle avoidance ; indicates as:

[0149] (7)

[0150] in, This represents the obstacle avoidance constraint function. Waypoints Location, Waypoints posture, Indicates the number of trajectory segments. This indicates the number of sampling points for each trajectory segment. Indicates the trajectory segment index. Indicates the index of the sampling point within the segment;

[0151] (8)

[0152] (9)

[0153] in, The first sphere in the multi-sphere collision envelope model of the aircraft represents the first sphere in the multi-sphere collision envelope model. Obstacle avoidance constraints for each sphere; Indicates the trajectory segment index. This indicates the number of spheres in the drone collision model. ; Represents the Euclidean notated distance field function. Indicates the first The position of a sphere in the world coordinate system; It is the rotation matrix corresponding to the attitude. It refers to the fixed position of the center of each spherical collision body of the drone within the machine system; These are the radii of the colliding spheres. It is a safety margin (small positive number, 0.02 meters).

[0154] 4) Based on smoothing penalty function Calculate the cost of angular velocity constraints ; indicates as:

[0155] (10)

[0156] in, Represents the angular velocity constraint function. express The first derivative, Waypoints The posture;

[0157] (11)

[0158] (12)

[0159] in, This represents the angular velocity of the drone's rotation. Represents the angular velocity transformation matrix, Converted to actual angular velocity in the physical world ; This indicates the maximum permissible rotational angular velocity of the drone;

[0160] 5) Based on smoothing penalty function Calculate the cost of angular acceleration constraints ; indicates as:

[0161] (13)

[0162] in, Represents the angular acceleration constraint function. Waypoints posture, express The first derivative, express The second derivative;

[0163] (14)

[0164] (15)

[0165] in, This represents the angular acceleration of the drone's rotation. This indicates the maximum permissible angular acceleration of the drone. Indicates intermediate variables in the operation;

[0166] (16)

[0167] in, Represents a third-order identity matrix. Representing vectors The cross product matrix, superscript This indicates finding the transpose of a matrix.

[0168] 6) Based on smoothing penalty function Calculate dynamic feasibility cost ; indicates as:

[0169] (17)

[0170] in, Represents a dynamic feasibility constraint function;

[0171] (18)

[0172] in, To maximize the permissible resultant translational force, This is the maximum permissible torque; Indicates the resultant force vector of translational motion; Represents the resultant torque vector; It is a norm.

[0173] The other steps and parameters are the same as those in one of the specific implementation methods one to eight.

[0174] Specific Implementation Method Ten: This implementation method differs from Specific Implementation Methods One through Nine in that: in step 45, the cost function... The solution process involves using the constraint equations obtained in step 43 to transform the MINCO trajectory into a polynomial trajectory, resulting in a spatiotemporally optimal polynomial trajectory. The specific process is as follows:

[0175] Existing methods use automatic differentiation to iteratively solve the cost function with respect to parameters. Even after applying the first derivative, the parameters parameterized by MINCO still cannot be directly applied. Optimize the solution using a commonly used Euclidean space solver, because the variables of the current parameterization method... It is not a variable in Euclidean space; it is bounded by the time manifold. Waypoints Since obstacle avoidance is also subject to constraints, this invention will include intermediate waypoints. Obstacle avoidance constraints soften into full-body obstacle avoidance costs (Formula 7, 8, 9);

[0176] The cost function is mapped using the relationship shown in formula (19). Constrained time allocation Transform into unconstrained variables The transformed cost function is obtained. ;

[0177] Based on the transformed cost function and the transformed cost function The first derivative is minimized using the L-BFGS (Limited-memory Broyden–Fletcher–Goldfarb–Shanno) solver. Solve the nonlinear unconstrained optimization problem with the objective as the goal;

[0178] Solving for the optimal variables and Then, the optimal time allocation is obtained by inverse mapping of equation (19). Optimal time allocation Combine intermediate waypoints Generate the spatiotemporally optimal MINCO trajectory under the corresponding cost function; use the constraint equations obtained in step 43 to convert the MINCO trajectory into a polynomial trajectory, thus obtaining a spatiotemporally optimal polynomial trajectory.

[0179] (19).

[0180] The other steps and parameters are the same as those in any of the specific implementation methods one to nine.

[0181] This invention may have other embodiments. Without departing from the spirit and essence of this invention, those skilled in the art can make various corresponding changes and modifications according to this invention, but these corresponding changes and modifications should all fall within the protection scope of the appended claims.

Claims

1. A method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft, characterized in that: The specific process of the method is as follows: Step 1: Construct discrete point cloud data; obtain a new 3D voxel raster map based on the discrete point cloud data; generate the Euclidean symbolic distance field for the entire field based on the new 3D voxel raster map; Step 2: Process the new 3D voxel grid map to obtain the path from the starting point to the target point; Query the Euclidean distance value corresponding to each voxel on the path in the Euclidean symbolic distance field of the entire field; The path from the starting point to the target point is divided into safe segments and narrow segments based on the Euclidean distance value; Step 3: Expand the start and end points of each narrow segment to obtain each new narrow segment; process each new narrow segment to obtain the path from the start to the end point of each new narrow segment; and obtain the final path based on the paths of all safe segments and all narrow segments. Step 4: The final path obtained in Step 3 is processed using a spatiotemporal joint trajectory optimization method based on MINCO parameterization to obtain a spatiotemporally optimal trajectory.

2. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 1, characterized in that: The specific process of step 1 is as follows: Step 11: Construct discrete point cloud data using Blender software; convert the discrete point cloud data into PCD format files; Step 12: Construct a 3D voxel raster map based on discrete point cloud data; the specific process is as follows: A 3D voxel raster map consists of multiple rasters, each of which is labeled with a raster index, and the raster index corresponds to a voxel. For each point in the discrete point cloud data, convert the coordinates of each point into a raster index, and mark the voxel corresponding to the raster index as occupied. Step 13: Perform spatial dilation on all voxels marked as occupied to obtain dilated voxels marked as occupied. Step 14: Mark the three-dimensional voxel grid map constructed in step 12 based on the voxels marked as occupied after dilation, and obtain a new three-dimensional voxel grid map. Step 15: Process the new 3D voxel grid map obtained in Step 14 using the Euclidean distance transformation algorithm to obtain the Euclidean distance value from each free voxel to the nearest occupied voxel and the Euclidean distance value from each occupied voxel to the nearest free voxel. Mark the Euclidean distance value from each free voxel to the nearest occupied voxel on each free voxel; Mark the Euclidean distance value from each occupied voxel to the nearest free voxel on each occupied voxel; Step 16: Generate the Euclidean signed distance field for the entire field based on the Euclidean distance from each idle voxel to the nearest occupied voxel and the Euclidean distance from each occupied voxel to the nearest idle voxel. Step 17: For the Euclidean symbol of the entire field, the distance from any point in the field... Query point The Euclidean distance values ​​of the surrounding 26 voxels; Using trilinear interpolation technique for the query point The query point is obtained by interpolating the Euclidean distance values ​​of the surrounding 26 voxels. Smooth distance value and gradient with respect to position ; Represented as: ； Among them, superscript This indicates the matrix transpose.

3. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 2, characterized in that: The specific process of step 2 is as follows: Step 21, based on The algorithm performs path search on the new 3D voxel grid map to obtain the path from the starting point to the target point; Step 22: Query the Euclidean distance value corresponding to each voxel on the path obtained in step 21 in the Euclidean symbolic distance field of the whole field generated in step 16; Step 23: Set the safe distance threshold; Step 24: If the Euclidean distance value corresponding to a voxel on the path obtained in Step 21 is less than the safe distance threshold, then use the gradient direction obtained in Step 17 to push the coordinate point of the voxel corresponding to the path point on the path away from the obstacle. If the path can be moved to a point where the Euclidean distance value corresponding to the voxel is greater than or equal to the distance threshold, the move is successful. The path between the moved path point and the previous path point is called the safe segment. If the path cannot be moved to a point where the Euclidean distance value corresponding to the voxel is greater than or equal to the distance threshold, the move fails, and the path between the path point before the move and the previous path point is called a narrow segment. If the Euclidean distance values ​​of all voxels in a certain segment of the path obtained in step 21 are greater than or equal to the distance threshold, then the corresponding segment is called a safe segment.

4. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 3, characterized in that: The specific process of step 3 is as follows: Step 31: Expand the start and end points of each narrow segment by extending 1 / 3 of the length of the adjacent safe segment into the narrow segment. Use the 1 / 3 position of the safe segment as the new start and end points of the narrow segment to obtain each new narrow segment. Step 32, adopt The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start point to the end point of each new narrow segment, and obtains the final path based on the paths of all safe segments and all narrow segments.

5. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 4, characterized in that: The specific process of step 32 is as follows: Step 321: Initialize the first search tree based on the root node, using the index of the UAV's initial attitude and the starting point of the current narrow segment being processed. Step 322: Initialize the second search tree based on the root node, using the UAV's final attitude and the index of the current processing narrow segment's final attitude. Step 323: Set the algorithm start time ; Step 324: Collect an index and a random UAV attitude in the current narrow processing segment as a new node; Step 325: Find the node A in the first search tree that is closest to the new node obtained in step 324; The distance is calculated as follows: The index of a node in the first search tree minus the index of the new node obtained in step 324 equals the index difference; The difference between the pose of a node in the first search tree and the pose of the new node obtained in step 324 is equal to the pose difference. The distance is obtained by multiplying the index difference by the scaling factor and then adding the attitude difference by the scaling factor. Step 326: Calculate the minimum rotation increment between the pose of the nearest node A and the pose of the new node obtained in step 324; Set the incremental threshold; If the increment is greater than the increment threshold, the increment will be reduced to be equal to the threshold. If the increment is less than or equal to the increment threshold, the increment remains unchanged; Step 327: Based on the nearest node A found in step 325 and the increment in step 326, obtain a new 4-dimensional node; The new 4D node consists of an index and a pose; The index of the new 4D node is taken as the index following the index of the nearest node A found in step 325; The pose of the new 4D node is the pose of the nearest node A found in step 325 plus the pose after the increment in step 326. Step 328: Set constraints. If the constraints are met, add the new 4D node obtained in step 327 to the first search tree and proceed to step 329; otherwise, check if the connection timed out. If the connection timed out, the connection failed, and the traditional six-dimensional whole-frame approach was used. The algorithm obtains the final path and executes step 4; if the connection does not time out, steps 324 to 328 are repeated until the constraints are met. Then, the new 4-dimensional node obtained in step 327 is added to the first search tree, and step 329 is executed. The specific process is as follows: Step 3281: Check whether the nearest node A found in step 325 and the new 4D node obtained in step 327 collide with the obstacle; Step 3282: Verify whether the UAV's journey from the nearest node A found in step 325 to the new 4D node obtained in step 327 satisfies the UAV's dynamic constraints; the UAV's dynamic constraints include UAV angular velocity constraints and UAV angular acceleration constraints. Step 3283: If both steps 3281 and 3282 are satisfied, add the new 4-dimensional node obtained in step 327 to the first search tree and execute step 329; otherwise, determine whether the connection timed out. If the connection times out, the connection fails, and the traditional six-dimensional holistic approach is used. The algorithm processes each new narrow segment obtained in step 31 to obtain a path from the start point to the end point. Based on the paths of all safe segments and all narrow segments, the final path is obtained, and step 4 is executed. If the connection does not time out, repeat steps 324 to 328 until steps 3281 and 3282 are satisfied at the same time. Then add the new 4-dimensional node obtained in step 327 to the first search tree and execute step 329. When repeating steps 324 to 328, the first search tree and the second search tree are replaced. Step 329: Find the node B in the second search tree that is closest to the new 4D node obtained in step 327; The distance is calculated as follows: The index of a node in the second search tree minus the index of the new 4-dimensional node obtained in step 327 equals the index difference; The difference between the pose of a node in the second search tree and the pose of the new 4D node obtained in step 327 is equal to the pose difference. The distance is obtained by multiplying the index difference by the scaling factor and then adding the attitude difference by the scaling factor. Step 3210: Set the distance threshold; Step 3211: If the distance between the nearest node B found in step 329 and the new 4D node obtained in step 327 is less than the distance threshold, then attempt to connect node B in the second search tree with the new 4D node obtained in step 327. If the connection is successful, the connection between the first and second search trees is completed, and the path search for the currently processed narrow segment is completed. Step 32 is repeated to complete the path search for all narrow segments, and finally the path from the start point to the end point of each new narrow segment is obtained. Based on the paths of all safe segments and all narrow segments, the final path is obtained, and step 4 is executed. If the connection fails, it is determined whether the connection timed out. If the connection timed out, the connection failed, and the traditional six-dimensional whole body is used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start point to the end point of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained, and step 4 is executed. If the connection does not time out, steps 324 to 3211 are repeated. When steps 324 to 3211 are repeated, the first search tree and the second search tree are replaced. Step 3212: If the distance between the nearest node B found in step 329 and the new 4D node obtained in step 327 is greater than or equal to the distance threshold, determine whether the connection timed out; if the connection timed out, the connection failed, and the traditional six-dimensional whole body was used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start point to the end point of each new narrow segment. Based on the paths of all safe segments and all narrow segments, the final path is obtained, and step 4 is executed. If the connection does not time out, steps 324 to 3212 are repeated. When steps 324 to 3212 are repeated, the first search tree and the second search tree are replaced.

6. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 5, characterized in that: The specific process of step 3211 is as follows: Step 32111: Using the spherical linear interpolation algorithm, calculate the pose transition sequence between the pose of node B in the second search tree and the pose of the new 4D node obtained in step 327. Step 32112: Assign a corresponding index to the attitude of each interpolated position in the attitude transition sequence, and form a node based on the attitude of each interpolated position and the corresponding index of the attitude; Each attitude and its corresponding index constitute one node in the attitude transition sequence; Step 32113: Check whether the path formed by all nodes corresponding to all interpolations in step 32112 collides with the obstacle; Step 32114: Verify whether all nodes corresponding to all interpolations in step 32112 satisfy the UAV dynamics constraints; UAV dynamics constraints include UAV angular velocity constraints and UAV angular acceleration constraints. Step 32115: Determine whether all nodes corresponding to all interpolations in step 32112 simultaneously satisfy steps 32113 and 32114; If so, connect all nodes corresponding to all interpolations in step 32112 into the second search tree to complete the connection between the first and second search trees, complete the path search for the currently processed narrow segment, repeat step 32 to complete the path search for all narrow segments, and finally obtain the path from the start point to the end point of each new narrow segment. Based on the paths of all safe segments and the paths of all narrow segments, obtain the final path and execute step 4. If not, in step 32112, all nodes corresponding to all interpolations will not be connected to the second search tree, and the connection will fail. Determine if the connection has timed out; If the connection times out, the connection fails, and the traditional six-dimensional holistic approach is used. The algorithm processes each new narrow segment obtained in step 31 to obtain the path from the start to the end of each new narrow segment, and obtains the final path based on the paths of all safe segments and all narrow segments. If the connection does not time out, repeat steps 324 to 3211; When repeating steps 324 to 3211, the first search tree and the second search tree are replaced.

7. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 6, characterized in that: The specific process of step 4 is as follows: Step 41, for a segment composed of Spacetime trajectory composed of segment polynomial trajectories ; ; in, Represents the first polynomial locus; Represent the second polynomial locus; Indicates the first Segmented polynomial locus; Indicates the first Segmented polynomial locus; Represents a global time variable; MINCO parameterization of spatiotemporal trajectory Equivalent land is represented as intermediate waypoints and Time allocation ; in, Representing spacetime trajectory of A set of intermediate waypoints; Representing spacetime trajectory The first intermediate waypoint; Representing spacetime trajectory The Middle One intermediate waypoint; Representing spacetime trajectory The Middle One intermediate waypoint; Representing spacetime trajectory of Time allocation; Representing spacetime trajectory The time corresponding to the first paragraph; Representing spacetime trajectory The Middle The time corresponding to the segment; Representing spacetime trajectory The Middle The time corresponding to the segment; Step 42: Set the order of MINCO parameterization , The optimal solution to the MINCO parameterization problem is derived from part Composed of polynomial loci of order 1, the polynomial locus can be expressed as follows: (1) in, Indicates the first Segmented polynomial locus; Represents a global time variable; superscript This indicates finding the transpose of a matrix. Indicates the number of trajectory segments; It is a polynomial coefficient matrix; These are basis functions of the polynomial locus. Represents variables, ; Representing spacetime trajectory The Middle The time corresponding to the segment; Step 43: In MINCO parameterization, according to intermediate waypoints and Time allocation Construct the constraint equations for the transformation between the MINCO trajectory and the polynomial trajectory; expressed as: (2) The MINCO trajectory is composed of intermediate waypoints and Time allocation Uniquely certain; in, Indicates about Time allocation The band constraint coefficient matrix; Indicates about intermediate waypoints and Time allocation The global polynomial coefficient matrix is ​​obtained from equation (1). polynomial coefficient matrix of segment polynomial locus get; Indicates about intermediate waypoints The waypoint constraint target value matrix; Step 44: Construct the cost function ; Step 45: Adjust the cost function Solve the equations and use the constraint equations obtained in step 43 to convert the MINCO trajectory into a polynomial trajectory, thus obtaining a spatiotemporally optimal polynomial trajectory.

8. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 7, characterized in that: Step 44 cost function Represented as: (3) in, The price of full-body obstacle avoidance, As a consequence of angular velocity constraints, The cost of angular acceleration constraints, As a cost of dynamic feasibility; The cost of full-body obstacle avoidance Weighting coefficients; Cost of angular velocity constraints Weighting coefficients; Cost of angular acceleration constraints Weighting coefficients; Cost of dynamic feasibility The weighting coefficients.

9. The method for planning the six-dimensional pose trajectory of a vector hexacopter aircraft according to claim 8, characterized in that: The cost of whole-body obstacle avoidance Angular velocity constraint cost Angular acceleration constraint cost Dynamic feasibility cost The solution process is as follows: 1) Define the rigid body dynamics equations for the UAV; expressed as: (4) (5) in, Represents the resultant torque vector; Indicates the moment of inertia; Indicates angular velocity, express The first derivative; Waypoints posture, express The first derivative, express The second derivative; Represents the resultant force vector in translation; Indicates the quality of the drone. Represents gravitational acceleration; express The second derivative, Representing spacetime trajectory The position in the middle; 2) Define the smoothing penalty function The expression is: (6) in, These are the original variables that need to be smoothed. It is the cutoff threshold for the ReLU smoothing approximation; 3) Construct a multi-sphere collision envelope model for the aircraft based on a vector hexacopter; The multi-sphere is a 7-sphere body, consisting of one central sphere and six terminal spheres; A central sphere encloses the main body of the drone, as well as the flight controller, power battery pack, airborne navigation unit, and mission payload installed on the main body of the drone; The six end spheres respectively enclose the rotors and motors at the ends of the six arms; Based on smoothing penalty function Calculate the cost of whole-body obstacle avoidance ; indicates as: (7) in, This represents the obstacle avoidance constraint function. Waypoints Location, Waypoints posture, Indicates the number of trajectory segments. This indicates the number of sampling points for each trajectory segment. Indicates the trajectory segment index. Indicates the index of the sampling point within the segment; (8) (9) in, The first sphere in the multi-sphere collision envelope model of the aircraft represents the first sphere in the multi-sphere collision envelope model. Obstacle avoidance constraints for each sphere; Indicates the trajectory segment index. This indicates the number of spheres in the drone collision model. ; Represents the Euclidean notated distance field function. Indicates the first The position of a sphere in the world coordinate system; It is the rotation matrix corresponding to the attitude. It refers to the fixed position of the center of each spherical collision body of the drone within the machine system; These are the radii of the colliding spheres. It is a safety margin; 4) Based on smoothing penalty function Calculate the cost of angular velocity constraints ; indicates as: (10) in, Represents the angular velocity constraint function. express The first derivative, Waypoints The posture; (11) (12) in, This represents the angular velocity of the drone's rotation. Represents the angular velocity transformation matrix, Converted to actual angular velocity in the physical world ; This indicates the maximum permissible rotational angular velocity of the drone; 5) Based on smoothing penalty function Calculate the cost of angular acceleration constraints ; indicates as: (13) in, Represents the angular acceleration constraint function. Waypoints posture, express The first derivative, express The second derivative; (14) (15) in, This represents the angular acceleration of the drone's rotation. This indicates the maximum permissible angular acceleration of the drone. Indicates intermediate variables in the operation; (16) in, Represents a third-order identity matrix. Representing vectors The cross product matrix, superscript This indicates finding the transpose of a matrix. 6) Based on smoothing penalty function Calculate dynamic feasibility cost ; indicates as: (17) in, Represents a dynamic feasibility constraint function; (18) in, To maximize the permissible resultant translational force, This is the maximum permissible torque; Represents the resultant force vector in translation; Represents the resultant torque vector; It is a norm.

10. A six-dimensional pose trajectory planning method for a vector hexacopter aircraft according to claim 9, characterized in that: The specific process of step 45 is as follows: The cost function is mapped using the relationship shown in formula (19). Constrained time allocation Transform into unconstrained variables The transformed cost function is obtained. ; Based on the transformed cost function and the transformed cost function The first derivative is minimized using the L-BFGS solver. Solve the nonlinear unconstrained optimization problem with the objective as the goal; Solving for the optimal variables and Then, the optimal time allocation is obtained by inverse mapping of equation (19). Optimal time allocation Combine intermediate waypoints Generate the spatiotemporally optimal MINCO trajectory under the corresponding cost function; use the constraint equations obtained in step 43 to convert the MINCO trajectory into a polynomial trajectory, thus obtaining a spatiotemporally optimal polynomial trajectory. (19)。

Images