Flight control law of unmanned aerial vehicles based on robust servo linear quadratic regulator and Kalman filtering

Abstract

A new flight control law for unmanned aerial vehicles based on robust servo linear quadratic regulator control and Kalman filtering is proposed. This flight control law has a simple structure with high dependability in engineering. The pitch angle controller, which is designed based on the robust servo linear quadratic regulator control, is given to show the flight control law. Simulation results show that the pitch angle controller works well under noise-free conditions. Finally, Kalman filtering is applied to the pitch angle controller under noisy conditions, and the simulation results show that the proposed method reduces the influence of noise.

Introduction

Unmanned aerial vehicles (UAVs) have shown their great value in recent years because of their small volume, lightweight, and low cost. As is known, dynamic modeling and computation of aerodynamic coefficients are the foundations of designing flight control laws. Thus, in the study of Jang and Tomlin, ¹ a nonlinear model was linearized to design a controller for UAVs. There are two different ways to design a flight control law: one based on classical control theory and the other on modern control theory. Proportional–integral–derivative (PID) control, which is a classical form of control, has been applied to flight control law to improve the response time of the systems. ² This kind of classical control method has a simple structure that is easy to engineer, but it is difficult for it to meet UAVs performance requirements because of its single-input, single-output characteristic. Compared to classical control, modern control theory further improves the performance of the control system. Cooperative receding horizon control, ³ adaptive control system design, ⁴ and decentralized nonlinear control ⁵ have been applied on automatic control system. In the study of Wei and Cassandras, ³ a receding horizon controller suitable for dynamic and uncertain environments is proposed to maximize the total reward accumulated over a given time interval. In the study by Fradkov and Andrievsky, ⁴ a combined adaptive control law for UAVs is proposed and improves the performance of the control system on different flight conditions. In the study of Singh et al., ⁵ a decentralized nonlinear robust control system for formation flying of multiple UAVs is designed. Based on linear quadratic regulator (LQR) control, modern flight control law has already been applied to different types of aircraft ^{6 –9} ; a method based on LQR realizes the optimal flights for an autonomous UAV, ⁶ and LQR-based controllers have proven to be quite effective in vertical flight models. ^7,8 LQR controller has been applied to path following system of small unmanned autonomous vehicle for surveillance application. ⁹ The LQR method is widely used in engineering and numerous methods have been developed and proposed. ¹⁰ An augmented optimal LQR control system design procedure has been implemented on the longitudinal flight dynamics of a theoretical UAV. ¹¹ Since filtering techniques reduce the influence of noise, a flight control law can work well under noise conditions if filtering techniques are applied. ¹² A controller based on LQR and Kalman filtering has been applied to a ballistic missile longitudinal autopilot system, ¹³ but the low stability margin and slow response time have limited its application. The combination of LQR controller with a Kalman filtering shows their improvements to control the longitudinal motion of UAVs, and the controller can work well under noisy conditions. ¹⁴ In the study of Mou and Bin, ¹⁵ a robust flight control scheme for the longitudinal flight dynamics is proposed and proved quite effect; backstepping technique and the compounded disturbance estimated output are used to enhance the disturbance rejection ability of the robust flight control for fighters. Neural networks combined disturbance observer is used to design robust flight control schemes for the hypersonic vehicle with uncertainties and external disturbances, which improves the robustness of the system. ¹⁶

Robust servo LQR control, ¹⁷ which is based on LQR control, introduces state error into the system and performs better because it can track the input value. A controller based on robust servo LQR exhibits better performance than traditional controllers, ¹⁸ and robust servo LQR control has been applied to flight control law in full flight envelope for UAVs. ¹⁹ In the study of Anderson and Moore, ²⁰ the optimal control input vector of LQR has been provided. The study by Fengqi et al. ²¹ provides the solutions to guarantee the stability of the system for UAVs. Kalman filtering ²² has been applied to the flight control law to reduce the influence of noise. The robustness of robust servo LQR control can be improved by choosing the appropriate matrices ²³ and is more applicable to improve the robustness of the system than PID control. ²⁴

In this article, a new flight control law based on robust servo LQR control is designed to control the longitudinal motion of UAVs. Also, a pitch angle controller is provided to demonstrate its advantages. In the pitch angle controller, according to the objective function of the robust servo LQR, we obtain the optimal control input vector, and then, the optimal control input vector is applied to the pitch angle rate feedback loop to control the pitch angle rate; the optimal control input vector can guarantee the stability of the system and improve the performances of system. Also, the continuous state equation of UAV is discredited so that the Kalman filtering can be applied to the flight control law, both the stability of the continuous and discredited systems are rigorously proved under the flight control law, and the model uncertainty is also considered. The robustness of the system is improved by choosing the appropriate matrices of robust servo LQR control. Simulation results show that the proposed flight control law based on robust servo LQR and Kalman filtering estimates the real state vector accurately, and it satisfies the performance of UAV under the noise conditions.

