Improved anti-interference path planning and optimizing system

Abstract

The invention belongs to the technical field of unmanned aerial vehicles, and particularly relates to an improved anti-interference path planning and optimizing system, which comprises the steps of initializing an opening list and a closing list, setting a starting point and an ending point, adding the starting point into a cost list and taking the starting point as a father node, setting a minimum cost point, and setting a minimum cost point. Adding the minimum cost point into a closing list, traversing surrounding eight points, starting search circulation, designing a perturbation acceleration penalty function, designing an obstacle distance penalty function, calculating a heuristic function, judging whether the current node is in an opening list, if not, adding the node into the opening list, and if yes, executing the next step; and judging the cost of the node and the old node, judging whether a father node is an end point or not, if the father node is the end point, jumping out of the search cycle, and if the father node is not the end point, continuing the cycle. According to the method, rapid, safe and efficient path planning can be realized in a complex environment, and the navigation performance and task success rate of the unmanned aerial vehicle are remarkably improved.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to an improved anti-interference path planning and optimization system. Background Art

[0002] With the rapid development of drone technology, multi-rotor drones have been widely used in military reconnaissance, agricultural plant protection, industrial inspection, civil aerial photography and other fields due to their vertical take-off and landing, hovering and strong maneuverability. In the process of performing tasks, multi-rotor drones need to perform effective path planning in complex and changing environments to ensure the smooth completion of tasks and flight safety. However, the existing path planning methods have many shortcomings when facing complex environments such as urban built-up areas and forests.

[0003] Traditional path planning algorithms, such as A*, RRT (Rapidly-exploring Random Tree), Dijkstra and BFS (Breadth-First Search), can provide a path from the starting point to the end point for multi-rotor drones to a certain extent, but these algorithms often pursue the shortest path too much, resulting in the planned trajectory being too extreme, close to obstacles, and lacking the necessary safety margin. In actual flight, due to the influence of wind disturbance or other disturbance factors, the drone can easily deviate from the predetermined trajectory, resulting in a collision accident;

[0004] In addition, the path planning algorithms in the existing technologies usually only focus on the generation of the path, but do not fully consider the quality of the path. For example, the planned path may contain small and sharp angles, which requires the UAV to make large-angle turns during flight, placing high demands on the flight controller and perception algorithm. If the UAV's response capability is insufficient, it may not be able to accurately fly along the planned path, thus affecting the execution efficiency and success rate of the mission.

[0005] Back-end trajectory optimization algorithms, such as B-spline, Bezier curve, polynomial curve, etc., can optimize the flight trajectory to a certain extent and improve the smoothness of flight, but these algorithms require the onboard computer to have strong computing power. More importantly, if the trajectory generated by the front-end path planning algorithm itself has defects, such as small acute angle problems, then even if the back-end trajectory optimization algorithm is applied, its effect will be limited and it will not be able to fundamentally solve the problem of poor trajectory quality;

[0006] Therefore, the existing multi-rotor UAV path planning technology cannot meet the high requirements for flight safety, efficiency and reliability in complex environments. Developing a system and method that can adapt to environmental changes, optimize flight paths, and adaptively resist interference is of great significance to improving the navigation capability and mission success rate of UAVs. Summary of the invention

[0007] The purpose of the present invention is to provide an improved anti-interference path planning and optimization system, which can realize fast, safe and efficient path planning in complex environments, and significantly improve the navigation performance and mission success rate of UAVs.

[0008] The technical solution adopted by the present invention is as follows:

[0009] An improved anti-disturbance path planning system comprises the following steps:

[0010] S1: Initialize the open list and the closed list, set the start point and the end point, add the start point to the cost list and make it the parent node;

[0011] S2: Set the minimum cost point, add the minimum cost point to the closed list, traverse the eight surrounding points, and start the search loop;

[0012] S3: Design disturbance acceleration penalty function;

[0013] S4: Design obstacle distance penalty function;

[0014] S5: Calculate heuristic function;

[0015] S6: Determine whether the current node is in the open list. If not, add the node to the open list and determine the cost of the node compared to the old node.

[0016] S7: Determine whether the parent node is the end point. If the parent node is the end point, exit the search loop; if not, continue the loop.

