Unmanned aerial vehicle route planning method and device for effectively tracking preset route

Abstract

The invention discloses an unmanned aerial vehicle route planning method for effectively tracking a preset route, and mainly solves the problem that an existing unmanned aerial vehicle route planning method is inaccurate in route planning in a wind field environment. According to the implementation scheme, the method comprises the steps that the air speed and the ground speed of an unmanned aerial vehicle at each preset flight path point are calculated according to a preset flight path, and stall and reverse flight conditions of the unmanned aerial vehicle are judged; constructing a multi-target optimization model according to the stall, the inverted flight path and the airspeed constraint of the unmanned aerial vehicle; iteratively solving the optimal solution of the next step position of the unmanned aerial vehicle by using a multi-objective optimization model, forming a planned track of a stall track section and an inverted flight track section by using all the solved next step positions of the unmanned aerial vehicle, and smoothing the planned track; iteratively calculating the next position of the unmanned aerial vehicle according to the non-stall and reverse flight track segment, the current position of the unmanned aerial vehicle, the wind field data and the minimum turning radius of the unmanned aerial vehicle, and sequentially connecting all calculation results to form a planned track of the non-stall and reverse flight track segment. The method is applicable to unmanned aerial vehicle reconnaissance

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle (UAV) trajectory planning, and further relates to a UAV trajectory planning method and device, which can be used for UAV reconnaissance. Background Art

[0002] In recent years, with the development of UAV aerospace technology, UAVs have been widely used in military and civilian fields. With its many advantages such as small size, light weight, low cost, and high maneuverability, UAVs are widely used in target reconnaissance, communication relay, and military strikes. Among them, reconnaissance is one of the important tasks of UAVs. When UAVs actually perform reconnaissance missions, they usually need to plan offline tracks first, and then scan the mission area by tracking the offline tracks. Track tracking is one of the important links for UAVs to complete their tasks. In UAV track tracking, wind is the biggest factor that causes UAVs to deviate from their routes. How to consider the impact of wind fields on UAVs is also one of the important issues in the field of UAV track tracking. Therefore, it is crucial to use track planning methods to replan track segments that cannot be tracked due to wind field factors to achieve effective tracking of preset routes.

[0003] In his Aspatially wind aware quadcopter (UAV) path planning approach, Georgios A. Thanellas used the A* algorithm to solve the problem of UAV path planning from the starting point to the end point in a wind field environment, and proposed a cost function in the A* algorithm that is suitable for a wind field environment. In this function, the angle between the wind direction and the UAV heading and the length of the UAV from the end point are normalized and then multiplied by weights and added. It calculates all possible points of the next track point at the current track point, substitutes them into the cost function, and selects the point with the smallest cost function value as the next track point. Repeat the above process until the end point is reached, and iterate to obtain the final track. Since this method only plans point-to-point offline tracks, it does not consider the preset track tracking problem, and the consideration of the wind field is limited to minimizing the angle between the wind direction and the UAV heading. Therefore, when tracking the offline track in an actual wind field environment, the original track may be offset and the UAV may stall.

[0004] The patent document with application number CN201710392618.0 discloses a method and device for UAV to visit multiple target points. It first obtains the location information of the UAV and multiple target points and the motion parameters of the UAV and the wind field. Then, based on the acquired location information, combined with the ground speed heading angle discreteness and the preset genetic algorithm, an initial population is constructed; based on the initial population and the acquired UAV parameters and wind field parameters, the flight state of the UAV and the flight time of the UAV to complete the track segment are determined, and the flight time corresponding to the chromosome in the initial population is obtained according to the flight time of the track segment and the SUAVVS-DVRP model. Finally, based on the genetic algorithm, each Dubins flight path in the initial population is crossover and mutation processed, and after reaching the predetermined number of iterations, the Dubins flight path with the shortest flight time is selected as the optimal flight path. Since this method plans the trajectory for visiting multiple target points, it only considers how to track to the next target point during the planning process, but does not fully consider the impact of the wind field on the UAV. It only considers the impact of the wind field on the UAV speed, and does not consider the possible deviation of the UAV track caused by the wind field. Therefore, when tracking the offline track in an actual wind field environment, the original track may be deviated, and it is difficult to plan a safe and feasible path when there is an area with excessive wind speed in the wind field environment.

