An unmanned aerial vehicle autonomous path planning and safe flight trajectory optimization method suitable for indoor scenes

Abstract

The application relates to the technical field of unmanned planes, and discloses a method for autonomous path planning and safe flight trajectory optimization of an unmanned plane suitable for indoor scenes, which comprises the following steps: collecting unmanned plane movement data and environment point cloud data through a multi-source sensor to complete positioning initialization; constructing a three-dimensional voxel grid map and generating an ESDF distance field, and combining target navigation points to complete planning of a starting point and an ending point configuration; generating a global reference smooth trajectory; converting the global initial smooth trajectory into a uniform cubic B-spline curve, performing global collision detection based on the voxel grid map, and generating a candidate B-spline trajectory without collision in the whole process; constructing a multi-dimensional cost function system and iteratively solving optimal control points by adopting an L-BFGS algorithm, and selecting a trajectory with the minimum cost as an optimal B-spline trajectory; and performing high-density sampling and time redistribution on the optimal trajectory, and executing the optimal trajectory by a flight control system after completing dynamic school verification, safety verification and shape verification.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, and particularly relates to the field of UAVs in indoor scenarios. Specifically, it is a method for autonomous path planning and safe flight trajectory optimization of UAVs suitable for indoor scenarios. Background Technology

[0002] With the continuous development of quadcopter drone technology, it is now widely used in inspection, surveying, and environmental monitoring in indoor computer rooms, shopping malls, stadiums, warehouses, underground parking garages, office buildings, and other scenarios. Compared to open outdoor airspace, indoor environments place higher demands on drone flight control and trajectory planning. Indoor environments lack GNSS positioning signals, forcing drones to rely solely on onboard sensors for autonomous positioning and environmental awareness. The confined indoor space, dense array of static obstacles, and limited usable flight space mean sudden trajectory changes can easily cause aircraft vibration and increase collision risks. Simultaneously, the random movement of dynamic obstacles such as personnel and mobile devices poses a high risk of sudden collisions. Based on these requirements, existing indoor single-drone path planning and trajectory optimization technologies have numerous shortcomings and cannot meet the needs of practical engineering applications. Specific problems are as follows: I. Existing path search algorithms have poor trajectory smoothness. In existing technologies, indoor path planning is based on A Discrete path search algorithms such as RRT output discrete polyline paths, which can easily cause sudden changes in the speed and acceleration of the drone, leading to body shaking and attitude deviation, significantly increasing the risk of indoor collisions, while also increasing the flight control load and shortening the flight time.

[0003] II. Insufficient ability to avoid dynamic obstacles Existing global planning relies on pre-built static maps, which cannot identify dynamic obstacles in real time. When obstacles suddenly appear, trajectory adjustment is delayed and safety is poor. In addition, some optimization algorithms have a large amount of computation, making it difficult to perform real-time calculations on airborne low-computing-power devices.

[0004] III. Weak multi-sensor data fusion and target localization capabilities Indoor environments lack GNSS signals, single visual sensors are easily obstructed by lighting conditions, and single lidar sensors have blind spots. Existing solutions suffer from poor multi-sensor fusion performance, require manual setting of target points, cannot autonomously identify and generate target points, and their automation levels are insufficient to meet the requirements for unmanned operations.

[0005] IV. Trajectory optimization is prone to deviating from the preset operation route. Existing trajectory optimization algorithms focus on obstacle avoidance, and when adjusting the trajectory, they often deviate from the preset inspection and mapping routes, making it impossible to guarantee operational accuracy.

[0006] V. Lack of systematic dynamic constraints Some planning schemes only optimize path geometry without constraining dynamic limits such as speed, acceleration, and jerk. The trajectory parameters exceed the equipment's capacity, making it difficult for the flight control system to track stably and increasing the risk of loss of control and crash.

[0007] In summary, existing technologies cannot simultaneously meet the five major requirements of trajectory smoothing, dynamic obstacle avoidance, autonomous positioning, flight path fidelity, and dynamic adaptation. Therefore, there is an urgent need to develop a drone trajectory planning and optimization method that is adapted to complex indoor environments. Summary of the Invention

[0008] To address the problems of existing indoor UAV path planning and trajectory optimization methods, such as poor trajectory smoothness, insufficient dynamic obstacle avoidance and slow response, weak multi-sensor fusion and target localization capabilities, easy deviation of flight path, reliance on manual target point setting, lack of systematic dynamic constraints, and excessive flight parameters, this invention provides an autonomous path planning and safe flight trajectory optimization method for UAVs suitable for indoor scenarios.

[0009] This invention is achieved using the following techniques: This invention provides a method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios, comprising the following steps: a. Multi-source sensor data acquisition and initial flight information acquisition The system activates the multi-source sensors on the drone, including the IMU, LiDAR, and RGB-D depth camera, to collect drone motion and pose data as well as indoor environmental point cloud data. It performs timestamp alignment and coordinate correction on the multi-source data, removes abnormal data such as noise, outliers, and delays, and completes the positioning initialization and environmental acquisition in the GNSS-free scenario, ensuring the temporal consistency and spatial accuracy of the mapping input data.

[0010] Specifically, the following steps are included: aa, drone preparation After the drone is powered on and completes its self-test, it takes off vertically to the preset safe flight altitude.

[0011] ab, collect drone motion data The IMU (Inertial Measurement Unit) is activated to collect high-frequency motion data of the UAV body, including three-axis attitude angles, angular velocity, and linear acceleration.

[0012] By combining the SLAM positioning results of LiDAR and RGB-D depth camera, the global pose information of the UAV in an unknown indoor environment is calculated in real time, achieving continuous and stable positioning in a GNSS-free environment.

[0013] ac, collecting point cloud data of the indoor environment Simultaneously activate the LiDAR and RGB-D depth camera to conduct environmental perception and data acquisition. The LiDAR performs mid-to-long-range large-area environmental contour scanning, while the RGB-D depth camera fills in the blind spots of close-range detection, achieving full coverage perception of the indoor environment without blind spots.

