Attitude Control of Fixed Wing UAVs

Let us consider now the the parametrization of the dimensionless torque parameters:



 C_l C_m

C_n



=





0 C_l

δa C_l

δr

C_m

δe 0 0

0 C_n

δa C_n

δr







 δ_e δa

δ_r





+





C_lp 0 C_lr 0 C_mq 0 C_np 0 C_nr









psb q2Vsc¯ 2Vrsb 2V





+





0 C_l

β

C_mα 0 0 C_n

β



 α

β

(3.49)

=L₁(δ_e,δ_a,δ_r)^T+L₂(p_sb 2V ,q_sc¯

2V,r_sb

2V)^T+L₃(α,β)^T (3.50) Notice that the second term belonging toL₂is usually an angular velocity dependent damping term linear inω_b:

L₂





b

2V 0 0

0 _2V^c¯ 0 0 0 _2V^b )









C_α 0 S_α

0 1 0

−S_α 0 C_α







 P Q R



 (3.51)

Simplifying the details in notations the main structure of the motion equations can be formulated in the following forms:

mv˙=−ω×(mv) +f_v+g_v1u_v+g_v2u_ω (3.52) Jω˙ =−ω×(Jω) + f_ω−D_ωω+G_ωu_ω (3.53) As can be seen the position equation is coupled with the attitude equation since it depends on both fromu_v=F_T andu_ω = (δ_e,δ_a,δ_r)^T hence the nonlinear position control is a complex problem. Fortunately, the orientation equation is not coupled thus the attitude control is less complex. Notice also that the total system is underactuated.

The identification of the model parameters can be performed using test-flight data and robust identification techniques (Bayes estimator, Least-Squares, nonlinear state estimator etc.). Notice that the problem is simpler in the stability axis model.

50 3.3 Attitude Control of Fixed Wing UAVs

3.3.1 Euler angles

The Euler (roll, pitch, yaw) angles based orientation description is defined as follows:

Rⁿ_b(Φ,Θ,Ψ) =Rot(z,Ψ)Rot(y,Θ)Rot(x,Φ)

=





C_ΨC_Θ C_ΨS_ΘS_Φ−S_ΨC_Φ C_ΨS_ΘC_Φ+S_ΨS_Φ S_ΨC_Θ S_ΨS_ΘS_Φ+C_ΨC_Φ S_ΨS_ΘC_Φ−C_ΨS_Φ

−S_Θ C_ΘS_Φ C_ΘC_Φ





(3.54)

Notice that the gravity acceleration inK_bandK_ware respectivelyg_b= (Rⁿ_b)^T(0,0,1)^Tg_scal andg_w=R^w_bg_b.

3.3.2 Quaternion exponential and logarithm

The fundamentals for quaternions can be found in many resources, see for example [18].

First the main relations are summarized.

A quaternion consists of a scalar and a vector:q= (s,w)¯ ∈R¹×R³. It can be identified also withq= (s,w) = (q¯ ₀,q₁,q₂,q₃)∈R⁴. The conjugate isq^?= (s,−w), the norm square¯ iskqk²=s²+w¯^Tw. Quaternion product is defined by¯

q₁⊗q₂= (s₁s₂−w¯^T₁w¯₂,s₁w¯₂+s₂w¯₁+w¯₁×w¯₂). (3.55) Considering only unit norm quaternions in the sequel a multiplicative unite= (1,¯0) can be introduced with respect to quaternion multiplication wheree=q⊗q^?=q^?⊗q thus the inverseq⁻¹=q^?can be identified. Denote[ω×]the matrix of the vector product.

The orientation matrixR and general rotation Rot(¯t,φ)can be described by unit norm quaternion according to ([S6])

R=I+2s[w×] +¯ 2[w×][¯ w×],¯ (3.56) Rot(¯t,φ) ⇐⇒ ϑ :q= (C_ϑ/2,S_ϑ/2t),¯ (3.57) q⊗(0,r)¯ ⊗q^?= (0,R¯r). (3.58) The quaternion satisfies the new kinetic equation as follows:

d dt

s

¯ w

= 1 2

−w¯^T sI₃+ [w×]¯

ω =:T(q)ω. (3.59)

Let us consider now the gerneralization of exp(q) and log(q). Usinge^z =∑^∞_n=0^z

n

n!

and the scalar and vector part ofq= (s,w)¯ it followse= (1,¯0),(0,w),¯ (0,w)¯ ⊗(0,w) =¯ (−|w|¯ ²,0),(0,w)⊗¯ (0,w)¯ ⊗(0,w)¯ = (0,−|w|¯ ²w),¯ (0,w)⊗¯ (0,w)⊗(0,¯ w)¯ ⊗(0,w)¯ = (−|w|¯ ⁴,0),etc.

