Fixed-wing unmanned aerial vehicle route following method based on velocity vector controller

Abstract

The invention discloses a fixed-wing unmanned aerial vehicle route following method based on a velocity vector controller, which belongs to the technical field of unmanned aerial vehicle control and comprises the following steps of: S1, generating a flight task and inputting route information; s2, constructing a velocity vector field: generating expected ground velocity vector information of the unmanned aerial vehicle at the current position point by designing an FMC-VF route following algorithm; and S3, control quantity generation: by designing a TDE-PID velocity vector controller, calculating to obtain expected steering engine deflection angle information and outputting the information to the fixed-wing unmanned aerial vehicle model. According to the fixed-wing unmanned aerial vehicle route following method based on the velocity vector controller, the FMC-VF route following algorithm is applied to the developed TDE-PID ground velocity vector controller, so that the unmanned aerial vehicle has the capability of accurately following a complex expected curve route under environmental wind.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle control, and in particular to a fixed-wing unmanned aerial vehicle route following method based on a velocity vector controller. Background Art

[0002] Small fixed-wing drones can fly using the lift generated by their wings, have longer hovering time and range, and play an important role in civil fields such as environmental monitoring, disaster relief, and long-distance transportation. In actual low-altitude flight missions, there are obstacles such as hills, mountains, and even skyscrapers, so drones must have accurate route-following capabilities. In addition, small drones at low altitudes often encounter low-altitude wind shear that changes in size and direction, which can easily cause them to deviate from the desired route. This requires that the drone route-following process needs to be robust to unpredictable wind disturbances.

[0003] At present, there are two strategies to solve the route following problem: geometric strategy and control strategy. The geometric strategy mainly refers to the guidance rate of the missile. Common algorithms are:

[0004] (1) Pure Pursuit (PP) route following algorithm introduces a virtual target point (VTP) to guide the aircraft to chase the VTP, and finally make the drone trajectory converge to the desired path. The pure pursuit algorithm is shown in the following formula:

[0005] ψ l =k2d;

[0006] where k2 is the gain and d is the lateral path error.

[0007] (2) The route following algorithm based on line of sight (LOS) makes the UAV trajectory converge to the desired path by making the UAV heading angle converge to the line of sight (LOS). The algorithm is shown in the following formula:

[0008] ψ p =k1(θ d -ψ);

[0009] where k1 is the gain, θ d is the desired angle and ψ is the vehicle heading angle.

[0010] (3) The tracking algorithm based on line of sight angle (PLOS) combines the distance criterion with the line of sight angle criterion, enabling the UAV to show good tracking effect in the straight line following scenario.

[0011] ψ d =k1(θ d -ψ)+k2d;

[0012] When k1>0 and k2>0, the PLOS guidance law is stable. The larger the k1 value, the more the aircraft will prioritize converging the direction angle to LOS, which will slow down the convergence of the lateral error; the larger the k2 value, the more the aircraft will prioritize converging the lateral error, which will easily cause the track to oscillate.

[0013] (4) Nonlinear Guidance Law (NLGL), which is achieved by drawing a circle with a radius L with the current position p of the UAV as the center. This circle will intersect the target course at two points, s and s' (the choice depends on the direction of the UAV's motion). The angle deviation can then be used to calculate the required lateral acceleration.

[0014]

[0015] Among them, v a is the airspeed of the aircraft, and η is the difference between the desired route inclination and the current heading angle of the UAV. The advantage of this algorithm is that it can be flexibly applied to any type of desired route. Experiments have shown that the route following algorithm still has good robustness when facing different airspeed conditions.

[0016] (5) The route following algorithm based on velocity vector fields (VF) indicates the airflow direction at each position in space through the velocity vector field. As long as the aircraft flies along the desired velocity vector direction at each spatial position, its final flight trajectory will converge to the desired route. And because the algorithm can introduce the known size and direction of natural wind when generating the velocity vector field, it can achieve a good tracking effect for the desired route with constant wind disturbance.

[0017] Most of the existing route-following algorithms suitable for fixed-wing UAVs are only applied to straight and circular routes, which makes it difficult to meet the maneuverability and flexibility required in various mission scenarios such as guidance and obstacle avoidance. Complex curves with changing curvature will greatly increase the complexity of the route-following algorithm.

[0018] Since fixed-wing aircraft usually use attitude controllers, it is difficult to directly control the flight direction, so the wind resistance performance is poor. At present, the wind field estimation and the introduction of trajectory error compensation items are mainly used to improve the wind resistance of drones. The effect of this solution still needs to be improved when facing unpredictable strong wind disturbances. The solution of using velocity vector control for fixed-wing drones often has large controller output oscillations and chattering effect of body attitude jitter after following trajectory convergence.