[0017] An improved anti-disturbance path optimization algorithm is used to optimize the above-mentioned anti-disturbance path planning system, and the optimization algorithm comprises the following steps:

[0018] A1: Design smoothing optimization algorithm;

[0019] A2: Design a dynamic curvature adjustment algorithm;

[0020] A3: Output the final path, trace back from the target node to the starting node, and output the final smooth path.

[0021] The technical effects achieved by the present invention are:

[0022] (1) The present invention is used to construct a dynamic disturbance acceleration penalty function, which calculates the impact of acceleration disturbance on path planning based on the real-time acceleration data of the UAV and the preset flight parameters, and uses it as part of the path cost. A real-time obstacle distance penalty function is developed, which dynamically adjusts the priority of obstacle avoidance in path planning based on the relative position and speed of the UAV and the obstacle, and designs a path sharp angle dynamic smoothing algorithm, which dynamically adjusts the path by calculating the curvature change of the path segment to reduce the sharp angle in the path and improve the flight safety of the UAV;

[0023] The above functions are integrated to form an improved A* algorithm, which can achieve optimal path planning while ensuring the shortest path and taking into account the dynamic characteristics of the drone and environmental obstacles.

[0024] (2) The path planned by the present application no longer has small acute angles, and is more in line with the kinematic and dynamic characteristics of the drone, reducing the pressure on the controller and perception system. The anti-disturbance acceleration penalty function and obstacle distance penalty function are introduced to improve the robustness of obstacle avoidance in complex environments and reduce the risk of collision. Furthermore, there is no need to rely on the back-end trajectory optimization algorithm. A safe path is planned in accordance with the kinematics and dynamics of the drone, reducing the computing power requirements of the onboard computer. A certain amount of anti-disturbance redundancy has been considered in path planning, and the ability to quickly avoid obstacles can be maintained even under wind speed interference; BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It is a flow chart of the technical solution in the present invention. DETAILED DESCRIPTION

[0026] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0027] Embodiment 1:

[0028] An improved anti-disturbance path planning system comprises the following steps:

[0029] S1: Initialize the open list (Openlist) and the closed list (ClosedList), and set the start point (Start) and the end point (Goal). Add the start point to the cost list and set its G value to 0. The G value represents the actual cost from the start point to the current node. The H value is the heuristic estimate from the start point to the end point. It is usually calculated using methods such as Euclidean distance and Manhattan distance. The cost function of the start point is calculated and used as the parent node. The open list is used to store nodes to be explored, and the closed list is used to store nodes that have been explored.

[0030] S2: Set the minimum cost point, add the minimum cost point to the closed list, traverse the eight surrounding points, and start the search loop. This method is a common way to explore surrounding nodes in two-dimensional space;

[0031] S3: Design a disturbance acceleration penalty function, which is used to quantify the difference between the expected acceleration and the current acceleration, and use the difference as the disturbance acceleration, wherein the calculation formula of the disturbance acceleration penalty function is as follows:

[0032] P a (a ref ,a cur ) = k a ||a ref -a cur || 2

[0033] Among them, a ref is the reference acceleration, a cur is the current acceleration vector obtained by the IMU sensor, which is a device that can measure the acceleration and angular velocity of an object. represents the distance from the first obstacle where n is 1 to the mth obstacle, λP a is the weight coefficient of the perturbation acceleration penalty function. In aircraft and spacecraft, IMU sensors are the core components of the inertial navigation system (INS). By integrating acceleration and angular velocity, the position, velocity and attitude of the aircraft can be determined. In satellites, IMU sensors can provide high-precision attitude and orbit data in a short time without external reference signals. a is a positive parameter used to adjust the penalty weight of acceleration deviation;

[0034] S4: Design an obstacle distance penalty function, which combines the distance between the current node and the obstacle and the disturbance acceleration penalty function to quantify the degree of obstacle avoidance in path planning. The calculation formula of the obstacle distance penalty function is as follows:

[0035]

[0036] Among them, d j (x i ,y i ) is the path point (x i ,y i ) to the jth obstacle, k d is a positive constant used to adjust the penalty weight of obstacle distance.

[0037] S5: Calculate a heuristic function, the calculation formula of the heuristic function is as follows:

[0038] f(n)=g(n)+h(n)+P d (n)

[0039] Among them, f(n) is the total cost function, g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, P d (n) is the obstacle distance penalty function.

[0040] S6: Determine whether the current node is in the open list. If not, the node is an unexplored node. Therefore, add the node to the open list and determine the cost of this node compared to the old node. This is to determine the cost of g(n) and P of this node. d (n) Is it less than the old cost? If so, update the new heuristic function. If not, skip the judgment and take the node with the minimum cost as the parent node.

[0041] S7: Determine whether the parent node is the end point. If the parent node is the end point, exit the search loop. If it is not the end point, continue the loop. The parent node is used to backtrack the path, that is, backtrack from the end point to the starting point to find the complete path.

[0042] Embodiment 2:

[0043] An improved anti-disturbance path optimization algorithm is used to optimize the above-mentioned anti-disturbance path planning system, and the optimization algorithm comprises the following steps:

[0044] A1: Design smoothing optimization algorithm;

[0045] The pseudo code of the smoothing optimization algorithm in A1 is as follows:

[0046] Algorithm: Curvature Dynamic Adjustment (CDA);

[0047] Input: path array path[], threshold angle thresholdAngle (the limit value of the acute angle);

[0048] Output: smoothed path smoothPath[].

[0049] 1. Initialize smoothPath to an empty array;

[0050] 2. For each segment in path from i to i+1;

[0051] 3. Calculate the turning angle angle of the path segment segment;

[0052] 4. If angle <thresholdAngle;

[0053] 5. Calculate the curvature of the segment;

[0054] 6. If curvature is greater than the threshold curvature thresholdCurvature;

[0055] 7. Dynamically adjust the curvature of the segment to make it smoothly transition to the threshold curvature;

[0056] 8. Reconstruct the segment using the points adjusted for curvature;

[0057] 9. Add the adjusted segment to smoothPath;

[0058] 10. Otherwise;

[0059] 11. Add the original path segment segment to smoothPath;

[0060] 12. Return smoothPath.

[0061] A2: Design a dynamic curvature adjustment algorithm;

[0062] The pseudo code is as follows:

[0063] Sub-algorithm: Dynamic Curvature Adjustment (DCA);

[0064] Input: path segment segment, threshold curvature thresholdCurvature;

[0065] Output: path segment adjustedSegment after adjusting the curvature;

[0066] 1. Initialize adjustedSegment to an empty array;

[0067] 2. Calculate the curvature of the segment;

[0068] 3. If curvature>thresholdCurvature;

[0069] 4. Calculate the curvature difference that needs to be adjusted;

[0070] Among them, deltaCurvature=curvature-thresholdCurvature;

[0071] 5. Dynamically adjust the control points of the segment according to deltaCurvature to reduce the curvature;

[0072] 6. For each point p in the segment;

[0073] 7. Calculate the new position p' of point p so that the curvature of point p' is close to thresholdCurvature;

[0074] 8. Add p' to adjustedSegment;

[0075] 9. Return adjustedSegment.

[0076] A3: Output the final path, trace back from the target node to the starting node, and output the final smooth path, which takes into account the dynamic characteristics of the drone and environmental obstacles, and realizes the optimal path planning.

[0077] This application is used to construct a dynamic disturbance acceleration penalty function, which calculates the impact of acceleration disturbance on path planning based on the real-time acceleration data of the drone and the preset flight parameters, and uses it as part of the path cost. It also develops a real-time obstacle distance penalty function, which combines the relative position and speed of the drone and the obstacle to dynamically adjust the priority of obstacle avoidance in path planning, and designs a path sharp angle dynamic smoothing algorithm, which dynamically adjusts the path by calculating the curvature change of the path segment to reduce the sharp angle in the path and improve the flight safety of the drone;

[0078] The above functions are integrated to form an improved A* algorithm, which can achieve optimal path planning while ensuring the shortest path and taking into account the dynamic characteristics of the drone and environmental obstacles.

