Indoor unmanned aerial vehicle path planning method

Abstract

The invention discloses an indoor unmanned aerial vehicle path planning method, and relates to the technical field of unmanned aerial vehicle path planning, and the method comprises the steps: constructing an indoor Euclidean symbol distance site map of an unmanned aerial vehicle, and generating an initial collision-free discrete path point sequence; constructing a safe flight corridor based on the discrete path point sequence, and generating a control point grid; in combination with a q-calculus theory, calculating a curve tension coefficient q based on a control point grid, and generating a three-dimensional space position path trajectory by using a tensor product q-Bezier curved surface equation; the geometric curvature of the three-dimensional space position path trajectory is calculated, shape parameters are calculated, and a continuous attitude path is generated according to the shape parameters by using a parametric quartic unit quaternion spline interpolation formula; and generating a complete flight path control instruction sequence. According to the method, in a complex indoor environment, a flight path which is highly safe, free of collision, extremely stable, free of oscillation and free of sudden change of angular acceleration is generated, and the flight safety and control quality of the unmanned aerial vehicle are remarkably improved.

Description

Technical Field

[0001] This invention relates to the technical field of unmanned aerial vehicle (UAV) path planning, and more particularly to an indoor UAV path planning method. Background Technology

[0002] With the development of drone technology, its application in indoor environments is becoming increasingly widespread, such as warehousing and logistics, indoor inspection, and emergency rescue. Indoor environments are typically characterized by limited space, dense obstacles, and unstructured environments, which places extremely high demands on drone path planning. Existing indoor drone path planning methods generally consist of two stages: front-end path search and back-end trajectory optimization. The path generated by front-end search (such as A or JPS) is usually a broken line composed of discrete points, lacking smoothness and unsuitable for direct flight control. Back-end optimization typically uses methods such as B-splines and polynomial curves to smooth the path. However, when approaching complex obstacles, traditional B-spline curves often require dense control points to meet obstacle avoidance constraints, resulting in drastic changes in the curvature of the generated path (i.e., excessive jerk). This geometric discontinuity or drastic change is directly transmitted to the flight control system, causing motor response lag or overshoot, triggering fuselage vibration, and affecting flight stability and video acquisition quality. Furthermore, existing path optimization methods often neglect the coupling relationship between attitude and position. At sharp turns, if the attitude adjustment (yaw angle change) does not match the position change, additional abrupt changes in angular acceleration will occur, further exacerbating oscillations. Traditional attitude interpolation methods (such as Slerp) cannot flexibly adjust the rate and smoothness of attitude changes.

[0003] Currently, Chinese invention patent application number CN202410710105.X discloses an indoor flight control method, electronic device, and storage medium for unmanned aerial vehicles (UAVs). The method includes: constructing a three-dimensional spatial model of the indoor space corresponding to the UAV based on the UAV's lidar and visual front-end; wherein the three-dimensional spatial model includes an octree map; planning a target flight path for the UAV based on the three-dimensional spatial model, the UAV's initial position, and the target position using a preset algorithm; wherein the preset algorithm includes a flight path planning algorithm and a flight path optimization algorithm; and controlling the UAV to perform a flight mission in the indoor space according to the target flight path. This invention solves the technical problem of inaccurate positioning of UAVs when used indoors, achieving precise positioning and navigation for UAVs indoors and avoiding drifting due to inaccurate positioning, which could pose safety hazards. However, existing indoor UAV path planning methods are prone to abrupt changes in curvature and angular acceleration in complex environments, leading to flight oscillations. Furthermore, traditional optimization methods involve large computational loads and struggle to balance obstacle avoidance safety with smooth flight. Summary of the Invention

[0004] The technical problem solved by this invention is that the paths generated by existing indoor UAV path planning methods are prone to sudden changes in curvature and angular acceleration in complex environments, leading to flight oscillations. Furthermore, traditional optimization methods involve large amounts of computation and are difficult to balance obstacle avoidance safety and smooth flight.

[0005] To solve the above-mentioned technical problems, the present invention provides the following technical solution: an indoor unmanned aerial vehicle (UAV) path planning method, comprising the following steps:

[0006] Step S1: Calculate and construct an indoor Euclidean distance field map of the UAV in real time. Based on the indoor Euclidean distance field map, generate an initial collision-free discrete path point sequence according to the start and end points of the UAV's flight path.

[0007] Step S2: Construct a safe flight corridor based on the discrete path point sequence, and extract the coordinates of the feature points and cross-section points of the safe corridor to generate a control point grid;

[0008] Step S3: Combining q-calculus theory, calculate the curve tension coefficient q based on the control point grid and the minimum obstacle distance, and use the tensor product q-Bézier surface equation to generate the three-dimensional spatial position path trajectory;

[0009] Step S4: Construct a reference attitude frame sequence, calculate the geometric curvature of the three-dimensional spatial position path trajectory based on the reference attitude frame sequence, calculate the shape parameters based on the geometric curvature, and generate a continuous attitude path based on the shape parameters using the parametric quartic unit quaternion spline interpolation formula.

[0010] Step S5: Parameterize the arc length of the three-dimensional spatial position path trajectory and attitude path to generate a complete flight trajectory control command sequence.

[0011] Preferably, step S1 includes:

[0012] The UAV constructs an indoor raw point cloud map of the indoor environment in real time using an airborne lidar and depth camera. After rasterizing the indoor raw point cloud map, an indoor Euclidean symbolic distance field map of the UAV is obtained. The start and end points of the UAV's flight path are determined according to the received mission instructions. Based on the start and end points, an initial discrete path point sequence is generated in the indoor Euclidean symbolic distance field map using a path search algorithm.

[0013] Preferably, step S2 includes:

[0014] Building a safe flight corridor includes:

[0015] Using each path point in the discrete path point sequence and the midpoint between adjacent path points as the center, an expansion detection is performed in an indoor Euclidean symbolic distance field map to generate a set of mutually overlapping maximum inscribed collision-free cubes. The set of maximum inscribed collision-free cubes is the safe flight corridor.

[0016] Expansion detection includes:

[0017] The initial center is an initial micro cube, and each side of the initial micro cube is a minimum value. The size of the initial micro cube is uniformly expanded along the dimensional direction in the indoor Euclidean symbolic distance field map until any face of the initial micro cube touches an obstacle in the indoor Euclidean symbolic distance field map, thus obtaining the largest inscribed collision-free cube.

[0018] Generating the control point mesh includes:

[0019] The vertex coordinates of each cube in the set of largest inscribed collision-free cubes and the feature point coordinates of the overlapping area between adjacent cubes are extracted. The feature points include center points and corner points. M cross-sectional points are selected on the cross-section of the safe flight corridor using a resampling method. All vertex coordinates, feature point coordinates of the overlapping area, and cross-sectional point coordinates are connected in path order to form a point matrix that is ordered along both the path direction and the cross-sectional direction of the discrete path point sequence. The point matrix is ​​defined as a control point grid. Each control point in the control point grid includes an index parameter along the path direction and an index parameter along the cross-sectional direction. The dimension of the control point grid is MN, where M is the number of control points on the cross-section and N is the number of cross-sectional layers along the path direction.

[0020] Preferably, step S3 includes:

[0021] Step S31: Combining q-calculus theory, calculate the minimum obstacle distance around each path segment in the discrete path point sequence, and construct an adaptive q-value mapping function based on the minimum obstacle distance;

[0022] Step S32: Based on the control point grid and the curve tension coefficient q calculated by the adaptive q-value mapping function, construct the tensor product q-Bézier surface equation, and generate a smooth three-dimensional spatial position path trajectory based on the tensor product q-Bézier surface equation.

[0023] Preferably, step S31 specifically includes:

[0024] initialization The defined q-Bernstein basis functions are used as the basis functions for UAV path generation. The mathematical expression of the basis functions is as follows:

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] in, for Second-rate Defined q-Bernstein basis functions, For normalized parameters and , The index of the pre-defined control point number in the control point grid and , The curve tension coefficient and >0, The coefficients are q-binomial coefficients. It is a natural number;

[0030] The minimum obstacle distances around each control point are obtained by querying an indoor Euclidean symbolic distance field map. An adaptive q-value mapping function is constructed based on these minimum obstacle distances. The mathematical expression of the adaptive q-value mapping function is as follows:

[0031] ;

[0032] in, The curve tension coefficient, This is the preset minimum curve tension coefficient. The preset safe distance threshold, This represents the minimum obstacle distance.

[0033] Preferably, step S32 specifically includes:

[0034] The mathematical expression for the equation of the tensor product q-Bézier surface is:

[0035] ;

[0036] in, The equation for the tensor product q-Bézier surface is given. Let m be the positional parameter on the cross-section of the safe flight corridor. The order of the q-Bernstein basis functions along the cross-sectional direction is defined as m = M⁻¹, and w is... The order of the defined q-Bernstein basis functions along the path direction, w=N-1, For control points of the control point grid, For the index parameter along the path direction, For index parameters along the cross-sectional direction, To use in the path direction Defined q-Bernstein basis functions, Standards adopted in the cross-sectional direction Defined q-Bernstein basis functions;

[0037] Generating smooth 3D spatial path trajectories based on the tensor product q-Bézier surface equation specifically includes:

[0038] Substituting v=0.5 into the tensor product q-Bézier surface equation, the three-dimensional spatial position path trajectory is obtained. The mathematical expression of the three-dimensional spatial position path trajectory is:

[0039] .

[0040] Preferably, step S4 includes:

[0041] Calculate the first derivative of the three-dimensional spatial position path trajectory, and construct a reference attitude frame sequence based on the Rotation Minimum Frame (RMF) algorithm. The construction process includes:

[0042] The first derivative is normalized to obtain the unit tangent vector, which is then used as the forward axis of the UAV.

[0043] The lateral axis of the UAV is obtained through the Rotation Minimum Frame (RMF) algorithm, specifically including:

[0044] At the starting point of the discrete path point sequence, a unit lateral vector is initialized that is perpendicular to the vertical axis of the indoor Euclidean symbolic distance field map and orthogonal to the forward axis. The unit lateral vector is propagated along the path of the discrete path point sequence using the parallel propagation method. During the propagation process, the unit lateral vector is kept orthogonal to the forward axis. The direction of all unit lateral vectors during the propagation process is the dynamic lateral axis of the UAV on the discrete path point sequence.

[0045] The upward axis is the direction of the cross product vector of the lateral axis direction vector and the forward axis direction vector;

[0046] The forward axis, lateral axis, and upward axis are combined according to their corresponding path points and arranged along the path direction to obtain a successfully constructed reference attitude frame sequence.

[0047] The reference attitude frames in the reference attitude frame sequence are concatenated into axis unit vectors to obtain a rotation matrix. The unit quaternion corresponding to each rotation matrix is ​​calculated by matrix basic operations. The discrete key attitude quaternion sequence is obtained by uniformly sampling along the path.

[0048] Preferably, the geometric curvature of the three-dimensional spatial path trajectory is calculated, and an adaptive shape parameter mapping function is constructed based on the geometric curvature. The construction process includes:

[0049] Calculate the geometric curvature of the three-dimensional spatial position path trajectory at each point corresponding to the key attitude quaternion sequence, and establish an adaptive shape parameter mapping function based on the geometric curvature. The mathematical expression of the adaptive shape parameter mapping function is as follows:

[0050] ;

[0051] in, The preset reference shape parameters, The preset adjustment coefficient, For mapping functions, Let S(t) be the geometric curvature of the three-dimensional spatial path trajectory S(t) at parameter t;

[0052] Using the key attitude quaternion sequence and the calculated shape parameters, a continuous attitude path is generated based on the parametric quartic unit quaternion spline interpolation formula. The mathematical expression of the parametric quartic unit quaternion spline interpolation formula is as follows:

[0053] ;

[0054] ;

[0055] in, This is the interpolation formula for a fourth-order unit quaternion spline with parameters. These are interpolation parameters. This represents the index number of the key pose quaternion sequence. It is a quaternion exponential mapping function. for arrive The relative angular velocity vector, For those with shape parameters The quartic polynomial basis functions, It is the z-th element of the key pose quaternion sequence.

[0056] Preferably, step S5 includes:

[0057] The continuous three-dimensional spatial location path trajectory is... The step size is discretized to obtain a high-density path point sequence. The Euclidean distance between adjacent path points in the high-density path point sequence is calculated and accumulated to obtain the physical arc length corresponding to each path point in the high-density path point sequence. The mathematical expression for the physical arc length is:

[0058] ;

[0059] in, path point The physical arc length, For natural numbers, The Euclidean distance between adjacent path points;

[0060] Calculate the total physical length of the three-dimensional spatial trajectory of the UAV flight path. The total physical length is... ;

[0061] The normalized parameters and physical arc length are established using an interpolation function. The mapping relationship between them;

[0062] when > At the same time, a trapezoidal velocity profile is constructed, including an acceleration segment, a constant velocity segment, and a deceleration segment;

[0063] when At this point, a triangular velocity profile containing only acceleration and deceleration segments is constructed, and the peak velocity reached is... ;

[0064] in, This is the maximum flight acceleration of the drone. This is the maximum flight speed of the drone;

[0065] The displacement-time function of the UAV is output based on the trapezoidal velocity profile and the triangular velocity profile.