[0005] In his journal article on robust control methods for stratospheric airship track tracking, Zhao Xinyun proposed a new robust control method for track tracking to solve the problem of easy deviation of UAV tracking tracks in wind fields. By introducing wind disturbance force and torque models, a stratospheric aircraft dynamics model with refined wind field disturbance effects was established. Simulation results show that this method further reduces the oscillation when approaching the desired track without changing the convergence speed of the tracking error when far away from the desired track. At the same time, it ensures that the system has strong robustness and high tracking accuracy under the influence of internal parameter perturbations and external wind disturbances, and takes into account the requirements of fast convergence speed when far away from the desired track under strong uncertainty and the requirements of suppressing oscillation when approaching the desired track under strong uncertainty to track the preset route. However, this method only considers the problem that the wind field may cause the UAV to deviate from the original track, but lacks the problem that the wind field may cause the UAV to stall and fly backwards. Therefore, when there is an area with excessive wind speed in the wind field environment, it is easy to stall and fly backwards when tracking the preset track. Summary of the invention

[0006] The purpose of the present invention is to address the deficiencies of the above-mentioned prior art and to propose a method and device for UAV trajectory planning in a wind field environment that can effectively track a preset route, so as to achieve effective tracking of the preset route in a wind field environment with the smallest possible track deviation and the shortest possible track length.

[0007] The technical solutions to achieve the purpose of the invention include:

[0008] 1. A method for effectively tracking a preset route in a wind environment UAV trajectory planning, characterized by comprising:

[0009] 1) Calculate the airspeed and ground speed of the drone at each track point on the preset track according to the wind field data at the track point and the flight speed of the drone, and use the track points that meet the stall and inverted flight conditions as the stall and inverted flight track points, and the track points that do not meet the stall and inverted flight conditions as the non-stall and inverted flight track points, and use the stall and inverted flight track points to form the stall and inverted flight track segments, and the non-stall and inverted flight track points to form the non-stall and inverted flight track segments;

[0010] 2) Establish a multi-objective optimization model based on the stall and inverted flight path segments and the UAV airspeed constraints;

[0011] 3) Substitute the current position of the UAV into the multi-objective optimization model, iteratively solve the optimal solution for the next position of the UAV until the UAV reaches the end of the stall and reverse flight segment, and use all the next positions of the UAV obtained to form the planned trajectory of the stall and reverse flight segment;

[0012] 4) Smoothing the stall and reverse flight segments in the planned trajectory obtained in (3) according to the minimum turning radius of the UAV, so that the turning part of the trajectory meets the turning maneuverability constraint of the UAV;

[0013] 5) Iteratively calculate the next position of the UAV according to the non-stall and inverted flight path, the current position of the UAV, the wind field data and the minimum turning radius of the UAV until the UAV reaches the end of the non-stall and inverted flight path, and use all the calculated next positions of the UAV to form the planned trajectory of the non-stall and inverted flight path;

[0014] 2. A wind field environment unmanned aerial vehicle track planning device that effectively tracks a preset route, characterized by comprising:

[0015] A preset track analysis module (1) is used to calculate the airspeed and ground speed of the UAV at each track point on the preset track, and determine whether the track point is a stall, inverted flight track point or a non-stall, inverted flight track point based on the calculation results;

[0016] The stall and inverted flight trajectory planning module (2) is used to construct a multi-objective optimization model according to the stall and inverted flight trajectory points, the UAV maneuverability parameters, and the wind field data, and obtain and smooth the UAV stall and inverted flight trajectory segment planning trajectory through the iterative solution module (4);

[0017] A non-stall and inverted flight trajectory planning module (3) is used to calculate the non-stall and inverted flight trajectory planning trajectory of the UAV using an iterative solution module (4) according to the non-stall and inverted flight trajectory points, the UAV maneuverability parameters, and the wind field data;

[0018] The iterative solution module (4) is used to iteratively solve the planned flight path of the UAV according to the stall and inverted flight path planning module (2) and the non-stall and inverted flight path planning module (3).

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

[0020] First, the present invention fully considers the impact of wind fields on UAVs, divides the preset track into stall and inverted flight track segments and non-stall and inverted flight track segments, and plans the wind field tracks for the two track segments respectively. Compared with the prior art that does not consider the possible stall and inverted flight track segments when planning the track, the present invention can avoid stall and inverted flight of the UAV and ensure that the planned track is safe and flyable.

[0021] Secondly, the present invention constructs a multi-objective optimization model based on a preset track, and uses iterative solution to plan the track of the UAV in the wind field, so that the UAV can pass through the stalled and inverted wind field areas. Compared with the existing technology that treats dangerous wind field areas as obstacles and bypasses them, the present invention shortens the track length and track deviation, and improves the UAV's reconnaissance rate of the mission area. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 A flow chart of the method for planning the trajectory of a UAV in a wind farm environment according to the present invention;