[0014] ad, multi-sensor time synchronization and data preprocessing The IMU motion data, laser point cloud, and depth point cloud collected in steps ab and ac are uniformly timestamped and coarsely corrected to obtain fused sensing information, and noise points, invalid outliers, and delayed data are removed; positioning initialization and environmental data acquisition in the non-GNSS environment are completed to ensure the temporal consistency and spatial accuracy of the input mapping module.

[0015] b. Indoor environment modeling and target point acquisition Based on the multi-source sensor data collected in step a, the raw point cloud data output by the LiDAR and RGB-D camera is processed by filtering and denoising, outlier removal and downsampling to remove environmental noise and redundant point clouds. Relying on the online SLAM mechanism, a three-dimensional voxel grid map of the indoor environment is constructed in the RVIZ visualization interface, dividing the free space and the area occupied by static obstacles, and updating the three-dimensional position and contour information of the obstacles in real time.

[0016] Based on the constructed 3D voxel grid map, the ESDF distance field is generated in real time to calculate the distance from each location in space to the nearest obstacle, providing the core physical basis for subsequent trajectory obstacle avoidance cost calculation, safe distance constraints, and local trajectory optimization.

[0017] Receive the flight target waypoints output by the task, combine the voxel grid map and ESDF distance field to verify the legality of the target points, eliminate invalid waypoints that fall into obstacle areas, determine the safe start and end points, that is, the planned start and end points are both located in safe and flyable airspace, and complete the configuration of the planned start and end points.

[0018] c, Construct the global initial smooth trajectory The Minimum Snap high-order polynomial is used to generate a global baseline smooth trajectory, eliminating A from the root cause. This addresses the issues of sudden changes in velocity and acceleration, and body jitter caused by path search algorithms such as RRT. By minimizing the integral of the fourth derivative of the trajectory as the optimization objective, the continuity of trajectory position, velocity, acceleration, and jerk throughout the entire trajectory can be guaranteed, serving as the baseline input for subsequent B-spline local optimization.

[0019] A flight trajectory time vector function is constructed, and the total flight time is segmented and polynomials are constructed segment by segment. An equality constraint equation system is established, which is then transformed into an unconstrained quadratic programming solution through matrix factorization. This solution process relies solely on matrix operations and requires no iteration. The airborne platform can quickly obtain a continuous high-order polynomial trajectory, which serves as the initial benchmark for B-spline local optimization.

[0020] Specifically, the following steps are included: ca, constructing a trajectory optimization problem A vector function of the flight trajectory with respect to time is constructed. Since the three-axis motion of the UAV is independent, a single-axis trajectory is used as an example for mathematical description. The total flight time is divided into several segments, and a global initial smooth trajectory stage is constructed. Complex constraints are ignored and simplified to an unconstrained standard form. That is, the optimization objective is to minimize the integral value of the fourth derivative (snap) of the trajectory, and the position at each waypoint and the speed, acceleration and jerk at the start and end points are constrained to be controllable.

[0021] cb, Derivation of the polynomial form of the optimal trajectory based on the variational method By introducing the Euler-Lagrange equations from the variational method to derive the functional extremum problem, we define the Lagrange function and substitute it into the higher-order Euler-Lagrange equations. After continuous integration, we can obtain that each trajectory segment is a 7th-degree polynomial in terms of time. The unknown coefficients of this polynomial can be uniquely determined based on the position, velocity, acceleration, and jerk of the starting and ending points of each trajectory segment.

[0022] However, in this invention, the connection points of multiple trajectory segments need to satisfy the continuity of higher-order derivatives, which cannot be solved directly at a single point. A unified global set of equality constraint equations needs to be constructed.

[0023] cc, Continuity condition analysis of piecewise trajectories According to the boundary term analysis theory in the variational method, the continuity order of the piecewise Minimum Snap trajectory at intermediate nodes is determined by the number of constraints imposed on that node. The positions of each intermediate waypoint are constrained as free variables in the optimization process. Based on the natural boundary condition theory of the variational method's boundary terms, this approach ensures that the optimal trajectory obtained automatically satisfies continuity up to the sixth derivative at intermediate nodes, thus guaranteeing a very high degree of smoothness for the entire trajectory even when only the waypoint positions are given.

[0024] CD, time normalization and matrixing Directly constructing a polynomial using absolute time and solving for the coefficients suffers from condition number deterioration. Therefore, a normalized time variable is introduced to map each trajectory segment onto a standard interval. Trajectory representation is achieved by defining duration, local time variables, and normalized time. According to the chain rule, absolute time... The derivatives of each order and The derivative of the domain satisfies the scaling relation. Let represent the snap cost integral for each trajectory segment. Summing the costs of all trajectory segments yields the global cost function.

[0025] ce, matrix representation of constraints All equality constraints are uniformly organized into a system of linear equations, including endpoint constraints and inter-segment continuity constraints. The endpoint constraints and inter-segment continuity constraints are combined to form a total constraint matrix.

[0026] Transform cf into an unconstrained quadratic programming problem and solve it. The solution is obtained by using the selection matrix decomposition method. The solution process is based entirely on matrix operations and does not require iteration. It can quickly obtain the globally optimal initial smooth trajectory on the UAV onboard embedded platform.

[0027] Specifically, a selection matrix is ​​introduced to divide the constraint vector into fixed vectors and free vectors. The fixed vectors include the derivatives of the start and end points and the positions of all waypoints. The free vectors include continuity constraints, such as inter-segment velocity, acceleration, and jerk. The derivatives of two adjacent segments are set to be equal. By constructing the selection matrix, the coefficient vectors of all piecewise polynomials are solved and substituted into the cost function. The constrained optimization problem is precisely transformed into an unconstrained quadratic optimization problem with respect to the free variables. By taking the derivative with respect to the free variables and setting the derivative to zero, a closed-form analytical solution can be obtained directly.

