Unmanned aerial vehicle path planning method based on improved bidirectional RRT and model predictive control

Abstract

The application belongs to the field of unmanned aerial vehicle path planning, and relates to an unmanned aerial vehicle path planning method and system based on improved bidirectional RRT and model predictive control. A kinematics and dynamics constraint model is established, and a bidirectional RRT is used to generate a global reference path, so that a global path meeting the physical constraints of the unmanned aerial vehicle is obtained, and the problems of traditional path zigzagging or being unable to track are avoided. A nonlinear model predictive controller is introduced with the global path as a tracking target and an obstacle repulsive potential penalty term, so that the local obstacle avoidance control quantity meeting the physical constraints is generated in real time while the global path is tracked, the cooperation and unification of global guidance and dynamic obstacle avoidance are realized, and the local obstacle avoidance deviating from the task direction is avoided. A global and local collaborative planning mechanism is constructed, a local reference path is regenerated with the current position as a starting point and connected to the global path when a feasible trajectory cannot be found within the preset time of the nonlinear model predictive controller, the problems of traditional replanning lag or failure are solved, and the success rate of the unmanned aerial vehicle path planning is improved.

Description

Technical Field

[0001] This invention belongs to the field of UAV path planning, and relates to a UAV path planning method and system based on improved bidirectional RRT and model predictive control. Background Technology

[0002] With the rapid development of drone technology and the continuous reduction in manufacturing costs, quadcopter drones have demonstrated enormous application potential in many fields. From initial aerial photography for entertainment, their applications have gradually expanded to critical areas such as urban fire rescue, industrial infrastructure inspection, logistics delivery in remote areas, and traffic monitoring. Especially in complex urban environments, drones, with their small size, high maneuverability, and vertical takeoff and landing capabilities, can perform high-risk tasks that are difficult for humans to complete.

[0003] In recent years, urban building density has been increasing, with a surge in the number of high-rise and super high-rise buildings. According to relevant statistics, my country has become the country with the largest number of high-rise buildings in the world. Fire rescue in high-rise buildings has always been a challenging problem. First, external rescue methods are limited. Existing fire ladder trucks typically have a maximum working height of less than 100 meters and have strict requirements for operating sites. In urban centers, narrow roads and widespread haphazard parking often prevent fire trucks from reaching the optimal operating position in a timely manner. Once the fire spreads to higher floors, external rescue methods become largely ineffective.

[0004] Secondly, the internal rescue environment is harsh. When a fire breaks out, the stairwells are filled with thick smoke, visibility is extremely low, temperatures rise rapidly, and toxic gases are present. Firefighters carrying heavy equipment climb the stairs, expending enormous physical energy, and by the time they reach the burning floor, it is difficult to immediately begin effective rescue operations. At the same time, the high temperatures may damage the building structure, posing a risk of collapse and seriously threatening the lives of firefighters.

[0005] Secondly, locating trapped individuals is difficult. In dense smoke, direction is imperceptible to the naked eye, and even if trapped individuals are near a window, they cannot transmit effective location information. Traditional search and rescue methods are inefficient, and the golden rescue time is easily missed.

[0006] In this context, drones can play an irreplaceable role: by carrying thermal imaging cameras, they can quickly penetrate smoke to locate trapped personnel; by carrying communication relay equipment, they can establish temporary communication networks; and by carrying simple rescue supplies such as gas masks, lighting equipment, and drinking water, they can provide initial survival support for trapped personnel. However, the realization of the above tasks depends on the drones being able to fly autonomously inside the narrow, complex, and dynamically changing interiors of high-rise buildings, which places extremely high demands on path planning and obstacle avoidance algorithms.

[0007] To meet the application requirements of the aforementioned complex scenarios, existing UAV path planning algorithms have significant shortcomings: First, the shortcomings of global path planning algorithms. Classical global path planning algorithms include the A* algorithm, the Quick Search Random Tree (RRT) algorithm, the artificial potential field method, and their variants.

[0008] The A* algorithm and its derivatives can find the shortest path on a grid map, but their computational complexity increases dramatically with the scale of the environment. Furthermore, the planned path consists of grid edges with abrupt transitions, making it difficult to meet the dynamic constraints of UAVs. In continuous space, the design of the heuristic function for the A* algorithm is challenging, and optimality cannot be guaranteed.

