A complex environment-oriented unmanned aerial vehicle full-autonomous three-dimensional navigation method and system

Abstract

The present application relates to the technical field of unmanned aerial vehicle autonomous flight control and navigation, and particularly relates to a complex environment-oriented unmanned aerial vehicle full-autonomous three-dimensional navigation method and system, aiming to break through the full-stack architecture of multi-source fusion perception, kinetic smoothing optimization, energy adaptive management and bottom layer strong robust control, and realize industrial-grade high-reliable full-autonomous flight in a complex and GNSS denial environment, the method comprising the following steps: step S1, high-precision state estimation and real-time incremental mapping; step S2, dimension reduction skeleton path search and safety flight corridor construction; step S3, space-time trajectory optimization considering kinetic constraints and aerodynamic energy adaptation; step S4, self-disturbance bottom layer cooperative control and state machine failure protection.

Description

Technical Field

[0001] This invention relates to the field of autonomous flight control and navigation technology for unmanned aerial vehicles (UAVs), and in particular to a fully autonomous three-dimensional navigation method and system for UAVs in complex environments. Background Technology

[0002] With the increasing application of drones in complex industry scenarios such as logistics delivery and wide-area inspection, fully autonomous 3D navigation and obstacle avoidance planning have become core technological bottlenecks. Currently, the industry generally adopts a 3D meshed environment modeling and path planning framework based on a Digital Terrain Model (DSM). The core logic of this basic technical approach is to discretize the continuous 3D geographic space into a fixed-resolution (e.g., 5m × 5m × 5m) 3D occupancy grid, and then use traditional graph search algorithms to find a polyline path among these discrete voxel nodes that avoids static obstacles and has the shortest geometric Euclidean distance. Under the assumption of constant speed, existing technologies typically equate minimizing flight time with minimizing the geometric length of the path.

[0003] However, this optimization scheme based on macroscopic static grids reveals significant limitations when facing real, complex low-altitude operating environments (such as urban canyons or dense forests). First, in the environmental perception and modeling stages, the coarse static grid with fixed resolution easily filters out small structures that pose a fatal threat to flight safety (such as high-voltage cables and outstretched branches), and is completely incapable of handling dynamic obstacles that suddenly appear during flight. More importantly, occupying a grid only provides a "black and white" (0 or 1) collision state, failing to provide a continuous spatial distance gradient away from obstacles for smooth trajectory optimization in the later stages. Simultaneously, such schemes often assume that the UAV can obtain high-precision absolute positioning throughout the entire process. Once the Global Navigation Satellite System (GNSS) signal is lost in complex scenarios, the pre-planned flight path will completely lose its basis for execution due to the lack of high-precision state estimation support based on airborne multi-source sensors.

[0004] Secondly, regarding the dynamic feasibility of the trajectory, existing technologies simplify path planning to a purely geometric polygonal connection between discrete nodes. This path suffers from a fatal flaw of derivative discontinuity at turning points, theoretically requiring the UAV to possess infinite acceleration and jerk at the nodes. Forcing the tracking of such a dynamically infeasible trajectory completely violates the physical limits of multi-rotor UAVs as underactuated, strongly coupled nonlinear systems, and is highly likely to cause control system saturation, trajectory overshoot, or even crashes.

[0005] Furthermore, in actual long-endurance outdoor flights, the shortest distance based solely on geometry is often not the energy-optimal path. Existing route planning ignores environmental wind field disturbances and the significant nonlinear aerodynamic energy losses generated by the multirotor itself; for example, a straight-line drive through a strong headwind will drastically deplete battery energy. At the system architecture and execution level, conventional algorithms mostly rely on upper-level computational power calculations, while the lower-level flight control largely depends on proportional-integral-derivative (PID) controllers with inherent hysteresis, which cannot effectively resist sudden gusts. At the same time, there is a lack of efficient communication and coordination between the upper-level onboard computer and the lower-level flight control, as well as a lack of robust failsafe logic, failing to meet industrial-grade safety standards.

[0006] In summary, existing UAV 3D path planning technologies are severely lacking in perception dynamics adaptability, system dynamics constraints, aerodynamic energy management, and hardware-software collaborative foundations, making them inadequate for fully autonomous flight requirements in complex environments. There is an urgent need in this field for a fully autonomous navigation and control architecture for UAVs that can overcome these shortcomings. Summary of the Invention

[0007] The technical problem to be solved by this invention is to overcome the shortcomings of existing methods and provide a fully autonomous three-dimensional navigation method and system for UAVs in complex environments. It aims to establish a full-stack architecture that integrates multi-source fusion perception, dynamic smooth optimization, energy adaptive management and underlying robust control, so as to achieve industrial-grade high-reliability fully autonomous flight in complex and GNSS-denied environments.