[0028] CG, trajectory generation and output After obtaining the coefficients of the seventh-order polynomial for each segment, the complete 3D trajectory is generated by stitching them together along the time axis. This globally initial smooth trajectory ensures high-order continuity of position, velocity, acceleration, and jerk throughout the entire trajectory. The start and end points strictly match the preset motion derivatives of each order, and it can accurately pass through all intermediate waypoints. The high-order derivatives of the trajectory automatically and smoothly transition at the waypoint junctions without any abrupt changes. This trajectory serves as the benchmark input for subsequent B-spline dynamic local obstacle avoidance optimization, ensuring excellent smoothness while reserving sufficient deformation margin for local adjustments.

[0029] d. Local obstacle avoidance trajectory optimization based on B-splines This invention addresses the problem of strong global coupling and difficulty in local adjustment of Minimum Snap polynomial trajectories by using uniform cubic B-splines for trajectory reparameterization and local obstacle avoidance optimization. Relying on the local support and convex hull properties of B-splines, only the trajectory in the dangerous area is locally adjusted, while the original shape of the remaining areas remains unchanged.

[0030] Specifically, the steps include: A uniform cubic B-spline model is constructed. Based on the parameterized model of the uniform cubic B-spline matrix, the complete kinematic expression of the trajectory is derived. The Minimum Snap polynomial trajectory is transformed into a uniform cubic B-spline trajectory through least squares fitting. The transformed B-spline trajectory reproduces the path shape and motion characteristics of the original trajectory, while having local independent adjustability. If dynamic or static obstacles are encountered during subsequent flight, only the control points corresponding to the danger zone are optimized, and the rest of the route maintains the global optimal state. The control points obtained in this stage are the unoptimized initial control points.

[0031] Based on the voxel grid map constructed in step b, and combined with real-time environmental perception data, db performs global collision detection on the B-spline trajectory, divides it into barrier-free and obstacle-prone sections, traverses all segments and control points of the entire B-spline trajectory, and introduces A into the obstacle-prone sections. The path search algorithm obtains feasible guiding paths outside obstacles and derives the repulsive force direction vector of control points relative to obstacles, eliminating the need to construct a global ESDF and reducing onboard computation.

[0032] DC, Control Point Preprocessing and Multiple Candidate Trajectory Generation Based on the output trajectory segmentation results, the global initial B-spline control points obtained by transformation are partitioned: control points in unobstructed sections are locked, and control points in obstructed sections are optimizable variables.

[0033] Multiple sets of initial control point values ​​are configured based on different detour obstacle avoidance paths to form multiple independent initial solutions. Combining the obstacle base point and the repulsive force direction, two obstacle avoidance schemes are set for each obstacle section: one is to travel along the original trajectory, and the other is to generate multiple collision-free candidate B-spline trajectories by combining the detour schemes based on the reverse base point and the repulsive force vector. The candidate trajectories share nodes and time steps, and only the local control points are different.

[0034] e. Construction of multi-dimensional cost function and trajectory iterative optimization solution A multi-dimensional cost function system is constructed, comprising six sub-objectives: dynamic obstacle avoidance, collision penalty, trajectory fitting, smoothness, endpoint constraint, and dynamic feasibility. Using B-spline control points as optimization variables, the global objective function is obtained by weighted fusion of the sub-cost functions and solved using the L-BFGS algorithm, achieving dynamic local obstacle avoidance and multi-constraint collaborative optimization. This algorithm utilizes historical gradients to approximate the Hessian matrix, resulting in low memory overhead and fast convergence. It iterative updates of control points along negative gradients, with line search determining the optimal step size until cost convergence. After candidate trajectory optimization, the trajectory with the lowest total cost is selected as the optimal B-spline trajectory, satisfying all constraints.

[0035] f, Trajectory dynamics verification and time reallocation The optimal B-spline trajectory obtained in step e satisfies the safety and smoothness conditions, but a fixed time step can easily cause the dynamic parameters to exceed the limits. This step maintains the control points and trajectory shape unchanged, performs high-density sampling of the trajectory, calculates the motion parameters in segments and marks the exceeding intervals, and calculates the time stretching coefficient based on the exceeding ratio to smooth the dynamic indices.

[0036] The trajectory after time redistribution is subjected to dynamic verification, safety verification, and morphological verification. After the verification is qualified, the B-spline trajectory is input into the trajectory service module and executed by the flight control system.

[0037] This invention designs a complete path planning and trajectory optimization scheme for indoor single-unit UAV operation scenarios. Compared with traditional path planning methods, it effectively solves problems such as narrow indoor space, dense obstacles, multiple dynamic interferences, easy trajectory deviation, flight parameter exceeding limits, and target reliance on manual setting. It has many technical advantages and practical application value.

[0038] Compared with the prior art, the present invention has the following beneficial effects: This invention discloses an autonomous path planning and safe flight trajectory optimization method for UAVs suitable for indoor scenarios. By fusing data from multiple sensors, including LiDAR, RGB-D depth camera, and IMU, and combining 3D voxel grid modeling and visual target detection technology, the method enables autonomous positioning, environmental modeling, and dynamic generation of operational target points in GNSS-free indoor environments. This eliminates the need for manual waypoint deployment, achieving fully autonomous operation of the UAV, reducing labor costs, and improving scenario adaptability.

[0039] This invention addresses the problem of insufficient trajectory smoothness and easy occurrence of aircraft jitter caused by traditional planning algorithms. It uses MinimumSnap high-order polynomials to generate a global initial trajectory, ensuring the continuity of position, velocity, acceleration, and jerk. Then, through B-spline local optimization, only the trajectory in the risk area is adjusted while the global path is preserved, which effectively suppresses aircraft jitter and attitude drift, reduces the risk of collision in confined spaces, optimizes power consumption, and extends range.

[0040] This invention addresses the problem of high collision risk caused by the coexistence of moving and static obstacles in indoor environments. It constructs a cost function that includes six components such as dynamic obstacle avoidance, collision penalty, and dynamic constraints. It relies on a three-dimensional voxel grid to perceive moving and static obstacles in real time and establishes a multi-layered safety protection system.

