A method and system for controlling a drone based on point cloud data to generate three-dimensional data

Abstract

The present application relates to the technical field of unmanned aerial vehicle control, and particularly relates to an unmanned aerial vehicle control method and system based on point cloud data to generate three-dimensional data, wherein a constraint hyperplane is directly extracted from a local point cloud subset and is summarized to form a local geometric constraint manifold, the constraint hyperplane in the manifold is converted into an inequality constraint and is directly embedded into a prediction time domain state evolution equation, a complete three-dimensional map construction and an independent path planning link are skipped, a link length from point cloud collection to control instruction generation is significantly shortened, and time delay accumulation caused by intermediate calculation steps is reduced. The system time delay is reduced, the control response speed in a dynamic environment is improved, the on-board calculation burden is reduced, the coupling of environmental constraints and dynamic constraints is realized, the problem of disconnection between a planning layer and a control layer is solved, the dynamic environment response capability is improved, and mathematical level safety guarantee is provided.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) control technology, and specifically to a UAV control method and system for generating 3D data based on point cloud data. Background Technology

[0002] In the field of autonomous flight control for unmanned aerial vehicles (UAVs), environmental perception and navigation control based on point cloud data is one of the key technologies. A typical approach involves a sequential link: point cloud acquisition, 3D map construction, path planning, and tracking control. First, onboard sensors acquire environmental point cloud data, which is then fused from multiple sources to construct a global or local 3D grid map or octree map. Based on this map, global path planning and local trajectory replanning are performed, and finally, the underlying tracking controller executes the planned trajectory. However, from raw point cloud acquisition to 3D map construction, path planning, and finally, control command generation, each stage involves computational delays. In dynamic and unknown environments, obstacle distribution may change within this time lag window, causing control decisions to be based on outdated environmental information, thus reducing flight safety. Summary of the Invention

[0003] In view of the above problems, the present invention provides a method and system for controlling unmanned aerial vehicles (UAVs) based on point cloud data to generate 3D data.

[0004] To achieve the above objectives, in a first aspect, the present invention provides a method for controlling a drone based on point cloud data to generate 3D data, comprising:

[0005] Acquire the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system, and extract a local point cloud subset;

[0006] Clustering and segmentation of local point cloud subsets are performed to identify point cloud clusters on continuous obstacle surfaces. Principal component analysis is used to extract the local surface normal vector of each point cloud cluster. Constraint hyperplanes are established by combining the shortest distance from the UAV centroid to the point cloud cluster. All constraint hyperplanes in the current frame are summarized to form a local geometric constraint manifold.

[0007] A six-degree-of-freedom nonlinear dynamic model of the UAV is obtained. The constraint hyperplane in the local geometrically constrained manifold is transformed into an inequality constraint with respect to the position variable. This constraint is then uniformly embedded into the prediction time-domain state evolution equation along with the thrust amplitude, angular velocity saturation, and attitude angle constraints, forming a constrained discrete-time prediction dynamic model.

[0008] Based on a constrained discrete-time predictive dynamics model, an optimal control problem is established with tracking deviation term, control smoothing term, and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization.

[0009] The first term of the optimal control input sequence is output as the actual control command to the actuator. The constraint hyperplane parameters of the previous frame are used between adjacent point cloud frames, combined with the velocity estimation obtained from the attitude position data, to perform linear extrapolation to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with the newly extracted local geometric constraint manifold.

[0010] At each point cloud frame update, the intersection of the current local geometrically constrained manifold and the UAV's dynamic reachable set is calculated. If the intersection is not empty, normal execution is performed. If the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated.

[0011] In some embodiments, a subset of local point clouds is clustered and segmented to identify point cloud clusters on the surface of continuous obstacles. The local surface normal vector of each point cloud cluster is extracted using principal component analysis. A constraint hyperplane is established by combining the shortest distance from the UAV's centroid to the point cloud cluster. All constraint hyperplanes in the current frame are then aggregated to form a local geometric constraint manifold, including:

[0012] Euclidean distance clustering is performed on local point cloud subsets to obtain multiple point cloud clusters;

[0013] Principal component analysis is performed on each point cloud cluster to extract the local surface normal vector of the point cloud cluster, and the shortest Euclidean distance from the UAV's centroid to the point cloud cluster is calculated.

[0014] Based on the local surface normal vector and the shortest Euclidean distance, a constraint hyperplane corresponding to the point cloud cluster is established. The constraint hyperplane represents the geometric boundary in which the UAV can move safely in this direction.

[0015] Obtain the positional difference of the same obstacle point cloud cluster in two consecutive frames of point cloud data, and estimate the relative velocity of the obstacle;

[0016] Based on relative velocity, motion compensation is performed on the constraint hyperplane corresponding to dynamic obstacles to generate a dynamic constraint hyperplane with velocity offset;

[0017] The constraint hyperplanes corresponding to all static obstacles in the current frame and the dynamic constraint hyperplanes corresponding to dynamic obstacles are combined to form a local geometric constraint manifold containing both static and dynamic information.

[0018] In some embodiments, a six-degree-of-freedom nonlinear dynamic model of the UAV is obtained, and the constraint hyperplane in the locally geometrically constrained manifold is transformed into inequality constraints with respect to position variables. These constraints are then uniformly embedded with thrust amplitude, angular velocity saturation, and attitude angle limits into the prediction time-domain state evolution equation, forming a constrained discrete-time prediction dynamic model, including:

[0019] Obtain a six-degree-of-freedom nonlinear rigid body dynamics model of the UAV, which includes the mechanical equations for translational motion of the center of mass and the dynamic equations for rotation about the center of mass.

[0020] The static constraint hyperplane in the locally geometrically constrained manifold is transformed into a static inequality constraint on the UAV's position and state variables. The parameters of the static inequality constraint are constant in the prediction time domain.

[0021] The dynamic constraint hyperplane in the local geometrically constrained manifold is transformed into a dynamic inequality constraint on the UAV's position and state variables. The distance parameter of the dynamic inequality constraint changes with the time step in the prediction time domain, and the rate of change is determined by the relative velocity carried by the dynamic constraint hyperplane.

[0022] Obtain the thrust amplitude limit, angular velocity saturation limit, and attitude angle limit of the UAV, and transform them into ontological inequality constraints regarding control inputs and state variables;

[0023] Static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are uniformly embedded into the predictive time-domain state evolution equation established based on a preset discretized time step, which is a constrained discrete-time predictive dynamic model.

[0024] In some embodiments, based on a constrained discrete-time predictive dynamics model, an optimal control problem is established, with tracking deviation, control smoothing, and energy consumption terms as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization, including:

[0025] Based on a constrained discrete-time predictive dynamics model, a cost function for the finite-time optimal control problem is established. The cost function includes a deviation term for tracking a preset guiding trajectory, a smoothing term for the rate of change of control input, and a propulsion energy consumption term.

[0026] Static and dynamic inequality constraints in locally geometrically constrained manifolds are used as environmental safety boundary constraints for optimal control problems.

[0027] The volumetric inequality constraints corresponding to thrust amplitude limitation, angular velocity saturation limitation, and attitude angle limitation are used as the dynamic state and input constraints of the optimal control problem.

[0028] Obtain the desired state of the preset guidance trajectory at the predicted end point in the time domain. Based on the deviation between the desired state and the predicted terminal state, establish terminal stability constraints and use the terminal stability constraints as the terminal constraints of the optimal control problem.

[0029] The cost function, environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints together constitute a complete mathematical expression of the optimal control problem.

[0030] The optimal control problem is solved by sequential quadratic programming or interior-point numerical optimization to obtain the optimal control input sequence in the prediction time domain.

[0031] In some embodiments, the equations of motion for translation of the center of mass and the equations of motion for rotation about the center of mass are expressed by formula (1), which is as follows:

[0032] ;

[0033] In formula (1), It is the position vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the velocity vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the total mass of the drone. It is the total thrust vector generated by the drone's motors, expressed in the body coordinate system. It is the gravitational acceleration vector. , It is the attitude angle vector of the UAV. for The first derivative, For roll angle, The pitch angle, Yaw angle This is the transformation matrix between the rate of change of attitude angle and the angular velocity of the aircraft. , This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let be the inertial tensor matrix of the UAV in the body coordinate system. The total torque vector generated by the drone's motors is represented in the body coordinate system.

[0034] Static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are expressed by formula (2), which is as follows:

[0035] ;

[0036] In formula (2), yes transpose, It is the first The unit normal vector of a statically constrained hyperplane points in the direction in which the UAV can move. The centroid of the UAV at discrete time step The world coordinate system position vector It is the first The distance parameter of each statically constrained hyperplane is a constant. For the set of statically constrained hyperplane indices, To predict the total number of steps in the time domain, yes transpose, It is the first The unit normal vector of the dynamically constrained hyperplane points in the direction in which the UAV can move. It is the first A dynamically constrained hyperplane at discrete time step The distance parameter changes with time step. It is a set of dynamically constrained hyperplane indices. Discrete time step The thrust vector, This refers to the minimum thrust amplitude that the drone's motor can provide. The maximum thrust amplitude that the drone motor can provide. Discrete time step The three axial angular velocity components in the body coordinate system This represents the maximum permissible angular velocity amplitude for the drone. The maximum allowable roll angle for the drone. The maximum allowable pitch angle for the drone. To predict the total number of steps in the time domain;

