An Improved Nonlinear Dynamic Inversion Method for Altitude and Attitude Control of Nano Quad-Rotors under Persistent Uncertainties

Chen Meili,Wang Yuan

College of Aerospace Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 210016,P.R.China

Abstract:Nonlinear dynamic inversion(NDI)has been applied to the control law design of quad-rotors mainly thanks to its good robustness and simplicity of parameter tuning.However,the weakness of relying on accurate model greatly restrains its application on quad-rotors,especially nano quad-rotors(NQRs).NQRs are easy to be influenced by uncertainties such as model uncertainties(mainly from complicated aerodynamic interferences,strong coupling in roll-pitch-yaw channels and inaccurate aerodynamic prediction of rotors)and external uncertainties(mainly from winds or gusts),particularly persistent ones.Therefore,developing accurate model for altitude and attitude control of NQRs is difficult.To solve this problem,in this paper,an improved nonlinear dynamic inversion(INDI)method is developed,which can reject the above-mentioned uncertainties by estimating them and then counteracting in real time using linear extended state observer(LESO).Comparison with the traditional NDI(TNDI)method was carried out numerically,and the results show that,in coping with persistent uncertainties,the INDI-based method presents significant superiority.

Key words:nonlinear dynamic inversion;extended state observer;nano quad-rotor;uncertainties rejection;altitude control;attitude control

Nomenclature

(φ,θ,ψ)T/radEuler angle

(p,q,r)T/(rad·s-1)Angular velocity

Tr/NLift

u2/(N·m)Roll torque

u3/(N·m)Pitch torque

u4/(N·m)Yaw torque

Ix/(kg·m2)Roll inertial

Iy/(kg·m2)Pitch inertial

Iz/(kg·m2)Yaw inertial

l/mLever

(φd,θd,ψd)T/radDesired Euler angle

fφ/(rad·s-2)Unknown nonlinear part of roll angle subsystem

fθ/(rad·s-2)Unknown nonlinear part of pitch angle subsystem

fψ/(rad·s-2)Unknown nonlinear part of yaw angle subsystem

gφ/(kg·m)Given-part of roll angle equation

gθ/(kg·m)Given-part of pitch angle equation

gψ/(kg·m2)Given-part of yaw angle equation

0 Introduction

During the past decades,many researchers have focused on various kinds of control methods,especially the ones rely on specific mathematical models,to improve the control of quad-rotors.For example,reduced model based PID and linear quadratic(LQ)methods[1-3],accurate model based nonlinear feedback linearization method[4-7],and nonlinear dynamic inversion[8]were developed.Some other methods were proposed for allowing partial model information to vary within certain range[9-10].Though there are some methods relying on no detailed model[11-12],they are quit tedious in design process.

Only a few works have discussed the flight of ordinary-sized and micro-sized quad-rotors under external uncertainties using the above mentioned methods[12-17].In addition,their mechanisms on disturbances rejection mainly focus on enhancing the robustness of the control system and resisting instantaneous uncertainties,but they take no consideration on persistent ones,which may be a usual case in reality.

Therefore,according to the above discussion,in the design of control system of nano quad-rotors(NQRs),two problems need to be addressed under persistent uncertainties.Firstly,controllers should have good robustness.Secondly,controllers should not rely on accurate model.To solve those problems,in this paper,a robust INDI method based on the TNDI method and the linear extended state observer(LESO)is developed to design the altitude and attitude control law for NQRs.As known to all,NDI-based controller has some advantages[18],for instance,its structure is very simple,only a few parameters need to be tuned and their determination is very simple according to the bandwidth of state variables.Generally,the control effect in the TNDI method completely depends on the accuracy of the model.As discussed above,the internal and external uncertainties cannot be modeled precisely or some even cannot be modeled.To overcome this drawback of TNDI,the LESO is introduced into NDI to estimate the uncertainties using only input and output data of the control system.Hence,the uncertain dynamics in the controllers can be predicted and then counteracted.To show the superiority of the INDI-based method,a numerical comparison with TNDI was carried out.