[0009] RRT / RRT* Algorithms: The RRT algorithm explores the state space quickly through random sampling, making it suitable for high-dimensional complex environments. However, the generated paths are tortuous, non-optimal, and exhibit strong randomness and slow convergence. The RRT* algorithm achieves asymptotic optimality by reselecting parent nodes and rewiring, but in complex obstacle environments, its low sampling efficiency and slow convergence remain prominent issues. It often requires a large number of iterations to generate high-quality paths, making it difficult to meet the time requirements of online planning for UAVs.

[0010] Artificial potential field method: This method guides the movement of drones by constructing gravitational and repulsive fields. It is simple to calculate and responds quickly, but it is prone to getting trapped in local minima and often stagnates or oscillates in complex environments, making it impossible to guarantee reaching the target point.

[0011] Second, the shortcomings of dynamic obstacle avoidance algorithms. Existing methods for real-time obstacle avoidance in dynamic environments mainly include: Reactive obstacle avoidance algorithms, such as dynamic windowing and velocity-based obstacle avoidance, generate obstacle avoidance commands in real time based on current sensor data. These methods have a fast response speed but lack a global perspective and are prone to making short-sighted decisions in complex environments, leading to detours or even getting stuck in dead zones.

[0012] Replanning strategy: Upon detecting an obstacle, the global planner is invoked again, starting from the current position. This method is theoretically sound, but the replanning computation is time-consuming, making it difficult to meet real-time requirements in highly dynamic environments. Frequent replanning can also lead to path discontinuities and drastic changes in the drone's flight attitude.

[0013] Model Predictive Control (MPC): MPC, through rolling optimization, can handle constraints and predict future trajectories, demonstrating advantages in dynamic obstacle avoidance. However, existing MPC obstacle avoidance methods often employ simplified point mass models, neglecting UAV attitude dynamics constraints. This results in the planned obstacle avoidance trajectory failing to accurately track during actual flight due to attitude response lag. Furthermore, the design of the MPC objective function lacks long-term guidance for the global path, making it prone to deviating from the mission objective.

[0014] Third, the problem of coordinating global and local perspectives. Most existing algorithms treat global path planning and local dynamic obstacle avoidance as two independent modules. Global planning generates static paths, while local planning enables real-time obstacle avoidance, lacking an effective coordination mechanism. Local obstacle avoidance is prone to deviating from the global path, leading to detours or even mission failure. The replanning trigger condition is singular and cannot adaptively adjust to environmental changes. The global path does not fully consider dynamic constraints, resulting in difficulties in local tracking. This fragmented design makes it difficult for UAVs to achieve mission objectives and ensure safe obstacle avoidance in complex dynamic environments, severely restricting the practical application of UAVs in complex scenarios. Summary of the Invention

[0015] The purpose of this invention is to address the problems in the existing UAV path planning methods, such as poor global path quality in complex dynamic environments, difficulty in meeting dynamic constraints, disconnect between local dynamic obstacle avoidance and global path, and lag in replanning, resulting in low planning success rate and insufficient flight safety. This invention provides a UAV path planning method and system based on improved bidirectional RRT and model predictive control.

[0016] To achieve the above objectives, the present invention employs the following technical solution: A UAV path planning method based on improved bidirectional RRT and model predictive control includes the following steps: Establish a kinematic and dynamic constraint model for the UAV and determine the range of flight physical constraints. Based on the kinematic and dynamic constraint model, a global reference path from the starting point to the target point is generated using a bidirectional RRT algorithm. Using the global reference path as the tracking target, a nonlinear model predictive controller is constructed. Based on the real-time acquired dynamic obstacle information, an obstacle repulsion potential penalty term is introduced into the nonlinear model predictive controller. Based on the introduced obstacle repulsion potential penalty term, a local obstacle avoidance control quantity that satisfies the flight physics constraints is generated. A global and local collaborative planning mechanism is constructed. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, a local reference path is regenerated starting from the current position of the UAV. The local reference path is then connected to the global reference path to obtain the UAV path planning result.

