Unmanned aerial vehicle flight path planning method based on multi-step particle swarm algorithm and improved RRT algorithm

Abstract

The invention provides an unmanned aerial vehicle flight path planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm, and the method comprises the specific steps: taking a step length as a fixed value, taking a maximum turning angle and a maximum height variation as optimization variables, and carrying out the iterative updating of the positions of particles in a randomly generated particle swarm under the flight constraint of an unmanned aerial vehicle, obtaining a planned course with an optimal turning angle and height variation, and determining an optimal planning point of an actual step length of flight along the course; when the distance between the optimal planning point and the end point is smaller than the actual step length, stopping iterative updating to obtain the optimal offline track of the unmanned aerial vehicle; and if the no-fly zone is detected in the flight process, performing online re-planning by using an improved RRT algorithm to generate a new flight path. According to the method, three-dimensional track planning dimension reduction is changed into two-dimensional optimization, and adaptive adjustment is performed according to the distance between the starting point and the ending point of the track in online track planning, so that the real-time precision and efficiency of online track planning are improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) trajectory technology, and in particular to a UAV trajectory planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm. Background Art

[0002] UAV trajectory planning is an important part of the autonomous navigation module. A good trajectory can greatly improve the success rate of the mission. Trajectory planning needs to consider flight performance and environmental factors. Flight performance includes the maximum turning angle and maximum climb rate of the UAV, and the environment includes no-fly zones and flight boundaries. The above factors together constitute the flight constraints of the UAV.

[0003] A good trajectory can be planned in detail before the mission begins, but considering the urgency of the mission, this planning still has certain time requirements. Currently, offline trajectory planning mostly uses mathematical methods with higher precision such as pseudo-spectral method. Different from the characteristics of offline trajectory planning, online trajectory planning has higher time requirements. The trajectory planned online may not meet the offline planning standards, so the two situations should be considered separately.

[0004] Heuristic algorithms are rarely used in current UAV trajectory planning technology, and the algorithms that have been applied are mostly targeted at specific scenarios. Therefore, an offline trajectory planning and online trajectory planning technology suitable for multiple scenarios is an important issue that needs to be solved urgently. Summary of the invention

[0005] In view of the problems existing in the prior art, the present invention provides a UAV trajectory planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm, i.e., a rapidly expanding random tree, which simplifies a three-dimensional offline trajectory into a two-dimensional trajectory planning, and obtains a planning direction with an optimal turning angle and height change by iteratively updating the position of particles in a randomly generated particle swarm through a multi-step particle swarm algorithm, and determines the optimal planning point of the actual step length of flight along the heading to obtain the optimal offline trajectory, thereby solving the offline trajectory planning problem of the UAV's reconnaissance and escort missions in the no-fly zone space. Through the improved RRT algorithm, two parameter values ​​of the maximum turning angle and the maximum climb rate are introduced in the process of selecting the next planning point, and the next planning point is obtained by performing distance adaptation between the planning starting point and the planning end point, and a new trajectory is obtained, thereby solving the local trajectory online planning problem of the UAV in the case of a sudden change in the no-fly zone.

[0006] The present invention provides a UAV trajectory planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm, which comprises the following steps:

[0007] S1. Determine the basic parameters of the UAV trajectory planning, which include obstacles, target points, flight boundaries under flight constraints, and basic parameters of the UAV's initial state.

[0008] S2, input the step length l, the maximum height change Δh and the maximum turning angle Δφ between adjacent track points in the track into the multi-step particle swarm algorithm for iteration, and use the multi-step particle swarm algorithm to plan the best obstacle avoidance path to obtain the offline track of the UAV track. The specific operation steps are as follows:

[0009] S21. Under the flight constraints of the UAV, the step length l is taken as a constant, the maximum turning angle Δφ and the maximum height change Δh are taken as optimization variables, and the position information p[φ, h] of the planned point in the offline trajectory is obtained.