1 Structure of Altitude and Attitude-Control Systems for NQRs

Before modeling NQRs,two coordinates(inertial and body),some forces,moments and geometrical parameters are introduced first,as shown in Fig.1.

Fig.1 Coordinates description of NQRs

In Fig.1,OEExEyEzrepresents the inertial coordinate,in which Exand Eyare on the horizontal plane and Exis perpendicular to Ey,Ezis perpendicular to the horizontal determined by the right-hand rule.OBBxByBzrepresents the body coordinate centered at the center of gravity of the NQRs.Bxis the normal flight orientation,Byis positive to starboard in the horizontal plane and Bzis orthogonal to the plane BxOBBy.

The nonlinear movement equations of NQRswithout uncertainties are expressed as

where P,v∈R3represent position and velocity of NQRs in inertial frame,respectively,m and g represent mass of NQRs and gravitational constant,respectively,ω=[p,q,r]T∈R3is angular velocity in body coordinate,Θ=[φ,θ,ψ]T∈R3is Euler angle in inertial coordinate,T andτare the total force and moment act on the frame of NQRs,ez=[0,0,1]Tis a vector along Ez,and I=diag(Ix,Iy,Iz)is the moment of inertial matrix.The rotation matrix R(Θ),which transforms a vector from inertial coordinate to body coordinate,has an expression as

And the attitude kinematic matrix K(Θ)is defined as

During the flight,NQRs may be influenced by internal and external uncertainties,for example,the vibration derives from the asymmetry of eccentricity of rotor shafts,asymmetry of rotor blades and asymmetry of frame,winds and gusts and so forth.Hence,taking consideration of uncertainties,Eq.(2)yields the altitude and attitude movement equations of NQRs,shown as

where

In Eq.(4), [Δτroll,Δτpitch,Δτyaw]Trepresents the movement disturbances and[Δdroll,Δdpitch,Δdyaw]Trepresents the internal uncertainties.

2 Brief Introduction of LESO

2.1 Fundamental theory of high-order LESO

The(n+1)th-order LESO is mainly used to observe n th-order control system.Take the following n th-order linear affine differential system as an example

where u and y are input and output ofΣ,respectively,and b＞0.Assume that f(x1,…,xn,t)is bounded and differentiable and its derivative with respect to t is n(x1,…,xn,t).Also bounded,this assumption is always reasonable in the control of quad-rotors.Then systemΣcan be extended as the following formation

Using a linear state observer[19-21]to observe Σextendyields

The above systemΣESOis the so-called LESO.Thus,zitracks xi(i=1,…,n)and zn+1estimates f(x1,…,xn,t).

2.2 Stability analysis of LESO

In this part,stability of the(n+1)th-order LESO is proved.Firstly,rewrite Eq.(7)as the following formation

where

And Eq.(8)can be rewritten as

where

Hence,subtracting Eq.(9)from Eq.(11)yields

Since n=n(x1,…,xn,t)is a bounded function as defined before,the(n+1)th-order LESO is bounded-input bounded-output(BIBO)if the roots of the characteristic polynomial of AE,shown as

are all in the left half plane.Thus,stability of the(n+1)th-order LESO is proved.

3 Design of Altitude Control Law

3.1 Design of NDI-based altitude control law

In this part,an altitude control law based on NDI is derived.During the motion of NQR,cosφ≈1,cosθ≈1 andΔTris uncertain.Hence,in NDI,Eq.(2)is usually rewritten as

Denoting fz=-g andyields

Solving the above equation yields the desired thrust,that is

Assume that hdrepresents the desired trajectory of altitude and h represents the feedback trajectory of altitude,then the tracking error e=hdh satisfies the following equation

Therefore,by substituting e=hd-h into the above equation,expression ofcan be obtained as

According to the above analysis,the overall structure of the altitude control system based on NDI is shown in Fig.2.

Fig.2 Structure of the altitude control system based on NDI for NQRs

3.2 INDI-based altitude control law improved using LESO