[0017] A further improvement of the present invention is that: The method, based on the kinematic and dynamic constraint model, uses a bidirectional RRT algorithm to generate a global reference path from the starting point to the target point, including: Initialize two random trees, with the starting point and the target point as the root nodes respectively; In each iteration, new nodes are generated by randomly sampling in the state space using heuristic target bias probabilities. Expand from each of the two trees toward the new node. When the distance between the two trees is less than a set threshold, use a greedy connection strategy to connect the two trees to obtain an initial feasible path. Redundant nodes are pruned from the initial feasible path, and the pruned path is smoothed using B-spline curves to generate a global reference path from the starting point to the target point.

[0018] The generation of local obstacle avoidance control quantities that satisfy flight physics constraints includes: Using the kinematic and dynamic constraint model of the UAV as the prediction model, the prediction time domain and control time domain are defined. Construct an objective function and use the maximum speed, maximum acceleration, and maximum yaw rate of change of the UAV as constraints for the optimization problem; Within each control cycle, based on the prediction time domain and the control time domain, the objective function is used as the optimization objective, and the constrained nonlinear optimization problem is solved in combination with the constraints to obtain the optimal control sequence that satisfies the flight physics constraints.

[0019] The objective function includes a trajectory tracking error term, a control variable change rate penalty term, and a dynamic obstacle avoidance penalty term based on the obstacle repulsion potential function; The trajectory tracking error term is used to minimize the deviation between the UAV's predicted position and the corresponding point on the global reference path; The control variable change rate penalty term is used to minimize the deviation between the UAV's predicted position and the corresponding point on the global reference path; The dynamic obstacle avoidance penalty is calculated based on the real-time acquired dynamic obstacle position information. When the distance between the drone and the obstacle is less than a preset safety threshold, the value of this penalty increases sharply as the distance decreases.

[0020] Solving constrained nonlinear optimization problems also includes introducing a rolling time-domain optimization strategy, including: When the optimal control sequence is obtained, the first control quantity in the optimal control sequence is output to the UAV for execution. At the next sampling time, the initial conditions are updated again with the actual state fed back by the UAV. The solution of the nonlinear optimization problem and the execution process of the UAV are repeated, and the time domain is optimized and corrected.

[0021] The global and local collaborative planning mechanism includes: Using the global reference path as the tracking target, dynamic obstacle information is detected in real time, and local obstacle avoidance is performed based on a nonlinear model predictive controller. When the nonlinear model predictive controller cannot solve for the local obstacle avoidance control quantity that satisfies the constraints within a preset time, the current global reference path is deemed to be invalid. The current nonlinear model predictive controller tracking task is paused. The current position of the UAV is taken as the new starting point, the original target endpoint remains unchanged, and the endpoint of the road segment on the global reference path that is closest to the new starting point and is not blocked is taken as the temporary sub-target. A local connection path between the new starting point and the temporary sub-target is generated based on the bidirectional RRT algorithm. The local join path is concatenated with the unreplaced portion of the global reference path to form a new global path; The nonlinear model predictive controller resumes its tracking task, using the updated global path as the tracking target, and continues its flight mission until the target endpoint is reached.

[0022] A UAV path planning system based on improved bidirectional RRT and model predictive control includes: The UAV dynamic model building module is used to establish the kinematic and dynamic constraint model of the UAV and determine the range of flight physical constraints. The global path generation module is used to generate a global reference path from the starting point to the target point based on the kinematic and dynamic constraint model and using the bidirectional RRT algorithm. The local obstacle avoidance control module is used to construct a nonlinear model predictive controller with the global reference path as the tracking target. Based on the real-time acquired dynamic obstacle information, an obstacle repulsion potential penalty term is introduced into the nonlinear model predictive controller, and a local obstacle avoidance control quantity that meets the flight physics constraints is generated based on the introduced obstacle repulsion potential penalty term. The path correction module is used to build a global and local collaborative planning mechanism. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, it regenerates a local reference path starting from the current position of the UAV and connects the local reference path to the global reference path to obtain the UAV path planning result.

[0023] A computer program product includes a computer program that, when executed by a processor, implements any one of the methods described.

[0024] A terminal device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the steps of any of the methods described above.

[0025] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of any of the methods described herein.