[0008] To address the aforementioned technical problems, this invention provides the following technical solution: a fully autonomous three-dimensional navigation method for unmanned aerial vehicles (UAVs) in complex environments, the method comprising the following steps: Step S1, High-precision state estimation and real-time incremental mapping: Acquire high-frequency inertial measurement data and low-frequency point cloud data collected by UAV onboard sensors, and use error state Kalman filtering to construct a tightly coupled odometry to obtain real-time state estimation of the UAV in continuous manifold space; at the same time, based on the point cloud data, by introducing independent insertion and deletion queues for local parallel processing, a dynamic Euclidean symbolic distance field containing continuous distance gradient information of spatial obstacles is constructed and updated in real time. Step S2, Dimensional Reduction Skeleton Path Search and Safe Flight Corridor Construction: In the free space provided by the dynamic Euclidean symbolic distance field, an initial discrete polyline path that avoids obstacles is generated using a jump point search algorithm; then, along the initial discrete polyline path, ellipsoidal expansion and half-space intersection iteration are performed in three-dimensional space to generate a safe flight corridor composed of a set of overlapping convex polyhedra. Step S3, considering spatiotemporal trajectory optimization with dynamic constraints and aerodynamic energy adaptation: The safe flight corridor is treated as a strict convex space inclusion constraint. Utilizing the differential flatness characteristics of the multi-rotor UAV, the constrained optimal control problem is transformed into an unconstrained optimization problem with the coordinates of the intermediate waypoints and the time span allocation as decision variables. In the objective function, the energy consumption of the higher-order derivatives of the trajectory is comprehensively minimized, and a nonlinear aerodynamic energy loss functional composed of induced power, shape drag power, and parasitic power is deeply integrated. A three-dimensional spatiotemporal trajectory that conforms to the real physical dynamic limits and energy optimization of the UAV is generated through spatiotemporal deformation solution. Step S4, Active Disturbance Rejection (ADRROR) Underlying Cooperative Control and State Machine Failure Protection: The three-dimensional spatiotemporal trajectory is sent to the underlying flight controller via a data distribution service protocol. In the underlying flight controller, an ADRROR algorithm is used to utilize an extended state observer to unify the unknown internal dynamic cross-coupling and the external environmental wind field as a total disturbance for real-time online estimation and to apply feedforward compensation calculation, thereby controlling the UAV to accurately track the three-dimensional spatiotemporal trajectory. Simultaneously, a heartbeat communication state machine is deployed between the accompaniment computer and the flight controller for lifecycle management. When a control heartbeat packet timeout is detected, the navigation and guidance rights of the external control mode are forcibly revoked and a failure protection mechanism is triggered.

[0009] Preferably, in the error state Kalman filtering of step S1: the UAV's true state vector is defined on a continuous manifold space, including the UAV's attitude rotation matrix, three-dimensional position, three-dimensional velocity, and the gyroscope and accelerometer zero bias of the inertial measurement unit; the tightly coupled odometry defines the small deviation between the true state and the nominal predicted state as the error state vector. The expression for the error state vector is: ; in, Indicates attitude error. Indicates positional error. Indicates speed error, and These represent the zero bias errors of the gyroscope and accelerometer, respectively. Meanwhile, an incremental kd-tree data structure is used when processing the point cloud data to support the insertion and deletion operations of dynamic obstacle point clouds, avoiding the reconstruction of the entire search tree during each frame matching.

[0010] This invention explicitly defines the UAV's true state vector on a continuous manifold space and uses the error state vector for Kalman updates. This effectively avoids the covariance singularity and orthogonality constraint violation problems that are easily caused by traditional full-state filtering when processing three-dimensional rotation matrices. Simultaneously, with the introduction of an incremental kd-tree (ikd-Tree) data structure, the system can consistently maintain the search time complexity at the lowest logarithmic level during the high-frequency insertion and deletion of dynamic obstacle point clouds. This completely eliminates the enormous overhead of rebuilding the entire search tree during each frame's matching, saving valuable computing power for micro-airborne computing platforms.

[0011] Preferably, in the ellipsoid expansion and half-space intersection iteration of step S2: the line segment searched at the front end is used as the seed point to initialize the ellipsoid and expand it outward. When the ellipsoid surface first contacts the obstacle point, a hyperplane is constructed with the contact point as the tangent point and a half-space is defined. After removing the external search domain of the half-space, the expansion continues iteratively until the intersection of the generated half-space set constitutes a strict convex polyhedron. The form based on the intersection of the half-space is converted into a form composed of the convex hull of the polyhedron vertices to ensure that the trajectory control point is completely located in a collision-free free space.

[0012] This invention innovatively incorporates the physical law that parasitic power is proportional to the cube of the fuselage's relative airflow vector into the optimization functional. By explicitly integrating this high-order drag penalty model during the optimization process, the system overcomes the cognitive limitations of pure geometric programming. It can automatically engage in intelligent game theory based on the current wind direction and mission time penalty weights. Not only can it autonomously calculate the physical maximum range speed, but it can also keenly utilize local tailwinds or physical obstructions generated by complex terrain to achieve energy-efficient detours on large-scale routes.

[0013] Preferably, in step S3, the nonlinear aerodynamic energy loss functional integrates the total propulsion energy consumption of the UAV flight and its total power demand. Decomposed into induced power Type I resistor power and parasitic power Wherein, the parasitic power The expression for overcoming the drag experienced by the drone's fuselage as it moves through the air is: ; in, air density, The drag coefficient of the fuselage. For the windward cross-sectional area, Let be the ground velocity vector of the UAV relative to the ground. The environmental wind speed vector is used; by integrating this parasitic power model during the optimization process, the maximum range speed is automatically found and intelligent flight around the environmental wind field is performed.

[0014] This invention specifically defines an iterative derivation mechanism based on the intersection of ellipsoidal dilation and half-space, and converts the generated half-space intersection into a vertex convex hull form. This geometric dimensionality enhancement ensures, on a rigorous mathematical level, that the trajectory control points completely fall within the collision-free free space, perfectly transforming the originally non-differentiable complex obstacle avoidance problem into a standard convex space containing constraints. Combined with the defined spatiotemporal objective function integral formula, the unconstrained optimization problem can directly yield closed-form analytical gradients for intermediate time segments. This precise gradient expression allows the system to fully utilize a quasi-Newton nonlinear optimizer for extremely rapid spatiotemporal deformation calculations, completely overcoming the fatal flaw of traditional multi-stage polynomial quadratic programming (QP) which is prone to local deadlock in narrow corridors due to fixed time allocation.