In NDI,ΔTris ignored,which always results in the low accuracy of the counteraction of nonlinear terms sinceΔTrusually affects the quality of the controller and the robustness of the system.In this part,ΔTris taken into account by taking advantage of LESO.In Eq.(16),considering the disturbanceΔTrand putting it into fzyields

Using third-order LESO to observe the above extended system yields

Therefore,z1tracks x1,z2tracks x2,and z3estimates

By using LESO to make the estimation,the control scheme changes into the following pattern,as shown in Fig.3.

Fig.3 Structure of the INDI-based altitude control system improved using LESO for NQRs

A direct method to verify the correctness of this theory is to check the agreement betweenand,which will be checked in simulations hereinafter.

3.3 Parameter tuning principles of altitude control

On one hand,in Eq.(18),following the relationship between kdand kpcan ensure the disappearance of overshoot

On the other hand,to derive the parameters of the third-order LESO,transfer function of Eq.(22)from z3to y and Trneeds to be obtained,shown as

whereωzis called bandwidth of roll angular velocity channel and thus the transfer function turns into

Therefore,in the altitude control system,only one parameter needs to be tuned sinceλcan be determined empirically,for example,its recommended value can be 1 or 2.

4 Design of Attitude Control Law

4.1 Design of NDI-based attitude control law

To derive the Euler angle control law,Eq.(2)needs to be addressed.

Denote

Hence,Eq.(2)can be rewritten as

Then the desired angular velocity and the input of angular velocity controller can be obtained as

and [φ,θ,ψ]Tis the real-time feedback value of attitude and K1=diag(ω1,ω1,ω1).

After deriving the Euler angle controller,the angular velocity controller needs to be designed.

In Eqs.(4),(5),denote

And rewrite Eq.(4)as

Then the desired virtual input can be solved,shown as

The desired angular velocityhas been obtained in above,[p,q,r]Tis the real-time feedback value of angular velocity,and K2=diag(ω2,ω2,ω2).

According to the above analysis,the overall structure of the attitude control system is shown in Fig.4.

Fig.4 Structure of the NDI-based attitude control system for NQRs

The above analysis also clearly shows the biggest weakness of TNDI,that is,the robustness and quality of the attitude controller depends on the modeling precision of[ fp,fq,fr]T.Ref.[19]has proved that TNDI can address the case with small perturbations in[ fp,fq,fr]T;while with large perturbations in[ fp,fq,fr]T,for instance,in the existence of persistent external uncertainties,the TNDI cannot perform well.In most cases,the nonlinearity of the model can be counteracted completely only when the nonlinear terms[ fp,fq,fr]Tare accurate enough,which restrains the application of TNDI.

4.2 INDI-based attitude control law improved using LESO

To overcome the weakness discussed above,LESO is used to estimate the accurate and real time value of[ fp,fq,fr]T.Hence,it is necessary to introduce the LESO first and then give the improved control law based on LESO.In this paper,only the estimation of fpusing LESO is introduced as fqand frare very similar.

In the first equation of Eq.(2),denote that

Hence,the roll angular velocity system is rewritten as

Using second-order LESO to observe the above extended system yields

Therefore,z1tracks p and z2estimates fp.

By employing LESO to conduct the estimation,the control scheme changes into the following pattern,as shown in Fig.5.

A direct method to verify the correctness of this theory is to check the agreement between[p,q,r]Tand[ pESO,qESO,rESO]T,which will be checked in simulations hereinafter.

Fig.5 Structure of the INDI-based attitude control system using LESO for NQRs

4.3 Parameter tuning principles of attitude control

In one way,in the attitude controller,the parametersω1andω2together determine both the tracking precision and the response speed of the system.Since the response speed of angular velocity is much faster than the one of Euler angle,thus,the two parameters usually satisfy the following relationship[19]

Put another way,in the LESO,anddetermine the tracking precision and speed,too small value may cause a bad tracking performance and too large value may result in divergence of the system.Taking the transfer function of Eq.(34)yields

Notice that it is a typical second order system and to avoid the overshoot,Gao[22]also recommended a method to determineand,shown as