[0041] This invention addresses the issue of optimized trajectory dynamics parameters exceeding limits and posing a risk of loss of control. It includes a trajectory time redistribution strategy to limit motion parameters within the UAV's hardware thresholds, thus preventing loss of control caused by dynamics exceeding limits. This invention addresses the problem of trajectory optimization easily deviating from the preset operation route. It integrates trajectory fitting, endpoint constraint, smoothness and dynamic cost function to limit trajectory deviation during obstacle avoidance optimization, maintain the route basically consistent with the original operation path, and constrain flight motion parameters to ensure that the UAV accurately reaches the target point, balancing operation accuracy and flight stability.

[0042] The algorithm employed in this invention features low computational complexity, fast iteration speed, and low computing power requirements. It can be directly deployed on an embedded platform on a UAV without the need for external computing devices, and is compatible with various indoor small multi-rotor UAVs. The entire algorithm supports simulation visualization debugging, facilitating rapid problem location, parameter optimization, and improved solution reliability. It can output smooth trajectories that meet dynamic constraints and dynamic obstacle avoidance conditions, effectively ensuring the autonomy, safety, and stability of indoor UAV operations. Attached Figure Description

[0043] Figure 1 This is a schematic diagram of the overall process.

[0044] Figure 2 This is a simulation diagram of the global initial smooth trajectory for Minimum Snap.

[0045] Figure 3 Simulation diagrams of multiple candidate trajectories after optimization.

[0046] Figure 4 A comparison chart of the distribution of control points before and after iterative optimization.

[0047] Figure 5 This is the simulation diagram of the optimal trajectory. Detailed Implementation

[0048] The specific embodiments of the present invention will be described in detail below.

[0049] A method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios, such as... Figures 1-5 This includes the following steps: a. Multi-source sensor data acquisition and initial flight information acquisition aa, drone preparation After the drone is powered on and completes its self-test, it takes off vertically to the preset safe flight altitude.

[0050] ab, collect drone motion data The IMU (Inertial Measurement Unit) is activated to collect high-frequency motion data of the UAV body, including three-axis attitude angles, angular velocity, and linear acceleration.

[0051] By combining the SLAM positioning results of LiDAR and RGB-D depth camera, the global pose information of the UAV in an unknown indoor environment is calculated in real time, achieving continuous and stable positioning in a GNSS-free environment.

[0052] ac, collecting point cloud data of the indoor environment Simultaneously activate the LiDAR and RGB-D depth camera to conduct environmental perception and data acquisition. The LiDAR performs mid-to-long-range large-area environmental contour scanning, while the RGB-D depth camera fills in the blind spots of close-range detection, achieving full coverage perception of the indoor environment without blind spots.

[0053] ad, multi-sensor time synchronization and data preprocessing The IMU motion data, laser point cloud, and depth point cloud collected in steps ab and ac are uniformly timestamped and coarsely corrected to obtain fused sensing information, and noise points, invalid outliers, and delayed data are removed; positioning initialization and environmental data acquisition in the non-GNSS environment are completed to ensure the temporal consistency and spatial accuracy of the input mapping module.

[0054] b. Indoor environment modeling and target point acquisition ba, constructing an indoor environment map The raw point cloud data output by the LiDAR and RGB-D camera is processed by filtering and denoising, outlier removal and downsampling to remove environmental noise and redundant point clouds. Based on the online SLAM mechanism, a three-dimensional voxel grid map of the indoor environment is constructed in the RVIZ visualization interface, dividing the free space and the area occupied by static obstacles, and updating the three-dimensional position and contour information of obstacles in real time.

[0055] bb, real-time solution of ESDF range field Based on the three-dimensional voxel grid map constructed in step ba, the ESDF distance field is generated in real time through iteration. The distance from each location in space to the nearest obstacle is calculated, providing the core physical basis for subsequent trajectory obstacle avoidance cost calculation, safety distance constraints, and local trajectory optimization.

[0056] bc, Configure flight target points and verify environment Receive the flight target waypoints output by the task, combine the voxel grid map with the ESDF distance field generated in step bb to verify the legality of the target points, eliminate invalid waypoints that fall into obstacle areas, and determine safe start and end points.