Sincesis scalar hence

exp(q) =e^se^(0,w)¯ =e^s

cos(|w|),¯ sin(|w|)¯ w¯

|w|¯

(3.60)

Let log(q) = (a,b)¯ then

e^log(q)=e^a cos(|b|),¯ sin(|b|)¯ b/|¯ b|¯

=q= (s,w)¯ (3.61) s=e^acos(|b|),¯ w¯ = e^asin(|b|)¯ b/|¯ b|¯ (3.62)

||q||²=s²+|w|¯ ²=e^2a(cos²(|b|) +¯ sin²(|b|)b¯^Tb¯

|b|¯ ²) =e^2a (3.63)

a=ln(||q||) (3.64)

log(q) =

ln(kqk), w¯

|w|¯ arccos(s/kqk)

(3.65) Using this with unit norm quaternions we get:

log(q) = (0,1

2t¯ϑ)⇒ d

dtlog(q) = (0,1

2t¯ϑ˙) (3.66)

Remark:Since|¯t|²=<t,¯ t¯>=1⇒t¯⊥t¯⁰hence ¯t,t¯⁰×t,¯ t¯⁰build orthogonal basis (prime denotes derivation here). On the other hand, ω =tϑ¯ ⁰+ (1−C_ϑ)¯t⁰×t¯+S_ϑt¯⁰, see [18].

thereforeω=tϑ¯ ⁰is only an approximation.

3.3.3 Standard Integral Backstepping Control using Euler angles

It is clear that the attitude control and some other problems can be brought to the following form:

˙

x₁=g₀x₂

˙

x₂= f₁+g₁u (3.67)

For attitude control x₁= (φ,θ,ψ)^T, x₂= (p,q,r)^T, g₀ is the matrix in the kinematic equation, f₁+g₁u=−I_c⁻¹(ω_B×(I_cω_B)) +I_c⁻¹T_Bwhereu=T_Bis the total torque and both g₀andg₁are invertible matrices.

Letz₁=x^d₁−x₁be the error andξ₁(t) =^R₀^tz₁(τ)dτthe error integral. Let the Lyapunov function and its derivative be

V₁=1

2z^T₁z₁+1

2ξ₁^TΛ1ξ1, Λ1=Λ^T₁ >0 V˙₁=z^T₁z˙₁+ξ₁^TΛ₁z₁

(3.68) Letv₁be the virtual control andz₂:=v₁−x₂⇔x₂=v₁−z₂then

˙

z₁=x˙^d₁−g₀(v₁−z₂) =z˙^d₁−g₀v₁+g₀z₂ V˙₁=z^T₁(x˙^d₁−g₀v₁+g₀z₂) +z^T₁Λ₁ξ₁

v₁:=g⁻¹₀ (x˙^d₁+Λ₁ξ₁+A₁z₁), A₁=A^T₁ >0 V˙₁=−z^T₁A₁z₁+z^T₁g₀z₂

(3.69)

As can be seen, the stability condition forz₁is not satisfied, hence let us continue with the

52 3.3 Attitude Control of Fixed Wing UAVs stabilization of the full system.

˙

z₂=v˙₁−x˙₂=v˙₁−f₁−g₁u V₂=V₁+1

2z^T₂z₂⇒V˙₂=V˙₁+z^T₂z˙₂

V˙₂=−z^T₁A₁z₁+z^T₁g₀z₂+z^T₂(v˙₁− f₁−g₁u) u:=g⁻¹₁ (v˙₁−f₁+A₂z₂+g^T₀z₁), A₂=A^T₂ >0

˙

z₂=v˙₁−f₁−g₁u=−A₂z₂−g^T₀z₁ V˙₂=−z^T₁A₁z₁−z^T₂A₂z₂<0

(3.70)

The full system with the IBSC is globally asymptotically stable (GAS). The closed loop system with the IBC controller is as follows:





˙ z₁

˙ z₂

ξ˙



=





−A₁ g₀ −Λ₁

−g^T₀ −A₂ 0

I 0 0







 z₁ z₂ ξ



 (3.71)

In order to realize the controller, the control signaluand all the variables in it should be computable. It means that beside the errorz₁=x^d₁−x₁ and the error integralξ₁also the virtual controlv₁and its derivative ˙v₁etc. have to be determined. The state variables for controller may be the results of high quality state estimation based on sensor fusion.

The derivatives ˙x^d₁and ¨x^d₁ need to be continuous and smooth which can be assured with appropriate path design or filtering and numerical derivation. Moreoveruneeds ˙v₁and ˙v₁ needs ^dg

−1 0

dt . Let us consider the details of IBSC attitude control algorithm:

g₀=





