Unmanned aerial vehicle adaptive path planning and real-time adjustment method in complex dynamic environment

Abstract

The invention discloses an unmanned aerial vehicle adaptive path planning and real-time adjustment method in a complex dynamic environment, and belongs to the technical field of unmanned aerial vehicle autonomous navigation and path planning. The method aims to solve the technical problem that the unmanned aerial vehicle is difficult to realize global path safety optimization and sudden threat real-time obstacle avoidance at the same time in a complex scene with static and high-speed dynamic obstacles. According to the technical scheme, the method comprises the steps of fusing multi-sensor data and predicting the future position of a dynamic obstacle; on the basis, a global cost map containing dynamic risk information is constructed, and an initial path is planned by adopting an improved A * algorithm in which a dynamic risk cost item is added into a heuristic function; the relative motion relation with a dynamic obstacle is calculated in real time during flight, the collision risk is judged through double threshold values, and when it is judged that collision is about to happen, a speed obstacle method is adopted to calculate an obstacle avoidance speed vector so as to adjust a path in real time. The method is mainly used for improving the flight autonomy and safety of the unmanned aerial vehicle in a dense dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and path planning technology for unmanned aerial vehicles (UAVs). More specifically, this invention relates to a method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments. Background Technology

[0002] In the field of autonomous drone flight, especially in complex environments where static obstacles coexist with dynamic obstacles whose motion is uncertain, achieving safe and reliable path planning and flight control remains a persistent technical challenge. Existing technologies typically face several interrelated problems and limitations when dealing with such complex dynamic environments.

[0003] First, there are shortcomings in environmental perception and understanding of dynamic obstacle behavior. While drones can carry various sensors such as lidar and visual sensors to acquire environmental data, each sensor has its own characteristics and limitations in terms of accuracy, frequency, range, and anti-interference capabilities. Specifically, the laser beam emitted by lidar experiences severe scattering and attenuation in media such as rain, fog, smoke, and dust, leading to a sharp reduction in effective detection range or even failure. Visual sensors heavily rely on ambient lighting conditions; their recognition and ranging performance significantly decreases or even becomes unusable at night, in backlight, or during drastic changes in lighting. Existing methods often struggle to generate consistent, reliable, and continuous estimates of the motion state of dynamic obstacles (including real-time position, speed, and, more importantly, future intentions) when fusing these heterogeneous, asynchronous, and potentially conflicting multi-source data. The fundamental reason is that the motion of dynamic obstacles is not always uniform or linear; it may involve complex maneuvers such as acceleration, deceleration, or turning. Traditional state estimation methods, if using a single, fixed motion model for prediction, will rapidly accumulate prediction errors when the actual target motion deviates from the model assumptions, leading to severely inaccurate judgments of the obstacle's future position. This inaccuracy in prediction directly creates safety risks for subsequent route planning, because the planning is based on potentially erroneous future environmental information.

[0004] Secondly, regarding path planning algorithms, traditional global planning methods (such as the classic A* algorithm and its many variants on grid maps) are mainly optimized for known static environments. When these methods are directly applied to dynamic environments, their core flaw lies in the lack of consideration for the "time" dimension and the probability of "risk." Algorithms typically treat the detected position of a dynamic obstacle at a given instant as a temporary static obstacle, or simply avoid it based on its relative position at the current moment. This results in a path that is essentially "spatially optimal" rather than "spatiotemporally safe." A drone might be planned to a position where it is currently collision-free, but will soon intersect with the trajectory of a moving obstacle. In other words, existing planning methods generally lack the ability to proactively and forward-lookingly avoid the future trajectories of dynamic obstacles, failing to quantify future collision risks as part of the path cost during the planning phase, thus only passively waiting for a conflict to occur before reacting.

[0005] Secondly, there is a disconnect and contradiction at the coordination level between real-time obstacle avoidance and global path planning. To cope with the uncertainty of dynamic environments, many systems have introduced local real-time obstacle avoidance modules (such as reactive behavior-based methods), which make rapid avoidance decisions based on instantaneous sensor information. However, this local reaction and the aforementioned global planning are usually designed independently in layers. This raises two main problems: First, there is a conflict in decision-making logic. Local obstacle avoidance behavior may issue commands that are completely opposite to the global path direction in order to avoid a temporarily approaching obstacle, resulting in disjointed and inefficient UAV movement, or even falling into local oscillations. Second, there is a lack of risk buffering mechanisms. There is no coherent risk assessment gradient between global planning and real-time obstacle avoidance. The system often can only switch between two extreme states: "completely risk-free" (which may be too conservative to plan) and "emergency collision threat has occurred," lacking an intermediate "risk warning and gradient avoidance" stage. This forces the UAV to either frequently perform sharp turns and other violent maneuvers, affecting mission efficiency and flight stability, or have its reaction time window compressed when a real danger occurs.

[0006] Finally, the underlying reason for the disconnect and lack of coordination among the aforementioned perception, planning, and control modules lies in the failure of the system architecture design to incorporate "prediction" as a central theme throughout. The predictive information from the perception module is not adequately transmitted to the planning module in an algorithmically understandable and computable form; the long-term intentions of the planning module fail to effectively guide the decision thresholds and strategies of the local obstacle avoidance module. This disconnect makes it difficult for the entire system to make intelligent flight decisions in dynamic environments that achieve both global optimization and local safety, as well as both forward-looking prediction and real-time response. The challenge in solving these problems lies in designing a unified framework within limited computing resources that organically integrates uncertain perception information, complex spatiotemporal prediction, efficient global search, and agile local responses, while ensuring its stability and real-time performance. This is precisely the bottleneck that UAV path planning technology urgently needs to overcome in complex dynamic environments. Summary of the Invention

[0007] To achieve these objectives and other advantages of the present invention, the present invention provides a method for adaptive path planning and real-time adjustment of unmanned aerial vehicles (UAVs) in complex dynamic environments, comprising the following steps: S1. Collect environmental data using multiple airborne sensors on the UAV, including at least lidar and vision sensors. The environmental data includes the three-dimensional coordinates of static obstacles, the three-dimensional coordinates and movement speed of dynamic obstacles. Use a Kalman filter algorithm to fuse the data from multiple airborne sensors to generate the tracking trajectory of the dynamic obstacles and predict the position of the dynamic obstacles in the next 2 to 5 seconds. S2. Based on the three-dimensional coordinates of the static obstacle and the predicted trajectory of the dynamic obstacle, a global cost map is constructed; based on the global cost map, an improved A* algorithm is used to plan the initial path of the UAV, and the predicted position of the dynamic obstacle is added as a cost factor to the heuristic function of the improved A* algorithm. S3. While the UAV is flying along the initial path, the environmental data is updated in real time through the various airborne sensors, and the real-time relative distance and real-time relative speed between the UAV and each dynamic obstacle are calculated based on the updated tracking trajectory of the dynamic obstacles. For sudden dynamic threats that are not fully considered in the global cost map, when the real-time relative distance is less than a first threshold and the real-time relative speed is greater than a second threshold, it is determined that a collision is about to occur. The range of the first threshold is 3m to 10m, and the range of the second threshold is 1m / s to 5m / s. S4. Once it is determined that a collision is about to occur, the speed obstacle method is used to solve for an obstacle avoidance speed vector. This obstacle avoidance speed vector must satisfy the condition that the minimum distance between the UAV and all dynamic obstacles in the future is greater than the safe distance, which is in the range of 1m to 5m. S5. Adjust the flight path of the UAV according to the obstacle avoidance speed vector, and send the adjusted path to the flight controller of the UAV to control the flight of the UAV.

[0008] Preferably, in step S1, generating the tracking trajectory of the dynamic obstacle and predicting the position of the dynamic obstacle within the next 2 to 5 seconds specifically includes the following steps: Data from the various airborne sensors are processed in time synchronization to form fused observation data for each dynamic obstacle; Using the fused observation data, the tracking trajectory of each dynamic obstacle is updated through the Kalman filter algorithm, and its current motion state characteristics are obtained. The motion state characteristics include the current acceleration value and the change in the current velocity direction. An acceleration threshold and a direction change threshold are set for each dynamic obstacle, with the acceleration threshold ranging from 0.5 m / s². 2 Up to 2 m / s 2 The range of the direction change threshold is 30° to 60°; For each dynamic obstacle, a Kalman filter process model for prediction is adaptively selected based on the comparison between its current acceleration value and the acceleration threshold, and the comparison between its current velocity direction change and the direction change threshold. If the current acceleration value is greater than the acceleration threshold, or the change in the current velocity direction is greater than the change in direction threshold, then a uniform acceleration motion model or a cooperative turning model is selected as the process model for the dynamic obstacle. If the above conditions are not met, then the dynamic obstacle maintains a constant velocity motion model as its process model; Using a process model selected for each dynamic obstacle, its position at a series of equally spaced time points within the next 2 to 5 seconds is predicted based on its current motion state characteristics, with the equally spaced time intervals ranging from 0.1 to 0.5 seconds. The predicted positions of each dynamic obstacle are connected in chronological order to form its corresponding predicted trajectory.