[0015] Preferably, in the spatiotemporal trajectory optimization of step S3, when transforming the constrained optimal control problem into an unconstrained optimization problem, the objective function is defined. for: ; in, The number of segments in the trajectory. The higher-order derivatives of the position trajectory are represented. This is an intermediate time segment. The regularization penalty weights are applied to the flight time; the unconstrained optimization problem can be directly solved by the objective function. Regarding the intermediate time segment The analytical gradient is obtained, and the time segment is automatically lengthened or shortened by a nonlinear optimizer to perform spatiotemporal deformation.

[0016] Preferably, in step S4, the extended state observer of the active disturbance rejection control algorithm is a linear extended state observer, which unifies the internal nonlinear dynamic coupling of the system and the external environmental wind field as a total disturbance for real-time estimation; the linear extended state observer sets the state vector to include the estimated value of the controlled position or angle, the estimated value of the velocity, and the estimated value of the total disturbance. It passes through closed-loop error The driving differential equation update, where the update of the total perturbation estimate satisfies: ;in, The observer gain; the actual control quantity output to the UAV motors. Designed as an ideal basic control quantity The difference between the interference compensation term and the interference compensation term, i.e. ;in, To control the gain coefficient; to eliminate disturbances through feedforward compensation and perform inverse dynamic compensation.

[0017] As a preferred state machine failure protection mechanism in step S4: the heartbeat communication state machine operates based on the micro-XRCE-DDS protocol. When the continuous heartbeat packet update frequency is detected to fall below the 2Hz threshold and the duration exceeds the set timeout parameter, the state machine of the underlying flight controller immediately deprives the advanced algorithm of control and forces a fallback to the in-place hovering, safe altitude return to home, or local landing mode. Furthermore, a specific remote controller joystick channel is configured through the ground control station. When a slight displacement of the remote controller joystick is detected, the underlying trigger interrupt input instantly cuts off the position writing right and switches back to the manual control mode with stabilization.

[0018] To address the aforementioned technical problems, this invention provides the following technical solution: a fully autonomous 3D navigation system for unmanned aerial vehicles (UAVs) in complex environments, comprising: a high-precision state estimation and real-time incremental mapping module: used to acquire high-frequency inertial measurement data and low-frequency point cloud data, construct a tightly coupled odometry system using error state Kalman filtering to obtain real-time state estimation, and introduce a dual-queue mechanism to construct and update a dynamic Euclidean symbolic distance field containing continuous distance gradient information of spatial obstacles in real time; and a dimensionality-reduced skeleton path search and safe flight corridor construction module: used to generate an initial discrete polyline path using a jump point search algorithm, and execute along the path. The system employs an ellipsoidal expansion and half-space intersection iteration to generate a safe flight corridor; a spatiotemporal trajectory optimization module considering dynamic constraints and aerodynamic energy adaptation transforms the constrained optimal control problem into an unconstrained optimization problem with intermediate waypoint coordinates and time span allocation as decision variables, and integrates a nonlinear aerodynamic energy loss functional into the objective function to generate a three-dimensional spatiotemporal trajectory; and an active disturbance rejection (ADRR) underlying cooperative control and state machine failure protection module uses an ADRR algorithm and an extended state observer to estimate and compensate for the total system disturbance, while deploying a heartbeat communication state machine through a data distribution service protocol to forcibly trigger a failure protection mechanism when the heartbeat packet times out.

[0019] Preferably, the system adopts a heterogeneous distributed deployment in terms of hardware architecture: the high-precision state estimation and real-time incremental mapping module, the dimensionality reduction skeleton path search and safe flight corridor construction module, and the spatiotemporal trajectory optimization module considering dynamic constraints and aerodynamic energy adaptation are deployed and run in the airborne escort computer; the self-disturbance rejection underlying cooperative control and state machine failure protection module are deployed and run in the underlying flight control board.

[0020] Preferably, the airborne escort computer and the underlying flight control board are configured with a data distribution service link based on ROS2 and micro-XRCE-DDS protocols for nanosecond-level synchronous read and write. The system also includes a ground control station and a remote controller that communicate with the underlying flight control board. The remote controller is configured with a specific joystick channel, which is used to cut off the position write right of the airborne escort computer from the physical logic link and switch back to manual control mode when the underlying flight control board detects an interruption input of a slight displacement of the remote controller joystick.

[0021] The linear extended state observer defined in this invention, through pure closed-loop error-driven differential equations, can approximate the total disturbance in the real physical world within tens of milliseconds. It is equivalent to equipping a UAV with a highly sensitive "flight anemometer" without adding extra Pitot tube hardware. It not only forces the strongly coupled nonlinear airframe back into an easily controllable dual integrator system through feedforward inverse dynamic compensation, but also forms a perfect data closed loop between the underlying aerodynamic perception and the top-level flight path planning. Simultaneously, the heterogeneous hardware architecture deployed in this invention, relying on the micro-XRCE-DDS protocol, bypasses the heavy packet encapsulation and decapsulation losses of traditional middleware, achieving nanosecond-level synchronization between the accompaniment computer and the underlying flight control core message bus. Combined with extremely stringent 2Hz continuous heartbeat packet drop threshold detection and a physical-level position write preemption mechanism derived from remote controller joystick interruptions, the system can instantly trigger underlying isolation and degraded takeover in the face of catastrophic failures such as AI process crashes or loose communication cables. This completely eliminates the possibility of unpredictable algorithmic behavior causing system crashes at the very bottom of the physical and logical framework, ensuring that the system can be deployed securely at scale in high-risk industry scenarios.

