Nonlinear dynamic inversion

Transcription

1 Nonlinea dynamic invesion Aicat don t always behave like linea systems. In some light egimes, o in some (ailue) scenaios, they behave in a nonlinea way. To contol them, you theeoe also need a nonlinea contolle. And this contolle should be as obust as possible, o the model neve exly matches eality. In this ile, we ll examine the technique called nonlinea dynamic invesion. It has poven to be an easy way o contolling nonlinea systems. Next to this, it oes possibilities o vey obust contol though an expansion called incemental nonlinea dynamic invesion. Basics o nonlinea dynamic invesion. Rewiting a system o NDI Let s examine the model o an aicat. Fo simplicity, we will assume that it is a single input single output (SISO) model. This model has the om ẋ = (x) + g(x)u, (.) with x the state vecto. (Note that (x) can be a nonlinea unction.) The above model can be ewitten to companion om ẋ x 2 0. ẋ n =.. u. (.2) x n 0 x n b(x) a(x) In othe wods, all the nonlinea tems now only aect x n. Also, the input only aects x n. As a second step, we deine the vitual contol input v as v = b(x) + a(x)u u = a (x) (v b(x)). (.3) The vitual contol input v can now be used to contol the entie system in a simple linea way. This method o contolling a nonlinea system as i it is linea is called (nonlinea) dynamic invesion ((N)DI)..2 Which input to use? The question emains on how to set v. Fo this, state eedback is oten used. So, Since we also have dn x n dx v = k 0 x k k d 2 x k d n x n. (.4) n = v, this tuns the whole system into a linea closed loop system o the om d n x n + k d n x n n k dx + k 0 = 0. (.5) By choosing the ight k i, the closed-loop system popeties can be set. And, by then using equation (.3), the equied input u is ound. The pocess o inding v is called the oute loop o NDI. Finding the coesponding value o u, and inseting it into the eal system, is the inne loop.

2 In case o a tacking task (whee x has to ollow some eeence signal x ), we can deine e = x x d. The evident contol law now is de v = k 0 e k k d 2 e k d n e n. (.6) n This tuns the system into d n e n + k d n e n n k de + k 0 = 0. (.7) So this is basically the same poblem as beoe. 2 Input-output lineaization 2. The woking pinciple o input-output lineaization It can occasionally be diicult to put a nonlinea system in companion om. An altenative to this is to apply input-output lineaization. When applying this, we take the deivative o the output y until the input u appeas in it. Fom this expession, you then deive a and b. As an example, conside the system Taking the deivative o y gives ẋ = x sin(x 3 ), (2.) ẋ 2 = cos(x ), (2.2) ẋ 3 = x + u, (2.3) y = sin(x ). (2.4) ẏ = ẋ cos(x ) = ( x sin(x 3 ) ) cos(x ). (2.5) Thee is no input u in this expession. So we take the deivative again. This gives ÿ = (2x 2 ẋ 2 + x 3 cos(x 3 )) cos(x ) ( x sin(x 3 ) ) ẋ sin(x ) (2.6) = (2x 2 cos(x ) + (x + u) cos(x 3 )) cos(x ) ( x sin(x 3 ) ) 2 sin(x ) (2.7) = 2 cos(x ) 2 x 2 + x cos(x ) cos(x 3 ) ( x sin(x 3 ) ) 2 sin(x ) + cos(x ) cos(x 3 )u. (2.8) As input, we can now again use u = a (v b), whee a = cos(x ) cos(x 3 ), (2.9) b = 2 cos(x ) 2 x 2 + x cos(x ) cos(x 3 ) ( x sin(x 3 ) ) 2 sin(x ). (2.0) In this way, the system can be seen as a linea system again. And, in ou example case, this linea system is simply ÿ = v. 2.2 Notes on NDI When applying NDI, it is impotant to note the singulaities. These occu when u. Fo ou example poblem, they thus occu when x = π/2 + kπ o x 3 = π/2 + kπ, with k an intege. At such points, the system can t be suiciently contolled. Anothe impotant thing to note is that, to apply NDI, it is equied to know the ull state o the system. I the state is not known, it needs to be appoximated in some away. Fo deteministic systems, a nonlinea obseve can be used, wheeas o stochastic systems a nonlinea state estimato is equied. Next to this, it is o couse also equied that the system model is known completely. I it is only patially known, some system identiication might be equied ist. 2