[0009] Preferably, the step of constructing the global cost map specifically involves: constructing a three-dimensional grid map as the global cost map; setting the spatial location of the three-dimensional coordinates of the static obstacle as an impassable area or a high-cost area; and setting the spatial location of the predicted trajectory of the dynamic obstacle as a risk cost area that changes over time and whose cost is positively correlated with the movement speed, based on the time information of the predicted trajectory and the movement speed of the corresponding dynamic obstacle.

[0010] Preferably, when performing path search based on the 3D raster map, the improved A* algorithm calculates the dynamic risk cost R(n) through the following process: Get the coordinates of path node n defined in the current 3D raster map; Obtain the predicted trajectories of all the dynamic obstacles within the time window [t, t+T], where the time window T ranges from 2s to 5s; For each dynamic obstacle i, calculate the Euclidean distance from the current node n to a series of discrete predicted position points on its predicted trajectory, and take the minimum value among them as the minimum distance di; Set a risk distance threshold Dr, ranging from 3 m to 15 m; For the i-th dynamic obstacle, if its distance di≤Dr, its individual risk cost ri is calculated according to the formula ri=(vi / di)×K, where vi is the scalar value of the current speed of the dynamic obstacle and K is an adjustment coefficient greater than zero. If the distance di > Dr, then the individual risk cost ri of the dynamic obstacle is set to zero; The dynamic risk cost R(n) is the sum of the individual risk costs of all dynamic obstacles, i.e., R(n) = ∑ri; The improved A* algorithm uses the heuristic function H(n)=G(n)+D(n)+R(n) to evaluate candidate nodes and selects the node with the smallest heuristic function value for path expansion, where G(n) represents the actual movement cost from the starting node of the path to the current node n, and D(n) represents the Euclidean distance from the current node n to the target node.

[0011] Preferably, in the formula ri=(vi / di)×K for calculating the individual risk cost ri, the value of distance di is constrained to be no less than a preset minimum protection distance dmin, wherein the range of dmin is 0.05 m to 0.2 m.

[0012] Preferably, in step S3, the first threshold is less than the risk distance threshold Dr.

[0013] Preferably, in step S3, the calculation process for the real-time relative distance and real-time relative velocity between the UAV and each dynamic obstacle is as follows: Obtain the current position coordinates Pu and velocity vector Vu of the UAV in the global coordinate system; Obtain the tracking trajectory of each dynamic obstacle that reflects its current state, and extract the position coordinates Po and velocity vector Vo of each dynamic obstacle at the current moment from the tracking trajectory; For each dynamic obstacle, calculate the Euclidean distance between the position coordinates Pu and Po, which is used as the real-time relative distance D to the dynamic obstacle. For each dynamic obstacle, calculate the vector difference between the velocity vector Vu and the velocity vector Vo to obtain the relative velocity vector V, i.e., V = Vu - Vo; Calculate the unit direction vector U corresponding to the real-time relative distance D, where U = (Pu - Po) / D; Calculate the dot product of the relative velocity vector V and the unit direction vector U; if the dot product is greater than zero, set the real-time approach velocity Vc to the dot product value; if the dot product is less than or equal to zero, set the real-time approach velocity Vc to zero. Set a maximum effective evaluation distance Dm, which ranges from 20 m to 50 m; If the real-time relative distance D is greater than the maximum effective evaluation distance Dm, then the dynamic obstacle is determined to be outside the effective evaluation range, and subsequent collision determination calculations for the dynamic obstacle are ignored. If the real-time relative distance D is less than or equal to the maximum effective evaluation distance Dm, then the calculated real-time relative distance D and the real-time approach speed Vc are retained for subsequent collision determination.

[0014] Preferably, the maximum effective assessment distance Dm is greater than the risk distance threshold Dr.

[0015] Preferably, the maximum effective evaluation distance Dm is adaptively adjusted according to the current flight speed of the UAV; the higher the current flight speed of the UAV, the larger the value of the maximum effective evaluation distance Dm.

[0016] Preferably, the multiple airborne sensors also include millimeter-wave radar; the millimeter-wave radar is used to provide supplementary environmental perception data in adverse weather conditions such as rain, fog, smoke, and dust, or in scenarios where stable speed measurement of non-cooperative dynamic targets is required.

[0017] The present invention includes at least the following beneficial effects: The UAV adaptive path planning and real-time adjustment method in complex dynamic environments described in the present invention, through a series of progressively advanced technical improvements, produces a series of interrelated and synergistic beneficial effects on path planning and flight safety of UAVs in complex dynamic environments.

[0018] First, by establishing a complete technical framework integrating environmental perception, trajectory prediction, global path planning, and real-time response adjustments, a systematic solution is provided for the autonomous flight of UAVs in dynamic environments. This framework enables UAVs to systematically handle the uncertainties brought about by dynamic obstacles, rather than making isolated reactions, thereby improving the overall consistency of mission execution and the rationality of decision-making.

[0019] Secondly, at the perception and prediction level, by analyzing the real-time motion characteristics of dynamic obstacles and adaptively switching prediction models accordingly, the accuracy of future trajectory predictions is significantly improved. This provides more reliable and forward-looking environmental information input for subsequent path planning, reducing the risk of planning failure due to prediction errors from the outset. Furthermore, by constructing a unified 3D grid map to structurally represent the environment and transforming the spatiotemporal prediction information of dynamic obstacles into a risk field that changes over time in the map, subsequent planning algorithms can perform efficient quantitative calculations within a unified space that integrates dynamic and static information.

[0020] In the core path planning stage, a dynamic risk cost term is innovatively introduced into the heuristic function of the traditional A* algorithm, directly quantifying the spatiotemporal conflict risk between UAV path nodes and predicted trajectories of dynamic obstacles into a calculable path cost. This improvement enables the path search process to proactively avoid future collision risks, planning safer global paths in both time and space, achieving a qualitative leap from static obstacle avoidance to dynamic collision avoidance. Simultaneously, by setting a minimum protection distance constraint, the numerical stability of the risk cost calculation under extreme conditions is ensured, improving the algorithm's robustness in practical engineering applications.

[0021] At the level of system coordination and real-time security, a tiered safety response mechanism, from far-field monitoring and risk warning to emergency collision avoidance, was constructed by setting and associating multiple distance thresholds. This not only clarifies the respective responsibilities of global planning and local response, but more importantly, ensures seamless logical integration between the two: the planning layer optimizes and avoids potential risks at a greater distance; if optimization fails or an accident occurs, the response layer initiates at a closer distance to ensure absolute safety. This design avoids conflicts and oscillations in system behavior. Furthermore, by accurately calculating the projection of relative velocity onto the collision direction as the approach velocity, a strict distinction is made between actual oncoming approach and movement in other directions, greatly improving the accuracy of collision threat identification and reducing false alarms. Finally, by adaptively adjusting the system's monitoring range according to the drone's own speed, an intelligent balance is achieved between early warning in high-risk (high-speed) conditions and resource conservation in low-risk (low-speed) conditions, further optimizing the overall performance and adaptability of the system.

[0022] In summary, these technological improvements work together to enable unmanned aerial vehicle (UAV) systems to achieve forward-looking safe path planning, accurate real-time threat assessment, and smooth and efficient emergency response in complex and dynamic environments. Ultimately, while ensuring flight safety, they effectively improve the intelligence level of autonomous flight and mission execution efficiency.

[0023] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0024] Figure 1 This is a flowchart illustrating the UAV adaptive path planning and real-time adjustment method under complex dynamic environments described in this invention. Detailed Implementation

[0025] The present invention will now be described in further detail with reference to specific embodiments, so that those skilled in the art can implement it based on the description.

[0026] It should be understood that terms such as “having,” “comprising,” and “including” as used herein do not exclude the presence or addition of one or more other elements or combinations thereof.