[0023] Figure 2 for Figure 1 The schematic diagram of smoothing the planned flight path of the stall and inverted flight path segment obtained in FIG.

[0024] Figure 3 for Figure 1 Schematic diagram of the xy coordinate system for iterative calculation of the planned trajectory of the non-stall and inverted flight segments;

[0025] Figure 4 for Figure 1 Schematic diagram of the actual nose direction of the UAV during the iterative calculation of the non-stall and inverted flight trajectory segment planning trajectory;

[0026] Figure 5 This is a schematic diagram of the structure of the UAV trajectory planning device in a wind field environment of the present invention;

[0027] Figure 6 A comparison diagram of the track shapes of the method of the present invention and the existing A* method;

[0028] Figure 7 This is a comparison diagram of the average deviation of the UAV track between the method of the present invention and the existing A* method;

[0029] Figure 8 This is a comparison chart of the UAV track length between the method of the present invention and the existing A* method. DETAILED DESCRIPTION

[0030] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme and effects in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, other embodiments obtained by ordinary technicians in this field without creative work should all fall within the scope of protection of the present invention.

[0031] Embodiment 1: A method for planning the flight path of a UAV in a wind environment that effectively tracks a preset route provided by the present invention

[0032] Reference Figure 1 The implementation steps of this example include the following:

[0033] Step 1: Calculate the airspeed and ground speed of the drone at each track point on the preset track, and determine its stall and inverted flight conditions.

[0034] The preset track is composed of the coordinates of n preset track points, and the track point set is P, and each preset track point is p i , the specific implementation of this step is as follows:

[0035] 1.1) Get p i The wind field vector v at the point w With drone speed vector v u , where v w The magnitude is the wind speed, and the direction is the wind direction; v u The magnitude is the flight speed of the UAV under given aircraft power and no wind conditions, and the direction is given by p i Track point points to p i+1 Track points;

[0036] 1.2) Set v w In v u The projection component in the direction is denoted as v p , calculate the drone in p i Airspeed at point v a and ground speed v at point g :

[0037] v a =v u -v p , v g =v u +v p ;

[0038] 1.3) Get the minimum airspeed v for the drone to fly normally without stalling m , p i |v at point a |with vm For comparison:

[0039] If |v a | less than v m , then p i Point is the stall track point,

[0040] Otherwise, p i Point is the non-stall track point;

[0041] 1.4) According to ground speed v g Determine whether the drone is in p i Flight direction at track point:

[0042] If the drone is flying forward, the ground speed vector v g is positive, then p i Point is a non-inverted flight track point;

[0043] If the drone is flying backwards, the ground speed vector v g If p is negative, i Point is the inverted flight track point.

[0044] Step 2: Establish a multi-objective optimization model.

[0045] For the track points of the preset stall and inverted flight path, the track of the stall and inverted flight path can be planned by establishing a multi-objective optimization model to ensure that the UAV will not stall or invert when flying along it, ensure the safe flight of the UAV, and realize the effective tracking of the preset track by the UAV. The specific implementation includes the following:

[0046] 2.1) Let the coordinates of the optimal position of the drone be (x next ,y next ), the end point coordinate is (x goal ,y goal ), preset distance point on the track (x next ,y next )The nearest track point is (x line ,y line ), calculate the distance d between the next optimal position and the end point goal and the deviation d between the optimal position and the preset track for the next step line :

[0047]

[0048] 2.2) With the goal of minimizing the track deviation and track length, the objective function of the multi-objective optimization model is constructed as follows:

[0049] minf=d goal +d line ;

[0050] 2.3) Get the coordinates of the current position of the drone (x now ,y now ), wind speed v wind , wind direction wind , UAV speed v u , the drone's nose is facing ω uav and the flight time step t. According to the UAV mass point kinematic equation, the next track point of the UAV is obtained by the UAV moving from the current track point at the ground speed for t seconds, and the ground speed is obtained by adding the UAV speed and the wind speed. Therefore, the kinematic constraints for the UAV to fly from the current position to the next position are set as follows:

[0051] x next =x now +t(v wind cos(ω wind )+v u cos(ω uav ))

[0052] y next =y now +t(v wind sin(ω wind )+v u sin(ω uav ))

[0053] Among them, x next The x coordinate and y coordinate of the next position of the drone next is the y coordinate of the next position of the drone, x now is the x coordinate of the current position of the drone, y now is the y coordinate of the current position of the drone;