[0010] S22. The spatial area between the actual step length 1 and the predicted step length 31 is set as the prediction domain, and the spatial area between the current planning point and the actual step length 1 is set as the execution domain.

[0011] S23, in the prediction domain and execution domain obtained in step S22, by iteratively updating and excluding the positions of particles in the particle swarm randomly generated by obstacles, a planned heading with an optimal turning angle and altitude change is obtained, and the optimal planning point of the actual step length l along the heading is determined. The optimization function expression of the iterative update is:

[0012]

[0013] In the formula, (x next ,y next ,h next ) is the three-dimensional coordinate of the best planning point in the offline trajectory of the UAV, (x tar ,y tar ,h tar ) is the three-dimensional coordinate of the end point in the offline trajectory of the UAV track, and α is a positive integer.

[0014] S24. Based on step S23, when the distance between the optimal planning point and the end point is less than the actual step length l, the iterative update is stopped to obtain the optimal offline trajectory of the UAV.

[0015] S3, input the optimal offline track obtained in step S2 into the UAV for flight, and perform real-time online planning during the flight. The specific operation steps are as follows:

[0016] S31. If no no-fly zone is detected during the flight, the UAV flies according to the optimal offline track obtained in step S2; if a no-fly zone is detected during the flight, the improved RRT algorithm is used to perform online replanning to generate a new track, and step S32 is performed.

[0017] S32: According to the basic parameters of the no-fly zone in step S1, set the planned starting point and the planned end point of the new track, and obtain the minimum straight segment length in the new track. The expression is:

[0018]

[0019] Where, d se is the distance between the planning start point and the mission end point, γ is the expected number of waypoints, Is considered se The corrected value after size.

[0020] S33, based on step S32, introducing planning constraints in the process of selecting the next planning point of the planning starting point, the planning constraints are adaptively distance-adapted between the planning starting point and the planning end point, obtaining the next planning point and adjusting the planning according to the judgment conditions, thereby obtaining a new track adjusted in real time;

[0021] The planning constraints include the minimum straight segment length Maximum turning angle and maximum rate of climb The maximum turning angle and maximum rate of climb The expression is:

[0022]

[0023]

[0024] In the formula, is the minimum straight-line segment length, is the maximum turning angle, θ max is the turning angle, τ is used to adjust the increase of θ max The rate of is the maximum climb rate, ρ is the proportional factor;

[0025] The specific process of distance adaptation is as follows: setting the coordinates of the current position of the drone (x now ,y now ,h now ), the heading angle θ of the UAV now and the coordinates of the next planned point of the drone (x next ,y next ,h next );

[0026] The coordinate value of the next planned point of the drone is based on the coordinates of the current position of the drone and the minimum straight line segment length. Maximum turning angle and maximum rate of climb OK, the specific expression is:

[0027]

[0028]

[0029]

[0030] If the next planned point obtained does not meet the judgment condition of the planning end point, the planning continues; if the next planned point obtained meets the judgment condition of the planning end point, a new track adjusted in real time is obtained.

[0031] Preferably, in step S1, the obstacle includes the obstacle position coordinates and the size of the obstacle; the target point includes the target point three-dimensional coordinates, the target point two-dimensional coordinates and the approach range; the initial state of the UAV includes the turning performance of the UAV, the climbing performance of the UAV, the flight speed constraint of the UAV, the starting point coordinates of the UAV and the initial heading of the UAV.

[0032] Preferably, in step S2, the step size set in the multi-step particle swarm algorithm is 3 times the actual step size l.

[0033] Preferably, in step S2, the iteration basis of the optimal planning point includes the inertia coefficient ω, the optimal solution position of the planning point and the optimal position shared among all planning points.

[0034] Preferably, in step S2, the iterative optimization rule expression of the optimal planning point is:

[0035] v new =w×v0+c1×r1×(i best -p i )+c2×r2×(g best -p i )

[0036] p i+i =p i +v new

