Track control strategy of unmanned aerial vehicle

Abstract

The invention discloses a trajectory control strategy of an unmanned aerial vehicle, belongs to the technical field of unmanned aerial vehicle flight control, and aims at changing the flight attitude and height in the flight stage of the unmanned aerial vehicle, ensuring that the unmanned aerial vehicle is in an expected flight path and attitude, and ensuring full-process ordered management and stable and standard flight. Advantages and effects are realized through multi-stage accurate control; accurate setting parameters are initialized, and a foundation is laid for follow-up flight; all flight stages are switched according to conditions in sequence, postures and tracks are flexibly adjusted in the stages of level flight, inclination, pulling up and the like, and control modes are switched according to needs to improve adaptability; a propeller safety protection mode is introduced, unnecessary operation instructions are automatically cut off under the sudden airflow or abnormal condition, attitude stability is preferentially guaranteed, and the problems that in the prior art, environment adaptability is poor, and control strategy generalization ability is insufficient are effectively solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) flight control, and particularly relates to a trajectory control strategy for a UAV. Background Art

[0002] With the development of robotics technology, UAVs have been widely used in many fields, and their precise trajectory control has become crucial. As an underactuated system, a quadrotor UAV has limitations in describing attitude control based on Euler angles, rotation matrices, and quaternions. In actual tasks, UAVs often need to plan trajectories, and the working environment is complex, with high requirements for trajectory control.

[0003] Currently, the common trajectory control technologies mainly include: (1) Linear Quadratic Regulator (LQR) algorithm: A classical optimal control algorithm. The basic idea is to weight the system state and control input to minimize the system performance index. In UAV trajectory control, the UAV state can be weighted to achieve control, which is divided into two steps: state feedback (calculating the control input based on the difference between the current state of the UAV and the target trajectory) and state estimation (estimating the UAV state using a Kalman filter to assist state feedback). In actual applications, it needs to be adjusted according to the UAV model and task requirements, and the influence of dynamics, control limitations, and environmental factors also needs to be considered; (2) Trajectory planning method based on the maximum a posteriori probability model: It involves the control of multi-rotor UAVs and can enable small quadrotor UAVs to navigate autonomously in unknown and complex environments. However, there are still some technical problems with the current trajectory control technologies: During actual flight, environmental factors such as wind speed and air pressure will affect the UAV, increasing the difficulty of trajectory control. These factors need to be considered and adjusted accordingly in the algorithm and control strategy; The differences between different UAV models and task requirements are large, and the existing control algorithms need to be adjusted according to specific situations, with insufficient generality and difficulty in being directly applied to various scenarios. Summary of the Invention

[0004] The present invention provides a trajectory control strategy for a UAV, which changes the flight attitude and altitude during the flight stage of the UAV to ensure that the UAV is on the desired flight path and attitude, and manages the whole process in an orderly manner to ensure stable and regular flight.

[0005] To achieve the above objectives, the present invention adopts the following technical solutions:

[0006] A trajectory control strategy for a UAV includes the following steps:

[0007] S1: Set the initial values of the key parameters and variables for flight control. The key parameters for flight control include lateral and directional guidance, longitudinal guidance, pitch stability augmentation, roll stability augmentation, and yaw stability augmentation control modes. Input the initial thrust, initial overload, initial attitude angle, and initial angle of attack to lay the foundation for subsequent flight, ensure that the system is ready, maintain the horizontal and stable flight of the UAV, fly smoothly according to the set key parameters for flight control, and maintain the basic flight state;

[0008] S2: Turn off the longitudinal guidance mode and the lateral and directional guidance mode. The pitch stability augmentation control changes to angle of attack control to change the flight attitude of the UAV and prepare for operations such as turning. Introduce the thruster to cooperate with the tilting operation, adjust the tilting angle of the fuselage to change the flight direction, and increase the flight altitude of the UAV for adjusting to a specific altitude to perform tasks. Turn on the thruster to provide power to meet the power requirements such as acceleration and changing the flight state;

[0009] S3: The pitch stability augmentation control mode changes to longitudinal acceleration control. The thruster assists in realizing the turning action of the UAV. Combine the tilting and turning operations to make it change the flight direction according to the plan;

[0010] S4: Adjust the track inclination command and the longitudinal acceleration command to calibrate the flight direction and the horizontal state to ensure that the UAV returns to or maintains the desired horizontal flight path and attitude;

[0011] S5: Set the final flight control parameters and update the command output variables to make the UAV reach the final stable flight state required by the task.