[0079] The path planned by this application no longer has small acute angles, and is more in line with the kinematic and dynamic characteristics of the drone, reducing the pressure on the controller and perception system. The anti-disturbance acceleration penalty function and obstacle distance penalty function are introduced to improve the robustness of obstacle avoidance in complex environments and reduce the risk of collision. In addition, there is no need to rely on the back-end trajectory optimization algorithm. In the case of conforming to the kinematics and dynamics of the drone, a safe path is planned, reducing the computing power requirements of the onboard computer. A certain amount of anti-disturbance redundancy has been considered in path planning, and the ability to quickly avoid obstacles can be maintained even under wind speed interference;

[0080] Based on the above description, the present invention provides a multi-rotor UAV adaptive path optimization system and method through the integration of adaptive heuristic functions, dynamic disturbance acceleration penalty functions, real-time obstacle distance penalty functions and path acute angle dynamic smoothing algorithms. The system and method can realize fast, safe and efficient path planning in complex environments, significantly improving the navigation performance and mission success rate of the UAV.

[0081] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. An improved anti-disturbance path planning system, characterized in that: The following steps are involved: S1: Initialize the open list and the closed list, set the start point and the end point, add the start point to the cost list and make it the parent node; S2: Set the minimum cost point, add the minimum cost point to the closed list, traverse the eight surrounding points, and start the search loop; S3: Design disturbance acceleration penalty function; S4: Design obstacle distance penalty function; S5: Calculate heuristic function; S6: Determine whether the current node is in the open list. If not, add the node to the open list and determine the cost of the node compared to the old node. S7: Determine whether the parent node is the end point. If the parent node is the end point, exit the search loop; if not, continue the loop.

2. The anti-disturbance path planning system according to claim 1, characterized in that: After the starting point is added to the open list in S1, its G value is set to 0, its H value is the heuristic estimated value from the starting point to the end point, and the cost function of the starting point is calculated.

3. The anti-disturbance path planning system according to claim 2, characterized in that: The perturbation acceleration penalty function in S3 is used to quantify the difference between the expected acceleration and the current acceleration, and the difference is used as the perturbation acceleration, wherein the calculation formula of the perturbation acceleration penalty function is as follows: P a (a ref ,a cur )=k a ||a ref -a cur || 2 Among them, a ref is the reference acceleration, a cur is the current acceleration vector obtained by the IMU sensor, k a is a positive parameter used to adjust the penalty weight of acceleration deviation, P a represents the perturbation acceleration penalty function.

4. The anti-disturbance path planning system according to claim 3, characterized in that: The obstacle distance penalty function in S4 combines the distance between the current node and the obstacle and the disturbance acceleration penalty function to quantify the degree of obstacle avoidance in path planning. The calculation formula of the obstacle distance penalty function is as follows: Among them, d j (x i ,y i ) is the path point (x i ,y i ) to the jth obstacle, k d is a positive constant used to adjust the penalty weight of obstacle distance. It means from the first obstacle where n is 1 to the mth obstacle, λP a is the weight coefficient of the perturbation acceleration penalty function.

5. The anti-disturbance path planning system according to claim 4, characterized in that: The calculation formula of the heuristic function in S5 is as follows: f(n)=g(n)+h(n)+P d (n) Among them, f(n) is the total cost function, g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, P d (n) is the obstacle distance penalty function.

6. The anti-disturbance path planning system according to claim 5, characterized in that: The cost of determining the node and the old node in S6 is: d (n) Is it less than the old cost? If so, update the new heuristic function. If not, skip the judgment and take the node with the minimum cost as the parent node.

7. An improved anti-interference path optimization algorithm, characterized in that: The optimization algorithm is used to optimize the anti-disturbance path planning system according to any one of claims 1 to 6, and the optimization algorithm comprises the following steps: A1: Design smoothing optimization algorithm; A2: Design a dynamic curvature adjustment algorithm; A3: Output the final path, trace back from the target node to the starting node, and output the final smooth path.

8. The optimization algorithm according to claim 7, characterized in that: The pseudo code of the smoothing optimization algorithm in A1 is as follows: Input: path array path[], threshold angle thresholdAngle; Output: smoothed path smoothPath[].

9. The optimization algorithm according to claim 7, characterized in that: The pseudo code of the dynamic curvature adjustment algorithm in A2 is as follows: Input: path segment segment, threshold curvature thresholdCurvature; Output: The path segment adjustedSegment after adjusting the curvature.

Images