[0026] Compared with the prior art, the present invention has the following beneficial effects: This invention discloses a UAV path planning method based on an improved bidirectional Regression-Redirection Response (RRT) and Model Predictive Control (MMRC). By establishing a kinematic and dynamic constraint model and using bidirectional RRT to generate a global reference path, a global path that satisfies the physical constraints of the UAV can be obtained, avoiding the problems of traditional paths being tortuous or untrackable. Simultaneously, a nonlinear model predictive controller, using the global path as the tracking target and introducing an obstacle repulsion potential penalty term, can generate local obstacle avoidance control quantities that satisfy physical constraints in real time while tracking the global path, achieving a unified coordination between global guidance and dynamic obstacle avoidance, and preventing local obstacle avoidance from deviating from the mission direction. Furthermore, this invention constructs a global and local collaborative planning mechanism. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, a local reference path is regenerated from the current position and integrated into the global path, solving the problems of lag or failure in traditional replanning and improving the success rate and flight safety of UAV path planning in complex dynamic environments. Attached Figure Description

[0027] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0028] Figure 1 This is a flowchart disclosed in an embodiment of the present invention; Figure 2 This is a simulation diagram of the test environment disclosed in an embodiment of the present invention; Figure 3a This is a front view of the simulation results of the RRT algorithm disclosed in this embodiment of the invention; Figure 3b This is a top view of the simulation results of the RRT algorithm disclosed in an embodiment of the present invention; Figure 4a This is the main view of the simulation results of the RRT* algorithm disclosed in the embodiments of the present invention; Figure 4b This is a top view of the simulation results of the RRT algorithm disclosed in an embodiment of the present invention; Figure 5a This is the main view of the simulation results of the bidirectional RRT* algorithm with a target bias sampling strategy disclosed in an embodiment of the present invention; Figure 5b This is a top view of the simulation results of the bidirectional RRT* algorithm with a target bias sampling strategy disclosed in an embodiment of the present invention; Figure 6a This is the main view of the simulation results of the optimized bidirectional RRT* algorithm disclosed in the embodiments of the present invention; Figure 6bThis is a top view of the simulation results of the optimized bidirectional RRT* algorithm disclosed in an embodiment of the present invention. Detailed Implementation

[0029] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0030] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0031] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0032] In the description of the embodiments of the present invention, it should be noted that if terms such as "upper," "lower," "horizontal," or "inner" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of the invention is in use, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention. Furthermore, terms such as "first" and "second" are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0033] Furthermore, the use of the term "horizontal" does not imply that the component must be absolutely horizontal, but rather that it can be slightly tilted. For example, "horizontal" simply means that its direction is more horizontal than "vertical," and does not mean that the structure must be completely horizontal, but can be slightly tilted.

[0034] In the description of the embodiments of the present invention, it should also be noted that, unless otherwise explicitly specified and limited, the terms "set," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in the present invention according to the specific circumstances.

[0035] The present invention will now be described in further detail with reference to the accompanying drawings: See Figure 1 This invention discloses a path planning method for unmanned aerial vehicles (UAVs) based on improved bidirectional RRT and model predictive control. This method generates high-quality global paths by introducing heuristic sampling, path optimization strategies, and dynamic constraints. At the same time, it combines nonlinear model predictive control to achieve local dynamic obstacle avoidance and trajectory tracking, which significantly improves the planning success rate and flight safety of UAVs in complex dynamic environments.

[0036] Specifically, the following steps are included: Step S1: Establish a mathematical model of the UAV During UAV flight, its attitude constantly changes, and the body coordinate system also changes accordingly, while the world coordinate system remains fixed. Therefore, the attitude information of the UAV, such as rotation angles, can be transformed to the world coordinate system through a homogeneous linear transformation using a rotation matrix. Based on the transformation relationship from the body coordinate system to the world coordinate system, the rotation matrices of the UAV around the x, y, and z axes of the body coordinate system can be obtained, thus yielding the complete rotation matrix from the body coordinate system to the world coordinate system. The attitude information in the world coordinate system is converted into quaternions, which are then represented and analyzed in the UAV's own body coordinate system. The attitude quaternion of the UAV is defined as follows: ,in:

[0037] Furthermore, the rotation matrix from the world coordinate system to the body coordinate system can be obtained:

[0038] Furthermore, a kinematic and dynamic mathematical model is established based on this. The dynamic model is mainly used to describe the dynamic relationship between the thrust acting on the UAV and the angular velocity of the airframe.

[0039]

[0040] Kinematic models are mainly used to describe the kinematic relationship between the position, velocity, and attitude of a UAV.

[0041]

[0042]

[0043]

[0044] Step S2: Global static path planning based on the improved bidirectional RRT* algorithm To address the issues of slow convergence speed and high path redundancy in the traditional RRT* algorithm, the following improvement strategies are proposed: S2.1 Heuristic Target Bias Sampling During the random sampling process, sampling is performed with probability p biased towards the target point. The formula for generating random sampling points is as follows:

[0045] Simultaneously, the bias probability is adaptively adjusted based on environmental complexity to accelerate the expansion of the search tree towards the target region. The adaptively adjusted bias probability is defined as:

[0046] S2.2, Bidirectional Search Tree Growth Strategy: Simultaneously, two random trees are generated from the starting point and the target point. Let the current iteration number be k. The expansion of the two trees is carried out alternately to avoid a single tree getting stuck in a local region: Extend(Ta) → Extend(Tb) → Connect(Ta,Tb) A greedy join strategy is introduced: when the distance between two trees is less than a threshold, a direct join is attempted to quickly obtain an initial feasible path.

[0047] S2.3, Path pruning and B-spline smoothing: Redundant nodes are pruned from the initially generated path to remove unnecessary turning points.

[0048] Furthermore, the pruned path is smoothed using a cubic uniform B-spline curve to ensure that the generated path meets the dynamic constraints of the UAV. The B-spline curve is described by the following formula:

[0049] Furthermore, the basis functions of B-splines are defined as follows:

[0050] When p = 3, the B-spline curve is a combination of cubic polynomials, and the cubic B-spline basis functions are described by the following formula:

[0051] S2.4 Path cost function optimization: Define a cost function that comprehensively considers path length, path smoothness, and obstacle safety distance. During the reselection of parent nodes and rewiring in RRT*, use this cost function as the optimization objective to ensure that the final generated global path is an approximate optimal solution.

[0052] Define the path cost from the starting point to any node q as:

[0053] Wherein, the path length cost is the cumulative Euclidean distance from the starting point to node q:

[0054] Path smoothness cost is used to penalize sharp turns in the path, ensuring that the curvature of the generated path changes gradually. The steering angle at node qi is defined as:

[0055] The path smoothness cost is then the weighted sum of the turning angles of each node:

[0056] To ensure flight safety, the path should maintain a sufficient safe distance from obstacles. Let di be the distance from node qi to the nearest obstacle, and the obstacle safety distance cost is designed using an exponential penalty function:

[0057] Step S3: Local dynamic obstacle avoidance and trajectory tracking based on nonlinear model predictive control.

[0058] Using the smooth global path generated in step S2 as a reference trajectory, a nonlinear model predictive controller is designed for real-time tracking and dynamic obstacle avoidance. S3.1 Prediction Model Construction: The UAV dynamics model established in step S1 is used as the prediction model, and the prediction time domain and control time domain are set.

[0059] S3.2, Objective Function Design: The design incorporates a multi-objective cost function that includes trajectory tracking error, control variable change rate, and obstacle repulsion potential function. The obstacle repulsion potential function continuously monitors the distance between the UAV and dynamic obstacles. When the distance is less than a safety threshold, the cost function automatically adds a penalty term, driving the UAV to generate a local obstacle avoidance trajectory.

[0060] Let the prediction time domain be Np and the control time domain be Nc. The objective function of the optimization problem at time t is defined as:

[0061] The trajectory tracking error term is used to guide the UAV to track the global reference trajectory and is defined as follows: +

[0062] The control cost includes control amplitude penalty and control change rate penalty, which correspond to energy consumption and actuator wear, respectively. Control amplitude penalty:

[0063] Penalty for rate of change of control quantity:

[0064] An exponential repulsive potential function senses dynamic obstacles in real time and generates obstacle avoidance commands. Let the UAV's position be p, and the position of the j-th obstacle be p. obs,j The velocity vector is v obs,j The repulsive potential function is then defined as:

[0065] S3.3, Constraint Handling: By using the speed, acceleration, and yaw rate of change of the UAV as hard constraints in the optimization problem, we can ensure that the generated local trajectory always remains within the physical limits of the UAV.