[0054] 2.4) Get the minimum airspeed v of the drone m , according to the drone parameters, set the constraints for normal flight without stalling as follows:

[0055] v u -v wind [cos(ω wind )cos(ω uav )+sin(ω wind )sin(ω uav )]≥v m ;

[0056] 2.5) Based on the above objective functions and constraints, the multi-objective optimization model is constructed as follows:

[0057]

[0058] Step 3, use the multi-objective optimization model to iteratively solve the stall and inverted flight trajectory.

[0059] 3.1) Obtain the coordinates of the current position of the drone, the wind speed and direction at the current position;

[0060] 3.2) Substitute the acquired data into the multi-objective optimization model to obtain the optimal solution for the next position coordinates of the drone;

[0061] 3.3) Update the current position coordinates of the drone to the next position coordinates of the drone obtained by solving the problem;

[0062] 3.4) Repeat steps 3.1)-3.3) until the drone reaches the end of the stall and inverted flight path;

[0063] 3.5) The optimal solutions of the next position coordinates of the UAV in all iterations are linked in sequence to form the planned trajectory of the stall and reverse flight segments.

[0064] Step 4: Smooth the track.

[0065] In order to avoid the unsolvable model caused by the inability of the UAV's maneuverability to track the sudden change in the wind field, the UAV's maneuverability constraints are not considered when constructing the multi-objective optimization model in step 2. Therefore, the planned track obtained in step 3 will have a turning angle that exceeds the UAV's turning maneuverability limit. Therefore, it is necessary to eliminate the turning angle that exceeds the UAV's turning maneuverability limit through track smoothing to ensure the flyability of the track. Existing track smoothing technologies include Chaikin method, arc substitution method, cubic spline interpolation method, Bezier curve method, etc. This example uses but is not limited to Chaikin method and arc substitution method to smooth the track, where:

[0066] The smoothing principle of the Chaikin method is as follows Figure 2 As shown in (a), it is smoothed by replacing a track point with the midpoint of the two track points before and after it.

[0067] The smoothing principle of the arc replacement method is as follows Figure 2 As shown in (b), the track is smoothed by replacing the corner track where the track point is located with a circular arc segment.

[0068] The specific implementation of this step includes the following:

[0069] 4.1) Let R min is the minimum turning radius of the drone, v u is the speed of the UAV, and the maximum value of the heading angle difference between two adjacent track points is calculated as:

[0070]

[0071] 4.2) Calculate the heading angle difference τ between each track point and the next track point in the planned track, and compare it with the maximum value ω:

[0072] If τ is greater than ω, the track point is replaced by the midpoint of the two track points before and after it.

[0073] Otherwise, keep the track point;

[0074] 4.3) Assume the number of smoothing times is λ, repeat step 4.2) λ times to reduce the heading angle difference between adjacent track points and obtain the planned track after preliminary smoothing;

[0075] 4.4) For the planned track after preliminary smoothing, recalculate the heading angle difference τ′ between each track point and the next track point in the track and compare it with the maximum value ω:

[0076] If τ′ is greater than ω, the arc substitution method is used, with a radius equal to R min The arc segment on the circle of replaces the angle segment track where the track point is located. After replacing all the planned track points where τ′ is greater than ω with arcs, a flyable track that meets the UAV turning maneuverability constraint is obtained;

[0077] Otherwise, the track segment where the track point is located is retained.

[0078] Step 5, iteratively calculate the next position of the UAV based on the non-stall, inverted flight track segment, the current position of the UAV, wind field data, and the minimum turning radius of the UAV.

[0079] Because when the drone is at position o, it will move to the next preset track point p i Moving forward, so ideally the drone ground speed v g The direction should be from o to p i However, due to the existence of the wind field, the ground speed vector v g The wind field vector v w With drone speed vector v u This may cause the ground speed direction of the UAV to shift. In order to solve the ground speed vector v′ of the UAV in actual conditions, g , according to the ideal v g , establish the xy coordinate system as Figure 3 As shown. According to the relationship between the three velocity vectors, calculate v in the ideal case u According to the turning ability constraint of the UAV, the actual speed v of the UAV is obtained. u ′, according to v u 'Calculate v' g , according to v′ g Get the next position of the drone. The specific implementation steps include the following:

[0080] 5.1) Let the coordinates of position o be (x now ,y now), the next preset track point p i The coordinates of (x track ,y track ), calculate v g Angle α with the x-axis:

[0081]