[0019] At present, the most widely used control method for the optimization of fixed-wing UAV velocity vector controller is the traditional proportional-integral-differential (PID) control. It has the characteristics of simple structure, wide application range, easy parameter adjustment, strong flexibility, and excellent control performance, and has been widely used. However, its control logic of eliminating errors by error feedback always causes the contradiction between "fast response" and "overshoot" in the closed-loop system. When faced with high-maneuverability UAVs and factors such as nonlinearity and uncertainty, PID control is limited in further application and development due to its inherent defects. In recent years, with the development of chip computing power, modern control methods derived from state space, represented by sliding mode variable structure control, robust control, backstepping control, and intelligent control, have made great progress. However, while improving control performance, this type of model-based control method also abandons the simplicity and flexibility of PID control, increasing the difficulty of controller development and the cost of use. Summary of the invention

[0020] The purpose of the present invention is to provide a fixed-wing UAV route following method based on a velocity vector controller, by applying the FMC-VF route following algorithm to the developed TDE-PID ground speed vector controller, so that the UAV has the ability to accurately follow a complex desired curve route under ambient wind.

[0021] To achieve the above object, the present invention provides a fixed-wing UAV route following method based on a velocity vector controller, comprising the following steps:

[0022] S1. Flight mission generation and route information input;

[0023] S2, speed vector field construction: by designing the FMC-VF route following algorithm, the expected ground speed vector information of the UAV at the current position is generated;

[0024] S3, control quantity generation: By designing the TDE-PID speed vector controller, the desired servo deflection angle information is calculated and output to the fixed-wing UAV model.

[0025] Preferably, the FMC-VF route following algorithm includes:

[0026] Route information storage method based on Frenet coordinate system: by decoupling the desired route in the tangent and normal directions, discrete waypoint information of complex routes is generated and stored;

[0027] Vector field construction algorithm based on distance and normal angle constraints: by finding the relevant points of the desired route at any position of the drone instead of the closest point, the expected ground speed angle at the current position is calculated;

[0028] Combined route following strategy based on distance and ground speed angle criteria: Generate a smooth transition curve connecting the two mission routes to achieve a good following effect of the UAV on the combined route.

[0029] Preferably, the Frenet coordinate system in the route information storage method based on the Frenet coordinate system is established based on a reference trajectory. For any differentiable curve, a set of local orthogonal bases consisting of tangent and normal vectors are defined at each point of the curve. In the Frenet reference system, any point (x, y) in space can be represented by the arc length s of the reference Bezier curve and the track error l (s, l), which can be regarded as a linear combination of a horizontal unit vector and a vertical unit vector. For a continuous curved desired route, discrete points are generated by linear interpolation, and the tangent and normal unit vectors are obtained by using the position information of adjacent discrete points to construct the Frenet coordinate system based on the desired route, thereby completing the route information storage based on the Frenet coordinate system.

[0030] Preferably, according to a fixed-wing UAV route following method based on a velocity vector controller as described in claim 3, it is characterized in that: the vector field construction algorithm based on distance and normal angle constraints is designed according to the desired route information stored in the Frenet coordinate system. A vector field construction mechanism under multiple constraints based on distance and normal angle is designed: for all discrete points on the parameterized curve, the s parameter value is found so that the distance from the input point P to the discrete point on the parameterized curve is minimized under the condition that the line between the input point P and the discrete point on the parameterized curve is collinear with the normal line of the point on the curve, thereby obtaining the vector field construction algorithm based on distance and normal angle constraints:

[0031] S2.1, find the relevant point r(s) of the desired route;

[0032]

[0033] Among them, min|Pr(s i )| represents the input point P to the discrete point r(s i ), subject to is used to limit the feasible domain of decision variables in optimization problems and ensure that decision variables meet specific conditions. The parameterized curve is r(s), and the discrete points on the curve are r(s i ), where i = 0, 1, ..., n, the input point is P, and the parameter value to be solved is t. Then the line connecting the input point and the corresponding discrete point on the curve is r(s i ) is the normal line

[0034] S2.2, calculate the expected ground speed vector of the position point;

[0035]

[0036] Among them, χ d (i) is the desired ground speed angle, is the ground speed angle in the Frenet coordinate system, It is the conversion angle from the Frenet coordinate system to the geodetic coordinate system.

[0037] Preferably, the combined route following strategy based on distance and ground speed angle criteria comprises the following steps:

[0038] First, define the signs for determining the end and start of the mission: introduce distance and ground speed angle information as the mission end judgment, and set a threshold for the difference between the current point ground speed angle and the tangent vector angle of the last discrete point on the route, thereby reducing the reliance on a single distance criterion and making the determination of the mission end more reliable; the specific criteria are:

[0039]

[0040] Among them, P x ,P y is the current location information of the drone, W x ,W y is the desired route endpoint location information, θ P is the ground speed angle information of the current position of the UAV, is the tangent angle information of the desired route end point, d threshold is the distance threshold, θ threshold is the angle threshold;

[0041] Then the transition route is planned in real time and the position of the starting point of the transition route and the ground speed direction angle must be designed; the real-time planning Dubins transition curve is used to generate a transition route between two routes. The Dubins transition curve generation strategy process includes:

[0042] According to the mission discrete waypoints {(x1,y1,ψ1),(x2,y2,ψ2),…,(x n ,y n ,ψ n )} and the cruise missile aerodynamic constraints to obtain its own position and ground speed angle information P0(x0,y0,ψ0), P1(x1,y1,ψ1), the cruise missile aerodynamic constraints include the minimum turning radius R min , maximum roll angle φ max and airspeed Va;

[0043] Six Dubins routes under the calculation constraints are generated and the curve length is calculated; the calculation steps are: coordinate system transformation, calculation of the initial point, acquisition of the end point and calculation of the total length, and L = {L LSL ,L LSR ,LRSL ,L RSR ,L LRL ,L RLR};

[0044] After the calculation is completed, select the transition route L with the optimal curve length optimal =min{L LSL ,L LSR ,L RSR ,L RSR ,L LRL ,L RLR};

[0045] Based on the Frenet coordinate system, the discrete point information of the transition route is stored, and the discrete waypoints are linear differences. Where Δs is the length of each line segment, and the waypoint information is stored based on the Frenet coordinate system Where (x i ,y i ) represents the coordinate position, represents the unit tangent vector, Represents the unit normal vector.

[0046] Preferably, the TDE-PID speed vector controller is composed of a TDE-PID ground speed vector control outer loop, an attitude control inner loop, a rudder inner loop, a UAV dynamics model and an extended Kalman filter. The TDE-PID ground speed control outer loop receives a desired ground speed angle input φ c To the discrete system tracking differentiator, output the desired roll angle to the attitude control inner loop, x 11 、x 12 are the desired ground speed angle and the desired ground speed angular velocity after the discrete system tracking differentiator transition respectively, and the attitude control inner loop outputs the desired attitude angular velocity φ sp To the rudder inner loop, the rudder inner loop distributes the output signal uniformly, and then the servo mechanism outputs it to the corresponding actuator rudder mechanism, and the output of the servo is δ; the attitude control inner loop and the rudder inner loop in the overall control scheme directly use the mature algorithm of PX4 flight control, and the development work of the TDE-PID speed vector controller is concentrated on the speed vector control outer loop of the TDE-PID.

[0047] Preferably, the mathematical model of the discrete system tracking differentiator is:

[0048]

[0049] Where k represents the step size, x1 is the tracking signal, x2 is the differential signal, r determines the tracking speed, called the "speed factor", and h determines the filtering effect on noise, called the "filter factor". The expression of fhan(x1,x2,r,h) is:

[0050]

[0051] By defining the error e(k) = x1(k)-y out ,y out Represents the system output, and can use the tracking signal x1 and the differential signal x2 generated by tracking the input signal v to introduce a smooth transition mechanism in the control process, thereby effectively improving the contradiction between the system overshoot and rapid response when the input signal changes suddenly;

[0052] The TDE-PID ground speed vector control outer loop tracks the desired ground speed angle x1 output by TD and the variable x2 tracking the desired ground speed angular velocity, and compares it with the actual ground speed angle information. and actual ground speed and angular velocity information After making the difference, the desired roll angle is solved by using the PID control algorithm and input into the attitude control inner loop of the controller;

[0053] The TDE-PID velocity vector controller will obtain more accurate UAV velocity information V in the geodetic coordinate system through the onboard IMU sensor and GPS, and through Kalman filter data fusion. N 、V E ; Let ground speed angle X be the angle between the velocity vector and the due east direction. The calculation formula is as follows:

[0054]

[0055] The controller uses PID algorithm to realize the conversion from the expected ground speed information to the expected attitude angle information. First, by analyzing the force of the UAV during the tilting turn flight, it can be obtained:

[0056] mgtanφ=ma;

[0057] Among them, a is the lateral acceleration, φ is the roll angle, and the ground speed angle X after the change can be obtained new expression:

[0058]

[0059] Among them, a accel represents the lateral acceleration;

[0060] Therefore, the ground speed angle φ can be controlled by the roll angle φ; based on the tracking desired ground speed angle x1(t) and the tracking desired ground speed angular velocity x2(t) output by the tracking differentiator, a PID ground speed vector controller is designed to output the desired roll angle to the attitude control inner loop. The control algorithm expression is shown in the following formula:

[0061]

[0062] Among them, Kp is the proportional gain, Indicates the actual ground speed angle information, K i is the integral gain, K d is the differential gain, Indicates the actual ground speed and angular velocity information.

[0063] Therefore, the present invention adopts the above-mentioned fixed-wing UAV route following method based on velocity vector controller, and applies the FMC-VF route following algorithm to the developed TDE-PID ground speed vector controller, so that the UAV has the ability to accurately follow the complex desired curve route under ambient wind.

[0064] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 It is a schematic diagram of the working process of applying the FMC-VF route following algorithm of a fixed-wing UAV route following method based on a velocity vector controller to a TDE-PID ground speed vector controller;

[0066] Figure 2 It is a schematic diagram of a problem of constructing a discontinuous velocity vector field under a single normal angle criterion in an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0067] Figure 3 It is a schematic diagram of a velocity vector field endpoint phenomenon constructed based on a single distance criterion in an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0068] Figure 4 It is a schematic diagram of a velocity vector field constructed under the constraints of distance and normal angle in an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0069] Figure 5 It is a schematic diagram of a Dubins transition curve generation strategy of an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0070] Figure 6 It is a schematic diagram of a design scheme of a velocity vector controller of an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0071] Figure 7 It is a force analysis diagram of a UAV tilt turning flight according to an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention. DETAILED DESCRIPTION

[0072] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0073] Unless otherwise defined, technical or scientific terms used in the present invention shall have the common meanings understood by one having ordinary skills in the field to which the present invention belongs.

[0074] Embodiment 1

[0075] like Figure 1 As shown, the present invention provides a route following method suitable for fixed-wing UAVs. By applying the designed FMC-VF route following algorithm to the developed TDE-PID ground speed vector controller, the UAV is enabled to have the ability to accurately follow a complex desired curve route under ambient wind.

[0076] First, establish the overall method flow chart, the whole process of the method includes:

[0077] S1. Flight mission generation and route information input;

[0078] S2, velocity vector field construction, through the designed FMC-VF route following algorithm, the expected ground speed vector information of the UAV at the current position is generated;

[0079] S3, control quantity generation, through the designed TDE-PID speed vector controller, the desired servo deflection angle information is calculated and output to the fixed-wing UAV model.

[0080] The designed route following algorithm and ground speed vector controller are described in detail below.

[0081] The present invention first designs a VF (FMC-VF) route following algorithm under multiple constraints in the Frenet coordinate system. The following will describe the three methods used in detail:

[0082] The first one is the route information storage method based on the Frenet coordinate system: by decoupling the desired route in the tangent and normal directions, discrete waypoint information of the complex route is generated and stored.

[0083] Traditional VF route following algorithms only need to obtain the coordinate information of the starting point and the radius and length of the circular point for straight and circular routes, and most of them are described in the Cartesian coordinate system. In the Cartesian coordinate system, it is more complicated to analyze and process the curved route, which increases the complexity of algorithm design and implementation and the difficulty of motion control.

[0084] The Frenet coordinate system is established based on the reference trajectory. For any differentiable curve, a set of local orthogonal bases consisting of tangent and normal vectors are defined at each point of the curve, which can better describe the spatial variation characteristics of the target curve.

[0085] In the Frenet reference system, any point (x, y) in space can be represented by the reference Bezier curve arc length s and the track error l (s, l), which can be regarded as a linear combination of the horizontal unit vector and the vertical unit vector. Therefore, when facing the control task of route following, which is highly dependent on the tangential and normal information of the desired curve route, using Frenet to follow the target curve route has greater advantages.

[0086] The present invention adopts a discrete flight control system. Therefore, for a continuous curved desired route, a linear interpolation method is used to generate discrete points. The position information of adjacent discrete points is used to obtain their tangent and normal unit vectors, and a Frenet coordinate system based on the desired route is constructed, thereby completing the route information storage based on the Frenet coordinate system.

[0087] The second is a vector field construction algorithm based on distance and normal angle constraints: by finding the "related point" of the expected route at any position of the drone rather than the "closest point", the expected ground speed angle at the current position is calculated.

[0088] In order to control the rate of change of the constructed vector field, the traditional VF route following algorithm chooses to rely on the product of the attenuation factor k and the minimum distance d from the current position to the desired route for control. Therefore, it is only necessary to calculate the perpendicular distance from the position point to the desired straight line route, or the distance to the center of the desired circular route minus the radius.