[0066] Preferably, the time step of the control command is set, and the time points of each integer multiple of the time step are traversed. The displacement at the current time point is calculated according to the displacement-time function, and the displacement is calculated based on the normalized parameters and the physical arc length. The mapping relationship between them is obtained, the normalized parameter corresponding to the displacement at the current time point is obtained as the physical arc length, the corresponding normalized parameter is input into the mathematical expression of the three-dimensional spatial position path trajectory, the target attitude quaternion is obtained, and the target velocity and target acceleration corresponding to the current time point are calculated according to the trapezoidal velocity profile and the triangular velocity profile.

[0067] The timestamp, target position, target velocity, target acceleration, and target attitude are pieced together to form a complete sequence of flight trajectory control commands;

[0068] The complete flight trajectory control command sequence is sent to the UAV's flight control system in real time. After receiving the commands, the PID controller inside the flight control system calculates the required rotational speed of each rotor and drives the UAV to fly along the planned path.

[0069] The beneficial effects of this invention are as follows: This invention introduces q-calculus geometry theory and utilizes the variation reduction and shape adjustability of q-Bézier surfaces to achieve dynamic optimization of path geometry features by adaptively adjusting the curve tension coefficient q, without changing the control points (i.e., without violating corridor constraints). It automatically tightens the path near obstacles to ensure safety and automatically relaxes the path in open areas to achieve maximum smoothness, reducing jerk from the geometric source and effectively suppressing flight oscillations. In addition to optimizing the position path, it also optimizes the attitude path using parametric quartic unit quaternion splines. By adaptively adjusting the shape parameters according to the path curvature, it eliminates abrupt changes in angular velocity in attitude interpolation, solving the fuselage oscillation problem caused by excessive attitude adjustment and achieving coordinated smoothing of position and attitude. The invention employs an analytical calculation method based on environmental perception (obstacle distance, path curvature) to determine the optimization parameters q and λ, replacing the traditional high-dimensional iterative optimization algorithm, greatly reducing computational complexity. This allows the path optimization method to run on onboard computers with limited computing power, meeting the real-time planning needs of indoor UAVs. Attached Figure Description

[0070] Figure 1 This is a basic flowchart illustrating an indoor drone path planning method according to an embodiment of the present invention. Detailed Implementation

[0071] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0072] Reference Figure 1 As an embodiment of the present invention, an indoor unmanned aerial vehicle (UAV) path planning method is provided, comprising the following steps:

[0073] Step S1: Calculate and construct an indoor Euclidean distance field map of the UAV in real time. Based on the indoor Euclidean distance field map, generate an initial collision-free discrete path point sequence according to the start and end points of the UAV's flight path.

[0074] Step S2: Construct a safe flight corridor based on the discrete path point sequence, and extract the coordinates of the feature points and cross-section points of the safe corridor to generate a control point grid;

[0075] Step S3: Combining q-calculus theory, calculate the curve tension coefficient q based on the control point grid and the minimum obstacle distance, and use the tensor product q-Bézier surface equation to generate the three-dimensional spatial position path trajectory;

[0076] Step S4: Construct a reference attitude frame sequence, calculate the geometric curvature of the three-dimensional spatial position path trajectory based on the reference attitude frame sequence, calculate the shape parameters based on the geometric curvature, and generate a continuous attitude path based on the shape parameters using the parametric quartic unit quaternion spline interpolation formula.

[0077] Step S5: Parameterize the arc length of the three-dimensional spatial position path trajectory and attitude path to generate a complete flight trajectory control command sequence.

[0078] This method generates flight trajectories that are highly safe and collision-free, extremely stable, and free from oscillations and sudden changes in angular acceleration in complex indoor environments, significantly improving the safety and control quality of UAV flights.

[0079] Step S1 includes:

[0080] The UAV constructs an indoor raw point cloud map of the indoor environment in real time using an airborne lidar and depth camera. After rasterizing the indoor raw point cloud map, an indoor Euclidean symbolic distance field map of the UAV is obtained. The start and end points of the UAV's flight path are determined according to the received mission instructions. Based on the start and end points, a path search algorithm is used to generate an initial discrete path point sequence in the indoor Euclidean symbolic distance field map.

[0081] The drone uses LiDAR to collect sparse geometric point cloud data and depth camera to collect dense texture point cloud data. The drone pose is calculated in real time using SLAM technology (LIO / VIO). Based on the geometric point cloud data and texture point cloud data, the multi-frame local point cloud calculated by SLAM technology is stitched together into a globally consistent indoor original point cloud map.

[0082] The original indoor point cloud map is voxelized to obtain a 3D occupied grid map, where each voxel is marked as occupied (obstacle), free (accessible), and unknown. Based on the Euclidean Symbolic Distance Field (ESDF) algorithm, the Euclidean distance from each free voxel to the nearest occupied obstacle voxel is calculated, generating an indoor Euclidean symbolic distance field map containing obstacle distance information, providing safe data for path planning.

[0083] Receive a path planning task instruction containing the start and end points, use the A-path search algorithm to search in an indoor Euclidean symbolic distance field map, generate a series of continuous and collision-free grid center point coordinates connecting the start and end points, and obtain an initial discrete path point sequence.

[0084] High-precision environmental perception and preliminary path exploration were achieved. A globally consistent map was constructed through multi-sensor fusion and SLAM technology, and obstacle distance information was provided by Euclidean symbolic distance field (ESDF), ensuring that the initial discrete path point sequence was collision-free and passable globally.

[0085] Step S2 includes:

[0086] Building a safe flight corridor includes:

[0087] Using each path point in the discrete path point sequence and the midpoint between adjacent path points as the center, dilation detection is performed in the indoor Euclidean symbolic distance field map to generate a set of mutually overlapping maximum inscribed collision-free cubes. The set of maximum inscribed collision-free cubes is the safe flight corridor.

[0088] Expansion detection includes:

[0089] The initial center is an initial micro cube, and each side of the initial micro cube is a minimum value. The size of the initial micro cube is uniformly expanded along the dimensional direction in the indoor Euclidean symbolic distance field map until any face of the initial micro cube touches an obstacle in the indoor Euclidean symbolic distance field map, that is, the grid state is occupied or the Euclidean symbolic distance is 0, thus obtaining the largest inscribed collision-free cube.

[0090] Generating the control point mesh includes:

[0091] The vertex coordinates of each cube in the set of maximum inscribed collision-free cubes and the feature point coordinates of the overlapping area between adjacent cubes are extracted. The feature points include the center point and corner points. M cross-sectional points are selected on the cross-section of the safe flight corridor using a resampling method. All vertex coordinates, feature point coordinates of the overlapping area, and cross-sectional point coordinates are connected in path order to form a point matrix that is ordered along both the path direction and the cross-sectional direction of the discrete path point sequence. The point matrix is ​​defined as a control point grid. Each control point in the control point grid includes an index parameter along the path direction and an index parameter along the cross-sectional direction. The control point grid has a dimension of MN, where M is the number of control points on the cross-section and N is the number of cross-sectional layers along the path direction.

