A fixed-wing path tracking method

Abstract

The present invention discloses a fixed-wing path tracking method. Set the desired speed and desired pose of the fixed-wing end point, and set the path planning function; obtain the pose of the fixed-wing at the current moment in real time, and update the set flight constraint conditions according to the actual desired flight path at the previous moment; process in the improved particle swarm waypoint prediction module to obtain a set of waypoints from the current pose of the fixed-wing to the desired travel path scheme with the shortest path and the least change in pose while avoiding adjacent obstacles; the B-spline route fitting module interpolates and fits the set of waypoints to obtain a predicted route, and then establishes an actual desired flight path according to the predicted route; return to the previous step and calculate the actual desired path again. The present invention efficiently conducts planning, sets B-spline curves, and updates the preset path in real time, greatly improving the obstacle avoidance effect and path tracking ability of the fixed-wing UAV, and reducing the pressure on the fixed-wing speed distribution and pose change, and better realizing the fixed-wing path tracking ability.

Description

Technical Field

[0001] The present invention belongs to a flight control method for a fixed-wing unmanned aerial vehicle in the technical field of unmanned aerial vehicles, and in particular to a method for path tracking of a fixed-wing unmanned aerial vehicle using a combination of an improved particle swarm optimization algorithm and a B-spline curve algorithm. Background Art

[0002] With the development of unmanned aerial vehicle technology, fixed wings are playing an increasingly important role in combat. When a fixed wing performs tasks such as detection, reconnaissance, and combat, a necessary method is to track the planned path in real time during the mission execution process.

[0003] The obstacle avoidance problem during the tracking process is an important link in the fixed-wing path tracking. During the fixed-wing path tracking process, the fixed wing generally faces problems such as obstacle avoidance in a small area that is difficult to handle during path planning, replanning the path, and efficient and stable tracking. Therefore, there is currently no relatively complete solution to the path tracking problem in the case of obstacle avoidance. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to quickly achieve obstacle avoidance and replanning the fixed-wing tracking path under wind speed disturbance and dynamic constraints within the known obstacle avoidance area, and perform dynamic allocation of pose and speed along the path to achieve efficient and stable tracking of the fixed-wing for the path.

[0005] To overcome the above problems, the present invention proposes a method for path tracking of a fixed-wing unmanned aerial vehicle using a combination of an improved particle swarm optimization algorithm and a B-spline curve algorithm.

[0006] To achieve the above technical objectives, the technical solution of the present invention is as follows:

[0007] Step S1: Given the end point and the desired flight path D of the fixed wing, set the desired speed and desired pose of the end point of the fixed wing, and set the path planning function to be optimized for the fixed-wing path in the form of a loss function;

[0008] The desired flight path D is a set composed of a series of position coordinate point sets [x_d, y_d, z_d]. The path planning function consists of three parts: a pose loss function, an obstacle avoidance loss function, and a trajectory tracking loss function, which are used to describe the variation relationship between relevant parameters such as position, speed, and attitude during the fixed-wing path tracking process. Solving the optimal solution for it can obtain the optimal path.

[0009] Step S2: Real-time obtain the pose P of the fixed wing at the current moment s , and update and set the flight constraint conditions of the fixed wing according to the actual desired flight path at the previous moment according to the pose including position and attitude;

