Robust path planning method and system for unmanned aerial vehicle in uncertain dynamic environment

Abstract

The application discloses a UAV robust path planning method and system for uncertain dynamic environment, relates to the field of path planning, and realizes real-time tracking of dynamic obstacles by using EKF, and constructs a probability repulsive field based on Mahalanobis distance, effectively solves the sensing vulnerability and static model limitation of a traditional APF algorithm, and provides a robust dynamic obstacle avoidance guide for random tree expansion; secondly, a fuzzy logic adaptive step (FLC-AS) module is designed; the module intelligently adjusts the expansion step according to the risk degree of a local environment and an exploration stage, and realizes dynamic balance of exploration safety and search efficiency; comparison and analysis of the method and various mainstream algorithms show that the PFLS-RRT* algorithm has excellent adaptability in a complex environment, and achieves the best comprehensive performance in key indexes such as path quality, planning efficiency and robustness.

Description

Technical Field

[0001] This invention belongs to the field of path planning, and more specifically, relates to a robust path planning method and system for unmanned aerial vehicles (UAVs) in uncertain dynamic environments. Background Technology

[0002] Unmanned Aerial Vehicle (UAV) technology, with its high mobility, low cost, and deployment flexibility, has demonstrated significant application value in fields such as military reconnaissance, logistics delivery, disaster relief, and environmental monitoring. As the key to achieving a high degree of autonomy for UAVs, autonomous navigation technology has become a cutting-edge research hotspot in robotics. Among them, the path planning module, as the core of the UAV decision-making system, has the main task of planning an optimal or near-optimal trajectory from the starting point to the destination in real time in a three-dimensional space full of uncertainties, satisfying dynamic constraints and effectively avoiding all static and dynamic obstacles.

[0003] Existing artificial potential field algorithms have significant drawbacks in complex environments: when the gravitational and repulsive forces are equal in magnitude and opposite in direction, the resultant force is zero, causing the drone to get stuck in the potential field depression at a non-target point and fail to reach the destination. This phenomenon is particularly common in environments with U-shaped obstacles or narrow passages. When the obstacle is too close to the target point, the repulsive force generated by the obstacle may cause the drone to oscillate near the target point and fail to converge. The classic APF is based on a static environment model and lacks the ability to predict the motion state of obstacles. In addition, the algorithm assumes that the environmental perception is ideal and noise-free. In practical applications, sensor noise is directly introduced into the repulsive force calculation, causing the planned trajectory to jitter or even leading to the risk of collision.

[0004] Classical RRT* algorithms rely on uniform random sampling across the entire space, lacking heuristic guidance. In vast or structurally complex spaces, these algorithms often require a large number of sampling points to find feasible paths, making it difficult to meet the demands of real-time navigation. The algorithms typically employ a globally fixed step size; an excessively large step size can easily lead to collisions or penetrations in areas with dense obstacles, while an excessively small step size can significantly reduce exploration efficiency in open areas. Classical RRT* algorithms are usually based on static snapshots of the environment for path searching, lacking prediction and real-time response mechanisms for dynamic environmental changes, making it difficult to cope with the threat of dynamic obstacles. Summary of the Invention

[0005] To address the problems in related technologies, this invention proposes a robust path planning method and system for unmanned aerial vehicles (UAVs) in uncertain dynamic environments, thereby overcoming the aforementioned technical problems in existing related technologies.

[0006] To solve the above-mentioned technical problems, the present invention is achieved through the following technical solution:

[0007] This invention provides a robust path planning method for unmanned aerial vehicles (UAVs) in uncertain dynamic environments, comprising the following steps:

[0008] S1. Define the four-dimensional state vector of the current obstacle of the UAV, introduce the anisotropic noise covariance matrix, and map it to the world coordinate system through the rotation matrix; then use the linear observation model and Kalman filter to update the state estimate.

[0009] S2. Set a dynamic time window, predict the state based on the estimation results in S1, the current distance between the obstacle and the drone, and the relative speed; perform probability prediction through the state transition matrix and process noise, extract the mean and covariance matrix of the predicted position; calculate the probability distribution statistical distance from the RRT* extension point to the predicted position.

[0010] S3. Set dynamic risk weights, integrate the probability distribution statistical distance, velocity phase, motion trend and predicted uncertainty risk calculated in S2 to reconstruct the probability repulsive field, and calculate the guiding resultant force;

[0011] S4. Calculation of the final step size of the guiding resultant force obtained from S3;

[0012] S5. Based on the calculation results in S4, add state variables to the RRT* node to detect hard and soft stagnation, and create dynamic restricted areas through random exploration; then inject the penalty cost of the dynamic restricted areas into the cost calculation and introduce a decay mechanism.

[0013] S6. Use the waypoints generated by PFL-RRT* as the initial control points for the B-spline; based on the injection in S5 and the cost calculation method after its introduction, iteratively optimize the control point set, generate cubic B-spline curves, perform curvature adaptive sampling and calculate trajectory costs until convergence and output the optimal control point set.

[0014] Preferably, step S1 includes the following steps:

[0015] S11. Define the current obstacle encountered by the drone during flight, denoted as the current obstacle; and assign the current obstacle to... The complete motion state at a given moment is defined as a four-dimensional vector;

[0016] S12. Construct the corresponding noise covariance matrix based on the four-dimensional vector in S11; specifically, this includes the following steps:

[0017] S121. Define two independent noise parameters: the standard deviation of the longitudinal acceleration noise of the current obstacle and the standard deviation of the lateral acceleration noise of the current obstacle.

[0018] S122, From the current obstacle in Extract the velocity vector from the optimal state at any given time; calculate a rotation matrix from the current obstacle's body coordinate system to the world coordinate system based on the velocity vector;

[0019] S123. Define the noise of the current obstacle in the body coordinate system as the current body coordinate system noise; then use the rotation matrix to map the current body coordinate system noise to the world coordinate system to obtain the acceleration covariance matrix in the world coordinate system.

[0020] The final noise covariance matrix is ​​constructed by using the components of the acceleration covariance matrix in the world coordinate system and combining them with the integral term of the sampling period.

[0021] S13. Use sensors to directly measure the current obstacle. The current position, and then the current obstacle in The position at any given time degenerates to a standard Kalman filter;

[0022] S14. Based on the current obstacles... The standard Kalman filter at each time point calculates the measurement residual, the covariance of the measurement residual, the optimal Kalman gain, and updates the posterior estimate and posterior covariance.

[0023] This anisotropic model enables EKF to generate predictive covariance ellipses that better reflect physical reality: when an obstacle is moving at high speed, its uncertainty ellipse expands primarily laterally (in the turning direction) rather than in the already determined longitudinal direction (in the velocity direction), thus achieving a smarter and more accurate risk assessment. Through continuous iteration of the prediction and update phases, EKF can robustly track dynamic obstacles in real time. Its final output, the posterior state (the most likely mean of the current obstacle) and posterior covariance (the uncertainty of this estimate), will serve as the core inputs for the P-APF repulsive field calculation in the next section.

[0024] Preferably, step S2 includes the following steps:

[0025] S21. Set a dynamic time window;

[0026] S22. Based on the calculation results in S14 and the dynamic time window, change the state from the current... Predicting at all times At any time, and according to The state transition matrix at each time step and the process noise are used to make a probabilistic prediction of the current obstacle state;

[0027] According to S11, the current obstacle is... Extracting the current obstacle from the complete motion state at any given moment The mean of the predicted position and the predicted position covariance matrix at each time step;

[0028] S23. Based on the current obstacles... Calculate the mean and covariance matrix of the predicted position at each time step, and calculate the statistical distance from the RRT* extension point to the probability distribution of the current obstacle's predicted position.

[0029] This heuristic definition allows Adaptive: When the current obstacle is far away or has a slow relative speed, this invention predicts a longer time. When an obstacle approaches at high speed, this invention predicts a shorter time... The focus is on the immediate collision risk; the introduction of Mahalanobis distance shifts the construction of the repulsive field from a purely geometric space to a probabilistic space, thereby enabling the perception of uncertainty.