[0092] In this embodiment, the cubes in the largest inscribed non-collision cube set are connected end to end to form a safe flight corridor.

[0093] Along the path direction and according to the path order are respectively the path point direction along the discrete path point sequence and the path point order according to the discrete path point sequence.

[0094] M defines the shape and complexity of the cross-section of the flight corridor. For example, if the cross-section is a simple rectangle, M may take a smaller value. M is used to define the structure of the corridor boundary, ensure that the centerline is within the safe area, and determine the control capability of the q-Bézier surface in the lateral obstacle avoidance direction.

[0095] N defines the fineness and length of the discrete path point sequence. If the discrete path point sequence has more major turning points, then N will be correspondingly larger, which determines the control capability and smoothness of the q-Bézier curve in the longitudinal direction (flight direction).

[0096] Step S2 aims to construct a collision-free volumetric corridor based on the initial discrete pathpoint sequence and transform it into a control structure for generating parametric surfaces. This control point mesh is strictly located at or within the boundary of the safe flight corridor and directly serves as the input parameter for subsequently generating the tensor product q-Bézier surface, ensuring that the generated surface is completely enclosed within the collision-free corridor.

[0097] The linear discrete path is transformed into a volumetric safety space. By constructing a safety flight corridor and control point mesh, strict spatial constraints are provided for subsequent surface generation, ensuring that the generated trajectory is always contained within the unobstructed area, thus solving the hard constraint problem in path optimization.

[0098] Step S3 includes:

[0099] Step S31: Combining q-calculus theory, calculate the minimum obstacle distance around each path segment in the discrete path point sequence, and construct an adaptive q-value mapping function based on the minimum obstacle distance;

[0100] Step S32: Based on the control point grid and the curve tension coefficient q calculated through the adaptive q-value mapping function, construct the tensor product q-Bézier surface equation, and generate a smooth three-dimensional spatial position path trajectory based on the tensor product q-Bézier surface equation.

[0101] The q-calculus theory was introduced, providing a mathematical foundation for adjusting the path shape. By calculating the minimum obstacle distance, a mechanism was implemented to automatically adjust the path tension according to the width of the environment, which is a prerequisite for adaptive planning.

[0102] Step S31 specifically includes:

[0103] initialization The q-Bernstein basis functions are defined as the basis functions for UAV path generation. The mathematical expression of the basis functions is:

[0104] ;

[0105] ;

[0106] ;

[0107] ;

[0108] in, for Second-rate Defined q-Bernstein basis functions, For normalized parameters and , The index of the pre-defined control point number in the control point grid and , The curve tension coefficient and >0, The coefficients are q-binomial coefficients. It is a natural number;

[0109] The minimum obstacle distances around each control point are obtained by querying an indoor Euclidean symbolic distance field map. An adaptive q-value mapping function is then constructed based on these minimum obstacle distances. The mathematical expression of the adaptive q-value mapping function is as follows:

[0110] ;

[0111] in, The curve tension coefficient, This is the preset minimum curve tension coefficient. The preset safe distance threshold, This represents the minimum obstacle distance.

[0112] To overcome the limitation of the fixed shape of traditional B-spline curves, a shape-adjustable feature is introduced. Basis functions serve as the mathematical foundation for trajectory generation. To achieve adaptive adjustment of the path shape according to environmental changes, a mapping relationship needs to be established between obstacle distances and the curve tension coefficient q. Each Basis functions are all weight functions, which determine the normalization parameters. The degree of influence on the i-th control point on the curve.

[0113] The minimum obstacle distance around the control point is the minimum Euclidean distance of the control point. The preset minimum curve tension coefficient is set to 0.1 to ensure the maximum path contraction capability in extremely narrow environments. The preset safe distance threshold is the radius of the drone.

[0114] Adaptive q-value mapping function representation:

[0115] When the minimum obstacle distance When the value is relatively small (close to the obstacle), the q value tends to the minimum curve tension coefficient. By utilizing the contraction property of the q-curve, the generated path is made to closely follow the control polygon, thereby strictly limiting it to the safe corridor and prioritizing obstacle avoidance safety.

[0116] When the minimum obstacle distance When the area is large (open area), the q value tends to 1. At this time, the curve degenerates into a standard Bézier curve with the greatest geometric smoothness, thereby reducing the jerk during flight and effectively suppressing fuselage vibration.

[0117] The normalization parameter u represents the progress of the path. u=0 corresponds to the starting point of the path, and u=1 corresponds to the ending point of the path. When generating the path, discrete sampling is performed in the interval [0,1] (e.g., 0,0.01,0.02,...,1.0), and each sampled value is a u.

[0118] The curve tension coefficient q controls the tension of the q-Bézier curve. The smaller q is, the closer the curve adheres to the control polygon (strong obstacle avoidance capability); the closer q is to 1, the smoother the curve (higher comfort).

[0119] In the q-Bézier curve, the q-binomial coefficient acts as a scaling factor for the weights, determining the proportion of contribution of different control points to the curve shape. As q changes, the q-binomial coefficient alters the distribution of weights, thus changing the shape of the curve.

[0120] Specifically, the mathematical implementation of adaptive obstacle avoidance is defined. By constructing an adaptive q-value mapping function, the q-value is reduced in narrow areas (enhancing path contraction and ensuring obstacle avoidance safety), and the q-value is increased in open areas (approaching the standard Bézier curve and improving smoothness), thus resolving the contradiction between safety and flight smoothness.

[0121] Step S32 specifically includes:

[0122] The mathematical expression for the equation of the tensor product q-Bézier surface is:

[0123] ;

[0124] in, The equation for the tensor product q-Bézier surface is given. Let m be the positional parameter on the cross-section of the safe flight corridor. The order of the q-Bernstein basis functions along the cross-sectional direction is defined as m = M⁻¹, and w is... The order of the defined q-Bernstein basis functions along the path direction, w=N-1, For control points of the control point grid, For the index parameter along the path direction, For index parameters along the cross-sectional direction, To use in the path direction Defined q-Bernstein basis functions, Standards adopted in the cross-sectional direction Defined q-Bernstein basis functions;

[0125] Generating smooth 3D spatial path trajectories based on the tensor product q-Bézier surface equation specifically includes:

[0126] Substituting v=0.5 into the tensor product q-Bézier surface equation, we obtain the three-dimensional spatial position path trajectory. The mathematical expression for the three-dimensional spatial position path trajectory is:

[0127] .

[0128] The value 'u' controls the position of the surface in the direction of the first parameter, representing the progress along the drone's flight path. As 'u' changes from 0 to 1, the drone moves along the path from the starting point to the ending point.

[0129] v is used to control the position of the surface in the direction of the second parameter, which represents the position on the cross-section of the safe flight corridor. When v changes from 0 to 1, the drone moves from one side to the other on the cross-section.

[0130] Setting v=0.5 indicates that the position parameter is fixed at 0.5 in the cross-sectional direction, representing the centerline of the flight corridor. Therefore, the generated smooth three-dimensional spatial position path trajectory is actually the surface S(u,0.5), which is a curve along the centerline of the corridor.

[0131] In the equations for the tensor product surface, a weighted summation of basis functions is used for both directions, including:

[0132] For along the path direction ( (Direction), use As a weight, 'a' here is the index parameter along the path direction, which corresponds to the control point grid. The first index in;

[0133] For along the cross-sectional direction ( (Direction), use As a weight, 'b' here is an index parameter along the cross-sectional direction, corresponding to the control point grid. The second index in.

[0134] standard The defined q-Bernstein basis functions are those where q=1. Define q-Bernstein basis functions to ensure the regularity of the cross-sectional shape;

[0135] The path S(u) is the final generated spatial position trajectory that balances obstacle avoidance safety and flight smoothness.

[0136] Step S3 aims to use q-calculus theory to adaptively adjust the geometry of the path based on the distribution of environmental obstacles, in order to achieve the best balance between obstacle avoidance safety and flight smoothness.

[0137] The final 3D position trajectory was generated using the tensor product q-Bézier surface equation. By extracting the surface centerline (v=0.5), a smooth curve with high-order geometric continuity was generated, which effectively reduced the jerk during UAV flight and suppressed fuselage vibration.

[0138] Step S4 includes:

[0139] Calculate the first derivative of the three-dimensional spatial position path trajectory, and construct a reference attitude frame sequence based on the Rotation Minimum Frame (RMF) algorithm. The construction process includes:

[0140] The first derivative is normalized to obtain the unit tangent vector, which is then used as the forward axis of the UAV.

[0141] The lateral axis of the UAV is obtained through the Rotation Minimum Frame (RMF) algorithm, specifically including:

[0142] At the starting point of the discrete path point sequence (u=0), a unit lateral vector is initialized that is perpendicular to the vertical axis of the indoor Euclidean symbolic distance field map and orthogonal to the forward axis. The unit lateral vector is propagated along the path of the discrete path point sequence using the parallel propagation method. During the propagation process, the unit lateral vector is kept orthogonal to the forward axis. The direction of all unit lateral vectors during the propagation process is the dynamic lateral axis of the UAV on the discrete path point sequence.

[0143] The upward axis is the direction of the cross product vector of the lateral axis direction vector and the forward axis direction vector;

[0144] Combine the forward axis, lateral axis, and upward axis according to their corresponding path points and arrange them along the path direction to obtain a successfully constructed reference attitude frame sequence;

[0145] The reference attitude frames in the reference attitude frame sequence are concatenated into axis unit vectors to obtain a rotation matrix. The unit quaternion corresponding to each rotation matrix is ​​calculated by matrix basic operations. The discrete key attitude quaternion sequence is obtained by uniformly sampling along the path.

[0146] At each point on the path, a local coordinate system (i.e., a frame) is defined. This coordinate system tells the drone its three-dimensional flight orientation. The local coordinate system consists of three mutually perpendicular vectors, representing the drone's forward direction, lateral direction, and upward direction, respectively.

[0147] The Rotating Minimum Frame Algorithm (RMF) aims to construct a frame that changes smoothly along a path, characterized by minimal rotation angles between adjacent frames, thus avoiding unnecessary twisting or flipping. It achieves this by ensuring that the derivative of the lateral vector in the tangent direction is parallel to the tangent vector. During path geometry changes (especially in regions with near-zero curvature), the RMF provides extremely stable and smooth frame changes, avoiding drone jitter issues.

[0148] This process transforms abstract mathematical curves into a concrete orientation of the drone in physical space. For each point on the path, a unique, smoothly changing body orientation is determined as the baseline for subsequent attitude planning. A unit quaternion represents the desired attitude of the drone at each path point.

[0149] A reference attitude baseline for the UAV was constructed. The Rotation Minimum Frame (RMF) algorithm was used to ensure that the change of the reference coordinate system along the path is minimized, avoiding abrupt attitude changes caused by path geometric variations, and providing a stable input for subsequent attitude smoothing.

[0150] Calculate the geometric curvature of the path trajectory in three-dimensional space, and construct an adaptive shape parameter mapping function based on the geometric curvature. The construction process includes:

[0151] Calculate the geometric curvature of the 3D spatial path trajectory at each point corresponding to the key pose quaternion sequence. Based on the geometric curvature, establish a mapping function for adaptive shape parameters. The mathematical expression of the mapping function for adaptive shape parameters is:

[0152] ;

[0153] in, The preset reference shape parameters, The preset adjustment coefficient, For mapping functions, Let S(t) be the geometric curvature of the three-dimensional spatial path trajectory S(t) at parameter t;

[0154] The baseline shape parameter represents the default smoothness during straight-line flight, while the adjustment factor controls the strength of the curvature's influence on the adaptive shape parameter.

[0155] By using a mapping function, the adaptive shape parameter values ​​are automatically reduced where the geometric curvature of the path is large (sharp turns). Based on the properties of parametric quartic unit quaternion splines, reducing the adaptive shape parameter makes the transition of the attitude curve at connection points more convergent and smooth, thus smoothing the rate of change of angular velocity and eliminating abrupt changes in angular acceleration. Where the geometric curvature of the path is small (straight flight), the adaptive shape parameter remains close to the reference shape parameter, ensuring natural and smooth attitude transformations.

[0156] Using the key attitude quaternion sequence and the calculated shape parameters, a continuous attitude path is generated based on the parametric quartic unit quaternion spline interpolation formula. The mathematical expression of the parametric quartic unit quaternion spline interpolation formula is as follows:

[0157] ;

[0158] ;

[0159] in, This is the interpolation formula for a fourth-order unit quaternion spline with parameters. These are interpolation parameters. This represents the index number of the key pose quaternion sequence. It is a quaternion exponential mapping function. for arrive The relative angular velocity vector, For those with shape parameters The quartic polynomial basis functions, It is the z-th element of the key pose quaternion sequence.

[0160] For those with shape parameters The quartic polynomial basis functions are scalar forms constructed based on parametric quartic polynomial harmonic basis functions. These harmonic basis functions satisfy normality and symmetry and are used to adjust the continuity and shape of the interpolation curve at connection points. The core of this approach lies in… The weights of the two intermediate basis functions were controlled.

[0161] ;

[0162] ;

[0163] ;

[0164] ;

[0165] ;

[0166] in, , , and All are parameterized quartic polynomial-based basis functions.

[0167] In the embodiment, shape parameters are included. quartic polynomial basis functions It is a polynomial defined on [0,1] that satisfies , , , , and And its shape is made of adjust.

[0168] In curve and surface modeling, basis functions are a set of predefined mathematical functions. By multiplying these basis functions with control points and summing the results, a curve or surface can be constructed. Each basis function corresponds to a control point, and the value of the basis function determines the influence of that control point at a specific location on the curve (or surface). Polynomial basis functions possess favorable mathematical properties, such as continuity and differentiability, resulting in smooth and computationally easy-to-compute curves. (Shape parameters are also included.) The quartic polynomial basis functions refer to polynomial functions with a highest degree of 4. Besides depending on the interpolation parameter p, they also depend on an externally adjustable parameter. By changing It can adjust the smoothness and shape of the attitude trajectory without changing the key attitude points.

[0169] The attitude path is a highly smooth path with continuous angular acceleration. The attitude path is a series of quaternions that change continuously over time, and each quaternion Q(t)=[w,x,y,z] uniquely describes the three-dimensional orientation of the UAV at time t. The relative angular velocity vector represents the velocity vector from the point where the angular velocity vector is the velocity vector from the point where the angular velocity Go to Which axis to rotate around and how fast.

[0170] It is a function that changes from 0 to 1, when When =0, =0, =1, therefore = (starting point);

[0171] when When =1, =1, = ,so = (end);

[0172] when When changing between 0 and 1 Control the progress of rotation. This determines whether the progress is uniform or slow at first, then fast, then slow again (S-shaped), thus eliminating abrupt changes in angular acceleration.

[0173] Step S4 aims to generate a highly smooth attitude trajectory for the UAV that matches the spatial position path and is free from abrupt changes in angular acceleration, thereby eliminating propeller oscillations caused by rapid attitude adjustments. This is another core step of the invention; to prevent the UAV from oscillating due to excessively rapid attitude adjustments during turns, the attitude path needs to be optimized. The shape parameter λ is adaptively calculated based on the geometric curvature of the spatial position path, and a smooth attitude path matching the position path is generated using a parametric quartic unit quaternion spline.

[0174] Dynamically smooth attitude control was achieved. An adaptive shape parameter mapping function automatically adjusted the stiffness of the attitude interpolation based on the path curvature. Reducing the λ value at sharp turns with high curvature resulted in a smoother attitude transition, eliminating abrupt changes in angular acceleration and effectively preventing oscillations during complex maneuvers.

[0175] Step S5 includes:

[0176] The continuous three-dimensional spatial location path trajectory is... Step size ( To find the minimum value, take Discretize the path points (=0.001) to obtain a high-density path point sequence. Calculate the Euclidean distance between adjacent path points in the high-density path point sequence and sum them to obtain the physical arc length corresponding to each path point in the high-density path point sequence. The mathematical expression for the physical arc length is:

[0177] ;

[0178] in, path point The physical arc length, For natural numbers, The Euclidean distance between adjacent path points;

[0179] Calculate the total physical length of the three-dimensional spatial trajectory of the UAV flight path. The total physical length is... ;

[0180] The normalized parameters and physical arc length are established using an interpolation function. The mapping relationship between them allows points on the path to be indexed by physical distance;

[0181] when > At that time, the construction includes an acceleration segment (uniform acceleration to...) ), constant speed segment (maintain) Trapezoidal velocity profiles during the flight phase and the deceleration phase (uniform deceleration to 0);

[0182] when At this point, a triangular velocity profile containing only acceleration and deceleration segments is constructed, and the peak velocity reached is... ;

[0183] in, This is the maximum flight acceleration of the drone. This is the maximum flight speed of the drone;

[0184] The displacement-time function of the UAV is output based on the trapezoidal velocity profile and the triangular velocity profile.

[0185] when > When the path is long enough, the UAV can accelerate to its maximum speed and maintain it for a period of time. Calculate the time nodes of the trapezoidal velocity profile. The time nodes include the time nodes of the first acceleration segment, the first uniform speed segment, and the first deceleration segment.

[0186] First acceleration phase time node The mathematical expression is:

[0187] ;

[0188] The first uniform speed segment time node The mathematical expression is:

[0189] ;

[0190] First deceleration phase time node The mathematical expression is:

[0191] ;

[0192] First time The mathematical expression is:

[0193] = + + ;

[0194] First displacement-time function The mathematical expression is:

[0195] ;

[0196] when When the path is short, the UAV needs to start decelerating before reaching its maximum speed. The time nodes of the triangular velocity profile are calculated, including the time nodes of the second acceleration phase and the second deceleration phase.

[0197] Second acceleration phase time node The mathematical expression is:

[0198] ;

[0199] Second deceleration phase time node The mathematical expression is:

[0200] ;

[0201] The mathematical expression for the second total time is:

[0202] ;

[0203] Second displacement-time function The mathematical expression is:

[0204] ;

[0205] Displacement-time function The mathematical expression is:

[0206] ;

[0207] Displacement-time function Ensures the speed of the drone at all times No more than or And acceleration The absolute value does not exceed By inputting any time t, you can calculate how many meters along the path the drone should have flown at that moment.

[0208] In this embodiment, the normalized parameters and physical arc length are established through piecewise linear interpolation. The mapping relationship between them.

[0209] The system achieves time parameterization and dynamic constraints on the trajectory. By parameterizing the arc length and planning the trapezoidal / triangle velocity profile, the geometric path is transformed into a time-series trajectory that follows maximum speed and maximum acceleration limits, ensuring that the planned commands are physically executable by the UAV.

[0210] Set the time step of the control command, iterate through each time point that is an integer multiple of the time step, calculate the displacement at the current time point based on the displacement-time function, and then apply the normalized parameters and the physical arc length. The mapping relationship between them is obtained, the normalized parameter corresponding to the displacement at the current time point is obtained as the physical arc length, the corresponding normalized parameter is input into the mathematical expression of the three-dimensional spatial position path trajectory, the target attitude quaternion is obtained, and the target velocity and target acceleration corresponding to the current time point are calculated according to the trapezoidal velocity profile and the triangular velocity profile.

[0211] The timestamp, target position, target velocity, target acceleration, and target attitude are pieced together to form a complete sequence of flight trajectory control commands;

[0212] In this embodiment, the time step is... Set to 0.02 seconds, corresponding to a control frequency of 50Hz;

[0213] The complete flight trajectory control command sequence is sent to the UAV's flight control system in real time. After receiving the commands, the PID controller inside the flight control system calculates the required rotational speed of each rotor and drives the UAV to fly along the planned path.

[0214] Because the path has been smoothed in terms of geometric curvature and rate of change of angular velocity, the drone can avoid propeller oscillation caused by abrupt maneuvers during flight.

[0215] Because the input spatial path undergoes adaptive smoothing of the geometric curvature using a q-Bézier surface, and the attitude path undergoes angular velocity smoothing using a parametric quaternion spline, and the velocity planning strictly adheres to dynamic constraints, the motor speed changes smoothly during the UAV's execution, effectively avoiding propeller oscillations caused by abrupt maneuvers (sharp turns, sudden stops), thus achieving stable and precise flight.

[0216] This step aims to transform purely geometric spatial position and attitude paths into physically constrained time-series trajectories executable by the UAV, and to generate the final control commands.

[0217] The drone flight planning and control process includes:

[0218] Step A1: Obtain the current physical time t;

[0219] Step A2: Based on the trapezoidal velocity curve, calculate the distance the drone flew in meters at physical time t;

[0220] Step A3: Find the normalized parameter and physical arc length From the list of mapping relationships between them, we can obtain the normalized parameter u corresponding to the flight distance of x meters;

[0221] Step A4: Substitute the normalized parameter u into the mathematical expression for the three-dimensional spatial position path trajectory. This gives the current coordinates (x, y, z).

[0222] Step A5: Determine which attitude segment the normalized parameter u falls into, and calculate the local interpolation parameter p;

[0223] Step A6: Substitute the interpolation parameter p into the parameterized quartic unit quaternion spline interpolation formula. This yields the current attitude path.

[0224] The system achieves the discretization and issuance of control commands. The planning results are transformed into a high-frequency (e.g., 50Hz) sequence of control commands (position, velocity, acceleration, attitude), which is directly connected to the PID controller of the flight control system to ensure that the algorithm can drive the UAV to fly accurately in real time.

[0225] This invention introduces q-calculus geometry theory and utilizes the variation reduction and shape adjustability of q-Bézier surfaces to dynamically optimize path geometry features by adaptively adjusting the curve tension coefficient q, without changing the control points (i.e., without violating corridor constraints). It automatically tightens the path near obstacles to ensure safety and automatically relaxes the path in open areas to achieve maximum smoothness, reducing jerk at the geometric source and effectively suppressing flight oscillations. In addition to optimizing the position path, it also optimizes the attitude path using parametric quartic unit quaternion splines. By adaptively adjusting the shape parameters according to the path curvature, it eliminates abrupt changes in angular velocity during attitude interpolation, solving the fuselage oscillation problem caused by excessive attitude adjustments and achieving coordinated smoothing of position and attitude. The optimization parameters q and λ are determined using an analytical calculation method based on environmental perception (obstacle distance, path curvature), replacing the traditional high-dimensional iterative optimization algorithm. This significantly reduces computational complexity, enabling the path optimization method to run on onboard computers with limited computing power, meeting the real-time planning needs of indoor UAVs.

[0226] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media containing computer-usable program code. The storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Red-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0227] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the protection scope of the present invention.

Claims

1. An indoor unmanned aerial vehicle (UAV) path planning method, characterized in that, Includes the following steps: Step S1: Calculate and construct an indoor Euclidean distance field map of the UAV in real time. Based on the indoor Euclidean distance field map, generate an initial collision-free discrete path point sequence according to the start and end points of the UAV's flight path. Step S2: Construct a safe flight corridor based on the discrete path point sequence, and extract the coordinates of the feature points and cross-section points of the safe corridor to generate a control point grid; Step S3: Combining q-calculus theory, calculate the curve tension coefficient q based on the control point grid and the minimum obstacle distance, and use the tensor product q-Bézier surface equation to generate the three-dimensional spatial position path trajectory; Step S4: Construct a reference attitude frame sequence, calculate the geometric curvature of the three-dimensional spatial position path trajectory based on the reference attitude frame sequence, calculate the shape parameters based on the geometric curvature, and generate a continuous attitude path based on the shape parameters using the parametric quartic unit quaternion spline interpolation formula. Step S5: Parameterize the arc length of the three-dimensional spatial position path trajectory and attitude path to generate a complete flight trajectory control command sequence.

2. The indoor UAV path planning method as described in claim 1, characterized in that, Step S1 includes: The UAV constructs an indoor raw point cloud map of the indoor environment in real time using an airborne lidar and depth camera. After rasterizing the indoor raw point cloud map, an indoor Euclidean symbolic distance field map of the UAV is obtained. The start and end points of the UAV's flight path are determined according to the received mission instructions. Based on the start and end points, an initial discrete path point sequence is generated in the indoor Euclidean symbolic distance field map using a path search algorithm.

3. The indoor UAV path planning method as described in claim 2, characterized in that, Step S2 includes: Building a safe flight corridor includes: Using each path point in the discrete path point sequence and the midpoint between adjacent path points as the center, an expansion detection is performed in an indoor Euclidean symbolic distance field map to generate a set of mutually overlapping maximum inscribed collision-free cubes. The set of maximum inscribed collision-free cubes is the safe flight corridor. Expansion detection includes: The initial center is an initial micro cube, and each side of the initial micro cube is a minimum value. The size of the initial micro cube is uniformly expanded along the dimensional direction in the indoor Euclidean symbolic distance field map until any face of the initial micro cube touches an obstacle in the indoor Euclidean symbolic distance field map, thus obtaining the largest inscribed collision-free cube. Generating the control point mesh includes: The vertex coordinates of each cube in the set of largest inscribed collision-free cubes and the feature point coordinates of the overlapping area between adjacent cubes are extracted. The feature points include center points and corner points. M cross-sectional points are selected on the cross-section of the safe flight corridor using a resampling method. All vertex coordinates, feature point coordinates of the overlapping area, and cross-sectional point coordinates are connected in path order to form a point matrix that is ordered along both the path direction and the cross-sectional direction of the discrete path point sequence. The point matrix is ​​defined as a control point grid. Each control point in the control point grid includes an index parameter along the path direction and an index parameter along the cross-sectional direction. The dimension of the control point grid is MN, where M is the number of control points on the cross-section and N is the number of cross-sectional layers along the path direction.

4. The indoor UAV path planning method as described in claim 3, characterized in that, Step S3 includes: Step S31: Combining q-calculus theory, calculate the minimum obstacle distance around each path segment in the discrete path point sequence, and construct an adaptive q-value mapping function based on the minimum obstacle distance; Step S32: Based on the control point grid and the curve tension coefficient q calculated by the adaptive q-value mapping function, construct the tensor product q-Bézier surface equation, and generate a smooth three-dimensional spatial position path trajectory based on the tensor product q-Bézier surface equation.

5. The indoor UAV path planning method as described in claim 4, characterized in that, Step S31 specifically includes: initialization The defined q-Bernstein basis functions are used as the basis functions for UAV path generation. The mathematical expression of the basis functions is as follows: ； ； ； ； in, for Second-rate Defined q-Bernstein basis functions, For normalized parameters and , The index of the pre-defined control point number in the control point grid and , The curve tension coefficient and >0, The coefficients are q-binomial coefficients. It is a natural number; The minimum obstacle distances around each control point are obtained by querying an indoor Euclidean symbolic distance field map. An adaptive q-value mapping function is constructed based on these minimum obstacle distances. The mathematical expression of the adaptive q-value mapping function is as follows: ； in, The curve tension coefficient, This is the preset minimum curve tension coefficient. The preset safe distance threshold, This represents the minimum obstacle distance.

6. The indoor UAV path planning method as described in claim 5, characterized in that, Step S32 specifically includes: The mathematical expression for the equation of the tensor product q-Bézier surface is: ； in, The equation for the tensor product q-Bézier surface is given. Let m be the positional parameter on the cross-section of the safe flight corridor. The order of the q-Bernstein basis functions along the cross-sectional direction is defined as m = M⁻¹, and w is... The order of the defined q-Bernstein basis functions along the path direction, w=N-1, For control points of the control point grid, For index parameters along the path direction, For index parameters along the cross-sectional direction, To use in the path direction Defined q-Bernstein basis functions, Standards adopted in the cross-sectional direction Defined q-Bernstein basis functions; Generating smooth 3D spatial path trajectories based on the tensor product q-Bézier surface equation specifically includes: Substituting v=0.5 into the tensor product q-Bézier surface equation, the three-dimensional spatial position path trajectory is obtained. The mathematical expression of the three-dimensional spatial position path trajectory is: 。 7. The indoor UAV path planning method as described in claim 6, characterized in that, Step S4 includes: Calculate the first derivative of the three-dimensional spatial position path trajectory, and construct a reference attitude frame sequence based on the Rotation Minimum Frame (RMF) algorithm. The construction process includes: The first derivative is normalized to obtain the unit tangent vector, which is then used as the forward axis of the UAV. The lateral axis of the UAV is obtained through the Rotation Minimum Frame (RMF) algorithm, specifically including: At the starting point of the discrete path point sequence, a unit lateral vector is initialized that is perpendicular to the vertical axis of the indoor Euclidean symbolic distance field map and orthogonal to the forward axis. The unit lateral vector is propagated along the path of the discrete path point sequence using the parallel propagation method. During the propagation process, the unit lateral vector is kept orthogonal to the forward axis. The direction of all unit lateral vectors during the propagation process is the dynamic lateral axis of the UAV on the discrete path point sequence. The upward axis is the direction of the cross product vector of the lateral axis direction vector and the forward axis direction vector; The forward axis, lateral axis, and upward axis are combined according to their corresponding path points and arranged along the path direction to obtain a successfully constructed reference attitude frame sequence. The reference attitude frames in the reference attitude frame sequence are concatenated with their axis unit vectors to obtain a rotation matrix. The unit quaternion corresponding to each rotation matrix is ​​calculated by matrix basic operations. The discrete key attitude quaternion sequence is obtained by uniformly sampling along the path.

8. The indoor UAV path planning method as described in claim 7, characterized in that, Calculate the geometric curvature of the path trajectory in the three-dimensional space, and construct an adaptive shape parameter mapping function based on the geometric curvature. The construction process includes: Calculate the geometric curvature of the three-dimensional spatial position path trajectory at each point corresponding to the key attitude quaternion sequence, and establish an adaptive shape parameter mapping function based on the geometric curvature. The mathematical expression of the adaptive shape parameter mapping function is as follows: ； in, The preset reference shape parameters, The preset adjustment coefficient, For mapping functions, Let S(t) be the geometric curvature of the three-dimensional spatial path trajectory S(t) at parameter t; Using the key attitude quaternion sequence and the calculated shape parameters, a continuous attitude path is generated based on the parametric quartic unit quaternion spline interpolation formula. The mathematical expression of the parametric quartic unit quaternion spline interpolation formula is as follows: ； ； in, This is the interpolation formula for a fourth-order unit quaternion spline with parameters. These are interpolation parameters. This refers to the index number of the key pose quaternion sequence. It is a quaternion exponential mapping function. for arrive The relative angular velocity vector, For those with shape parameters The quartic polynomial basis functions, It is the z-th element of the key pose quaternion sequence.

9. The indoor UAV path planning method as described in claim 8, characterized in that, Step S5 includes: The continuous three-dimensional spatial location path trajectory is... The step size is discretized to obtain a high-density path point sequence. The Euclidean distance between adjacent path points in the high-density path point sequence is calculated and accumulated to obtain the physical arc length corresponding to each path point in the high-density path point sequence. The mathematical expression for the physical arc length is: ； in, path point The physical arc length, For natural numbers, The Euclidean distance between adjacent path points; Calculate the total physical length of the three-dimensional spatial trajectory of the UAV flight path. The total physical length is... ; The normalized parameters and physical arc length are established using an interpolation function. The mapping relationship between them; when > At the same time, a trapezoidal velocity profile is constructed, including an acceleration segment, a constant velocity segment, and a deceleration segment; when At this point, a triangular velocity profile containing only acceleration and deceleration segments is constructed, and the peak velocity reached is... ; in, This is the maximum flight acceleration of the drone. This is the maximum flight speed of the drone; The displacement-time function of the UAV is output based on the trapezoidal velocity profile and the triangular velocity profile.

10. The indoor UAV path planning method as described in claim 9, characterized in that, Set the time step of the control command, iterate through each time point that is an integer multiple of the time step, calculate the displacement at the current time point based on the displacement-time function, and then apply the normalized parameters and the physical arc length. The mapping relationship between them is obtained, the normalized parameter corresponding to the displacement at the current time point is obtained as the physical arc length, the corresponding normalized parameter is input into the mathematical expression of the three-dimensional spatial position path trajectory, the target attitude quaternion is obtained, and the target velocity and target acceleration corresponding to the current time point are calculated according to the trapezoidal velocity profile and the triangular velocity profile. The timestamp, target position, target velocity, target acceleration, and target attitude are pieced together to form a complete sequence of flight trajectory control commands; The complete flight trajectory control command sequence is sent to the UAV's flight control system in real time. After receiving the commands, the PID controller inside the flight control system calculates the required rotational speed of each rotor and drives the UAV to fly along the planned path.

Images