[0012] Beneficial effects: The present invention provides a trajectory control strategy for a UAV, which achieves advantageous effects through multi-stage precise control; accurately set the parameters during initialization to lay the foundation for subsequent flight; switch in sequence according to conditions in each flight stage, and flexibly adjust the attitude and trajectory in stages such as level flight, tilting, and pulling up, and switch the control mode as needed to improve adaptability; the thruster is precisely regulated through a special function, and the command bits are finely operated according to specific conditions to ensure appropriate power supply, and the whole process is managed orderly to ensure stable and regular flight. The present invention effectively solves the problems of environmental interference and versatility, real-time senses the flight state (such as sideslip angle, airspeed, altitude), automatically adjusts the control parameters, introduces a thruster safety protection mode, and automatically truncates unnecessary operation commands in case of sudden airflow or abnormalities, giving priority to ensuring attitude stability, significantly improving the robustness and cross-platform applicability of the UAV trajectory control, and effectively solving the problems of poor environmental adaptability and insufficient generalization ability of control strategies in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 It is a schematic diagram of the trajectory control process in the embodiment of the present invention;

[0014] Figure 2Schematic diagram of the control method in the first stage of the embodiment of the present invention, where a is the lateral and directional control mode, b is the longitudinal control mode, c is the pitch stability augmentation control mode, and d is the yaw stability augmentation control mode;

[0015] Figure 3 Schematic diagram of the changes of each parameter in the first stage of the embodiment of the present invention, where a is the roll angle command, b is the roll angular velocity command, c is the angle of attack command, d is the airspeed command, e is the altitude command, and f is the yaw angle command;

[0016] Figure 4 Schematic diagram of the changes of each parameter in the second stage of the embodiment of the present invention, where a is the lateral and directional guidance mode, b is the longitudinal guidance mode, c is the pitch stability augmentation control mode, d is the angle of attack command, e is the thruster command, and f is the ground speed;

[0017] Figure 5 Schematic diagram of the changes of each parameter in the third stage of the embodiment of the present invention, where a is the pitch stability augmentation control mode, b is the roll angle command, c is the longitudinal acceleration command, and d is the thruster command;

[0018] Figure 6 Schematic diagram of the changes of each parameter in the fourth stage of the embodiment of the present invention, where a is the flight path angle and b is the longitudinal acceleration command;

[0019] Figure 7 Schematic diagram of the changes of each parameter in the fifth stage of the embodiment of the present invention, where a is the lateral and directional control mode, b is the longitudinal control mode, c is the pitch stability augmentation control mode, d is the yaw stability augmentation control mode, e is the airspeed command, and f is the throttle command. Detailed implementation manner

[0020] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments:

[0021] As Figure 1 shown, a trajectory control strategy for an unmanned aerial vehicle divides the trajectory control into the following five stages:

[0022] The first stage: In the lateral and directional guidance, roll is used to control yaw, in the longitudinal guidance, pitch angle is used to control altitude, the pitch stability augmentation control mode is pitch angle control, the roll stability augmentation control mode is roll control, the yaw stability augmentation control is lateral acceleration control, the automatic throttle is set to the command mode, the throttle command is 1, and the longitudinal acceleration is -9.8 m / s 2, the roll angular velocity is 0 rad / s, the roll angle is 0 rad, the angle of attack command = 0, the counter count is 0, the initial angle of attack is 0 rad. Input the thruster command, and the maneuver command, radar command, radar command sequence, image command, and image command sequence use the commands of the previous beat. Set the values of the altitude, airspeed, and yaw angle measured by the sensor as the initial values in the orbit control process in real time. Until 200 milliseconds later, the counter count increases by one every second. The initial angle of attack becomes the initial angle of attack of the previous beat plus the real-time angle of attack of the sensor. After one second, it enters the stage of the UAV tilting and pulling up;

[0023] The method of controlling the yaw angle with the roll angle in the lateral and longitudinal guidance is shown in the following formula:

[0024]

[0025] where, φ c is the roll angle command, V g is the ground speed, g is the acceleration due to gravity, ω as is the heading angle deviation proportional control gain in the roll angle correction side deviation mode, ψ c is the yaw angle command, ψt is the yaw angle, is the heading angle rate of the planned flight path.

[0026] The method of controlling the altitude with the pitch angle in the longitudinal guidance is shown in the following formula:

[0027] θ c = k p (h d - h) + k d (hdot d - hdot) + k i ∫(h d - h)dt

[0028] where, θ c is the pitch angle command, h d , h are the target altitude and the current altitude respectively, hdot d , hdot are the target lift / drop rate and the current lift / drop rate respectively, k p , k i , k d are the PID parameters.

[0029] The method of controlling with the lateral acceleration in the yaw stability augmentation control is shown in the following formula:

[0030] a y = k r r + k φ φ

[0031] where, a y is the lateral acceleration, k ris the contribution coefficient of yaw rate to lateral acceleration, k φ is the contribution coefficient of the roll angle to the lateral acceleration, r The yaw rate, φ is the roll angle.