[0037] The constrained discrete-time prediction dynamics model is represented by Equation (3), which is as follows:

[0038] ;

[0039] In formula (3), , Discrete time step The state vector includes position, velocity, attitude angle, and angular velocity. , Discrete time step The control input vector includes thrust and torque. The state evolution function is obtained by discretizing formula (1) based on the fourth-order Runge-Kutta method;

[0040] The cost function is expressed by formula (4), which is as follows:

[0041] ;

[0042] In formula (4), Let cost function be Discrete time step The preset guiding trajectory state vector. This is the state tracking deviation weight matrix. To control the smoothing weight matrix of the input rate of change, To advance the energy consumption weighting matrix, This is the terminal cost weight matrix;

[0043] The complete mathematical expression is represented by formula (5), which is as follows:

[0044] ;

[0045] In formula (5), To predict the control input sequence in the time domain, This is the allowable threshold for terminal deviation.

[0046] In some embodiments, the first term of the optimal control input sequence is output as the actual control command to the actuator. Linear extrapolation is performed between adjacent point cloud frames using the constraint hyperplane parameters of the previous frame, combined with the velocity estimate obtained from the attitude position data, to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with a newly extracted local geometric constraint manifold, including:

[0047] The first term of the optimal control input sequence is used as the actual control command for the current control cycle and output to the UAV motor electronic speed controller and flight control actuator.

[0048] Between adjacent point cloud frames, obtain the normal vector and distance parameters of the static constraint hyperplane extracted in the previous frame;

[0049] Based on the UAV velocity estimate obtained from attitude position data, the distance parameters of the static constraint hyperplane are linearly extrapolated and predicted to generate the extrapolated static constraint hyperplane.

[0050] Obtain the normal vector, distance parameters, and relative velocity carried by the dynamic constraint hyperplane extracted from the previous frame;

[0051] Based on relative velocity, kinematic extrapolation is performed on the distance parameters of the dynamically constrained hyperplane to generate the externally driven constrained hyperplane;

[0052] The extrapolated static constraint hyperplane and the extrapolated dynamic constraint hyperplane are combined to form the extrapolated local geometric constraint manifold, which is used to maintain the continuity of environmental constraints within the control period between adjacent point cloud frames;

[0053] When a new point cloud frame arrives, the newly extracted local geometric constraint manifold is obtained and replaced with the currently used extrapolated local geometric constraint manifold to complete the constraint update.

[0054] In some embodiments, based on relative velocity, kinematic extrapolation prediction is performed on the distance parameters of the dynamically constrained hyperplane to generate an extrapolated dynamically constrained hyperplane, including:

[0055] Obtain the dynamic constraint hyperplane extracted from the previous frame. The dynamic constraint hyperplane contains the normal vector, the initial distance parameter, and the relative velocity vector it carries.

[0056] Based on the relative velocity vector, the distance change of the dynamic constraint hyperplane at each control step in the prediction time domain is calculated. The distance change is the product of the projection component of the relative velocity vector in the direction of the normal vector and the time step.

[0057] The initial distance parameters and the distance changes are accumulated step by step over time to generate the extrapolated distance parameter sequence of the dynamic constraint hyperplane in each control period between adjacent point cloud frames.

[0058] Based on the sequence of normal vector and extrapolated distance parameters, generate the extrapolated dynamic constraint hyperplane corresponding to each control cycle;

[0059] The externally driven state constraint hyperplanes corresponding to all control cycles are summarized to form a set of externally driven state constraint hyperplanes between adjacent point cloud frames.

[0060] In some embodiments, at each point cloud frame update time, the intersection of the current local geometrically constrained manifold and the UAV dynamic reachable set is calculated. If the intersection is not empty, normal execution is performed; if the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated, including:

[0061] At each point cloud frame update time, the newly extracted local geometric constraint manifold is obtained. The local geometric constraint manifold includes all static constraint hyperplanes and dynamic constraint hyperplanes in the current frame.

[0062] Based on the six-degree-of-freedom nonlinear dynamics model of the UAV and its current state, the dynamic reachable set of the UAV in the next prediction time domain is calculated. The dynamic reachable set represents the spatial set of all reachable states of the UAV under the constraints of thrust amplitude, angular velocity saturation and attitude angle.

[0063] Calculate the intersection of the locally geometrically constrained manifold and the dynamically reachable set, and determine whether the intersection is an empty set;

[0064] If the intersection is not empty, it indicates that there exists a control solution that simultaneously satisfies environmental safety constraints and dynamic feasibility. The system maintains normal execution mode and continues to execute subsequent control cycles.

[0065] If the intersection is empty, it indicates that there is no feasible control solution under the current environmental constraints, triggering the safety braking mode, and the UAV performs deceleration and hovering according to the current velocity vector direction;

[0066] In the safety braking mode, the new local geometric constraint manifold corresponding to the new arrival point cloud frame is continuously monitored, and the intersection of the new local geometric constraint manifold and the current dynamic reachable set is repeatedly calculated;

[0067] When the intersection becomes non-empty again, exit the safety braking mode and resume the normal predictive control execution process.

[0068] In some embodiments, calculating the intersection of the locally geometrically constrained manifold and the dynamically reachable set, and determining whether the intersection is an empty set, includes:

[0069] Obtain the local geometrically constrained manifold, which consists of multiple constrained hyperplanes. Each constrained hyperplane is determined by the normal vector and the distance parameter.

[0070] Each constraint hyperplane is transformed into a linear inequality constraint with respect to the UAV's position and state variables. The geometric meaning of the linear inequality constraint is that the UAV's centroid lies within the half-space defined by the constraint hyperplane.

[0071] By summing up all the linear inequality constraints, a feasible state space polyhedron corresponding to the local geometrically constrained manifold is formed.

[0072] Obtain the dynamic reachability set, which is the set of reachable state spaces obtained by forward reachability analysis of the UAV's six-degree-of-freedom nonlinear dynamic model under the current state and input constraints;

[0073] The intersection region is obtained by performing convex hull intersection calculation on the feasible state space polyhedron and the dynamic reachable set;

[0074] To determine whether the intersection region is an empty set, if the intersection region contains at least one state point, then the intersection is determined to be non-empty; if the intersection region does not contain any state points, then the intersection is determined to be an empty set.

[0075] In a second aspect, the present invention also provides a UAV control system for generating 3D data based on point cloud data, for executing the method described in the first aspect, the system comprising:

[0076] The point cloud preprocessing module is used to acquire the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system, and extract a local point cloud subset.

[0077] The local constraint extraction module is used to cluster and segment local point cloud subsets, identify point cloud clusters on continuous obstacle surfaces, extract the local surface normal vector of each point cloud cluster through principal component analysis, establish constraint hyperplanes by combining the shortest distance from the UAV centroid to the point cloud cluster, and summarize all constraint hyperplanes in the current frame to form a local geometric constraint manifold.

[0078] The constrained dynamics construction module is used to obtain a six-degree-of-freedom nonlinear dynamic model of the UAV. It transforms the constrained hyperplane in the local geometrically constrained manifold into inequality constraints with respect to position variables, and embeds them into the prediction time-domain state evolution equation along with thrust amplitude, angular velocity saturation and attitude angle constraints to form a constrained discrete-time prediction dynamic model.

[0079] The predictive control solution module is used to establish an optimal control problem based on a constrained discrete-time predictive dynamics model. The optimal control input sequence is obtained by numerical optimization, with tracking deviation term, control smoothing term and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints and terminal stability constraints determined by the local geometric constraint manifold.

[0080] The instruction execution and asynchronous compensation module is used to output the first term of the optimal control input sequence as the actual control instruction to the actuator. It uses the constraint hyperplane parameters of the previous frame and the velocity estimation obtained from the attitude position data to perform linear extrapolation between adjacent point cloud frames to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with the newly extracted local geometric constraint manifold.

[0081] The safety verification and braking module is used to calculate the intersection of the current local geometric constraint manifold and the UAV dynamic reachable set at each point cloud frame update. If the intersection is not empty, it will execute normally; if the intersection is empty, it will trigger the safety braking mode to decelerate and hover until the intersection is regenerated.

[0082] Unlike existing technologies, the above-mentioned technical solution directly extracts constraint hyperplanes from local point cloud subsets and summarizes them to form a local geometric constraint manifold. The constraint hyperplanes in this manifold are then transformed into inequality constraints and directly embedded into the prediction time-domain state evolution equation. This skips the complete 3D map construction and independent path planning stages, significantly shortening the link length from point cloud acquisition to control command generation and reducing the accumulated time delay caused by intermediate calculation steps. This shortens the perception-to-control link length, reduces system time delay, and improves control response speed in dynamic environments; avoids excessive computation in complete 3D map construction, reducing the onboard computing burden; achieves unified coupling of environmental constraints and dynamic constraints, solving the problem of disconnect between the planning layer and the control layer; improves dynamic environment response capability through the asynchronous extrapolation mechanism of constraint hyperplanes between point cloud frames; and provides mathematical security guarantees through the intersection verification of the local geometric constraint manifold and the dynamic reachable set.

[0083] The above description of the invention is merely an overview of the technical solution of this application. In order to enable those skilled in the art to better understand the technical solution of this application and to implement it based on the description and drawings, and to make the above-mentioned objectives and other objectives, features and advantages of this application easier to understand, the following description is provided in conjunction with the specific embodiments and drawings of this application. Attached Figure Description

