Incremental Nonlinear Dynamic Inversion For Quadrotor Control

Simulated Quadrotor ellers T, τ Ang. Dynamics ω̇dwindRPKinematicsθθ̇RθLin. Dynamics edSensorFigure 3: Block diagram of the Quadrotor Model implemented in simulink.it when uncertainties exist in both the inertia and control matrices in equation 19.Quad approximation2datafitted curve1.8ẋ ẋ0 G ( x0 , u0 ) u G ( x ) u1.6(19)1.4Applying the INDI developed for the incrementalmodel to the uncertain system shown in equation 19,results in the closed loop system:Thrust (N)1.210.8ẋ ẋ0 G ( x0 , u0 ) G ( x0 , u0 ) 1 [ν x 0 ]0.6 G ( x ) G ( x0 , u0 ) 1 [ν x 0 ]0.4 ẋ0 I [ν x 0 ] G ( x ) G ( x0 , u0 ) 1 [ν ẋ0 ]0.20406080100120140160 ν G ( x ) G ( x0 , u0 ) 1 ν180Propeller rotational speed(Hz) G ( x ) G ( x0 , u0 ) 1 ẋ0Figure 4: Quadratic regression line fit and data(20)Assuming that the sensor provides ideal measurecontroller, the real control inputs need to be updated ments, the difference between new and current angularby increments as unew ucurrent u. When applying acceleration is very small for a high sample rate, rethe resulting controller to the original system, eq. 16, sulting in ẋ0 ' ẋ, this allows to rewrite equation 20as:the following is obtained:ẋ ' ẋ0 G ( x0 , u0 ) G ( x0 , u0 ) 1 (ν ẋ0 )ẋ ν G ( x ) G ( x, u0 ) 1 ν(18) G ( x ) G ( x, u0 ) 1 ẋ'ν(21)In figure 5 the usual architecture that was explained in Equation 21 can be rearranged into equation 22 inthis section is presented in a block diagram, shown in order to conclude that ẋ ν.hi 1figure 5. The advantages of INDI are clear in equationẋ I G ( x ) G ( x, u0 ) 1hiI G ( x ) G ( x, u0 ) 1 νLinearController νAngular AccelerationStatesVirtual InputINDIIncremental Input-QuadcopterDynamics Previous Input-1Z(22)StatesThis is an interesting result that shows that even withuncertainties in the control matrix, with a high samplerate, the INDI controller is robust.InputsInputsFigure 5: General block diagram of an INDI controller.V.17, by using the angular acceleration measurementsall uncertainties that do not depend on the input areeliminated. The dynamic inversion is considerably simplified by including in the measurements the uncertaincomponents. Recalling the system in its incrementalform presented in equation 16, it is possible to rewriteSensor Noise, Delay and SamplingFrequencyThe angular acceleration measurements are a criticalissue in the design of an INDI controller. Usually,the angular acceleration is not measured directly, itssensors are not widely available, specially for small4