[0066] S3.4, Optimization of Scrolling Time Domain: Within each control cycle, a constrained nonlinear optimization problem is solved to obtain the optimal control sequence, and the first control variable is applied to the UAV to achieve rolling optimization and feedback correction.

[0067] At time t, the rolling time-domain optimization problem can be formulated as: Optimize variables:

[0068] Optimization issues:

[0069] Constraints:

[0070]

[0071]

[0072]

[0073]

[0074] Step S4: Global-Local Cooperative Planning Strategy Construct a collaborative mechanism between the global planning layer and the local planning layer, including: The UAV flies along the global path generated in step S2, and the local planner in step S3 tracks it in real time. When encountering dynamic obstacles that prevent the local planner from finding a feasible trajectory within the constraints, a local replanning request is triggered. The UAV's current position is used as the new starting point, the target point remains unchanged, and the improved bidirectional RRT* is used to perform local fast replanning, and the new path is connected to the global path.

[0075] The tracking error threshold is used for determination. First, if the position tracking error exceeds the limit, the maximum position tracking error in the prediction time domain is defined as follows:

[0076] When the maximum tracking error exceeds the safety threshold, it indicates that the local planning has deviated significantly from the global path, and continued tracking may lead to mission failure. The maximum threshold is typically 1.5 to 2 times the size of the drone's fuselage.

[0077] Secondly, considering the cumulative tracking error, the average tracking error within the sliding window is defined, taking into account the time accumulation effect of the error:

[0078] The judgment condition is: Furthermore, the duration exceeds 0.5-1 second, and the continuous condition avoids misjudgment triggered by momentary interference.

[0079] To verify the effectiveness of the algorithm improvement, simulation experiments were conducted. A test environment was built using simulation software to compare the algorithm at different optimization stages, running the experiment 40 times and recording the data. The simulation test environment was as follows: Figure 2 As shown, this simulates a drone flying in the indoor environment of a high-rise building. The protrusions in the image represent obstacles of varying sizes and heights, simulating various sufficiently tall obstacles that could affect the drone's flight in a confined indoor environment. The map environment is set to 100dm. 100dm In a 20dm three-dimensional space, the starting coordinates are set to (0, 0, 0) and the ending coordinates are set to (100, 100, 10). The parameter settings of the algorithm are shown in Table 1.

[0080] Table 1 Algorithm Parameter Settings

[0081] In this simulation environment, simulation experiments were conducted to verify the RRT algorithm, the RRT* algorithm, the bidirectional RRT* algorithm incorporating a target bias sampling strategy, and the final algorithm after midpoint trimming and B-spline smoothing optimization of the generated path. The simulation results are shown in the figure below. Figures 3a-6b As shown in the diagram, the green dots represent the starting point, the red dots represent the target endpoint, the green lines represent strips of random trees starting from the starting point, the blue lines represent lines of random trees starting from the endpoint, and the final generated path is represented by the red lines.

[0082] First, from Figure 3a and Figure 3bIt can be seen that the traditional RRT algorithm generates nodes with too much randomness, resulting in excessively long paths with significant curves and low exploration efficiency. The simulation results of the RRT* algorithm after sampling, reselecting parent nodes, and rewiring are as follows: Figure 4a and Figure 4b As shown, the RRT* algorithm increases the number of nodes generated, which alleviates the problem of large tortuosity in the RRT algorithm. However, the RRT* algorithm explores a large amount of invalid space and still has large tortuosity, so the effect is still not good. Figure 5a and Figure 5b The figure shows the experimental results after introducing the target bias sampling strategy and implementing the bidirectional RRT algorithm. It is clear from the figure that the number of invalid branches generated by the random tree is significantly reduced, and the large, obvious tortuosity problem in the generated path is effectively solved. To make the generated path smoother, the simulation results after introducing the midpoint pruning strategy and the B-spline smoothing optimization strategy are shown below. Figure 6a and Figure 6b As shown in the results, the generated path becomes better and smoother.