[0027] It should be noted that, unless otherwise specified, the experimental methods described in the following implementation plan are all conventional methods, and the reagents and materials described are all commercially available unless otherwise specified.

[0028] like Figure 1 As shown, this invention provides a method for adaptive path planning and real-time adjustment of unmanned aerial vehicles (UAVs) in complex dynamic environments, comprising the following steps: S1. Collect environmental data using multiple airborne sensors on the UAV, including at least lidar and vision sensors. The environmental data includes the three-dimensional coordinates of static obstacles, the three-dimensional coordinates and movement speed of dynamic obstacles. Use a Kalman filter algorithm to fuse the data from multiple airborne sensors to generate the tracking trajectory of the dynamic obstacles and predict the position of the dynamic obstacles in the next 2 to 5 seconds. S2. Based on the three-dimensional coordinates of the static obstacle and the predicted trajectory of the dynamic obstacle, a global cost map is constructed; based on the global cost map, an improved A* algorithm is used to plan the initial path of the UAV, and the predicted position of the dynamic obstacle is added as a cost factor to the heuristic function of the improved A* algorithm. S3. While the UAV is flying along the initial path, the environmental data is updated in real time through the various airborne sensors, and the real-time relative distance and real-time relative speed between the UAV and each dynamic obstacle are calculated based on the updated tracking trajectory of the dynamic obstacles. For sudden dynamic threats that are not fully considered in the global cost map, when the real-time relative distance is less than a first threshold and the real-time relative speed is greater than a second threshold, it is determined that a collision is about to occur. The range of the first threshold is 3m to 10m, and the range of the second threshold is 1m / s to 5m / s. S4. Once it is determined that a collision is about to occur, the speed obstacle method is used to solve for an obstacle avoidance speed vector. This obstacle avoidance speed vector must satisfy the condition that the minimum distance between the UAV and all dynamic obstacles in the future is greater than the safe distance, which is in the range of 1m to 5m. S5. Adjust the flight path of the UAV according to the obstacle avoidance speed vector, and send the adjusted path to the flight controller of the UAV to control the flight of the UAV.

[0029] In the above technical solution, the UAV carries at least LiDAR and visual sensors for sensing hardware. For example, a multi-beam LiDAR can be selected as the core ranging sensor, combined with a visual camera using a global shutter CMOS image sensor. To cope with complex and changing real-world environments, especially to compensate for the sensing shortcomings of LiDAR and visual sensors in adverse weather (rain, fog) and low-light conditions, and to improve system robustness, the various airborne sensors can further include millimeter-wave radar. Millimeter-wave radar (such as the 77 GHz band) has a significantly stronger penetrating ability than lasers due to its longer wavelength, and its performance remains stable under changing lighting conditions, providing continuous and reliable distance and velocity information. In terms of assembly, the LiDAR is usually mounted on a gimbal bracket above or in front of the UAV to obtain a wide field of view; the visual camera and millimeter-wave radar are respectively arranged in sensor compartments reserved in the front and sides of the fuselage to ensure multi-angle coverage. The working process is as follows: After each sensor is powered on, it collects data at its own frequency. The onboard computing unit (such as a high-performance embedded processor) performs timestamp synchronization and coordinate system unification. Subsequently, the Kalman filter algorithm is applied to correlate and estimate the state of the same dynamic obstacle in the observation points of different sensors. The Kalman filter process model can use a uniform velocity or uniform acceleration model, where the state vector includes position and velocity, and the observation vector is the fused coordinates and velocity. Through the prediction steps of this filter, the state of the target in the future can be calculated, for example, continuously predicting the position at the end of each second within the next 3 seconds, thus forming a predicted trajectory. The length of the prediction time window, for example, is set to 3 seconds, which is determined based on the braking distance and reaction time of a typical UAV.

[0030] After receiving the list of static obstacle coordinates and the predicted trajectories of dynamic obstacles from the perception module, the path planning module begins its work. First, it constructs a digitized global cost map within the task-defined 3D space. This map can be a uniformly divided 3D grid, with each grid cell measuring 0.1 m square. During construction, the cost of the grid cell containing the static obstacle coordinates is set to an extremely high constant (e.g., 255), representing impassable status. For the predicted trajectories of dynamic obstacles, the grid cells they might occupy at each future moment are assigned a gradient of risk costs based on the distance to the predicted moment and the obstacle's current speed; the faster the speed and the closer the time, the higher the cost. This process can be accomplished by querying a predefined cost-speed-time relationship table. Based on this, an improved A* algorithm is used for searching. The heuristic function H(n) in the algorithm adds a dynamic risk cost term R(n) in addition to the traditional terms G(n) (actual movement cost) and D(n) (Euclidean distance to the target). The calculation method for R(n) is as follows: For the current path node n to be evaluated, calculate its spatial distance to all points on the predicted trajectories of all dynamic obstacles, and take the minimum value dmin. Set a risk distance threshold, for example, 8 m. If dmin is less than this threshold, calculate the risk cost contributed by the obstacle according to the formula (obstacle speed / dmin) × coefficient K, where the coefficient K can be 1.5 to adjust the risk sensitivity; otherwise, the risk cost is zero. Finally, sum them up to obtain R(n). The algorithm will continuously expand the search until it finds a path from the starting point to the ending point that has the lowest combined distance cost and dynamic risk cost, which is then output as the initial flight path.

[0031] As the drone flies along its planned path, the perception module continuously operates, providing the latest environmental data. The real-time collision avoidance module cycles at a high frequency (e.g., 20 Hz). First, it acquires the latest position coordinates and velocity vectors of the drone itself and each dynamic obstacle from the updated tracking trajectory. Then, it calculates the real-time Euclidean distance between the drone and each obstacle, as well as their relative velocity vectors. A crucial step is calculating the "approach velocity": this is obtained by calculating the projection scalar of the relative velocity vector onto the unit direction vector "from the obstacle towards the drone". A positive value indicates that the two are approaching; a negative or zero value indicates that they are moving away or relatively stationary. Next, a collision determination is made: a first distance threshold, such as 5 m, and a second approach velocity threshold, such as 2 m / s, are set. Only when the real-time distance is less than 5 m and the approach velocity is greater than 2 m / s does the system determine that a collision is imminent and immediately trigger an obstacle avoidance response. Once the determination is valid, the module invokes the velocity obstacle method for calculation. This method uses geometric analysis to find a velocity vector among all possible new velocity vectors that allows the UAV to maintain at least a safe distance (e.g., 2 m) from all future threats and obstacles, while remaining as close as possible to the originally planned velocity direction. This calculated optimal obstacle avoidance velocity vector is converted into attitude and throttle commands and sent to the UAV's flight controller (such as the Pixhawk series of open-source flight controllers). The flight controller then executes the actual motor control, resulting in a smooth local adjustment from the original path to complete the emergency collision avoidance maneuver. After the collision avoidance action is executed, the system reassesses the environment and may trigger a global path replanning.

[0032] Through the above specific implementation, this method can construct a complete and collaborative autonomous flight control system for unmanned aerial vehicles (UAVs). Its technical effects are mainly reflected in the following aspects: First, by fusing multi-source heterogeneous sensor data and performing adaptive trajectory prediction, the reliability and foresight of the system's understanding of the behavioral intentions of dynamic obstacles in complex environments are improved, providing a more accurate information basis for safety decisions. Second, by innovatively introducing a dynamic risk cost term based on predicted trajectory into the global path planning algorithm, the path search process can not only avoid static obstacles but also actively quantify and avoid potential spatiotemporal conflicts with dynamic obstacles in the future, realizing a shift from passive reaction to proactive forward planning and significantly improving the safety of the global path. Third, by designing a dual-threshold collision judgment criterion based on precise geometric relationships (relative distance and approach speed), and using this to trigger real-time local obstacle avoidance based on the speed obstacle method, the accuracy of emergency threat identification and the reliability of obstacle avoidance actions are greatly improved, effectively reducing misjudgments and unnecessary maneuvers. Fourth, by establishing a collaborative mechanism between the "global risk planning layer" and the "local emergency collision avoidance layer," and by setting tiered distance thresholds (such as maximum assessment distance, risk planning threshold, and emergency collision avoidance threshold), the system can adopt different strategies under different risk levels. This ensures both the efficiency and smoothness of daily flights while guaranteeing a safety baseline in extreme situations, achieving a balance between safety and efficiency. The tight coupling of all modules within the system ultimately enables the UAV to possess more stable and intelligent autonomous flight capabilities in complex environments containing unpredictable moving obstacles.