[0084] The accompanying drawings are only used to illustrate the principles, implementation methods, applications, features, and effects of specific embodiments of the present invention and other related contents, and should not be considered as limitations on this application.

[0085] In the accompanying drawings of the instruction manual:

[0086] Figure 1 This is a schematic diagram illustrating steps S101 to S106 as described in the specific implementation method.

[0087] Figure 2 This is a schematic diagram illustrating steps S201 to S206 as described in the specific implementation method;

[0088] Figure 3 This is a schematic diagram illustrating steps S301 to S305 as described in the specific implementation method.

[0089] Figure 4 This is a schematic diagram illustrating steps S401 to S406 as described in the specific implementation method.

[0090] Figure 5 This is a schematic diagram of the modules of the system described in a specific implementation.

[0091] The reference numerals used in the above figures are explained as follows:

[0092] 1. Unmanned aerial vehicle (UAV) control system;

[0093] 11. Point cloud preprocessing module;

[0094] 12. Local constraint extraction module;

[0095] 13. Constraint Dynamics Construction Module;

[0096] 14. Predictive Control Solver Module;

[0097] 15. Instruction execution and asynchronous compensation module;

[0098] 16. Safety verification and braking module. Detailed Implementation

[0099] To illustrate the possible application scenarios, technical principles, implementable specific solutions, and achievable objectives and effects of this application in detail, the following description, in conjunction with the listed specific embodiments and accompanying drawings, provides a detailed explanation. The embodiments described herein are merely illustrative of the technical solutions of this application and are therefore intended to limit the scope of protection of this application.

[0100] In this document, the term "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The term "embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment, nor does it specifically limit its independence or connection with other embodiments. In principle, in this application, as long as there are no technical contradictions or conflicts, the technical features mentioned in each embodiment can be combined in any way to form corresponding implementable technical solutions.

[0101] Unless otherwise defined, the technical terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains; the use of related terms herein is merely for the purpose of describing particular embodiments and is not intended to limit this application.

[0102] Without further limitations, the use of terms such as “comprising,” “including,” “having,” or other similar open-ended expressions in this application is intended to cover non-exclusive inclusion, which does not exclude the presence of additional elements in a process, method, or product that includes the stated elements, such that a process, method, or product that includes a list of elements may include not only those defined elements but also other elements not expressly listed, or elements inherent to such a process, method, or product.

[0103] Please see Figure 1 In a first aspect, embodiments of the present invention provide a method for controlling a drone based on point cloud data to generate 3D data, comprising:

[0104] S101. Obtain the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system and extract a local point cloud subset.

[0105] S102. Cluster and segment the local point cloud subsets to identify point cloud clusters on the surface of continuous obstacles. Extract the local surface normal vector of each point cloud cluster through principal component analysis. Combine the shortest distance from the UAV centroid to the point cloud cluster to establish a constraint hyperplane. Summarize all constraint hyperplanes in the current frame to form a local geometric constraint manifold.

[0106] S103. Obtain the six-degree-of-freedom nonlinear dynamic model of the UAV, transform the constrained hyperplane in the local geometrically constrained manifold into inequality constraints on position variables, and embed them together with thrust amplitude, angular velocity saturation and attitude angle constraints into the prediction time domain state evolution equation to form a constrained discrete-time prediction dynamic model.

[0107] S104. Based on a constrained discrete-time predictive dynamics model, an optimal control problem is established with tracking deviation term, control smoothing term, and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization.

[0108] S105. The first term of the optimal control input sequence is output as the actual control command to the actuator. The constraint hyperplane parameters of the previous frame are used between adjacent point cloud frames. Combined with the velocity estimation obtained from the attitude position data, linear extrapolation is performed to maintain the continuity of the constraint. When a new point cloud frame arrives, the extrapolated manifold is replaced with the newly extracted local geometric constraint manifold.

[0109] S106. At each point cloud frame update time, calculate the intersection of the current local geometric constraint manifold and the UAV dynamic reachable set. If the intersection is not empty, execute normally. If the intersection is empty, trigger the safety braking mode to decelerate and hover until the intersection is regenerated.

[0110] In step S101, the original point cloud data frame and synchronized attitude and position data are acquired. The point cloud data frame is transformed to the body coordinate system, and a local point cloud subset is extracted. The attitude and position data is output by the inertial measurement unit and used to construct the coordinate system transformation matrix. The local point cloud subset is extracted with the UAV's center of mass as the center and a preset neighborhood radius as the boundary. This radius is calibrated based on the detection distance and flight speed.

[0111] In step S102, the local point cloud subset is clustered and segmented to identify point cloud clusters on the surface of continuous obstacles. Principal component analysis is used to extract the local surface normal vector of each point cloud cluster. A constraint hyperplane is established by combining the shortest distance from the UAV's centroid to the point cloud cluster. All constraint hyperplanes in the current frame are then combined to form a local geometric constraint manifold. The clustering and segmentation uses the Euclidean distance thresholding method, and the eigenvector corresponding to the smallest eigenvalue in the principal component analysis is the normal vector. The constraint hyperplane is determined by the normal vector and the shortest distance, representing the safe motion boundary.

[0112] In step S103, a six-degree-of-freedom nonlinear dynamic model of the UAV is obtained. The constraint hyperplane in the locally geometrically constrained manifold is transformed into inequality constraints with respect to position variables. These constraints, along with thrust amplitude, angular velocity saturation, and attitude angle limits, are uniformly embedded into the predictive time-domain state evolution equation, forming a constrained discrete-time predictive dynamic model. The constraint hyperplane is transformed in the form that the dot product of the normal vector and the position variables is not less than the distance parameter. Thrust amplitude, angular velocity saturation, and attitude angle limits are all added to the state evolution equation in the form of inequalities. The mathematical unification of the environmental boundary and the dynamic boundary is achieved through discretization.

[0113] In step S104, based on the constrained discrete-time predictive dynamics model, an optimal control problem is established with tracking deviation, control smoothing, and energy consumption terms as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints as conditions. The optimal control input sequence is obtained through numerical optimization. The tracking deviation term measures the deviation between the state and the guided trajectory, the control smoothing term suppresses control input fluctuations, and the energy consumption term reduces power consumption. The constraints are derived from local geometric constraints (manifold), body dynamics limitations, and terminal deviation tolerance thresholds, respectively.

[0114] In step S105, the first term of the optimal control input sequence is output as the actual control command to the actuator. Linear extrapolation is performed between adjacent point cloud frames using the constraint hyperplane parameters from the previous frame, combined with velocity estimation, to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with a newly extracted local geometric constraint manifold. The static constraint hyperplane is linearly extrapolated based on the projection of the velocity estimation onto the normal, while the dynamic constraint hyperplane is kinematically extrapolated based on the projection of the relative velocity onto the normal. The extrapolation results are summarized into the extrapolated local geometric constraint manifold.

[0115] In step S106, at each point cloud frame update, the intersection of the current local geometric constraint manifold and the UAV's dynamic reachable set is calculated. If the intersection is not empty, normal execution proceeds; if the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated. The intersection calculation is achieved by transforming the constraint hyperplane into a feasible state space polyhedron and performing a convex hull intersection with the dynamic reachable set. When the intersection is empty, a safety braking mode is triggered, and new point cloud frames are continuously monitored until the intersection becomes non-empty again.

[0116] This embodiment uses a locally geometrically constrained manifold as a direct coupling bridge between environmental perception and dynamic control, skipping the complete 3D map construction and independent path planning stages, shortening the link length from perception to control, and reducing system latency. Simultaneously, constraint continuity is maintained through the extrapolation mechanism of the constrained hyperplane between point cloud frames, and safety is guaranteed through the intersection verification of the locally geometrically constrained manifold and the dynamically reachable set. Thus, direct coupling from point cloud acquisition to control command generation is achieved in a dynamic and unknown environment, improving flight safety and control response speed.

[0117] Please see Figure 2 In some embodiments, local point cloud subsets are clustered and segmented to identify point cloud clusters on continuous obstacle surfaces. Principal component analysis is used to extract the local surface normal vectors of each point cloud cluster. Constraint hyperplanes are then established based on the shortest distance from the UAV's centroid to the point cloud cluster. All constraint hyperplanes in the current frame are then aggregated to form a local geometric constraint manifold, including:

[0118] S201. Perform Euclidean distance clustering on the local point cloud subsets to obtain multiple point cloud clusters;

[0119] S202. Perform principal component analysis on each point cloud cluster, extract the local surface normal vector of the point cloud cluster, and calculate the shortest Euclidean distance from the centroid of the UAV to the point cloud cluster.

[0120] S203. Based on the local surface normal vector and the shortest Euclidean distance, establish the constraint hyperplane corresponding to the point cloud cluster. The constraint hyperplane represents the geometric boundary in which the UAV can move safely in this direction.

[0121] S204. Obtain the positional difference of the same obstacle point cloud cluster in two consecutive frames of point cloud data, and estimate the relative velocity of the obstacle.

[0122] S205. Based on relative velocity, perform motion compensation on the constraint hyperplane corresponding to the dynamic obstacle to generate a dynamic constraint hyperplane with velocity offset.