Robust servo LQR control

The linearized state equation of longitudinal motion for the UAV is given by

$\{\begin{array}{l}\dot{x}=Ax+Bu+g(x,u,t)\\ y=Cx\end{array}$

1

where x is the state vector, u is the input vector, A is the transition matrix, B is the control matrix, y is the output vector, C is the transition matrix, and $g(x,u,t)$ is the uncertainty function. If only the deterministic functions of state equation (1) are considered, the LQR control can obtain the minimum value of the objective function; the objective function of the LQR control is given by

$J=\frac{1}{2}{\displaystyle \underset{0}{\overset{\infty }{\int }}(x^{T}Qx+u^{T}Ru)dt}$

2

where Q is the symmetric positive semidefinite matrix and R is the symmetric positive definite matrix. Both matrices Q and R affect the adjustment speed and control ability. In engineering, the matrix R is generally chosen as the identity matrix. According to the requirements of the control system, the appropriate matrix Q is generally a diagonal matrix. The optimal control input vector of the LQR control can be obtained as ²⁰

$u=-R^{-1}{B}^{T}Px=Kx$

3

where K is the gain matrix of LQR control and P is a positive semidefinite matrix as well as the solution to the Riccati equation ²⁰

$0=A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q$

4

The LQR control can make the state vector zero; however, in engineering, the state vector must not only be zero, there are some constant values that are needed to meet the UAV performance requirements. Robust servo LQR control, which is based on LQR control, can make state variable by adjusting the state deviation to zero so that it can track the input of control instructions. If the output feedback vector is supposed to be $y_c=C_{c}x$ , where C_c is the transition matrix, then the deviation in the control instruction is $e=y_c-r$ , where r is the control instruction. If only the deterministic functions of state equation (1) are considered, the state equation of the robust servo LQR is designed to be

$\dot{z}=\tilde{A}z+\tilde{B}\mu$

5

where $z=\left[\begin{array}{c}e\\ \dot{x}\end{array}\right], \hspace{0.17em}\tilde{A}=\left[\begin{array}{cc}0& C_c\\ 0& A\end{array}\right], \hspace{0.17em}\tilde{B}=\left[\begin{array}{c}0\\ B\end{array}\right],\text{and}\mu \text{=}\dot{u}$ .

The objective function of the robust servo LQR control is given by

$J=\frac{1}{2}{\displaystyle \underset{0}{\overset{\infty }{\int }}\left(z^{T}Qz+{\mu }^{T}R\mu \right)dt}$

6

The gain matrix of robust servo LQR control can be obtained in the same way as

$K_c=\left[\begin{array}{cc}{K}_1& K_2\end{array}\right]$

7

where K_c is the gain matrix of robust servo LQR control, K₁ and K₂ are the different blocks of K_c, and the matrix dimensions of K₁ and K₂ are the same as e and x. The optimal control input vector of the robust servo LQR control can be obtained as

$u={\displaystyle \int \mu }d\tau =-K_c{\displaystyle \int z}d\tau =-K_1{\displaystyle \int e}d\tau -K_2{\displaystyle \int \dot{x}}d\tau =-K_1{\displaystyle \int e}d\tau -K_{2}x$

8

According to the objective function of the robust servo LQR, we obtain the optimal control input vector u and apply u to the system (1) to design the flight control law. The robust servo LQR control improves the system type and enhances antijamming ability because it adds the integral element in the state feedback control loop. Therefore, this flight control law, based on robust servo LQR control, can improve the dynamic behavior and steady-state performance of the control system.

In the foregoing, we do not consider the uncertainty function $g(x,u,t)$ , which can affect the robustness of the system. If the uncertainty function $g(x,u,t)$ meets the conditions

$\Vert g\left(x,u,t\right)\Vert <\rho \Vert x\Vert$

9

the system (1) is robust stable, ²³ where ρ is the robust performance index, and

$\rho ={\lambda }_{\text{min}}\left(Q+{\text{PBR}}^{-1}{B}^{T}P\right)/{\lambda }_{\text{max}}\left(P\right)$

10

where ${\lambda }_{\text{min}}\left(Q+{\text{PBR}}^{-1}{B}^{T}P\right)$ is the minimum eigenvalue of the matrix $\left(Q+{\text{PBR}}^{-1}{B}^{T}P\right)\text{and}{\lambda }_{\text{max}}(P)$ is the maximum eigenvalue of P. The matrices Q and R can be adjusted to improve the robustness of the system. ²³

The state-space equations of the UAV in this study are the same as equation (1), and the matrices are $A\in R^{5\times 5},B\in R^{5\times 2},\text{and}C\in R^{5\times 5}.x^T=\left[\theta ,u,w,h,q\right]$ is the state vector, where θ is the pitch angle, u is the X component of the velocity, w is the Z component of the velocity, h is the height, and q is the pitching angular velocity. $u^T=\left[\delta e,\delta t\right]$ is the input vector, where δe is the angle of elevator deflection, and δt is the opening degree of the accelerator. $y=[\theta ,\hspace{0.17em}V,\hspace{0.17em}h,\hspace{0.17em}q,\hspace{0.17em}\dot{h}]$ is the output vector, where V is the velocity. The uncertainty function $\text{g}(x,u,t)$ of system (1) is set to be white Gaussian noise. Since, in this article, a flight control law has been designed based on robust servo LQR, the robust servo LQR has been applied to the pitch angle rate feedback loop in the pitch angle controller, and classical control is used to eliminate the pitch angle error. The structure of the pitch angle controller based on the robust servo LQR is shown in Figure 1, where θg is the expectancy value of the pitch angle, θ is the output value, and q_g is the input value of the pitch angle rate feedback loop.

Figure 1.

Structure of pitch angle controller.

The pitch angle rate feedback loop is shown in Figure 2.

Figure 2.

Structure of pitch angle rate feedback loop.

From Figures 1 and 2, we obtain

$q_g=K_p\left({\theta }_g-\theta \right)$

11

${\delta }_e=\frac{K_1}{S}\left(q_g-q\right)-K_{2}q$

12

From equations (11) and (12), we obtain

$\dot{\theta }=q=K_p({\theta }_g-\theta )$

13

Thus, the transfer function of the pitch angle controller is

$\frac{\theta }{{\theta }_g}=\frac{K_p}{S+K_p}$

14

The parameter K_p is determined based on the bandwidth ratio between the inner and outer rings. Therefore, the transfer function of the pitch angle controller can be seen as a first-order system, and the pitch angle controller can guarantee the stability of the system.

The flight altitude of the UAV ¹⁹ is between 0 m and 5000 m, the minimum speed of level flight is 34.3 m/s, and the maximum speed of level flight is 126.6 m/s. At the same speed, the altitude of the UAV is related proportionally to climbing ability; at the same altitude, the speed of the UAV is related proportionally to climbing ability. The flying angle is suggested to be less than 8° to guarantee the stability of the UAV. The longitudinal modal damping in the short cycle is 0.34 and the longitudinal modal damping in the long cycle is 0.09. The trim point of the UAV is $V=50\text{ m/s},h=2500\text{ m}$ ; at the trim point, the flying angle is 6.1°, δ_T = 74%, δ_e=−11.1°, and A, B, and C of UAV are set to be ¹⁹

$A=\left[\begin{array}{ccccc}0& 0& 0& 0& 1\\ -9.75& -0.04& 0.07& 0& -5.48\\ -1.03& -0.11& -2.48& 0& 51.71\\ -52& -0.11& 0.99& 0& 0\\ 0& 0.06& 0.53& 0& -1.47\end{array}\right]\text{,}B=\left[\begin{array}{cc}0& 0\\ 1.33& 0.42\\ -15.38& 0\\ 0& 0\\ -23.78& 0\end{array}\right]\text{, and }C=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0.99& 0.11& 0& 0\\ 0& 0& 0& -1& 0\\ 0& 0& 0& 0& 1\\ 52& 0.11& -0.99& 0& 0\end{array}\right]$