[0033] In other technical solutions, step S1 involves generating the tracking trajectory of the dynamic obstacle and predicting its position within the next 2 to 5 seconds, specifically including the following steps: Data from the various airborne sensors are processed in time synchronization to form fused observation data for each dynamic obstacle; Using the fused observation data, the tracking trajectory of each dynamic obstacle is updated through the Kalman filter algorithm, and its current motion state characteristics are obtained. The motion state characteristics include the current acceleration value and the change in the current velocity direction. An acceleration threshold and a direction change threshold are set for each dynamic obstacle, with the acceleration threshold ranging from 0.5 m / s². 2 Up to 2 m / s 2 The range of the direction change threshold is 30° to 60°; For each dynamic obstacle, a Kalman filter process model for prediction is adaptively selected based on the comparison between its current acceleration value and the acceleration threshold, and the comparison between its current velocity direction change and the direction change threshold. If the current acceleration value is greater than the acceleration threshold, or the change in the current velocity direction is greater than the change in direction threshold, then a uniform acceleration motion model or a cooperative turning model is selected as the process model for the dynamic obstacle. If the above conditions are not met, then the dynamic obstacle maintains a constant velocity motion model as its process model; Using a process model selected for each dynamic obstacle, its position at a series of equally spaced time points within the next 2 to 5 seconds is predicted based on its current motion state characteristics, with the equally spaced time intervals ranging from 0.1 to 0.5 seconds. The predicted positions of each dynamic obstacle are connected in chronological order to form its corresponding predicted trajectory.