[0030] Preferably, step S3 includes the following steps:

[0031] S31. Set dynamic risk weights;

[0032] S32. Based on the dynamic risk weight and the statistical distance from the RRT* extension point to the current obstacle prediction position probability distribution in S23, reconstruct the probabilistic repulsion field to obtain the final repulsion vector formula; calculate the guiding resultant force based on the reconstructed probabilistic repulsion field.

[0033] In this way, the tree expansion process of the PF-RRT* algorithm evolves from a simple geometric interpolation into a multi-objective force-guided process that integrates goal orientation, random exploration, dynamic intelligent avoidance, and static safety constraints. The P-APF repulsion (b) can sense the predicted uncertainty ellipse and intelligently avoid it along the minor axis (the most certain direction); while the traditional APF (a) blindly pushes away along the center point, ignoring the collision probability. Therefore, it has a significant advantage over the Euclidean distance-based APF algorithm. It also provides comprehensive and intelligent guidance for subsequent fuzzy logic adaptive step size (FLC-AS) decision-making.

[0034] Preferably, step S4 includes the following steps:

[0035] S41. The adaptive step size controller based on two-input fuzzy logic calculates the final step size.

[0036] Preferably, step S5 includes the following steps:

[0037] S51. Based on S1, S2, S4, and S4, add a state variable to each node of the RRT* tree to record the number of consecutive expansion failures or invalidities; in each iteration of the RRT* algorithm's main loop, select a node. And attempt to generate Afterwards, if Returning false indicates that the path has collided with the current obstacle, which is considered a hard stop. The state variable increments by 1; if the extended vector The projection length onto the direction vector toward the target is less than a preset minimum progress threshold. If this happens, the expansion is considered invalid and classified as a soft standstill. The state variable increments by 1; if there is no collision and the extension vector The projection length onto the direction vector toward the target is greater than ,Will The state variable is cleared to zero; if the state variable is greater than the preset threshold, S52 is executed and the node is reset. The center point of the trap is identified; the preset threshold ranges from 500 to 1000; CollisionFree is a collision detection function used to detect whether a straight path between two points collides with an obstacle in the environment; and These are the two input points; This represents the existing nodes in the tree. This represents the newly generated sampling point; the output is a Boolean value: True indicates that the path has no collision (safe), meaning the line connecting the two points does not cross any obstacle area; False indicates that the path has a collision (dangerous), meaning the line connecting the two points overlaps with or intersects with an obstacle.

[0038] S52. Call a simple random exploration function to perform multiple iterations; create a new dynamic restricted area at the center point of the trap. And add it to a global set of restricted areas;

[0039] S53. Inject the penalty cost of the dynamic restricted area into the cost calculation of RRT*, when RRT* attempts to... Connected to When the total cost is calculated, the formula is as follows:

[0040]

[0041] in, , They represent , The total cost; This is a weight used to balance the importance of path length and avoiding penalties; express Total DNZ penalty received;

[0042] S54. Based on S51, S52, and S53, a cost attenuation is introduced into the basic penalty cost, calculated as follows:

[0043]

[0044] in, This is the current simulation time. Attenuation rate; Indicates the number of simulation times corresponding to the current simulation time. The penalty for each dynamic restricted area; Indicates the creation timestamp; Indicates the basic penalty cost;

[0045] Preferably, step S6 includes the following steps:

[0046] S61. Input the initial waypoints generated by PFL-RRT* as the initial control point set for the B-spline. Based on the cost calculation method introduced in S54 after cost attenuation, it sets up information including current obstacle information and maximum curvature. Penalty weight , parameter;

[0047] S62. In each iteration, the current set of control points... Fine-tuning is performed to generate a new set of candidate control points. Then construct based on the candidate control point set Generate a complete cubic B-spline curve ;along Perform curvature adaptive sampling to obtain the current candidate trajectory sampling point set; and calculate the total cost of the current candidate trajectory sampling point set. ;Will Minimum cost compared to the current record Compare;

[0048] like Then As the minimum cost of the current record, Let this be the set of optimal candidate control points for the current record;

[0049] S63. Repeat S62 until the maximum number of iterations is reached or the cost function converges; output the optimal set of control points.

[0050] A robust path planning system for UAVs in uncertain dynamic environments includes a real-time estimation module for the current dynamic obstacle state, a probabilistic prediction module for future collision risks, a probabilistic repulsion field reconstruction module, a final extended step size calculation module, a dual-criteria stagnation detection logic-cost decay module, and a control point iterative optimization module.

[0051] The present invention has the following beneficial effects:

[0052] 1. This invention effectively addresses the perceptual vulnerability and static model limitations of the traditional APF algorithm by utilizing EKF to track dynamic obstacles in real time and constructing a probabilistic repulsion field based on Mahalanobis distance, thus providing robust dynamic obstacle avoidance guidance for random tree expansion. Secondly, a fuzzy logic adaptive step size (FLC-AS) module is designed. This module intelligently adjusts the expansion step size according to the risk level of the local environment and the exploration stage, achieving a dynamic balance between exploration safety and search efficiency. Finally, through simulation experiments in dense static and dynamic environments, the method of this invention is compared and analyzed with several mainstream algorithms. The results show that the PFLS-RRT* algorithm exhibits excellent adaptability in complex environments and achieves the best comprehensive performance in key indicators such as path quality, planning efficiency, and robustness.

[0053] 2. This invention constructs a closed-loop adaptive system of "monitoring-learning-avoidance," which mainly includes three core stages: First, the Trap Detection and Recognition (TDI) module monitors the growth status of the random tree in real time and uses a stall counter to determine whether the algorithm is trapped in a deadlock; once a trap is confirmed, DNZ registration (TER) is triggered, the current region is marked as a high-cost dynamic restricted area, and a short-term random walk is performed to physically escape the trap center; finally, in the Cost-Based Planner Integration (CPI)* stage, the algorithm integrates the time-varying Gaussian penalty field generated by DNZ into the cost function of RRT*; this mechanism forces the planner to actively avoid known trap regions in subsequent iterations, while the exponential decay characteristic of the penalty value ensures that the algorithm will not permanently block feasible paths due to misjudgment, thus achieving intelligent escape while ensuring probabilistic completeness.

[0054] 3. In this invention, by optimizing a small number of control points, the overall performance of the entire trajectory is comprehensively improved. Compared with pure geometric optimization methods, by explicitly quantizing and balancing multiple performance indicators, a trajectory with better overall performance can be generated, providing powerful and reliable backend support for the PFL-RRT* algorithm.

[0055] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0056] To more clearly illustrate the technical solutions of the embodiments of the invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0057] Figure 1 This is a flowchart illustrating the robust path planning method for UAVs in uncertain dynamic environments according to the present invention.

[0058] Figure 2 This is a schematic diagram illustrating the risks of this invention from different dimensions; in the diagram, d min d0 represents the boundary of the dynamic obstacle; r represents the repulsive force sensing range of the dynamic obstacle; and r represents the Euclidean distance between the UAV and the dynamic obstacle.

[0059] Figure 3 This is a schematic diagram of the repulsion ratio membership function of the present invention;

[0060] Figure 4 This is a schematic diagram of the normalized target distance membership function of the present invention;

[0061] Figure 5 This is a schematic diagram of the membership function of the step size scaling factor of the present invention; in the figure, SSF is the abbreviation for the output variable: step size scaling factor. Detailed Implementation

[0062] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0063] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0064] Example 1

[0065] Please see Figure 1 This embodiment presents a robust path planning method for UAVs in uncertain dynamic environments, comprising the following steps:

[0066] S1. Define the four-dimensional state vector of the current obstacle of the UAV, introduce the anisotropic noise covariance matrix, and map it to the world coordinate system through the rotation matrix; then use the linear observation model and Kalman filter to update the state estimate.

[0067] Specifically, in order to achieve real-time estimation of the state of dynamic obstacles, this invention provides a method for estimating the state of each tracked dynamic obstacle. i Maintain an independent Kalman filter;