In the pitch angle rate feedback loop, the transition matrix of system (5) is ${\tilde{A}}_q=\left[\begin{array}{cc}0& 1\\ 0& -1.47\end{array}\right]$ , the control matrix of system (5) is ${\tilde{B}}_q=\left[\begin{array}{c}0\\ -23.78\end{array}\right]$ , we choose $Q=\text{diag}\{\hspace{0.17em}75,\hspace{0.17em}0.5\}$ , and suppose $D{D}^T=Q$ , we can easily obtain $\text{Rank}[D]=2,\text{Rank}[\begin{array}{cc}\tilde{B}& \tilde{A}\tilde{B}\end{array}]=2,\text{and}\text{Rank}[\begin{array}{cc}{D}^T& {\tilde{A}}^T{D}^T\end{array}]=2.\text{So},\left\{{\tilde{A}}_q,{\tilde{B}}_q\right\}$ is completely controllable and $\left\{{\tilde{A}}_q,D\right\}$ is completely observable; the optimal control input vector u can guarantee the stability of system (5). ²¹ System (1) is a part of system (5), and the optimal control input vector u can guarantee the stability of system (1).

Pitch angle controller based on robust servo LQR and Kalman filtering

The pitch angle controller based on robust servo LQR control does not work well under noisy conditions, because it is not possible to obtain the real states all the time. Therefore, Kalman filtering ²² is applied to the pitch angle controller to reduce the influence of noise. The linear discretized Kalman filtering is