[0022] The complete technical solution of this invention can have the following beneficial effects: At the level of environmental coarseness, this invention completely breaks away from the traditional coarse grid occupancy and the heavy reliance on absolute positioning of the Global Navigation Satellite System (GNSS). By constructing an error state tightly coupled odometer and an incremental dual-queue Euclidean symbol distance field on a continuous manifold space, it not only achieves millisecond-level extremely low latency response to sudden dynamic obstacles, but also provides a crucial analytical repulsion gradient for smooth trajectory optimization in the back end. This enables UAVs to still have extremely agile perception and maneuvering capabilities in complex denial environments full of small branches.

[0023] Based on high-precision gradient perception, the trajectory planning of this invention rigorously guarantees obstacle avoidance safety and flight dynamics feasibility at the mathematical level. Unlike traditional pure geometric polygonal paths, which are prone to motor saturation and trajectory overshoot, this invention innovatively constructs a safe flight corridor through ellipsoidal expansion iteration, transforming the complex non-convex obstacle avoidance problem into a strictly convex space inclusion constraint. Furthermore, by combining the differential flatness characteristics of multi-rotor UAVs with a spatiotemporal deformation optimization framework, the system automatically realizes the spatial pushing and dynamic redistribution of intermediate waypoints, ensuring that the generated trajectory perfectly conforms to the real physical limits of the UAV, and completely solving the physical distortion problem caused by the discontinuity of trajectory derivatives in existing technologies.

[0024] In this optimization process, the present invention further subverts the traditional blind geometric optimization logic based on "constant speed" and "shortest distance," and for the first time deeply integrates the induced power, drag power, and parasitic power of multi-rotor UAVs, which increase cubically with relative wind speed, into the global objective functional. By jointly optimizing time, higher-order derivatives, and end-to-end aerodynamic energy consumption, the system no longer gets stuck in local deadlocks, but can automatically solve for the maximum range speed of the entire aircraft, and intelligently plan the most energy-efficient route by utilizing terrain shielding or tailwind when facing severe wind fields, greatly improving the extreme endurance capability of UAVs in high-energy-consuming operations such as wide-area industrial inspection.

[0025] Ultimately, at the underlying architecture and control execution level, this invention completely eliminates the inherent feedback lag of traditional PID controllers when resisting low-altitude wind disturbances. By introducing an active disturbance rejection control algorithm and an extended state observer, the system unifies unknown internal dynamic couplings and complex external aerodynamic disturbances into a total disturbance and performs inverse dynamic feedforward compensation, achieving extremely robust wind resistance. Simultaneously, this invention builds a heterogeneous hardware and software collaborative foundation based on a nanosecond-level data distribution service protocol. By deploying extremely stringent communication heartbeat packet monitoring and physical interruption preemption logic based on the ground control station, it completely eliminates safety and control failures caused by high-level algorithm crashes or data link deadlocks, establishing a truly industrial-grade, highly reliable safety barrier for the fully autonomous flight of UAVs. Attached Figure Description

[0026] Appendix Figure 1 This is a schematic diagram of the fully autonomous three-dimensional navigation method for unmanned aerial vehicles (UAVs) according to the present invention.

[0027] Appendix Figure 2 This is a flowchart illustrating step S1 of the present invention;

[0028] Appendix Figure 3 This is a flowchart illustrating step S2 of the present invention;

[0029] Appendix Figure 4 This is a flowchart illustrating step S3 of the present invention;

[0030] Appendix Figure 5 This is a flowchart illustrating step S4 of the present invention. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of this invention clearer, the following detailed description of the fully autonomous three-dimensional navigation method and system for unmanned aerial vehicles (UAVs) in complex environments, in conjunction with the accompanying drawings and specific embodiments, provides an extremely in-depth explanation. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. All equivalent evolutionary solutions derived by those skilled in the art based on these embodiments without creative effort are within the scope of protection of this invention.

[0032] In a core embodiment of this invention, a full-stack UAV 3D navigation and flight control method for complex denied environments is proposed. The overall architecture of this method abandons the rudimentary system that relies solely on discrete static occupancy grids and pure geometric graph search, instead delving into the underlying data streams of multi-source sensors, the aerodynamics of multi-rotor rigid bodies, and nanosecond-level communication buses, implementing a full-link reconstruction encompassing four dimensions: perception, planning, optimization, and control.

[0033] In the first stage of system execution, namely high-precision state estimation and real-time incremental mapping (corresponding to step S1), the system first addresses the problem of "self-localization" and "refined surrounding environment cognition" of UAVs in complex environments lacking absolute positioning signals from Global Navigation Satellite System (GNSS) (such as urban high-rise canyons or dense forest areas). Specifically, the system is mounted on an airborne companion computer (such as an edge computing platform with powerful heterogeneous computing capabilities, such as Nvidia Jetson Orin NX) to receive low-frequency point cloud data (e.g., 10Hz to 20Hz) from an airborne solid-state lidar (LiDAR) and high-frequency measurement data (typically greater than 200Hz) from an inertial measurement unit (IMU) in real time. Since the true kinematic state of the UAV does not exist in a simple three-dimensional Euclidean space, but is strictly defined on a continuous manifold space SO(3)×R12 containing a three-dimensional rotation group and translation vectors, the system deploys an error state Kalman filter (ESKF) tightly coupled odometry based on the FAST-LIO2 architecture. In this filtering architecture, the true state vector is precisely defined as including the UAV's attitude rotation matrix R, 3D position p, 3D velocity v, and the gyroscope bias bg and accelerometer bias ba, which are highly susceptible to drift with temperature and time. To avoid the mathematical curse of covariance singularity or violation of orthogonality constraints that is easily caused by traditional full-state filtering when processing the 3D rotation matrix, this invention utilizes the mapping relationship of Lie algebra to strictly define the small physical deviation between the true state and the nominal state predicted forward using high-frequency IMU data as the error state vector δx = [δ ,δp ,δv ,δbg ,δba ] ∈R15. When a new LiDAR point cloud frame arrives, the system does not perform any time-consuming geometric feature extraction. Instead, it directly projects the original scanned point cloud onto the global coordinate system through the current predicted pose, constructing a rigorous geometric residual equation from point to plane. Subsequently, by obtaining the Jacobian matrix of the residual equation relative to the error state vector, the Kalman gain is calculated, and the 15-dimensional error state is iteratively optimized and updated, thereby achieving high-precision self-state estimation with extremely low latency.