3 2.3 Intenal dynamics The amount o times we need to dieentiate the output is called the elative degee o the system. In ou example poblem, this elative degee is thus = 2. The ode o the system (the amount o state vaiables) is denoted by n. We always have n. I < n, then pat o the input-output lineaization is unobsevable. This unobsevable pat is called the intenal dynamics. They have to be stable (bounded) o the contolle to wok popely. (In case o intenal instability, the pat b can become vey big. Although mathematically u can become vey big as well to compensate, this is physically oten impossible.) Howeve, the unobsevable pat is oten also nonlinea. So inding out whethe it s ually stable can be vey diicult. Fo simplicity let s examine the intenal dynamics o linea systems. In linea systems, n coesponds to the amount o poles o the system, while n is the amount o zeoes. (So is the amount o excess poles.) It can also be shown that the intenal dynamics ae stable i all the zeoes ae in the let hal o the complex plane. In othe wods, the intenal dynamics ae stable i the system is minimum-phase. 3 State tansomation 3. The Lie deivative Let s examine a system o the om ẋ = (x) + g(x)u, (3.) y = h(x). (3.2) Note that and g ae both vecto unctions (i.e. they etun a vecto) while h is a scala unction. The Lie deivative L h(x) is now deined as the gadient o a cetain (scala) unction h(x) pojected along a cetain vecto unction (x). So, L h(x) = h(x)(x) = n i= h(x) x i i (x). (3.3) It is also possible to apply the Lie deivative multiple times. You then get the k th Lie deivative, deined as ( ) ( ) L k h(x) = L L k h(x) = h(x) (x) with L 0 h(x) = h(x). (3.4) L k 3.2 The state tansomation We will use the Lie deivative to apply a state tansomation. This tansomation goes om the old state x to a new state z (called the lineaizing state and puts the system in a canonical om. Fist, using the Lie deivative, we deine the unctions φ i (x) as z i = φ i (x) = L i h(x), with i. (3.5) We have L g φ i (x) = 0 o all i, except o i =. In, the deinition o elative degee implies that L g φ (x) 0. (This popety is oten convenient to use when inding the elative degee.) Analogously, we can also deine the unctions z i = φ i (x) with + i n. They also must have the popety that L g φ i (x) = 0. (3.6) A numbe n o such additional unctions always exist. Howeve, we won t discuss hee how to ind them, since it s not elevant o ou discussion. The whole state tansomation is now denoted by z = Φ(x). The invese is given by x = Φ (z). 3

4 3.3 Popeties o the state tansomation It can be shown that, o i, the new coodinates satisy ż = z 2, ż 2 = z 3,..., ż = z and ż = a(z) + b(z)u, (3.7) whee the unctions a(z) and b(z) (with z = Φ (x)) ae deined as a(z) = L g L h(x) = L g z and b(z) = L h(x). (3.8) (These unctions ae exly the same as the unctions a and b ound in the section on input-output lineaization. This method is just a somewhat moe geneal way o inding them.) Fo + i n, the new coodinates satisy (Remembe that L g φ i (x) = 0 o + i n.) The esult? We have a system whee z = h(x) = y and ż i = L φ i (x) + L g φ(x)u = L φ i (x) = L z i. (3.9) a(z) + b(z)u = dz = d2 z 2 =... = d z = d y. (3.0) We have put the system in canonical om! And the unctions a and b ae the same as what we ve seen in the section on input-output lineaization. So we can again contol the system as i it s linea. Next to this, the states z + to z n don t diectly aect the output y. So we have eectively split up the obsevable om the unobsevable pat. 4 MIMO systems and time scale sepaation 4. The MIMO system om Peviously, we have consideed SISO systems. Now let s expand ou ideas to multiple input multiple output (MIMO) systems. These systems have the om ẋ = (x) + G(x)u, (4.) y = h(x). (4.2) Note that G(x) is a matix depending (possibly nonlinealy) on x. Also note that we do assume that the state deivative ẋ ainely depends on the input u. Also, the input vecto u has size m, while the output vecto y has size p. 4.2 The state tansomation o MIMO systems In a MIMO system, thee ae individual elative degees,..., p. Togethe, they om the total elative degee = p, which still satisies n. Thee ae also the φ unctions. These ae deined as φ i j(x) = L j h i (x). (4.3) These unctions then satisy φ i (x) = φ i 2(x),..., φi i (x) = φ i i (x) and φ i i (x) = L i h i(x) + m j= L gj L i h i (x)u j. (4.4) 4

5 This holds o evey i (with i p). We can use the above elations to ind an expession o the vitual contol input v. (Note that v now also is a vecto, having size p.) We then get L h (x) L g L h (x) L g2 L h (x) L gm L h (x) u L 2 v = b(x) + A(x)u = h 2(x). + L g L 2 h 2 (x) L g2 L 2 h 2 (x) L gm L 2 h 2 (x) u L p h p(x) L g L p h p (x) L g2 L p h p (x) L gm L p hp(x) u m (4.5) Solving o the input vecto u then gives u = A (x)(v b(x)). (4.6) 4.3 Using time-scale sepaation When contolling two dieent paametes, a technique called time-scale sepaation may be applied. To know when this is the case, we ist have to discuss contol eectiveness. The contol eectiveness can be seen as the eect on a contolled paamete, due to a unity change in the contolling paamete. (Think o q/ δ e o aicat pitch ate contol.) Based on the contol eectiveness, we can make a distinction between slow dynamics and ast dynamics. Slow dynamics means that the contol eectiveness o a cetain paamete is low. Fast dynamics means that the contol eectiveness is high. When thee ae two paametes to be contolled, one with slow dynamics and one with ast dynamics, time-scale sepaation (TSS) can be applied. This means that we split up the slow and the ast dynamics. The ast dynamics can then be seen as the inne loop, while the slow dynamics make up the oute loop. Fo evey pat, dynamic invesion is applied sepaately. An example o time-scale sepaation occus in an aicat. Pitch ate contol geneally woks a lot aste than pitch angle contol. So pitch ate contol oms the inne loop o the contolle, while pitch angle contol oms the oute loop. When applying TSS, an assumption is made. As input, the inne loop eceives a eeence value. (In ou example the desied pitch ate.) The oute loop assumes that this desied value is ually achieved by the inne loop. This is oten a valid assumption to make, because the inne loop is much aste than the oute loop. The oute loop (the pitch angle contol) then opeates by simply supplying the ight eeence input (the desied pitch ate) to the inne loop. 5 Incemental NDI 5. The basic idea o INDI Thee is a big downside to NDI. To apply it, the model o the system has to be known quite accuately. An aicat model can, howeve, be vey complicated, especially when nonlineaities stat being pesent. A possible solution is oeed by Incemental NDI (INDI). This technique doesn t give the equied input to contol the system. It gives the equied change in the input. (Fo example, it does not tell the elevato to delect to 6 degees, but to delect (e.g.) degee moe o 2 degees less.) A big advantage o INDI is that only a small pat o the model is equied. Also, INDI is bette able to cope with model inaccuacies like, o example, wong coeicients. As such, INDI is moe obust than NDI. 5

6 5.2 INDI applied to an aicat om moments to contol suace delections Let s demonstate the INDI idea by looking at an aicat. When contolling the attitude o an aicat, moments need to be applied. And, when woking with moments, we usually simply use moment coeicients. The moment coeicient o L is deined as C l = L des 2 ρv 2 Sb. (5.) Thee is a simila deinition o C m and C n. To contol the aicat, thee ae also desied moment coeicients C l,des, C m,des and C n,des. Next to this, thee ae the coeicient deivatives. An example is C lδe = C l δ e. (5.2) Simila deinitions hold o combinations with the othe moments (M and N) and the othe contol inputs (δ a and δ ). Based on the above deinitions, we can ind the equied contol suace delections δ to povide the ight moments. We have δ e C lδe C lδa C lδ C l,des δ = δ a = C mδe C mδa C mδ C m,des = Mc C lmn,des. (5.3) C nδe C nδa C nδ δ C n,des (Note the deinitions o δ, M c and C lmn,des in the above equation.) This technique woks well in case the moments L, M and N vay linealy with the contol suace delections δ e, δ a and δ, and i all the coeicient deivatives ae accuately known. I eithe is not the case, we have to do something else: INDI. When applying INDI, we look at the equied change in moment. That is, we compae the cuent moment coeicients ing on the aicat (C l, C m and C n ) to the desied moment coeicients. Fom this, we deive the changes in the contol suace delections. This goes accoding to δ e C lδe C lδa C lδ C l,des C l δ = δ a = C mδe C mδa C mδ C m,des C m = Mc (C lmn,des C lmn ). (5.4) δ C nδe C nδa C nδ C n,des C n This time, when the coeicient deivatives aen t accuate, the equied moments will eventually still be eached. In this way, INDI is much moe obust than NDI. 5.3 INDI applied to an aicat om motion to contol suace delections When contolling an aicat, we don t just stat out with moments. Instead, we want to contol the motion o the aicat. To ind out how this woks, we examine the equation o motion I ω + ω Iω = M = M a + M c. (5.5) Hee, M a denotes the moments due to aeodynamics, while M c denotes the moments due to the contols. (I equied, we can tansom the above equation, such that it uses the coeicients C instead o the moments M.) Now examine what happens when we change the contol moment M c by a small amount M c. In an ininitesimal timestep, the angula ates ω don t change yet, no do the aeodynamic moments M a. Only the angula acceleation ω changes. (Note that we ae applying the pinciple o time-scale sepaation hee.) So we can wite M c = I( ω) = I( ω new ω old ). (5.6) 6

7 Using an on-boad Inetial Measuement Unit (IMU), we can deive ω old. Now assume that the contolled quantity o ou system (the output y) is the angula ate ω. We can then wite the system as ẏ = ω new = ω old + I ( M c ). (5.7) We want to ind the equied change in contol input δ, such that the desied ẏ is obtained. To do that, we ist substitute M c by M c ( δ). (We apply lineaization.) The equied contol suace delection is then given by δ = Mc I(ẏ ω cu ). (5.8) Let s examine the above equation. When we apply nomal NDI, the value o δ stongly depends on a lot o aicat popeties. Now, δ only depends on M c and I. Because we use measuements o ω new, any othe model uncetainties ae cancelled. This is especially so i the measuements ae vey accuate and the sampling time is small. In this way, INDI is much moe obust than NDI. 6 Contolling an aicat with NDI 6. Aicat attitude contol Now that we know how NDI woks, we should apply it to contol an aicat. We ll discuss hee how that woks. Although we don t show the deivation o this technique, we will explain how to ind the contol suace delections equied to contol the aicat. This is done in six steps.. We stat o with the eeence light angles φ, θ and β. Fom them, we deive the eeence light angle deivatives φ, θ and β. This is done using a PID contolle, like φ φ φ φ φ φ φ θ = K P θ θ +K I θ θ +K d D θ θ. β β β β β β β e e 2. Fom these deivatives, the otational ates o the aicat should be deived. This is done using the matix equation p sin φ tan θ cos φ tan θ q = 0 cos φ sin φ φ 0 θ 0 ( ( ) ). w u e u 0 2 +w 2 u β uv 2 +w 2 e u2 +w 2 V a 2 x + v2 V a 2 y vw V a 2 z (6.2) The coeicients a x, a y and a z ae, espectively, given by e e (6.) a x = X m g sin θ, a y = Y m + g sin φ cos θ and a z = Z + g cos φ cos θ, (6.3) m with X, Y and Z the oces in the coesponding diections. They can be measued by an IMU. 3. We now have the desied otational ates o the aicat. Again, using a PID contolle, we can ind the desied otational acceleations o the aicat. So, ṗ p p p p p p q = K P2 q q + K I 2 q q + K d D 2 q q. ṙ e e e e (6.4) 7

8 4. To ind the equied moments, we can use L ṗ M = I q N ṙ eq e p + q p I q. (6.5) Note that we don t use incemental NDI hee, but just the nomal vesion o NDI. I equied, also the INDI vesion can be implemented. 5. The equied moments should be nomalized beoe we can use them. Fo that, we use C leq = L eq 2 ρv 2 Sb, C m eq = M eq 2 ρv 2 S c and C neq = N eq 2 ρv 2 Sb. (6.6) 6. Now that the equied moment coeicients ae known, the equied elevato delections can be calculated. This time we can use δ e C lδe C lδa C lδ C l C l δ a = C mδe C mδa C mδ C m C m. (6.7) C nδe C nδa C nδ δ eq 6.2 Aicat position contol deiving equations C n eq C n computed With the contolle o the pevious paagaph, we could only contol the oientation o the aicat. But how do we tavel aound the wold then? Fo that, we need seveal equations, which we ll discuss in this paagaph. (We then use them in the next paagaph.) We will stat with the equations o motion. They ae given by F ext = ma C = m dṙ C ( ) dṙe = m + Ω EC ṙ E. (6.8) Hee, the subscipt C denotes the Eath-centeed Eath-ixed eeence ame. (We assume that this eeence ame is inetial.) Howeve, we will be using the equation eeence ame F E. This eeence ame has its Z axis pointing up (away om Eath s cente), the X axis pointing East and the Y axis pointing Noth. The oigin is at the cente o the Eath. The position o the aicat is now simply given by 0 E = 0. (6.9) The deivative depends on the light path angle γ and the heading angle χ, accoding to cos γ sin χ ṙ E = V cos γ cos χ, (6.0) sin γ The tem Ω EC is the otation ate o the F C eeence ame with espect to the F E eeence ame. As seen om the F E eeence ame, it is given by δ Ω EC = τ cos δ, (6.) τ sin δ 8

9 whee τ is the longitude and δ the latitude. Thei time-deivatives depend on V, γ and χ, accoding to Based on the above elations, we can eventually deive that V cos γ sin χ sin γ sin χ cos χ V γ = cos γ cos χ sin γ cos χ sin χ V χ cos γ sin γ cos γ 0 ṙ = V sin γ, (6.2) τ = V sin χ cos γ, cos δ (6.3) δ = V cos χ cos γ. (6.4) F E m + 0 V 2 cos γ V 2 cos 2 γ sin χ tan δ. (6.5) The above equation will look a bit pettie i we tansom it to the velocity eeence ame F V. This eeence ame has the X axis pointing in the diection o the aicat velocity vecto. Now examine the plane spanned by this X axis and the local gavity vecto. The Y axis is pependicula to this plane and points to the ight o the aicat. Finally, the Z axis is pependicula to both the X and Y axes and points oughly downwad. I we use this eeence ame, then we will get V 0 0 F B V γ = 0 0 T V B m + V χ cos γ V 2 cos γ V 2 cos 2 γ sin χ tan δ. (6.6) In the above equation, the ightmost tem is oten small enough to be neglected. But, o accuacy (and just because we can), we will keep it. Do note that the oce vecto F is now expessed in the body eeence ame F B. The tansomation vecto T V B between the velocity and the body eeence ame is given by cos α cos β sin β sin α cos β T V B = cos α sin β cos µ + sin α sin µ cos β cos µ sin α sin β cos µ cos α sin µ. (6.7) cos α sin β sin µ sin α cos µ cos β sin µ sin α sin β sin µ + cos α cos µ 6.3 Aicat position contol the ual plan We now have a lot o equations concening an aicat lying aound planet Eath. But how do we contol it? Fist, we examine the velocity. We want to keep a cetain eeence velocity V e. Using PID contol, we ind the coesponding value o V e. We have V e = K P (V e V ) + K I d (V e V ) (V e V ) + K D. (6.8) We then use this eeence value to ind the equied thust. We also use the top ow o equation (6.6). This gives us the equation V = [ ] X + T cos α cos β sin β sin α cos β Y = ((X + T ) cos α cos β + Y sin β + Z sin α cos β). m m Z (6.9) 9

10 Solving o the equied thust gives T eq = m V e (X cos α cos β + Y sin β + Z sin α cos β). (6.20) cos α cos β Fom this, the coesponding thust setting δ T should be deived. Now let s examine the light path. This is speciied by the climb angle γ and the heading χ. Fo both these paametes, we have cetain desied values γ e and χ e that need to be ollowed. We use these desied values to ind d (γ e γ ) γ e = K D (γ e γ ) + K I (γ e γ ) + K D, (6.2) d (χ e χ ) χ e = K D (χ e χ ) + K I (χ e χ ) + K D. (6.22) Using these values, we can deive equied values o the angle o attack α and the bank angle µ. But we do have to make some assumptions o that. We assume that the eeence angle o the sideslip angle β is always zeo, so β e = 0. In, o simplicity, we assume that the sideslip angle is always kept at zeo by the autopilot, so β = 0. We also assume that, because the sideslip angle is zeo, thee ae no side oces, so Y = 0. Finally, we assume that the angle o attack is small, so sin α = α and cos α =. I we now solve the bottom two ows o equation (6.6), we get (( m µ e = acsin α e = m X cos µ ) ( V χ cos γ V 2 cos 2 γ sin χ tan δ Xα Z ( V γ + Z m cos µ + V 2 cos γ Using the eeence angles α e, µ e and β e (which is zeo), we can ind µ e = K D (µ e µ ) + K I α e = K D (α e α ) + K I β e = K D (β e β ) + K I )), (6.23) ). (6.24) d (µ e µ ) (µ e µ ) + K D, (6.25) d (α e α ) (α e α ) + K D, (6.26) d (β e β ) (β e β ) + K D. (6.27) Fom the desied angle deivatives, we can ind the equied aicat otational ates p, q and. Fo this, we do need to deive some moe complicated kinematic equations. We won t discuss the deivation hee. We ll only mention the esult, which is p q e cos α cos β 0 sin α = sin β 0 sin α cos β 0 cos α µ e α e β e + T BV χ sin γ γ χ cos γ. (6.28) Once these eeence values ae known, we can continue with step 3 o the oiginal oientation contol plan to ind the equied contol suace delections. In this way, we only have to speciy a desied aicat velocity V e, light path angle γ e and heading angle χ e, and the aiplane will ollow it. Who still needs a pilot? 0