[0057] c, Construct the global initial smooth trajectory ca, constructing a trajectory optimization problem Time vector function for constructing the three-dimensional spatial trajectory of a UAV In this system, the motion of each axis is independent, following a single-axis trajectory. For example: the total flight time interval Divided into In the segment interval, during the stage of constructing the global initial smooth trajectory, ignoring complex constraints such as dynamic obstacles and feasible region boundaries, the optimization problem is directly simplified to the following unconstrained standard form: in, Indicates the order of the derivative being optimized ( This corresponds to minimizing the snap, which means only constraining the position of each waypoint and the velocity, acceleration, and jerk of the origin and destination, while the derivatives of intermediate waypoints are not constrained and are treated as free variables in the optimization process.

[0058] cb, Derivation of the polynomial form of the optimal trajectory based on the variational method To solve the functional extremum problem in step ca, the Euler-Lagrange equations from the variational method are introduced for derivation. The Lagrange function is defined. Substituting this into the higher-order Euler-Lagrange equations, we can obtain... After eight consecutive integrations, it can be known that... It's about time The 7th degree polynomial. That is, each segment of the trajectory can be represented as: in For the first The polynomial coefficients of the trajectory segments. For each trajectory segment, given the position, velocity, acceleration, and jerk of the starting and ending points, all coefficients can be uniquely determined. In this embodiment, the junctions of multiple trajectory segments must satisfy the continuity of higher-order derivatives, which cannot be solved directly at a single point; a unified global system of equality constraint equations must be constructed.

[0059] cc, Continuity condition analysis of piecewise trajectories According to the boundary term analysis theory in the variational method, the continuity order of the piecewise Minimum Snap trajectory at intermediate nodes is determined by the number of constraints set at those nodes. In this embodiment, by constraining the positions of each intermediate waypoint, the optimal trajectory obtained can achieve continuity of the trajectory up to the sixth derivative at the nodes. Specifically, at the nodes... place ( It should satisfy: CD, time normalization and matrixing Directly constructing polynomials using absolute time can easily lead to numerical instability and deterioration of the matrix condition number. Therefore, this embodiment introduces a normalized time variable to map each trajectory segment to a standard interval. Above. Define the first The duration of the segment trajectory is Local time variables Further define normalized time Then in In the domain, the first The segment trajectory is represented as: According to the chain rule, absolute time The derivatives of each order and The derivative of the domain satisfies the scaling relation. Therefore, the first... The snap cost integral of the segment trajectory is: in, This is the vector of polynomial coefficients; It is The constant matrix; through time scale transformation and coordinate mapping, the normalized coefficients can be... Convert to polynomial coefficients in absolute time The absolute time domain single-segment cost matrix is ​​obtained. .

[0060] All Summing the costs of the segments yields the global cost function: in Let be the column vector of coefficients of all segment polynomials. It is a block diagonal matrix.

[0061] ce, matrix representation of constraints After constructing the global cost function in step cd, all equality constraints are uniformly organized into a system of linear equations. This includes endpoint constraints and inter-segment continuity constraints. By combining the endpoint constraints and inter-segment continuity constraints, a total constraint matrix is ​​formed.

[0062] Specifically, endpoint constraints: for the first The starting moment of the segment trajectory Its position, velocity, acceleration, and jerk can be expressed as: Similarly, by solving for the endpoint motion state and simplifying, we obtain the single-segment constraints. .

[0063] Inter-segment continuity constraint: For the first segment Section and the Segment at public node The following conditions must be met: It can also be written as about and A linear combination of these. Solving the system of equations (endpoint constraints and inter-segment continuity constraints), we get: in It is the total constraint matrix. It is a known constant vector (including takeoff point position, velocity, acceleration, jerk, intermediate waypoint positions, landing point position, velocity, acceleration, jerk).

[0064] Transform cf into an unconstrained quadratic programming problem and solve it. The solution is obtained using the selection matrix factorization method, which includes the following steps: Minimum Snap trajectory optimization is essentially a quadratic programming problem with linear equality constraints, in its standard form as follows: , in, Represents the coefficient vector of the entire piecewise polynomial. The global cost matrix, , The components are, in order, the global constraint matrix and the constraint constant vector. To adapt to airborne embedded platforms and balance numerical stability with real-time computational performance, this embodiment directly employs the selection matrix decomposition method to complete the solution.

[0065] Introducing the selection matrix constraint vector Decompose into fixed variables and free variables Specifically, by constructing a selection matrix Make Then the original constraint becomes Substituting into the cost function, we get: in yes The left inverse (satisfies) ), It is a symmetric matrix. (The rest is missing.) Divided into blocks ,correspond Divided into blocks Then the cost function is: right By differentiating and taking the extremum condition, the optimal analytical solution for the variable can be directly derived; after back substitution, all polynomial coefficients can be calculated. This solution process is based solely on matrix operations, requires no iteration, and can quickly obtain the globally optimal initial smooth trajectory on an UAV-borne embedded platform.

[0066] CG, trajectory generation and output After obtaining the coefficients of the seventh-order polynomial for each segment, they are stitched together along the time axis to generate a complete 3D trajectory. The globally initial smooth trajectory ensures high-order continuity of position, velocity, acceleration, and jerk throughout the entire trajectory. The start and end points strictly match the preset derivatives of motion, and the trajectory accurately traverses all intermediate waypoints, with automatic smooth transitions of high-order derivatives at waypoint junctions. This trajectory serves as the baseline input for subsequent B-spline dynamic local obstacle avoidance optimization, ensuring excellent smoothness while preserving sufficient deformation margin for local adjustments. Specific simulation results are shown below. Figure 2 As shown.

[0067] d. Local obstacle avoidance trajectory optimization based on B-splines da, Construction of uniform cubic B-spline model and trajectory format conversion The global initial smooth trajectory generated in step c is converted into a uniform cubic B-spline curve to obtain the B-spline trajectory constructed from the control point sequence.

[0068] Specifically, (1) Basic definition of B-spline B-spline curves are determined by three main elements: control points, node vectors, and basis functions. The general expression for an order B spline curve is: In the formula: Indicates time The corresponding three-dimensional spatial position of the drone; For the first indivual B-order B-spline basis functions are used to characterize the influence weight of a single control point on the trajectory.

[0069] This embodiment uses uniform cubic B-splines, i.e., fixed order. The node vectors are uniformly distributed at equal time intervals, and the time step between adjacent nodes is... To facilitate numerical calculations, a locally normalized time parameter is introduced: In the node interval Within the cubic B-spline trajectory, there are only 4 consecutive control points. , , , It is decided that, after rearranging the basis functions into matrix form, a standard cubic B-spline matrix is ​​defined: Then the interval The trajectory within can be represented as: in: It is a time polynomial vector; This is the vector of local control points.

[0070] (2) Trajectory derivative models To meet the flight dynamics constraints of the UAV, continuous velocity and acceleration information of the trajectory is required. This embodiment is based on a uniform cubic B-spline matrix parameterization model, which normalizes the time... By taking the derivative, the complete kinematic expression of the trajectory can be derived: Location: speed: Acceleration: (3) Conversion of polynomial locus to B-spline locus format This embodiment employs a least-squares fitting method, using a polynomial trajectory as the fitting benchmark, to convert the Minimum snap polynomial trajectory into a uniform cubic B-spline trajectory. This conversion method can preserve the original trajectory shape and motion characteristics with high precision while giving the trajectory the ability to be independently adjusted locally.

[0071] Suppose the global polynomial reference trajectory at any time... Spatial position, velocity, and acceleration are respectively Let the uniform cubic B-spline locus to be solved be... To ensure that the B-spline trajectory perfectly matches the original optimal trajectory, a time-domain least squares fitting objective function is constructed: Add motion constraints at the start and end points to ensure that the velocities and accelerations at the beginning and end of the two trajectories are completely consistent, thus eliminating transition distortion. By combining the linear parameterization characteristics of B-splines with the unified arrangement of position fitting constraints and initial and final velocity and acceleration boundary constraints, the above constrained optimization problem can be transformed into a standard overdetermined linear equation system: In the formula: The system constraint matrix is ​​composed of uniform cubic B-spline basis functions, time differential operators, and normalized time coefficients, and can characterize the linear mapping relationship between control points and trajectory position, velocity, and acceleration. Let be the B-spline control point vector to be solved; It is a reference vector composed of the sampling position of the polynomial trajectory and the motion state of the first and last boundaries.

[0072] To address the characteristic that overdetermined equation systems have no exact solutions, this embodiment employs the least squares criterion to solve for the globally optimal control point solution: The optimal control point can be obtained by analytical solution using least squares: The B-spline trajectory constructed from the obtained control point sequence can reproduce the path shape and motion characteristics of the original polynomial optimal trajectory with high accuracy, and also has the advantage of local adjustment. The control points obtained in this stage are the unoptimized initial control points.

[0073] db, obstacle segmentation detection Based on the voxel grid map constructed in step b, and combined with real-time environmental perception data, global collision detection is performed on the converted B-spline trajectory to divide the obstacle-free and obstacle-prone sections; simultaneously, A... The path search algorithm obtains feasible guiding paths outside obstacles and derives the direction of the obstacle's repulsive force, eliminating the need to construct a global ESDF and reducing onboard computation.

[0074] Traverse all segments and control points along the entire B-spline trajectory, and perform collision detection point by point based on a preset safety distance threshold. If a control point or trajectory segment enters the danger zone, mark the segment as an obstacle segment, and record the start and end control points of the segment to complete trajectory partitioning. For each obstacle segment, use the position where the trajectory enters or exits the obstacle as the start and end nodes, and run A within the voxel grid. Search for feasible guiding paths outside the obstacles to serve as a reference benchmark for trajectory detour.

[0075] For collision control points Construct the local tangent vector of the trajectory by taking the adjacent control points before and after it. Characterizes the initial flight direction of the UAV. Select A. Using the midpoint of the guide path as a reference point, construct a path perpendicular to... The normal plane is used to solve for the perpendicular intersection point on the guide path through vector dot product, which serves as the reference for obstacle position retrieval. Starting from this perpendicular intersection point, the voxel grid is traversed along the line connecting the intersection point to the collision control point, and the grid occupancy status is queried. The critical coordinates of the grid switching from free space to obstacle-occupied area are the base points of the obstacle's outer surface. .

[0076] Connect the collision control point and the obstacle base point, and perform normalization to obtain the unit repulsive force direction vector: This vector represents the optimal direction for the trajectory to avoid obstacles. Combining the base point and the repulsion vector, the distance between the control point and the obstacle is defined as follows: In the formula: The current control point being monitored. It represents the distance of the control point relative to the obstacle.

[0077] DC, Control Point Preprocessing and Multiple Candidate Trajectory Generation (1) Control point preprocessing Based on the output trajectory segmentation results, the transformed global initial B-spline control points are partitioned: control points in obstacle-free sections are locked, while control points in obstacle sections are optimizable variables. Multiple sets of initial control point values ​​are configured based on different detour and obstacle avoidance paths, forming multiple independent initial solutions, providing a foundation for generating multiple candidate trajectories. Leveraging the local support characteristics of B-splines, subsequent iterations only adjust control points in obstacle areas, ensuring the global flight path does not deviate, and providing models and initial data for cost function construction and L-BFGS iteration.

[0078] (2) Generation of multiple candidate trajectories Based on the multiple initial control point solutions from the previous step, and combining the obstacle base points and repulsion direction, two obstacle avoidance schemes are set for each obstacle section: traveling along the original trajectory and detouring based on the reverse base points and repulsion vector. Multiple collision-free candidate B-spline trajectories are generated through scheme combination. These candidate trajectories share nodes and time steps, differing only in local control points. After independent optimization of each trajectory using L-BFGS, the optimal curve is selected based on the total cost. The simulation results of the optimized trajectory are as follows: Figure 3 As shown.

[0079] e. Construction of multi-dimensional cost function and trajectory iterative optimization solution A multi-dimensional cost function system comprising six independent sub-objectives is constructed. Using B-spline control points as the sole optimization variable, a unified global optimization objective is formed through weighted fusion. The L-BFGS quasi-Newton iterative algorithm is employed to solve this objective, achieving dynamic local obstacle avoidance and multi-constraint collaborative optimization of the trajectory. The overall optimization objective expression is: In the formula: The cost of obstacle avoidance for dynamic obstacles, For the cost of collision penalties, Cost of trajectory fitting For the cost of smoothness, Cost of end point constraint This is at the cost of dynamic feasibility.

[0080] 1. Cost of dynamic obstacle avoidance This cost is designed for dynamic obstacles such as pedestrians and mobile devices that change over time. It combines the real-time location of the obstacle with motion prediction results to calculate the real-time distance between the control point and the dynamic obstacle. Let the control point location be... The location of the obstacle is Euclidean distance between the two The cost function is constructed as follows: This cost, also known as the static obstacle bounce cost, is used to push the trajectory away from fixed obstacles such as walls and equipment. A segmented penalty model is constructed for static obstacle areas identified by the point cloud map. Control points are defined. To the obstacle Distance: In the formula: For a safe distance. The cost function is in piecewise form: Total cost: 2. Trajectory Fitting Cost This cost is used to constrain the fit between the optimized trajectory and the global reference trajectory, preventing excessive deviation of local obstacle avoidance optimization from the preset route. It defines the offset between trajectory points and reference points. Reference line forward direction unit vector The cost function is: In the formula: For vertical offset, This is a horizontal offset.

[0081] In engineering, the midpoint of a cubic B-spline interval is used to approximate the trajectory points. The trajectory points can be represented as a weighted average of adjacent control points: This cost is achieved by statistically analyzing trajectory deviations across the entire domain, forcing the trajectory to be optimized to closely resemble the original flight path, thus ensuring the consistency of the mission path.

[0082] 3. Smoothing Cost This cost optimizes trajectory acceleration and jerk, suppressing higher-order trajectory abrupt changes and jitter, thereby improving the flight stability of the UAV. The cost function is: In the formula: For trajectory acceleration, Add jerk to the trajectory. This cost is achieved by constraining the amplitudes of the second and third derivatives of the trajectory, allowing the trajectory curvature to change continuously without severe jitter, thus reducing the load on the flight control system and motors.

[0083] 4. Endpoint constraint cost This cost constraint ensures the trajectory's endpoint approximates the mission target point, guaranteeing the UAV's precise arrival at the preset destination. The endpoint position is approximated using the midpoint of a cubic B-spline interval. The cost function is: 5. Cost of Dynamic Feasibility This cost is a hard constraint on the UAV hardware, limiting the trajectory speed, acceleration, and jerk to within the rated limits of the aircraft, and employing a piecewise penalty model. The cost function is: in To represent a single axis, the piecewise penalty function for a single axis is: In the formula: As a hard upper limit for dynamics, As the segmentation point, This is the safety zone coefficient. The cost is 0 when the trajectory motion parameters are within the safety zone; a weak penalty occurs when approaching the limit; and a strong penalty occurs when exceeding the limit, strictly ensuring compliance of the dynamic parameters.

[0084] After constructing a globally unified optimization objective function, the L-BFGS quasi-Newton iterative algorithm is used to solve it. The B-spline control points are updated iteratively along the negative gradient of the cost function, and the optimal step size is determined through line search to continuously reduce the total cost. Convergence is determined when the cost change is less than the convergence threshold or the number of iterations reaches the upper limit. After all candidate trajectories are optimized, the curve with the lowest total cost is selected as the optimal trajectory. The iterative distribution of control points is compared as follows: Figure 4 As shown.

[0085] f, Trajectory dynamics verification and time reallocation The optimal B-spline trajectory obtained in step e satisfies the safety and smoothness conditions, but a fixed time step can easily cause the dynamic parameters to exceed the limits. This step maintains the control points and trajectory shape unchanged, performs high-density sampling of the trajectory, calculates the motion parameters in segments and marks the exceeding intervals, and calculates the time stretching coefficient based on the exceeding ratio to smooth the dynamic indices.

[0086] After time reallocation is completed, a full-dimensional secondary verification is performed: dynamic verification ensures motion parameters are within safe thresholds; safety verification relies on the convex hull characteristics of B-splines to re-check collision risks; morphological verification checks trajectory smoothness, path fit, and endpoint accuracy. Once verification is successful, the trajectory is output to the trajectory service module, and control commands are issued to achieve safe and stable flight of the UAV, completing the entire process of global planning, local optimization, and temporal calibration to obtain the optimal executable trajectory, such as... Figure 5 As shown.

[0087] The scope of protection claimed by this invention is not limited to the specific embodiments described above. Moreover, for those skilled in the art, this invention can have various modifications and alterations. Any modifications, improvements, and equivalent substitutions made within the concept and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios, characterized in that, Includes the following steps: a. Multi-source sensor data acquisition and initial information gathering The drone is equipped with multiple sensors, including IMU, LiDAR, and RGB-D depth camera, to collect drone motion and pose data and indoor environmental point cloud data. The multi-source data is time-stamped and coordinate-corrected, and abnormal data such as noise, outliers, and delays are removed. This completes the positioning initialization and environmental data collection in the GNSS-free scenario, ensuring the temporal consistency and spatial accuracy of the mapping input data. b. Indoor environment modeling and target point acquisition Based on the multi-source sensor data collected in step a, a three-dimensional voxel grid map of the indoor environment is constructed, and an ESDF distance field is generated in real time to calculate the distance from each location in space to the nearest obstacle. The flight target waypoints output by the task are received, and the legality of the target points is verified by combining the voxel grid map and the ESDF distance field to complete the configuration of the planning start point and end point. c, Construct the global initial smooth trajectory A global baseline smooth trajectory is generated using Minimum Snap high-order polynomials, with the optimization objective being to minimize the integral of the fourth derivative of the trajectory. A flight trajectory time vector function is constructed, and the total flight time is segmented and polynomials are constructed segment by segment. An equation set of equality constraints is established and transformed into an unconstrained quadratic programming solution through selection matrix decomposition. The solution process relies solely on matrix operations and requires no iteration. The airborne platform can quickly solve for continuous high-order polynomial trajectories, which serve as the initial benchmark for B-spline local optimization. d. Local obstacle avoidance trajectory optimization based on B-splines The global initial smooth trajectory generated in step c is converted into a uniform cubic B-spline curve to obtain the B-spline trajectory constructed from the control point sequence. Based on the voxel grid map constructed in step b, and combined with real-time environmental perception data, global collision detection is performed on the B-spline trajectory. Based on different detour obstacle avoidance paths, multiple sets of initial control point values ​​are configured to generate candidate B-spline trajectories with no collisions throughout the entire process. Each candidate B-spline trajectory shares a set of node vectors and time steps, with only local control points differing. e. Construction of multi-dimensional cost function and trajectory iterative optimization solution A multi-dimensional cost function system containing six independent sub-objective cost functions is constructed. Using B-spline control points as optimization variables, the cost functions of each independent sub-objective are weighted and fused to form a unified global optimization objective function, which is solved using the L-BFGS quasi-Newton iterative algorithm. During the iteration process, the coordinates of the B-spline control points are updated along the negative gradient direction of the cost function, and the optimal step size is determined by line search. After all candidate trajectories have converged, the total cost value of each group of candidate trajectories is compared, and the trajectory with the minimum cost is selected as the optimal B-spline trajectory. f, Trajectory dynamics verification and time reallocation The optimal B-spline trajectory obtained in step e is sampled at high density, and the velocity, acceleration and jerk are calculated segment by segment. All segments that exceed the limit are marked. The time stretching coefficient is dynamically calculated according to the proportion of exceeding the limit. The trajectory after time redistribution is verified for dynamics, safety and morphology. After the verification is qualified, the B-spline trajectory is input into the trajectory service module and executed by the flight control system.

2. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 1, characterized in that, In step a, Includes the following steps: aa, drone preparation After the drone is powered on and completes its self-test, it takes off vertically to the preset safe flight altitude. ab, collect drone motion data The IMU (Inertial Measurement Unit) is activated to collect high-frequency motion data of the UAV body, including three-axis attitude angles, angular velocity, and linear acceleration. By combining the SLAM positioning results of LiDAR and RGB-D depth camera, the global pose information of the UAV in an unknown indoor environment is calculated in real time. ac, collecting point cloud data of the indoor environment Simultaneously activate the LiDAR and RGB-D depth camera to carry out environmental perception and data acquisition. The LiDAR performs large-area environmental contour scanning at medium and long distances, while the RGB-D depth camera fills in the blind spots of close-range detection. ad, multi-sensor time synchronization and data preprocessing The IMU motion data, laser point cloud, and depth point cloud collected in steps ab and ac are uniformly timestamped and coarsely corrected to obtain fused sensing information, and noise points, invalid outliers, and delayed data are removed; positioning initialization and environmental data acquisition in the non-GNSS environment are completed to ensure the temporal consistency and spatial accuracy of the input mapping module.

3. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 1, characterized in that, Step b involves constructing a 3D voxel grid map of the indoor environment, including the following steps: The raw point cloud data is processed by filtering and denoising, outlier removal and downsampling to remove environmental noise and redundant point clouds. Based on the online SLAM mechanism, a three-dimensional voxel grid map of the indoor environment is constructed in the RVIZ visualization interface, dividing the free space and the area occupied by static obstacles, and updating the three-dimensional position and contour information of obstacles in real time.

4. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 1, characterized in that, Step c includes the following steps: ca, constructing a trajectory optimization problem We construct a vector function of the flight trajectory with respect to time, and use a single-axis trajectory as an example for mathematical description. We divide the total flight time into several segments and construct a global initial smooth trajectory stage. We ignore complex constraints and simplify it to an unconstrained standard form. cb, Derivation of the polynomial form of the optimal trajectory based on the variational method By introducing the Euler-Lagrange equation from the variational method to derive the functional extremum problem, the Lagrange function is defined and substituted into the higher-order Euler-Lagrange equation. After continuous integration, each trajectory segment can be obtained as a 7th-degree polynomial in terms of time. The unknown coefficients of this polynomial can be uniquely determined based on the position, velocity, acceleration, and jerk of the starting and ending points of each trajectory segment. cc, Continuity condition analysis of piecewise trajectories Constrain the positions of each intermediate waypoint as free variables in the optimization process; CD, time normalization and matrixing By introducing a normalized time variable, each trajectory segment is mapped onto a standard interval. Trajectory is represented by defining duration, local time variable, and normalized time, which represents the snap cost integral of each trajectory segment. The costs of all trajectory segments are summed to obtain the global cost function. ce, matrix representation of constraints All equality constraints are uniformly organized into a system of linear equations, including endpoint constraints and inter-segment continuity constraints. The endpoint constraints and inter-segment continuity constraints are combined to form a total constraint matrix. Transform cf into an unconstrained quadratic programming problem and solve it. The solution is obtained by using the selection matrix decomposition method, in which a selection matrix is ​​introduced to divide the constraint vector into fixed vectors and free vectors. The fixed vectors include the derivatives of the origin and destination and the positions of all waypoints, while the free vectors include continuity constraints. CG, trajectory generation and output After obtaining the coefficients of the seventh-degree polynomial for each segment, the complete three-dimensional trajectory is generated by splicing them along the time axis.

5. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 1, characterized in that, Step d includes the following steps: da, construct a uniform cubic B-spline model, and based on the uniform cubic B-spline matrix parameterization model, derive the complete kinematic expression of the trajectory. The Minimum Snap polynomial trajectory is transformed into a uniform cubic B-spline trajectory through least squares fitting, so that the transformed B-spline trajectory reproduces the path shape and motion characteristics of the original trajectory. Based on the voxel grid map constructed in step b, and combined with real-time environmental perception data, db performs global collision detection on the B-spline trajectory, divides it into obstacle-free and obstacle-prone sections, and traverses all segments and control points of the entire B-spline trajectory. A-type collision detection is introduced into the obstacle sections. The path search algorithm obtains feasible guiding paths outside obstacles and derives the repulsive force direction vector of control points relative to obstacles. DC, Control Point Preprocessing and Multiple Candidate Trajectory Generation Multiple sets of initial control point values ​​are configured based on different obstacle avoidance paths to form multiple independent initial solutions. Combining the obstacle base point and the repulsive force direction, the obstacle avoidance schemes in the obstacle area are arranged and combined to generate multiple collision-free candidate B-spline trajectories. The candidate trajectories share nodes and time steps, and only the local control points are different.

6. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 5, characterized in that, In step dc, the obstacle avoidance scheme includes traveling along the original trajectory and a bypass scheme based on the reverse base point and the repulsion vector.

7. The method for autonomous path planning and safe flight trajectory optimization of unmanned aerial vehicles (UAVs) suitable for indoor scenarios according to claim 1, characterized in that, In step e, the independent sub-objective cost function includes dynamic obstacle avoidance cost, collision penalty cost, trajectory fitting cost, smoothness cost, endpoint constraint cost, and dynamic feasibility cost.

Images