[0032] like Figure 2 and Figure 3 The control method and parameter changes of the first stage are shown. Figure 1 The control method in the above is consistent with the control method described in the first stage. a The lateral heading guidance uses roll to control yaw, b The longitudinal guidance uses pitch angle to control altitude, c The pitch augmentation control mode is pitch angle control, d The yaw augmentation control is lateral acceleration control; Figure 2 Among them, a roll angle command is 0 rad, b roll angular velocity command is 0 rad / s, c angle of attack command is 0 rad, d airspeed command is 120m / s, e altitude command is 5000m, and f yaw angle command is 1.34rad.

[0033] The second stage: the initial angle of attack is changed to the initial angle of attack of the previous shot divided by the technical value of the counter. The longitudinal guidance mode is turned off, the pitch stabilization control is changed to the angle of attack control, the lateral heading guidance mode is turned off, and the input of the thruster command is converted into a 3-bit binary mask and assigned to the thruster command. The 3 bits correspond to different control commands of different thrusters, which are used to adjust the working state of the thrusters to cooperate with the tilt operation. The angle of attack command is the initial angle of attack of this shot. After 2.5 seconds, the thruster command is input to facilitate the aircraft to pull up. The angle of attack is changed to make the aircraft pull up. The angle of attack command is calculated as follows:

[0034]

[0035] α c <0rad,α c =0rad

[0036] α c >0.26rad,α c =0.26rad

[0037] In the formula, αc is the angle of attack command, n is the longitudinal overload, and g is the acceleration of gravity, which is 9.8 m / s here. 2 , Z a is the derivative parameter of the track inclination angle to the angle of attack, V g is the ground speed, α0 is the initial angle of attack;

[0038] like Figure 4As shown, a lateral course guidance mode is turned off, b longitudinal guidance mode is turned off, c pitch stability augmentation control becomes angle of attack control, d angle of attack command is 0.04 rad, e ground speed is between 188 m / s and 189 m / s, f thruster command is 4; one second after pulling up, the thruster command is input, and three seconds later, it enters the turning and banking stage.

[0039] Third stage: The pitch stability augmentation control mode becomes longitudinal acceleration control, and the longitudinal acceleration command is 0 m / s 2 , the roll angle command is 0 rad, the third digit of the thruster command is set to 0, and the first digit is set to 0 to facilitate thruster-assisted turning and banking, Figure 5 For the third stage, a the pitch stability augmentation control mode becomes longitudinal acceleration control, b the roll angle command is 0 rad, c the longitudinal acceleration command is 0 m / s 2 , d the thruster command is 0, and when the absolute value of the roll angle is less than 0.1 rad or five seconds later, it enters the horizontal adjustment stage.

[0040] Fourth stage: The flight path angle command and the longitudinal acceleration command are as shown in the following formula:

[0041]

[0042] γ c >0 rad, γ c =0 rad

[0043] γ c <-1 rad, γ c =-1 rad

[0044] az c =-9.8·cosγ+(γ - γ c )·V g

[0045]

[0046] az c <-35 m / s 2 , az c =-35 m / s 2

[0047] az c >0 m / s 2 , az c =0 m / s 2

[0048] In the formula, γ c is the flight path angle command, V0 is the initial indicated airspeed, V is the current indicated airspeed, g is the acceleration due to gravity, which is taken as 9.8 m / s 2 , az cis the longitudinal acceleration command, γ is the flight path inclination angle, Q is the dynamic pressure, S is the wing reference area, and CL max is the maximum lift coefficient, and m is the mass of the UAV.

[0049] In this stage, the flight path inclination command and the normal acceleration command are continuously adjusted until the absolute value of the flight path inclination angle is less than 0.1 rad and after checking for one second, or after ten seconds, it enters the final stage. Figure 6 is the fourth stage, the flight path inclination command a is 0 rad, and the longitudinal acceleration command b is 0 m / s 2 .

[0050] Fifth stage: The lateral and directional guidance is turned on and the lateral guidance is controlled by the roll angle, the longitudinal guidance is turned on and the longitudinal guidance is controlled by the pitch angle. The pitch, roll, and yaw stability augmentation controls become the control modes in the first stage. The automatic throttle is set to airspeed control, and the airspeed command is the cruise speed (120 m / s). Figure 7 is the fifth stage, a the lateral and directional guidance is turned on and the lateral guidance is controlled by the roll angle, b the longitudinal guidance is turned on and the longitudinal guidance is controlled by the pitch angle, the pitch c and yaw d stability augmentation controls become the control modes in the first stage, and f the automatic throttle is set to airspeed control.