[0068] This invention employs a classic model widely used in target tracking—the Constant Velocity (CV) model. The core assumption of the CV model is that obstacles tend to maintain their current velocity over short periods (i.e., between two sampling periods). This invention does not assume that obstacles "have no" acceleration, but rather treats unknown, abrupt accelerations. The model is treated as a perturbation to the CV model, that is, it is incorporated into the process noise. This approach greatly improves the robustness and applicability of the model, perfectly fitting the framework of Kalman filtering.

[0069] Based on the CV model, this invention will address obstacles. i exist k The complete motion state at a given moment is defined as a four-dimensional vector. :

[0070]

[0071] in, It is the position component of the obstacle in the world coordinate system. It is its velocity component;

[0072] Based on fundamental kinematic principles and the basic assumptions of the CV model, this invention can derive a linear state transition equation:

[0073]

[0074] in, yes k The optimal estimate (posterior estimate) at time -1. yes k Prior estimates (predicted values) at time 1;

[0075] It is a 4×4 state transition matrix:

[0076]

[0077] in It is a 2×2 identity matrix. It is a 2×2 zero matrix. It is the sampling period;

[0078] The prediction phase must not only predict the mean of the state, but also the uncertainty of the state, i.e., the propagation of the covariance matrix P:

[0079]

[0080] here, It is the predicted state covariance matrix, which quantifies the impact of this invention on the predicted values. The uncertainty stems from two parts: firstly... k The uncertainty of time -1 itself The spread (i.e.) The first is the uncertainty of the model itself, namely the process noise covariance matrix. ;

[0081] The derivation of the matrix is ​​key to the implementation of EKF; in classic CV models, The matrix is ​​isotropic, meaning it assumes that an obstacle has the same probability of accelerating randomly in any direction; this does not conform to physical reality; for example, a car is more likely to accelerate or decelerate in its longitudinal direction (forward / backward) than in its lateral direction (translation).

[0082] To address this issue, this invention introduces an anisotropic process noise model; instead of defining a single noise parameter, this invention defines two independent noise parameters, as follows:

[0083] Standard deviation of acceleration noise in the longitudinal direction (along its velocity direction) of the obstacle; Standard deviation of the lateral (perpendicular to its velocity direction) acceleration noise of the obstacle;

[0084] The matrix is ​​now a time-varying matrix. Its value depends on the obstacle. k velocity at time -1 The derivation process is as follows:

[0085] First, obtain the obstacle's speed: from k The optimal state at time -1 Extracting the velocity vector ;

[0086] Next, calculate the rotation matrix. According to speed Calculate a rotation matrix from the obstacle's "body coordinate system" to the "world coordinate system";

[0087]

[0088] Then define the noise in the body coordinate system. In the body coordinate system of the obstacle, the acceleration noise is decoupled.

[0089]

[0090] Next, map the noise to the world coordinate system: Use a rotation matrix to map the volume noise to the world coordinate system, and obtain the acceleration covariance in the world coordinate system. :

[0091]

[0092] Finally build the final Matrix: Using The amount and combined The integral term (which maps acceleration noise to position and velocity states) is used to construct the final (non-diagonal) equation. matrix:

[0093]

[0094] This anisotropic model enables EKF to generate predictive covariance ellipses that are more closely aligned with physical reality: when an obstacle moves at high speed, its uncertainty ellipse expands primarily in the lateral direction (turning direction) rather than in the already determined longitudinal direction (velocity direction), thus achieving a smarter and more accurate risk assessment.

[0095] The prediction phase provides a priori estimates of the state for this invention. and ;when k New measurements from the time sensor (LiDAR) Upon arrival, EKF enters the update phase to revise the prior estimates;

[0096] This invention assumes that the sensor directly measures the position of the obstacle (or obtains the position through sensor fusion), then the measurement vector... as follows:

[0097]

[0098] The measurement equation is linear, therefore the EKF (Extended Kalman Filter) degenerates into the standard Kalman Filter (KF) at this stage:

[0099]

[0100] in, It is observation noise, which follows a Gaussian distribution. ; It is a 2×4 observation matrix that maps a four-dimensional state vector to a two-dimensional measurement vector: This represents the complete motion state vector of the dynamic obstacle at time k, which is a four-dimensional vector constructed based on a constant velocity (CV) model;

[0101]

[0102] It is a 2×2 measurement noise covariance matrix, representing the inherent accuracy of the sensor:

[0103]

[0104] The standard Kalman update equation is as follows: , These represent the measurement variance of the sensor in the X and Y axes, respectively. The larger the variance, the more drastic the data fluctuation and the less accurate the measurement. In the simulation, these two values ​​are mainly used as noise artificially added. They can be used to calculate the Kalman gain or to determine the size of the "safe range".

[0105] Calculate the measurement residual (Innovation):

[0106]

[0107] Calculate the innovation covariance of the residuals:

[0108]

[0109] Calculate the optimal Kalman gain:

[0110]

[0111] Update state (posterior estimate):

[0112]

[0113] Update covariance (posterior covariance):

[0114]