[0123] S206. Summarize the constraint hyperplanes corresponding to all static obstacles in the current frame and the dynamic constraint hyperplanes corresponding to dynamic obstacles to form a local geometric constraint manifold containing dynamic and static information.

[0124] In step S201, the local point cloud subset is segmented by Euclidean distance clustering to obtain multiple point cloud clusters. The clustering segmentation is based on a preset spatial distance threshold. When the Euclidean distance between two point cloud points is less than the threshold, they are determined to belong to the same obstacle surface. This threshold is calibrated based on the point cloud density and the surface roughness of the obstacle.

[0125] In step S202, principal component analysis is performed on each point cloud cluster to extract the local surface normal vector of the point cloud cluster, and the shortest Euclidean distance from the UAV's centroid to the point cloud cluster is calculated. The normal vector is determined by the eigenvector corresponding to the smallest eigenvalue of the covariance matrix, and the shortest Euclidean distance is obtained by taking the minimum value of the distance between the centroid and all points in the point cloud cluster.

[0126] In step S203, a constraint hyperplane corresponding to the point cloud cluster is established based on the local surface normal vector and the shortest Euclidean distance. The constraint hyperplane represents the geometric boundary through which the UAV can move safely in that direction. The constraint hyperplane is determined by the normal vector and the distance parameter, and its geometric meaning is that the UAV's center of mass must be located in a half-space with the normal vector as the positive direction.

[0127] In step S204, the positional difference of the same obstacle point cloud cluster in two consecutive frames of point cloud data is obtained, and the relative velocity of the obstacle is estimated. Point cloud cluster pairs of the same obstacle are determined by nearest neighbor matching, and the relative velocity vector is obtained by calculating the centroid coordinate difference and dividing it by the frame interval time.

[0128] In step S205, motion compensation is performed on the constraint hyperplane corresponding to the dynamic obstacle based on the relative velocity to generate a dynamic constraint hyperplane with velocity offset. The distance parameter of the dynamic constraint hyperplane changes with time step in the prediction time domain, and the rate of change is determined by the projection component of the relative velocity in the normal vector direction.

[0129] In step S206, the constraint hyperplanes corresponding to all static obstacles and the dynamic constraint hyperplanes corresponding to dynamic obstacles in the current frame are summarized to form a local geometric constraint manifold containing both static and dynamic information. The static constraint hyperplane and the dynamic constraint hyperplane together constitute a complete safety boundary description of the UAV's surrounding environment in the current frame, thereby considering the influence of both the static and dynamic environments on the UAV's motion in subsequent predictive control optimization problems.

[0130] This embodiment extracts static and dynamic constraint hyperplanes from a subset of local point clouds and combines them into a local geometrically constrained manifold containing both static and dynamic information. The static constraint hyperplane provides a fixed safety boundary for stationary obstacles, while the dynamic constraint hyperplane achieves a time-varying safety boundary through relative velocity compensation. This allows subsequent predictive control optimization problems to simultaneously consider the constraints of both stationary and moving obstacles on the UAV's flight trajectory, thereby improving the UAV's obstacle avoidance capability and flight safety in complex environments containing dynamic obstacles.

[0131] Please see Figure 3 In some embodiments, a six-degree-of-freedom nonlinear dynamic model of the UAV is obtained, and the constraint hyperplane in the locally geometrically constrained manifold is transformed into inequality constraints with respect to position variables. These constraints are then uniformly embedded with thrust amplitude, angular velocity saturation, and attitude angle limits into the predictive time-domain state evolution equation, forming a constrained discrete-time predictive dynamic model, including:

[0132] S301. Obtain a six-degree-of-freedom nonlinear rigid body dynamics model of the UAV, which includes the mechanical equations for translational motion of the center of mass and the dynamic equations for rotation about the center of mass.

[0133] S302. The static constraint hyperplane in the local geometric constraint manifold is transformed into a static inequality constraint on the UAV position and state variables. The parameters of the static inequality constraint are constant in the prediction time domain.

[0134] S303. The dynamic constraint hyperplane in the local geometric constraint manifold is transformed into a dynamic inequality constraint on the UAV position and state variables. The distance parameter of the dynamic inequality constraint changes with the time step in the prediction time domain, and the rate of change is determined by the relative velocity carried by the dynamic constraint hyperplane.

[0135] S304. Obtain the thrust amplitude limit, angular velocity saturation limit, and attitude angle limit of the UAV, and convert them into ontological inequality constraints regarding the control input and state variables;

[0136] S305. Static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are uniformly embedded into the prediction time-domain state evolution equation established based on a preset discretized time step, i.e., a constrained discrete-time prediction dynamic model.

[0137] In step S301, a six-degree-of-freedom nonlinear rigid body dynamics model of the UAV is obtained. The model includes the translational dynamics equations of the center of mass and the rotational dynamics equations of the center of mass. The translational dynamics equations describe the changes in position and velocity under the action of thrust and gravity, while the rotational dynamics equations describe the changes in attitude and angular velocity under the action of torque and inertia tensor.

[0138] In step S302, the static constraint hyperplane in the local geometric constraint manifold is transformed into a static inequality constraint with respect to the UAV's position and state variables. The parameters of the static inequality constraint are constants in the prediction time domain. The static constraint hyperplane originates from a stationary obstacle, and its normal vector and distance parameter remain fixed. The transformed inequality constraint is expressed in the form that the dot product of the normal vector and the position variable is not less than the distance parameter.

[0139] In step S303, the dynamic constraint hyperplane in the local geometric constraint manifold is transformed into a dynamic inequality constraint with respect to the UAV's position and state variables. The distance parameter of the dynamic inequality constraint changes with time steps in the prediction time domain, and the rate of change is determined by the relative velocity carried by the dynamic constraint hyperplane. The dynamic constraint hyperplane originates from moving obstacles, and its distance parameter is updated step-by-step according to the projection of the relative velocity in the normal direction.

[0140] In step S304, the thrust amplitude limit, angular velocity saturation limit, and attitude angle limit of the UAV are obtained and transformed into ontological inequality constraints regarding the control input and state variables. The thrust amplitude limit is expressed as a thrust vector norm between the minimum and maximum thrust; the angular velocity saturation limit is expressed as the absolute value of the angular velocity in each axis not exceeding the maximum angular velocity; and the attitude angle limit is expressed as the absolute values ​​of the roll angle and pitch angle not exceeding the corresponding maximum angle.

[0141] In step S305, static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are uniformly embedded into the prediction time-domain state evolution equation established based on a preset discretized time step, i.e., a constrained discrete-time prediction dynamics model. The discretized state evolution equation is obtained by time discretizing the continuous dynamics model. All inequality constraints are embedded in the form of additional conditions, ensuring that the state evolution at each time step in the prediction time domain must simultaneously satisfy the constraints of the environmental safety boundary and the ontological dynamics boundary.

[0142] In this embodiment, the static constraint hyperplane and the dynamic constraint hyperplane are transformed into inequality constraints with constant parameters and time-varying parameters, respectively. These constraints are then uniformly embedded into the discretized state evolution equation along with the ontological inequality constraints corresponding to thrust amplitude, angular velocity saturation, and attitude angle limits. This forms a prediction model that directly couples the environmental safety boundary and the dynamic reachability boundary, thereby avoiding the problem of difficulty in unifying the safety boundary and reachability boundary caused by the disconnect between the planning layer and the control layer.

[0143] Please see Figure 4 In some embodiments, based on a constrained discrete-time predictive dynamics model, an optimal control problem is established with tracking deviation, control smoothing, and energy consumption terms as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization, including:

[0144] S401. Based on the constrained discrete-time predictive dynamics model, establish the cost function for the finite-time optimal control problem. The cost function includes a deviation term for tracking the preset guiding trajectory, a smoothing term for the rate of change of the control input, and a propulsion energy consumption term.

[0145] S402. Treat the static and dynamic inequality constraints in the locally geometrically constrained manifold as environmental safety boundary constraints for the optimal control problem.

[0146] S403. The body inequality constraints corresponding to the thrust amplitude limit, the body inequality constraints corresponding to the angular velocity saturation limit, and the body inequality constraints corresponding to the attitude angle limit are used as the dynamic state and input constraints of the optimal control problem.

[0147] S404. Obtain the desired state of the preset guidance trajectory at the end of the predicted time domain. Based on the deviation between the desired state and the predicted terminal state, establish terminal stability constraints and use the terminal stability constraints as the terminal constraint conditions of the optimal control problem.

[0148] S405. The cost function, environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints together constitute a complete mathematical expression of the optimal control problem.

[0149] S406. Solve the optimal control problem using sequential quadratic programming or interior-point numerical optimization methods to obtain the optimal control input sequence in the prediction time domain.

[0150] In step S401, based on the constrained discrete-time predictive dynamics model, a cost function for the finite-time optimal control problem is established. The cost function includes a deviation term for tracking the preset guide trajectory, a smoothing term for the rate of change of the control input, and a propulsion energy consumption term. The tracking deviation term is realized by the weighted sum of squared deviations between the state vector and the guide trajectory; the control smoothing term is realized by the weighted sum of squared changes in adjacent control inputs; and the energy consumption term is realized by the weighted sum of squared control inputs.

[0151] In step S402, the static and dynamic inequality constraints in the local geometrically constrained manifold are used as environmental safety boundary constraints for the optimal control problem. The static constraint parameters are constant, while the dynamic constraint distance parameters are time-varying. Together, they constitute the spatial range constraint that the UAV position must satisfy in the prediction time domain.