[0051] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A trajectory control strategy for an unmanned aerial vehicle, characterized in that: The following steps are involved: S1: Set the key flight control parameters and initial values ​​of variables. Roll control is used for lateral guidance and yaw control is used for longitudinal guidance. The pitch augmentation control mode is pitch angle control, the roll augmentation control mode is roll control, and the yaw augmentation control is lateral acceleration control. Fly steadily according to the set key flight control parameters and maintain the basic flight state. S2: The longitudinal guidance mode is turned off, the lateral heading guidance mode is turned off, the pitch stabilization control is changed to the angle of attack control, the UAV flight attitude is changed, the thruster is introduced to cooperate with the tilt operation, the fuselage tilt angle is adjusted to change the flight direction, and the UAV flight altitude is increased to adjust to a specific altitude to perform tasks. The thruster is turned on to provide power to meet the power requirements for acceleration and changing the flight state; S3: The pitch stabilization control mode changes to longitudinal acceleration control, and the thrusters assist the UAV in turning. Combining tilt and turning operations, it changes the flight direction as planned. S4: Adjust the track inclination command and longitudinal acceleration command to calibrate the flight direction and horizontal state to ensure that the drone returns to or maintains the desired horizontal flight path and attitude; S5: Set the final flight control parameters, update the command output variables, turn on the lateral heading guidance and use the roll angle to control the lateral guidance, turn on the longitudinal guidance and use the pitch angle to control the longitudinal guidance, so that the UAV reaches the final stable flight state required by the mission.

2. The trajectory control strategy of the UAV according to claim 1, characterized in that: The key flight control parameters include lateral guidance, longitudinal guidance, pitch stabilization, roll stabilization, and yaw stabilization control modes; the initial values ​​of the variables are initial thrust, initial overload, initial attitude angle, and initial angle of attack.

3. The trajectory control strategy of the UAV according to claim 1 or 2, characterized in that: The method of using the roll angle to control the yaw angle in the lateral heading guidance is: Among them, φ c is the roll angle command, V g is the ground speed, g is the acceleration due to gravity, ω as is the heading angle deviation proportional control gain in the roll angle correction side deviation mode, ψ c is the yaw angle command, ψ t is the yaw angle, is the heading angular rate of the planned route.

4. The trajectory control strategy of the UAV according to claim 1 or 2, characterized in that: The control method of the pitch angle control altitude of the longitudinal guidance is: θ c =k p (h d -h)+k d (hdot d -hdot)+k i ∫(h d -h)dt Among them, θ c is the pitch angle command, h d ,h are the target height and current height respectively, hdot d ,hdot is the target lifting rate and the current lifting rate, k p ,k i ,k d is the PID parameter.

5. The trajectory control strategy of the UAV according to claim 1 or 2, characterized in that: The control method of the lateral acceleration control for the yaw stabilization control is as follows: a y =k r r+k φ f Among them, a y is the lateral acceleration, k r is the contribution coefficient of yaw rate to lateral acceleration, k φ is the contribution coefficient of the roll angle to the lateral acceleration, r is the yaw angular velocity, and φ is the roll angle.

6. The trajectory control strategy of the UAV according to claim 1, characterized in that: The angle of attack command in angle of attack control is: α c 0rad,α c 0 rad a c >0.26rad,α c =0.26rad In the formula, α c is the angle of attack command, n is the longitudinal overload, and g is the acceleration of gravity, which is 9.8 m / s here. 2 , Z a is the derivative parameter of the track inclination angle to the angle of attack, V g is the ground speed and α0 is the initial angle of attack.

7. The trajectory control strategy of the UAV according to claim 1, characterized in that: In S3, the third bit of the thruster command is set to 0 and the first bit is set to 0 in the thruster-assisted turning action of the drone.

8. The trajectory control strategy of the UAV according to claim 1, characterized in that: The track inclination instruction is: γ c >0rad,γ c 0 rad c c <-1rad,γ c =-1rad In the formula, γ c is the track inclination instruction, V0 is the initial indicated speed, V is the current indicated speed, and g is the acceleration due to gravity.

9. The trajectory control strategy of the UAV according to claim 1, characterized in that: The longitudinal acceleration command is: az c =-9.8·cosγ+(γ-γ c )·V g az c <-35m / s 2 ,az c =-35m / s 2 the c >0m / s 2 ,the c =0m / s 2 In the formula, az c is the longitudinal acceleration command, γ is the track inclination, Q is the dynamic pressure, S is the wing reference area, CL max is the maximum lift coefficient, and m is the mass of the UAV.

10. The trajectory control strategy of a UAV according to claim 1 or 2, characterized in that: The pitch, roll and yaw stabilization control in S5 is changed to the control position in S1, the autothrottle is set to airspeed control, and the airspeed command is the cruising speed.

Images