where a short notation was used: c(), s() and t() representing cos(), sin() and tan() respectively:unmaned aircrafts.This results in the need of using indirectly obtainedangular accelerations. The process of differentiationof the velocity amplifies the noise of the measurement,resulting in an angular acceleration measurement withamplified noise[3]. Consequentially, this indirectlyobtained angular acceleration measurement does notagree with the assumptions of perfect measurementsand ideal sensors made until now.As the INDI controller decouples the states of the system it is possible to address the filtering problem byconsidering simple signal filtering. A compromise between delay and noise with a low pass filter needs tobe found. The development of INDI also assumed thatthe sampling rate was high in order to allow somesimplifications to take place. The model implementedis inspired on a quadrotor that has a sampling rate of512Hz, which was considered high and allowed for theassumptions to be considered valid.VI.d( J (θ )ω )d2 y 2dtdt 1 s ( θ1 ) t ( θ2 )d 0c ( θ1 ) dx angs ( θ1 )0c(θ )2 c ( θ1 ) t ( θ2 )ω1 s ( θ1 ) ω2 ẋ angc ( θ1 )ω3c(θ )(26)2It is then clear that in this case the input explicitly appears in the expression. From this point it is possibleto deduce the INDI controller for the complete systemby performing a Taylor expansion in the current timestep. This expansion will provide the linearised relation between the speed of the propellers and the Eulerangles.ÿ f ( x ang , u)(27) f ( x ang , u)ÿ f ( x ang0 , u0 ) x ang Attitude Controller f ( x ang , u) ux ang0 ,u0x ang0 ,u0( x ang x ang0 ) ( u u0 )(28)The linearisation shown in 28, assumes f ( x ang0 , u0 ) ẍ ang0 as the angular acceleration measurement in theprevious time step. With a very small sampling time,and considering that the actuator dynamics is fasterthan the other dynamics of the system, it is possible to assume that ( x ang x ang0 ) 0 when compared withθ1 (u u0 ). The assumption that the actuator dynamics Ω1 θ2 isfaster than the system dynamics is quite bold in a θ3 θ u Ω2 (23) quadrotor system, especially because the quadrotorx ang ωΩ3 ω1 ω2 system dynamics is primarily dependent on the actuaΩ4ω3tors dynamics. The simplified expression that will belater used to obtain the control law can be seen as:Where Ω, represents the rotational speed of the propellers of the quadrotor.ÿ ' ÿ0 G ( x ang0 , u0 ) u(29)Since the control variable that we are aiming to control is the Euler angles, we can define θ as our output where matrix G ( x ang0 , u0 ) is simply the derivative invariable and later as control variable, as seen in 24.order of the control inputs of the system, resulting in: G ( x ang0 , u0 ) y θ Hx ang I3 3 03 3 x ang(24) From now on we will take our state as x ang that includes θ and ω as can be seen in 23, where θ1,2,3 arethe Euler angles (pitch, roll and yaw) and ω1,2,3 are thebody rotational speed around x ang , y ang and z ang bodyaxes.I. F1 r Ω1 J (θ ) 0 I F1 Kτ ΩINDI Loop1The goal of the attitude loop is to control the Eulerangles. The direct relation between input and outputvariables was determined by executing the second order derivative of the control variable vector, as the firstorder time derivative of the control variable vector doesnot contain the control input u, as can be seen: dHx angdyJ (θ )ω H H x ang J (θ )ω (25)ω̇dtdt003 4 F2 r Ω2 F2Kτ Ω2 F3 r Ω030 F3 Kτ Ω3 F4r Ω4 F4Kτ Ω4 (30)Ωi Ωi0Where (2Kt2 Ωi Kt2 ), to complete the deduction of the control law from equation 29, one just hasto solve in order of the incremental input. One canthen obtain the input for the system with the simplerelation u (u u0 ), where u0 is the previous input.The state that includes the control of the total forceproduced by the system was not yet considered. Thismeans that the control law will have to consider TF asa control variable. Resulting in a new control law that Fi Ωi5

θ̈refwas obtained from: dangΩreq Quadcopter ModelAttitude Controllerω̇1lF1 lF3 ω̇2 lF2 lF4 I 1 I 1 ω Iω (31)quad τ τ τ τ quad ω̇3 01 12314FTF1 F2 F3 re Iquad is a newly defined 4 4 matrix, as seen inRθθFilter32. Ixx 00 0Figure 6: Block diagram of the attitude controller with 0 Iyy 0 0 filters. Iquad (32) 00 Izz 0 Attitude Controller000 1FTref u Gquad ( x ang0 , u0 ) 1 (cvquad ẋ0quad ) θ Rθ̈G ( x ang0 , u0 )Gquad ( x ang0 , u0 ) G f orce (u0 ) Kaθ̈θθΩ0Ω0 RKdνcINDIθ̇Ωreqθ̈(33)Figure 7: Block diagram of the attitude controller detail.where cvquad [θ̈1 3 FT ] T is the virtual control vectorand ẋ0quad [θ̈01 3 FT0 ] T is the measurement of theangular acceleration and FT . In this last measurement,TF0 is not obtained directly but relies on the modelthat relates the propeller speed to the thrust produced.And the new matrix Gquad ( x ang0 , u0 ) is defined as seenin 34, with G f orce (u0 ) [2Kt2 Ω10 Kt1 2Kt2 Ω20 Kt1 2Kt2 Ω30 Kt1 2Kt2 Ω40 Kt1 ]. FTref θrefFor implementation purposes this control change wasimplemented by simply changing the previously mentioned control law 29 in matrix G to also consider therecent addition. This way the new control law will bethe same:for a Euler rate controller and only afterwards the second loop was implemented, followed by the tuningof the second gain related with the attitude controller.The filters chosen had a cutting frequency of 50Hzand a damping ratio of 5, as explained above theseparameters were shared among all the filters.(34)III.ResultsDegreesDegreesWith the controller implemented the doublet responseThe controller obtained is therefore a non-linear confor a Euler angle reference is shown in figure 8.troller that is slightly less dependent on the model, butthat is built upon sound angular acceleration measurePitch angle with noisements that cannot be obtained directly. Even though50referencethe controller does not depend on part of the model,measurement0it is verifiable that the part of the controller that is-50012345678910time (s)neglected only slightly influences the quadrotor beRoll angle with noise50haviour and therefore the advantage gained by dis0regarding the slower dynamics of the model is not-50critically meaningful.012345678910time (s)Yaw angle with noiseII.Degrees50Attitude Loop0-50012345time (s)678910NTotal Force without noiseWith the correct cancelling of the non-linearities and10the possible decoupling between states, it is feasible5to implement two additional linear proportional loops0012345678910time (s)to control the Euler rates and the Euler angles sequentially. The overall attitude controller block diagram Figure 8: Attitude response to a doublet on the attitudecan be visualized in figure 6. As it is noticeable in reference.figure 7, the total force does not require any additionalloop to be controlled. The gains for the proportionalWhen requesting very steep pitch or roll angles it isloop where chosen sequentially, first tuning the gain clear that some coupling occurs between the states, as6