1 T_θS_φ T_θC_φ

0 C_φ −S_φ

0 S_φ/C_θ C_φ/C_θ





g⁻¹₀ =





1 0 −S_φ

0 C_φ C_θS_φ 0 −S_φ C_θC_φ



 dg⁻¹₀

dt =





0 0 −C_φφ˙

0 −S_φφ˙ −S_θS_φθ˙+C_θC_φφ˙ 0 −C_φφ˙ −S_θC_φθ˙−C_θS_φφ˙



 (φ˙,θ˙,ψ)˙ ^T =g₀(p,q,r)^T

v₁=g⁻¹₀ (x˙^d₁+Λ1ξ1+A₁z₁)

˙

v₁= dg⁻¹₀

dt (x˙₁^d+Λ₁ξ₁+A₁z₁) +g⁻¹₀ (x¨^d₁+Λ₁z₁+A₁z˙₁) u:=g⁻¹₁ (v˙₁−f₁+A₂z₂+g^T₀z₁)

(3.72)

According these formulas the algorithm for computing control deflections is the fol-lowing:

1) The result of the IBSC attitude control algorithm is the driving torqueu=T_B in the basis of thebody frame K_B:

u=T_B=diag([qS¯ _wab,qS¯ _wac,¯ qS¯ _wab])

| {z }

D



 l m

n



 (3.73)

Here(l,m,n)^T is the sum of two effects:

(l,m,n)^T =L₁(δ) +L₂(p,ˆ q,ˆ r,ˆ α,β, . . .) (3.74) where ˆp=pb/(2V), ˆq=qc/V¯ , ˆr=rb/(2V)and the factors L₁ andL₂ are linear in their variables with coefficients of the aircraft dynamic model. Therefore

L₁(δ) =D⁻¹u−L₂(p,ˆ q,ˆ r,αˆ ,β, . . .) (3.75) which is a linear equation forδ that can be solved easily.

2) In many cases the parametrization of the force/torque effects is given in thewind axes frame, see [8], [23]. SinceT_B=S^TT_W andT_W =D(l,m,n)^T =D(L₁+L₂)therefore

L₁(δ) =D⁻¹Su−L₂(p,ˆ q,ˆ r,ˆ α,β, . . .) (3.76) The characteristic equation can be used for robust controller parameter design. However the kinematic differential equations are singular forΘ=±π/2 and cannot be used for attitude control in its neighborhood, where cos(θ) =p

˙

x²+y˙²/p

˙

x²+y˙²+z˙².

3.3.4 Attitude control without the use of quaternion logarithm

LetK_nandK_bbe the earlier introduced inertia and body frames, respectively. A new frame K_d will be introduced which is the prescribed (desired) frame for the body COG. It is the result of the path design and the goal is to matchK_band K_d after the control transients satisfyingK_b=K_d. SinceR^b_n=R^d_nR^b_d hence

R^b_d= (R^d_n)⁻¹R^b_n⇐⇒q_d,b=q⁻¹_n,d⊗q_n,b=q_d,n⊗q_n,b, (3.77) R^b_d=I₃(goal) ⇐⇒q^∞_d,b= (±1,¯0) (3.78) which means that(±1,¯0)should be equilibrium state. Notice that two solutions are possible and the shortest rotation angle should be chosen during the transients.

The quaternion error and the angular velocity error will be defined as

q_e:=q^∞_d,b−q_d,b= (1,¯0)−(s_d,b,−w¯_d,b) = (1−s_d,b,w¯_d,b) (3.79) ω_d,b^b =ω_n,b^b −R^b_dω_n,d^d (3.80) Hereq_e is the difference of two quaternions hencekq_ek 6=1 whilekq_d,bk=1 thusq_d,b satisfies the quaternion differential equation.

54 3.3 Attitude Control of Fixed Wing UAVs The system to be controlled consists of two parts (J=I_c):

˙

q_e= (−s˙_d,b,w˙¯_d,b) =T(−s_d,b,w¯_d,b)ω_d,b^b =:T_eω_d,b^b , (3.81) Jω˙_b=−[ω_b×]Jω_b+f(x)−D(x)ω_b+G(x)u. (3.82) Notice that the original backstepping control (BSC) cannot be applied becauseT_eis of type 4×3 and not invertible. Therefore some modifications are necessary.

Using ˙R=R[ω×]and (3.80) the time derivative ofω_d,b^b can be determined for use in Jω˙_d,b^b :

ω˙_d,b^b =ω˙_n,b^b −R˙^b_dω_n,d^d −R^b_dω˙_n,d^d (3.83)

=ω˙_n,b^b −R^b_d[ω_b,d^d ×]ω_n,d^d −R^b_dω˙_n,d^d . (3.84) Multiplying the result by the inertia matrix it follows