[0037] Where: v new is the speed after iterative optimization, p i+1 is the position after iterative optimization, p i is the coordinate parameter of a single planning point in the particle swarm, v0 is the original velocity of a single planning point in the particle swarm, ω is the inertia coefficient, i best is the individual optimal solution of a single planning point in the particle swarm, c1 is the learning factor from the individual; r1 and r2 are random numbers, g best The optimal solution for the group.

[0038] Preferably, in step S2, the segment direction of the offline trajectory is along the optimal planning point position p after particle swarm optimization. i+1The determined heading, the segment length of the offline trajectory is the actual step length l.

[0039] Preferably, in step S3, the planning starting point is the current position of the UAV, and the determination of the planning end point needs to meet the following two conditions at the same time: the distance between the planning end point and the no-fly zone is greater than the current minimum straight segment length i min The distance between the planning endpoint and the task endpoint is kept between 50% and 100% of the distance between the task endpoint and the current position; if the above two conditions are not met, the task endpoint is selected as the planning endpoint.

[0040] Compared with the prior art, the present invention has the following advantages:

[0041] 1. The multi-step particle swarm algorithm used for offline planning of UAV trajectory in the present invention reduces the multi-dimensional trajectory planning problem to a two-dimensional optimization problem, so that the initial trajectory of the UAV does not need to be planned in advance. At the same time, it is insensitive to the initial solution and can be generated randomly, saving the calculation time of the initial solution and avoiding the poor planning effect caused by the use of a bad initial solution. In addition, the multi-step particle swarm algorithm only uses two variable values ​​as optimization variables, so that higher accuracy and convergence speed can be achieved in the UAV trajectory planning.

[0042] 2. The improved RRT algorithm in the present invention has an adaptive characteristic. By dynamically adjusting the step size according to the distance between the starting point and the end point, the step size is reduced to improve the accuracy and quality of the path when approaching the target, thereby having better performance near the end point; the direction and size of the planning point for online planning of the UAV in offline trajectory planning can be adaptively changed according to the distance between the starting point and the end point of the offline trajectory, thereby improving the accuracy and efficiency of the online calculation.

[0043] 3. The present invention uses a multi-step particle swarm algorithm to perform offline planning before the UAV takes off to formulate an initial route. After takeoff, if it is found that the obstacle area has changed, an improved RRT algorithm is used for online planning, thereby solving the path planning problem of the UAV when encountering dynamic environmental changes during flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a flow chart of the UAV trajectory planning method based on the multi-step particle swarm algorithm and the improved RRT algorithm of the present invention;

[0045] Figure 2 It is a flow chart of a multi-step particle swarm algorithm in a UAV trajectory planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm of the present invention;

[0046] Figure 3 It is a constraint diagram of planning points in the UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm of the present invention;

[0047] Figure 4 It is a planning domain and an execution domain diagram in the UAV trajectory planning method based on the multi-step particle swarm algorithm and the improved RRT algorithm of the present invention;

[0048] Figure 5 These are three offline track diagrams in the UAV track planning method based on the multi-step particle swarm algorithm and the improved RRT algorithm of the present invention;

[0049] Figure 6 This is an online re-planning diagram of the offline trajectory in the UAV trajectory planning method based on the multi-step particle swarm algorithm and the improved RRT algorithm of the present invention. DETAILED DESCRIPTION

[0050] In order to fully describe the technical content, structural features, objectives and effects of the present invention, the following will be described in detail with reference to the accompanying drawings.

[0051] UAV trajectory planning method based on multi-step particle swarm optimization and improved RRT algorithm, such as Figure 1 As shown, the following steps are included:

[0052] S1. Determine the basic parameters of the UAV trajectory planning, where the flight boundaries under the flight constraints are [xmin, xmax], [ymin.ymax], [hmin, hmax], the flight obstacle position coordinates [xobs, yobs, hobs], the obstacle size lobs, the UAV's turning performance β, the UAV's climbing performance γ, the UAV's flight speed constraint [vmin, vmax], the UAV's starting point coordinates [xsta, ysta, hsta], the UAV's initial heading α0, the target point three-dimensional coordinates [xtar, ytar, htar], the target point two-dimensional coordinates [xtar, ytar] and the approach range lacc, the target point three-dimensional coordinates are to reach the predetermined target point, the target point two-dimensional coordinates are to reach a certain range of the predetermined target, and the no-fly zone information is input, including the three-dimensional coordinates and radius length of the center of the spherical no-fly zone or the two-dimensional coordinates and radius length of the center of the cylindrical no-fly zone.