[0152] In step S403, the ontological inequality constraints corresponding to thrust amplitude limits, angular velocity saturation limits, and attitude angle limits are used as the dynamic state and input constraints of the optimal control problem. The thrust amplitude limit controls the range of the input norm, the angular velocity saturation limit restricts the amplitude of the angular velocity components, and the attitude angle limits the range of roll and pitch angles.

[0153] In step S404, the expected state of the preset guidance trajectory at the end of the prediction time domain is obtained, and the terminal stability constraint is established based on the deviation between the expected state and the predicted terminal state as the terminal constraint condition of the optimal control problem to ensure that the terminal state converges to the vicinity of the guidance trajectory.

[0154] In step S405, the cost function, environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints are combined to form a complete mathematical expression of the optimal control problem. This complete mathematical expression aims to minimize the cost function and uses the environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints as restrictions to form a constrained finite-time optimal control problem, where the optimization variable is the control input vector at each discrete time step in the prediction time domain.

[0155] In step S406, the optimal control problem is solved using either sequential quadratic programming or interior-point numerical optimization to obtain the optimal control input sequence in the prediction time domain. Sequential quadratic programming transforms the original problem into a series of quadratic programming subproblems that iteratively approximate the optimal solution, while the interior-point method constructs a barrier function within the feasible region for iterative search. Both methods are suitable for handling optimization problems with nonlinear constraints. The obtained optimal control input sequence contains the control input vector for each discrete time step in the prediction time domain. The first term of this sequence will be output as the actual control command for the current control cycle to the actuator.

[0156] This embodiment unifies environmental safety boundary constraints, dynamic state and input constraints, terminal stability constraints, and a cost function containing tracking deviation, control smoothing, and energy consumption terms to form an optimal control problem. The optimal control input sequence is obtained through numerical optimization, enabling the UAV to balance tracking accuracy and energy efficiency while meeting environmental safety and dynamic constraints, thus avoiding performance loss caused by the separation of path planning and tracking control.

[0157] Furthermore, in some embodiments, the equations of motion for translation of the center of mass and the equations of motion for rotation about the center of mass are expressed by formula (1), which is as follows:

[0158] ;

[0159] In formula (1), It is the position vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the velocity vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the total mass of the drone. It is the total thrust vector generated by the drone's motors, expressed in the body coordinate system. It is the gravitational acceleration vector. , It is the attitude angle vector of the UAV. for The first derivative, For roll angle, The pitch angle, Yaw angle This is the transformation matrix between the rate of change of attitude angle and the angular velocity of the aircraft. , This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let be the inertial tensor matrix of the UAV in the body coordinate system. The total torque vector generated by the drone's motors is represented in the body coordinate system.

[0160] Static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are expressed by formula (2), which is as follows:

[0161] ;

[0162] In formula (2), yes transpose, It is the first The unit normal vector of a statically constrained hyperplane points in the direction in which the UAV can move. The centroid of the UAV at discrete time step The world coordinate system position vector It is the first The distance parameter of each statically constrained hyperplane is a constant. For the set of statically constrained hyperplane indices, To predict the total number of steps in the time domain, yes transpose, It is the first The unit normal vector of the dynamically constrained hyperplane points in the direction in which the UAV can move. It is the first A dynamically constrained hyperplane at discrete time step The distance parameter changes with time step. It is a set of dynamically constrained hyperplane indices. Discrete time step The thrust vector, This refers to the minimum thrust amplitude that the drone's motor can provide. The maximum thrust amplitude that the drone motor can provide. Discrete time step The three axial angular velocity components in the body coordinate system This represents the maximum permissible angular velocity amplitude for the drone. The maximum allowable roll angle for the drone. The maximum allowable pitch angle for the drone. To predict the total number of steps in the time domain;

[0163] The constrained discrete-time prediction dynamics model is represented by Equation (3), which is as follows:

[0164] ;

[0165] In formula (3), , Discrete time step The state vector includes position, velocity, attitude angle, and angular velocity. , Discrete time step The control input vector includes thrust and torque. The state evolution function is obtained by discretizing formula (1) based on the fourth-order Runge-Kutta method;

[0166] The cost function is expressed by formula (4), which is as follows:

[0167] ;

[0168] In formula (4), Let cost function be Discrete time step The preset guiding trajectory state vector. This is the state tracking deviation weight matrix. To control the smoothing weight matrix of the input rate of change, To advance the energy consumption weighting matrix, This is the terminal cost weight matrix;

[0169] The complete mathematical expression is represented by formula (5), which is as follows:

[0170] ;

[0171] In formula (5), To predict the control input sequence in the time domain, This is the allowable threshold for terminal deviation.

[0172] In some embodiments, the first term of the optimal control input sequence is output as the actual control command to the actuator. Linear extrapolation is performed between adjacent point cloud frames using the constraint hyperplane parameters of the previous frame, combined with the velocity estimate obtained from the attitude position data, to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with a newly extracted local geometric constraint manifold, including:

[0173] The first term of the optimal control input sequence is used as the actual control command for the current control cycle and output to the UAV motor electronic speed controller and flight control actuator.

[0174] Between adjacent point cloud frames, obtain the normal vector and distance parameters of the static constraint hyperplane extracted in the previous frame;

[0175] Based on the UAV velocity estimate obtained from attitude position data, the distance parameters of the static constraint hyperplane are linearly extrapolated and predicted to generate the extrapolated static constraint hyperplane.

[0176] Obtain the normal vector, distance parameters, and relative velocity carried by the dynamic constraint hyperplane extracted from the previous frame;

[0177] Based on relative velocity, kinematic extrapolation is performed on the distance parameters of the dynamically constrained hyperplane to generate the externally driven constrained hyperplane;

[0178] The extrapolated static constraint hyperplane and the extrapolated dynamic constraint hyperplane are combined to form the extrapolated local geometric constraint manifold, which is used to maintain the continuity of environmental constraints within the control period between adjacent point cloud frames;

[0179] When a new point cloud frame arrives, the newly extracted local geometric constraint manifold is obtained and replaced with the currently used extrapolated local geometric constraint manifold to complete the constraint update.

[0180] In this embodiment, the first item of the optimal control input sequence is used as the actual control command for the current control cycle and output to the UAV motor electronic speed controller and flight control actuator. The actual control command includes a thrust vector and a torque vector. The thrust vector is used to drive the motor to generate lift, and the torque vector is used to drive the control surfaces to generate attitude adjustment torque, thereby realizing the motion control of the UAV in three-dimensional space.

[0181] Between adjacent point cloud frames, the normal vector and distance parameters of the statically constrained hyperplane extracted from the previous frame are obtained. The UAV velocity estimate obtained from the attitude position data is multiplied by the projection component of its normal vector direction by the time interval between adjacent point cloud frames to obtain the change in the distance parameter. This change is then superimposed on the original distance parameter to generate the extrapolated statically constrained hyperplane.

[0182] Obtain the normal vector, distance parameter, and relative velocity carried by the dynamic constraint hyperplane extracted from the previous frame. Multiply the projection component of the relative velocity in the direction of the normal vector by the time interval between adjacent point cloud frames to obtain the change in the distance parameter. Add this change to the original distance parameter to generate the externally driven dynamic constraint hyperplane.

[0183] The extrapolated static constraint hyperplane and the extrapolated dynamic constraint hyperplane are combined to form an extrapolated local geometric constraint manifold, which is used as a substitute for environmental constraints during point cloud frame intervals. When a new point cloud frame arrives, the newly extracted local geometric constraint manifold is obtained and replaces the currently used extrapolated local geometric constraint manifold, completing the constraint update.

[0184] In this embodiment, velocity estimation and relative velocity are used to extrapolate and predict the static constraint hyperplane and the dynamic constraint hyperplane during the point cloud frame interval, respectively. This maintains the continuity of environmental constraints within the control cycle and avoids the loss of constraint information caused by the point cloud frame rate being lower than the control frequency, thereby improving the control stability and flight safety of the UAV in dynamic environments.

[0185] In some embodiments, based on relative velocity, kinematic extrapolation prediction is performed on the distance parameters of the dynamically constrained hyperplane to generate an extrapolated dynamically constrained hyperplane, including:

[0186] Obtain the dynamic constraint hyperplane extracted from the previous frame. The dynamic constraint hyperplane contains the normal vector, the initial distance parameter, and the relative velocity vector it carries.

[0187] Based on the relative velocity vector, the distance change of the dynamic constraint hyperplane at each control step in the prediction time domain is calculated. The distance change is the product of the projection component of the relative velocity vector in the direction of the normal vector and the time step.

[0188] The initial distance parameters and the distance changes are accumulated step by step over time to generate the extrapolated distance parameter sequence of the dynamic constraint hyperplane in each control period between adjacent point cloud frames.

[0189] Based on the sequence of normal vector and extrapolated distance parameters, generate the extrapolated dynamic constraint hyperplane corresponding to each control cycle;

[0190] The externally driven state constraint hyperplanes corresponding to all control cycles are summarized to form a set of externally driven state constraint hyperplanes between adjacent point cloud frames.

[0191] In this embodiment, the relative velocity vector is obtained by dividing the difference in the centroid position of the same obstacle point cloud cluster in two consecutive frames of point cloud data by the frame interval time, and is used to describe the motion trend of dynamic obstacles in the body coordinate system.

