TRAJECTORY TRACKING CONTROL METHOD FOR UNMANNED HELICOPTERS BASED ON FILTER OBSERVER

Abstract

A trajectory tracking control method for the unmanned helicopter based on the filter observer is provided. This method establishes a position system model of the disturbed unmanned helicopter including a lumped disturbance term and a measurement noise term, then, the state vector is constructed based on the actual position and actual speed of the unmanned helicopter, the position system model of the disturbed unmanned helicopter is transformed into the system state equation based on the state vector, according to the system state equation, the filter observer is designed based on the continuous-time Kalman filter, and the composite PID controller is designed for the position system model of the disturbed unmanned helicopter, the trajectory tracking control of the disturbed unmanned helicopter using the composite PID controller combined with the filter observer can perform dynamic real-time feedforward compensation for the lumped disturbance and achieve effective suppression of the measurement noise.

Claims

1. A trajectory tracking control method for an unmanned helicopter based on a filter observer, wherein the trajectory tracking control method for the unmanned helicopter comprises: establishing a position system model of a disturbed unmanned helicopter, the position system model of the disturbed unmanned helicopter comprises a position dynamic equation of the unmanned helicopter under an action of a control variable and a position measurement equation of a position measurement sensor in the unmanned helicopter, the position dynamic equation comprises a lumped disturbance term configured to characterize an external disturbance of the unmanned helicopter, the position measurement equation comprises a measurement noise term configured to characterize a measurement error disturbance of the position measurement sensor; constructing a state vector by an actual position and an actual speed of the unmanned helicopter, transforming the position system model of the disturbed unmanned helicopter into a system state equation based on the state vector, and designing the filter observer based on a continuous-time Kalman filter according to the system state equation; designing a composite proportion integration differentiation (PID) controller for the position system model of the disturbed unmanned helicopter; when a trajectory tracking control of the unmanned helicopter is performed, inputting the control variable of the unmanned helicopter and a position measurement value of the position measurement sensor into the filter observer to obtain an actual position estimated value and an estimated value of the lumped disturbance term, inputting a position error between the actual position estimated value and a three-dimensional reference trajectory and the estimated value of the lumped disturbance term into the composite PID controller, and controlling the unmanned helicopter according to a command value of the control variable output by the composite PID controller; the position dynamic equation of the unmanned helicopter is established as follows: $\{\begin{array}{c}\stackrel{.Math.}{x}=-\frac{k_d}{m}\stackrel{}{x}+\frac{T_m}{m}\left(\cos \mathrm{sincos}+\sin \sin \right)+D_x\\ \stackrel{.Math.}{y}=-\frac{k_d}{m}\stackrel{}{y}+\frac{T_m}{m}\left(\cos \mathrm{sinsin}-\sin \cos \right)+D_y\\ \stackrel{.Math.}{z}=-\frac{k_d}{m}\stackrel{}{z}+\frac{T_m}{m}\cos \cos +g+D_z\end{array}$ the position measurement equation of the position measurement sensor in the unmanned helicopter is established as follows: $\{\begin{array}{c}{x}_m=x+v_{\mathrm{mx}}\\ y_m=y+v_{\mathrm{my}}\\ z_m=z+v_{mz}\end{array}$ wherein (x, y, z) is an actual position of the unmanned helicopter in three axis directions of an inertial coordinate system, (x.sub.m, y.sub.m, z.sub.m) is a position measurement value in the three axis directions of the inertial coordinate system obtained by the position measurement sensor in the unmanned helicopter, (v.sub.mx, v.sub.my, v.sub.mz) is the measurement noise term of the position measurement sensor in the unmanned helicopter in the three axis directions of the inertial coordinate system; (D.sub.x, D.sub.y, D.sub.z) is the lumped disturbance term in the three axis directions of the inertial coordinate system; the control variables of the unmanned helicopter comprise a main rotor lift force T.sub.m, roll angle , a pitching angle , and a heading angle ; m is a mass of the unmanned helicopter, g is an acceleration constant of gravity, k.sub.d is a velocity damping coefficient; the same three-channel composite PID controller is designed for the three axis directions of the inertial coordinate system: $\{\begin{array}{c}\frac{T_m}{m}\left(\cos \mathrm{sincos}+\sin \sin \right)=\\ -K_{px}\left(\stackrel{}{x}-x_r\right)-K_{\mathrm{ix}}\left(\stackrel{}{x}-x_r\right)-K_{dx}\left(\stackrel{}{\stackrel{}{x}}-{\stackrel{}{x}}_r\right)-{\stackrel{}{D}}_x\\ \frac{T_m}{m}\left(\cos \mathrm{sinsin}-\sin \cos \right)=\\ -K_{\mathrm{py}}\left(\stackrel{}{y}-y_r\right)-K_{\mathrm{iy}}\left(\stackrel{}{y}-y_r\right)-K_{\mathrm{dy}}\left(\widehat{\stackrel{.}{y}}-{\stackrel{.}{y}}_r\right)-{\stackrel{}{D}}_y\\ \frac{T_m}{m}\cos \cos +g=-K_{pz}\left(\stackrel{}{z}-z_r\right)-\\ K_{\mathrm{iz}}\left(\stackrel{}{z}-z_r\right)-K_{\mathrm{dz}}\left(\widehat{\stackrel{}{z}}-z_r\right)-{\stackrel{}{D}}_z\end{array}$ wherein (x.sub.r, y.sub.r, z.sub.r) is a position reference indicated by the three-dimensional reference trajectory to be followed by the unmanned helicopter; ({circumflex over (x)}, , {circumflex over (z)}) is an actual position estimated value in the three axis directions of the inertial coordinate system, and ({circumflex over (D)}.sub.x, {circumflex over (D)}.sub.y, {circumflex over (D)}.sub.z) is an estimated value of the lumped disturbance term in the three axis directions of the inertial coordinate system; K.sub.px, K.sub.ix, K.sub.dx are proportional coefficient, integral coefficient and differential coefficient in an x-axis direction of the inertial coordinate system, K.sub.py, K.sub.iy, K.sub.dy are proportional coefficient, integral coefficient and differential coefficient in a y-axis direction of the inertial coordinate system; K.sub.pz, K.sub.iz, K.sub.dz are proportional coefficient, integral coefficient and differential coefficient in a z-axis direction of the inertial coordinate system; K.sub.pj, K.sub.ij, K.sub.dj in any direction of j=x, y, z are positive constants and satisfy a polynomial P.sup.j(s)=s.sup.3+K.sub.djs.sup.2+K.sub.pjs+K.sub.ij=0 and belong to Hurwitz, s is a complex frequency domain parameter.

2. The trajectory tracking control method for the unmanned helicopter based on the filter observer according to claim 1, wherein the step of designing the filter observer comprises: constructing a state vector X=[x v.sub.x].sup.T and a derivative {dot over (X)}=[{dot over (x)} {dot over (v)}.sub.x].sup.T of the state vector of the unmanned helicopter in the x-axis direction of the inertial coordinate system, according to the position system model of the disturbed unmanned helicopter, the system state equation in the x-axis direction of the inertial coordinate system is $\{\begin{array}{c}\stackrel{}{X}=A_{x}X+B_x{a}_x+B_{dx}{D}_x\\ x_m=H_{x}X+v_{\mathrm{mx}}\end{array},$ and designing a filter observer in the x-axis direction of the inertial coordinate system to observe the actual position estimated value and the estimated value of the lumped disturbance term in the x-axis direction, wherein $A_x=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_x=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dx}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_x=\left[\begin{array}{c}1\\ 0\end{array}\right],$ a virtual control variable $a_x=\frac{T_m}{m}\left(\cos \mathrm{sincos}+\sin \sin \right),$ v.sub.x={dot over (x)} denotes an actual speed of the unmanned helicopter in the x-axis direction of the inertial coordinate system; constructing a state vector Y=[y v.sub.y].sup.T and a derivative {dot over (Y)}=[{dot over (y)} {dot over (v)}.sub.y].sup.T of the state T vector of the unmanned helicopter in the y-axis direction of the inertial coordinate system, according to the position system model of the disturbed unmanned helicopter, the system state equation in the y-axis direction of the inertial coordinate system is $\{\begin{array}{c}\stackrel{}{Y}=A_{y}Y+B_y{a}_y+B_{\mathrm{dy}}{D}_y\\ y_m=H_{y}y+v_{\mathrm{my}}\end{array},$ and designing a filter observer in the y-axis direction of the inertial coordinate system to observe the actual position estimated value and the estimated value of the lumped disturbance term in the y-axis direction, wherein $A_y=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_y=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dy}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_y=\left[\begin{array}{c}1\\ 0\end{array}\right],$ a virtual control variable $a_y=\frac{T_m}{m}\left(\cos \mathrm{sinsin}-\sin \cos \right),$ v.sub.y={dot over (y)} denotes an actual speed of the unmanned helicopter in the y-axis direction of the inertial coordinate system; constructing a state vector Z=[z v.sub.z].sup.T and a derivative =[ {dot over (v)}.sub.z].sup.T of the state vector of the unmanned helicopter in the z-axis direction of the inertial coordinate system, according to the position system model of the disturbed unmanned helicopter, the system state equation in the x-axis direction of the inertial coordinate system is $\{\begin{array}{c}\stackrel{}{Z}=A_{z}Z+B_z{a}_z+B_{\mathrm{dz}}{D}_z\\ z_m=H_{z}Z+v_{\mathrm{mz}}\end{array},$ and designing a filter observer in the z-axis direction of the inertial coordinate system to observe the actual position estimated value and the estimated value of the lumped disturbance term in the z-axis direction, wherein $A_z=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_z=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dz}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_z=\left[\begin{array}{c}1\\ 0\end{array}\right],$ a virtual control variable $a_z=\frac{T_m}{m}\cos \cos +g,$ v.sub.z= denotes an actual speed of the unmanned helicopter in the z-axis direction of the inertial coordinate system.

3. The trajectory tracking control method for the unmanned helicopter based on the filter observer according to claim 2, wherein the filter observer in the x-axis direction of the inertial coordinate system is designed as: $\begin{array}{c}\{\begin{array}{c}{K}_x=P_x{H}_x^T{R}_x^{-1}\\ \stackrel{.}{\stackrel{~}{X}}=A_x\tilde{X}+B_x{a}_x+B_{\mathrm{dx}}{\hat{D}}_x+K_x\left(x_m-H_x\tilde{X}\right)\\ {\stackrel{.}{P}}_x=-P_x{H}_x^T{R}_x^{-1}{H}_x{P}_x+A_x{P}_x+P_x{A}_x^T+Q_x\end{array}\\ \{\begin{array}{c}{e}_{\mathrm{Ox}}=z_{x1}-\tilde{x}\\ {\stackrel{}{z}}_{x1}=z_{x2}-{}_{01}^x{e}_{\mathrm{Ox}}\\ {\stackrel{.}{z}}_{x2}=z_{x3}-{}_{02}^x{e}_{\mathrm{Ox}}-\frac{k_d}{m}{z}_{x2}+a_x\\ {\stackrel{}{z}}_{x3}=-{}_{03}^x{e}_{\mathrm{Ox}}\\ \stackrel{}{x}=z_{x1}\\ \stackrel{}{\stackrel{.}{x}}=z_{x2}\\ {\stackrel{}{D}}_x=z_{x3}\end{array}\end{array}$ wherein K.sub.xR.sup.21 is a Kalman gain, P.sub.xR.sup.22 is a covariance matrix of a state vector estimation X, {tilde over (X)}=[{tilde over (x)} {tilde over (v)}.sub.x].sup.T is an estimated value of a system state in the x-axis direction obtained by the Kalman filter, {tilde over ({dot over (X)})}=[{tilde over ({dot over (x)})} {tilde over ({dot over (v)})}.sub.x].sup.T is a first derivative of the estimated value {tilde over (X)}, {tilde over (x)} denotes a position estimated value in the x-axis direction obtained by the Kalman filter, and, {tilde over (v)}.sub.x denotes a velocity estimated value in the x-axis direction obtained by the Kalman filter; {circumflex over (x)} denotes an estimated value of the actual position of the unmanned helicopter in the x-axis direction of the inertial coordinate system, and {circumflex over (v)}.sub.x={dot over ({circumflex over (x)})} denotes an actual velocity estimated value of the unmanned helicopter in the x-axis direction of the inertial coordinate system, {circumflex over (D)}.sub.x denotes the estimated value of the lumped disturbance term in the x-axis direction of the inertial coordinate system; Q.sub.xR.sup.22 denotes a covariance matrix of an estimated lumped disturbance term {circumflex over (D)}.sub.x; z.sub.x1, z.sub.x2, z.sub.x3 are area intermediate variables of the filter observer, ${}_{01}^x,{}_{02}^x,{}_{03}^x$ are positive constants and values of the positive constants satisfy $P^x\left(s\right)=s^3+{}_{01}^x{s}^2+{}_{02}^{x}s+{}_{03}^x$ parameter, the measurement noise term v.sub.mx is Gaussian white noise, R.sub.x is a covariance of the measurement noise term v.sub.mx.

4. The trajectory tracking control method for the unmanned helicopter based on the filter observer according to claim 2, wherein the filter observer in the y-axis direction of the inertial coordinate system is designed as: $\begin{array}{c}\{\begin{array}{c}{K}_y=P_y{H}_y^T{R}_y^{-1}\\ \stackrel{.}{\stackrel{~}{Y}}=A_y\tilde{Y}+B_y{a}_y+B_{\mathrm{dy}}{\hat{D}}_y+K_y\left(y_m-H_y\tilde{Y}\right)\\ {\stackrel{.}{P}}_y=-P_y{H}_y^T{R}_y^{-1}{H}_y{P}_y+A_y{P}_y+P_y{A}_y^T+Q_y\end{array}\\ \{\begin{array}{c}{e}_{\mathrm{Oy}}=z_{y1}-\tilde{y}\\ {\stackrel{}{z}}_{y1}=z_{y2}-{}_{01}^y{e}_{\mathrm{Oy}}\\ {\stackrel{.}{z}}_{y2}=z_{y3}-{}_{02}^y{e}_{\mathrm{Oy}}-\frac{k_d}{m}{z}_{y2}+a_y\\ {\stackrel{}{z}}_{y3}=-{}_{03}^y{e}_{\mathrm{Oy}}\\ \stackrel{}{y}=z_{y1}\\ \stackrel{}{\stackrel{}{y}}=z_{y2}\\ {\stackrel{}{D}}_y=z_{y3}\end{array}\end{array}$ wherein K.sub.yR.sup.21 is a Kalman gain, P.sub.yR.sup.22 is a covariance matrix of a state vector estimation Y, {tilde over (Y)}=[{tilde over (y)} {tilde over (v)}.sub.y].sup.T is an estimated value of a system state in the y-axis direction obtained by the Kalman filter, {tilde over ({dot over (Y)})}=[{tilde over ({dot over (y)})} {tilde over ({dot over (y)})}.sub.y].sup.T is a first derivative of the estimated value {tilde over (Y)}, {tilde over (y)} denotes a position estimated value in the y-axis direction obtained by the Kalman filter, and {tilde over (v)}.sub.y denotes a velocity estimated value in the y-axis direction obtained by the Kalman filter; denotes an estimated value of the actual position of the unmanned helicopter in the y-axis direction of the inertial coordinate system, and {circumflex over (v)}.sub.y={dot over ()} denotes the actual velocity estimated value of the unmanned helicopter in the y-axis direction of the inertial coordinate system, {circumflex over (D)}.sub.y denotes the estimated value of the lumped disturbance term in the y-axis direction y of the inertial coordinate system; Q.sub.yR.sup.22 denotes a covariance matrix of the estimated lumped disturbance term {circumflex over (D)}.sub.y; z.sub.y1, z.sub.y2, z.sub.y3 are area intermediate variables of the filter observer; ${}_{01}^y,{}_{02}^y,{}_{03}^y$ are positive constants and values of the positive constants satisfy $P^y\left(s\right)=s^3+{}_{01}^y{s}^2+{}_{02}^{y}s+{}_{03}^y$ and belong to Hurwitz, s is the complex frequency domain parameter, the measurement noise term v.sub.my is Gaussian white noise, R.sub.y is a covariance of the measurement noise term V.sub.mx.

5. The trajectory tracking control method for the unmanned helicopter based on the filter observer according to claim 2, wherein the filter observer in the z-axis direction of the inertial coordinate system is designed as: $\begin{array}{c}\{\begin{array}{c}{K}_z=P_z{H}_z^T{R}_z^{-1}\\ \stackrel{.}{\stackrel{~}{Z}}=A_z\tilde{Z}+B_z{a}_z+B_{\mathrm{dz}}{\hat{D}}_z+K_z\left(z_m-H_z\tilde{Z}\right)\\ {\stackrel{.}{P}}_z=-P_z{H}_z^T{R}_z^{-1}{H}_z{P}_z+A_z{P}_z+P_z{A}_z^T+Q_z\end{array}\\ \{\begin{array}{c}{e}_{\mathrm{Oz}}=z_{z1}-\tilde{z}\\ {\stackrel{}{z}}_{z1}=z_{z2}-{}_{01}^z{e}_{\mathrm{Oz}}\\ {\stackrel{.}{z}}_{z2}=z_{z3}-{}_{02}^z{e}_{\mathrm{Oz}}-\frac{k_d}{m}{z}_{z2}+a_z\\ {\stackrel{}{z}}_{z3}=-{}_{03}^z{e}_{\mathrm{Oz}}\\ \stackrel{}{z}=z_{z1}\\ \stackrel{}{\stackrel{}{z}}=z_{z2}\\ {\stackrel{}{D}}_z=z_{z3}\end{array}\end{array}$ wherein K.sub.zR.sup.21 is a Kalman gain, P.sub.zR.sup.22 is a covariance matrix of a state vector estimation Z, {tilde over (Z)}=[{tilde over (z)} {tilde over (v)}.sub.z].sup.T is an estimated value of a system state in the z-axis direction obtained by the Kalman filter, {tilde over ()}=[{tilde over ()} {tilde over ({dot over (v)})}.sub.z].sup.T is a first derivative of the estimated value {tilde over (Z)}, {tilde over (z)} denotes a position estimated value in the z-axis direction obtained by the Kalman filter, and {tilde over (v)}.sub.z, denotes a velocity estimated value in the z-axis direction obtained by the Kalman filter; {circumflex over (z)} denotes an estimated value of the actual position of the unmanned helicopter in the z-axis direction of the inertial coordinate system, and, {circumflex over (v)}.sub.z={dot over ({circumflex over (z)})} denotes the actual velocity estimated value of the unmanned helicopter in the z-axis direction of the inertial coordinate system, {circumflex over (D)}.sub.z denotes the estimated value of the lumped disturbance term in the z-axis direction of the inertial coordinate system; Q.sub.zR.sup.22 denotes a covariance matrix of the estimated lumped disturbance term {circumflex over (D)}.sub.z, z.sub.z1, z.sub.z2, z.sub.z3 are area intermediate variables of the filter observer; ${}_{01}^z,{}_{02}^z,{}_{03}^z$ are positive constants and values of the positive constants satisfy $P^z\left(s\right)=s^3+{}_{01}^z{s}^2+{}_{02}^{z}s+{}_{03}^z$ parameter, the measurement noise term v.sub.mz is Gaussian white noise, R.sub.z is a covariance of the measurement noise term v.sub.mz.

6. The trajectory tracking control method for the unmanned helicopter based on the filter observer according to claim 1, wherein the step of obtaining the command value of the control variable of the unmanned helicopter comprises: on a basis of keeping the command value of the heading angle .sup.d=0, obtaining a command value of the main rotor lift force $T_m^d,$ a command value of the roll angle .sup.d and a command value of the pitching angle .sup.d by using a designed filter observer combined with the composite PID controller: $\{\begin{array}{c}{T}_m^d=m\sqrt{a_x^2+a_y^2+{\left(a_z-g\right)}^2}\\ {}^d=\arctan \left(\frac{a_x}{a_z-g}\right)\\ {}^d=\arcsin \left(m\frac{a_y}{T_m^d}\right)\end{array}\mathrm{wherein}\{\begin{array}{c}{a}_x=\frac{T_m}{m}\left(\cos \sin \cos +\sin \sin \right)\\ a_y=\frac{T_m}{m}\left(\cos \sin \sin -\sin \cos \right)\\ a_z=\frac{T_m}{m}\cos \cos +g\end{array}$

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0013] FIG. 1 is a control block diagram of the trajectory tracking control method for the unmanned helicopter in the embodiment of this invention.

[0014] FIG. 2 is a comparison diagram of the tracking trajectories of the two control methods in the simulation example.

[0015] FIGS. 3A-3C show a comparison diagram of the three-channel tracking error response curves of the two control methods in the simulation example.

[0016] FIGS. 4A-4C show a comparison diagram of the three-channel MAE response curves of the two control methods in the simulation example.

[0017] FIGS. 5A-5C show a comparison diagram of the response curves of the virtual control variable of the two control methods in the simulation example.

[0018] FIGS. 6A-6C show a comparison diagram of the response curves of the virtual control variable change rate of the two control methods in the simulation example.

[0019] FIGS. 7A-7C show a comparison diagram of the estimation errors of the three-channel lumped disturbance term of the two control methods in the simulation example.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0020] The following is a further explanation of the specific embodiment of this application in combination with the accompanying figures.

[0021] This application discloses a trajectory tracking control method for the unmanned helicopter based on the filter observer, combined with the control block diagram shown in FIG. 1, the trajectory tracking control method for the unmanned helicopter includes the following contents:

[0022] Step 1, the position system model of the disturbed unmanned helicopter is established, the position system model of the disturbed unmanned helicopter includes the position dynamic equation of the unmanned helicopter under the action of the control variable and the position measurement equation of the position measurement sensor in the unmanned helicopter.

[0023] The position dynamic equation indicates the position change of the unmanned helicopter, because the unmanned helicopter will be affected by external disturbance, the position dynamic equation contains a lumped disturbance term used to characterize the external disturbance of the unmanned helicopter. The position dynamic equation of unmanned helicopter is established as follows:

[00001]$\begin{array}{cc}\{\begin{array}{c}\stackrel{.Math.}{x}=-\frac{k_d}{m}\stackrel{.}{x}+\frac{T_m}{m}\left(\cos \sin \cos +\sin \sin \right)+D_x\\ \stackrel{.Math.}{y}=-\frac{k_d}{m}\stackrel{.}{y}+\frac{T_m}{m}\left(\cos \sin \sin -\sin \cos \right)+D_y\\ \stackrel{.Math.}{z}=-\frac{k_d}{m}\stackrel{.}{z}+\frac{T_m}{m}\cos \cos +g+D_z\end{array}& \left(1\right)\end{array}$

[0024] where (x, y, z) is the actual position of the unmanned helicopter in the three axis directions of the inertial coordinate system, (D.sub.x, D.sub.y, D.sub.z) is the lumped disturbance term in the three axis directions of the inertial coordinate system.

[0025] {dot over (x)} denotes the first derivative of the actual position in the x-axis direction of the inertial coordinate system, that is, the actual speed in the x-axis direction of the inertial coordinate system. {umlaut over (x)} denotes the second derivative of the actual position in the x-axis direction of the inertial coordinate system. The first derivative and second derivative of other parameters are expressed in the same way, and they are not repeated one by one in the subsequent content.

[0026] m is the mass of the unmanned helicopter, g is the acceleration constant of gravity, k.sub.d is the velocity damping coefficient, these parameters are known parameters. In one embodiment, m=6 kg, g =9.8m/s.sup.2, k.sub.d=0.09.

[0027] The control variables of the unmanned helicopter include the main rotor lift force T.sub.m, the roll angle , the pitching angle and the heading angle .

[0028] In order to simplify the description and design, the virtual control variables of a.sub.x, a.sub.y, a.sub.z are introduced:

[00002]$\begin{array}{cc}\{\begin{array}{c}{a}_x=\frac{T_m}{m}\left(\cos \sin \cos +\sin \sin \right)\\ a_y=\frac{T_m}{m}\left(\cos \sin \sin -\sin \cos \right)\\ a_z=\frac{T_m}{m}\cos \cos +g\end{array}& \left(2\right)\end{array}$

[0029] The position dynamic equation based on the control variable shown in Formula (1) can be rewritten as the position dynamic equation based on the virtual control variable as follows:

[00003]$\begin{array}{cc}\{\begin{array}{c}\stackrel{.Math.}{x}=a_x-\frac{k_d}{m}\stackrel{.}{x}+D_x\\ \stackrel{.Math.}{y}=a_y-\frac{k_d}{m}\stackrel{.}{y}+D_y\\ \stackrel{.Math.}{z}=a_z-\frac{k_d}{m}\stackrel{.}{z}+D_z\end{array}& \left(3\right)\end{array}$

[0030] The position measurement equation indicates the relationship between the position measurement value obtained by the position measurement sensor in the unmanned helicopter and the actual position, since the measurement sensor will be disturbed by the measurement error, the position measurement equation contains the measurement noise term used to characterize the measurement error disturbance of the position measurement sensor. The position measurement equation of the position measurement sensor in the unmanned helicopter is established as follows:

[00004]$\begin{array}{cc}\{\begin{array}{c}{x}_m=x+v_m\\ y_m=y+v_{\mathrm{my}}\\ z_m=z+v_{\mathrm{mz}}\end{array}& \left(4\right)\end{array}$

[0031] where (x.sub.m, y.sub.m, z.sub.m) is the position measurement value in the three axis directions of the inertial coordinate system obtained by the position measurement sensor in the unmanned helicopter, (v.sub.mx, v.sub.my, v.sub.mz) is the measurement noise term of the position measurement sensor in the unmanned helicopter in the three axis directions of the inertial coordinate system. The measurement noise term is usually Gaussian white noise satisfying v.sub.mx(0, R.sub.x), v.sub.my(0, R.sub.y) and v.sub.mz(0, R.sub.z), where R.sub.x is the covariance of the measurement noise term v.sub.mx, R.sub.y is the covariance of the measurement noise term v.sub.my, R.sub.z is the covariance of the measurement noise term v.sub.mz. In one embodiment, R.sub.x=R.sub.x=R.sub.x=0.04.

[0032] Step 2, the state vector is constructed based on the actual position (x, y, z) and actual speed ({dot over (x)}, {dot over (y)}, ) of the unmanned helicopter, the position system model of the disturbed unmanned helicopter is transformed into a system state equation based on the state vector, and the filter observer is designed based on the continuous-time Kalman filter according to the system state equation.

[0033] When designing the filter observer in this step, three-channel filter observers are designed for the three axis directions of the inertial coordinate system, including:

(1) For the X-Axis Direction of the Inertial Coordinate System

[0034] Firstly, the state vector X=[x v.sub.x].sup.T and the derivative {dot over (X)} =[{dot over (x)} {dot over (v)}.sub.x].sup.T of the state vector of the unmanned helicopter in the x-axis direction of the inertial coordinate system are constructed. v.sub.x={dot over (x)} denotes the actual speed of the unmanned helicopter in the x-axis direction of the inertial coordinate system, and {dot over (v)}.sub.x={umlaut over (x)} denotes the actual acceleration of the unmanned helicopter in the x-axis direction of the inertial coordinate system.

[0035] Then, according to (3) and (4) of the position system model of the disturbed unmanned helicopter, the system state equation in the x-axis direction of the inertial coordinate system is as follows:

[00005]$\begin{array}{cc}\{\begin{array}{c}\stackrel{}{X}=A_{x}X+B_x{a}_x+B_{\mathrm{dx}}{D}_x\\ x_m=H_{x}X+v_{\mathrm{mx}}\end{array}& \left(5\right)\end{array}$$\mathrm{where}{A}_x=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_x=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dx}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_x=\left[\begin{array}{c}1\\ 0\end{array}\right].$

[0036] Then, based on the system state equation (5), a filter observer in the x-axis direction of the inertial coordinate system is designed to observe the actual position estimated value and the estimated value of the lumped disturbance term in the x-axis direction. The filter observer in the x-axis direction of the inertial coordinate system is designed as:

[00006]$\begin{array}{cc}\{\begin{array}{c}{K}_x=P_x{H}_x^T{R}_x^{-1}\\ \stackrel{.}{\stackrel{~}{X}}=A_x\tilde{X}+B_x{a}_x+B_{\mathrm{dx}}{\hat{D}}_x+K_x\left(x_m-H_x\tilde{X}\right)\\ {\stackrel{.}{P}}_x=-P_x{H}_x^T{R}_x^{-1}{H}_x{P}_x+A_x{P}_x+P_x{A}_x^T+Q_x\end{array}& \left(6\right)\end{array}$$\begin{array}{cc}\{\begin{array}{c}{e}_{\mathrm{Ox}}=z_{x1}-\tilde{x}\\ {\stackrel{.}{z}}_{x1}=z_{x2}-{}_{01}^x{e}_{\mathrm{Ox}}\\ {\stackrel{.}{z}}_{x2}=z_{x3}-{}_{02}^x{e}_{\mathrm{Ox}}-\frac{k_d}{m}{z}_{x2}+a_x\\ {\stackrel{.}{z}}_{x3}=-{}_{03}^x{e}_{\mathrm{Ox}}\\ \hat{x}=z_{x1}\\ \widehat{\stackrel{.}{x}}=z_{x2}\\ {\hat{D}}_x=z_{x3}\end{array}& \left(7\right)\end{array}$

[0037] where K.sub.xR.sup.21 is the Kalman gain, P.sub.xR.sup.22 is the covariance matrix of the state vector estimation X. {tilde over (X)}=[{tilde over (x)} {tilde over (v)}.sub.x].sup.T is the estimated value of the system state in the x-axis direction obtained by the Kalman filter, {tilde over ({dot over (X)})}=[{tilde over ({dot over (x)})} {tilde over ({dot over (v)})}.sub.x].sup.T is the first derivative of the estimated value {tilde over (X)}, {tilde over (x)} denotes the position estimated value in the x-axis direction obtained by the Kalman filter, and {tilde over (v)}.sub.x denotes the velocity estimated value in the x-axis direction obtained by the Kalman filter, denotes the estimated value of the actual position of the unmanned helicopter in the x-axis direction of the inertial coordinate system, and {circumflex over (v)}.sub.x={dot over ({circumflex over (x)})} denotes the actual velocity estimated value of the unmanned helicopter in the x-axis direction of the inertial coordinate system, that is, the estimated value of the first derivative of the actual position. {circumflex over (D)}.sub.x denotes the estimated value of the lumped disturbance term in the x-axis direction of the inertial coordinate system. Q.sub.xR.sup.22 denotes the covariance matrix of the estimated lumped disturbance term {circumflex over (D)}.sub.x, in an embodiment,

[00007]$Q_x=\left[\begin{array}{cc}0& 0\\ 0& 5000\end{array}\right].$

The estimated values of other parameters are also expressed in the same way, and they are not repeated one by one in the subsequent content.

[0038] z.sub.x1, z.sub.x2, z.sub.x3 are the intermediate variables of the filter observer.

[00008]${}_{01}^x,{}_{02}^x,{}_{03}^x$

are positive constants and their values satisfy

[00009]$P^x\left(s\right)=s^3+{}_{01}^x{s}^2+{}_{02}^{x}s+{}_{03}^x$

and belong to Hurwitz, s is the complex frequency domain parameter. In one embodiment,

[00010]${}_{01}^x=18,{}_{02}^x=108,{}_{03}^x=216.$

(2) For the Y-Axis Direction of the Inertial Coordinate System

[0039] The state vector Y=[y v.sub.y].sup.T and the derivative {dot over (Y)}=[{dot over (y)} {dot over (v)}.sub.y].sup.T of the state vector of the unmanned helicopter in the y-axis direction of the inertial coordinate system are constructed. v.sub.y={dot over (y)} denotes the actual speed of the unmanned helicopter in the y-axis direction of the inertial coordinate system, and {dot over (v)}.sub.y= denotes the actual acceleration of the unmanned helicopter in the y-axis direction of the inertial coordinate system.

[0040] Then, according to (3) and (4) of the position system model of the disturbed unmanned helicopter, the system state equation in the y-axis direction of the inertial coordinate system is as follows:

[00011]$\begin{array}{cc}\{\begin{array}{c}\stackrel{.}{Y}=A_{y}Y+B_y{a}_y+B_{\mathrm{dy}}{D}_y\\ y_m=H_{y}Y+v_{\mathrm{my}}\end{array}& \left(8\right)\end{array}$$\mathrm{where}{A}_y=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_y=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dy}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_y=\left[\begin{array}{c}1\\ 0\end{array}\right].$

[0041] Based on the system state equation (8), a filter observer in the y-axis direction of the inertial coordinate system is designed to observe the actual position estimated value and the estimated value of the lumped disturbance term in the y-axis direction. The filter observer in the y-axis direction of the inertial coordinate system is designed as:

[00012]$\begin{array}{cc}\{\begin{array}{c}{K}_y=P_y{H}_y^T{R}_y^{-1}\\ \stackrel{.}{\stackrel{~}{Y}}=A_y\tilde{Y}+B_y{a}_y+B_{\mathrm{dy}}{\stackrel{}{D}}_y+K_y\left(y_m-H_y\tilde{Y}\right)\\ {\stackrel{.}{P}}_y=-P_y{H}_y^T{R}_y^{-1}{H}_y{P}_y+A_y{P}_y+P_y{A}_y^T+Q_y\end{array}& \left(9\right)\end{array}$$\begin{array}{cc}\{\begin{array}{c}{e}_{\mathrm{Oy}}=z_{y1}-\tilde{y}\\ {\stackrel{.}{z}}_{y1}=z_{y2}-{}_{01}^y{e}_{\mathrm{Oy}}\\ {\stackrel{.}{z}}_{y2}=z_{y3}-{}_{02}^y{e}_{\mathrm{Oy}}-\frac{k_d}{m}{z}_{y2}+a_y\\ {\stackrel{.}{z}}_{y3}=-{}_{03}^y{e}_{\mathrm{Oy}}\\ \hat{y}=z_{y1}\\ \widehat{\stackrel{.}{y}}=z_{y2}\\ {\hat{D}}_y=z_{y3}\end{array}& \left(10\right)\end{array}$

[0042] where K.sub.yR.sup.21 is the Kalman gain, P.sub.yR.sup.22 is the covariance matrix of the state vector estimation Y. {tilde over (Y)}=[{tilde over (y)} {tilde over (v)}.sub.y].sup.T is the estimated value of the system state in the y-axis direction obtained by the Kalman filter, {tilde over ({dot over (Y)})}=[{tilde over ({dot over (y)})} {tilde over ({dot over (v)})}.sub.y].sup.T is the first derivative of the estimated value {tilde over (Y)}, {tilde over (y)} denotes the actual position estimated value in the y-axis direction obtained by the Kalman filter, and {tilde over (v)}.sub.y denotes the velocity estimated value in the y-axis direction obtained by the Kalman filter, denotes the estimated value of the actual position of the unmanned helicopter in the y-axis direction of the inertial coordinate system, and {circumflex over (v)}.sub.y={dot over ()} denotes the actual velocity estimated value of the unmanned helicopter in the y-axis direction of the inertial coordinate system. {circumflex over (D)}.sub.y denotes the estimated value of the lumped disturbance term in the y-axis direction of the inertial coordinate system. Q.sub.yR.sup.22 denotes the covariance matrix of the estimated lumped disturbance term {circumflex over (D)}.sub.y, in an embodiment,

[00013]$Q_y=\left[\begin{array}{cc}0& 0\\ 0& 5000\end{array}\right].$

[0043] z.sub.y1, z.sub.y2, z.sub.y3 are the intermediate variables of the filter observer.

[00014]${}_{01}^y,{}_{02}^y,{}_{03}^y$

are positive constants and their values satisfy

[00015]$P^y\left(s\right)=s^3+{}_{01}^y{s}^2+{}_{02}^{y}s+{}_{03}^y$

and belong to Hurwitz. In one embodiment,

[00016]${}_{01}^y=18,{}_{02}^y=108,{}_{03}^y=216.$

(3) For the Z-Axis Direction of the Inertial Coordinate System

[0044] The state vector Z=[z v.sub.z].sup.T and the derivative =[ {dot over (v)}.sub.z].sup.T of the state vector of the unmanned helicopter in the z-axis direction of the inertial coordinate system are constructed. v.sub.z= denotes the actual speed of the unmanned helicopter in the z-axis direction of the inertial coordinate system, and {dot over (v)}.sub.z={umlaut over (z)} denotes the actual acceleration of the unmanned helicopter in the z-axis direction of the inertial coordinate system.

[0045] Then, according to (3) and (4) of the position system model of the disturbed unmanned helicopter, the system state equation in the z-axis direction of the inertial coordinate system is as follows:

[00017]$\begin{array}{cc}\{\begin{array}{c}\stackrel{.}{Z}=A_{z}Z+B_z{a}_z+B_{\mathrm{dz}}{D}_z\\ z_m=H_{z}Z+v_{\mathrm{mz}}\end{array}& \left(11\right)\end{array}$$\mathrm{where}{A}_z=\left[\begin{array}{cc}0& 1\\ 0& -\frac{k_d}{m}\end{array}\right],B_z=\left[\begin{array}{c}0\\ 1\end{array}\right],B_{\mathrm{dz}}=\left[\begin{array}{c}0\\ 1\end{array}\right],H_z=\left[\begin{array}{c}1\\ 0\end{array}\right].$

[0046] Based on the system state equation (11), a filter observer in the z-axis direction of the inertial coordinate system is designed to observe the actual position estimated value and the estimated value of the lumped disturbance term in the z-axis direction. The filter observer in the z-axis direction of the inertial coordinate system is designed as:

[00018]$\begin{array}{cc}\{\begin{array}{c}{K}_z=P_z{H}_z^T{R}_z^{-1}\\ \stackrel{.}{\stackrel{~}{Z}}=A_z\tilde{Z}+B_z{a}_z+B_{\mathrm{dz}}{\stackrel{}{D}}_z+K_z\left(z_m-H_z\tilde{Z}\right)\\ {\stackrel{.}{P}}_z=-P_z{H}_z^T{R}_z^{-1}{H}_z{P}_z+A_z{P}_z+P_z{A}_z^T+Q_z\end{array}& \left(12\right)\end{array}$$\begin{array}{cc}\{\begin{array}{c}{e}_{\mathrm{Oz}}=z_{z1}-\tilde{z}\\ {\stackrel{.}{z}}_{z1}=z_{z2}-{}_{01}^z{e}_{\mathrm{Oz}}\\ {\stackrel{.}{z}}_{z2}=z_{z3}-{}_{02}^z{e}_{\mathrm{Oz}}-\frac{k_d}{m}{z}_{\mathrm{z2}}+a_z\\ {\stackrel{.}{z}}_{\mathrm{z3}}=-{}_{03}^z{e}_{\mathrm{Oz}}\\ \hat{z}=z_{z1}\\ \widehat{\stackrel{.}{z}}=z_{z2}\\ {\hat{D}}_z=z_{z3}\end{array}& \left(13\right)\end{array}$

[0047] where K.sub.zR.sup.21 is the Kalman gain, P.sub.zR.sup.22 is the covariance matrix of the state vector estimation Z. {tilde over (Z)}=[{tilde over (z)} {tilde over (v)}.sub.z].sup.T is the estimated value of the system state in the z-axis direction obtained by the Kalman filter, {tilde over ()}=[{tilde over ()} {tilde over ({dot over (v)})}].sup.T is the first derivative of the estimated value {tilde over (Z)}, {tilde over (z)} denotes the position estimated value in the z-axis direction obtained by the Kalman filter, and {tilde over (v)}.sub.z denotes the velocity estimated value in the z-axis direction obtained by the Kalman filter, {circumflex over (z)} denotes the estimated value of the actual position of the unmanned helicopter in the z-axis direction of the inertial coordinate system, and {circumflex over (v)}.sub.z={dot over ({circumflex over (z)})} denotes the actual velocity estimated value of the unmanned helicopter in the z-axis direction of the inertial coordinate system. {circumflex over (D)}.sub.z denotes the estimated value of the lumped disturbance term in the z-axis direction of the inertial coordinate system. Q.sub.zR.sup.22 denotes the covariance matrix of the estimated lumped disturbance term {circumflex over (D)}.sub.z, in an embodiment,

[00019]$Q_z=\left[\begin{array}{cc}0& 0\\ 0& 5000\end{array}\right].$

[0048] z.sub.z1, z.sub.z2, z.sub.z3 are the intermediate variables of the filter observer.

[00020]${}_{01}^z,{}_{02}^z,{}_{03}^z$

are positive constants and their values satisfy

[00021]$P^z\left(s\right)=s^3+{}_{01}^z{s}^2+{}_{02}^{z}s+{}_{03}^z$

and belong to Hurwitz. In one embodiment,

[00022]${}_{01}^z=18,{}_{02}^z=108,{}_{03}^z=216.$

[0049] Step 3, a composite PID controller is designed for the position system model of the disturbed unmanned helicopter, the same three-channel composite PID controller is designed for the three axis directions of the inertial coordinate system:

[00023]$\begin{array}{cc}\{\begin{array}{c}\frac{T_m}{m}\left(\cos \mathrm{sincos}+\sin \sin \right)=\\ -K_{px}\left(\stackrel{}{x}-x_r\right)-K_{\mathrm{ix}}\left(\stackrel{}{x}-x_r\right)-K_{dx}\left(\stackrel{}{\stackrel{}{x}}-{\stackrel{}{x}}_r\right)-{\stackrel{}{D}}_x\\ \frac{T_m}{m}\left(\cos \mathrm{sinsin}-\sin \cos \right)=\\ -K_{\mathrm{py}}\left(\stackrel{}{y}-y_r\right)-K_{\mathrm{iy}}\left(\stackrel{}{y}-y_r\right)-K_{\mathrm{dy}}\left(\widehat{\stackrel{.}{y}}-{\stackrel{.}{y}}_r\right)-{\stackrel{}{D}}_y\\ \frac{T_m}{m}\cos \cos +g=-K_{pz}\left(\stackrel{}{z}-z_r\right)-\\ K_{\mathrm{iz}}\left(\stackrel{}{z}-z_r\right)-K_{\mathrm{dz}}\left(\widehat{\stackrel{}{z}}-z_r\right)-{\stackrel{}{D}}_z\end{array}& \left(14\right)\end{array}$

[0050] where (x.sub.r, y.sub.r, z.sub.r) is the position reference indicated by the three-dimensional reference trajectory to be followed by the unmanned helicopter. ({circumflex over (x)}, , {circumflex over (z)}) is the actual position estimated value in the three axis directions of the inertial coordinate system, and ({circumflex over (D)}.sub.x, {circumflex over (D)}.sub.y, {circumflex over (D)}.sub.z) is the estimated value of the lumped disturbance term in the three axis directions of the inertial coordinate system.

[0051] K.sub.px, K.sub.ix, K.sub.dx are the proportional coefficient, integral coefficient, and differential coefficient in the x-axis direction of the inertial coordinate system. K.sub.py, K.sub.iy, K.sub.dy are the proportional coefficient, integral coefficient and differential coefficient in the y-axis direction of the inertial coordinate system. K.sub.pz, K.sub.iz, K.sub.dz are the proportional coefficient, integral coefficient and differential coefficient in the z-axis direction of the inertial coordinate system. K.sub.pj, K.sub.ij, K.sub.dj in any direction of j=x, y, z are positive constants and satisfy the polynomial P.sup.j(s)=s.sup.3+K.sub.djs.sup.2+K.sub.pjs+K.sub.ij=0 and belong to Hurwitz. In one embodiment, K.sub.px=12, K.sub.py=K.sub.pz=18.75, K.sub.ix=8, K.sub.iy=K.sub.ix=15.625, K.sub.dx=6, K.sub.dy=K.sub.dz=7.5.

[0052] Step 4, when the trajectory tracking control of the unmanned helicopter is performed, the current control variable of the unmanned helicopter and the position measurement value (x.sub.m, y.sub.m, z.sub.m) of the position measurement sensor are input into the filter observer to obtain the actual position estimated value ({circumflex over (x)}, , {circumflex over (z)}) and the lumped disturbance term estimated value ({circumflex over (D)}.sub.x, {circumflex over (D)}.sub.y, {circumflex over (D)}.sub.z).

[0053] Then, the position error between the actual position estimated value ({circumflex over (x)}, , {circumflex over (z)}) and the three-dimensional reference trajectory (x.sub.r, y.sub.r, z.sub.r) and the estimated value of the lumped disturbance term ({circumflex over (D)}.sub.x, {circumflex over (D)}.sub.y, {circumflex over (D)}.sub.z) are input into the composite PID controller (14), and the unmanned helicopter is controlled according to the command value of the control variable output by the composite PID controller.

[0054] In the actual flight process of the unmanned helicopter, in order to facilitate the system control, it is always hoped that the heading angle of the unmanned helicopter is kept to zero, since this application mainly involves position control, the command value .sup.d of the heading angle can be set to 0, that is, the fixed =0 is substituted into Formula (14), and then the command value

[00024]$T_m^d$

of the main rotor lift force T.sub.m, the command value .sup.d of the roll angle , and the command value .sup.d of the pitching angle 0 can be obtained by the negative solution:

[00025]$\begin{array}{cc}\{\begin{array}{c}{T}_m^d=m\sqrt{a_x^2+a_y^2+{\left(a_z-g\right)}^2}\\ {}^d=\arctan \left(\frac{a_x}{a_z-g}\right)\\ {}^d=\arcsin \left(m\frac{a_y}{T_m^d}\right)\end{array}& \left(15\right)\end{array}$

[0055] In order to illustrate the effect of the trajectory tracking control method for the unmanned helicopter applied in this invention, the model of the unmanned helicopter is built by the Simulink toolbox in the simulation software MATLAB R2020b for simulation verification. In the case of fully considering the system lumped disturbance and measurement noise, in addition to the parameter settings provided in the above embodiment, other parameters in the simulation scenario are set as follows:

[0056] In the simulation example, m=6 kg, g=9.8 m/s.sup.2, k.sub.d=0.09 are set and the initial state of the unmanned helicopter as:

[00026]$\{\begin{array}{c}x\left(0\right)=0,y\left(0\right)=0,z\left(0\right)=0\\ v_x\left(0\right)=0,v_y\left(0\right)=0,v_z\left(0\right)=0\end{array}$

[0057] The position command of the three-dimensional reference trajectory to be followed by the unmanned helicopter at any time t is as follows:

[00027]$\{\begin{array}{c}{x}_r\left(t\right)=\cos \left(0.2t\right)\\ y_r\left(t\right)=\sin \left(0.2t\right)\\ z_r\left(t\right)=t\end{array}$

[0058] The lumped disturbance term is set to:

[00028]$\{\begin{array}{cc}{D}_x=D_y=3,D_z=2& 0s<t<20s\\ D_x=-3,D_y=D_z=-2& 20s<t<50s\end{array}$

[0059] In order to make the simulation results closer to the real situation, in the control process, the value range of the virtual control variable and its change rate in each channel direction is limited as follows:

[00029]$\{\begin{array}{cc}-30a_{x}30,& -50{\stackrel{.}{a}}_{x}50\\ -30a_{y}30,& -50{\stackrel{.}{a}}_{y}50\\ -30a_{z}30,& -50{\stackrel{.}{a}}_{z}50\end{array}$

[0060] In the same simulation scenario, the traditional method of composite PID controller combined with an extended state observer (denoted as PID+ESO) and the method of composite PID controller combined with a filter observer (denoted as PID+KFESO) are used to control the trajectory tracking of the unmanned helicopter, and then the two control methods are compared and verified.

[0061] The three-dimensional space trajectory tracking response curve of the unmanned helicopter under the two control methods is shown in FIG. 2, and the three-channel tracking error response curve is shown in FIGS. 3A-3C. It can be seen from FIGS. 3A-3C, the steady-state tracking accuracy of the proposed PID+KFESO control method is higher than that of the traditional PID+ESO method. In order to show the steady-state tracking accuracy of the two control methods more intuitively, the mean absolute value of tracking error (MAE) is introduced to quantify the steady-state tracking accuracy. When the system reaches a steady state (t>30 s), the three-channel MAE response curve of trajectory tracking under the two control methods is shown in FIGS. 4A-4C, the statistical comparison results are as follows:

TABLE-US-00001 x -axis MAE(m) y -axis MAE(m) z -axis MAE(m) PID + ESO 0.0336 0.0346 0.0422 PID + KFESO 0.0397 0.0295 0.0373

[0062] It can be clearly seen from the above table data and FIGS. 4A-4C that the steady-state tracking accuracy of the proposed control method is better than that of the traditional PID+ESO method.

[0063] The response curves of the virtual control variable of the unmanned helicopter under the two control methods ARE shown in FIGS. 5A-5C. From FIGS. 5A-5C, it can be seen that the smoothness of the control variable of the PID+KFESO control method applied in this application is better than that of the traditional PID+ESO method, indicating that the method applied in this application can reduce the frequency of the reciprocating motion of the actuator, which is more in line with the engineering practice. In order to more intuitively show the control smoothness of the two control methods, the mean absolute value of acceleration change rate (MAR) is introduced to quantify the control smoothness. The three-channel MAR response curve of the control volume change rate under the two control methods is shown in FIGS. 6A-6C, the statistical comparison results are as follows:

TABLE-US-00002 x -axis MAR y -axis MAR z -axis MAR PID + ESO 43.5975 44.5635 45.4097 PID + KFESO 5.4346 6.6564 7.2974

[0064] It can be clearly seen from the above table data and FIGS. 6A-6C that the smoothness of the control variable of this application control method is better than that of the traditional PID+ESO method.

[0065] The estimation error response curves of the two control methods for the lumped disturbance term are shown in FIGS. 7A-7C. From the estimation error response curve of FIGS. 7A-7C, it can be seen that the proposed control method improves the estimation accuracy of the lumped disturbance term compared with the traditional PID+ESO method.

[0066] The above is only the preferred embodiment of this application, and this application is not limited to the above embodiment. Understandably, other improvements and changes directly derived or associated by technicians in this field without deviating from the spirit and conception of this application should be considered to be included in the scope of protection of this application.