[0034] In the above technical solution, at the hardware level, a multi-beam lidar supporting Ethernet or CAN bus communication can be selected. Its internal circuit board uses FR-4 epoxy glass cloth laminate, and the laser emitter and receiver use semiconductor materials. A stereo vision module based on a global shutter sensor can be selected, with its lens module using optical glass lenses. An integrated millimeter-wave radar chip module can be selected, with its antenna substrate using high-frequency PCB materials. In terms of assembly, these sensors are all fixed to rigid structural parts of the UAV body via shock-absorbing brackets, such as the central platform and forward crossbeam on top of the body, and connected to the corresponding interfaces (such as Ethernet, USB, and CAN ports) of the onboard main control computer via cables. The collaborative working process is as follows: the main control computer runs a time synchronization daemon process, which receives timestamps attached to the data streams from each sensor (usually provided by the sensor's internal clock or GPS timing module) and aligns them using network time protocols or hardware trigger signals to compensate for millisecond-level differences caused by varying processing delays among the sensors. The aligned data is fed into a data fusion middleware, which, based on a preset coordinate system transformation relationship (obtained through joint calibration), uniformly transforms the LiDAR point cloud, visual detection box, and millimeter-wave radar point traces to a body coordinate system centered on the UAV. For observations of the same physical obstacle from different sensors, the fusion algorithm performs correlation matching based on their similarity in spatial location, size, and motion speed. Ultimately, it outputs a set of time-synchronized, coordinate-unified fused observation data for each successfully tracked dynamic obstacle, including its three-dimensional coordinates and instantaneous velocity vector.

[0035] After acquiring the fused observation data, the system maintains an independent Kalman filter instance for each tracked dynamic obstacle. This filter can be implemented using an open-source filtering algorithm library. The filter operates iteratively: in each processing cycle (e.g., 50ms), the optimal estimate from the previous cycle (i.e., the latest state of the tracking trajectory) is used as a priori, and state prediction is performed using a process model (such as a basic uniform velocity model). Subsequently, the fused observation data of the current cycle is used as news, compared with the predicted value, and the state estimate is updated using the Kalman gain formula to obtain the optimal state of the current cycle, thus completing the continuation of the tracking trajectory. Position and velocity information can be directly extracted from the updated state vector. To obtain the current acceleration value, the rate of change of the velocity vector between two consecutive cycles can be calculated. To obtain the change in the current velocity direction, the angle between the velocity vectors of the current cycle and the previous cycle can be calculated (using the inverse solution of the dot product formula). These two values ​​constitute the current motion state characteristics. The acceleration threshold can be set to 1 m / s². 2 The directional change threshold can be set to 45°. These thresholds are empirical values ​​set based on statistical analysis of the behavioral patterns of moving bodies such as pedestrians and vehicles in common urban environments, aiming to effectively distinguish between uniform linear motion and motion with acceleration, deceleration, or turning intentions.

[0036] Based on the calculated motion state characteristics, the system dynamically adjusts the process model within its Kalman filter for each obstacle. This is a process of logical judgment and model switching. Specifically, for each tracked obstacle, the system compares its current acceleration value with a preset acceleration threshold (1 m / s²). 2 The system compares the current velocity direction change with a preset direction change threshold (45°). If either comparison is true, the obstacle is determined to be in a maneuvering state. At this point, the system switches the filter's process model from the default uniform motion model to a model that better describes the maneuvering state, such as a uniform acceleration (CA) model or a cooperative turning (CT) model. This model switching involves updating the Kalman filter's state transition matrix and process noise matrix. If neither comparison is true, the uniform velocity (CV) model is maintained. After determining the process model, the system uses this model and the optimal state estimate of the Kalman filter at the current moment to perform forward iterative prediction. During prediction, starting from the current moment, the system recursively calculates the predicted position coordinates of the obstacle at a series of discrete time points (15 points in total) within a future period (e.g., the next 3 seconds) by forward calculation at fixed time intervals (e.g., 0.2 s). Finally, these discrete locations are connected by line segments in chronological order to form a continuous spatial polyline. This polyline is the predicted trajectory of the dynamic obstacle within a future time window and will be provided as a key input to the path planning module.

[0037] Through the aforementioned step-by-step implementation, this solution achieves the following beneficial effects in the dynamic obstacle trajectory prediction stage. First, by rigorously synchronizing and spatially fusing multi-source sensor data, inconsistencies and noise in observations of the same target are reduced, providing higher-quality and more consistent input for subsequent tracking algorithms, thereby improving the stability of trajectory initialization and maintenance. Second, by calculating acceleration and heading rate of change—two key motion features—in real time from continuously updated tracking trajectories, the system can quantitatively perceive changes in obstacle motion patterns, not just positional changes, enhancing the depth of understanding of obstacle behavior. Finally, and most notably, by comparing real-time perceived motion features with preset thresholds and adaptively switching the Kalman filter's intrinsic mathematical model (CV, CA, or CT), the prediction algorithm no longer relies on a single, fixed motion assumption. This allows the system to more flexibly adapt to different motion patterns that dynamic obstacles may exhibit at different times (such as changing from straight ahead to turning), thus providing trajectory predictions that more closely reflect actual future motion scenarios in various situations. Compared to prediction methods that use a single fixed model, this adaptive mechanism can reduce long-term prediction errors caused by model mismatch to a certain extent, providing more reliable information on future environmental situations for UAV forward-looking path planning.

[0038] A hardware-triggered synchronization scheme is adopted. The airborne main control board generates a precise hardware interrupt signal (such as a PPS pulse) and sends it to the trigger interfaces of the lidar, vision sensor, and millimeter-wave radar, forcing each sensor to acquire data at the same physical moment, and using this moment as the reference timestamp of the data frame.

[0039] In other technical solutions, the step of constructing a global cost map specifically involves: constructing a three-dimensional grid map as a global cost map; setting the spatial location of the three-dimensional coordinates of the static obstacle as an impassable area or a high-cost area; and setting the spatial location of the predicted trajectory of the dynamic obstacle as a risk cost area that changes over time and whose cost is positively correlated with the movement speed, based on the time information of the predicted trajectory and the movement speed of the corresponding dynamic obstacle.

[0040] In the above technical solution, at the software level, existing open-source or commercial robot operating system map building libraries can be used as the foundation. During implementation, a three-dimensional array data structure representing a three-dimensional raster map is first defined and maintained in the memory of the onboard computing unit (which can be a high-performance embedded processor or an onboard industrial computer). This map covers the three-dimensional airspace planned for the UAV mission, for example, a cube with dimensions of 100 meters long, 100 meters wide, and 50 meters high. The map resolution, i.e., the side length of each three-dimensional raster, can be set to 0.5 meters, a compromise value chosen after considering map accuracy, memory usage, and the computational complexity of path search. During map initialization, the cost of all rasters is set to a base value representing "free passage," such as 0. This map acts as a dynamically updated database, with its update frequency synchronized with the call frequency of the path planning module, for example, 2 Hz. Map data is stored in the random access memory of the computing unit, its physical carrier being a semiconductor chip, and it exchanges data with the processor core via a bus on the circuit board.

[0041] For a list of 3D coordinates of static obstacles provided by the perception module, the system iterates through each coordinate point in the list. For each coordinate point, the algorithm first calculates the specific grid index in the 3D grid map where that point is located. Then, the grid cost corresponding to that index is modified to a very high constant, such as 1000, to represent "impassable". To increase safety and avoid the planned path getting too close to obstacles due to map resolution or sensor errors, a "dilation" process is usually performed. This dilation process can be implemented based on a distance transformation algorithm, whereby for each grid marked as an obstacle, the cost of all grids within a certain Euclidean distance range around it is gradually increased according to the distance. The dilation radius can be set according to the size of the UAV and safety redundancy, for example, 1.5 meters. Through this process, static obstacles are represented on the map as an area with clear boundaries and high cost.

[0042] This step is the core of processing dynamic obstacle prediction trajectories. For each dynamic obstacle prediction trajectory provided by the prediction module (consisting of a series of timestamped future location points), the system injects its discretized spatiotemporal information into the raster map. The specific steps are: traversing every future predicted location point on the trajectory; calculating the 3D raster index for each location point; and then calculating the dynamic risk cost that the raster should be assigned at this moment based on the time difference Δt (in seconds) between the predicted point's future time and the current time, and the current scalar velocity v (in meters per second) of the dynamic obstacle. The risk cost calculation can follow a preset function, for example: Risk Value = (v / (Δt+1)) × W. Here, W is an adjustment weight coefficient, which can be set to 50. This formula embodies two core principles: first, risk is positively correlated with the obstacle's velocity v; the faster the velocity, the greater the potential threat; second, risk decays over time; the farther the predicted point is from the current time (the larger Δt), the higher the uncertainty, and the lower its cost. Finally, the calculated dynamic risk cost is added to the original cost of the current grid cell (which may be a base value of 0 or a static obstacle expansion value). Since the predicted trajectories of all dynamic obstacles are processed in this way, a risk cost field that dynamically changes over time and is spatially superimposed will be formed in the entire 3D grid map. Grid cells close to the recently predicted positions of high-speed moving obstacles will have a high comprehensive cost.

[0043] Constructing a global cost map through the specific steps described above yields the following beneficial technical effects. First, it encodes both dynamic and static obstacle information in the environment into a standardized 3D raster data structure, providing a uniformly formatted and directly computable input for subsequent path planning algorithms, simplifying the algorithm interface. Second, its dilation process for static obstacles intuitively reflects the physical dimensions and safety margin requirements of the UAV itself at the algorithm level, enhancing the safety of the planned path. The most crucial effect lies in the handling of dynamic obstacles: this method creatively integrates the spatiotemporal prediction information (when and where it will appear in the future) and motion intensity information (speed magnitude) of dynamic obstacles, transforming them into a static risk gradient field in map space. This allows traditional path planning algorithms, originally designed for static environments (such as A*), to indirectly perceive and understand future dynamic risks by querying raster cost values, thereby guiding the UAV to avoid high-risk spatiotemporal areas in advance. This method of constructing a "spatiotemporal risk field" is a crucial foundation for achieving forward-looking dynamic obstacle avoidance planning.

[0044] In other technical solutions, when performing path search based on the 3D raster map, the improved A* algorithm calculates the dynamic risk cost R(n) through the following process: Get the coordinates of path node n defined in the current 3D raster map; Obtain the predicted trajectories of all the dynamic obstacles within the time window [t, t+T], where the time window T ranges from 2s to 5s; For each dynamic obstacle i, calculate the Euclidean distance from the current node n to a series of discrete predicted position points on its predicted trajectory, and take the minimum value among them as the minimum distance di; Set a risk distance threshold Dr, ranging from 3 m to 15 m; For the i-th dynamic obstacle, if its distance di≤Dr, its individual risk cost ri is calculated according to the formula ri=(vi / di)×K, where vi is the scalar value of the current speed of the dynamic obstacle and K is an adjustment coefficient greater than zero. If the distance di > Dr, then the individual risk cost ri of the dynamic obstacle is set to zero; The dynamic risk cost R(n) is the sum of the individual risk costs of all dynamic obstacles, i.e., R(n) = ∑ri; The improved A* algorithm uses the heuristic function H(n)=G(n)+D(n)+R(n) to evaluate candidate nodes and selects the node with the smallest heuristic function value for path expansion, where G(n) represents the actual movement cost from the starting node of the path to the current node n, and D(n) represents the Euclidean distance from the current node n to the target node.

[0045] In the above technical solution, firstly, the algorithm reads the coordinates of the current path node n to be evaluated from memory. Node n is the center point of a specific grid in a 3D raster map, and its coordinates are stored as 3D floating-point numbers. Subsequently, the algorithm retrieves the predicted trajectory data of all dynamic obstacles published by the perception and prediction modules within a unified future time window from shared memory or a message queue. The time window T can be set to 3.5 s, a trade-off between prediction effectiveness and computational load. Each predicted trajectory is represented as a series of 3D coordinate points arranged in chronological order, with the time interval between points set to 0.2 s. This data is transmitted from the perception module to the planning module via the internal bus of the onboard computing unit. The computing unit can be a high-performance microprocessor with floating-point arithmetic capabilities, whose internal arithmetic logic unit and registers are made of silicon-based semiconductor materials, responsible for executing specific calculation instructions.

[0046] For each dynamic obstacle i, the algorithm begins by calculating the spatial relationship between node n and its predicted trajectory. Specifically, it iterates through each discrete predicted location point in the list of predicted trajectories for that obstacle. For each location point, a mathematical library function is called to calculate the three-dimensional Euclidean distance from node n to that point. After iterating through all predicted points, the minimum value is found through comparison and recorded as the minimum distance di. This process essentially solves for the shortest distance from the node to a discrete polyline segment. Next, risk assessment is performed. The system presets a risk distance threshold Dr, for example, set to 8 m. The calculated minimum distance di is compared with this threshold. If di is less than or equal to 8 m, node n is considered to be within the risk impact range of the obstacle, and a risk cost needs to be calculated. The individual risk cost ri is calculated according to the formula ri=(vi / di)×K, where vi is the current scalar value of the movement speed (in m / s) obtained in real time from the tracking trajectory of the dynamic obstacle, and K is a positive adjustment coefficient, which can be set to 1.2, to adjust the weight of the risk cost in the entire cost function. If di is greater than 8 m, then node n is determined to be outside the safe distance, and the individual risk cost ri of the obstacle is directly assigned to zero.

[0047] After traversing all dynamic obstacles and assessing individual risks, the algorithm sums all non-zero individual risk costs *ri* to obtain the total dynamic risk cost *R(n)* corresponding to the current node *n*, i.e., *R(n)* = ∑*ri*. This *R(n)* value is then incorporated into the comprehensive evaluation system of the A* algorithm. The improved heuristic function is defined as *H(n)* = *G(n)* + *D(n)* + *R(n)*. Here, *G(n)* is the accumulated actual cost of moving from the starting point to node *n*, typically determined by the number of grids moved or the actual distance; *D(n)* is the Euclidean distance from node *n* to the target point, serving as a heuristic estimate of the remaining path. At each expansion step, the algorithm calculates the *H(n)* value for all candidate next-hop nodes. The final path selection decision is based on minimizing *H(n)*: the algorithm selects the node with the smallest current *H(n)* value from the open set as the next expansion point. Thus, the dynamic risk cost *R(n)*, as an additional penalty term, directly affects the node's priority. Even if a path has a short geometric distance (small G(n)+D(n)), but passes through the recent prediction area of ​​multiple high-speed dynamic obstacles, its total cost H(n) will be high due to its large R(n) value, thus being eliminated in the search; conversely, a slightly longer path but far away from dynamic threats may be selected because its total cost is lower.

[0048] The dynamic risk cost calculation process integrated into the A* algorithm achieves the following beneficial technical effects. Its core effect lies in transforming external, complex spatiotemporal risk predictions into quantifiable and comparable path node costs within the algorithm, thus embedding the abstract goal of "safety" into a classic search framework driven by "minimizing cost." Specifically, firstly, by calculating the minimum distance from a node to all predicted trajectories and applying a threshold, the algorithm can accurately perceive the spatial proximity of each candidate location to a dynamic threat over a future period. Secondly, by introducing a cost formula proportional to obstacle speed and inversely proportional to distance, risk assessment considers not only spatial proximity but also the urgency of the threat—obstacles approaching at high speed incur greater costs, which better aligns with the physical laws of real collision risks. Finally, and most decisively, by linearly superimposing the dynamic risk cost R(n) with the traditional costs G(n) and D(n), the algorithm automatically performs a multi-objective optimization of safety and efficiency at each path decision. This ensures that the final planned path is a balanced solution that combines the shortest path length with the minimum dynamic collision risk, rather than simply being the shortest path based on geometric information. From an algorithmic perspective, this provides a core mechanism to guarantee that UAVs can generate forward-looking, low-risk global paths in dynamic environments.

[0049] Within the dynamic risk cost region, the dynamic risk cost Cdynamic of the grid corresponding to the predicted location point at a future time t is calculated using the following formula: Cdynamic = α × v / (t + τ). Where v is the current velocity scalar value of the dynamic obstacle (m / s), t is the time difference between the predicted time and the current time (s), τ is the time decay constant (e.g., 1 s), and α is the cost scaling factor (e.g., 50). This value is superimposed on the static obstacle cost.

[0050] In other technical solutions, in the formula ri=(vi / di)×K for calculating the individual risk cost ri, the value of distance di is constrained to be no less than a preset minimum protection distance dmin, wherein the range of dmin is 0.05 m to 0.2 m.

[0051] In the above technical solution, when the discrete location points of the predicted trajectory of the dynamic obstacle are extremely close to the path planning node n of the UAV, the calculated minimum distance di will approach zero. In the original formula ri=(vi / di)×K, if di approaches 0 infinitely, the individual risk cost ri will approach infinity. Although this mathematically expresses the intention of "absolute avoidance", in engineering systems implemented by finite precision digital systems (such as computer processors using the IEEE 754 floating-point standard), directly calculating and dividing by a very small value will bring two main risks: first, it may trigger floating-point operation anomalies or cause numerical overflow, making the program unstable; second, it will produce a very large or even unreasonable cost value, which may excessively dominate the entire cost function H(n), causing the path search algorithm to make non-optimal or oscillating decisions due to this single extreme value, affecting the smoothness and practicality of the planned path.

[0052] Therefore, introducing a minimum protection distance dmin adds an engineering safety boundary to the mathematical model. The function di=max(actual calculated distance, dmin) ensures that the denominator in the formula is always no less than a small positive number (e.g., 0.1 m), fundamentally avoiding division by zero errors and drastic numerical fluctuations caused by excessively small denominators. This guarantees the numerical stability of the algorithm and the robustness of the program under all operating conditions. The introduction of dmin effectively sets a reasonable upper limit for the risk cost ri. When the actual distance is less than dmin, the formula will calculate the cost based on dmin. This means that at extremely close distances, the risk cost is "saturated" and no longer increases indefinitely. This reflects an engineering trade-off: when the distance between the UAV and the predicted trajectory of the obstacle is as small as the danger threshold represented by dmin (e.g., 0.1 m), the system has already determined it to be in an "extremely high risk" state. There is no need to further distinguish between 0.01 meters and 0.09 meters; instead, the higher-level emergency collision avoidance logic should be triggered. This prevents the planning algorithm from idly oscillating between two extremely high-risk path options due to the oversensitivity of the cost function. Setting dmin within the range of 0.05 m to 0.2 m is based on a comprehensive consideration of the physical dimensions of a typical UAV system, sensor positioning errors, and safety redundancy. The lower limit of 0.05 m takes into account the typical error level of high-precision positioning systems (such as RTK-GPS and LiDAR) under good conditions, ensuring that the protection value is greater than the noise amplitude. The upper limit of 0.2 m is comparable to the physical radius (or safety buffer radius) of a small UAV, ensuring that when the algorithm determines that the risk cost is saturated, the theoretical distance between the UAV and the obstacle has entered the danger zone of physical collision, at which point the planning algorithm should give way to the more urgent real-time obstacle avoidance layer. This range is a compromise engineering parameter that effectively addresses numerical issues without excessively distorting the behavior of the original risk model at normal distances.

[0053] In some other technical solutions, in step S3, the first threshold is less than the risk distance threshold Dr.

[0054] In the above technical solution, the risk distance threshold Dr acts on the global path planning layer. Its function is to proactively identify and avoid potentially risky areas during the pathfinding phase, essentially a preventative strategy. The first threshold acts on the real-time collision avoidance layer, its function is to determine immediate collision threats during the flight execution phase, a last-minute safeguard strategy. Setting the first threshold to be less than Dr logically forces the risk warning range of the planning layer to be greater than the emergency braking range of the reaction layer, establishing a clear "warning-response" gradient. Its direct effect is to avoid confusion in system decision-making: it ensures that when the UAV enters an area marked as risky by the planning layer due to prediction errors or other reasons (where the distance is less than Dr but greater than the first threshold), the system remains within the scope of "planning optimization," with an opportunity to mitigate the risk by adjusting the path. Only when the risk continues to approach, exceeding the more stringent first threshold, is the highest-priority emergency collision avoidance triggered. This design eliminates the logical contradiction of the planning layer deeming the path safe while the execution layer suddenly issues an alarm, and provides a continuous decision buffer for the transition from optimization to emergency intervention, ensuring smooth and predictable system behavior. From a safety engineering perspective, it essentially allocates different risk response time windows for drones, using the longer lead time of the planning layer to optimize efficiency, while reserving the necessary minimum reaction time for the response layer to ensure absolute safety.

[0055] It should be specifically noted that the set risk distance threshold Dr and the first threshold satisfy the relationship of "the first threshold < Dr", which reflects the hierarchical defense and cooperation mechanism of the system design. The planning layer (using the risk distance threshold Dr) is an optimization layer, and its goal is to prospectively guide the UAV to avoid the risk area (within Dr) that may be affected by the future trajectory of dynamic obstacles during the global path planning stage, so as to optimize the safety and efficiency of the path. However, due to the uncertainty of environmental perception, the inherent error of dynamic obstacle trajectory prediction, and the sudden high-speed maneuver that may occur to obstacles, global planning cannot guarantee 100% coverage of all immediate threats. Therefore, the real-time collision avoidance layer (using the first threshold) serves as a safety guarantee layer, and its responsibility is to handle the emergency collision threats that the planning layer fails to completely avoid or suddenly appears. This layer makes precise real-time collision determination based on the latest sensor data at a closer distance (within the first threshold). This dual-threshold design establishes a safety redundancy: the planning layer is responsible for handling normal and predictable risks, striving to prevent problems before they occur; the execution layer is responsible for covering extreme and sudden risks, ensuring flight safety in the worst-case scenario. The two are clearly distinguished in terms of spatial range (Dr > the first threshold) and functional positioning and work together to form a complete and coherent safety system from risk warning to emergency response, rather than a logical contradiction.

[0056] In some other technical solutions, in step S3, the calculation process of the real-time relative distance and real-time relative speed between the UAV and each dynamic obstacle is as follows: Obtain the position coordinate Pu and velocity vector Vu of the UAV in the global coordinate system at the current moment; Obtain the tracking trajectory of each dynamic obstacle reflecting the state at the current moment, and extract the position coordinate Po and velocity vector Vo of each dynamic obstacle at the current moment from the tracking trajectory; For each dynamic obstacle, calculate the Euclidean distance between the position coordinate Pu and the position coordinate Po as the real-time relative distance D from this dynamic obstacle; For each dynamic obstacle, calculate the vector difference between the velocity vector Vu and the velocity vector Vo to obtain the relative velocity vector V, that is, V = Vu - Vo; Calculate the unit direction vector U corresponding to the real-time relative distance D, where U = (Pu - Po) / D; Calculate the dot product of the relative velocity vector V and the unit direction vector U; if the dot product is greater than zero, set the real-time approaching speed Vc to the value of the dot product; if the dot product is less than or equal to zero, set the real-time approaching speed Vc to zero; Set a maximum effective evaluation distance Dm, and its range is 20 m to 50 m; If the real-time relative distance D is greater than the maximum effective evaluation distance Dm, then the dynamic obstacle is determined to be outside the effective evaluation range, and subsequent collision determination calculations for the dynamic obstacle are ignored. If the real-time relative distance D is less than or equal to the maximum effective evaluation distance Dm, then the calculated real-time relative distance D and the real-time approach speed Vc are retained for subsequent collision determination.

[0057] In the above technical solution, the UAV's own position coordinates Pu and velocity vector Vu can be derived from its navigation system. This navigation system can be a loosely coupled integrated navigation unit, containing a global satellite navigation system receiver and an inertial measurement unit (IMU). The receiver's antenna can be mounted on top of the UAV's fuselage, and its substrate is made of high-frequency composite material. The IMU's gyroscope and accelerometer chips are made of silicon-based microelectromechanical systems (MEMS) materials and are typically mounted near the center of gravity to reduce the impact of vibration. The operation is as follows: the navigation software continuously fuses pseudorange and carrier phase information from satellites with angular velocity and force data from inertial sensors. Through Kalman filtering, it outputs the UAV's three-dimensional position, velocity, and attitude in a global coordinate system (such as the N / A coordinate system). The position coordinates Pu and velocity vector Vu are published in floating-point format via the onboard bus. Simultaneously, the tracking trajectory reflecting the current state of each dynamic obstacle is read from the shared memory area of ​​the perception module. This trajectory is maintained by the perception module's filtering algorithm (such as a Kalman filter), and its latest output represents the optimal state estimate for the current moment. The current position coordinates Po and velocity vector Vo of each obstacle are extracted from it, and the data format is consistent with the state of the UAV itself.

[0058] After obtaining the drone's state (Pu, Vu) and the state of each obstacle (Po, Vo), the following calculation sequence is performed independently for each obstacle. First, the real-time relative distance D is calculated: a mathematical library function is called, based on the formula D=sqrt((Pux-Pox)). 2 +(Puy-Poy) 2 +(Puz-Poz) 2 First, calculate the three-dimensional Euclidean distance between the two points, which characterizes the absolute spatial separation. Second, calculate the relative velocity vector V: perform vector subtraction V = Vu - Vo, resulting in a three-dimensional vector V describing the drone's direction and speed relative to the obstacle. Then, calculate the unit direction vector U: first calculate the vector difference (Pu - Po) from the obstacle to the drone, then divide it by its magnitude, D obtained in the previous step, to obtain the normalized direction vector U = (Pu - Po) / D. This step implicitly checks if D is non-zero. Finally, calculate the approach velocity Vc: this is crucial for threat assessment. Calculate the dot product (scalar product) of the relative velocity vector V and the unit direction vector U, i.e., Vc.candidate =Vx×Ux+Vy×Uy+Vz×Uz. The physical meaning of this dot product is the projected component of the relative velocity along the line connecting the two velocities. According to geometric principles, if this value is greater than zero, it indicates that V has a positive component in the U direction, meaning the two are approaching each other; in this case, the real-time approaching velocity Vc is set to this dot product value. If this value is less than or equal to zero, it indicates that the two are moving away or remaining relatively stationary; in this case, Vc is set to zero. This step precisely distinguishes between head-on approach and other relative motion states.

[0059] After completing the core calculations described above, the system executes a filtering logic to improve computational efficiency. A maximum effective evaluation distance, Dm, is set for this purpose, for example, 30 m. This threshold is an empirical value designed to define a reasonable range of concern. Subsequently, the calculated real-time relative distance D is compared with Dm. If D is greater than 30 m, the dynamic obstacle is determined to be too far away and not within the range requiring focused evaluation in the current cycle. The system will ignore subsequent collision determination calculations for this obstacle and directly process the next obstacle in the list. If D is less than or equal to 30 m, the obstacle is considered a valid evaluation target. The system retains the two key parameters calculated in this cycle: the real-time relative distance D and the real-time approach speed Vc (which may be a positive number or zero), and sends them to the subsequent collision determination logic (i.e., the module that compares with the first and second thresholds) for final risk assessment. This filtering mechanism ensures that computational resources are concentrated on a small number of targets that pose a potential short-range threat to flight safety.

[0060] In some other technical solutions, the maximum effective assessment distance Dm is greater than the risk distance threshold Dr.

[0061] In the above technical solution, the maximum effective assessment distance Dm defines the starting boundary for the system to continuously track and calculate dynamic obstacles. Its primary function is to filter targets and concentrate computational resources on obstacles that may be relevant in the near future. The risk distance threshold Dr is the decision boundary at which the path planning algorithm begins to substantially quantify and avoid the future trajectory of the obstacle. By forcibly setting Dm to be greater than Dr, it ensures that any obstacle that may enter the planning risk zone (within the range of Dr) will inevitably be captured and included in the tracking by the system earlier (while it is still in the "monitoring buffer" between Dm and Dr). This provides the planning algorithm with a crucial "early warning time window" and a stable tracking data stream: when an obstacle gradually approaches from outside Dm into inside Dr, the planning module no longer calls the unstable initial value of a suddenly appearing new target, but rather trajectory information with more convergent state estimation based on a period of continuous observation, thereby significantly improving the reliability of risk assessment and the smoothness of path decision-making. If Dm is not greater than Dr, there may be situations where obstacles suddenly "intrude" into the planning risk zone before being detected by the system. In this case, due to the lack of sufficient historical observation data, the estimation of its state (especially speed and intention) may be inaccurate, causing the planning algorithm to either overreact or fail to make an effective avoidance due to insufficient information. Therefore, this parameter relationship ensures a smooth transition and information continuity from long-distance monitoring to short-distance planning decisions from the system architecture perspective.

[0062] In other technical solutions, the maximum effective evaluation distance Dm is adaptively adjusted according to the current flight speed of the UAV; the higher the current flight speed of the UAV, the larger the value of the maximum effective evaluation distance Dm.

[0063] In the aforementioned technical solution, the drone's flight speed directly determines the distance it can move per unit time and the braking distance required from sensing to executing obstacle avoidance. When the drone flies at high speed, its inertia is greater, and the physical space and time window required from threatening identification to completing the avoidance maneuver increases significantly. If the maximum effective assessment distance Dm remains a fixed value at this time, the system may face risks: when an obstacle enters the fixed monitoring range, the time left for the drone to assess, make decisions, and maneuver may be insufficient, especially for obstacles approaching head-on at high speed, which can easily lead to emergency situations. Therefore, adaptively increasing the Dm value according to the current flight speed essentially dynamically extends the system's warning distance. For example, by using a linear or piecewise function relationship, Dm can increase with the speed, which is equivalent to turning on a farther "searchlight" in advance for high-speed states. In this way, the system can detect potential threats at a greater distance and initiate tracking and assessment, reserving more reaction time and decision space for subsequent path planning or emergency collision avoidance. Conversely, when the drone is moving at low speed or hovering, the required braking distance is short, so the Dm value is automatically reduced to avoid unnecessary continuous tracking calculations for obstacles that are too far away and do not pose an imminent threat, thus saving limited computing resources. This adaptability allows the system's perception focus range to dynamically expand and contract with the subject's movement, ensuring a corresponding safety lead time at various speeds while also improving overall computational efficiency. The maximum effective assessment distance Dm is dynamically adjusted according to the drone's current ground speed Vu, specifically: Dm = Dm min +η×Vu. Where Dm min The minimum monitoring distance is η (e.g., 20 m), and η is the speed gain coefficient (e.g., 2 s), meaning that for every 1 m / s increase in speed, the monitoring distance increases by 2 meters. This linear relationship ensures a longer warning time during high-speed flight.

[0064] In other technical solutions, the various airborne sensors also include millimeter-wave radar; the millimeter-wave radar is used to provide supplementary environmental perception data in adverse weather conditions such as rain, fog, smoke, and dust, or in scenarios where stable speed measurement of non-cooperative dynamic targets is required.

[0065] This technical solution utilizes a heterogeneous sensor combination, with LiDAR and vision sensors at its core and optional millimeter-wave radar as a supplement. The system fully leverages the high-precision 3D ranging of LiDAR, the rich texture and recognition capabilities of vision sensors, and the stable speed and ranging advantages of millimeter-wave radar in adverse weather conditions. This combination is not simply a hardware stacking exercise, but a collaborative enhancement design to address the uncertainty of perception in complex dynamic environments. Especially in challenging environments such as rain, fog, smoke, and dust, when the performance of LiDAR and vision sensors degrades, the continuous and stable target motion data provided by millimeter-wave radar, fused with data from other sensors, effectively maintains the continuity of the system's tracking trajectory for dynamic obstacles and the accuracy of its state estimation. Experimental data shows that, under simulated moderate rain conditions, the stability of trajectory prediction for dynamic obstacles (measured by the variance of the prediction position error) is improved compared to a system using only LiDAR and vision, significantly enhancing the system's reliability and environmental adaptability under all-weather conditions.

[0066] Although embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, other modifications can be easily made. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and embodiments shown and described herein.

Claims

1. A method for adaptive path planning and real-time adjustment of unmanned aerial vehicles (UAVs) in complex dynamic environments, characterized in that, Includes the following steps: S1. Collect environmental data using multiple airborne sensors on the UAV, including at least lidar and vision sensors. The environmental data includes the three-dimensional coordinates of static obstacles, the three-dimensional coordinates and movement speed of dynamic obstacles. Use a Kalman filter algorithm to fuse the data from multiple airborne sensors to generate the tracking trajectory of the dynamic obstacles and predict the position of the dynamic obstacles in the next 2 to 5 seconds. S2. Construct a global cost map based on the three-dimensional coordinates of the static obstacle and the predicted trajectory of the dynamic obstacle; Based on the global cost map, the initial path of the UAV is planned using an improved A* algorithm, and the predicted position of the dynamic obstacle is added as a cost factor to the heuristic function of the improved A* algorithm. S3. While the UAV is flying along the initial path, the environmental data is updated in real time through the various airborne sensors, and the real-time relative distance and real-time relative speed between the UAV and each dynamic obstacle are calculated based on the updated tracking trajectory of the dynamic obstacles. For sudden dynamic threats that are not fully considered in the global cost map, when the real-time relative distance is less than a first threshold and the real-time relative speed is greater than a second threshold, it is determined that a collision is about to occur. The range of the first threshold is 3m to 10m, and the range of the second threshold is 1m / s to 5m / s. S4. Once it is determined that a collision is about to occur, the speed obstacle method is used to solve for an obstacle avoidance speed vector. This obstacle avoidance speed vector must satisfy the condition that the minimum distance between the UAV and all dynamic obstacles in the future is greater than the safe distance, which is in the range of 1m to 5m. S5. Adjust the flight path of the UAV according to the obstacle avoidance speed vector, and send the adjusted path to the flight controller of the UAV to control the flight of the UAV.

2. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 1, characterized in that, In step S1, the tracking trajectory of the dynamic obstacle is generated and the position of the dynamic obstacle is predicted within the next 2 to 5 seconds, specifically including the following steps: Data from the various airborne sensors are processed in time synchronization to form fused observation data for each dynamic obstacle; Using the fused observation data, the tracking trajectory of each dynamic obstacle is updated through the Kalman filter algorithm, and its current motion state characteristics are obtained. The motion state characteristics include the current acceleration value and the change in the current velocity direction. An acceleration threshold and a direction change threshold are set for each dynamic obstacle, with the acceleration threshold ranging from 0.5 m / s². 2 Up to 2 m / s 2 The range of the direction change threshold is 30° to 60°; For each dynamic obstacle, a Kalman filter process model for prediction is adaptively selected based on the comparison between its current acceleration value and the acceleration threshold, and the comparison between its current velocity direction change and the direction change threshold. If the current acceleration value is greater than the acceleration threshold, or the change in the current velocity direction is greater than the change in direction threshold, then a uniform acceleration motion model or a cooperative turning model is selected as the process model for the dynamic obstacle. If the above conditions are not met, then the dynamic obstacle maintains a constant velocity motion model as its process model; Using a process model selected for each dynamic obstacle, its position at a series of equally spaced time points within the next 2 to 5 seconds is predicted based on its current motion state characteristics, with the equally spaced time intervals ranging from 0.1 to 0.5 seconds. The predicted positions of each dynamic obstacle are connected in chronological order to form its corresponding predicted trajectory.

3. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 2, characterized in that, The steps for constructing the global cost map are as follows: constructing a three-dimensional grid map as the global cost map; setting the spatial location of the three-dimensional coordinates of the static obstacle as an impassable area or a high-cost area; and setting the spatial location of the predicted trajectory of the dynamic obstacle as a risk cost area that changes with time and whose cost is positively correlated with the movement speed, based on the time information of the predicted trajectory and the movement speed of the corresponding dynamic obstacle.

4. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 3, characterized in that, When performing path search based on the aforementioned 3D raster map, the improved A* algorithm calculates the dynamic risk cost R(n) through the following process: Get the coordinates of path node n defined in the current 3D raster map; Obtain the predicted trajectories of all the dynamic obstacles within the time window [t, t+T], where the time window T ranges from 2s to 5s; For each dynamic obstacle i, calculate the Euclidean distance from the current node n to a series of discrete predicted position points on its predicted trajectory, and take the minimum value among them as the minimum distance di; Set a risk distance threshold Dr, ranging from 3 m to 15 m; For the i-th dynamic obstacle, if its distance di≤Dr, its individual risk cost ri is calculated according to the formula ri=(vi / di)×K, where vi is the scalar value of the current speed of the dynamic obstacle and K is an adjustment coefficient greater than zero. If the distance di > Dr, then the individual risk cost ri of the dynamic obstacle is set to zero; The dynamic risk cost R(n) is the sum of the individual risk costs of all dynamic obstacles, i.e., R(n) = ∑ri; The improved A* algorithm uses the heuristic function H(n)=G(n)+D(n)+R(n) to evaluate candidate nodes and selects the node with the smallest heuristic function value for path expansion, where G(n) represents the actual movement cost from the starting node of the path to the current node n, and D(n) represents the Euclidean distance from the current node n to the target node.

5. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 4, characterized in that, In the formula ri=(vi / di)×K for calculating the individual risk cost ri, the value of distance di is constrained to be no less than a preset minimum protection distance dmin, wherein the range of dmin is 0.05 m to 0.2 m.

6. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 4, characterized in that, In step S3, the first threshold is less than the risk distance threshold Dr.

7. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 6, characterized in that, In step S3, the calculation process for the real-time relative distance and real-time relative velocity between the UAV and each dynamic obstacle is as follows: Obtain the current position coordinates Pu and velocity vector Vu of the UAV in the global coordinate system; Obtain the tracking trajectory of each dynamic obstacle that reflects its current state, and extract the position coordinates Po and velocity vector Vo of each dynamic obstacle at the current moment from the tracking trajectory; For each dynamic obstacle, calculate the Euclidean distance between the position coordinates Pu and Po, which is used as the real-time relative distance D to the dynamic obstacle. For each dynamic obstacle, calculate the vector difference between the velocity vector Vu and the velocity vector Vo to obtain the relative velocity vector V, i.e., V = Vu - Vo; Calculate the unit direction vector U corresponding to the real-time relative distance D, where U = (Pu - Po) / D; Calculate the dot product of the relative velocity vector V and the unit direction vector U; If the dot product is greater than zero, then the real-time approach velocity Vc is set to the dot product value; If the dot product is less than or equal to zero, then the real-time approach velocity Vc is set to zero; Set a maximum effective evaluation distance Dm, which ranges from 20 m to 50 m; If the real-time relative distance D is greater than the maximum effective evaluation distance Dm, then the dynamic obstacle is determined to be outside the effective evaluation range, and subsequent collision determination calculations for the dynamic obstacle are ignored. If the real-time relative distance D is less than or equal to the maximum effective evaluation distance Dm, then the calculated real-time relative distance D and the real-time approach speed Vc are retained for subsequent collision determination.

8. The method for adaptive path planning and real-time adjustment of UAVs in complex dynamic environments as described in claim 7, characterized in that, The maximum effective assessment distance Dm is greater than the risk distance threshold Dr.

9. The method for adaptive path planning and real-time adjustment of unmanned aerial vehicles in complex dynamic environments as described in claim 8, characterized in that, The maximum effective evaluation distance Dm is adaptively adjusted according to the current flight speed of the UAV. The higher the current flight speed of the UAV, the larger the value of the maximum effective evaluation distance Dm.

10. The method for adaptive path planning and real-time adjustment of unmanned aerial vehicles in complex dynamic environments as described in claim 1, characterized in that, The various airborne sensors also include millimeter-wave radar; the millimeter-wave radar is used to provide supplementary environmental perception data in adverse weather conditions such as rain, fog, smoke, and dust, or in scenarios where stable speed measurement of non-cooperative dynamic targets is required.

Images