[0082] 5.2) According to α and the angle β between the wind field vector and the x-axis, calculate the angle θ between the wind field vector and the ground speed vector:

[0083] θ = β - α;

[0084] 5.3) v g 、v u 、v w The triangle formed by the three velocity vectors is solved according to the sine formula to obtain the ideal UAV velocity vector v u The angle χ with the x-axis:

[0085]

[0086] 5.4) Let the actual velocity vector of the current position of the drone be v u ′, based on the ideal drone velocity vector v at the current position u And the actual velocity vector v at the previous position of the drone u ″Get v u The angle χ′ between ′ and the x axis:

[0087] According to v u ″The angle χ″ with the x-axis, calculate the difference φ between χ and χ″;

[0088] φ and For comparison, Figure 4 As shown:

[0089] If φ is less than or equal to Then χ′ is equal to χ,

[0090] Otherwise, χ′ is equal to χ″ as the center. The degree value closest to χ within the degree range;

[0091] 5.5) According to v u 'Calculate the next position coordinates of the drone in actual situation:

[0092] x next =x now +|v w |cos(β)+|v u ′|cos(χ′)

[0093] y next =ynow +|v w |sin(β)+|v u ′|sin(χ′)

[0094] Among them, x next The x coordinate and y coordinate of the next position of the drone next is the y coordinate of the next position of the drone, x now is the x coordinate of the current position of the drone, y now is the y coordinate of the current position of the drone;

[0095] 5.6) Update the current position coordinates of the drone to the next position coordinates of the drone, and obtain the nearest preset track point (x track ,y track );

[0096] 5.7) Repeat steps 5.1)-5.6) until the drone reaches the end of the non-stall, inverted flight path segment;

[0097] 5.8) Link the next position coordinates of the UAV in all iterations in sequence to form the planned trajectory of the non-stall and inverted flight segment.

[0098] Embodiment 2: The present invention provides a device for planning a trajectory of an unmanned aerial vehicle in a wind environment that effectively tracks a preset route.

[0099] Reference Figure 5, this example includes: a preset track analysis module 1, a stall and inverted flight track planning module 2, a non-stall and inverted flight track planning module 3, and an iterative solution module 4. The preset track analysis module 1 is used to calculate the airspeed and ground speed of the drone at each track point on the preset track, and judge whether the track point is a stall and inverted flight track point or a non-stall and inverted flight track point according to the calculation result, and transmit all stall and inverted flight track points to the stall and inverted flight track planning module 2, and transmit all non-stall and inverted flight track points to the non-stall and inverted flight track planning module 3; the stall and inverted flight track planning module 2 constructs a multi-objective optimization model based on the stall and inverted flight track points, drone maneuverability parameters and wind field data, and transmits the model to the iterative solution module 4, waiting for it to return the drone stall. After the stall and inverted flight track segment is planned, the returned planned track is smoothed to obtain the stall and inverted flight track segment planned track; the non-stall and inverted flight track planning module 3 calculates the next position of the UAV according to the non-stall and inverted flight track points, the UAV maneuverability parameters and the wind field data, and sends the calculation result to the iterative solution module 4; the iterative solution module 4 iteratively solves the planned track according to the model sent by the stall and inverted flight track planning module 2, returns the track to the stall and inverted flight track planning module 2, and iteratively solves the planned track according to the calculation result sent by the non-stall and inverted flight track planning module 3.

[0100] The stall and inverted flight trajectory planning module 2 includes: a multi-objective optimization model building submodule 21 and a trajectory smoothing submodule 22. The multi-objective optimization model building submodule 21 is used to establish a multi-objective optimization model according to the stall and inverted flight trajectory segments and the UAV airspeed constraints; the trajectory smoothing submodule 22 is used to smooth the planned trajectory obtained in the iterative solution module 4 according to the UAV turning maneuverability parameters.

[0101] The non-stall, inverted flight trajectory planning module 3 includes: a drone ideal speed vector angle calculation submodule 31, a drone actual speed vector calculation submodule 32 and a drone next position calculation submodule 33. The drone ideal speed vector angle calculation submodule 31 calculates the angle between the drone ideal speed vector and the x-axis according to the drone current position, the nearest preset track point in front of the current position and the wind field data, and transmits it to the drone actual speed vector calculation submodule 32; the drone actual speed vector calculation submodule 32 calculates the actual speed vector of the drone current position according to the angle between the drone ideal speed vector and the x-axis and the actual speed vector at the previous position of the drone, and transmits it to the drone next position calculation submodule 33; the drone next position calculation submodule 33 calculates the drone next position coordinates according to the drone current position actual speed vector and the wind field data.