[0010] Step S3: Set up an improved particle swarm waypoint prediction module. In the improved particle swarm waypoint prediction module, according to the current pose P of the fixed-wing aircraft s , the obstacle position information and its threat radius, the known expected flight path D, and the turning radius interval [R MIN , R MAX , where R MIN represents the minimum turning radius of the fixed-wing aircraft, and R MAX represents the maximum turning radius of the fixed-wing aircraft. Process to obtain a set of waypoints for the scheme with the least pose change and the shortest path while avoiding adjacent obstacles to the expected travel path D s .

[0011] Specifically, the above is to set the constraint conditions for the path planning function in step S1 according to the current pose P of the fixed-wing aircraft s , the obstacle position information and its threat radius, the known expected flight path D, and the turning radius interval [R MIN , R MAX , where R MIN represents the minimum turning radius of the fixed-wing aircraft, and R MAX represents the maximum turning radius of the fixed-wing aircraft, and solve through the improved particle swarm algorithm

[0012] The obstacle position information and its threat radius are expressed as [X OBS , Y OBS , Z OBS , R OBS , where X OBS , Y OBS , Z OBS respectively represent the three-dimensional coordinates of the obstacle position, and R OBS represents the threat radius of the obstacle

[0013] Each waypoint in the waypoint set is expressed as [X OPT , Y OPT , Z OPT , where X OPT , Y OPT , Z OPT respectively represent the three-dimensional coordinates of the waypoint

[0014] In the present invention, the fixed-wing aircraft avoids obstacles when the obstacle information is known, and there are no other unknown obstacles on the plane where the flight altitude is located

[0015] Step S4: Set up a B-spline route fitting module. Through the B-spline route fitting module, interpolate and fit the waypoint set to obtain a B-spline curve, so as to obtain a smooth predicted route. Extract the position at the next moment in the predicted route and establish an actual expected flight path D with the position at the current moment s, according to the actual expected flight path D s In step S5, set the expected attitude and speed of the fixed-wing aircraft;

[0016] In the said step S4, specifically, control the fixed-wing aircraft to fly forward from the current position to the position at the next moment in the planned flight path.

[0017] In the said step S4, evenly divide the moments in the planned flight path to determine the position at the next moment.

[0018] Step S5: In the improved particle swarm waypoint prediction module, change in real time according to the relationship between the current pose after reaching the next moment and the expected flight path D of the fixed-wing aircraft respectively, and the relationship between the position information of the obstacle and its threat radius, and return to step S2 again to calculate the actual expected path D s , thereby continuously iterating to control the flight of the fixed-wing aircraft at each moment.

[0019] Thus, keep updating the actual expected path of the fixed-wing aircraft, and optimize and reset the traveling state under its optimal path.

[0020] In the said step S2, update and set the flight constraint conditions of the fixed-wing aircraft according to the actual expected flight path of the previous moment, specifically:

[0021]

[0022]

[0023] Among them, L D represents the total length of the expected flight path D, L REF represents the straight-line distance from the current position to the starting position, v min represents the minimum speed to maintain the controllability under the aerodynamic conditions of the fixed-wing aircraft, w max represents the maximum yaw angular velocity under the mixing control of the rudder and aileron of the fixed-wing aircraft.

[0024] The turning radius of the fixed-wing aircraft should be within the turning radius interval [R MIN , R MAx .

[0025] In the improved particle swarm waypoint prediction module of the said step S3, according to the input current pose P of the fixed-wing aircraft s , obstacle position information and its threat radius [X OBS , Y OBS , Z OBS , R OBs , expected traveling path D of the fixed-wing aircraft, minimum attitude change of the fixed-wing aircraft, turning radius interval [R MIN , R MAXIn the case of [], the following objective function is established:

[0026] Pose loss function:

[0027]

[0028] Velocity loss function:

[0029]

[0030] Trajectory tracking loss function:

[0031] Cost_tra=((psi_d - psi_a) / Π) 2 +((pit_d - pit_a) / Π) 2

[0032] And the total loss function is set according to the following formula:

[0033] Func_tra=lam_1*Cost_pos + lam_2*Cost_speed + lam_3*Cost_tra

[0034] Where, Cost pos represents the pose loss function, Cost speed represents the formula velocity loss function, Cost tra represents the trajectory tracking loss function, Func tra represents the total loss function, that is, the path planning function to be optimized in step S1, Π represents pi, x_a, y_a, z_a are the three coordinate position information at the current moment, x_d, y_d, z_d are the coordinate position information of the expected flight path at the current moment, u_a, v_a, w_a are the velocity information in the three directions at the current moment, u_d, v_d, w_d are the velocity information in the three directions of the expected flight path, psi_a is the heading angle at the current moment, pit_a is the pitch angle at the current moment, psi_d is the heading angle under the expected flight path, pit_d is the pitch angle under the expected flight path, and lam1, lam2, lam3 are the weighting coefficients of the three loss functions.

[0035] Then, the improved particle swarm optimization algorithm is used to solve the objective function to obtain the optimal set of waypoints. In this way, the improved particle swarm optimization algorithm is used to quickly estimate a series of fixed-wing waypoints at the same interval in the next time period, and the optimal set of waypoints is selected as the expected set of waypoints.

[0036] Its process is as Figure 2As shown, first initialize the particle swarm parameters such as the number of particles in the swarm, the maximum number of iterations, the mixing probability, the size ratio of the mixing pool, the acceleration factor, and the inertia weight coefficient. Then calculate the fitness for each particle (the fitness is the value of the function at the position of the particle in the function to be calculated). The inertia coefficient w is increased in advance according to the increase in the number of iterations. An iteration means a process of looping once through steps S2 to S5 in the algorithm flow. Then update pBest and gBest according to the fitness value. After that, take the set of fitness values of each particle as the parent set, make a selection in the parent set, select two relatively high-quality ones, and then cross them between the two parents converted into single numbers to combine the final solution. Finally, update the velocity and position of the particle according to the update formula to complete one cycle, and then perform the next cycle according to the new particles until the upper limit of the number of iterations is reached and the optimal solution is output.

[0037] Selection means setting a random number with a uniform value in the range of [1, 2], and performing a dot product with the set of fitness values. The dot product means multiplying each fitness value by this random number, and then comparing the sizes with the original set of fitness values in turn until a larger value is found or the traversal is complete.

[0038] If a larger value is found, use it as one of the parents; if no larger value is found after the traversal is complete, use itself as one of the parents. And this process of finding a larger fitness value is the process of selecting high-quality particles in the previous paragraph.

[0039] Crossover means a process in which a part of the particles randomly intersects pairwise to generate the same number of offspring particles. And then the offspring particles can replace the parent particles, thus keeping the number of particles in the population unchanged. The position of the offspring particles is calculated by the arithmetic weighted sum of the positions of the two parent particles:

[0040]

[0041]

[0042] Among them, is the position vector of the target search space dimension. In this algorithm, since the considered direction is the loss function composed of four related functions, this vector is four-dimensional, and and k = 1, 2 respectively indicate whether it is the position of the offspring particle or the parent particle; is a uniformly distributed random vector in this dimension space, and its components in each dimension are randomly and uniformly taken in the range of [0, 1].

[0043] The improved particle swarm algorithm mentioned above sets the weight w of the inertia coefficient in the particle swarm algorithm to be calculated according to the following formula:

[0044] w = 0.4 + 0.006 * (100 - i)

[0045] Among them, i represents the number of iterations, and the total number of set iterations is 100 times.

[0046] In the improved particle swarm waypoint prediction module of the fixed wing, a dynamic adjustment inertia weight w module is added on the basis of the original particle swarm algorithm. The characteristics of this module are as follows: during the iteration process, a module that reduces the inertia weight coefficient w with the increase of the number of iterations is set. In this way, the function can make the particles jump out of the local minimum point due to having a larger inertia weight value in the early stage of iteration, and in the later stage of iteration, it can make the particles search more accurately for the optimal solution in the current area, and the convergence speed is also improved.

[0047] In the B-spline route fitting module of the step S4, the traveled waypoints, the waypoint set output in real time by the improved particle swarm waypoint prediction module, and the current heading angle are used as the preconditions for the interpolation fitting of the B-spline curve. Coupled with using the minimum yaw angular velocity under the lowest speed maneuverability as a constraint, a third-order B-spline function is selected for solution, and a transition-smooth predicted route is obtained through rapid processing.

[0048] In specific implementation, the following third-order B-spline function is set:

[0049]

[0050] Among them, N i，k (u) represents the function of the i-th position point at the k-th power. i represents the known i-th position point, k represents the power number, u represents the current position point to be obtained, and u i represents the position of the known i-th position point.

[0051] The present invention calculates in real time the B-spline curve in the predicted next time period according to the preset flight track, current pose, minimum turning radius, obstacle position and threat radius of the fixed-wing unmanned aerial vehicle, and selects the path position at the next moment as the optimal actual path; through the improved particle swarm algorithm, the predicted flight path planning problem is changed into finding the particle with the minimum cost by combining constraints, so as to find the flight path composed of the best waypoints; the predicted waypoints are fitted to make a B-spline curve as the actual path, and the target pose and flight speed of the fixed-wing unmanned aerial vehicle are set accordingly.

[0052] The present invention can perform planning efficiently, set the B-spline curve and update the preset path in real time, which can greatly improve the obstacle avoidance effect and path tracking ability of the fixed-wing unmanned aerial vehicle, and relieve the pressure of fixed-wing speed distribution and pose change, and better realize the fixed-wing path tracking ability.

[0053] The benefits brought by the technical solution provided by the present invention are:

[0054] The present invention estimates the new waypoint after obstacle avoidance by improving the particle swarm algorithm, and estimates it again when it reaches the next moment, so that the calculated waypoint can effectively avoid the local optimum and greatly improve the stability of the obstacle avoidance effect. The improved particle swarm algorithm has faster convergence speed and higher accuracy.

[0055] The present invention constructs a B-spline route fitting module by adding and constructing the properties of the B-spline convex hull to properly avoid obstacles. The required order can be selected during calculation to avoid the problem of a significant increase in calculation complexity caused by an increase in the number of waypoints. The formed curve will be smoother, the fixed-wing posture adjustment required will be smaller, the stability will be higher, and the influence of wind speed interference on the tracking effect of the drone will be further eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 is a flow chart of the method of the present invention;

[0057] Figure 2 It is a flowchart of the improved particle swarm algorithm;

[0058] Figure 3 It is an effect diagram generated by using the present invention. DETAILED DESCRIPTION

[0059] The specific working process of the present invention is further described in detail below in conjunction with the accompanying drawings.

[0060] like Figure 1 As shown, the embodiments of the present invention and their implementation process are as follows:

[0061] Step S1: Set the desired speed V of the drone E and the desired pose P E ,The posture includes the position coordinates of the fixed wing in the three-dimensional space and the yaw angle, pitch angle and roll angle of the fixed wing. By setting the control rate of various parameters and constraints related to the path formation, the fixed wing's optimized path planning function is set in the form of a loss function by weighting each part. The planning function consists of: a position loss function composed of the error with the expected position and the relationship with the obstacle position, a speed loss function composed of the error with the expected speed and the error with the expected angular velocity, a loss function with the expected route error, and a constraint loss function composed of the constraints of the turning radius and the route angular velocity.

[0062] Step S2: Obtain the current position P of the fixed wing in real time s , and set the desired flight path D of the fixed wing:

[0063] P s= [x_a, y_a, z_a, χ_a]

[0064] D = [x_d, y_d, z_d]

[0065] Among them, x, y, and z represent the three-axis direction coordinates in the north-east local navigation coordinate system, and χ represents the set formed by the yaw angle, pitch angle, and roll angle of the fixed wing. The desired flight path D is a set of a series of position points describing this path. The desired flight path is a preset fixed-wing flight mission and is thus set as known. x_a, y_a, z_a, and χ_a respectively represent the three coordinate positions and the heading angle at the current moment, x D , y D , z D respectively represent the three coordinate positions of the desired flight path.

[0066] Step S3: Set up an improved particle swarm waypoint prediction module. According to the current poses P of each fixed wing obtained in Step S1 and Step S2 s and the desired flight path D of the fixed wing, the obstacle position information and its threat radius [X OBS , Y OBS , Z OBS , R OBS and the turning radius interval [R MIN , R MAX of the fixed wing, where R MIN represents the minimum turning radius R of the fixed wing MAX represents the maximum turning radius of the fixed wing, and is calculated by the following formula:

[0067]

[0068]

[0069] Among them, L D represents the total length of the desired travel path, L REF represents the relative length of the travel path at the current position, v min represents the minimum speed to maintain maneuverability under the aerodynamic conditions of the fixed wing, w max represents the maximum yaw angular velocity under the combined control of the rudder and aileron of the fixed wing.

[0070] Calculate, through the improved particle swarm waypoint prediction module, the waypoint set [X s from the current pose P of the fixed wing to the desired fixed-wing travel path D in the case of avoiding adjacent obstacles and with the shortest path and the least pose change OPT , Y OPT , Z OPT . Its process is as Figure 2As shown, first initialize the particle swarm parameters such as the number of particles in the swarm, the maximum number of iterations, the mixing probability, the size ratio of the mixing pool, the acceleration factor, and the inertia weight coefficient. Then, calculate the fitness for each particle (the fitness is the value of the function at the position of the particle in the function to be calculated). Initially, increase the value of the inertia coefficient w according to the increase in the number of iterations (iteration means the process of looping once according to steps S2 to S5 in the algorithm process). Then, update pBest and gBest according to the fitness value. After that, use the set of fitness values of each particle as the parent set, make a selection in the parent set, select two relatively high-quality ones, and then perform crossover between the two parents composed of single digits to combine the final solution. Finally, update the velocity and position of the particles according to the update formula to complete one loop, and then perform the next loop based on the new particles until the upper limit of the number of iterations is reached and the optimal solution is output.

[0071] Selection means setting a random number with a uniform value in the range of [1, 2], performing a dot product with the set of fitness values (dot product means multiplying each fitness value by this random number), and then comparing the size with the original set of fitness values in turn until a larger value is found or the traversal is complete. If a larger value is found, use it as one of the parents; if no larger value is found after the traversal is complete, use itself as one of the parents. And this process of finding the larger fitness value is the process of selecting high-quality particles in the previous paragraph.

[0072] Crossover means that a part of the particles randomly intersects pairwise to generate the same number of offspring particles. Then, the offspring particles can replace the parent particles, thus keeping the number of particles in the population unchanged. The position of the offspring particles is calculated by the arithmetic weighted sum of the positions of the two parent particles:

[0073]

[0074]

[0075] where is the position vector of the target search space dimension. In this algorithm, since the considered direction is the loss function composed of four related functions, this vector has four dimensions, and and k = 1, 2 respectively indicate whether it is the position of the offspring particle or the parent particle; is a uniformly distributed random vector in this dimension space, and its components in each dimension are randomly and uniformly taken in the range of [0, 1].

[0076] In the present invention, the fixed-wing performs obstacle avoidance when the relevant information of the obstacle is known, and there are no other unknown obstacles on the plane where the flight altitude is located. According to the known properties of the fixed-wing itself, the minimum turning radius R of the fixed-wing is obtained in advanceMIN , take half of the distance between the current position and the end point as the maximum turning radius R of the fixed-wing MAX , construct the turning radius interval [R of the fixed-wing MIN , R MAX . The improved particle swarm optimization algorithm is based on the hybrid PSO algorithm with modifications to the inertia weight coefficient

[0077] Step S4: Set the B-spline route fitting module, perform third-order interpolation fitting on the set of waypoints to obtain a B-spline curve, thus obtaining a smooth route, and use the position information at the next moment in the obtained predicted route as the actual expected travel path D s , and accordingly set the expected attitude and speed of the fixed-wing.

[0078] Among them, the B-spline is established as the following formula:

[0079]

[0080] In the present invention, the waypoints obtained by the improved particle swarm optimization algorithm are at equal distances, satisfying the design of the uniform B-spline curve.

[0081] Step S5: After the above steps, the fixed-wing flies to the next expected waypoint. In the improved particle swarm waypoint prediction module, according to the relationship between the pose after traveling to the next moment and the expected flight path D of the fixed-wing, and the position information of the obstacle and its threat radius [X OBS , Y OBS , Z OBS , R OBS , the relationship also changes, so it returns to step S2 again to calculate the actual expected path D s and the output expected heading angle and speed.

[0082] Thus, the actual expected path of the fixed-wing is continuously updated, and the traveling state under its optimal path is optimized and reset.

[0083] In order to verify the feasibility of the method of the present invention, simulation is carried out with the help of Matlab, and at the same time, kinematic and dynamic functions that meet the aerodynamic conditions of the fixed-wing are added to describe the feedback of the fixed-wing after receiving the expected heading and speed, so as to increase the credibility of the simulation.

[0084] The results after adopting the present invention are as Figure 3 shown. The solid lines in the figure are the paths that the fixed-wing has passed, the dot lines are the predicted expected routes, and the spaced lines are the fixed-wing expected routes set in advance. As Figure 3 shown, the fixed-wing can track the preset expected route while avoiding obstacles.

[0085] The results show that the present invention can achieve the purpose of path tracking under obstacle avoidance conditions.

Claims

1. A fixed-wing path tracking method, characterized in that: The method specifically includes: Step S1: Set the expected speed and expected pose of the fixed-wing end point, and set the path planning function to be optimized for the fixed-wing path through a loss function; Step S2: Obtain the pose P of the fixed-wing at the current moment in real time s , and update and set the flight constraint conditions of the fixed-wing according to the actual expected flight path at the previous moment; Step S3: Set up an improved particle swarm waypoint prediction module. In the improved particle swarm waypoint prediction module, based on the current pose P of the fixed-wing aircraft s , the obstacle position information and its threat radius, the desired flight path D, and the turning radius interval [R MIN , R MAX , where R MIN represents the minimum turning radius of the fixed-wing aircraft, and R MAX represents the maximum turning radius of the fixed-wing aircraft. Process to obtain a set of waypoints from the current pose P of the fixed-wing aircraft s under the condition of avoiding adjacent obstacles and with the shortest path and the least change in pose to the desired travel path D In the improved particle swarm waypoint prediction module of the step S3, according to the current pose P of the fixed-wing aircraft input s 、the obstacle position information and its threat radius [X OBS , Y OBS , Z OBS , R OBS , the expected fixed-wing aircraft travel path D, the minimum fixed-wing aircraft attitude change, and the turning radius interval [R MIN , R MAX , the following objective function is established: Pose loss function: Speed loss function: Trajectory tracking loss function: Cost_tra = ((psi_d - psi_a) / Π) 2 + ((pit_d - pit_a) / Π) 2 And set the total loss function according to the following formula: Func_tra = lam_1 * Cost_pos + lam_2 * Cost_speed + lam_3 * Cost_tra Among them, Cost pos represents the pose loss function, Cost spead represents the formula speed loss function, Cost tra represents the trajectory tracking loss function, Func tra represents the total loss function, Π represents pi, x_a, y_a, z_a are the three coordinate position information at the current moment, x_d, y_d, z_d are the coordinate position information of the expected flight path at the current moment, u_a, v_a, w_a are the speed information in the three directions at the current moment, u_d, v_d, w_d are the speed information in the three directions of the expected flight path, psi_a is the heading angle at the current moment, pit_a is the pitch angle at the current moment, psi_d is the heading angle under the expected flight path, pit_d is the pitch angle under the expected flight path, and lam1, lam2, lam3 are the weighting coefficients of the three loss functions; Then use the improved particle swarm optimization algorithm to solve the objective function to obtain the optimal set of waypoints; The improved particle swarm optimization algorithm is that in the particle swarm optimization algorithm, the weight w of the inertia coefficient is set to be calculated according to the following formula: w = 0.4 + 0.006 * (100 - i) where i represents the number of iterations, and the total number of iterations set is 100 times; Step S4: Set up a B-spline route fitting module. Through the B-spline route fitting module, interpolate and fit the set of waypoints to obtain a B-spline curve, thereby obtaining a smooth predicted route. Extract the position at the next moment in the predicted route and establish an actual expected flight path D between it and the position at the current moment s ; Step S5: In the improved particle swarm waypoint prediction module, change in real time according to the relationship between the current pose after moving to the next moment and the expected flight path D of the fixed-wing respectively, and the relationship between the position information of the obstacle and its threat radius, and return to Step S2 again to calculate the actual expected path.

2. A fixed-wing path tracking method according to claim 1, characterized in that: In step S2, update and set the flight constraint conditions of the fixed-wing according to the actual expected flight path of the previous moment, specifically: Among them, L D represents the total length of the expected flight path D, L REF represents the straight-line distance from the current position to the starting position, v min represents the minimum speed to maintain maneuverability under the aerodynamic conditions of a fixed-wing aircraft, w max represents the maximum yaw angular velocity under the combined control of the rudder and ailerons of a fixed-wing aircraft.

3. A fixed-wing path tracking method according to claim 1, characterized in that: In the B-spline route fitting module of step S4, use the waypoints that have been traveled, the set of waypoints output in real time by the improved particle swarm waypoint prediction module, and the current heading angle as the preconditions for B-spline curve interpolation fitting. Additionally, with the minimum yaw angular velocity under the lowest speed maneuverability as a constraint, select a third-order B-spline function for solution to obtain a smoothly transitioning predicted route.

Images