whereωpis called bandwidth of roll angular velocity channel and thus,the transfer function turns into

Hence,in each angular velocity channel(roll,pitch and yaw),only two parameters need to be determined.

5 Numerical Validation

Two numerical simulations were conducted.The first one aims to demonstrate the superiority of INDI compared with TNDI in the existence of model uncertainties;and the second one in turn demonstrates the superiority of INDI compared with TNDI in the existence of both external uncertainties and model uncertainties.

5.1 Variables and parameters

The parameters of the NQR used in the simulations are listed in Table 1.

The initial conditions are given as

Table 1 Parameters of NQR

The desired altitude and attitude trajectories are given as

Values of parameters of NDI controllers are shown in Table 2.Values of parameters of LESOs are shown in Table 3.

Table 2 Values of parameters of NDI controllers

Table 3 Values of parameters of LESOs

5.2 Case study:INDI vs.TNDI under low and high frequency uncertainties

In this part,low and high frequency uncertainties were considered together.Δdroll,ΔdpitchandΔdyawwere assumed to be the triangle functions including low and high frequency components.This assumption is reasonable since such uncertainties derive from many aspects,for instance,eccentricity of rotor shafts,asymmetry of both rotor blades and frame,body vibration on sensors,unstable voltage of circuit and larger ripple current,and they can be modeled and extended to the trigonometric series.Furthermore,there was an additional mass(8%of the mass of NQR)added on the NQR during its flight meanwhile.The figures of state variables and outputs of NQR are shown in Figs.6—21.

Fig.6 Response of altitude

Fig.7 Tracking error of altitude response

Fig.8 Response of roll angle

Fig.9 Response of pitch angle

Fig.6,Fig.7 and Fig.14 show the simulation results in altitude control using the INDI-based and TNDI-based methods.Fig.6 and Fig.7 together depict the tracking results,and obviously the tracking effect derived from the INDI-based method is much better(also acceptable)than the one from the TNDI-based method since the former one owns much higher accuracy.The correctness of the estimation obtained by LESO is also demonstrated in the three figures since the curves of the estimated state variables are overlapped with the ones of the real state variables.

Fig.10 Response of yaw angle

Fig.11 Roll angular rate

Fig.12 Pitch angular rate

Fig.13 Yaw angular rate

Fig.14 Estimation of fzby LESO

Fig.15 Estimation of fpby LESO

Fig.16 Estimation of fqby LESO

Fig.17 Estimation of frby LESO

Figs.8—10 show the results in attitude control using both methods.It is clear that the INDI-based method can still hold the attitude of the NQR steady even under persistent high/low frequency disturbances.

Fig.18 Speed of rotor 1

Fig.19 Speed of rotor 2

Fig.20 Speed of rotor 3

Fig.21 Speed of rotor 4

Figs.18—21 show the desired rotor speed of the NQR.Notice that the curves of the input variables in the TNDI-based method are much smoother than those in the INDI-based method.The reason is that,in TNDI uncertainties rejec-tion mostly relies on the robustness of its controller.When uncertainties are added into the plant,the controller cannot make response to them,which results in the smooth curves.While situations are different in the INDI-based method since LESO has the ability to estimate uncertainties,and then counteract them in each time step,which results in the fluctuation of the curves of the input variables in INDI-based method.This also in turn explains why the INDI-based method has better tracking precision in both altitude and attitude controls.

6 Conclusions

An INDI-based method is developed to design the altitude and attitude control systems for NQRs.To solve the problem that TNDI relies heavily on accurate model of NQR,which is difficult to be obtained,and to retain the robustness of TNDI,the LESO is introduced into TNDI to estimate the model and external uncertainties and then counteract in real time.Comparison between simulation results of the two methods shows that the INDI-based method can reject the uncertainties better,and it does not rely on the accurate model,presenting significant superiority.

Acknowledgements

This work was supported by the Priority Academic Program Development of Jiangsu Higher Education Institutions(PAPD)and the Advanced Research Project of Army Equipment Development(No.301020803).