[0089] When facing a curved route described by the Frenet coordinate system, the key to constructing the velocity vector field is to find the corresponding point on the desired curve at the current position. At this time, if the normal angle constraint method based on the Frenet coordinate system is selected, the strategy of finding the corresponding point is based on the angle between the line between the input point and the discrete point on the parameterized curve and the normal line of the discrete point. Assuming that the parameterized curve is r(s), and the discrete point on the curve is r(s i ), where i = 0, 1, ..., n, the input point is P, and the parameter value to be solved is t. Then the line connecting the input point and the corresponding discrete point on the curve is r(s i ) is the normal line The judgment basis can be expressed as follows:

[0090]

[0091] However, Figure 2 As shown, the vector field construction mechanism under this criterion is very likely to cause the problem of discontinuous changes in the constructed expected velocity vector when facing a complex curve with a fast changing curvature.

[0092] If we choose to rely solely on distance constraints to find the "nearest point" on the desired curve to construct the vector field:

[0093] min t |Pr(s i )| (i=t);

[0094] like Figure 3 As shown in the figure, although the constructed velocity vector field is continuous, when facing a multi-valued non-closed curve, the strategy cannot represent the expected velocity direction of the aircraft at these positions because the tangent direction at the endpoint in the area near the endpoint can no longer represent the expected velocity direction of the aircraft at these positions. Therefore, the "closest point" is only a theoretical corresponding point under the restriction of the curve endpoint, and its actual equivalent corresponding point is far away from the expected curve, resulting in a serious "endpoint phenomenon" (Endpoint Phenomenon) in the constructed velocity vector field, which in turn restricts the scope of application of the VF algorithm.

[0095] Therefore, based on the limitation of the mechanism of finding the corresponding points of the expected curve by using a single criterion, the present invention designs a vector field construction mechanism based on multiple constraints of distance and normal angle by using the expected route information stored in the Frenet coordinate system in the previous section: for all discrete points r(s) on the parameterized curve, find the s parameter value so that the distance from the input point P to the discrete point on the parameterized curve r(s) is the minimum under the condition that the line between the input point P and the discrete point on the parameterized curve r(s) is collinear with the normal line of the point on the curve. Therefore, the vector field construction algorithm based on distance and normal angle constraints is obtained as follows:

[0096] S2.1, find the relevant point r(s) of the desired route;

[0097]

[0098] Among them, min|Pr(s i )| represents the input point P to the discrete point r(s i ), subject to is used to limit the feasible domain of decision variables in optimization problems and ensure that decision variables meet specific conditions. The parameterized curve is r(s), and the discrete points on the curve are r(s i ), where i = 0, 1, ..., n, the input point is P, and the parameter value to be solved is t. Then the line connecting the input point and the corresponding discrete point on the curve is r(s i ) is the normal line

[0099] S2.2, calculate the expected ground speed vector of the position point;

[0100]

[0101] Among them, X d (i) is the desired ground speed angle, is the ground speed angle in the Frenet coordinate system, It is the conversion angle from the Frenet coordinate system to the geodetic coordinate system.

[0102] Under this vector field construction mechanism, the generated vector field is as follows Figure 4 As shown, it effectively solves the problems of discontinuity and "endpoint phenomenon" in the generated vector field, ensures the generation of ideal velocity vector field for multi-valued non-closed curves, and expands the scope of application for fixed-wing aircraft route following.

[0103] Therefore, the present invention adopts the above-mentioned fixed-wing UAV route following method based on velocity vector controller, and applies the FMC-VF route following algorithm to the developed TDE-PID ground speed vector controller, so that the UAV has the ability to accurately follow the complex desired curve route under ambient wind.

[0104] The third type is a combined route following strategy based on distance and ground speed angle criteria: generating a transition curve that smoothly connects the two mission routes to achieve a good following effect of the UAV on the combined route.

[0105] In actual UAV missions, there are often a large number of round-trip routes. If the UAV's expected speed direction in the entire map is described by a single generated velocity vector field, the accuracy is low. Therefore, the reasonable use of a segmented route combination strategy will help the UAV complete more complex route following tasks.

[0106] For the route following strategy, the steps are as follows:

[0107] First, define the signs for determining the end and start of the mission: introduce distance and ground speed angle information as the mission end judgment, and set a threshold for the difference between the current point ground speed angle and the tangent vector angle of the last discrete point on the route, reducing the reliance on a single distance criterion, making the determination of the end of the mission more reliable. The specific criteria are:

[0108]

[0109] Among them, P x ,P y is the current location information of the drone, W x ,W y is the desired route endpoint location information, θ P is the ground speed angle information of the current position of the UAV, is the tangent angle information of the desired route end point, d threshold is the distance threshold, θ threshold is the angle threshold;