[0192] In this embodiment, the distance change is the product of the projection component of the relative velocity vector in the direction of the normal vector and the time step. This projection component is obtained by the dot product of the relative velocity vector and the normal vector. The time step is preset according to the operating cycle of the control loop. The product of the two yields the change in the distance parameter at each control step.

[0193] In this embodiment, the initial distance parameter corresponds to the original distance at the extraction time of the previous frame. After each control step, the corresponding distance change is superimposed on the distance parameter of the previous step to obtain the extrapolated distance parameter at that control step.

[0194] In this embodiment, the externally driven constraint hyperplane is determined by the invariant normal vector and the extrapolation distance parameter updated step by step. The normal vector determines the constraint direction, and the extrapolation distance parameter determines the position of the constraint boundary.

[0195] In this embodiment, an externally driven state constraint hyperplane set is constructed between adjacent point cloud frames. Each externally driven state constraint hyperplane in the set corresponds to a control period and is used to replace the original dynamic constraint hyperplane extracted in the previous frame during the control period, thereby maintaining the continuity of dynamic obstacle constraints during the point cloud frame interval.

[0196] This embodiment uses the projection component of the relative velocity vector in the direction of the normal vector to calculate the distance change step by step over time, and accumulates the initial distance parameter and the distance change step by step over time to generate an extrapolated distance parameter sequence, thereby forming an extrapolated dynamic constraint hyperplane set. This maintains the continuity of dynamic obstacle constraints during the point cloud frame interval, avoids the loss of dynamic constraint information caused by the point cloud frame rate being lower than the control frequency, and improves the UAV's ability to continuously perceive and avoid moving obstacles.

[0197] In some embodiments, at each point cloud frame update time, the intersection of the current local geometrically constrained manifold and the UAV dynamic reachable set is calculated. If the intersection is not empty, normal execution is performed; if the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated, including:

[0198] At each point cloud frame update time, the newly extracted local geometric constraint manifold is obtained. The local geometric constraint manifold includes all static constraint hyperplanes and dynamic constraint hyperplanes in the current frame.

[0199] Based on the six-degree-of-freedom nonlinear dynamics model of the UAV and its current state, the dynamic reachable set of the UAV in the next prediction time domain is calculated. The dynamic reachable set represents the spatial set of all reachable states of the UAV under the constraints of thrust amplitude, angular velocity saturation and attitude angle.

[0200] Calculate the intersection of the locally geometrically constrained manifold and the dynamically reachable set, and determine whether the intersection is an empty set;

[0201] If the intersection is not empty, it indicates that there exists a control solution that simultaneously satisfies environmental safety constraints and dynamic feasibility. The system maintains normal execution mode and continues to execute subsequent control cycles.

[0202] If the intersection is empty, it indicates that there is no feasible control solution under the current environmental constraints, triggering the safety braking mode, and the UAV performs deceleration and hovering according to the current velocity vector direction;

[0203] In the safety braking mode, the new local geometric constraint manifold corresponding to the new arrival point cloud frame is continuously monitored, and the intersection of the new local geometric constraint manifold and the current dynamic reachable set is repeatedly calculated;

[0204] When the intersection becomes non-empty again, exit the safety braking mode and resume the normal predictive control execution process.

[0205] In this embodiment, the static constraint hyperplane and the dynamic constraint hyperplane are derived from the point cloud clusters corresponding to stationary obstacles and moving obstacles, respectively. Together, they constitute a complete safety boundary description of the environment surrounding the UAV in the current frame.

[0206] In this embodiment, the dynamic reachability set is realized through forward reachability analysis. Starting from the current state, within the input and state boundary formed by thrust amplitude, angular velocity saturation and attitude angle limit, the state space range that the UAV can reach in the prediction time domain is deduced step by step.

[0207] In this embodiment, the intersection calculation is achieved by transforming the constraint hyperplane in the local geometrically constrained manifold into linear inequality constraints and summarizing them into a feasible state space polyhedron. Then, the intersection calculation is performed with the convex hull of the dynamically reachable set. This allows for a mathematical determination of whether there exists a state point that simultaneously satisfies environmental safety constraints and dynamic feasibility.

[0208] In this embodiment, deceleration and hovering are achieved by gradually reducing the thrust vector to hovering thrust and adjusting the attitude to a horizontal state, thereby reducing the risk of the drone colliding with obstacles.

[0209] This embodiment determines whether a feasible control solution exists by calculating the intersection of the local geometrically constrained manifold and the dynamically reachable set. When the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering. Normal execution is resumed when the intersection becomes non-empty again. This provides a safety guarantee for the UAV at the mathematical level and avoids the risk of blindly executing infeasible control commands when the control solution does not exist due to overly strict environmental constraints or dynamic limitations.

[0210] In some embodiments, calculating the intersection of the locally geometrically constrained manifold and the dynamically reachable set, and determining whether the intersection is an empty set, includes:

[0211] Obtain the local geometrically constrained manifold, which consists of multiple constrained hyperplanes. Each constrained hyperplane is determined by the normal vector and the distance parameter.

[0212] Each constraint hyperplane is transformed into a linear inequality constraint with respect to the UAV's position and state variables. The geometric meaning of the linear inequality constraint is that the UAV's centroid lies within the half-space defined by the constraint hyperplane.

[0213] By summing up all the linear inequality constraints, a feasible state space polyhedron corresponding to the local geometrically constrained manifold is formed.

[0214] Obtain the dynamic reachability set, which is the set of reachable state spaces obtained by forward reachability analysis of the UAV's six-degree-of-freedom nonlinear dynamic model under the current state and input constraints;

[0215] The intersection region is obtained by performing convex hull intersection calculation on the feasible state space polyhedron and the dynamic reachable set;

[0216] To determine whether the intersection region is an empty set, if the intersection region contains at least one state point, then the intersection is determined to be non-empty; if the intersection region does not contain any state points, then the intersection is determined to be an empty set.

[0217] In this embodiment, the constrained hyperplane is determined by the normal vector and the distance parameter. The normal vector is obtained through principal component analysis of the point cloud cluster, and the distance parameter is determined by the shortest Euclidean distance between the UAV's centroid and the point cloud cluster. Together, they define the safe motion boundary of the UAV in this direction.

[0218] In this embodiment, the linear inequality constraint is expressed in the form that the dot product of the normal vector and the position state variable is not less than the distance parameter. This is used to transform the geometric boundary into a mathematical constraint, so that the subsequent optimization problem can directly handle environmental constraints.

[0219] In this embodiment, the feasible state space polyhedron is formed by the intersection of all linear inequality constraints. This polyhedron is the spatial region in which the UAV position and state variables must be located, and its boundary is defined by all constraint hyperplanes.

[0220] In this embodiment, the dynamic reachability set is obtained through forward reachability analysis. Starting from the current state, within the input and state boundary formed by thrust amplitude, angular velocity saturation and attitude angle limit, the state space range that the UAV can reach in the prediction time domain is deduced step by step.

[0221] In this embodiment, the convex hull intersection calculation is used to determine whether there is an overlapping region between the feasible state space polyhedron and the dynamic reachable set. If there is an overlap, it indicates that there is a state point that simultaneously satisfies environmental safety constraints and dynamic feasibility.

[0222] In this embodiment, the intersection region is determined to be non-empty when it contains at least one state point, and the intersection region is determined to be empty when it does not contain any state points. The empty set determination will trigger the safety braking mode to avoid executing infeasible control instructions.

[0223] This embodiment transforms the constraint hyperplane into linear inequality constraints and summarizes them into a feasible state space polyhedron. Then, it performs convex hull intersection calculation with the dynamic reachable set. This embodiment mathematically determines whether there is a control solution that simultaneously satisfies environmental safety constraints and dynamic feasibility. Thus, when the intersection is empty, a safety braking mode is triggered, avoiding the risk of blindly executing infeasible control commands.

[0224] Please see Figure 5 In a second aspect, this embodiment also provides a UAV control system 1 for generating 3D data based on point cloud data, used to execute the method described in the first aspect, the system comprising:

[0225] The point cloud preprocessing module 11 is used to acquire the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system, and extract a local point cloud subset.

[0226] The local constraint extraction module 12 is used to cluster and segment local point cloud subsets, identify point cloud clusters on continuous obstacle surfaces, extract the local surface normal vector of each point cloud cluster through principal component analysis, establish a constraint hyperplane by combining the shortest distance from the UAV centroid to the point cloud cluster, and summarize all constraint hyperplanes in the current frame to form a local geometric constraint manifold.

[0227] The constrained dynamics construction module 13 is used to obtain the six-degree-of-freedom nonlinear dynamics model of the UAV. It transforms the constrained hyperplane in the local geometric constrained manifold into inequality constraints on position variables, and embeds them together with thrust amplitude, angular velocity saturation and attitude angle constraints into the prediction time domain state evolution equation to form a constrained discrete-time prediction dynamics model.

[0228] The predictive control solution module 14 is used to establish an optimal control problem based on a constrained discrete-time predictive dynamics model, with tracking deviation term, control smoothing term and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization solution.