[0053] S2, such as Figure 2 As shown in the figure, the step length l, the maximum height change Δh and the maximum turning angle Δφ between adjacent track points in the track are input into the multi-step particle swarm algorithm for iteration. The multi-step particle swarm algorithm is used to plan the optimal obstacle avoidance path and obtain the offline track of the UAV track. The specific operation steps are as follows:

[0054] S21. Under the flight constraints of the UAV, the step length l is taken as a constant, the maximum turning angle Δφ and the maximum height change Δh are taken as optimization variables, and the position information p[φ, h] of the planned point in the offline trajectory is obtained.

[0055] In this step, the step length l is used as a constant, and the three-dimensional offline planning is simplified to a two-dimensional problem, which can significantly reduce the computational complexity and make the optimization process more efficient. Two-dimensional optimization problems are easier to implement in practical systems, especially in embedded systems or real-time control systems with limited resources. This can make the flight control algorithm of the drone more stable and reliable. By determining the step length l, discrete points can be generated on the offline trajectory and optimized between these points. The complexity of directly processing continuous turning angles and altitude changes is avoided. During the optimization process, different turning angles and altitude changes can be considered at these discrete points to obtain an optimized offline trajectory.

[0056] S22. The spatial area between the actual step length l and the predicted step length 3l is set as the prediction domain. When planning the waypoints in the terminal area, continuing to use the end points of the prediction domain to optimize the waypoints will have a negative effect on the optimization. Therefore, when the distance d between the current waypoint and the target point is less than 3l, the prediction domain should be cancelled and the spatial area between the current planning point and the actual step length l should be set as the execution domain.

[0057] S23, such as Figure 3 and Figure 4 As shown, in the prediction domain and execution domain obtained in step S22, the positions of particles in the randomly generated particle swarm are iteratively updated to obtain a planned heading with an optimal turning angle and height change. If there is an obstacle in the area between the prediction domain and the execution domain under this heading, it is necessary to exclude this heading and determine the optimal planning point of the actual step length l along this heading. The iteratively updated optimization function expression is:

[0058]

[0059] In the formula, (x next ,y next ,h nex ) is the three-dimensional coordinate of the best planning point in the offline trajectory of the UAV, (x tar ,y tar ,h tar ) is the three-dimensional coordinate of the end point in the offline trajectory of the UAV track, and α is a positive integer.

[0060] Specifically, the multi-step particle swarm algorithm is not very sensitive to the initial solution, so the randomly generated initial state is used as the initial solution of the particle swarm algorithm. The randomly generated particle speed and particle state parameters are used as the initial particle state, the fitness of the current particle is calculated, and the optimal fitness individual in the particle swarm is selected. By selecting the optimal fitness individual and state parameters of the particle swarm, the state parameters of the optimal fitness of each particle are recorded. The particle speed and state parameters are updated, and it is iterated repeatedly until the iteration threshold is reached.

[0061] S24, on the basis of step S23, several iterations are performed, and the next best planning point and heading are generated after several generations. Then, the best planning point and heading are used as the starting state, and the multi-step particle swarm algorithm iteration of the next cycle is performed until the position of the newly generated best planning point is less than an actual step length l from the end point position, and the multi-step particle swarm algorithm is terminated. The end point is used as the final waypoint, and each waypoint is connected to generate an offline planning trajectory.