[0083] The simulation experiment was repeated 40 times, and the average simulation data was recorded in Table 2. The data in the table shows that the traditional RRT algorithm has a long path length, many iterations, and numerous large bends, which significantly increases the energy consumption of the UAV and compromises its safety. The RRT* algorithm significantly reduces the path length by approximately 9.59%, optimizes the number of iterations, and reduces the number of large bends by 52.63%. After introducing a target bias sampling strategy and implementing a bidirectional RRT* algorithm, the number of iterations decreased by 76.62%, indicating a significant improvement in the algorithm's convergence speed, while further reducing the number of large bends generated by the random tree. Finally, by introducing a midpoint pruning strategy and a B-spline smoothing optimization strategy, the smooth length of the generated path was further improved, achieving zero large bends.

[0084] Table 2 Simulation Data Table

[0085] A UAV path planning system based on improved bidirectional RRT and model predictive control includes: The UAV dynamic model building module is used to establish the kinematic and dynamic constraint model of the UAV and determine the range of flight physical constraints. The global path generation module is used to generate a global reference path from the starting point to the target point based on the kinematic and dynamic constraint model and using the bidirectional RRT algorithm. The local obstacle avoidance control module is used to construct a nonlinear model predictive controller with the global reference path as the tracking target. Based on the real-time acquired dynamic obstacle information, an obstacle repulsion potential penalty term is introduced into the nonlinear model predictive controller, and a local obstacle avoidance control quantity that meets the flight physics constraints is generated based on the introduced obstacle repulsion potential penalty term. The path correction module is used to build a global and local collaborative planning mechanism. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, it regenerates a local reference path starting from the current position of the UAV and connects the local reference path to the global reference path to obtain the UAV path planning result.

[0086] Compared with existing traditional UAV path planning and obstacle avoidance algorithms, the method disclosed in this invention has the following advantages: First, by combining the improved bidirectional RRT* algorithm with B-spline smoothing, a high-quality global path with shorter length, continuous curvature, and satisfying UAV dynamic constraints is generated; second, the introduction of nonlinear model predictive control enables real-time perception and avoidance of dynamic obstacles while tracking the global path, avoiding the lag problem of traditional replanning methods; finally, the global-local cooperative mechanism ensures rapid replanning in extreme dynamic environments, improving the success rate of task completion.

[0087] A schematic diagram of a terminal device according to an embodiment of the present invention. The terminal device of this embodiment includes: a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the various method embodiments described above. Alternatively, when the processor executes the computer program, it implements the functions of each module / unit in the various device embodiments described above.

[0088] The computer program can be divided into one or more modules / units, which are stored in the memory and executed by the processor to complete the present invention.

[0089] The terminal device can be a desktop computer, laptop computer, cloud server, or other device with strong computing power. The terminal device may include, but is not limited to, a processor and memory.

[0090] The optimal choice for the processor is a multi-core high-speed central processing unit (CPU).

[0091] The memory can be used to store the computer program and / or module. The processor implements various functions of the terminal device by running or executing the computer program and / or module stored in the memory and calling the data stored in the memory.

[0092] If the modules / units integrated into the terminal device are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0093] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A path planning method for unmanned aerial vehicles (UAVs) based on improved bidirectional RRT and model predictive control, characterized in that, Includes the following steps: Establish a kinematic and dynamic constraint model for the UAV and determine the range of flight physical constraints. Based on the kinematic and dynamic constraint model, a global reference path from the starting point to the target point is generated using a bidirectional RRT algorithm. Using the global reference path as the tracking target, a nonlinear model predictive controller is constructed. Based on the real-time acquired dynamic obstacle information, an obstacle repulsion potential penalty term is introduced into the nonlinear model predictive controller. Based on the introduced obstacle repulsion potential penalty term, a local obstacle avoidance control quantity that satisfies the flight physics constraints is generated. A global and local collaborative planning mechanism is constructed. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, a local reference path is regenerated starting from the current position of the UAV. The local reference path is then connected to the global reference path to obtain the UAV path planning result.

2. The UAV path planning method based on improved bidirectional RRT and model predictive control according to claim 1, characterized in that, The method, based on the kinematic and dynamic constraint model, uses a bidirectional RRT algorithm to generate a global reference path from the starting point to the target point, including: Initialize two random trees, with the starting point and the target point as the root nodes respectively; In each iteration, new nodes are generated by randomly sampling in the state space using heuristic target bias probabilities. Expand from each of the two trees toward the new node. When the distance between the two trees is less than a set threshold, use a greedy connection strategy to connect the two trees to obtain an initial feasible path. Redundant nodes are pruned from the initial feasible path, and the pruned path is smoothed using B-spline curves to generate a global reference path from the starting point to the target point.

