A complex path following guidance method for UAV based on nonlinear model predictive control
Through the nonlinear model prediction control method, an error model for the drone to follow complex routes is established, and the chaotic particle swarm algorithm is solved, and the problem of low follow-up accuracy in complex routes is solved, achieving higher follow-up accuracy and safety.
Patent Information
- Application Number
- CN202211299557.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-10-21
- Publication Date
- 2025-04-18
- Estimated Expiration
- 2042-10-21
AI Technical Summary
When drones follow complex routes, the curvature of the route varies, resulting in the drone rolling angle amplitude being limited to a safe range, with low following accuracy, and certain route sections may not be followed.
Using a nonlinear model prediction control method, a reference point for variable speed movement along a predetermined complex route is introduced, and a position and direction error model of the drone following the route is established under the Serret-Frenet coordinates. A nonlinear optimization problem is constructed based on rolling angle and dynamic reference point velocity constraints, and a chaotic particle swarm algorithm and elite retention strategy are solved to obtain guidance instructions.
It improves the accuracy and safety of drones in complex routes, ensures the continuity and followability of routes, and takes into account the following accuracy and safe flight requirements.
Smart Images
Figure CN115597603B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of aircraft route following, and in particular to a complex route following guidance method for an unmanned aerial vehicle based on nonlinear model predictive control. Background Art
[0002] With the emergence of various combat scenarios, higher requirements are placed on the route-following capabilities of drones. In future combat, drones not only need to follow simple routes such as straight lines or arcs, but also need to have the ability to follow more complex routes with more diverse shapes.
[0003] When a drone follows a simple route such as a conventional straight line or arc, the route curvature is often a constant, and the drone's rolling maneuverability is rarely considered. However, when a drone follows a complex route, the route curvature is often variable, and the drone's roll angle needs to be constantly adjusted as the route curvature changes. However, the drone's horizontal maneuverability is limited, and the roll angle amplitude is restricted to a safe range, resulting in low following accuracy for complex routes, and a certain section of the planned route may not be followable.
[0004] Therefore, a complex path following guidance method for UAV based on nonlinear model predictive control is proposed to achieve the goal of taking into account both following accuracy and safe flight requirements. Summary of the invention
[0005] The purpose of the present invention is to provide a complex route following guidance method for an unmanned aerial vehicle based on nonlinear model predictive control, so as to solve the problem proposed in the above background technology that when the unmanned aerial vehicle follows a simple route such as a conventional straight line segment or a circular arc, the route curvature is often a constant, and the rolling maneuverability of the unmanned aerial vehicle is rarely considered. When the unmanned aerial vehicle follows a complex route, the horizontal maneuverability of the unmanned aerial vehicle is limited, and the roll angle amplitude is limited to a safe range, resulting in low following accuracy of the complex route, and a certain section of the planned route may be unfollowable.
[0006] To achieve the above object, the present invention provides the following technical solution: a complex route following guidance method for unmanned aerial vehicle based on nonlinear model predictive control, comprising the following steps;
[0007] S1. Introduce a reference point that moves at a variable speed along a predetermined complex route, and establish a position and direction error model of the UAV following the route in Serret-Frenet coordinates;
[0008] S2, combining the constraints of the UAV roll angle and the moving reference point speed, construct the nonlinear optimization problem of route following;
[0009] S3. Chaotic particle swarm algorithm is used to solve the problem and combined with elite retention strategy to obtain guidance instructions.
[0010] Preferably, the step S1 comprises the following sub-steps:
[0011] S1.1: Introduce a reference point that starts from the initial waypoint and moves along the predetermined complex route at a variable speed, and record the reference point at a certain moment Location coordinates The direction of motion is χ d (l), where l is The distance from the initial waypoint along the reference route;
[0012] S1.2: Record the current drone location coordinates The track bearing is χ;
[0013] S1.3: With northward movement as the positive direction of the horizontal axis x, eastward movement as the positive direction of the vertical axis y, and the azimuth of the track in the true north direction as zero, the position error is defined as:
[0014]
[0015] In the formula, is the coordinate rotation matrix from the inertial coordinate system to the Serret-Frenet coordinate system, and defines e s is the forward error, e d is the lateral error;
[0016] S1.4: Derivative of the position error:
[0017]
[0018] Where V g is the ground speed of the UAV, V d is the movement speed of the reference point;
[0019] S1.5: Define the directional error as:
[0020]
[0021] Derivative of the directional error:
[0022]
[0023] Where ω is the track azimuth velocity, κ(l) is the current moment The curvature of
[0024] S1.6: Combining the position error derivative and the direction error derivative, the dynamic error model of the UAV following the route is obtained:
[0025]
[0026] By designing ω and V d Adjustable forward error es , lateral error e d and direction error dynamic change process.
[0027] Preferably, step S2 comprises the following sub-steps:
[0028] S2.1: First, describe the constraints of the drone's roll angle and the speed of the moving reference point. Consider the drone making a coordinated turn. At this time, the sideslip angle is zero and the drone's horizontal direction satisfies:
[0029] F lift sinφcos(χ-ψ)=mV g ω
[0030] F lift is the lift, φ is the roll angle, m is the mass of the drone, and the vertical direction satisfies:
[0031] F lift cosφ=mg
[0032] g is the acceleration due to gravity, so the relationship between φ and ω is:
[0033]
[0034] S2.2: The maximum rolling angle allowed during the flight of the drone is φ max , so the roll angle amplitude constraint can be expressed as |φ|≤φ max , accordingly, there is a maximum track azimuth velocity ω max , and satisfy |ω|≤ω max , for the moving reference point, let V d min Represents the lower limit of the reference point movement speed, V d max represents the upper limit of the reference point motion speed, so the moving reference point speed constraint can be expressed as V d min ≤V d ≤V d max .
[0035] Second, define and U=(V d ,ω) T , rewrite the error model as:
[0036]
[0037] In the formula, Defined as the error input to the error model;
[0038] S2.3: The error model is discretized into time intervals T by the Euler method and converted into a difference equation form:
[0039]
[0040] k is a certain moment, so the nonlinear optimization problem can be described as:
[0041]
[0042]
[0043] In the formula, k+i|k represents the prediction of the k+i moment in the future at the k moment, Q is the weight matrix of the state error, and by adjusting Q, the weights of various errors in the convergence process can be changed, thereby causing different following effects, R is the weight matrix of the error input, and by adjusting R, the stability of the control can be changed, and N P Represents the prediction time domain.
[0044] Preferably, step S3 comprises the following sub-steps:
[0045] S3.1: First, the standard particle swarm algorithm is used to solve the optimization problem containing multiple local extreme points;
[0046] The mathematical expression of the standard particle swarm algorithm is:
[0047]
[0048] In the formula, w represents the inertia weight coefficient of the particle, c1 and c2 represent the cognitive and social acceleration coefficients of the particle, r1 and r2 are two random numbers distributed between [0, 1]. and denote the position and velocity of the ith particle at the kth iteration, respectively, and p i represents the individual optimal position of the i-th particle at the k-th iteration, p g represents the global optimal position of the group at the kth iteration;
[0049] S3.2: If the particles in the population fall into the local optimum and mature prematurely using the standard particle swarm algorithm, a chaotic search is performed in the specified space to replace some particles in the original population, thereby leading the population out of the local optimum. The chaotic search space is defined as p g Centered by ηp g In the spatial neighborhood of radius, η is the chaotic search coefficient, and the chaotic mapping is:
[0050]
[0051] Preferably, in step S3.1, in order to improve the searching ability of particles, three coefficients are selected:
[0052]
[0053] Where w1 and w2 are the maximum and minimum values of the inertia weight coefficient, respectively, and c 1i and c 1f are the maximum and minimum values of the cognitive acceleration coefficient, c 2i and c 2f are the minimum and maximum values of the social acceleration coefficient, t represents the current iteration number, and M represents the maximum iteration number.
[0054] Preferably, in step S3.2, in the process of solving the problem using the chaotic coefficient, the optimal solution obtained in the previous iterative search is assigned to a particle in the current population in combination with the elite retention strategy to improve the solving efficiency of the chaotic particle swarm algorithm.
[0055] Compared with the prior art, the present invention has the following beneficial effects:
[0056] 1. The present invention introduces a reference point that moves at a variable speed along a predetermined complex route by design, and establishes a position and direction error model of the UAV following the route in Serret-Frenet coordinates. The movement speed of the reference point can be used as an additional input degree of freedom to adjust the error;
[0057] 2. The present invention adds a roll angle amplitude constraint when constructing a nonlinear optimization problem using an error model, thereby obtaining a guidance instruction that takes into account both the following accuracy and the safety flight requirements;
[0058] 3. The present invention solves nonlinear optimization problems by adopting a chaotic particle swarm algorithm combined with an elite retention strategy, thereby improving the efficiency of solving nonlinear optimization problems. BRIEF DESCRIPTION OF THE DRAWINGS
[0059] Figure 1 It is a route following schematic diagram of the present invention;
[0060] Figure 2 It is the curvature variation diagram of the route that meets the minimum turning radius constraint;
[0061] Figure 3 It is the result graph of the drone following the route that meets the minimum turning radius constraint;
[0062] Figure 4 It is the ground speed change diagram of the UAV while following the route that meets the minimum turning radius constraint;
[0063] Figure 5 It is the speed change diagram of the reference point of the drone following the route that meets the minimum turning radius constraint;
[0064] Figure 6 It is the change diagram of the forward error of the UAV when following the route that meets the minimum turning radius constraint;
[0065] Figure 7 It is the lateral error change diagram of the UAV when following the route that meets the minimum turning radius constraint;
[0066] Figure 8 It is the change diagram of the directional error of the UAV following the route that meets the minimum turning radius constraint;
[0067] Fig. 9 It is the curvature variation diagram of the route that does not meet the minimum turning radius constraint;
[0068] Fig.10 It is the result graph of the drone following the route that does not meet the minimum turning radius constraint;
[0069] Fig.11 It is the change diagram of the forward error when the UAV is following the route that does not meet the minimum turning radius constraint;
[0070] Fig.12 It is the lateral error change diagram of the UAV when following the route that does not meet the minimum turning radius constraint;
[0071] Fig.13 It is the change diagram of the direction error of the UAV following the route that does not meet the minimum turning radius constraint;
[0072] Fig.14 It is a graph of the roll angle change of the UAV when following a route that does not meet the minimum turning radius constraint. DETAILED DESCRIPTION
[0073] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0074] See also Figure 1 , a complex path following guidance method for UAV based on nonlinear model predictive control, the specific design steps are as follows:
[0075] In step 1, a reference point is introduced that starts from the initial waypoint and moves along the predetermined complex route at a variable speed. The reference point at a certain moment is recorded as Location coordinates The direction of motion is χ d (l), where l is The distance from the initial waypoint along the reference route direction, and the current position coordinates of the drone The track azimuth is χ. The northward movement is the positive direction of the horizontal axis x, the eastward movement is the positive direction of the vertical axis y, the track azimuth in the true north direction is zero, and the position error is defined as:
[0076]
[0077] In the formula, is the coordinate rotation matrix from the inertial coordinate system to the Serret-Frenet coordinate system. Define e s is the forward error, e d is the lateral error. Derivative of the position error:
[0078]
[0079] Where V g is the ground speed of the UAV, V d is the velocity of the reference point. The direction error is defined as:
[0080]
[0081] Derivative of the directional error:
[0082]
[0083] Where ω is the track azimuth velocity, κ(l) is the current moment The curvature.
[0084] Therefore, the dynamic error model of the UAV following the route is:
[0085]
[0086] By designing ω and V d Adjustable forward error e s , lateral error e d and direction error dynamic change process.
[0087] In step 2, first describe the constraints such as the roll angle of the drone and the speed of the moving reference point. Consider that the drone performs a coordinated turn, at which time the sideslip angle is zero and the horizontal direction of the drone satisfies:
[0088] F lift sinφcos(χ-ψ)=mV g ω
[0089] F lift is the lift, φ is the roll angle, m is the mass of the drone, and the vertical direction satisfies:
[0090] F lift cosφ=mg
[0091] g is the acceleration due to gravity, so the relationship between φ and ω is:
[0092]
[0093] The maximum rolling angle allowed during the flight of the drone is φ max , so the roll angle amplitude constraint can be expressed as |φ|≤φ max , accordingly, there is a maximum track azimuth velocity ω max , and satisfy |ω|≤ω max For the moving reference point, let V d min Represents the lower limit of the reference point movement speed, V d max represents the upper limit of the reference point motion speed, so the moving reference point speed constraint can be expressed as V d min ≤V d ≤V d max .
[0094] Second, define and U=(V d ,ω) T , rewrite the error model as:
[0095]
[0096] In the formula, Defined as the error input of the error model. The error model is discretized into time intervals T by the Euler method and converted into the form of a difference equation:
[0097]
[0098] k is a certain moment, so the nonlinear optimization problem can be described as:
[0099]
[0100]
[0101] In the formula, k+i|k represents the prediction of the k+i moment in the future at the k moment, Q is the weight matrix of the state error, and by adjusting Q, the weights of various errors in the convergence process can be changed, thereby causing different following effects, R is the weight matrix of the error input, and by adjusting R, the stability of the control can be changed, and N P Represents the prediction time domain.
[0102] In step 3, the mathematical expression of the standard particle swarm algorithm is:
[0103]
[0104] In the formula, w represents the inertia weight coefficient of the particle, c1 and c2 represent the cognitive and social acceleration coefficients of the particle, r1 and r2 are two random numbers distributed between [0, 1]. and denote the position and velocity of the ith particle at the kth iteration, respectively, and p i represents the individual optimal position of the i-th particle at the k-th iteration, p g Indicates the global optimal position of the group at the kth iteration. At the same time, in order to improve the search ability of particles, three coefficients are selected as follows:
[0105]
[0106] Where w1 and w2 are the maximum and minimum values of the inertia weight coefficient, respectively, and c 1i and c 1f are the maximum and minimum values of the cognitive acceleration coefficient, c 2i and c 2f are the minimum and maximum values of the social acceleration coefficient, t represents the current number of iterations, and M represents the maximum number of iterations.
[0107] When using the standard particle swarm algorithm to solve optimization problems containing multiple local extreme points, the population particles are prone to fall into the local optimum and become premature. To address this defect, when the algorithm falls into premature maturity, a chaotic search is performed in the specified space to replace some particles in the original population, thereby leading the population out of the local optimum. The chaotic search space is defined in terms of p g Centered by ηp g In the spatial neighborhood of radius, η is the chaotic search coefficient, and the chaotic mapping is:
[0108]
[0109] At the same time, in the solution process, the elite retention strategy is combined to assign the optimal solution obtained in the previous iterative search to a particle in the current population to improve the solution efficiency of the chaotic particle swarm algorithm.
[0110] The guidance parameters mainly include the prediction time domain N P ,Q,R,Total number of particles S,Number of iterations M,w1,w2,c 1i 、c 2f 、c 1f 、c 2i 、η。
[0111] Prediction time domain N P There is no fixed principle for selecting the size of . Both smaller and larger values will affect the effect, so it is better to achieve a better following effect.
[0112] Q is the weight matrix of the state error. By adjusting Q, the weights of various errors in the convergence process can be changed, thereby causing different following effects. R is the weight matrix of the error input. By adjusting R, the stability of the control can be changed. The principle of selecting the two is to achieve a good following effect and meet the stability requirements of the control.
[0113] As for the selection of M, the optimization goal is to find a feasible suboptimal solution based on the elite retention strategy, and NMPC (nonlinear model predictive control) performs rolling optimization at every moment, so M does not need to be very large. As for the selection of S, since V d The variation range of and ω is small, and 15 to 30 particles can cover all intervals of the value range.
[0114] w1, w2, c 1i , C 2f 、c 1f 、c 2i The selection is based on the references RATNAWEERA A, HALGAMUGE SK, WATSON HC.Se1f-organizing hierarchical particle swarm optimizer with time-varyingacceleration coefficients[J].IEEE Trans.on evolutionary computation, 2004, 8-(3): 240-255.
[0115] There is no fixed selection principle for η, and it is selected based on experience.
[0116] The guidance law is an algorithm, including steps 1-3, because the whole algorithm is needed to complete the route following (this is common knowledge for those skilled in the art). The nonlinear optimization problem of step 2 is solved by using the chaotic particle swarm algorithm and the elite retention strategy, and the guidance instruction is obtained, which is also the actual instruction.
[0117] The UAV roll angle amplitude constraint |φ|≤30° and the moving reference point speed constraint 70m / s≤V d ≤160m / s, prediction time domain N P =4, Total number of particles S = 20, number of iterations M = 15, w1 = 0.9, w2 = 0.4, c 1i =c zf =2.5, c 1f =c 2i =0.5, η=1.5 as an example, the guidance law is used to simulate the following of the routes that meet the minimum turning radius constraint and those that do not meet the minimum turning radius constraint, wherein the routes are generated from a number of waypoints by cubic spline interpolation, and have the characteristics of smoothness and continuous curvature.
[0118] At each sampling moment, the position coordinates and track azimuth of the UAV are obtained, and the position coordinates and movement direction of the reference point are obtained. Steps 1-3 are performed in sequence to obtain guidance instructions, and then the status of the UAV and the reference point are updated, and finally the entire process of route following is completed. The effect is as follows Figure 2-Figure 14 shown.
[0119] thereby Figure 2-Figure 8 It can be seen that the UAV has a good following effect on the route that meets the minimum turning radius constraint, among which Figure 2 It is concluded that the curvature of the reference route is always within the safe curvature range, and the relationship between the route arc length and the route curvature in the route is obtained by cubic spline interpolation;
[0120] from Figure 3 See the result of the drone’s following;
[0121] from Figure 4 See the change of the drone's ground speed;
[0122] From 5, we can see the change of the speed of the drone reference point;
[0123] from Figure 6-Figure 8 It can be seen that the error of the UAV changes. The position error and direction error are small, and the UAV's following accuracy is high;
[0124] from Figure 9-14 It can be seen that the UAV follows the route that does not meet the minimum turning radius constraint. Fig. 9 It can be seen that the curvature of the reference route is 1.05×10 4 m~1.55×10 4 The m range is partially outside the safe curvature range. Fig.10 Represents the result graph of following, Figure 11-13 The variation diagram representing the error shows that the UAV can quickly and accurately approach the reference route after flying through the unfollowable segment;
[0125] from Fig.14 It can be seen that the roll angle of the drone changes, and the roll angle of the drone is always within a safe range.
[0126] In summary, the UAV guidance law of the present invention takes into account both the following accuracy and the safe flight requirements. The guidance law of the present invention can better complete the task of following a complex route and has a strong ability to resist wind disturbance.
[0127] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.
Claims
1. A complex route following guidance method for unmanned aerial vehicles based on nonlinear model predictive control, characterized in that: The method comprises the following steps: S1. Introduce a reference point that moves at a variable speed along a predetermined complex route, and establish a position and direction error model of the UAV following the route in Serret-Frenet coordinates; S2. Combine the constraints of the UAV's roll angle and the speed of the moving reference point to construct a nonlinear optimization problem for path following; S3, using the chaotic particle swarm algorithm to solve, and combining it with the elite retention strategy to obtain the guidance instructions; Wherein, step S1 comprises the following steps: S1.1: Introduce a reference point that starts from the initial waypoint and moves along the predetermined complex route at a variable speed. Note the reference point P at a certain moment. d The position coordinates P d =(x d (l),y d (l)), the direction of movement is χ d (l), where l is P d The distance from the initial waypoint along the reference route; S1.2: Record the current position coordinates of the drone P c =(x c ,y c ), the track azimuth is χ; S1.3: With northward movement as the positive direction of the horizontal axis x, eastward movement as the positive direction of the vertical axis y, and the azimuth of the track in the true north direction as zero, the position error is defined as: In the formula, is the coordinate rotation matrix from the inertial coordinate system to the Serret-Frenet coordinate system, and defines e s is the forward error, e d is the lateral error; S1.4: Derivative of the position error: Where V g is the ground speed of the UAV, V d is the movement speed of the reference point; S1.5: Define the directional error as: Derivative of the directional error: Where ω is the track azimuth velocity, κ(l) is the current moment P d The curvature of S1.6: Combining the position error derivative and the direction error derivative, the dynamic error model of the UAV following the route is obtained: By designing ω and V d Adjustable forward error e s , lateral error e d and direction error The dynamic change process of Wherein, the step S2 comprises the following steps: S2.1: First, describe the constraints on the roll angle and the moving reference point speed of the drone. Consider the drone making a coordinated turn. At this time, the sideslip angle is zero and the horizontal direction of the drone satisfies: F lift sinφcos(χ-ψ)=mV g oh F lift is the lift, φ is the roll angle, m is the mass of the drone, and the vertical direction satisfies: F lift cosφ=mg g is the acceleration due to gravity, so the relationship between φ and ω is: S2.2: The maximum rolling angle allowed during the flight of the drone is φ max , so the roll angle amplitude constraint can be expressed as φ≤φ max , accordingly, there is a maximum track azimuth velocity ω max , and satisfy |ω≤ω max , for the moving reference point, let V dmin Represents the lower limit of the reference point movement speed, V dmax represents the upper limit of the reference point motion speed, so the moving reference point speed constraint can be expressed as V dmin ≤V d ≤V dmax ; Second, define and U=(V d ,ω) T , rewrite the error model as: In the formula, Defined as the error input to the error model; S2.3: The error model is discretized into time intervals T by the Euler method and converted into a difference equation form: k is a certain moment, so the nonlinear optimization problem can be described as: In the formula, k+i|k represents the prediction of the k+i moment in the future at the k moment, Q is the weight matrix of the state error, and by adjusting Q, the weights of various errors in the convergence process can be changed, thereby causing different following effects, R is the weight matrix of the error input, and by adjusting R, the stability of the control can be changed, and N P Represents the prediction time domain.
2. The method for complex route following guidance of an unmanned aerial vehicle based on nonlinear model predictive control according to claim 1, characterized in that: The step S3 comprises the following sub-steps: S3.1: First, the standard particle swarm algorithm is used to solve the optimization problem containing multiple local extreme points; The mathematical expression of the standard particle swarm algorithm is: In the formula, w represents the inertia weight coefficient of the particle, c1 and c2 represent the cognitive and social acceleration coefficients of the particle, r1 and r2 are two random numbers distributed between [0,1]. and denote the position and velocity of the ith particle at the kth iteration, respectively, and p i represents the individual optimal position of the i-th particle at the k-th iteration, p g represents the global optimal position of the group at the kth iteration; S3.2: If the particles in the population fall into the local optimum and mature prematurely using the standard particle swarm algorithm, a chaotic search is performed in the specified space to replace some particles in the original population, thereby leading the population out of the local optimum. The chaotic search space is defined as p g Centered by ηp g In the spatial neighborhood of radius, η is the chaotic search coefficient, and the chaotic mapping is:
3. The method for complex route following guidance of an unmanned aerial vehicle based on nonlinear model predictive control according to claim 2, characterized in that: In step S3.1, in order to improve the search capability of particles, three coefficients are selected: Where w1 and w2 are the maximum and minimum values of the inertia weight coefficient, respectively, and c 1i and c1f are the maximum and minimum values of the cognitive acceleration coefficient, respectively. 2i and c 2f are the minimum and maximum values of the social acceleration coefficient, t represents the current iteration number, and M represents the maximum iteration number.
4. The method for complex route following guidance of an unmanned aerial vehicle based on nonlinear model predictive control according to claim 2, characterized in that: In step S3.2, in the process of solving the problem using the chaotic coefficient, the optimal solution obtained in the previous iterative search is assigned to a particle in the current population in combination with the elite retention strategy, so as to improve the solving efficiency of the chaotic particle swarm algorithm.