[0062] Specifically, the step size set in the multi-step particle swarm algorithm is 3 times the actual step size, and the segment direction of the offline trajectory is along the optimal planning point position p after particle swarm optimization. i+1 The determined heading, the segment length of the offline trajectory is the actual step length l. The iteration basis of the optimal planning point includes the inertia coefficient ω, which represents the influence of the original velocity of the particle, the optimal solution position of the planning point, the optimal solution position of the planning point represents the part of the particle action learning its own experience and the optimal position shared between all planning points, and the optimal position represents the part of the experience shared between particles. The iterative optimization rule expression of the optimal planning point is:

[0063] v new =w×v0+c1×r1×(i best -p i )+c2×r2×(g best -p i )

[0064] p i+1 =p i +v new

[0065] Where: v new is the speed after iterative optimization, p i+1 is the position after iterative optimization, p i is the coordinate parameter of a single planning point in the particle swarm, v0 is the original velocity of a single planning point in the particle swarm, ω is the inertia coefficient, i best is the individual optimal solution of a single planning point in the particle swarm, c1 is the learning factor from the individual; r1 and r2 are random numbers between 0 and 1, the purpose is to increase the scalability of the algorithm, gbest is the group optimal solution, v0, i best 、p i and g best All are in two-dimensional vector form.

[0066] S3, input the optimal offline track obtained in step S2 into the UAV for flight, and perform real-time online planning during the flight. The specific operation steps are as follows:

[0067] S31. If no no-fly zone is detected during the flight, the UAV continues to fly according to the offline track and the planning ends; if a no-fly zone is detected during the flight, the improved RRT algorithm is used to perform online re-planning to generate a new track, and step S32 is performed.

[0068] S32, set the planned starting point and planned end point of the new track, and obtain the minimum straight line segment length in the new track The expression is:

[0069]

[0070] Where, d se is the distance between the planning start point and the mission end point, γ is the expected number of waypoints, Is considered se The corrected value after size.

[0071] Furthermore, in order to dynamically adjust the step size and reduce the planning time, the current position of the drone is used as the planning starting point. The determination of the planning end point needs to meet the following conditions: the distance between the planning end point and the no-fly zone is greater than the current minimum straight segment length The distance between the planning endpoint and the task endpoint is kept between 50% and 100% of the distance between the task endpoint and the current position; if the above two conditions are not met, the task endpoint is selected as the planning endpoint.

[0072] S33: Based on step S32, the maximum turning angle is introduced in the process of selecting the next planning point of the planning starting point. and maximum rate of climb Two parameter values, the two parameter values ​​are adaptively adjusted in distance between the planning start point and the planning end point, so as to obtain the next planning point, and adjust the planning according to the judgment conditions, so as to obtain a new track adjusted in real time;

[0073] The planning constraints include the minimum straight segment length Maximum turning angle and maximum rate of climb The maximum turning angle and maximum rate of climb The expression is:

[0074]

[0075]

[0076] In the formula, is the minimum straight-line segment length, is the maximum turning angle, θ max is the turning angle, τ is used to adjust the increase of θ max The rate of is the maximum climb rate, ρ is the proportional factor;

[0077] The specific process of distance adaptation is as follows: setting the coordinates of the current position of the drone (x now ,y now ,h now ), the heading angle θ of the UAV now and the coordinates of the next planned point of the drone (x next ,y next ,h next );

[0078] The coordinate value of the next planned point of the drone is based on the coordinates of the current position of the drone and the minimum straight line segment length. Maximum turning angle and maximum rate of climb OK, the specific expression is:

[0079]

[0080]

[0081]

[0082] If the next planned point obtained does not meet the judgment condition of the planning end point, the planning continues; if the next planned point obtained meets the judgment condition of the planning end point, a new track adjusted in real time is obtained.

[0083] In a preferred embodiment of the present invention, the improved RRT algorithm introduces probability in the process of selecting waypoints for extension, and the waypoint closest to the end point has a greater probability of being selected for extension. Maximum turning angle Maximum climb rate The values ​​of the three parameters are adaptive according to the distance between the planning starting point and the mission end point, which can improve the search efficiency of the RRT algorithm and further release the turning and climbing capabilities of the drone.