[0115] By continuously iterating between the prediction and update phases, EKF can robustly track dynamic obstacles in real time; its final output is the posterior state. (Most likely mean of obstacles) and posterior covariance (The uncertainty of this estimate) will serve as the core input for the calculation of the P-APF repulsive field in the next section; This represents the measurement residual, which is the difference between the actual sensor data observed and the data expected to be observed (based on model predictions); if =0 indicates that the prediction is very accurate and completely matches the observation; if the value is large, it means that something unexpected has happened (either the obstacle suddenly changes direction, the prediction is wrong, or the sensor is noisy); it is the basis signal for all subsequent correction work - if there is no residual (no difference), there is no need for correction. This represents the residual covariance, and its role is to act as the denominator when calculating the Kalman gain. If The total uncertainty is very large, so the algorithm becomes very conservative when making corrections. This represents the optimal Kalman gain, which determines how much of the residual ( Add it to the state estimation; the relationship between the three is as follows: first calculate the difference ( Then assess the total amount of noise contained in this gap. ); Calculate the correction strength based on the noise ratio. Then use this force to correct the final state;

[0116] S2. Set a dynamic time window, predict the state based on the estimation results in S1, the current distance between the obstacle and the drone, and the relative speed; perform probability prediction through the state transition matrix and process noise, extract the mean and covariance matrix of the predicted position; calculate the probability distribution statistical distance from the RRT* extension point to the predicted position.

[0117] Specifically, EKF solves the problem of estimating the "current" state; but the core of APF is "obstacle avoidance", and this invention must predict the "future" collision risk;

[0118] At each step of the RRT* expansion (i.e., selecting in the tree) (After the node), this invention cannot only consider the current estimated position of the obstacle. However, it is necessary to predict its future performance. The position after; Indicates the nearest point, Indicates a new node;

[0119] (1) Dynamic time window

[0120] This predicted time It should not be a fixed value; it should be a dynamic, context-dependent "interaction time"; this invention defines it as:

[0121]

[0122] in It is the current Euclidean distance between the obstacle and the drone, and It is the magnitude of the relative velocity; this heuristic definition allows Adaptive: When the obstacle is far away or the relative speed is slow, this invention predicts a longer time. When an obstacle approaches at high speed, this invention predicts a shorter time... Focusing on the immediate collision risks; among them ;

[0123] (2) Probabilistic prediction of state

[0124] This invention utilizes the prediction equation of EKF to change the state from the current k Predicting at all times Time; the state transition matrix at this time and process noise All Functions:

[0125]

[0126]

[0127] ;

[0128] This represents the mean of future state predictions at time k, based on all information available at time k. A priori estimate (predicted value) of the obstacle's motion state at a given time. This represents the future... At any given moment, the expected (mean) values ​​of the obstacle's position and velocity. This represents the predicted covariance matrix at future time k, which is related to the predicted state mentioned above. The associated error covariance matrix defines the shape of a multidimensional probability distribution. Specifically, at the location level, it corresponds to an uncertainty ellipse. Larger values ​​in the matrix result in a larger ellipse area, meaning a wider range of obstacles around the predicted location and a more dispersed distribution of collision risk.

[0129] (3) Extraction of predicted location distribution

[0130] This invention focuses on The "position" distribution of obstacles at any given time; the state vector is... Its covariance matrix It is a 4×4 matrix;

[0131] Obstacles The predicted location at time follows a Gaussian distribution. :

[0132] The predicted position mean (a 2×1 vector) is derived directly from the predicted state vector. Extracting the first two elements:

[0133]

[0134] in, express The predicted x-coordinate; express The predicted y-coordinate;

[0135] Predicted location covariance (a 2×2 matrix):

[0136] The covariance matrix is ​​derived from the entire 4×4 state covariance matrix. Extracted from the top left corner; the state covariance matrix can be represented in blocks as follows:

[0137]

[0138] Among them, location covariance This is the required 2×2 matrix:

[0139]

[0140] in, Indicates the variance of the X-axis position. Indicates the variance of the Y-axis position. , Both represent the correlation between the X-axis position error and the Y-axis position error; since the covariance matrix is ​​a symmetric matrix, Geometrically, An uncertain ellipse is defined, and the true location of the obstacle is highly likely to be within this elliptical region;

[0141] (4) Repulsive field based on Mahalanobis distance

[0142] The classical artificial potential field method uses Euclidean distance. To calculate RRT* extension points Mean of predicted obstacle location The distance between them; this is essentially comparing two "points," completely ignoring the uncertainty information obtained through estimation. ;

[0143] The drawback of Euclidean distance is that it treats all directions the same; in directions where the predicted position of the obstacle is more uncertain (i.e., the major axis of the uncertain ellipse), it is relatively safe for the drone to get closer; while in directions where the uncertainty is less uncertain (the minor axis of the ellipse), the drone needs to keep a greater distance to be safe.

[0144] To address this problem, the present invention introduces Mahalanobis distance. To replace Euclidean distance; Mahalanobis distance measures the RRT* extension points. Statistical distance to the probability distribution of the predicted location of the obstacle:

[0145]

[0146] Mahalanobis distance is a dimensionless number representing Located at the obstacle prediction mean Beyond the "several standard deviations" in the formula; It is the inverse of the covariance matrix, also known as the precision matrix; the introduction of Mahalanobis distance changes the construction of the repulsive field from a purely geometric space to a probability space, thereby realizing the perception of uncertainty.

[0147] S3. Set dynamic risk weights, integrate the probability distribution statistical distance, velocity phase, motion trend and predicted uncertainty risk calculated in S2 to reconstruct the probability repulsive field, and calculate the guiding resultant force;

[0148] Specifically, in the classic APF, repulsive force The size is usually determined by a fixed gain. Control; however, in complex dynamic environments, a fixed weight is clearly unreasonable; such as Figure 2 As shown, drones face risks in different dimensions; for example, an obstacle coming at you at high speed and an obstacle moving slowly in the same direction pose completely different risks, and the repulsive force weight should be dynamically adjusted accordingly.

[0149] Therefore, this invention designs a dynamic risk weight. To replace the original fixed weight ; It is a comprehensive assessment factor that integrates three different dimensions of risk:

[0150]

[0151] in, As a fundamental repulsive force gain coefficient; It is the fusion coefficient of the three risk factors, satisfying ;

[0152] These are three dimensionless risk factors:

[0153] (1) Velocity phase factor (Directional Risk)

[0154] Used to assess the correlation of speed and direction between the drone and the obstacle, that is, to determine whether the two are "moving towards each other", "crossing each other", or "moving in the same direction";

[0155]

[0156] in, This is the drone's current speed. From the predicted state The obstacle prediction speed extracted from it It is a tiny positive number set to prevent the denominator from being zero; The range of values ​​is :

[0157] when and When in the same direction (cosine of the included angle) ), It is considered low risk;

[0158] When the two are perpendicular ( ), It is classified as medium risk;

[0159] When the two are in opposite directions (facing each other) ), This is considered high-risk.

[0160] (2) Motion trend factor (Approaching Risk)

[0161] This invention is used to assess whether a drone and an obstacle are "approaching each other" or "moving away" from each other in space; it first defines a relative position vector and a relative velocity vector (both in...). (Time Prediction)

[0162] Relative position vector: ;

[0163] Relative velocity vector: ;

[0164] Radial approach speed (That is, the projection of relative velocity onto the line connecting relative positions) is defined as:

[0165]

[0166] Use the Sigmoid function to smoothly quantify this convergence risk:

[0167]

[0168] in, It is a speed threshold used for standardization (e.g.) ), It is the slope of the Sigmoid function (typically 5); when When the value is a large negative value (indicating rapid departure), ;when When the value is a large positive value (indicating a rapid approach), ;

[0169] (3) Predicting uncertainty risk

[0170] Used to assess the degree of "confidence" of the invention in predicting obstacle locations; if the EKF's prediction of obstacles is highly uncertain (i.e. (Very large), this invention should adopt a more conservative strategy and apply a larger repulsive weight; this invention uses the trace of the predicted location covariance matrix. As a scalar measure of overall uncertainty;

[0171]

[0172] in, It is a normalized threshold; using The advantage of a function is that it can smoothly limit risk factors to a certain level. Within the interval; even if the uncertainty tends to infinity, It will only gradually saturate at 1, thus preventing the infinite growth of the repulsive force weight;

[0173] Reconstructing the probabilistic repulsive field (P-APF): combined with Mahalanobis distance and dynamic risk weights This invention can now reconstruct a completely new, robust probabilistic repulsive field;

[0174] (1) New probabilistic repulsive potential field

[0175] This invention adopts the potential field form of the classical APF, but uses the Euclidean distance... Replace with Mahalanobis distance :

[0176]

[0177] in: Indicates dynamic risk weights; It is a safe Mahalanobis distance threshold, replacing the one in the classic APF. ; The physical meaning is very clear: setting This means that when the drone expansion point Entering the range of 3 standard deviations of the obstacle prediction probability cloud ( Within a certain timeframe, the repulsive field takes effect; this is a probability-based, statistically robust safety boundary.

[0178] (2) New probabilistic repulsion

[0179] The repulsive force is the negative gradient of the potential field; this invention addresses... Seeking information about gradient:

[0180]

[0181] The calculation yields:

[0182]

[0183] The core of this repulsive force lies in the gradient of the Mahalanobis distance. ;

[0184] (3) Gradient of Mahalanobis distance

[0185] This invention firstly Find the gradient, according to the definition:

[0186]

[0187] The calculation yielded:

[0188]

[0189] According to the chain rule The present invention can obtain :

[0190]

[0191] Substitute this equation into This invention yields the final repulsive force vector formula for P-APF:

[0192]

[0193] RRT* Guiding Resultant Force: The P-APF repulsive force formula reveals a profound property; in the classical APF, the direction of the repulsive force... The direction of the repulsive force is always along the line connecting the center of the drone and the center of the obstacle; however, in P-APF, the direction of the repulsive force is:

[0194]

[0195] The direction of repulsive force is no longer simply along and The connection is not made by the precision matrix. It has undergone a "twist"; this means that the P-APF will prioritize pushing the drone away along the minor axis (i.e., the most certain direction) of the obstacle prediction uncertainty ellipsoid; this is a smarter and safer avoidance strategy that takes advantage of uncertainty information rather than being plagued by uncertainty.

[0196] Finally, this invention integrates P-APF repulsion with the original exploration mechanism of RRT*; in the RRT* algorithm selection... After the node, the present invention no longer blindly moves towards Instead of extending the fixed step size, calculate a guiding resultant force. This combined force will determine the new node. Generation direction:

[0197]

[0198] The resultant force consists of four components:

[0199] Target gravity ( ):

[0200]

[0201] Indicates the target gravity gain coefficient;

[0202] Pointing to the ultimate goal , Control the weights to ensure that the path planning is goal-oriented;

[0203] Random exploration ability ( ):

[0204]

[0205] This represents a coordinate point randomly generated in the state space by the RRT* algorithm; This represents the random exploration force gain coefficient, used to control the attractive force weight of random sampling points on the drone; it points to the random sampling point. This invention retains a smaller This inherits the stochastic exploration nature of the RRT* algorithm, which is key to the algorithm escaping local minima and ensuring probabilistic completeness.

[0206] Dynamic obstacle probabilistic repulsion ( ):

[0207] This is the core innovation proposed in this invention, namely... It is for each dynamic obstacle i Calculate a repulsive force based on Mahalanobis distance and prediction uncertainty to provide intelligent avoidance of dynamic obstacles;

[0208] Static obstacle repulsion force ):