for example near second 4. In this situation, the noise The control law obtained by solving to the incrementaland delay are not a critical factor. The reason behind input is partially dependent on the defined model, butthe coupling seen in figure 8 is actuators saturation.does not depend neither on the earth acceleration northe aerodynamic drag:VII.Position Controller 1 U Gouter(ν V̈earth )(38)Ucurrent U previous UThe proposed position controller follows a similar ideaas the one developed for the attitude control. Thefirst control layer will make use of the IncrementalNon-linear Dynamic Inversion technique, this timeto cancel the non-linearities associated to the lineardynamics system. This proposed controller also includes a Pseudo Control Hedging to shape the inputrequested to the controller, taking into considerationthe limitations of the system.It is possible to notice a direct relation between boththe Euler angles in matrix R and FT and the angular accelerations that are the states that need to be controlledat this stage. The virtual control variables chosen forthe position INDI controller are defined as ν in 35 andinclude the linear accelerations and the input of thesystem will be the pitch and roll angle, that togethercompose the tilt angle, and the total force but does notinclude the yaw angle. ẍθ1ν ÿ U θ2 (35)z̈ EFTI.(39)ResultsA doublet response for a reference of linear accelerations of the system is presented in figure 9, whereit is possible to verify a very satisfactory results thatshows us that indeed the non-linearities of the systemare correctly cancelled. There is very clear couplingsbetween the horizontal accelerations and the verticalaccelerations. The couplings mentioned before can beX 78910678910678910time (s)Y acceleration10m/s250-5-1001234The approximated system, after performing the Taylorexpansion and disregarding the slower dynamics ofthe system, will result in the system presented in:5time (s)Z acceleration10m/s250-5-10012345time (s)ν ẍ0 Gouter U(36) Figure 9: Linear acceleration response for a doublet inthe linear acceleration reference.Where matrix Gouter can be defined as the derivative inorder to the input variables, θ1 and θ2 and TF as:traced back to the angular references given from theouter loop to the attitude controller. The attitude an a b dgles required to achieve a requested acceleration areGouter e f h (37) very steep and therefore the maximum thrust is notk l oenough to maintain the vertical acceleration to the referenced value. This can be verified in figure 10, wherewith each element specified ahead due to space man- the attitude requested by the outer loop is shown as aagement issues:reference with the attitude response.a FT0 (cos(θ1 ) cos(θ2 ) cos(θ3 ))II.b FT0 ( sin(θ2 ) sin(θ1 ) cos(θ3 ) cos(θ2 ) sin(θ3 ))d sin(θ1 ) cos(θ2 ) cos(θ3 ) sin(θ2 ) sin(θ3 );Pseudo Control Hedging (PCH)Due to the incremental nature of the controller and thetwo additional linear PD an P gains it is possible for thecontroller to request a roll or pitch angle bigger than90o . For this reason a saturation of 80o was includedin the reference provided for the pitch and roll angle.The reach of the saturations might affect controllabilitybecause discontinuities violate the dynamic inversionrequirements, reinforcing the need to find a policy toavoid saturations.e FT0 (cos(θ2 ) sin(θ3 ) cos(θ1 ))f FT0 ( sin(θ2 ) sin(θ1 ) sin(θ3 ) cos(θ2 ) cos(θ3 ))h cos(θ3 ) sin(θ2 ) sin(θ3 ) sin(θ1 ) cos(θ2 )k FT0 cos(θ2 ) sin(θ1 )l FT0 ( sin(θ2 ) cos(θ1 ))o cos(θ1 ) cos(θ2 )7