[0084] The following is a further description of a UAV trajectory planning method based on a multi-step particle swarm algorithm and an improved RRT algorithm in combination with an embodiment of the present invention:

[0085] S1. Determine the basic parameters of the UAV trajectory planning, where the starting coordinates of the UAV are [0,0,0], the target coordinates of the UAV are [40000,4000,40000], and the flight boundaries of the UAV are [[0,40000],[0,4000],[0,40000]].

[0086] In this embodiment, the drone knows the coordinates of the center positions of three spherical flight obstacles before takeoff, which are [5000, 0, 20000], [20000, 0, 20000], and [20000, 0, 5000], and the radius of the three spherical flight obstacles is 5000. After takeoff, the drone can detect that the coordinates of the center position of a spherical flight obstacle are [30000, 0, 30000], and the radius of the spherical flight obstacle is 10000.

[0087] In this embodiment, since the multi-step particle swarm algorithm and the improved RRT algorithm are both the logic of iterating the next planning point from the current planning point, the flight performance parameters of the drone itself, such as the turning performance, climbing performance and flight speed constraints of the drone, are not involved.

[0088] S2, such as Figure 2 As shown, the step length l between adjacent track points in the track, the maximum height change Δh and the maximum turning angle Δφ are input into the multi-step particle swarm algorithm for iteration, and the multi-step particle swarm algorithm is used to plan the optimal obstacle avoidance path to obtain the offline track of the UAV track. In this embodiment, the step length set in the multi-step particle swarm algorithm is 3 times the actual step length l. The specific operation steps are:

[0089] S21, when the current position P and current heading of the drone are determined, they are used as references, such as Figure 3 Any point within the surface formed by the maximum turning angle Δφ, the maximum height change Δh and the step length l of the UAV satisfies the flight constraints of the UAV. Therefore, when the step length l is determined, the turning angle φ and the height change h are used as optimization variables, and the three-dimensional problem of optimizing the spatial position can be simplified to a two-dimensional optimization problem of optimizing the heading.

[0090] S22, such as Figure 4 As shown in the figure, the spatial area between the actual step length l and the predicted step length 3l is set as the prediction domain. When planning the waypoints in the terminal area, continuing to use the end points of the prediction domain to optimize the waypoints will have a negative effect on the optimization. Therefore, when the distance d between the current waypoint and the target point is less than 3l, the prediction domain should be cancelled and the spatial area between the current planning point and the actual step length l should be set as the execution domain.

[0091] S23, such as Figure 3 As shown, in the prediction domain and execution domain obtained in step S22, the positions of particles in the randomly generated particle swarm are iteratively updated to obtain a planned heading with an optimal turning angle and height change. If there is an obstacle in the area between the prediction domain and the execution domain under this heading, it is necessary to exclude this heading and determine the optimal planning point of the actual step length l along this heading. The iteratively updated optimization function expression is:

[0092]

[0093] In the formula, (x next ,y next ,h nex ) is the three-dimensional coordinate of the optimal planning point, (x tar ,y tar ,h tar ) is the three-dimensional coordinate of the end point in the offline trajectory, and α is a positive integer.

[0094] S24, on the basis of step S23, several iterations are performed, and the next best planning point and heading are generated after several generations. Then, the best planning point and heading are used as the starting state to perform the multi-step particle swarm algorithm iteration of the next cycle. Since continuing to use the prediction domain end point to optimize the planning point at the planning point in the planning terminal area will bring adverse effects to the optimization, the position of the newly generated best planning point is less than an actual step length l from the terminal position, and the multi-step particle swarm algorithm is terminated. The terminal is used as the final waypoint, and each waypoint is connected to generate an offline planning trajectory, such as Figure 5 shown.