[0209] For static obstacles in the map j (e.g., buildings, walls) This invention still uses the classic APF repulsive force based on Euclidean distance to ensure obstacle avoidance capability in static environments;

[0210]

[0211] Indicates the static repulsion gain coefficient; This represents the Euclidean distance, which is the straight-line distance between the current extended node of the drone and the surface of the static obstacle. This indicates the range of influence of the obstacle's repulsive force; it is a preset threshold distance. Only when the drone is at a certain distance from the obstacle... Less than or equal to Only when the obstacle generates a repulsive force will it do so; otherwise, the repulsive force is zero. This determines which way the thrust should be directed (ensuring that the drone is pushed away from the obstacle along the line connecting the points).

[0212] In this way, the tree expansion process of the PF-RRT* algorithm evolves from a simple geometric interpolation into a multi-objective force-guided process that integrates target orientation, random exploration, dynamic intelligent avoidance, and static safety constraints. The P-APF repulsion force (b) can sense the predicted uncertainty ellipse and intelligently avoid it along the minor axis of the ellipse (the most certain direction). In contrast, the traditional APF (a) blindly pushes away along the center point, ignoring the collision probability. Therefore, it has a significant advantage over the APF algorithm based on Euclidean distance. It also provides a comprehensive and intelligent guiding direction for the subsequent fuzzy logic adaptive step size (FLC-AS) decision-making.

[0213] S4. Calculation of the final step size of the guiding resultant force obtained from S3;

[0214] Specifically, this invention designs a two-input, single-output Mamdani-type fuzzy logic controller; this controller is based on The local context information of the node is used to intelligently infer an adaptive step size scaling factor.

[0215] 1. Input 1: Repulsive Force Ratio (RFR)

[0216] This input is used for quantization. The local comprehensive risk experienced by a node is defined as the total repulsive force. (The sum of static and dynamic repulsive forces, i.e.) In guiding the combined force (repulsive and attractive forces) The proportion of (the sum of) in;

[0217]

[0218] in To prevent the removal of tiny positive values ​​from zero; RFR is naturally normalized to Interval; when When, it indicates In a low-risk open space; when This indicates that the person is in a high-risk, crowded area;

[0219] Fuzzy set: {Low load, Medium load, High load}

[0220] Please refer to Figure 3 Low load: trapezoidal distribution, [0.0, 0.0, 0.1, 0.3]; Medium load: triangular distribution, [0.1, 0.5, 0.9]; High load: trapezoidal distribution, [0.7, 0.9, 1.0, 1.0];

[0221] 2. Input 2: Normalized target distance (Distance to Goal, DTG)

[0222] This parameter is used to determine the current exploration stage of the RRT* tree, i.e. The relative proximity of the node to the target;

[0223]

[0224] This value is in In-interval normalization; express It is still in the initial exploration phase (far end) near the starting point; express It is close to the target and is in the final fine-tuning stage (near end).

[0225] Fuzzy set: {near end, mid-range, far end}

[0226] Please refer to Figure 4 Proximal end: trapezoidal distribution, [0.0, 0.0, 0.1, 0.3]; Mid-range: triangular distribution, [0.1, 0.5, 0.9]; Distal end: trapezoidal distribution, [0.7, 0.9, 1.0, 1.0];

[0227] 3. Output variable: StepSizeFactor )

[0228] FLC Single Output It is a normalization to The scaling factor of the interval is used to dynamically adjust the final expansion step size; Corresponding to the minimum step size, Corresponding to the maximum step size;

[0229] Fuzzy set: {minimal, small, reference, large, maximal}

[0230] Please refer to Figure 5Minimum: triangular distribution, [0,0,0.2]; Small: triangular distribution, [0.1,0.3,0.5]; Baseline: triangular distribution, [0.3,0.5,0.7]; Large: triangular distribution, [0.5,0.7,0.9]; Maximum: trapezoidal distribution, [0.8,0.9,1.0,1.0];

[0231] Fuzzy Rule Base

[0232] The core intelligence of FLC is embodied in its carefully designed nine "IF-THEN" heuristic rules; this invention constructs a system containing... A simplified Mamdani rule base with 10 rules is shown in Table 1 below:

[0233]

[0234] The core idea of ​​this rule base is that Local Risk (RFR) determines the safety margin (i.e., the upper limit of the safe step size), while the Exploration Phase (DTG) determines the efficiency requirements (i.e., the specific value of the step size) within this margin.

[0235] Defuzzification

[0236] This invention employs standard Mamdani fuzzy inference and uses the efficient Center of Gravity (CoG) method for defuzzification to calculate the accurate step scaling factor. ;

[0237] For a given input and Each rule i (Total of 9) Activation intensity Calculated using the fuzzy AND operator (minimum value):

[0238]

[0239] final Calculated using the centroid method formula, which is weighted by the activation intensity of all rules:

[0240]

[0241] in, For the first i The centroid of the fuzzy set of rule conclusions; the centroid method can smoothly integrate the influence of all relevant rules and produce continuous control output;

[0242] Final step size calculation and safety constraints

[0243] Precise values ​​obtained by deblurring It is a normalized scaling factor, which requires two more steps of calculation and constraint to be converted into the actual physical step size of the drone in the environment. ;

[0244] Precise values ​​obtained by deblurring It is merely a dimensionless normalization coefficient. In order to convert it into an actual executable expansion step size for the UAV in the physical environment, it is mapped to a physical scale range determined by both the UAV's dynamic constraints and environmental characteristics. superior;

[0245] Final physical step size Calculated directly using the following linear mapping formula:

[0246]

[0247] in: (Minimum safe step size): Defines the minimum exploration granularity of the algorithm in extremely constrained environments; this value is usually set to a very small positive value, on the one hand to prevent the algorithm from stalling due to a zero step size, and on the other hand to ensure that high-precision fine expansion can be performed in narrow passages with dense obstacles. (Maximum Dynamic Step Size): Defines the maximum single-step distance that a UAV is allowed to move in an open area. This upper limit must take into account the UAV's maximum flight speed, the effective detection radius of the sensors, and the refresh rate of the control cycle to ensure flight safety and controllability during high-speed expansion. It indicates the magnitude of the total repulsive force. The larger the value, the closer to the obstacle, or the greater the threat from the dynamic obstacle. This represents the magnitude of the total gravitational force, the force pointing towards the target point. It indicates the prevention and elimination of zero small positive quantities; RFR represents the repulsive force ratio (RFR); DTG represents the normalized distance to goal (DTG). This indicates how far the current node is from the destination; This represents the total straight-line distance (constant) from the starting point to the ending point. This represents the activation strength (or confidence level) of the i-th rule. For example, if the rule is "if the risk is high and the distance is short, then the step size is minimal", then the rule is "if the risk is high and the distance is short, then the step size is minimal". It's about how "correct" this rule is in the current situation; , Both represent the values ​​of the membership function. For example, if the current RFR is 0.8, its membership degree to the "high-risk" set might be 0.9. This represents the step size scaling factor, which is the direct output of the fuzzy controller. It ranges from [0,1], where 0 represents the suggested minimum step size and 1 represents the suggested maximum step size. This represents the centroid value of the conclusion of the i-th rule; Indicates the starting coordinates;

[0248] Formula (3-46) directly transforms the intelligent decision of the fuzzy controller into the actual control quantity that meets the physical constraints, which not only ensures rapid convergence in open areas, but also ensures precise obstacle avoidance in complex areas, thus achieving a balance between algorithm performance and physical safety.

[0249] S5. Based on the calculation results in S4, add state variables to the RRT* node to detect hard and soft stagnation, and create dynamic restricted areas through random exploration; then inject the DNZ penalty cost into the cost calculation and introduce a decay mechanism.