[0034] While obtaining high-precision pose, the system must construct a 3D environment map in real time that reflects the distance gradient of obstacles. Considering that traditional static octree maps or coarse 5-meter resolution grids are completely unable to cope with dynamic obstacles such as birds or other drones that suddenly appear in flight, and can only provide Boolean-type black-and-white collision information, which cannot guide back-end trajectory optimization, this embodiment innovatively introduces a dynamic Euclidean Symbolic Distance Field (ESDF) based on the FIESTA algorithm architecture. In the engineering implementation of this mapping subsystem, the ESDF value of any voxel point x in space is strictly defined as the minimum Euclidean distance to the nearest obstacle surface point set, and is positive in free space and negative inside the obstacle. To solve the exponential computing power overhead caused by calculating the ESDF of the entire map, the system constructs an extremely efficient incremental data structure in memory, using independent "insertion queues" and "deletion queues" based on doubly linked lists to specifically handle obstacles newly detected by airborne sensors and old obstacles that have left the field of view. In conjunction with the Breadth-First Search (BFS) algorithm, the system only updates the distance values ​​and nearest obstacle coordinates (COCs) of local voxels directly affected by environmental changes. This mechanism rapidly reduces mapping latency from hundreds of milliseconds to single-digit millisecond levels. Simultaneously, in the global registration stage of the point cloud, the system employs an incremental kd-tree (ikd-Tree) data structure. This data structure can spontaneously adjust its internal balance when faced with frequent insertion and deletion operations in dynamic point clouds, maintaining an extremely low search complexity of O(logN), completely eliminating the enormous computational overhead of rebuilding the entire search tree for point cloud matching in traditional algorithms.

[0035] In the second stage of system execution, namely the dimensionality reduction skeleton path search and safe flight corridor construction (corresponding to step S2), the UAV has obtained a real-time ESDF continuous free space with repulsive gradients in its digital twin space. Traditional graph search schemes typically rigidly divide the continuous geographic space into voxel nodes with a fixed resolution (e.g., 5 meters) and search for a polyline path based on the Euclidean distance of the node centers. However, once this purely geometric discrete polyline is directly executed, its non-differentiable characteristic at node turns requires the UAV to provide theoretically infinite acceleration, which can easily lead to control saturation and crashes. To solve the non-differentiability problem while retaining the optimization efficiency of graph search, this embodiment adopts a two-step dimensionality reduction strategy. First, in the ESDF map, the heuristic jump point search (JPS) algorithm is used to prune redundant grid expansion nodes on a large scale by identifying spatial symmetry and straight-line jump rules, generating an initial discrete polyline skeleton path that avoids obstacles at an extremely fast speed of microseconds. Then, the system performs a rigorous geometric dimensionality increase operation in three-dimensional space along this skeleton path—constructing a safe flight corridor (SFC). The mathematical implementation of this process is an extremely rigorous iterative process: the system uses the spatial line segments searched by JPS as geometric seed points, initializes an extremely slender initial ellipsoid in free space, and gradually expands the space outward along the path direction. When the mathematical surface of the ellipsoid first contacts the obstacle point fed back from the ESDF map at the coordinate point pc(0), the algorithm immediately stops the expansion in the current direction and constructs a hyperplane in three-dimensional space with the normal vector pointing inward to the center of the ellipsoid, using this contact point as the tangent point. This hyperplane rigorously divides the entire three-dimensional space, defining an open half-space H0 that is certain to contain the original ellipsoid. In subsequent algorithm loops, the system resolutely eliminates the search domain outside the half-space and continues to expand the ellipsoid within the remaining domain, generating new contact points and new half-spaces H1. When the volume of the ellipsoid no longer increases, the mathematical intersection P of all generated half-space sets is obtained. iHi thus constitutes a strictly convex polyhedron. To facilitate subsequent inclusion verification of the polynomial trajectory, the system uses computational geometry algorithms to perfectly transform the H-representation based on the intersection of half-spaces into a V-representation composed of the convex hulls of the polyhedron vertices. By repeating this operation along the entire skeleton path, the system generates a sequence of overlapping convex polyhedra, mathematically transforming the originally complex flight domain filled with irregular non-convex obstacles into a set of highly regular convex space inclusion constraints.

[0036] Once the convex hull of the safe flight corridor is established, the system enters the third stage, which involves spatiotemporal trajectory optimization considering dynamic constraints and aerodynamic energy adaptation (corresponding to step S3). In this core embodiment, the system makes deep use of the differential flatness characteristic of multi-rotor UAVs. According to this characteristic theory, the UAV's extremely complex six-degree-of-freedom state-space variables and four control inputs can all be losslessly analyzed and mapped to a low-dimensional flat output vector σ(t)=[x(t),y(t),z(t),ψ(t)]. Within its finite-order time derivative. Based on this, the system no longer pre-defines the flight time on the broken line segment and blindly executes a constrained quadratic programming (QP) solution as in traditional schemes. Instead, it deploys a more advanced MINCO (Minimum Control Effort) spatiotemporal deformation parameterization framework. The revolutionary breakthrough of the MINCO framework lies in its dimensionality reduction of the original optimal control problem, which originally involved a massive polynomial coefficient matrix, into an unconstrained nonlinear optimization problem with only the coordinate sequence q of the intermediate waypoints of the trajectory and the intermediate time segment assignment vector T as independent variables.