[0102] The iterative solution module 4 includes: a stall and inverted flight trajectory planning iterative solution submodule 41 and a non-stall and inverted flight trajectory planning iterative solution submodule 42. The stall and inverted flight trajectory planning iterative solution submodule 41 iteratively solves the next position of the UAV according to the model constructed by the multi-objective optimization model construction submodule 21, and uses all solutions of the iterative process to form the stall and inverted flight trajectory planning trajectory of the UAV; the non-stall and inverted flight trajectory planning iterative solution submodule 42 iteratively solves the next position of the UAV through each functional submodule in the non-stall and inverted flight trajectory planning module 3, and uses all solutions of the iterative solution to form the non-stall and inverted flight trajectory planning trajectory of the UAV.

[0103] The effect of the present invention is further described below in conjunction with simulation experiments:

[0104] 1. Simulation experiment conditions

[0105] The simulation experiment software platform of the present invention is: the experimental computer operating system is Window11 (64-bit), the memory is 16GB, the processor configuration is: Intel(R) Core(TM) i7-8750H CPU@2.20GHz2.20GHz, implemented in Python language, the version is Python3.11, and the experimental operating environment is JetBrains PyCharm Community Edition2022.2.

[0106] The wind field data set used in the simulation experiment of the present invention comes from NASA data.

[0107] 2. Simulation content and results analysis:

[0108] Simulation 1, using the present invention and the existing UAV trajectory planning method based on the A* algorithm to plan the same preset trajectory under the same wind field environment, and comparing the trajectory shapes of the two methods. The results are as follows: Figure 6 .

[0109] like Figure 6 It can be seen that compared with the track planned by the A* algorithm, the track planned by the present invention has less fluctuation, is smoother, and has a higher degree of consistency with the preset track.

[0110] Simulation 2, using the present invention and the existing UAV trajectory planning method based on the A* algorithm to plan the same preset trajectory under the same wind field environment, the trajectories planned by the two methods are evaluated and compared in terms of the average trajectory deviation. The results are as follows: Figure 7 shown.

[0111] like Figure 7 It can be seen that compared with the track planned by the A* algorithm, the track planned by the present invention has a lower average deviation and a better effect of tracking the preset track.

[0112] Simulation 3, using the present invention and the existing UAV trajectory planning method based on the A* algorithm to plan the same preset trajectory under the same wind field environment, the trajectories planned by the two methods are evaluated and compared in terms of the total trajectory length. The results are as follows: Figure 8 shown.

[0113] like Figure 8 It can be seen that compared with the track planned by the A* algorithm, the track planned by the present invention has a lower total length and shortens the flight distance by 300m.

[0114] The above simulation results show that the method for UAV trajectory planning in a wind field environment that effectively tracks a preset route proposed in the present invention can plan a track with smaller track deviation and shorter total track length in a wind field environment for an existing preset track, thereby achieving effective tracking of the preset route in a wind field environment.

[0115] It should be noted that the step numbers in the specification and claims of the present invention are only for the purpose of clearly describing the implementation scheme of the present invention and facilitating understanding, and the sequence of the step numbers is not limited.

Claims

1. A method for effectively tracking a preset route in a wind environment UAV trajectory planning, characterized in that: include: 1) Calculate the airspeed and ground speed of the drone at each track point on the preset track according to the wind field data at the track point and the flight speed of the drone, and use the track points that meet the stall and inverted flight conditions as the stall and inverted flight track points, and the track points that do not meet the stall and inverted flight conditions as the non-stall and inverted flight track points, and use the stall and inverted flight track points to form the stall and inverted flight track segments, and the non-stall and inverted flight track points to form the non-stall and inverted flight track segments; 2) Establish a multi-objective optimization model based on the stall and inverted flight path segments and the UAV airspeed constraints; 3) Substitute the current position of the UAV into the multi-objective optimization model, iteratively solve the optimal solution for the next position of the UAV until the UAV reaches the end of the stall and reverse flight segment, and use all the next positions of the UAV obtained to form the planned trajectory of the stall and reverse flight segment; 4) Smoothing the stall and reverse flight segments in the planned trajectory obtained in (3) according to the minimum turning radius of the UAV, so that the turning part of the trajectory meets the turning maneuverability constraint of the UAV; 5) The next position of the UAV is iteratively calculated according to the non-stall and inverted flight path, the current position of the UAV, the wind field data and the minimum turning radius of the UAV until the UAV reaches the end of the non-stall and inverted flight path. All the calculated next positions of the UAV are used to form the planned trajectory of the non-stall and inverted flight path.