3. The UAV path planning method based on improved bidirectional RRT and model predictive control according to claim 1, characterized in that, The generation of local obstacle avoidance control quantities that satisfy flight physics constraints includes: Using the kinematic and dynamic constraint model of the UAV as the prediction model, the prediction time domain and control time domain are defined. Construct an objective function and use the maximum speed, maximum acceleration, and maximum yaw rate of change of the UAV as constraints for the optimization problem; Within each control cycle, based on the prediction time domain and the control time domain, the objective function is used as the optimization objective, and the constrained nonlinear optimization problem is solved in combination with the constraints to obtain the optimal control sequence that satisfies the flight physics constraints.

4. The UAV path planning method based on improved bidirectional RRT and model predictive control according to claim 3, characterized in that, The objective function includes a trajectory tracking error term, a control variable change rate penalty term, and a dynamic obstacle avoidance penalty term based on the obstacle repulsion potential function; The trajectory tracking error term is used to minimize the deviation between the UAV's predicted position and the corresponding point on the global reference path; The control variable change rate penalty term is used to minimize the deviation between the UAV's predicted position and the corresponding point on the global reference path; The dynamic obstacle avoidance penalty is calculated based on the real-time acquired dynamic obstacle position information. When the distance between the drone and the obstacle is less than a preset safety threshold, the value of this penalty increases sharply as the distance decreases.

5. The UAV path planning method based on improved bidirectional RRT and model predictive control according to claim 3, characterized in that, Solving constrained nonlinear optimization problems also includes introducing a rolling time-domain optimization strategy, including: When the optimal control sequence is obtained, the first control quantity in the optimal control sequence is output to the UAV for execution. At the next sampling time, the initial conditions are updated again with the actual state fed back by the UAV. The solution of the nonlinear optimization problem and the execution process of the UAV are repeated, and the time domain is optimized and corrected.

6. The UAV path planning method based on improved bidirectional RRT and model predictive control according to claim 1, characterized in that, The global and local collaborative planning mechanism includes: Using the global reference path as the tracking target, dynamic obstacle information is detected in real time, and local obstacle avoidance is performed based on a nonlinear model predictive controller. When the nonlinear model predictive controller cannot solve for the local obstacle avoidance control quantity that satisfies the constraints within a preset time, the current global reference path is deemed to be invalid. The current nonlinear model predictive controller tracking task is paused. The current position of the UAV is taken as the new starting point, the original target endpoint remains unchanged, and the endpoint of the road segment on the global reference path that is closest to the new starting point and is not blocked is taken as the temporary sub-target. A local connection path between the new starting point and the temporary sub-target is generated based on the bidirectional RRT algorithm. The local join path is concatenated with the unreplaced portion of the global reference path to form a new global path; The nonlinear model predictive controller resumes its tracking task, using the updated global path as the tracking target, and continues its flight mission until the target endpoint is reached.

7. A UAV path planning system based on improved bidirectional RRT and model predictive control, characterized in that, include: The UAV dynamic model building module is used to establish the kinematic and dynamic constraint model of the UAV and determine the range of flight physical constraints. The global path generation module is used to generate a global reference path from the starting point to the target point based on the kinematic and dynamic constraint model and using the bidirectional RRT algorithm. The local obstacle avoidance control module is used to construct a nonlinear model predictive controller with the global reference path as the tracking target. Based on the real-time acquired dynamic obstacle information, an obstacle repulsion potential penalty term is introduced into the nonlinear model predictive controller, and a local obstacle avoidance control quantity that meets the flight physics constraints is generated based on the introduced obstacle repulsion potential penalty term. The path correction module is used to build a global and local collaborative planning mechanism. When the nonlinear model predictive controller cannot find a feasible trajectory within a preset time, it regenerates a local reference path starting from the current position of the UAV and connects the local reference path to the global reference path to obtain the UAV path planning result.

8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-6.

9. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1-6.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1-6.

Images