[0037] More revolutionary is that the objective functional of this invention is no longer limited to the higher-order derivatives of the penalized trajectory (such as the fourth-order Snap to ensure extremely smooth control commands), but unprecedentedly integrates a real physical aerodynamic loss model into the global objective function Je. Based on helicopter momentum theory, the total propulsion energy requirement of the UAV during flight is precisely decomposed into three parts: induced power, drag power, and parasitic power. In the optimizer's background engine, the system solves in real-time, based on a fourth-order polynomial equation, for the induced velocity required by the rotor to counteract the forced acceleration of the air by gravity, and calculates the dynamically changing induced power in hovering and forward flight states. Simultaneously, it calculates the drag power, which is proportional to the square of the local airflow velocity, resulting from the high-speed rotation of the blades overcoming viscous friction. More importantly, the system extremely keenly grasps the parasitic power—the energy black hole during high-speed cruise—its physical expression is rigorously set to be proportional to the cube of the relative velocity between the UAV fuselage and the wind speed vector of the surrounding air environment, i.e., Pparasite = 21ρCDA. vg-vw 3. The system integrates the aforementioned extremely complex nonlinear aerodynamic energy loss functional with the flight time penalty weight ρ, and leverages the mathematical advantage of the MINCO framework, which can directly provide the closed-form analytical gradient of the objective function with respect to intermediate time segments, to drive the L-BFGS quasi-Newton nonlinear optimizer for extremely rapid solution. In microsecond-level iterations, the spatiotemporal deformation algorithm automatically pushes intermediate waypoints within safe corridors and intelligently stretches or compresses the flight time of each path segment. This prevents the system from falling into the deadlock caused by traditional constant speed logic. Instead, it can not only automatically solve for the "maximum range speed" that physically minimizes the total energy integral, but also automatically steer the UAV to deflect its course when facing strong valley gusts, intelligently bypassing local building obstructions or tailwinds, thus achieving an astonishing leap in endurance in long-endurance industrial inspection scenarios.

[0038] Once the perfect 3D spatiotemporal trajectory is generated, the system enters the final execution and fallback defense phase, namely, self-disruption low-level collaborative control and state machine failure protection (corresponding to step S4). To ensure that the UAV can track this energy-optimal flat trajectory with high fidelity, this system completely abandons the traditional modular architecture where upper-level algorithm crashes result in crashes and the low-level PID has extremely poor wind resistance. At the computing power architecture level, this system adopts a very clear heterogeneous distributed deployment. All environmental perception, incremental mapping, corridor generation, and MINCO spatiotemporal solving algorithms are deployed on an onboard flight companion computer with powerful parallel computing capabilities; while high-frequency attitude control and motor PWM wave drive are strictly retained in the low-level Pixhawk flight control board with extremely high hard real-time performance. The high-latency traditional serial port middleware has been eliminated between the two, and nanosecond-level synchronous memory read and write are performed directly through a data distribution service link based on ROS 2 and the micro-XRCE-DDS (Data Distribution Service) protocol. The flight computer publishes spatiotemporal data frames containing desired position, velocity, and acceleration feedforward to the trajectory setpoint topic of the PX4 firmware at an ultra-high frequency of 50Hz to 100Hz, forcibly seizing the offboard mode guidance control of the underlying flight controller.

[0039] Running within the underlying flight control system is an extremely powerful Active Disturbance Rejection Control (ADRC) algorithm. Due to the complex gyroscopic effects and unpredictable gusts present in real-world multi-rotor environments, the ADRC algorithm ingeniously packages all unknown internal nonlinear dynamics and external environmental disturbances into a single macroscopic "total disturbance f". The system constructs a Linear Extended State Observer (LESO) at the bottom layer, which simultaneously tracks the controlled attitude and its first derivative in the feedback loop, driving the differential equations through a highly refined closed-loop error. =-β3e, within tens of milliseconds, the newly added extended state variable z3 of the system rapidly converges and infinitely approximates the total disturbance in the real physical world. After obtaining an extremely accurate estimate of the total disturbance, the underlying control law adopts an inverse dynamic feedforward compensation strategy, forcing the control quantity u output to the motor to be equal to the difference between the ideal basic control quantity and the disturbance compensation term (i.e., u=b0b0u0-z3). This inverse compensation instantly reduces the originally extremely tightly coupled UAV system to an extremely easy-to-control dual integrator model at the physical level. Therefore, when the UAV encounters downbursts or turbulence in urban canyons at low altitudes, it does not need to wait to be blown off course before performing delayed feedback correction, but is fully perceived and actively canceled by the system the instant the disturbance occurs, and its trajectory tracking lateral error is suppressed to a very low limit.