2. The method according to claim 1, characterized in that In step 1), the airspeed and ground speed of the drone at each track point on the preset track are calculated based on the wind field data at the track point and the flight speed of the drone. The formula is as follows: v g =v u +v p v a =v u -v p Among them, v g is the ground speed vector of the UAV, v a is the drone airspeed vector, v u is the windless velocity vector of the drone, whose magnitude is the flight velocity of the drone under given aircraft power and windless conditions. The direction is given by p i Track point points to p i+1 Track point, v p is the wind field vector v w In p i Track point points to p i+1 The component of the track point direction.

3. The method according to claim 1, characterized in that In step 2), a multi-objective optimization model is established according to the stall, inverted flight path segments and the UAV airspeed constraints, and its implementation includes the following: 2a) Let the coordinates of the optimal position of the drone be (x next ,y next ), the end point coordinate is (x goal ,y goal ), preset distance point on the track (x next ,y next )The nearest track point is (x line ,y line ), calculate the distance d between the next optimal position and the end point goal and the deviation d between the optimal position and the preset track for the next step line : 2b) Based on the results of 2a), with the goal of minimizing the track deviation and track length, the objective function of the multi-objective optimization model is constructed as follows: minf=d goal +d line ; 2c) Let the coordinates of the current position of the drone be (x now ,y now ), the wind speed at the current track point is v wind , the wind direction at the current track point is ω wind , the current UAV head direction is ω uav , the flight time step is t, and the speed of the drone is v u According to the kinematic equation of the drone mass, the kinematic constraints for the drone to fly from the current position to the next position are set as follows: x next =x now +t(v wind cos(ω wind )+v u cos(ω uav )) and next =and now +t(v wind sin(ω wind )+v u sin(ω uav )) Among them, x next The x coordinate and y coordinate of the next position of the drone next is the y coordinate of the next position of the drone, x now is the x coordinate of the current position of the drone, y now is the y coordinate of the current position of the drone; 2d) Assume the minimum airspeed of the drone is v m , according to the drone parameters, set the constraints for normal flight without stalling as follows: v u -v wind [cos(ω wind )cos(ω uav )+sin(ω wind )sin(ω uav )]≥v m ; 2e) Based on the above objective function and constraints, a multi-objective optimization model is constructed as follows:

4. The method according to claim 1, characterized in that In step 3), the current position of the UAV is substituted into the multi-objective optimization model, and the optimal solution for the next position of the UAV is iteratively solved. The implementation includes the following: 3a) Obtain the coordinates of the current position of the drone, and the wind speed and direction at the current position; 3b) Based on the result of 3a), the acquired data is substituted into the multi-objective optimization model to obtain the optimal solution for the next position coordinates of the UAV; 3c) Based on the result of 3b), the current position coordinates of the UAV are updated to the next position coordinates of the UAV obtained by solving the solution; 3d) Repeat steps 3a)-3c) until the UAV reaches the end of the stall and inverted flight path segment.

5. The method according to claim 1, characterized in that In step 4), the stall and reverse flight segments in the planned trajectory obtained in step 3) are smoothed according to the minimum turning radius of the drone, and the implementation thereof includes the following steps: 4a) Let R min is the minimum turning radius of the drone, v u is the speed of the UAV, and the maximum value of the heading angle difference between two adjacent track points is calculated as: 4b) Calculate the heading angle difference τ between each track point and the next track point in the planned track, and compare it with the maximum value ω: if τ is greater than ω, replace the track point with the midpoint of the two track points before and after the track point, otherwise, keep the track point; 4c) Assume the number of smoothing times to be λ, and repeat the step of 4b) for λ times to reduce the heading angle difference between adjacent track points; 4d) According to the result of 4c), for the planned track after preliminary smoothing, recalculate the heading angle difference τ′ between each track point and the next track point in the track, and compare it with the maximum value ω: If τ′ is greater than ω, the arc substitution method is used, with a radius equal to R min The arc segment on the circle replaces the angle segment track where the track point is located; Otherwise, the track segment where the track point is located is retained.