ime (s)Roll angle50DegreesIn the block diagram presented in figure 12 it is possible to verify the framework were the PCH algorithm isincluded, receiving both the attitude angles requiredand the current angular state of the system, and providing the νhedged to be considered in the reference model.Pitch angle500-50012345Degreesẍrefẍθtime (s)Yaw angle50θ3refPosition ControllerAttitude controllerandAngular Dynamicsθ1&2refFTrefFTθLinear edgedtime (s)Total ForcePCHθFilterθ3ẍ210123456789ẍFilter10time (s)Figure 10: Attitude requested from the outer for the Figure 12: Block diagram including the Pseudo ControlHedging blocks.inner loop, and the attitude responseIII.PCH modifies the virtual control, providing a preadaptation of the control reference before it is providedto the INDI controller. PCH can be divided into twomain parts, a part that includes the reference model(RM), a first order filter that will cut-off frequencies thatare physically infeasible, as seen in the block diagrampresented in figure 11. The second part of the PCHPosition controllerWith the INDI controller of the outer loop it is possibleto control the linear acceleration of the quadrotor. Byadding two linear outer loops to control the velocityand position, results in the controller shown in theblock diagram presented in figure 13. The linear gainsPosition ControllerνhedgedνhedgedReference Modelxref x RνhedgedvcSaturation Kh Rvchedgedẍ0 RKddpνcRMvchedgedKdpINDIposẋθΩ0ddt Kp[θ1θ2TFT ]refẍ0θΩ0Propeller’sModelF T0Figure 13: Block diagram of the position INDI controller.Figure 11: Reference model block diagram.shown were obtained, once again by first analysing thespeed controller and tuning its gains. And only afteralgorithm consists on the estimation of how much that addressing the proportional gain for the positionthe plant did not move due to the limitations of the error.attitude dynamics, or as seen from the outer loop, theactuators that are not considered in the controller. This IV. Resultsestimation of the PCH will limit the requested virtualcontrol in the reference model, once the saturations no After implementing the two additional outer loops thatlonger occur the attenuation on the commands is no aim to sequentially control the linear speed and the linlonger active. For the INDI controller the PCH assumes ear position it is possible to obtain a position responsethe form of νhedged as seen in equation 40, depending while providing a position reference, as seen in figureon the same matrix as the outer INDI control loop, 14. The position response obtained is quite satisfiablewhere the virtual control is the linear acceleration and as allows the quadrotor to quickly change its position.The coupling between the horizontal and vertical movethe inputs are the Euler angles.ment is very noticeable in figure 14 where the altitudeνhedged ν ν̂ decreases around 1 meter when the quadrotor changesits position from x 0m to x 10m. Again due to [ ẋ0 g( x0 )(Ureq U0 )] [ ẋ0 g( x0 )(U U0 )]the very steep roll and pitch angles required to the g( x0 )(Ureq U )(40) system.8

I.X position15measurementreferencem10The most influential components of the quadrotormodel are its actuators, therefore it is important tostudy the influence actuator faults in the performance.While executing the same task, one of the rotors suffersa decrease of its effectiveness around second 2. Theresults of these simulations, are presented in figure 15where it is possible to verify the increasing effects ofthe actuator effectiveness loss in the maximum deviation from the position hold reference and the increaseddifficulty to perform the tracking task. In figure 1550-50246810121416182012141618201214161820time (s)Y position5m0-5-10-150246810time (s)Z position0.50mActuator effectiveness-0.5-1-1.50246810time (s)X position4Figure 14: Position response of the quadrotor to thereference provided.40% decrease50% decrease60% decrease70% decreasem20-2-4Results and Comparisons12345678910time (s)Y position42mVIII.00m-2The position controller will be qualitatively compared-4with two other controllers implemented in literature. In012345678910time (s)Z positiontable 2 a summary of the simulation results presented10in ? and the simulation results obtained by the INDI-1controller implemented are shown. This simulation-2consisted in a position step response, with a reference-3012345678910change of 2 meters in the horizontal axes. This results,time (s)for the INDI controller, on an temporary increase of Figure 15: Control performance comparison betweenthe tilt angle to 30o .NDI and INDI attitude controllers.ControllerPID [8]NDI [8]INDII. Pos.(m)[0, 0, 2][0, 0, 2][0, 0, 2]F. Pos.(m)[2, 2, 2][2, 2, 2][2, 2, 2]Rising Time(s) 2.5 22.25it is possible to verify the different effects of differentchanges on the actuator effectiveness.ControllersPID [8]NDI [8]PID* [8]NDI* [8]INDITable 1: Position Tracking performance comparisontable.I. Pos.(m)[0, 0, 2][0, 0, 2][0, 0, 2][0, 0, 2][0, 0, 2]Final Pos.(m)NANA[2, 2, 2][2, 2, 2][2, 2, 2]Rising Time(s)inf .inf . 2.5 22.25It is not possible to clearly show the benefits of usinga non-linear controller. Because the task executed isnot the ideal, as it does not require for large attitudeTable 2: Control per

Incremental Nonlinear Dynamic Inversion for Quadrotor Control Eduardo Lima Simões da Silva November 16, 2015 . the art for linear and non-linear adaptive controllers is given, to provide a review foundation and to motivate the study of . FT represents the total force applied by the actuators, F drag is the aerodynamic drag described in13 .