[0040] Finally, to ensure the system never crashes under extreme conditions (such as visual perception process deadlock or loose DDS communication cables), this invention employs an extremely stringent heartbeat communication state machine and fail-safe logic at the lowest logical level. During flight, the flight companion computer must continuously send heartbeat packets to the underlying flight controller via the DDS link. The system internally sets strict COM_OF_LOSS_T timeout monitoring parameters. Once the update frequency of the control heartbeat packets is detected to fall below the 2Hz safety threshold and last for more than 0.5 seconds, the underlying flight controller's state machine lifecycle management module will immediately and ruthlessly strip the flight companion computer of its advanced algorithm navigation control. The system will refuse to execute any outdated position commands received last time and will directly trigger the Failsafe failure protection mechanism, forcibly switching to Hold (hovering in place), Return (ascending to a safe altitude and returning to base), or Land (landing in place) degraded modes. Simultaneously, this embodiment also grants the ground control station pilot the highest physical interruption preemption privilege. By configuring a specific remote controller joystick mapping channel in the ground control station software, during high-risk tests, the pilot does not need to input commands through complex terminals. A mere flick of the remote controller joystick by the pilot's finger will trigger the underlying flight controller to detect the interruption signal generated by this minute displacement and instantly sever the advanced write permissions of the accompanying computer at the very bottom of the physical control chain. The entire system seamlessly switches back to manual control mode with stability augmentation assistance. This comprehensive closed-loop design, encompassing sensing hardware, geometric planning, aerodynamics, and nanosecond-level DDS safety firewall, completely fills all the theoretical gaps in existing technologies that prevent practical application. This enables the method and system provided by this invention to truly possess the practical capability to handle highly reliable industrial-grade commercial flights in complex denied scenarios.

[0041] The above description is merely a preferred and detailed embodiment of the present invention and is not intended to limit the invention. For those skilled in the art, the sensing architecture based on error state manifold filtering, the spatiotemporal constraint optimization based on aerodynamic energy cube, and the highly reliable failure protection system based on ADRC feedforward wind resistance, all covered by the present invention, can be modified and combined in various ways. Any modifications, equivalent substitutions, or scenario adaptations made within the core logic and engineering principles of the present invention should be included within the strict protection scope of the present invention.

Claims

1. A fully autonomous three-dimensional navigation method for unmanned aerial vehicles (UAVs) in complex environments, characterized in that: The method includes the following steps: Step S1, High-precision state estimation and real-time incremental mapping: Acquire high-frequency inertial measurement data and low-frequency point cloud data collected by UAV onboard sensors, and use error state Kalman filtering to construct a tightly coupled odometry to obtain real-time state estimation of the UAV in continuous manifold space; at the same time, based on the point cloud data, by introducing independent insertion and deletion queues for local parallel processing, a dynamic Euclidean symbolic distance field containing continuous distance gradient information of spatial obstacles is constructed and updated in real time. Step S2, Dimensional Reduction Skeleton Path Search and Safe Flight Corridor Construction: In the free space provided by the dynamic Euclidean symbolic distance field, an initial discrete polyline path that avoids obstacles is generated using a jump point search algorithm; then, along the initial discrete polyline path, ellipsoidal expansion and half-space intersection iteration are performed in three-dimensional space to generate a safe flight corridor composed of a set of overlapping convex polyhedra. Step S3, considering spatiotemporal trajectory optimization with dynamic constraints and aerodynamic energy adaptation: The safe flight corridor is treated as a strict convex space inclusion constraint. Utilizing the differential flatness characteristics of the multi-rotor UAV, the constrained optimal control problem is transformed into an unconstrained optimization problem with the coordinates of the intermediate waypoints and the time span allocation as decision variables. In the objective function, the energy consumption of the higher-order derivatives of the trajectory is comprehensively minimized, and a nonlinear aerodynamic energy loss functional composed of induced power, shape drag power, and parasitic power is deeply integrated. A three-dimensional spatiotemporal trajectory that conforms to the real physical dynamic limits and energy optimization of the UAV is generated through spatiotemporal deformation solution. Step S4, Active Disturbance Rejection (ADRROR) Underlying Cooperative Control and State Machine Failure Protection: The three-dimensional spatiotemporal trajectory is sent to the underlying flight controller via a data distribution service protocol. In the underlying flight controller, an ADRROR algorithm is used to utilize an extended state observer to unify the unknown internal dynamic cross-coupling and the external environmental wind field as a total disturbance for real-time online estimation and to apply feedforward compensation calculation, thereby controlling the UAV to accurately track the three-dimensional spatiotemporal trajectory. Simultaneously, a heartbeat communication state machine is deployed between the accompaniment computer and the flight controller for lifecycle management. When a control heartbeat packet timeout is detected, the navigation and guidance rights of the external control mode are forcibly revoked and a failure protection mechanism is triggered.

2. The fully autonomous three-dimensional navigation method for UAVs in complex environments according to claim 1, characterized in that, In the error state Kalman filtering of step S1: the UAV's true state vector is defined on a continuous manifold space, including the UAV's attitude rotation matrix, three-dimensional position, three-dimensional velocity, and the gyroscope and accelerometer zero bias of the inertial measurement unit; the tightly coupled odometry defines the small deviation between the true state and the nominal predicted state as the error state vector. The expression for the error state vector is: ； in, Indicates attitude error. Indicates positional error. Indicates speed error, and These represent the zero bias errors of the gyroscope and accelerometer, respectively. Meanwhile, an incremental kd-tree data structure is used when processing the point cloud data to support the insertion and deletion operations of dynamic obstacle point clouds, avoiding the reconstruction of the entire search tree during each frame matching.

3. The fully autonomous three-dimensional navigation method for UAVs in complex environments according to claim 1, characterized in that, In the execution of step S2, the ellipsoid expansion and half-space intersection iteration are performed as follows: the line segment searched at the front end is used as the seed point to initialize the ellipsoid and expand it outward. When the ellipsoid surface first contacts the obstacle point, the hyperplane is constructed and the half-space is defined with the contact point as the tangent point. After removing the external search domain of the half-space, iterative expansion continues until the intersection of the generated half-space sets constitutes a strict convex polyhedron. The form based on the intersection of the half-spaces is then transformed into a form composed of the convex hull of the polyhedron vertices to ensure that the trajectory control points are completely located in a collision-free free space.