[0229] The instruction execution and asynchronous compensation module 15 is used to output the first item of the optimal control input sequence as the actual control instruction to the actuator. It uses the constraint hyperplane parameters of the previous frame and the velocity estimation obtained from the attitude position data to perform linear extrapolation between adjacent point cloud frames to maintain the continuity of the constraint. When a new point cloud frame arrives, the extrapolated manifold is replaced by the newly extracted local geometric constraint manifold.

[0230] The safety verification and braking module 16 is used to calculate the intersection of the current local geometric constraint manifold and the UAV dynamic reachable set at each point cloud frame update. If the intersection is not empty, it will execute normally. If the intersection is empty, it will trigger the safety braking mode to decelerate and hover until the intersection is regenerated.

[0231] The system provided in this embodiment and the method described in the first aspect belong to two different protection dimensions under the same inventive concept. The method described in the first aspect describes the complete process steps from point cloud data acquisition to control command generation, while the system described in this embodiment uses modular division to map each functional unit in the method to an independent hardware or software module, thereby realizing the specific deployment of the technical solution of the method on a physical entity. The point cloud preprocessing module 11, local constraint extraction module 12, constraint dynamics construction module 13, predictive control solution module 14, command execution and asynchronous compensation module 15, and safety verification and braking module 16 in the system are respectively used to execute the corresponding steps in the method of the first aspect. The modules are connected by data flow to form a complete processing link. The output data of the module is used as the input data of the next module, thereby realizing a complete closed-loop processing from the original point cloud data frame to the final control command and safety braking decision.

[0232] The above technical solution extracts constraint hyperplanes directly from a local point cloud subset and summarizes them to form a local geometric constraint manifold. The constraint hyperplanes in this manifold are then transformed into inequality constraints and directly embedded into the prediction time-domain state evolution equation. This skips the complete 3D map construction and independent path planning stages, significantly shortening the link length from point cloud acquisition to control command generation and reducing the accumulated time delay caused by intermediate calculation steps. This shortens the perception-to-control link length, reduces system time delay, and improves control response speed in dynamic environments; avoids excessive computation in complete 3D map construction, reducing the onboard computing burden; achieves unified coupling of environmental constraints and dynamic constraints, solving the problem of disconnect between the planning layer and the control layer; improves dynamic environment response capability through the asynchronous extrapolation mechanism of constraint hyperplanes between point cloud frames; and provides mathematical security guarantees through the intersection verification of the local geometric constraint manifold and the dynamic reachable set.

[0233] Finally, it should be noted that although the above embodiments have been described in the text and drawings of this application, this should not limit the scope of patent protection of this application. Any technical solutions that are based on the essential concept of this application and utilize the content described in the text and drawings of this application, resulting in equivalent structural or procedural substitutions or modifications, as well as the direct or indirect application of the technical solutions of the above embodiments to other related technical fields, are all included within the scope of patent protection of this application.

Claims

1. A method for controlling a drone based on point cloud data to generate 3D data, characterized in that, include: Acquire the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system, and extract a local point cloud subset; Clustering and segmentation are performed on the local point cloud subset to identify point cloud clusters on the surface of continuous obstacles. The local surface normal vector of each point cloud cluster is extracted by principal component analysis. The constraint hyperplane is established by combining the shortest distance from the centroid of the UAV to the point cloud cluster. All constraint hyperplanes in the current frame are summarized to form a local geometric constraint manifold. A six-degree-of-freedom nonlinear dynamic model of the UAV is obtained. The constraint hyperplane in the local geometric constraint manifold is transformed into an inequality constraint with respect to the position variable. This constraint is then uniformly embedded into the prediction time-domain state evolution equation along with the thrust amplitude, angular velocity saturation, and attitude angle constraints to form a constrained discrete-time prediction dynamic model. Based on the constrained discrete-time predictive dynamics model, an optimal control problem is established with tracking deviation term, control smoothing term, and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained by numerical optimization. The first term of the optimal control input sequence is output to the actuator as the actual control command. Linear extrapolation is performed between adjacent point cloud frames using the constraint hyperplane parameters of the previous frame and the velocity estimation obtained from the attitude position data to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with the newly extracted local geometric constraint manifold. At each point cloud frame update, the intersection of the current local geometrically constrained manifold and the UAV's dynamic reachable set is calculated. If the intersection is not empty, normal execution is performed. If the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated.

2. The UAV control method for generating 3D data based on point cloud data according to claim 1, characterized in that, Clustering and segmentation are performed on the local point cloud subset to identify point cloud clusters on continuous obstacle surfaces. Principal component analysis is used to extract the local surface normal vector of each point cloud cluster. A constraint hyperplane is established based on the shortest distance from the UAV's centroid to the point cloud cluster. All constraint hyperplanes in the current frame are then aggregated to form a local geometric constraint manifold, including: The local point cloud subset is segmented by Euclidean distance clustering to obtain multiple point cloud clusters; Principal component analysis is performed on each point cloud cluster to extract the local surface normal vector of the point cloud cluster, and the shortest Euclidean distance from the UAV's centroid to the point cloud cluster is calculated. Based on the local surface normal vector and the shortest Euclidean distance, a constraint hyperplane corresponding to the point cloud cluster is established. The constraint hyperplane represents the geometric boundary in which the UAV can move safely in this direction. Obtain the positional difference of the same obstacle point cloud cluster in two consecutive frames of point cloud data, and estimate the relative velocity of the obstacle; Based on the relative velocity, motion compensation is performed on the constraint hyperplane corresponding to the dynamic obstacle to generate a dynamic constraint hyperplane with velocity offset. The constraint hyperplanes corresponding to all static obstacles in the current frame and the dynamic constraint hyperplanes corresponding to dynamic obstacles are combined to form a local geometric constraint manifold containing both static and dynamic information.

3. The UAV control method for generating 3D data based on point cloud data according to claim 1, characterized in that, A six-degree-of-freedom nonlinear dynamic model of the UAV is obtained. The constraint hyperplane in the local geometrically constrained manifold is transformed into inequality constraints with respect to position variables. These constraints, along with thrust amplitude, angular velocity saturation, and attitude angle limitations, are uniformly embedded into the predictive time-domain state evolution equation to form a constrained discrete-time predictive dynamic model, including: A six-degree-of-freedom nonlinear rigid body dynamics model of an unmanned aerial vehicle is obtained, the model including the mechanical equations for translational motion of the center of mass and the dynamic equations for rotation about the center of mass; The static constraint hyperplane in the local geometric constraint manifold is transformed into a static inequality constraint with respect to the UAV position and state variables. The parameters of the static inequality constraint are constant in the prediction time domain. The dynamic constraint hyperplane in the local geometric constraint manifold is transformed into a dynamic inequality constraint with respect to the UAV position and state variables. The distance parameter of the dynamic inequality constraint changes with time step in the prediction time domain, and the rate of change is determined by the relative velocity carried by the dynamic constraint hyperplane. Obtain the thrust amplitude limit, angular velocity saturation limit, and attitude angle limit of the UAV, and transform them into ontological inequality constraints regarding control inputs and state variables; The static inequality constraints, the dynamic inequality constraints, and the ontological inequality constraints are uniformly embedded into the prediction time-domain state evolution equation established based on a preset discretized time step, which is a constrained discrete-time prediction dynamic model.

4. The UAV control method for generating 3D data based on point cloud data according to claim 3, characterized in that, Based on the constrained discrete-time predictive dynamics model, an optimal control problem is established, with tracking deviation, control smoothing, and energy consumption terms as cost functions, and environmental safety boundary constraints, dynamic state and input constraints, and terminal stability constraints determined by the local geometric constraint manifold as conditions. The optimal control input sequence is obtained through numerical optimization, including: Based on the constrained discrete-time predictive dynamics model, a cost function for the finite-time optimal control problem is established. The cost function includes a deviation term for tracking a preset guiding trajectory, a smoothing term for the rate of change of the control input, and a propulsion energy consumption term. The static and dynamic inequality constraints in the local geometrically constrained manifold are used as the environmental safety boundary constraints for the optimal control problem. The ontological inequality constraints corresponding to the thrust amplitude limit, the ontological inequality constraints corresponding to the angular velocity saturation limit, and the ontological inequality constraints corresponding to the attitude angle limit are used as the dynamic state and input constraints of the optimal control problem. Obtain the desired state of the preset guidance trajectory at the predicted end point in the time domain. Based on the deviation between the desired state and the predicted terminal state, establish terminal stability constraints and use the terminal stability constraints as the terminal constraint conditions of the optimal control problem. The cost function, the environmental safety boundary constraints, the dynamic state and input constraints, and the terminal stability constraints together constitute the complete mathematical expression of the optimal control problem. The optimal control problem is solved by sequential quadratic programming or interior-point numerical optimization to obtain the optimal control input sequence in the prediction time domain.