[0250] Specifically, the "Dynamic Forbidden Zone" (DNZ) mechanism designed in this invention aims to systematically resolve the contradiction between "efficiency" and "completeness." Its core idea is to shift from "passive escape" to "active learning": for the vast majority of the algorithm's time, it efficiently "utilizes" the APF potential field for rapid convergence; only when the algorithm is precisely detected to be stuck is the event of "falling into a trap" regarded as a learning signal of "planning failure." The algorithm no longer "passively" tries to "struggle" to escape from the trap point, but "actively" registers the area as a high-cost "forbidden zone," and relies on the planner's (RRT*) own cost awareness ability (such as rewiring mechanism) to automatically and intelligently avoid this area in subsequent iterations.

[0251] This strategy differs from traditional "blind" random exploration (inefficient) or complex heuristics (difficult to implement). It ensures the theoretical completeness of the entire hybrid algorithm without sacrificing the efficiency of APF guidance. The DNZ mechanism is an online learning loop with three core stages, which will be elaborated in detail in subsequent chapters: (1) Trap Detection and Recognition (TDI), (2) DNZ Registration (TER), and (3) Cost-Based Planner Integration (CPI).

[0252] Trap Detection and Recognition (TDI)

[0253] The effectiveness of the LMDE mechanism begins with its precise, quantitative definition of the state of "algorithm trapped"; this invention applies to each node of the RRT* tree. Add a state variable above: (Integer, initially 0), used to record nodes used as... The number of consecutive expansion failures or invalidities.

[0254] In each iteration of the main loop of the RRT* algorithm, a node is selected. And attempt to generate Then, the system will execute the following dual-criteria stall detection logic:

[0255] 1. Collision detection (hard stop): If Returning false indicates a collision between the path and an obstacle, which is considered a "hard stop". The state variable increments by 1;

[0256] 2. Progress Stagnation Detection (Soft Stagnation): Even if the path is collision-free, expansion may not be effective (e.g., oscillation in a potential field depression); this invention introduces "effective progress". That is, the extended vector Direction vector toward the target Projected length on:

[0257]

[0258] in It is a tiny positive number that prevents the denominator from being zero; if Less than a preset minimum progress threshold If the condition is not met, the expansion is considered invalid and classified as a "soft standstill". The state variable increments by 1;

[0259] 3. Successful expansion and counter reset: Conversely, a successful expansion (without collisions and...) This will cause the counter to be reset to zero; when the state variable of any node exceeds the preset threshold... When the threshold range is 500-1000, the TDI phase is triggered, and the node is... Identified as the "trap center point" ;

[0260]

[0261] Temporary Escape and DNZ Registration (TER)

[0262] Once TDI is triggered, the algorithm immediately performs two actions:

[0263] 1. Temporary Escape: First, a simple random exploration function is called to execute... This is the next iteration; the sole purpose of this stage is to physically "leave" the active nodes of the tree. This provides a safe boundary for subsequent "annotation" operations;

[0264] 2. DNZ Registration (Core): In The location creates a new "dynamic restricted area". And add it to a global "forbidden zone" set. In the middle; a DNZ object It is defined as a tuple containing its core attributes:

[0265]

[0266] in Indicates the center of the restricted area, that is ; Indicates the radius of the restricted area; defines the range of influence of the "trap", which can be set to a fixed value (such as the average entrance width of a U-shaped trap). This represents the base penalty cost; a very large positive value (e.g., 10). 6 This is used to significantly increase the total cost of any path that "traverses" the area; This represents the creation timestamp; it is used to implement subsequent "memory decay". This represents the cost decay rate; it is a key parameter for implementing the "forgetting" mechanism.

[0267] Cost-based planner integration (CPI)

[0268] The essence of the DNZ mechanism lies in modifying the cost function of the RRT* algorithm, rather than modifying its complex tree growth logic;

[0269] In standard RRT*, nodes Total cost for:

[0270]

[0271] This invention incorporates the penalty cost of DNZ into this cost calculation; first, a "DNZ penalty field" is defined. It represents any point in space. q Total DNZ penalty received:

[0272]

[0273] To ensure the smoothness of the cost function, this invention employs a Gaussian soft boundary penalty model. Defined as:

[0274]

[0275] in yes At the current time The penalty value after decay, Is with The standard deviation of the associated Gaussian function;

[0276] Integration into the RRT* cost function: This invention injects this penalty term into the cost calculation of RRT; when RRT attempts to... Connected to At that time, its total cost Revised to:

[0277]

[0278] Among them: It is a weight used to balance the importance of "path length" and "avoidance penalty"; express The total DNZ penalty received; with this modification, RRT's Rewire() process will work automatically; if the total cost (path + penalty) of a "shortcut" through the DNZ is higher than that of a "longcut" that bypasses the DNZ, RRT will automatically choose the latter, thus achieving intelligent avoidance;

[0279] DNZ's lifecycle management and "forgetting" mechanism:

[0280] DNZ should not be "permanent", otherwise it will lead to a fatal problem: if the trap is located on the only feasible path to the target, a permanent DNZ will cause the algorithm to incorrectly determine that the path does not exist;

[0281] Therefore, this invention introduces a "cost decay" or "forgetting" mechanism to give DNZ a "lifecycle"; this invention enables Basic punishment Over time Exponential decay:

[0282]

[0283] in This is the current simulation time. It is the creation timestamp. It is the attenuation rate;

[0284] The brilliance of this mechanism lies in:

[0285] 1.Initial stage ( ): Very high, RRT* is "forced" to explore other paths;