4. The fully autonomous three-dimensional navigation method for UAVs in complex environments according to claim 1, characterized in that, In step S3, the nonlinear aerodynamic energy loss functional integrates the total propulsion energy consumption of the UAV flight and its total power demand. Decomposed into induced power Type I resistor power and parasitic power Wherein, the parasitic power The expression for overcoming the drag experienced by the drone's fuselage as it moves through the air is: ； in, air density, The drag coefficient of the fuselage. For the windward cross-sectional area, Let be the ground velocity vector of the UAV relative to the ground. The environmental wind speed vector is used; by integrating this parasitic power model during the optimization process, the maximum range speed is automatically found and intelligent flight around the environmental wind field is performed.

5. The fully autonomous three-dimensional navigation method for unmanned aerial vehicles (UAVs) in complex environments according to claim 1, characterized in that, In the spatiotemporal trajectory optimization of step S3, when transforming the constrained optimal control problem into an unconstrained optimization problem, the objective function is defined. for: ； in, The number of segments in the trajectory. The higher-order derivatives of the position trajectory are represented. This is an intermediate time segment. The regularization penalty weights are applied to the flight time; the unconstrained optimization problem can be directly solved by the objective function. Regarding the intermediate time segment The analytical gradient is obtained, and the time segment is automatically lengthened or shortened by a nonlinear optimizer to perform spatiotemporal deformation.

6. The fully autonomous three-dimensional navigation method for UAVs in complex environments according to claim 1, characterized in that, In step S4, the extended state observer of the active disturbance rejection control algorithm is a linear extended state observer, which integrates the internal nonlinear dynamic coupling of the system with the external environmental wind field as a total disturbance for real-time estimation; the linear extended state observer sets the state vector to include the estimated values ​​of the controlled position or angle, the estimated value of the velocity, and the estimated value of the total disturbance. It passes through closed-loop error The driving differential equation update, where the update of the total perturbation estimate satisfies: ;in, The observer gain; the actual control quantity output to the UAV motors. Designed as an ideal basic control quantity The difference between the interference compensation term and the interference compensation term, i.e. ;in, To control the gain coefficient; to eliminate disturbances through feedforward compensation and perform inverse dynamic compensation.

7. The fully autonomous three-dimensional navigation method for UAVs in complex environments according to claim 1, characterized in that, In the state machine failure protection mechanism of step S4: the heartbeat communication state machine operates based on the micro-XRCE-DDS protocol. When the continuous heartbeat packet update frequency is detected to fall below the 2Hz threshold and the duration exceeds the set timeout parameter, the state machine of the underlying flight controller immediately deprives the advanced algorithm of control and forces a fallback to the mode of hovering in place, returning to home at a safe altitude, or landing in place. Furthermore, a specific remote controller joystick channel is configured through the ground control station. When a slight displacement of the remote controller joystick is detected, the underlying trigger interrupt input instantly cuts off the position writing right and switches back to the manual control mode with stabilization.

8. A fully autonomous three-dimensional navigation system for unmanned aerial vehicles (UAVs) in complex environments, characterized in that: The system includes: a high-precision state estimation and real-time incremental mapping module: used to acquire high-frequency inertial measurement data and low-frequency point cloud data, construct a tightly coupled odometry using error state Kalman filtering to obtain real-time state estimation, and introduce a dual-queue mechanism to construct and update a dynamic Euclidean symbolic distance field containing continuous distance gradient information of spatial obstacles in real time; a dimensionality-reduced skeleton path search and safe flight corridor construction module: used to generate an initial discrete polyline path using a jump point search algorithm, and perform ellipsoidal expansion and half-space intersection iteration along the path to generate a safe flight corridor; a spatiotemporal trajectory optimization module considering dynamic constraints and aerodynamic energy adaptation: used to transform the constrained optimal control problem into an unconstrained optimization problem with intermediate waypoint coordinates and time span allocation as decision variables, and integrate a nonlinear aerodynamic energy loss functional into the objective function to generate a three-dimensional spatiotemporal trajectory; and an active disturbance rejection (ADRR) low-level cooperative control and state machine failure protection module: used to employ an ADRR algorithm and use an extended state observer to estimate and compensate for the total system disturbance, and deploy a heartbeat communication state machine through a data distribution service protocol to forcibly trigger a failure protection mechanism when the heartbeat packet times out.

9. The fully autonomous three-dimensional navigation system for unmanned aerial vehicles (UAVs) in complex environments according to claim 8, characterized in that, The system adopts a heterogeneous distributed deployment in terms of hardware architecture: the high-precision state estimation and real-time incremental mapping module, the dimensionality reduction skeleton path search and safe flight corridor construction module, and the spatiotemporal trajectory optimization module considering dynamic constraints and aerodynamic energy adaptation are deployed and run in the airborne escort computer; the self-disturbance rejection underlying cooperative control and state machine failure protection module are deployed and run in the underlying flight control board.

10. The fully autonomous three-dimensional navigation system for unmanned aerial vehicles (UAVs) in complex environments according to claim 9, characterized in that, The airborne escort computer and the underlying flight control board are configured with a data distribution service link based on ROS2 and micro-XRCE-DDS protocols for nanosecond-level synchronous read and write. The system also includes a ground control station and a remote controller that communicate with the underlying flight control board. The remote controller is configured with a specific joystick channel, which is used to cut off the position write right of the airborne escort computer from the physical logic link and switch back to manual control mode when the underlying flight control board detects an interruption input of a slight displacement of the remote controller joystick.

Images