5. The UAV control method for generating 3D data based on point cloud data according to claim 4, characterized in that, The equations of motion for translation of the center of mass and the equations of motion for rotation about the center of mass are expressed by formula (1), which is as follows: ； In formula (1), It is the position vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the velocity vector of the UAV's center of mass in the world coordinate system. for The first derivative, It is the total mass of the drone. It is the total thrust vector generated by the drone's motors, expressed in the body coordinate system. It is the gravitational acceleration vector. , It is the attitude angle vector of the UAV. for The first derivative, For roll angle, The pitch angle, Yaw angle This is the transformation matrix between the rate of change of attitude angle and the angular velocity of the aircraft. , This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let be the inertial tensor matrix of the UAV in the body coordinate system. The total torque vector generated by the drone's motors is represented in the body coordinate system. The static inequality constraints, dynamic inequality constraints, and ontological inequality constraints are expressed by formula (2), which is as follows: ； In formula (2), yes transpose, It is the first The unit normal vector of a statically constrained hyperplane points in the direction in which the UAV can move. The centroid of the UAV at discrete time step The world coordinate system position vector It is the first The distance parameter of each statically constrained hyperplane is a constant. For the set of statically constrained hyperplane indices, To predict the total number of steps in the time domain, yes transpose, It is the first The unit normal vector of the dynamically constrained hyperplane points in the direction in which the UAV can move. It is the first A dynamically constrained hyperplane at discrete time step The distance parameter changes with time step. It is a set of dynamically constrained hyperplane indices. Discrete time step The thrust vector, This refers to the minimum thrust amplitude that the drone's motor can provide. The maximum thrust amplitude that the drone motor can provide. Discrete time step The three axial angular velocity components in the body coordinate system This represents the maximum permissible angular velocity amplitude for the drone. The maximum allowable roll angle for the drone. The maximum allowable pitch angle for the drone. To predict the total number of steps in the time domain; The constrained discrete-time prediction dynamics model is represented by Equation (3), which is as follows: ; In formula (3), , Discrete time step The state vector includes position, velocity, attitude angle, and angular velocity. , Discrete time step The control input vector includes thrust and torque. The state evolution function is obtained by discretizing formula (1) based on the fourth-order Runge-Kutta method; The cost function is expressed by formula (4), which is as follows: ； In formula (4), Let cost function be Discrete time step The preset guiding trajectory state vector. This is the state tracking deviation weight matrix. To control the smoothing weight matrix of the input rate of change, To advance the energy consumption weighting matrix, This is the terminal cost weight matrix; The complete mathematical expression is represented by formula (5), which is as follows: ； In formula (5), To predict the control input sequence in the time domain, This is the allowable threshold for terminal deviation.

6. The UAV control method for generating 3D data based on point cloud data according to claim 1, characterized in that, The first term of the optimal control input sequence is output as the actual control command to the actuator. Linear extrapolation is performed between adjacent point cloud frames using the constraint hyperplane parameters of the previous frame, combined with the velocity estimate obtained from the attitude position data, to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with a newly extracted local geometric constraint manifold, including: The first item of the optimal control input sequence is used as the actual control command for the current control cycle and output to the UAV motor electronic speed controller and flight control actuator. Between adjacent point cloud frames, obtain the normal vector and distance parameters of the static constraint hyperplane extracted in the previous frame; Based on the UAV velocity estimate obtained from the attitude position data, the distance parameters of the static constraint hyperplane are linearly extrapolated and predicted to generate the extrapolated static constraint hyperplane. Obtain the normal vector, distance parameters, and relative velocity carried by the dynamic constraint hyperplane extracted from the previous frame; Based on the relative velocity, kinematic extrapolation is performed on the distance parameters of the dynamic constraint hyperplane to generate the externally driven dynamic constraint hyperplane; The extrapolated static constraint hyperplane and the extrapolated dynamic constraint hyperplane are combined to form an extrapolated local geometric constraint manifold, which is used to maintain the continuity of environmental constraints within the control period between adjacent point cloud frames. When a new point cloud frame arrives, the newly extracted local geometric constraint manifold is obtained and replaced with the currently used extrapolated local geometric constraint manifold to complete the constraint update.

7. The UAV control method for generating 3D data based on point cloud data according to claim 6, characterized in that, Based on the relative velocity, kinematic extrapolation prediction is performed on the distance parameters of the dynamic constraint hyperplane to generate the extrapolated dynamic constraint hyperplane, including: Obtain the dynamic constraint hyperplane extracted from the previous frame. The dynamic constraint hyperplane includes a normal vector, an initial distance parameter, and a relative velocity vector. Based on the relative velocity vector, the distance change of the dynamic constraint hyperplane at each control step in the prediction time domain is calculated. The distance change is the product of the projection component of the relative velocity vector in the direction of the normal vector and the time step. The initial distance parameter and the distance change are accumulated step by step over time to generate the extrapolated distance parameter sequence of the dynamic constraint hyperplane in each control period between adjacent point cloud frames. Based on the normal vector and the extrapolation distance parameter sequence, an extrapolated dynamic constraint hyperplane corresponding to each control cycle is generated; The externally driven state constraint hyperplanes corresponding to all control cycles are summarized to form a set of externally driven state constraint hyperplanes between adjacent point cloud frames.

8. The UAV control method for generating 3D data based on point cloud data according to claim 1, characterized in that, At each point cloud frame update, the intersection of the current local geometrically constrained manifold and the UAV's dynamic reachable set is calculated. If the intersection is not empty, normal execution proceeds; if the intersection is empty, a safety braking mode is triggered to perform deceleration and hovering until the intersection is regenerated, including: At each point cloud frame update time, the newly extracted local geometric constraint manifold is obtained, which includes all static constraint hyperplanes and dynamic constraint hyperplanes in the current frame; Based on the six-degree-of-freedom nonlinear dynamics model of the UAV and its current state, the dynamic reachable set of the UAV in the next prediction time domain is calculated. The dynamic reachable set represents the spatial set of all reachable states of the UAV under thrust amplitude, angular velocity saturation and attitude angle constraints. Calculate the intersection of the local geometrically constrained manifold and the dynamically reachable set, and determine whether the intersection is an empty set; If the intersection is not empty, it indicates that there exists a control solution that simultaneously satisfies environmental safety constraints and dynamic feasibility. The system maintains normal execution mode and continues to execute subsequent control cycles. If the intersection is empty, it indicates that there is no feasible control solution under the current environmental constraints, triggering the safety braking mode, and the UAV performs deceleration and hovering according to the current velocity vector direction; In the safety braking mode, the new local geometric constraint manifold corresponding to the new arrival point cloud frame is continuously monitored, and the intersection of the new local geometric constraint manifold and the current dynamic reachable set is repeatedly calculated; When the intersection becomes non-empty again, exit the safety braking mode and resume the normal predictive control execution process.

9. The UAV control method for generating 3D data based on point cloud data according to claim 8, characterized in that, Calculating the intersection of the locally geometrically constrained manifold and the dynamically reachable set, and determining whether the intersection is empty, includes: The local geometrically constrained manifold is obtained, which is composed of multiple constrained hyperplanes, and each constrained hyperplane is determined by a normal vector and a distance parameter. Each of the constraint hyperplanes is transformed into a linear inequality constraint with respect to the UAV's position and state variables. The geometric meaning of the linear inequality constraint is that the UAV's centroid lies within the half-space defined by the constraint hyperplane. All the linear inequality constraints are summarized to form the feasible state space polyhedron corresponding to the local geometric constraint manifold. Obtain the dynamic reachability set, which is the set of reachable state spaces obtained by forward reachability analysis of the UAV six-degree-of-freedom nonlinear dynamic model under the current state and input constraints; The intersection region is obtained by performing a convex hull intersection calculation between the feasible state space polyhedron and the dynamic reachable set. Determine whether the intersection region is an empty set. If the intersection region contains at least one state point, the intersection is determined to be non-empty; if the intersection region does not contain any state points, the intersection is determined to be an empty set.

10. A UAV control system for generating 3D data based on point cloud data, characterized in that, The system for performing the method according to any one of claims 1 to 9, the system comprising: The point cloud preprocessing module is used to acquire the original point cloud data frame and synchronized attitude and position data, transform the point cloud data frame to the body coordinate system, and extract a local point cloud subset. The local constraint extraction module is used to cluster and segment the local point cloud subset, identify point cloud clusters on the surface of continuous obstacles, extract the local surface normal vector of each point cloud cluster through principal component analysis, establish a constraint hyperplane by combining the shortest distance from the centroid of the UAV to the point cloud cluster, and summarize all constraint hyperplanes in the current frame to form a local geometric constraint manifold. The constrained dynamics construction module is used to obtain a six-degree-of-freedom nonlinear dynamics model of the UAV. It transforms the constrained hyperplane in the local geometric constrained manifold into inequality constraints with respect to position variables, and embeds them together with thrust amplitude, angular velocity saturation and attitude angle constraints into the prediction time domain state evolution equation to form a constrained discrete-time prediction dynamics model. The predictive control solution module is used to establish an optimal control problem based on the constrained discrete-time predictive dynamics model, with tracking deviation term, control smoothing term and energy consumption term as cost functions, and environmental safety boundary constraints, dynamic state and input constraints and terminal stability constraints determined by the local geometric constraint manifold as conditions, and obtain the optimal control input sequence through numerical optimization solution; The instruction execution and asynchronous compensation module is used to output the first item of the optimal control input sequence as the actual control instruction to the actuator. It uses the constraint hyperplane parameters of the previous frame and the velocity estimation obtained from the attitude position data to perform linear extrapolation between adjacent point cloud frames to maintain constraint continuity. When a new point cloud frame arrives, the extrapolated manifold is replaced with the newly extracted local geometric constraint manifold. The safety verification and braking module is used to calculate the intersection of the current local geometric constraint manifold and the UAV dynamic reachable set at each point cloud frame update. If the intersection is not empty, it will execute normally; if the intersection is empty, it will trigger the safety braking mode to decelerate and hover until the intersection is regenerated.

Images