[0286] 2. Later stage ( ): If RRT* fails to explore all other paths (Note: (Possibly blocking the only passage), at this time It has decayed to a low value;

[0287] 3. "Reconsider": At this point, the total cost of the path through the "old trap" is... It may become "acceptable" again (i.e., become the lowest-cost path); RRT* will be "re-evaluated" and eventually pass through this area;

[0288] This perfectly balances the contradiction between "avoiding known pitfalls" (efficiency) and "not giving up any possibilities" (completeness);

[0289] S6. Use the waypoints generated by PFL-RRT* as the initial control points of the B-spline; based on the injection in S5 and the cost calculation method after its introduction, iteratively optimize the control point set, generate cubic B-spline curves, perform curvature adaptive sampling and calculate trajectory cost until convergence and output the optimal control point set;

[0290] Specifically, this invention uses the waypoint sequence generated by the front-end PFL-RRT*. The initial control point set for the B-spline curve A cubic B-spline curve Its control points can be parameterized. With B-spline basis functions Weighted sum:

[0291]

[0292] in Indicates the control points to be optimized. This indicates that the number of control points has been reduced by 1. The basis functions are defined by the classic Cox-deBoor recurrence relation; the trajectory optimization problem is therefore transformed into: finding a set of optimal control points. , making Comprehensive cost function J Minimum;

[0293] Design and Analysis of Multi-Objective Cost Functions

[0294] To unify the three conflicting optimization objectives (economy, safety, and feasibility) into a solvable mathematical framework, this invention designs a comprehensive cost function. J This function takes the path length as an example. Based on this, penalties representing collision risk and curvature exceeding limits were imposed:

[0295]

[0296] in It is the path length. Indicates the obstacle collision coefficient. This represents the curvature constraint penalty term; Indicates the obstacle penalty weight. Indicates the curvature penalty weight; weight coefficient and Must meet Its value is dynamically adjusted according to the priority of the scenario;

[0297] 1. Economic indicators: Path length With adaptive sampling

[0298] Path length It is a direct measure of trajectory economy, and its precise value is the integral of the curve arc length. Since this integral typically lacks an analytical solution, this invention employs a numerical approximation method to discretize the curve into... 1 point and sum the Euclidean distances between adjacent points:

[0299]

[0300] To improve evaluation efficiency and accuracy, this invention employs a curvature adaptive sampling strategy; in the trajectory curvature Dense sampling is performed in large areas (sharp bends), while sparse sampling is performed in straight areas; new sampling step size. Calculated dynamically by the following formula:

[0301]

[0302] in, It is the basic sampling interval. It is the sensitivity coefficient of the sampling density to curvature;

[0303] 2. Safety indicators: Obstacle collision penalty ( )

[0304] Obstacle Conflict Coefficient It is a comprehensive quantification of the collision risk along the entire trajectory, defined as all collisions along the path. Each sampling point and Interaction penalty between obstacles The average of the sums:

[0305]

[0306] in It is the first sampling points To the One obstacle Signed distance at the boundary; interactive penalty function The design of this module is crucial; it must provide the optimizer with smooth and efficient gradient information. This invention uses a piecewise function to define this penalty:

[0307]

[0308] in, This is the set safety margin; the design logic of this function is as follows: safety area ( ): When the distance between the trajectory point and the obstacle is greater than the safety margin, the penalty is zero.

[0309] restricted area ( When a trajectory point enters the safety margin range but has not yet collided, a monotonically increasing cubic polynomial penalty is applied. This function exhibits a "J-shaped" growth characteristic. At this point, both the function value and the first derivative smoothly converge to zero, achieving a seamless connection with the safe zone; as d approaches 0 (the surface of the obstacle), the penalty gradient increases rapidly, thereby generating an increasingly strong "virtual repulsive force" that pushes the trajectory away from the danger zone;

[0310] Collision zone ( When a trajectory point intrudes into the interior of an obstacle, the penalty function switches to linear growth mode. To ensure strict mathematical continuity, the slope of this linear segment is set to... This perfectly matches the warning zone in The derivative value at that point. This high-slope linear penalty provides a constant and strong gradient signal, ensuring that even in the extreme case of deep collisions, the optimizer can obtain a clear escape direction and quickly pull the path back to the feasible region;

[0311] 3. Feasibility Indicator: Curvature Feasibility Penalty ( )

[0312] Curvature is a core indicator for evaluating the feasibility of trajectory dynamics; curvature penalty term Used to ensure that the generated trajectory is at any point None of them will exceed the maximum permissible curvature of the drone. ;

[0313] The excellent properties of B-splines make the calculation of curvature and its derivatives highly efficient, avoiding complex numerical differentiation:

[0314]

[0315] This invention addresses curvature exceeding the maximum permissible curvature. A secondary penalty is applied to the portion; this secondary penalty imposes a more severe "penalty" on large violations, while providing a smooth gradient near the constraint boundary, which is beneficial for the stable convergence of the optimizer; the penalty term is defined as the average of the squares of the normalized out-of-limit curvature over all sampling points:

[0316]

[0317] By incorporating this penalty term into the total cost function, the optimizer can minimize path length and collision risk while also actively adjusting control points to "straighten" overly curved path segments.

[0318] Optimize algorithm process

[0319] The optimization process in this module is an iterative search for the optimal set of control points. The process essentially involves finding a cost function in a high-dimensional parameter space. The minimum solution; its core process is as follows:

[0320] 1. Initialization: Input the initial waypoints generated by PFL-RRT* as the initial control point set for the B-spline. And set obstacle information and maximum curvature. Penalty weight , Parameters;

[0321] 2. Iterative Optimization Loop: In each iteration, the following steps are performed: Generate candidate trajectories: Use a gradient-based optimization algorithm to optimize the current set of control points. To optimize the trajectory, the B-spline control points are moved along the negative gradient direction, taking into account path length, obstacle repulsion, and curvature constraints, to generate candidate trajectories with better shapes. A new set of candidate control points is then generated. Constructing B-spline curves: based on the candidate control point set Generate a complete cubic B-spline curve using the formula. Evaluation cost function: along Perform curvature adaptive sampling and calculate the total cost of the current candidate trajectory according to the formula. Selection and updating: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full Minimum cost compared to the current record Compare; if Then update the optimal solution: and ;

[0322] 3. Termination and Output: Repeat step 2 until the termination condition is met (reaching the maximum number of iterations or the cost function converges); finally, the algorithm outputs the optimal set of control points. The B-spline curves they define are the final optimized trajectories that satisfy the triple constraints of safety, economy, and feasibility.

[0323] Compared to traditional interpolation methods (such as cubic spline interpolation, which is prone to Runge phenomenon), this method achieves a comprehensive improvement in the multi-objective performance of the entire trajectory by optimizing a small number of control points. Compared to pure geometric optimization methods, this framework can generate trajectories with better overall performance by explicitly quantizing and balancing multiple performance indicators, providing powerful and reliable backend support for the PFL-RRT* algorithm.

[0324] Example 2

[0325] This embodiment discloses a robust path planning system for unmanned aerial vehicles (UAVs) in uncertain dynamic environments. The system can implement the methods of the above embodiments, including a real-time estimation module for the current dynamic obstacle state, a probabilistic prediction module for future collision risks, a probabilistic repulsion field reconstruction module, a final extended step size calculation module, a dual-criteria stagnation detection logic-cost decay module, and a control point iterative optimization module.

[0326] The current dynamic obstacle state real-time estimation module defines the four-dimensional state vector of the current obstacle of the UAV, introduces anisotropic noise covariance matrix, and maps it to the world coordinate system through a rotation matrix; then it uses a linear observation model and Kalman filter to update the state estimate.

[0327] The future collision risk probabilistic prediction module sets a dynamic time window and predicts the state based on the estimation results in the current dynamic obstacle state real-time estimation module, the distance between the current obstacle and the drone, and the relative speed; it performs probabilistic prediction through the state transition matrix and process noise, extracts the mean and covariance matrix of the predicted position, and calculates the probability distribution statistical distance from the RRT* extension point to the predicted position.

[0328] The probability repulsion field reconstruction module sets dynamic risk weights, integrates the probability distribution statistical distance, velocity phase, motion trend and predicted uncertainty risk calculated in the future collision risk probability prediction module to reconstruct the probability repulsion field, and calculates the guiding resultant force.

[0329] The final step size calculation module calculates the final step size based on the guiding resultant force calculated by the probability repulsion field reconstruction module.

[0330] The dual-criteria stagnation detection logic-cost decay module adds state variables to the RRT* node based on the calculation results in the final extended step size calculation module, detects hard stagnation and soft stagnation, creates dynamic restricted areas through random exploration, and then injects the DNZ penalty cost into the cost calculation and introduces a decay mechanism.

[0331] The control point iterative optimization module uses the waypoints generated by PFL-RRT* as the initial control points for the B-spline; based on the dual-criteria stagnation detection logic-cost attenuation module injection and the cost calculation method after its introduction, it iteratively optimizes the control point set, generates cubic B-spline curves, performs curvature adaptive sampling and calculates trajectory costs, until convergence and outputs the optimal control point set.

[0332] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0333] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, because according to the present invention, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are all optional embodiments, and the actions and modules involved are not necessarily essential to the present invention.

Claims

1. A robust path planning method for unmanned aerial vehicles (UAVs) in uncertain dynamic environments, characterized in that, Includes the following steps: S1. Define the four-dimensional state vector of the current obstacle of the UAV, introduce the anisotropic noise covariance matrix, and map it to the world coordinate system through the rotation matrix; Then, the state estimate is updated using a linear observation model and Kalman filtering; S2. Set a dynamic time window, predict the state based on the estimation results in S1, the current distance between the obstacle and the drone, and the relative speed; perform probability prediction through the state transition matrix and process noise, extract the mean and covariance matrix of the predicted position; calculate the probability distribution statistical distance from the RRT* extension point to the predicted position. S3. Set dynamic risk weights, integrate the probability distribution statistical distance, velocity phase, motion trend and predicted uncertainty risk calculated in S2 to reconstruct the probability repulsive field, and calculate the guiding resultant force; S4. Calculate the final step size and node expansion direction of the guiding resultant force obtained from S3; S4 specifically includes: S41. The adaptive step size controller based on two-input fuzzy logic calculates the final step size; S5. Based on the calculation results in S4, add state variables to the RRT* node to detect hard and soft stagnation, and create dynamic restricted areas through random exploration; then inject the penalty cost of the dynamic restricted areas into the cost calculation and introduce a decay mechanism. S6. Use the waypoints generated by PFL-RRT* as the initial control points of the B-spline; based on the injection in S5 and the cost calculation method after its introduction, iteratively optimize the control point set, generate cubic B-spline curves, perform curvature adaptive sampling and calculate trajectory cost until convergence and output the optimal control point set; S6 specifically includes: S61. Input the initial waypoints generated by PFL-RRT* as the initial control point set for the B-spline. Based on the cost calculation method introduced in S54 after cost attenuation, it sets up information including current obstacle information and maximum curvature. Penalty weight , parameter; S62. In each iteration, the current set of control points... Fine-tuning is performed to generate a new set of candidate control points. Then construct based on the candidate control point set Generate a complete cubic B-spline curve ;along Perform curvature adaptive sampling to obtain the current candidate trajectory sampling point set; and calculate the total cost of the current candidate trajectory sampling point set. ;Will Minimum cost compared to the current record Compare; like Then As the minimum cost of the current record, Let this be the set of optimal candidate control points for the current record; S63. Repeat S62 until the maximum number of iterations is reached or the cost function converges; output the optimal set of control points.

2. The robust path planning method for UAVs in uncertain dynamic environments according to claim 1, characterized in that, S1 includes the following steps: S11. Define the current obstacle encountered by the drone during flight, denoted as the current obstacle; and assign the current obstacle to... The complete motion state at a given moment is defined as a four-dimensional vector; S12. Construct the corresponding noise covariance matrix based on the four-dimensional vector in S11; S13. Use sensors to directly measure the current obstacle. The current position, and then the current obstacle in The position at any given time degenerates to a standard Kalman filter; S14. Based on the current obstacles... Standard Kalman filtering at time points calculates the measurement residual and the covariance of the measurement residual, respectively. Variance, optimal Kalman gain, and update posterior estimates and posterior covariance.

3. The robust path planning method for UAVs in uncertain dynamic environments according to claim 2, characterized in that, S12 includes the following steps: S121. Define two independent noise parameters: the standard deviation of the longitudinal acceleration noise of the current obstacle and the standard deviation of the lateral acceleration noise of the current obstacle. S122, From the current obstacle in Extract the velocity vector from the optimal state at any given time; calculate a rotation matrix from the current obstacle's body coordinate system to the world coordinate system based on the velocity vector; S123. Define the noise of the current obstacle in the body coordinate system as the current body coordinate system noise; then use the rotation matrix to map the current body coordinate system noise to the world coordinate system to obtain the acceleration covariance matrix in the world coordinate system. The final noise covariance matrix is ​​constructed by using the components of the acceleration covariance matrix in the world coordinate system and combining them with the integral term of the sampling period.

4. The robust path planning method for UAVs in uncertain dynamic environments according to claim 3, characterized in that, S2 includes the following steps: S21. Set a dynamic time window; S22. Based on the calculation results in S14 and the dynamic time window, change the state from the current... Predicting at all times At any time, and according to The state transition matrix at each time step and the process noise are used to make a probabilistic prediction of the current obstacle state; Indicates the predicted time; According to S11, the current obstacle is... Extracting the current obstacle from the complete motion state at any given moment The mean of the predicted position and the predicted position covariance matrix at each time step; S23. Based on the current obstacles... Calculate the statistical distance from the RRT* extension point to the probability distribution of the current obstacle's predicted position, based on the mean and covariance matrix of the predicted position at each time step.

5. The robust path planning method for UAVs in uncertain dynamic environments according to claim 4, characterized in that, S3 includes the following steps: S31. Set dynamic risk weights; S32. Based on the dynamic risk weight and the statistical distance from the RRT* extension point to the current obstacle prediction position probability distribution in S23, reconstruct the probabilistic repulsion field to obtain the final repulsion vector formula; calculate the guiding resultant force based on the reconstructed probabilistic repulsion field.

6. The robust path planning method for UAVs in uncertain dynamic environments according to claim 5, characterized in that, S5 includes the following steps: S51. Based on S1, S2, S4, and S4, add a state variable to each node of the RRT* tree to record the number of consecutive expansion failures or invalidities; in each iteration of the RRT* algorithm's main loop, select a node. And attempt to generate Afterwards, if Returning false indicates that the path has collided with the current obstacle, which is considered a hard stop. The state variable increments by 1; if the extended vector The projection length onto the direction vector toward the target is less than a preset minimum progress threshold. If this happens, the expansion is considered invalid and classified as a soft standstill. The state variable increments by 1; if there is no collision and the extension vector The projection length onto the direction vector toward the target is greater than ,Will The state variable is cleared to zero; if the state variable is greater than the preset threshold, S52 is executed and the node is reset. The center point of the trap is identified; the preset threshold ranges from 500 to 1000; CollisionFree is a collision detection function used to detect whether a straight path between two points collides with an obstacle in the environment; and These are the two input points; This represents the existing nodes in the tree. This indicates the newly generated sampling point; the output is a Boolean value: True indicates that the path has no collision, that is, the line connecting the two points does not pass through any obstacle area; False indicates that the path has a collision, that is, the line connecting the two points overlaps or intersects with an obstacle. S52. Call a simple random exploration function to perform multiple iterations; create a new dynamic restricted area at the center point of the trap. And add it to a global set of restricted areas; S53. Inject the penalty cost of the dynamic restricted area into the cost calculation of RRT*, when RRT* attempts to... Connected to When the total cost is calculated, the formula is as follows: in, , They represent , The total cost; This is a weight used to balance the importance of path length and avoiding penalties; express The total dynamic restricted area penalty received.

7. The robust path planning method for UAVs in uncertain dynamic environments according to claim 6, characterized in that, S5 further includes the following steps: S54. Based on S51, S52, and S53, a cost attenuation is introduced into the basic penalty cost, calculated as follows: in, This is the current simulation time. Attenuation rate; Indicates the number of simulation times corresponding to the current simulation time. The penalty for each dynamic restricted area; Indicates the creation timestamp; This indicates the basic penalty cost.

8. A system for implementing the robust path planning method for unmanned aerial vehicles (UAVs) in uncertain dynamic environments as described in any one of claims 1-7, characterized in that: It includes a real-time estimation module for the current dynamic obstacle state, a probabilistic prediction module for future collision risks, a probabilistic repulsion field reconstruction module, a final expansion step size calculation module, a dual-criteria stagnation detection logic-cost decay module, and a control point iterative optimization module. The current dynamic obstacle state real-time estimation module defines the four-dimensional state vector of the current obstacle of the UAV, introduces anisotropic noise covariance matrix, and maps it to the world coordinate system through a rotation matrix; Then, the state estimate is updated using a linear observation model and Kalman filtering; The future collision risk probabilistic prediction module sets a dynamic time window and predicts the state based on the estimation results in the current dynamic obstacle state real-time estimation module, the distance between the current obstacle and the drone, and the relative speed; it performs probabilistic prediction through the state transition matrix and process noise, extracts the mean and covariance matrix of the predicted position, and calculates the probability distribution statistical distance from the RRT* extension point to the predicted position. The probability repulsion field reconstruction module sets dynamic risk weights, integrates the probability distribution statistical distance, velocity phase, motion trend and predicted uncertainty risk calculated in the future collision risk probability prediction module to reconstruct the probability repulsion field, and calculates the guiding resultant force. The final step size calculation module calculates the final step size based on the guiding resultant force calculated by the probability repulsion field reconstruction module. The dual-criteria stagnation detection logic-cost decay module adds state variables to the RRT* node based on the calculation results in the final extended step size calculation module, detects hard stagnation and soft stagnation, creates dynamic restricted areas through random exploration, and then injects the DNZ penalty cost into the cost calculation and introduces a decay mechanism. The control point iterative optimization module uses the waypoints generated by PFL-RRT* as the initial control points for the B-spline; Based on the dual-criteria stagnation detection logic, the cost decay module is injected and the cost calculation method is introduced. The control point set is iteratively optimized, a cubic B-spline curve is generated, curvature adaptive sampling is performed and the trajectory cost is calculated until the optimal control point set is output through convergence.

Images