A route following method for fixed-wing UAV based on velocity vector controller

Abstract

This invention discloses a fixed-wing UAV route-following method based on a velocity vector controller, belonging to the field of UAV control technology. The method comprises the following steps: S1, generating a flight mission and inputting route information; S2, constructing a velocity vector field: generating the desired ground speed vector information of the UAV at the current position by designing an FMC-VF route-following algorithm; and S3, generating a control variable: calculating the desired servo deflection angle information by designing a TDE-PID velocity vector controller and outputting it to the fixed-wing UAV model. This invention adopts the aforementioned 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, the UAV is able to accurately follow a complex desired curved route in ambient wind conditions.

Description

Technical Field

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

[0002] Small fixed-wing drones (UAVs) utilize the lift generated by their wings to fly, allowing for longer flight times and ranges. They play a vital role in civilian applications such as environmental monitoring, disaster relief, and long-distance transportation. In actual low-altitude flight missions, obstacles such as hills, mountains, and even skyscrapers are common, requiring UAVs to possess precise route-following capabilities. Furthermore, small UAVs at low altitudes often encounter low-altitude wind shear, which varies in magnitude and direction and can easily cause them to deviate from their desired route. This necessitates robustness to unpredictable wind disturbances during the UAV's route-following process.

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

[0004] (1) The Pure Pursuit (PP) route following algorithm introduces a virtual target point (VTP) to guide the aircraft to chase the VTP, ultimately making the UAV 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 drone’s trajectory converge to the desired path by making the drone’s 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 aircraft heading angle.

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

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

[0012] The PLOS guidance law is stable when k1 > 0 and k2 > 0. A larger k1 value prioritizes convergence of the aircraft's bearing angle to the LOS, which slows down lateral error convergence. A larger k2 value prioritizes convergence of the aircraft's lateral error, which can easily cause track oscillation.

[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 route at two points, s and s' (the choice depends on the direction of the UAV's movement). The required lateral acceleration can then be calculated using the angular deviation.

[0014]

[0015] Among them, v a is the aircraft airspeed, and η is the difference between the desired route inclination and the current heading angle of the drone. The advantage of this algorithm is that it can be flexibly applied to any desired route. Experiments have shown that this route-following algorithm remains robust under varying airspeed conditions.

[0016] (5) The route-following algorithm based on velocity vector fields (VF) uses the velocity vector field to indicate the airflow direction at each position in space. As long as the aircraft is guided along the desired velocity vector direction at each spatial position, its final flight trajectory will converge to the desired route. Furthermore, since the algorithm can introduce the known magnitude and direction of natural wind when generating the velocity vector field, it can achieve good tracking results for the desired route in the presence of constant wind disturbances.

[0017] Most existing route-following algorithms 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] Because fixed-wing aircraft typically use attitude controllers, direct flight direction control is difficult, resulting in poor wind resistance. Currently, wind field estimation and trajectory error compensation are the primary approaches to improving UAVs' wind resistance. However, this approach remains effective in the face of unpredictable strong wind disturbances. Fixed-wing UAVs that use velocity vector control often experience large controller output oscillations and a chattering effect, characterized by jitter in the aircraft's attitude, after trajectory convergence.

[0019] Currently, the most widely used control method for optimizing fixed-wing UAV velocity vector controllers is the traditional proportional-integral-derivative (PID) control. Its simple structure, wide applicability, easy parameter adjustment, strong flexibility, and excellent control performance have earned it widespread application. However, its control logic, which relies on error feedback to eliminate errors, leads to a persistent conflict between "fast response" and "overshoot" in closed-loop systems. Faced with highly maneuverable UAVs and factors such as nonlinearity and uncertainty, PID control's inherent flaws limit its further application and development. In recent years, with the advancement of chip computing power, modern control methods based on state-space control, represented by sliding-mode variable structure control, robust control, backstepping control, and intelligent control, have made significant progress. However, while these model-based control methods improve control performance, they also sacrifice the simplicity and flexibility of PID control, increasing the difficulty and cost of controller development. 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, the UAV is able to accurately follow a complex desired curved route under ambient wind conditions.