[0110] In traditional mission planning, most of the time, the distance criterion is used, that is, the previous mission is judged to be over when the end point is close to a certain area. However, UAVs are easily affected by wind shear in the air above the mission, which causes large track errors in a short period of time. If the distance threshold dthreshold If the setting is too small, it may lead to failure to effectively judge the end of the task. threshold If the value is set too high, it is easy to enter the mission end zone by mistake and judge the mission end prematurely. This paper introduces distance and ground speed angle information as the mission end judgment. By setting a threshold for the difference between the current point ground speed angle and the tangent vector angle of the last discrete point on the route, the dependence on the distance judgment is reduced, making the judgment of the mission end more reliable.

[0111] Then the transition route is planned in real time and the position of the starting point of the transition route and the ground speed direction angle must be designed: the Dubins curve can plan the shortest smooth curve between two points considering the direction constraint in the form of a combination of arc segments and straight line segments. Therefore, the real-time planning of the Dubins curve is suitable for the generation strategy of the transition route between the two routes. The Dubins transition curve generation strategy process is as follows: Figure 5 As shown, it includes:

[0112] According to the mission discrete waypoints {(x1,y1,ψ1),(x2,y2,ψ2),…,(x n ,y n ,ψ n )} and the cruise missile aerodynamic constraints to obtain its own position and ground speed angle information P0(x0,y0,ψ0), P1(x1,y1,ψ1), the cruise missile aerodynamic constraints include the minimum turning radius R min , maximum roll angle φ max and airspeed Va.

[0113] Six Dubins routes under calculation constraints are generated and calculated. The calculation steps are: coordinate system transformation, calculation of the initial point, acquisition of the end point and calculation of the total length, and L = {L LSL ,L LSR ,L RSL ,L RSR ,L LSL ,L RLR};

[0114] After the calculation is completed, select the transition route L with the optimal curve length optimal =min{L LSL ,L LSR ,L RSL ,L RSR ,L LRL ,L RLR};

[0115] Based on the Frenet coordinate system, the discrete point information of the transition route is stored. The discrete waypoints are linear differences. Where Δs is the length of each line segment, and the waypoint information is stored based on the Frenet coordinate system Where (xi ,y i ) represents the coordinate position, represents the unit tangent vector, Represents the unit normal vector.

[0116] The present invention develops a route following method based on a velocity vector controller. First, the overall schematic diagram of the velocity vector controller is given, as shown in FIG. Figure 6 As shown. Among them, χ c is the desired ground speed angle, x 11 、x 12 are the desired ground speed angle and the desired ground speed angular velocity after the tracking differentiator transition, φ sp is the desired roll angle, and δ is the servo output.

[0117] like Figure 6 As shown in the figure, the TDE-PID ground speed control outer loop receives the desired ground speed angle input to the discrete system tracking differentiator, outputs the desired roll angle to the attitude control inner loop, and the attitude control inner loop outputs the desired attitude angular velocity to the rudder inner loop. After the rudder inner loop distributes the output signal uniformly, the servo mechanism outputs it to the corresponding actuator rudder mechanism. In the overall control scheme, the attitude control inner loop and the rudder inner loop directly affect the attitude stability of the aircraft, so the mature algorithm of PX4 flight control is directly used. The development work of the TDE-PID speed vector controller is mainly concentrated on the speed vector control outer loop. The design scheme of the speed vector controller based on TDE-PID will be described below.

[0118] The mathematical model of the discrete system tracking differentiator is:

[0119]

[0120] Where k represents the step size, x1 is the tracking signal, x2 is the differential signal, r determines the tracking speed, called the "speed factor", and h determines the filtering effect on noise, called the "filter factor". The expression of fhan(x1,x2,r,h) is:

[0121]

[0122] By defining the error e(k) = x1(k)-y out ,y out Represents the system output, and can use the tracking signal x1 and the differential signal x2 generated by tracking the input signal v to introduce a smooth transition mechanism in the control process, thereby effectively improving the contradiction between the system overshoot and rapid response when the input signal changes suddenly;

[0123] The outer loop of the TDE-PID ground speed vector control combines the tracking desired ground speed angle x1 and the variable x2 of the tracking desired ground speed angular velocity output by TD with the actual ground speed angle information and actual ground speed and angular velocity information After the difference is made, the desired roll angle is solved using the PID control algorithm and input into the attitude control inner loop of the controller; the key to the controller is to obtain accurate actual ground speed angle and actual ground speed angular velocity information and to use the ground speed information to complete the calculation of the desired attitude angle information.

[0124] The TDE-PID velocity vector controller will obtain more accurate UAV velocity information V in the geodetic coordinate system through the onboard IMU sensor and GPS, and through Kalman filter data fusion. N 、V E ; Let ground speed angle χ be the angle between the velocity vector and the due east direction, and the calculation formula is as follows:

[0125]