[0095] S3. Input the optimal offline track obtained in step S2 into the drone for flight. After the drone is launched, it will fly along the offline planned track. Although offline planning has a certain timeliness, it takes some time for the drone to fly, and it cannot be guaranteed that the flight environment will not change at all. During this period, the possibility of new no-fly zones cannot be completely ruled out. New no-fly zones may be air defense zones or early warning radars that have not been discovered before, or reconnaissance areas after changes in enemy aircraft, etc., which will have an impact on the successful execution of the mission. After the drone is launched, it is necessary to detect the no-fly zones within a certain range ahead to ensure timely discovery and route replanning. The specific steps are as follows:

[0096] S31. If no no-fly zone is detected during the flight, the UAV continues to fly according to the offline track and the planning ends; if a no-fly zone is detected during the flight, the improved RRT algorithm is used to perform online re-planning to generate a new track, and step S32 is performed.

[0097] S32, set the planned starting point and planned end point of the new track. The new track must satisfy the turning and climbing constraints from the planned starting point to the end point of the call, and the track before and after the planned starting point and the planned end point, and obtain the minimum straight segment length in the new track.

[0098] S33: Based on step S32, the maximum turning angle is introduced in the process of selecting the next planning point of the planning starting point. and maximum rate of climb Two parameter values ​​are used to adapt the distance between the planning start point and the planning end point, so as to obtain the next planning point. If the next planning point obtained meets the judgment condition of the planning end point, a new track is obtained. If the next planning point obtained does not meet the judgment condition of the planning end point, the planning continues until a new track is obtained. The use of adaptive parameters can improve the search efficiency of the RRT algorithm and further release the turning and climbing capabilities of the drone. The re-planning result of one offline route is shown in the figure below. Figure 6 shown.

[0099] In summary, the present invention integrates two traditional algorithms, gives full play to their respective advantages, and the path planning runs through the entire flight phase of the drone. The present invention cleverly integrates the multi-step particle swarm algorithm and the improved RRT algorithm and gives full play to their advantages, runs through the entire flight phase of the drone, and plans the entire flight process track of the drone; online planning is performed in offline track planning, and the extended step size is adaptively adjusted according to the distance between the starting point and the end point of offline planning. By using a larger step size in a farther target area, the process of finding the target area is accelerated, thereby reducing the planning time algorithm, which has good timeliness.

[0100] The embodiments described above are only descriptions of the preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm, characterized in that: It includes the following steps: S1. Determine the basic parameters of the UAV trajectory planning, which include obstacles, target points, flight boundaries under flight constraints, and basic parameters of the initial state of the UAV; S2, input the step length l, the maximum height change Δh and the maximum turning angle Δφ between adjacent track points in the track into the multi-step particle swarm algorithm for iteration, and use the multi-step particle swarm algorithm to plan the best obstacle avoidance path to obtain the offline track of the UAV track. The specific operation steps are as follows: S21. Under the flight constraints of the UAV, the step length l is taken as a constant, the maximum turning angle Δφ and the maximum height change Δh are taken as optimization variables, and the position information p[φ, h] of the planning point in the offline trajectory is obtained; S22, set the spatial area between the actual step length l and the predicted step length 3l as the prediction domain, and set the spatial area between the current planning point and the actual step length l as the execution domain; S23, in the prediction domain and execution domain obtained in step S22, by iteratively updating and excluding the positions of particles in the particle swarm randomly generated by obstacles, a planned heading with an optimal turning angle and altitude change is obtained, and the optimal planning point of the actual step length l along the heading is determined. The optimization function expression of the iterative update is: In the formula, (x next ,y next ,h next ) is the three-dimensional coordinate of the best planning point in the offline trajectory of the UAV, (x tar ,y tar ,h tar ) is the three-dimensional coordinate of the end point in the offline trajectory of the UAV track, and α is a positive integer; S24, based on step S23, when the distance between the optimal planning point and the end point is less than the actual step length l, the iterative update is stopped to obtain the optimal offline track of the UAV; S3, input the optimal offline track obtained in step S2 into the UAV for flight, and perform real-time online planning during the flight. The specific operation steps are as follows: S31. If no no-fly zone is detected during the flight, the UAV flies according to the optimal offline track obtained in step S2; if a no-fly zone is detected during the flight, the improved RRT algorithm is used to perform online replanning to generate a new track, and step S32 is performed; S32: According to the basic parameters of the no-fly zone in step S1, set the planned starting point and the planned end point of the new track, and obtain the minimum straight segment length in the new track. The expression is: Where, d se is the distance between the planning start point and the mission end point, γ is the expected number of waypoints, Is considered se Correction value after size; S33, based on step S32, introducing planning constraints in the process of selecting the next planning point of the planning starting point, the planning constraints are adaptively distance-adapted between the planning starting point and the planning end point, obtaining the next planning point and adjusting the planning according to the judgment conditions, thereby obtaining a new track adjusted in real time; The planning constraints include the minimum straight segment length Maximum turning angle and maximum rate of climb The maximum turning angle and maximum rate of climb The expression is: In the formula, is the minimum straight-line segment length, is the maximum turning angle, θ max is the turning angle, τ is used to adjust the increase of θ max The rate of is the maximum climb rate, ρ is the proportional factor; The specific process of distance adaptation is as follows: setting the coordinates of the current position of the drone (x now ,y now ,h now ), the heading angle θ of the UAV now and the coordinates of the next planned point of the drone (x next ,y next ,h next ); The coordinate value of the next planned point of the drone is based on the coordinates of the current position of the drone and the minimum straight line segment length. Maximum turning angle and maximum rate of climb OK, the specific expression is: If the next planned point obtained does not meet the judgment condition of the planning end point, the planning continues; if the next planned point obtained meets the judgment condition of the planning end point, a new track adjusted in real time is obtained.

2. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: In step S1, the obstacle includes the obstacle position coordinates and the obstacle size; The target point includes the three-dimensional coordinates of the target point, the two-dimensional coordinates of the target point and the approach range; the initial state of the drone includes the turning performance of the drone, the climbing performance of the drone, the flight speed constraint of the drone, the starting point coordinates of the drone and the initial heading of the drone.

3. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: In step S2, the predicted step length 3l in the multi-step particle swarm algorithm is three times the actual step length l.

4. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: In step S2, the iteration basis of the optimal planning point includes the inertia coefficient ω, the optimal solution position of the planning point and the optimal position shared among all planning points.

5. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 or 4, characterized in that: In step S2, the iterative optimization rule expression of the best planning point is: v new =w×v0+c1×r1×(i best -p i )+c2×r2×(g best -p i ) p i+1 =p i +v new Where: v new is the speed after iterative optimization, p i+1 is the position after iterative optimization, p i is the coordinate parameter of a single planning point in the particle swarm, v0 is the original velocity of a single planning point in the particle swarm, ω is the inertia coefficient, i best is the individual optimal solution of a single planning point in the particle swarm, c1 is the learning factor from the individual; r1 and r2 are random numbers, g best The optimal solution for the group.

6. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: In step S2, the segment direction of the offline trajectory is along the optimal planning point position p after particle swarm optimization. i+1 The determined heading, the segment length of the offline trajectory is the actual step length l.

7. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: The three-dimensional coordinate value of the optimal planning point is obtained from the three-dimensional coordinate of the current planning point. The specific expression is: l 2 =(x next -x now ) 2 +(y next -y now ) 2 +(h next -h now ) 2 h next =Δh In the formula, (x now ,y now ,h now ) is the three-dimensional coordinate of the current planning point.

8. The UAV trajectory planning method based on multi-step particle swarm algorithm and improved RRT algorithm according to claim 1 is characterized in that: In step S3, the planning starting point is the current position of the drone, and the determination of the planning end point needs to meet the following two conditions at the same time: the distance between the planning end point and the no-fly zone is greater than the current minimum straight segment length 1.5 times of the planned end point, and the distance between the planned end point and the task end point is kept between 50% and 100% of the distance between the task end point and the current position; if the above two conditions are not met at the same time, the task end point is selected as the planned end point.

Images