$\{\begin{array}{l}x\left(k+1\right)=\phi \left(k\right)x\left(k\right)+G\left(k\right)u\left(k\right)+\varphi \left(k\right)w\left(k\right)\\ z\left(k\right)=H\left(k\right)x\left(k\right)+v\left(k\right)\end{array}$

15

where x(k) is the state vector, u(k) is the input vector, $\phi (k)$ is the transition matrix of the system, G(k) is the control matrix, $\varphi (k)$ is the transition matrix of the noise, z(k) is the measurement vector, H(k) is the measurement matrix, w(k) is the system noise, and v(k) is the measurement noise. We further suppose that

$E\left[w\left(k\right)\right]=0E\left[v\left(k\right)\right]=0E\left[w^2\left(k\right)\right]=Q_{k}E\left[v^2\left(k\right)\right]=R_{k}E\left[w\left(k\right)v\left(k\right)\right]=C_k$

16

The optimal state estimation is

$\widehat{x}\left(k+1/k+1\right)=\widehat{x}\left(k+1/k\right)-K\left(k+1\right)\left[z\left(k+1\right)H\left(k+1\right)\widehat{x}\left(k+1/k\right)\right]$

17

The optimal predictive state estimation is

$\widehat{x}\left(k+1/k\right)=\phi \left(k\right)\widehat{x}\left(k/k\right)+G\left(k\right)u\left(k\right)+\varphi \left(k\right)C_k{R}_k^{-1}\left[z\left(k\right)-H\left(k\right)\widehat{x}\left(k/k\right)\right]$

18

The gain matrix of Kalman filtering is

$K\left(k+1\right)=P\left(k+1\right)H^{\text{T}}\left(k+1\right){\left[H\left(k+1\right)P\left(k+1/k\right)H^{\text{T}}\left(k+1\right)+R_{k+1}\right]}^{-1}$

19

The covariance matrix of forecast error is

$\begin{array}{c}P\left(k+1/k\right)=\left[\phi \left(k\right)-\varphi \left(k\right)C_k{R}_k^{-1}H\left(k\right)\right]P\left(k/k\right)\\ \begin{array}{c}\end{array}{\left[\phi \left(k\right)-\varphi \left(k\right)C_k{R}_k^{-1}H\left(k\right)\right]}^{\text{T}}\\ +\begin{array}{c}\end{array}\varphi \left(k\right)Q_k^{\text{T}}\varphi \left(k\right)-\varphi \left(k\right)C_k{R}_k^{-1}{C}_k^{\text{T}}{\varphi }^{\text{T}}\left(k\right)\end{array}$

20

and the covariance matrix of evaluated error is

$P\left(k+1/k+1\right)=(I-K\left(k+1\right)H\left(k+1\right))P\left(k+1/k\right)$

21

The state equation (1) of the UAV is continuous, but the Kalman filtering in equation (15) is discrete. The state equation (1) needs to be discredited to apply Kalman filtering to the flight control law. If the sampling time is supposed to be T = 0.01, the state equation (1) can be discredited; the discredited $\phi (k)$ and G(k) matrices of state equation (1) are

$\phi \left(k\right)=AT+I$

22

and

$G\left(k\right)=BT$

23

where I is identity matrix. $\varphi \left(k\right)\text{and}H\left(k\right)$ are both identity matrices. Thus, the state equation (1) is discretized so that the Kalman filtering can be applied to the flight control law and used to estimate the real states in the pitch angle controller. In the pitch angle rate feedback loop, the transition matrix of system (1) is $A_q=-1.47$ , and the control matrix of system (1) is $B_q=-23.78$ . From equations (22) and (23), we obtain $\phi \left(k\right)=1-1.47T\text{and}G\left(k\right)=-23.78T$ . For any one variable E > 0, suppose F is the solution to the following equation

$-E=\phi {\left(k\right)}^{T}F\phi \left(k\right)-F$

24

we obtain

$F=\frac{E}{1-{\left(1-1.47T\right)}^2}>0$

25

Therefore, for any one variable E > 0, we always obtain a variable F > 0 to meet equation (24); the discretized system (15) is stable. ²¹ After discretization, $\phi (k)$ , G(k), and H(k) are all nonzero; therefore, the discretized system is completely controllable and completely observable; the process of Kalman filtering is stable. ²¹

Simulation results

In the first described simulations, noises are not considered. In the pitch angle controller, the robust servo LQR controller is compared to the conventional PID controller to demonstrate its advantages; the simulation results are shown in Figures 3 and 4. The time- and frequency-domain attributes of this controller are given in Table 1. Figure 3 shows that the PID and robust servo LQR controllers can meet the system requirements, but the robust servo LQR controller does not exhibit overshoot and is gentler than the PID controller in same response time. Figure 4 shows the change of pitching angular velocity in robust servo LQR and PID controllers; we can see that the change of pitching angular velocity in robust servo LQR controllers is smaller than the PID controller. Also, the robust stability condition of PID control is independent of the parameters of PID control in the low frequency; some systems can’t achieve robust stability by adjusting the parameters of PID control, ²⁴ and it limits the PID control to design robust control system. Compared to a PID control, the robust serve LQR control is more applicable to improve the robustness of the system. Therefore, compared to a PID controller, a controller based on robust servo LQR can improve the performance of the system.

Figure 3.

Comparison of pitch angle in robust servo LQR and PID controllers. LQR: linear quadratic regulator; PID: proportional–integral–derivative.

Figure 4.

Comparison of pitching angular velocity in robust servo LQR and PID controllers. LQR: linear quadratic regulator; PID: proportional–integral–derivative.

Table 1.

Time- and frequency-domain attributes of pitch angle controllers.

Step response characteristics	Rise time (s) 1.945	Steady time (s) 4.032	Overshoot (%) 0
Frequency response characteristics	Phase margin (°)	Cut-off frequency (rad/s)	Magnitude margin (db)
	70.85	0.51	65.13

In the following described simulations, noises are considered, and the parameters are set as $Q_k=0.5,R_k=0.5,\text{and}{C}_k=0.5$ . The response of the pitch angle controller with and without Kalman filtering is tested under noisy conditions. Figure 5 shows the responses of the pitch angle controller with and without Kalman filtering under noisy and noise-free conditions. It can be seen from the figure that the pitch angle controller with Kalman filtering maintains the quality of its performance compared to the real pitch angle under noisy conditions, and moreover, it exhibits better performance than the pitch angle controller without Kalman filtering under noisy conditions. Figure 6 shows the pitch angle difference between noisy and noise-free conditions for the pitch angle controller. In the figure, the error in pitch angle is between −0.6 and 0.6; since noise has a great influence on pitch angle, noise may cause the improper function of the pitch angle controller. Figure 7 shows the pitch angle differences between a pitch controller based on robust servo LQR under noise-free conditions, under noisy conditions without Kalman filtering, and under noisy conditions with Kalman filtering. In the figure, the error in pitch angle is between −0.3 and 0.2; since noise has a small influence on pitch angle, the pitch angle controller based on robust servo LQR and Kalman filtering can work well under noisy conditions. The simulation results show that Kalman filtering can estimate the real states under noisy conditions, and that the proposed controller based on robust servo LQR and Kalman filtering works well.

Figure 5.

Comparison of the pitch angle responses of a pitch controller based on robust servo LQR under noise-free conditions, under noisy conditions without Kalman filtering, and under noisy conditions with Kalman filtering. LQR: linear quadratic regulator.

Figure 6.

Pitch angle differences of the pitch controller based on robust servo LQR under noise-free and noisy conditions. LQR: linear quadratic regulator.

Figure 7.

Pitch angle differences of the pitch controller based on robust servo LQR under noise-free and Kalman-filtered noisy conditions. LQR: linear quadratic regulator.

Conclusions

In this article, a new flight control law is proposed based on robust servo LQR control. Also, the pitch angle controller of the longitudinal motion is designed for UAVs. The Kalman filtering is applied to the pitch angle controller to estimate the real states in order to reduce the influence of noise. The results show that the proposed method can increase the effectiveness of the pitch angle controller.