[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. Velocity 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 variable generation: By designing a TDE-PID velocity 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 good UAV following of 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 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 the horizontal unit vector and the vertical unit vector; for the desired route of a continuous curve, discrete points are generated by linear interpolation, and the position information of adjacent discrete points is used to obtain its tangent and normal unit vectors, and the Frenet coordinate system based on the desired route is constructed, thereby completing the route information storage based on the Frenet coordinate system.

[0030] Preferably, the fixed-wing UAV route following method based on a velocity vector controller is characterized in that: the vector field construction algorithm based on distance and normal angle constraints is designed based on the desired route information stored in the Frenet coordinate system, and a vector field construction mechanism based on 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 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 points 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 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 at 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, 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 includes 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. By setting a threshold for the difference between the current ground speed angle and the tangent vector angle of the last discrete point on the route, the reliance on a single distance criterion is reduced, making the determination of mission end more reliable. The specific criteria are:

[0039]

[0040] Among them, P x ,P y is the current location information of the UAV, 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 tangential angle information of the desired route end point, d threshold is the distance threshold, θ threshold is the angle threshold;

[0041] Then, a transition route is planned in real time, and the position of the starting point and the ground speed direction angle of the transition route must be designed. A 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 constraints are calculated to generate and calculate the curve length; 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 velocity 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 the 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, 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 servo output is δ; the attitude control inner loop and the rudder inner loop in the overall control scheme directly use the mature PX4 flight control algorithm, and the development work of the TDE-PID speed vector controller is concentrated on the TDE-PID speed vector control outer loop.

[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 and is called the "speed factor", and h determines the filtering effect on noise and is called the "filter factor". The expression fhan(x1,x2,r,h) is:

[0050]

[0051] By defining the error e(k) = x1(k)-y out ,y out Representing the system output, it is possible to 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 suddenly changes;

[0052] The TDE-PID ground speed vector control outer loop will track the desired ground speed angle x1 and the variable x2 of the desired ground speed angular velocity output by TD, and the actual ground speed angle information and actual ground speed and angular velocity information After making the difference, the desired roll angle is solved 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 the PID algorithm to convert the desired ground speed information into the desired attitude angle information. First, by analyzing the force on the UAV during banked turning flight, we can obtain:

[0056] mgtanφ=ma;

[0057] Where a is the lateral acceleration and φ is the roll angle, from which 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 as follows:

[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 conditions.

[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 This is a schematic diagram of the workflow of applying the FMC-VF route following algorithm to the TDE-PID ground speed vector controller in an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

[0066] Figure 2 This is a schematic diagram of a velocity vector field discontinuity problem constructed based on 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 This is a schematic diagram of a velocity vector field endpoint phenomenon constructed under 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 This is a schematic diagram of a velocity vector field constructed under distance and normal angle constraints in an embodiment of a fixed-wing UAV route following method based on a velocity vector controller of the present invention;

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

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

[0071] Figure 7 This is a force analysis diagram of a UAV tilt-turn 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 with reference to the accompanying drawings and embodiments.

[0073] Unless otherwise defined, technical or scientific terms used in the present invention shall have the same meaning as commonly understood by one of ordinary skill in the art to which the present invention belongs.

[0074] Example 1

[0075] like Figure 1 As shown in the figure, 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 accurately follow a complex desired curved route under ambient wind conditions.

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

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

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

[0079] S3, control quantity generation, through the designed TDE-PID velocity 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 multi-constrained VF (FMC-VF) route following algorithm in the Frenet coordinate system. The following describes 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 path-following algorithms for linear and circular routes only require the coordinates of the starting point and the radius and length of the circle, respectively, and are typically described in a Cartesian coordinate system. However, analyzing and processing curved routes in a Cartesian coordinate system is more complex, increasing 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 is defined at each point of the curve, which can better describe the spatial variation characteristics of the target curve.

[0085] In the Frenet reference frame, any point (x, y) in space can be represented by the arc length s of a reference Bezier curve and the track error l, representing the coordinates (s, l). This can be viewed as a linear combination of a transverse unit vector and a longitudinal unit vector. Therefore, using Frenet for target curve following offers significant advantages in control tasks that rely heavily on information about the tangent and normal directions of the desired curve.

[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 method is a vector field construction algorithm based on distance and normal angle constraints: by finding the "related point" of the desired 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 faced with 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 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 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, if Figure 2 As shown in Figure 3, the vector field construction mechanism under this criterion is very likely to cause discontinuous changes in the constructed expected velocity vector when facing a complex curve with rapidly 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 this strategy faces a multi-valued non-closed curve, since 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, the "nearest point" is only a theoretical corresponding point under the curve endpoint restriction, 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 limitations of the above-mentioned mechanism of using a single criterion to find the corresponding points of the expected curve, the present invention uses the expected route information stored in the Frenet coordinate system in the previous section to design a vector field construction mechanism based on multiple constraints of distance and normal angle: for all discrete points r(s) on the parameterized curve, find the s parameter value so that the distance between the input point P and the discrete points on the parameterized curve r(s) is minimized under the condition that the line connecting the input point P and the discrete points 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:

[0096] S2.1. Find the relevant points 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 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 at 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, 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 in the figure, 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 conditions.

[0104] The third type is a combined route following strategy based on distance and ground speed angle criteria: a transition curve is generated to smoothly connect the two mission routes, enabling the UAV to follow the combined route well.

[0105] In actual UAV missions, there are often a large number of round-trip routes. If the UAV's expected velocity direction in the entire map is described by relying on a single generated velocity vector field, the accuracy is low. Therefore, the reasonable adoption of a segmented route combination strategy can 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. 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 reliance on a single distance criterion is reduced, making the judgment of the mission end more reliable. The specific criteria are:

[0108]

[0109] Among them, P x ,P y is the current location information of the UAV, 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 tangential 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 Setting it too high can easily lead to the flight entering the mission end zone and prematurely declaring the mission over. This paper introduces distance and ground speed angle information as a mission end judgment. By setting a threshold for the difference between the current ground speed angle and the tangent vector angle of the last discrete point on the route, it reduces the reliance on distance judgment and makes the mission end judgment 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 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] Calculate the six Dubins routes under the constraints and generate and calculate the curve length. The calculation steps are: coordinate system transformation, calculation of the initial point, acquisition of the end point and calculation of the total length, and obtain 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, the TDE-PID ground speed control outer loop receives the desired ground speed angle as input to the discrete system tracking differentiator, outputting the desired roll angle to the attitude control inner loop. The attitude control inner loop then outputs the desired attitude angular velocity to the rudder inner loop. The rudder inner loop then distributes the output signals, which are then fed by the servo mechanism to the corresponding actuator control surface mechanisms. Because the attitude control inner loop and the rudder inner loop in the overall control scheme directly affect the aircraft's attitude stability, the mature PX4 flight control algorithms are directly used. Development work on the TDE-PID velocity vector controller primarily focuses on the velocity vector control outer loop. The following describes the design of a velocity vector controller based on TDE-PID.

[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 and is called the "speed factor", and h determines the filtering effect on noise and is called the "filter factor". The expression fhan(x1,x2,r,h) is:

[0121]

[0122] By defining the error e(k) = x1(k)-y out ,y out Representing the system output, it is possible to 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 suddenly changes;

[0123] The outer loop of TDE-PID ground speed vector control compares the tracking desired ground speed angle x1 and the tracking desired ground speed angular velocity variable x2 output by TD 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 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. The calculation formula is as follows:

[0125]

[0126] The controller uses PID algorithm to realize the conversion from the desired ground speed information to the desired 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] Where a is the lateral acceleration and φ is the roll angle, from which the ground speed angle X after the change can be obtained new expression:

[0129]

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

[0131] Thus far, it has been proved that the ground speed angle χ can be controlled by the roll angle φ. Based on the tracking differentiator output of the desired ground speed angle x1(t) and the desired ground speed angular velocity x2(t), 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 as follows:

[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 solutions of the present invention rather than to limit the same. 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 solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A fixed-wing UAV route following method based on a velocity vector controller, characterized by: The following steps are involved: S1. Flight mission generation and route information input; S2. Velocity 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; 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: Generates a smooth transition curve connecting the two mission routes, enabling the drone to follow the combined route effectively. S3. Control variable generation: By designing a TDE-PID velocity vector controller, the desired servo deflection angle information is calculated and output to the fixed-wing UAV model.

2. A fixed-wing UAV route following method based on a velocity vector controller according to claim 1, 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 is defined at each point of the curve. In the Frenet reference system, any point in space is Can be determined by the arc length of the reference Bezier curve and track error Indicated , which can be regarded as a linear combination of the horizontal unit vector and the vertical unit vector; for the desired route of a continuous curve, 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.

3. The fixed-wing UAV route following method based on a velocity vector controller according to claim 2, 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 based on multiple distance and normal angle constraints is designed: for all discrete points on the parameterized curve, the s parameter value is found so that the distance between the input point P and the discrete point on the parameterized curve is minimized under the condition that the line connecting 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 relevant points on the desired route ; ； in, Represents the input point P to discrete points The minimum distance, It is used to limit the feasible domain of decision variables in optimization problems and ensure that the decision variables meet specific conditions. The parameterized curve is , the discrete points on the curve are ,in , the input point is P, the parameter value to be solved is t, then the line connecting the input point and the corresponding discrete point on the curve is , The normal line is ; S2.

2. Calculate the expected ground speed vector at the position point; ； in, is the ground speed angle in the Frenet coordinate system, is the conversion angle from the Frenet coordinate system to the geodetic coordinate system.

4. The fixed-wing UAV route following method based on a velocity vector controller according to claim 1, characterized in that: The combined route following strategy based on distance and ground speed angle criteria includes 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. By setting a threshold for the difference between the current ground speed angle and the tangent vector angle of the last discrete point on the route, the reliance on a single distance criterion is reduced, making the determination of mission end more reliable. The specific criteria are: ； Among them, P x ,P y is the current location information of the UAV, W x ,W y is the desired route endpoint location information, is the ground speed angle information of the current position of the UAV, is the tangential angle information of the desired route end point, is the distance threshold, is the angle threshold; Then, a transition route is planned in real time, and the position of the starting point and the ground speed direction angle of the transition route must be designed. A real-time planning Dubins transition curve is used to generate a transition route between two routes. The Dubins transition curve generation strategy process includes: Discrete waypoints according to tasks { } and cruise missile aerodynamic constraints to obtain its own position and ground speed angle information The aerodynamic constraints of the loitering missile include the minimum turning radius , maximum roll angle and airspeed ; Calculate the six Dubins routes under the constraints, generate and calculate the curve length; the calculation steps are: coordinate system transformation, calculate the initial point, obtain the end point and calculate the total length, and get ; After the calculation is completed, select the transition route with the optimal curve length ; The discrete point information of the transition route is stored based on the Frenet coordinate system, and the discrete waypoints are linearly interpolated. ,in The length of each line segment, waypoint information is stored based on the Frenet coordinate system ,in Indicates the coordinate position, represents the unit tangent vector, Represents the unit normal vector.

5. The fixed-wing UAV route following method based on a velocity vector controller according to claim 1, characterized in that: The TDE-PID velocity 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 vector control outer loop accepts the desired ground speed angle input. To the discrete system tracking differentiator, output the desired roll angle to the attitude control inner loop, are the desired ground speed angle and the desired ground speed angular velocity after the discrete system tracking differentiator transition, and the attitude control inner loop outputs the desired attitude angular velocity To the rudder inner ring, the rudder inner ring distributes the output signal uniformly, and the servo mechanism outputs it to the corresponding actuator rudder mechanism. 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 PX4 flight control algorithm, and the development work of the TDE-PID speed vector controller is concentrated on the TDE-PID ground speed vector control outer loop.

6. The fixed-wing UAV route following method based on a velocity vector controller according to claim 5, characterized in that: The mathematical model of the discrete system tracking differentiator is: ； in Indicates step length ， To track the signal, is the differential signal, Determines the tracking speed, called the "speed factor"; Determines the filtering effect on noise, called "filter factor"; The expression is: ； By defining the error , Represents the system output, which can be used to track the input signal Generated tracking signal and the differential signal ,Introducing a smooth transition mechanism in the control process, thereby effectively improving the contradiction between system overshoot and rapid response when the input signal changes suddenly; The TDE-PID ground speed vector control outer loop will track the desired ground speed angle output by TD and the variables that track the desired ground speed angular velocity , and the actual ground speed angle information and actual ground speed and angular velocity information After making the difference, the desired roll angle is solved 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 speed information in the geodetic coordinate system through the onboard IMU sensor and GPS, and through Kalman filter data fusion. ;Record ground speed angle is the angle between the velocity vector and the due east direction, and the calculation formula is as follows: ； The controller uses the PID algorithm to convert the desired ground speed information into the desired attitude angle information. First, by analyzing the force on the UAV during banked turning flight, we can obtain: ； in, is the lateral acceleration, is the roll angle, from which the ground speed angle after the change can be obtained expression: ； in, represents the lateral acceleration; Therefore, the roll angle Control ground speed angle ; Tracking desired ground speed angle based on tracking differentiator output and track the desired ground speed angular velocity , design a PID ground speed vector controller to output the desired roll angle to the attitude control inner loop. The control algorithm expression is shown as follows: ； in, is the proportional gain, Indicates the actual ground speed angle information, is the integral gain, is the differential gain, Indicates the actual ground speed and angular velocity information.

Images