[0126] The controller uses PID algorithm to realize the conversion from expected ground speed information to expected attitude angle information; Figure 7 As shown in the figure, firstly, by analyzing the force of the UAV during tilting and turning flight, we can get:

[0127] mgtanφ=ma;

[0128] Among them, a is the lateral acceleration, φ is the roll angle, and the ground speed angle X after the change can be obtained new expression:

[0129]

[0130] Among them, a accel represents the lateral acceleration;

[0131] So far, it has been proved that the ground speed angle χ can be controlled by the roll angle φ; based on the tracking desired ground speed angle x1(t) and the tracking desired ground speed angular velocity x2(t) output by the tracking differentiator, a PID ground speed vector controller is designed to output the desired roll angle to the attitude control inner loop. The control algorithm expression is shown in the following formula:

[0132]

[0133] Among them, K p is the proportional gain, Indicates the actual ground speed angle information, K i is the integral gain, K d is the differential gain, Indicates the actual ground speed and angular velocity information.

[0134] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A fixed-wing UAV route following method based on a velocity vector controller, characterized in that: The following steps are involved: S1. Flight mission generation and route information input; S2, speed vector field construction: by designing the FMC-VF route following algorithm, the expected ground speed vector information of the UAV at the current position is generated; S3, control quantity generation: By designing the TDE-PID speed vector controller, the desired servo deflection angle information is calculated and output to the fixed-wing UAV model.

2. The fixed-wing UAV route following method based on a velocity vector controller according to claim 1, characterized in that: The FMC-VF route following algorithm includes: Route information storage method based on Frenet coordinate system: by decoupling the desired route in the tangent and normal directions, discrete waypoint information of complex routes is generated and stored; Vector field construction algorithm based on distance and normal angle constraints: by finding the relevant points of the desired route at any position of the drone instead of the closest point, the expected ground speed angle at the current position is calculated; Combined route following strategy based on distance and ground speed angle criteria: Generate a smooth transition curve connecting the two mission routes to achieve a good following effect of the UAV on the combined route.

3. A fixed-wing UAV route following method based on a velocity vector controller according to claim 2, characterized in that: The Frenet coordinate system in the route information storage method based on the Frenet coordinate system is established based on the reference trajectory. For any differentiable curve, a set of local orthogonal bases consisting of tangent and normal vectors are defined at each point of the curve. In the Frenet reference system, any point (x, y) in space can be represented by the arc length s of the reference Bezier curve and the track error l (s, l), which can be regarded as a linear combination of a horizontal unit vector and a vertical unit vector. For a continuous curve desired route, discrete points are generated by linear interpolation, and the tangent and normal unit vectors are obtained by using the position information of adjacent discrete points to construct the Frenet coordinate system based on the desired route, thereby completing the route information storage based on the Frenet coordinate system.

4. The fixed-wing UAV route following method based on a velocity vector controller according to claim 3 is characterized in that: The vector field construction algorithm based on distance and normal angle constraints is designed based on the expected route information stored in the Frenet coordinate system. A vector field construction mechanism under multiple constraints of distance and normal angle is designed: for all discrete points on the parameterized curve, the s parameter value is found so that the distance from the input point P to the discrete point on the parameterized curve is the minimum under the condition that the line between the input point P and the discrete point on the parameterized curve is collinear with the normal line of the point on the curve. Therefore, the vector field construction algorithm based on distance and normal angle constraints is: S2.1, find the relevant point r(s) of the desired route; Among them, min|Pr(s i )| represents the input point P to the discrete point r(s i ), subject to is used to limit the feasible domain of decision variables in optimization problems and ensure that decision variables meet specific conditions. The parameterized curve is r(s), and the discrete points on the curve are r(s i ), where i = 0, 1, ..., n, the input point is P, and the parameter value to be solved is t. Then the line connecting the input point and the corresponding discrete point on the curve is r(s i ) is the normal line S2.2, calculate the expected ground speed vector of the position point; Among them, χ d (i) is the desired ground speed angle, is the ground speed angle in the Frenet coordinate system, It is the conversion angle from the Frenet coordinate system to the geodetic coordinate system.