Jω˙_d,b^b = −[ω_n,b^b ×]Jω_n,b^b +f(x)−D(x)(ω_d,b^b +R^b_dω_n,d^d ) +G(x)u−JR^b_dω˙_n,d^d

(3.85)

= −[ω_n,b^b ×]Jω_n,b^b + f(x)−D(x)(ω_d,b^b +R^b_dω_n,d^d )

−JR^b_dω˙_n,d^d +G(x)u+J[ω_n,b^b ×]R^b_dω_n,d^d

(3.86) where it was used that

R^b_d[ω_b,d^d ×]ω_n,d^d =R^b_d[(ω_n,d^d −ω_n,b^d )×]ω_n,d^d =−[ω_n,b^b ×]R^b_dω_n,d^d .

Hereω_n,b^b −R^b_dω_n,d^b can be considered to be the angular velocity error. Let the first Lyapunov function beV₁=¹₂q^T_eq_ethenV₁is positive definite and

V˙₁=q^T_eq˙_e=q^T_eT_eω_d,b^b =q^T_eT_e(ω_n,b^b −R^b_dω_n,d^d ) (3.87) Choosek_q>0∈R¹and define the virtual control as

z=ω_d,b^b +k_qT_e^Tq_e⇒

˙

z=ω˙_d,b^b +k_qd

dt(T_e^Tq_e) (3.88)

V˙₁=−k_qq^T_eT_eT_e^Tq_e+q^T_eT_ez (3.89) Here ˙V₁is not negative because of the second term.

With the earlier results forω_n,b^b and ˙ω_d,b^b yields

ω_n,b^b =z−k_qT_e^Tq_e+R^b_dω_n,d^d (3.90) z=ω_n,b^b −R^b_dω_n,d^d +k_qT_e^Tq_e (3.91) Jz˙=Jω˙_d,b^b +k_qJ d

dt(T_e^Tq_e)

=−[ω_n,b^b ×]Jω_n,b^b +f(x)−D(x)ω_n,b^b

−JR^b_dω˙_n,d^d +G(x)u+J[ω_n,b^b ×]R^b_dω_n,d^d +k_qJd

dt(T_e^Tq_e)

(3.92)

Now it is possible to prescribeT_e^Tq_e and choose the controlu using Lyapunov theory.

Define the 3D vector byT_e^Tq_e:= ¹₂w¯_d,band letV₂be the new Lyapunov function, then V₂=V₁+1

2z^TJz (3.93)

V˙₂=−k_qq^T_eT_eT_e^Tq_e+q^T_eT_ez+z^TJz˙ (3.94) The use of the above form of ω_n,b^b allows of separating a damping term −z^TD(x)zfor stabilization. Hence the control can be chosen as

u=G⁻¹[JR^b_dω˙_n,d^d −f(x) +D(x)(R^b_dω_n,d^d −k_qT_e^Tq_e) +[ω_n,b^b ×]Jω_n,b^b −J[ω_n,b^b ×]R^b_dω_n,d^d −T_e^Tq_e−k_q

2Jw˙¯_d,b−k_ωz] (3.95) wherek_ω >0∈R¹. With this control law a lot of terms are canceled in ˙V₂and with the remaining terms yields

V˙₂=−k_qq^T_eT_eT_e^Tq_e−z^T(D(x) +k_ωI₃)z (3.96) Here we used a special choice for the matrixT_eT_e^T which is only positive semidefinit.

However,q^T_eT_eT_e^Tq_e= ¹₄w¯^T_d,bw¯_d,b= ¹₄(1−s²_d,b)≥ ¹₈((1−s_d,b)²+w¯^T_d,bw¯_d,b)≥ ¹₈kq_ek². Similarly, if the wind velocity is bounded below byV_a>βv>0,x_min= (β_v,0,0)^T) and the eigenvalue satisfiesλ_min{D(x_min} ≥β_D>0, then

V˙₂≤ −k_q

8kq_ek²−(β_D+k_ω)kzk² (3.97) and the equilibrium point(q_e,z) = (0,0)is uniformly exponentially stable (UES).

Control law without the use of log(q):

The quaternion based BSC attitude control law consists of (3.88) and (3.95). The case q_e− = (−1+s_d,b,w¯_d,b)can be proved similarly.

3.3.5 Attitude control using quaternion logarithm

Let K_n and K_b be the earlier introduced inertia and body frames, respectively. A new frameK_d will be introduced which is the prescribed (desired) frame for the body COG.

56 3.4 Altitude control of fixed wing UAVs

In document BélaLantos Ph.D.Dissertation ZsófiaBodó ModernControlMethodsforUnmannedAerialandGroundVehicles (Pldal 59-66)