6. The method according to claim 1, characterized in that In step 5), the next position of the drone is iteratively calculated based on the non-stall and inverted flight track segments, the current position of the drone, the wind field data, and the minimum turning radius of the drone. The implementation includes the following: 5a) Let the current position of the drone be (x now ,y now ), the nearest preset track point in front of the current position is (x track ,y track ), the ideal ground speed vector of the current position of the drone is v g , calculate v g Angle α with the x-axis: 5b) Based on the result of 5a), let the wind field vector at the current position of the drone be v w , v w The angle between the x-axis and the x-axis is β, calculate v g With v w The angle θ is: θ = β - α; 5c) Based on the results of 5a) and 5b), let the ideal velocity vector of the current position of the drone be v u , calculate v u The angle χ with the x-axis: 5d) Based on the results of 4a) and 5c), let the actual velocity vector of the current position of the drone be v u ′, calculate v u ′ and the x-axis, denoting χ′ at the previous position of the drone as χ″, calculating the difference φ between χ and χ″, and comparing φ with ω: If φ is less than or equal to ω, then χ′ is equal to χ, Otherwise, χ′ is equal to the degree value closest to χ within the ω degree range with χ″ as the center; 5e) Based on the result of 5d), calculate the next position coordinates of the drone in the actual situation: x next =x now +|v w |cos(β)+|v′ u |cos(x′) and next =and now +|v w |sin(β)+|v′ u |sin(χ′) Among them, x next The x coordinate and y coordinate of the next position of the drone next is the y coordinate of the next position of the drone, x now is the x coordinate of the current position of the drone, y now is the y coordinate of the current position of the drone; 5f) According to the result of 5e), the current position coordinates of the UAV are updated to the next position coordinates of the UAV, and the nearest preset track point (x track ,y track ); 5g) Repeat steps 5a)-5f) until the UAV reaches the end of the non-stall, inverted flight path segment.

7. A wind field environment unmanned aerial vehicle track planning device that effectively tracks a preset route, characterized in that ,include: A preset track analysis module (1) is used to calculate the airspeed and ground speed of the UAV at each track point on the preset track, and determine whether the track point is a stall, inverted flight track point or a non-stall, inverted flight track point based on the calculation results; The stall and inverted flight trajectory planning module (2) is used to construct a multi-objective optimization model according to the stall and inverted flight trajectory points, the UAV maneuverability parameters, and the wind field data, and obtain and smooth the UAV stall and inverted flight trajectory segment planning trajectory through the iterative solution module (4); A non-stall and inverted flight trajectory planning module (3) is used to calculate the non-stall and inverted flight trajectory planning trajectory of the UAV using an iterative solution module (4) according to the non-stall and inverted flight trajectory points, the UAV maneuverability parameters, and the wind field data; The iterative solution module (4) is used to iteratively solve the planned flight path of the UAV according to the stall and inverted flight path planning module (2) and the non-stall and inverted flight path planning module (3).

8. The device according to claim 7, characterized in that The stall and inverted flight trajectory planning module (2) comprises: A multi-objective optimization model building submodule (21) is used to build a multi-objective optimization model according to the stall and inverted flight trajectory segments and the airspeed constraints of the UAV; The track smoothing submodule (22) is used to smooth the planned track obtained in the iterative solution module (4) according to the UAV turning maneuverability parameter.

9. The device according to claim 7, characterized in that The non-stall, inverted flight trajectory planning module (3) comprises: The drone ideal velocity vector angle calculation submodule (31) is used to calculate the angle between the drone ideal velocity vector and the x-axis according to the drone's current position, the nearest preset track point in front of the current position, and wind field data; The drone actual speed vector calculation submodule (32) is used to calculate the actual speed vector of the drone at the current position according to the angle obtained by the drone ideal speed vector angle calculation submodule (31) and the actual speed vector at the previous position of the drone; The drone next position calculation submodule (33) is used to calculate the drone's next position coordinates based on the drone's current position actual velocity vector and wind field data obtained by the drone actual velocity vector calculation submodule (32).

10. The device according to any one of claims 7 to 9, characterized in that The iterative solution module (4) comprises: The stall and inverted flight trajectory planning iterative solution submodule (41) is used to iteratively solve the next position of the UAV according to the model constructed by the multi-objective optimization model construction submodule (21) in the stall and inverted flight trajectory planning module (2), and use all the solutions of the iterative process to construct the stall and inverted flight trajectory segment planning trajectory of the UAV; The non-stall, inverted flight trajectory planning iterative solution submodule (42) is used to iteratively solve the next position of the drone according to the drone ideal speed vector angle calculation submodule (31), the drone actual speed vector calculation submodule (32) and the drone next position calculation submodule (33) in the non-stall, inverted flight trajectory planning module (3), and all the iterative solutions constitute the drone non-stall, inverted flight trajectory planning trajectory.

Images