5. The fixed-wing UAV route following method based on a velocity vector controller according to claim 2, characterized in that: The combined route following strategy based on distance and ground speed angle criteria comprises the following steps: First, define the signs for determining the end and start of the mission: introduce distance and ground speed angle information as the mission end judgment, and set a threshold for the difference between the current point ground speed angle and the tangent vector angle of the last discrete point on the route, thereby reducing the reliance on a single distance criterion and making the determination of the mission end more reliable; the specific criteria are: Among them, P x ,P y is the current location information of the drone, W x ,W y is the desired route endpoint location information, θ P is the ground speed angle information of the current position of the UAV, is the tangent angle information of the desired route end point, d threshold is the distance threshold, θ threshold is the angle threshold; Then the transition route is planned in real time and the position of the starting point of the transition route and the ground speed direction angle must be designed; the real-time planning Dubins transition curve is used to generate a transition route between two routes. The Dubins transition curve generation strategy process includes: According to the mission discrete waypoints {(x1,y1,ψ1),(x2,y2,ψ2),…,(x n ,y n ,ψ n )} and the cruise missile aerodynamic constraints to obtain its own position and ground speed angle information P0(x0,y0,ψ0), P1(x1,y1,ψ1), the cruise missile aerodynamic constraints include the minimum turning radius R min , maximum roll angle φ max and airspeed Va; Six Dubins routes under the calculation constraints are generated and the curve length is calculated; the calculation steps are: coordinate system transformation, calculation of the initial point, acquisition of the end point and calculation of the total length, and L = {L LSL ,L LSR ,L RSL ,L RSR ,L LRL ,L RLR }; After the calculation is completed, select the transition route L with the optimal curve length optimal =min{L LSL ,L LSR ,L RSL ,L RSR ,L LRL ,L RLR }; Based on the Frenet coordinate system, the discrete point information of the transition route is stored, and the discrete waypoints are linear differences. Where Δs is the length of each line segment, and the waypoint information is stored based on the Frenet coordinate system Where (x i ,y i ) represents the coordinate position, represents the unit tangent vector, Represents the unit normal vector.

6. The fixed-wing UAV route following method based on a velocity vector controller according to claim 2, characterized in that: The TDE-PID speed vector controller consists of a TDE-PID ground speed vector control outer loop, an attitude control inner loop, a rudder inner loop, a UAV dynamics model and an extended Kalman filter. The TDE-PID ground speed control outer loop accepts the desired ground speed angle input x c To the discrete system tracking differentiator, output the desired roll angle to the attitude control inner loop, x 11 、x 12 are the desired ground speed angle and the desired ground speed angular velocity after the discrete system tracking differentiator transition respectively, and the attitude control inner loop outputs the desired attitude angular velocity φ sp To the rudder inner loop, the rudder inner loop distributes the output signal uniformly, and then the servo mechanism outputs it to the corresponding actuator rudder mechanism, and the output of the servo is δ; the attitude control inner loop and the rudder inner loop in the overall control scheme directly use the mature algorithm of PX4 flight control, and the development work of the TDE-PID speed vector controller is concentrated on the speed vector control outer loop of the TDE-PID.

7. A fixed-wing UAV route following method based on a velocity vector controller according to claim 6, characterized in that: The mathematical model of the discrete system tracking differentiator is: Where k represents the step size, x1 is the tracking signal, x2 is the differential signal, r determines the tracking speed, called the "speed factor", and h determines the filtering effect on noise, called the "filter factor". The expression of fhan(x1,x2,r,h) is: By defining the error e(k) = x1(k)-y out ,y out Represents the system output, and can use the tracking signal x1 and the differential signal x2 generated by tracking the input signal v to introduce a smooth transition mechanism in the control process, thereby effectively improving the contradiction between the system overshoot and rapid response when the input signal changes suddenly; The TDE-PID ground speed vector control outer loop tracks the desired ground speed angle x1 output by TD and the variable x2 tracking the desired ground speed angular velocity, and compares it with the actual ground speed angle information. and actual ground speed and angular velocity information After making the difference, the desired roll angle is solved by using the PID control algorithm and input into the attitude control inner loop of the controller; The TDE-PID velocity vector controller will obtain more accurate UAV velocity information V in the geodetic coordinate system through the onboard IMU sensor and GPS, and through Kalman filter data fusion. N 、V E ; Let ground speed angle χ be the angle between the velocity vector and the due east direction, and the calculation formula is as follows: The controller uses PID algorithm to realize the conversion from the expected ground speed information to the expected attitude angle information. First, by analyzing the force of the UAV during the tilting turn flight, it can be obtained: mgtanφ=ma; Among them, a is the lateral acceleration, φ is the roll angle, and the ground speed angle X after the change can be obtained new expression: Among them, a accel represents the lateral acceleration; Therefore, the ground speed angle φ can be controlled by the roll angle φ; based on the tracking desired ground speed angle x1(t) and the tracking desired ground speed angular velocity x2(t) output by the tracking differentiator, a PID ground speed vector controller is designed to output the desired roll angle to the attitude control inner loop. The control algorithm expression is shown in the following formula: Among them, K p is the proportional gain, Indicates the actual ground speed angle information, K i is the integral gain, K d is the differential gain, Indicates the actual ground speed and angular velocity information.

Images