Feedback Linearization approach for Standard and

Home

Search

Collections

Journals

About

Contact us

My IOPscience

Feedback Linearization approach for Standard and Fault Tolerant control: Application to a Quadrotor UAV Testbed

This content has been downloaded from IOPscience. Please scroll down to see the full text. 2014 J. Phys.: Conf. Ser. 570 082003 (http://iopscience.iop.org/1742-6596/570/8/082003) View the table of contents for this issue, or go to the journal homepage for more

Download details: IP Address: 46.36.75.54 This content was downloaded on 14/02/2016 at 07:42

Please note that terms and conditions apply.

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

IOP Publishing doi:10.1088/1742-6596/570/8/082003

Feedback Linearization approach for Standard and Fault Tolerant control: Application to a Quadrotor UAV Testbed J. Ghandour, S. Aberkane, J-C. Ponsart University of Lorraine, CRAN, UMR 7039, Campus Sciences, B.P. 70239, 54506 Vandoeuvre-lès-Nancy Cedex, France CNRS, CRAN, UMR 7039, France [email protected] Abstract. In this paper the control problem of a quadrotor vehicle experiencing a rotor failure is investigated. We develop a Feedback linearization approach to design a controller whose task is to make the vehicle performs trajectory following. Then we use the same approach to design a controller whose task is to make the vehicle enter a stable spin around its vertical axis, while retaining zero angular velocities around the other axis when a rotor failure is present. These conditions can be exploited to design a second control loop, which is used to perform trajectory following. The proposed double control loop architecture allows the vehicle to perform both trajectory and roll/pitch control. At last, to test the robustness of the feedback linearization technique, we applied wind to the quadrotor in mid flight.

1. Introduction While the military and civil interest in Unmanned Arial Vehicles (UAVs) grows exponentially, the research centers race to publishing new and more efficient control techniques ([5], [16]). Among the various proposed control methods in the literature, one can cite for example LQ ([10]), H ([11], [12], [14]), backstepping ([7], [9], [15]) or sliding mode ([4], [13]) control techniques. In this paper, we propose to consider a nonlinear feedback linearization control ([2], [6]) in a fault tolerant control framework ([3]). A UAV presents several basic advantages over a Manned Aerial Vehicle (MAV), including lower cost, prolonged continuous flight time, reduced radar signatures, smaller size, increased maneuverability, and the most important benefit is being less risky to crews. Particular UAVs that have received much attention in the literature during the last ten years are quadrotors. This is mainly due to there suitability for a wide variety of UAV research applications and test-fly methods in control, fault diagnosis, fault tolerant control (FTC) as well as multi-agent based technologies in cooperative control, distributed control, formation flight, mobile wireless networks and communications. During certain operations, a UAV cannot remains controlled from ground, for example in long continuous flights, in surveillance operations where multiple UAVs are in flight simultaneously, or behind the enemy line where signal transmission can put the stealth aspect of the UAV in jeopardy. This created the need for autonomous UAVs, which are programmed in a way to accomplish a given task without the need of human intervention at any point during the procedure. But with autonomous UAVs came another problem, these UAVs could perfectly handle themselves when fully operational, but a faulty sensor, or a damaged body, gets the system unstable, crashing the vehicle right away. Due Content from this work may be used under the terms of the Creative Commons Attribution 3.0 licence. Any further distribution of this work must maintain attribution to the author(s) and the title of the work, journal citation and DOI. Published under licence by IOP Publishing Ltd 1

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

IOP Publishing doi:10.1088/1742-6596/570/8/082003

to the high cost of vehicles used by the military and civil organisms around the world, a FTC system was needed to be installed on these vehicles, assuring the damaged vehicle returns to its base without further damages. This paper is divided into several sections. At first, the quadrotor description and modeling is presented. Then, the control law based on feedback linearization is detailed. The last section presents simulation results done on Matlab/Simulink®. 2. Quadrotor description The quadrotor is a simple to manufacture aircraft with vertical landing/take-off capability, hovering ability, and is greatly maneuverable. The figure 1 shows the four actuators in a cross configuration, producing three rotations, Roll-Pitch-Yaw, and a vertical translation, Lift. The first problem that faces us when creating a hovering machine is the problem of torque reaction based on Newton's third law of motion. This problem is solved in a quadrotor by turning two rotors clockwise, while turning the others counter-clockwise, thus canceling the global torque applied on the quadrotor.

Figure 1. The Quadrotor A relation exists between the vector U = [U z , Uϕ , Uθ , Uψ ]T and F = [F1, F2 , F3, F4 ]T : ! # # # # # "

$ ! Uz $ ! & 1 1 1 1 $# F1 & & # Uϕ & 0 d 0 −d &# F2 & &=# & # Uθ & # −d 0 d 0 &# F3 & & # Uψ & " c −c c −c %#" F4 &% %

(1)

where d is the distance from the epicenter of the quadrotor to the actuator axes, c > 0 is the drag factor, Fi is the lift generated by the i th actuator, U z is the total lift force generated by all four actuators and Uϕ , Uθ , Uψ are the couples generated respectively in the Roll, Pitch and Yaw directions. The mathematical model describing the position and attitude of a quadrotor is based on that of a rotating rigid body with six degrees of freedom. Close to hovering conditions (ϕ ≈ 0 et θ ≈ 0) one obtains ([14], [15]): # m !! x = (Cϕ Sθ Cψ + Sϕ Sψ ) U z − K x x! % % m !! y = (Cϕ Sθ Sψ − Sϕ Cψ ) U z − K y y! % %% m !! z = (Cϕ Cθ ) U z − m g − K z z! $ % J p ϕ!! = Uϕ − θ! ψ! ( J r − J q ) − θ! JT ΩT − K p ϕ! % % J q θ!! = Uθ − ϕ! ψ! ( J p − J r ) − ϕ! JT ΩT − K q θ! % %& J r ψ!! = Uψ − ϕ! θ! ( J q − J p ) − K r ψ!

2

(2)

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

IOP Publishing doi:10.1088/1742-6596/570/8/082003

where m is the mass of the quadrotor, J p , J q , J r are the moments of inertia of the quadrotor, JT is the moment of inertia for the engines and ΩT = Ω1-Ω2+Ω3-Ω4 is the overall speed of the propellers wherein Ωi is the speed of the ith propeller. S• and C• are the respective abbreviations of sin(.) and cos(.). 3. Controlling the vehicle With 6 degrees of freedom, and 4 actuators, the quadrotor is an under-actuated system, making it impossible to control all of the states simultaneously. The controlled outputs are generally { x, y, z, ψ } , thus with stable pitch and roll angles, a quadrotor is able to reach a desired position in space and a desired yaw angle. As stated in [1] several UAV flight control algorithms already exist, including the Gain—Scheduled PID, adaptive control, Model Reference Adaptive Control (MRAC), Sliding Mode Control, Backstepping Control (BSC), Control allocation, Model Predictive Control (MPC), and Feedback Linearization (FL). 3.1. Feedback Linearization 3.1.1. Procedure's description Feedback linearization is a common approach used in controlling nonlinear systems ([2], [6]). The approach involves coming up with a transformation of the nonlinear system into an equivalent linear system through a change of variables and a suitable control input. Feedback linearization may be applied to nonlinear systems as: !# x! (t ) = f ( x ) + g ( x ) u (t ) " $# y (t ) = h ( x )

(3)

where x ∈ ℜn is the state vector, u ∈ ℜ p is the vector of inputs, and y ∈ ℜm is the vector of outputs. The goal is to develop a control input u (t ) = a ( x ) + b ( x ) v (t ) that induces a linear input-output map between the new input v(t) and the output y(t). An outer-loop control strategy for the resulting linear system can then be applied. 3.1.2. Procedure's application In this section, we use the model (2) after neglecting the gyroscopic effects and the drag forces, while using the steps of the work in [2], to develop a feedback Linearization controller of a quadrotor ensuring that the position { x (t ), y (t ), z (t ), ψ (t )} tracks the desired trajectory { xd (t ), yd (t ), zd (t ), ψ d (t )} asymptotically. T

T

Let ξ = !" x, y, z, x!, y!, z!, ϕ, θ , ψ, ϕ!, θ!, ψ! #$ be the state vector, U = !"T, Tϕ , Tθ , Tψ #$

the control input, and

T

η (ξ ) = [ x, y, z, ψ ] the output vector. Being under-actuated, we cannot use a static but a dynamic output

feedback control. By successive differentiation, a nonsingular input appears when we reach the forth derivative of x, y, and z, and the second derivative of ψ : x(

4)

(t ) =

U z " !! −ψ Sψ Sθ Cϕ − ψ! 2 Cψ Sθ Cϕ − ψ! θ! Sψ Cθ Cϕ + ψ! ϕ! Sψ Sθ Sϕ + θ!!Cψ Cθ Cϕ − θ! ψ! Sψ Cθ Cϕ − θ! 2 Cψ Sθ Cϕ − θ! ϕ! Cψ Cθ Sϕ − ϕ!! Cψ Sθ Sϕ m# +ϕ! ψ! Sψ Sθ Sϕ + ϕ! θ! Cψ Cθ Sϕ − ϕ! 2 Cψ Sθ Cϕ + ψ!! Cψ Sϕ − ψ! 2 Sψ Sϕ + ψ! ϕ! Cψ Cϕ +ϕ!! Sψ Cϕ + ψ! ϕ! Cψ Cϕ − ϕ! 2 Sψ Sϕ

%$(4)

U!! 2 U! + z "#−ψ! Sψ Sθ Cϕ + θ! Cψ Cθ Cϕ − ϕ! Cψ Sθ Sϕ + ψ! Cψ Sϕ + ϕ! Sψ Cϕ $% + z "#Cψ Sθ Cϕ + Sψ Sϕ $% m m y(

4)

(t ) =

U z " !! ψ Cψ Sθ Cϕ − ψ! 2 Sψ Sθ Cϕ + ψ! θ! Cψ Cθ Cϕ − ψ! ϕ! Cψ Sθ Sϕ + θ!!Sψ Cθ Cϕ + θ! ψ! Cψ Cθ Cϕ − θ! 2 Sψ Sθ Cϕ − θ! ϕ! Sψ Cθ Sϕ − ϕ!! Sψ Sθ Sϕ m# −ϕ! ψ! Cψ Sθ Sϕ − ϕ! θ! Sψ Cθ Sϕ − ϕ! 2 Sψ Sθ Cϕ −ϕ!! Cψ Cϕ + ϕ! 2 Cψ Sϕ + ψ! ϕ! Sψ Cϕ + ψ!! Sψ Sϕ + ψ! ϕ! Sψ Cϕ + ψ! 2 Cψ Sϕ $% U!! 2 U! + z "#ψ! Cψ Sθ Cϕ + θ! Sψ Cθ Cϕ − ϕ! Sψ Sθ Sϕ + ψ! Sψ Sϕ − ϕ! Cψ Cϕ $% + z "#Sψ Sθ Cϕ − Cψ Sϕ $% m m

3

(5)

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

z(

4)

(t ) =

IOP Publishing doi:10.1088/1742-6596/570/8/082003

2 U! z " ! U!! U z " !! −θ Sθ Cϕ − θ! 2 Cθ Cϕ + θ! ϕ! Sθ Sϕ − ϕ!! Cθ Sϕ + ϕ! θ! Sθ Sϕ − ϕ! 2 Cθ Cϕ $% + −θ Sθ Cϕ − ϕ! Cθ Sϕ $% + z "#Cθ Cϕ $% m# m # m

with ϕ!! =

Uϕ !! Uθ ,θ= Jp Jq

and

ψ!! =

(6)

Uψ T T . Let ς = !"U z , U! z #$ and P = [ x, y, z ] thus we can write: Jr

! ( 4) # P (t ) # (2) # ψ (t ) "

T

With u! = !"U""z , Uϕ , Uθ , Uψ #$ , l = [l1, l2 , l3, l4 ]

T

$ & & = l (ξ , ς ) + J (ξ , ς ) u! (t ) & % ! J J12 J13 J14 # 11 # J J J J and J = # 21 22 23 24 # J31 J32 J33 J34 # J 41 J 42 J 43 J 44 "

(7) $ & & & where & & %

Uz " ! 2 −ψ Cψ Sθ Cϕ − ψ! θ! Sψ Cθ Cϕ − ψ! ϕ! Sψ Sθ Sϕ − θ! ψ! Sψ Cθ Cϕ − θ! 2 Cψ Sθ Cϕ − θ! ϕ! Cψ Cθ Sϕ + ϕ! ψ! Sψ Sθ Sϕ − ϕ! θ! Cψ Cθ Sϕ − ϕ! 2 Cψ Sθ Cϕ m# 2 U! z " ! −ψ! 2 Sψ Sϕ +2 ψ! ϕ! Cψ Cϕ − ϕ! 2 Sψ Sϕ $% + −ψ Sψ Sθ Cϕ + θ! Cψ Cθ Cϕ − ϕ! Cψ Sθ Sϕ + ψ! Cψ Sϕ + ϕ! Sψ Cϕ $% m # U l2 (t ) = z "#−ψ! 2 Sψ Sθ Cϕ + ψ! θ! Cψ Cθ Cϕ − ψ! ϕ! Cψ Sθ Sϕ + θ! ψ! Cψ Cθ Cϕ − θ! 2 Sψ Sθ Cϕ − θ! ϕ! Sψ Cθ Sϕ − ϕ! ψ! Cψ Sθ Sϕ − ϕ! θ! Sψ Cθ Sϕ − ϕ! 2 Sψ Sθ Cϕ m 2 U! z " ! +ψ! 2 Cψ Sϕ + 2 ψ! ϕ! Sψ Cϕ + ϕ! 2 Cψ Sϕ $% + ψ Cψ Sθ Cϕ + θ! Sψ Cθ Cϕ − ϕ! Sψ Sθ Sϕ + ψ! Sψ Sϕ − ϕ! Cψ Cϕ $% m # 2 U! z " ! U l3 (t ) = z "#−θ! 2 Cθ Cϕ + θ! ϕ! Sθ Sϕ + ϕ! θ! Sθ Sϕ − ϕ! 2 Cθ Cϕ $% + −θ Sθ Cϕ − ϕ! Cθ Sϕ $% m m #

l1 (t ) =

(8)

(9) (10) (11)

l4 (t ) = 0 1 J11 = !"Cψ Sθ Cϕ + Sψ Sϕ #$, m J 21 =

1! "Sψ Sθ Cϕ − Cψ Sϕ #$, m

J31 =

1! "Cθ Cϕ #$, m

J32 =

U J12 = z !"−Cψ Sθ Sϕ + Sψ Cϕ #$, m Jp J 22 =

Uz ! "−Sψ Sθ Sϕ − Cψ Cϕ #$, m Jp

Uz ! "−Cθ Sϕ #$, m Jp

J33 =

U J13 = z !"Cψ Cθ Cϕ #$, m Jq J 23 =

Uz ! "−Sθ Cϕ #$, m Jq

Uz ! "Sψ Cθ Cϕ #$, m Jq

J34 = 0,

U J14 = z !"−Sψ Sθ Cϕ + Cψ Sϕ #$, m Jr J 24 =

J 41 = J 42 = J 43 = 0,

Uz ! "Cψ Sθ Cϕ + Sψ Sϕ #$, m Jr J 44 =

(12)

1 Jr

By taking u! (t ) = J −1 (−l + v (t )) , we linearize the system and obtain: x(

4)

(t ) = η1(4) (t ) = v1 (t ) 4 4 y ( ) (t ) = η2( ) (t ) = v2 (t )

z(

4)

(t ) = η3(4) (t ) = v3 (t ) 2 2 ψ ( ) (t ) = η4( ) (t ) = v4 (t )

Remark 1: It should be noted that the determinant of the matrix J is when

" π π% $− < ϕ < ' , # 2 2&

Cϕ J p Jq m2

(13)

therefore; J is nonsingular

which is satisfied generally.

For trajectory tracking ([1]), with ei = (ηid − ηi ) , we can use: 4

v1 (t ) = η1d( ) (t ) + ∑ C1j−1 e1j−1 4

4

j=1

( 4)

4

( ) v3 (t ) = η3d (t ) + ∑C 3j−1 e3j−1 j=1

4

v2 (t ) = η2d (t ) + ∑ C e 2 j−1

j−1 2

(2)

(14)

4

v4 (t ) = η4d (t ) + ∑ C e

j=1

4 j−1

j−1 4

j=1

3.2. Feedback Linearization Approach to Fault Tolerance An effective fault recovery is achieved after two major successive procedures. A Fault Detection and Isolation (FDI), is a fast algorithm that indicates to the system the sensor, actuator, or body part that has been damaged. Following the FDI decision, the system switches to the adequate FTC algorithm

4

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

IOP Publishing doi:10.1088/1742-6596/570/8/082003

capable of stabilizing and maneuvering the damaged quadrotor. This paper does not cover the FDI procedure. Simulations to the post-FDI stage were made by manually indicating the damaged actuator, and started with initial conditions chosen far from the hover equilibrium point, representing conditions after the accomplishment of the FDI. In other words, we assume in this work that the FDI decision is available with some delay. We don’t consider here the case of false alarms, missed detection or incorrect fault isolation. The fault tolerant controller is developed following a double control loop architecture in which an inner and faster controller has the task to regulate the attitude angles and the altitude of the vehicle, while an outer and slower controller has the aim of modifying the desired values of the attitude angles in order to perform trajectory tracking. The control law of the whole system is computed by using the same steps of the work in [3].

ϕ, θ , ψ, ϕ!, θ!, ψ! , x, y, z

Figure 2. The double control loop architecture T

! y!, z!#$ the state vector. The dynamic equations (2), after neglecting Let χ = !"ϕ, θ , ψ, ϕ!, θ!, ψ! , x, y, z, x, the gyroscopic effects due to the propeller's rotation, can be rewritten in a state-space form: & Sχ Sχ ( χ! 1 = χ 4 + χ 5 S χ1 2 + χ 6 C χ1 2 C C χ χ2 ( 2 ( χ! = χ C − χ S 5 χ1 6 χ1 ( 2 ( 1 " ( χ! 3 = C # χ 5 S χ1 + χ 6 C χ1 $% χ2 (( ' 1" ( χ! 4 = #−K p χ 4 − χ 5 χ 6 ( J r − J q ) +Uϕ $% Jp ( ( 1 ( χ! 5 = "#−K q χ 5 − χ 4 χ 6 ( J p − J r ) +Uθ $% Jq ( ( 1 ( χ! 6 = "#−K r χ 6 − χ 4 χ 5 ( J q − J p ) +Uψ $% () Jr

& χ! 7 = χ10 ( ( χ! 8 = χ11 ( χ! 9 = χ12 ( ( χ! = 1 " C S C + S S U − K χ $ χ1 χ 3 ) z x 10 % ' 10 m #( χ1 χ 2 χ 3 ( 1 ( χ! = " C S S − S C U − K χ $ χ1 χ 3 ) z y 11 % ( 11 m #( χ1 χ 2 χ 3 ( 1 ( χ! 12 = #"C χ1 C χ 2 U z − K z χ12 − m g%$ ) m

(15)

Uψ $ &. c % U % T −d " In case of failure on M2: the control input vector is chosen as u = !"U z , Uθ , Uψ #$ , where Uϕ = $U z − ψ ' . 2 # c & U % T −d " In case of failure on M3: the control input vector is chosen as u = !"U z , Uϕ , Uψ #$ , where Uθ = $U z + ψ ' . 2 # c & U % T d" In case of failure on M4: the control input vector is chosen as u = !"U z , Uθ , Uψ #$ , where Uϕ = $U z − ψ ' . 2# c & T

d! 2"

In case of failure on M1: the control input vector is chosen as u = !"U z , Uϕ , Uψ #$ , where Uθ = #U z +

3.2.1. Inner Control Loop From the state-space form it can be seen that the dynamics of the state variables χ1, χ 2 , χ 4 , χ 5, χ 6 , χ 9 χ12 , which we will call χ for notation simplicity, can be written as χ! = f ( χ ) + g ( χ ) u . The dynamics of the states χ1, χ 2 and χ 9 can be similarly written as:

5

European Workshop on Advanced Control and Diagnosis Journal of Physics: Conference Series 570 (2014) 082003

! # # # #"

χ! 1 χ! 2 χ! 9

IOP Publishing doi:10.1088/1742-6596/570/8/082003

! S S # χ 4 + χ 5 Sχ χ 2 + χ 6 Cχ χ 2 1 1 $ # Cχ 2 Cχ 2 & # &=# χ 5 C χ1 − χ 6 S χ1 & # χ12 &% # # "

$ & & & ˆ & = f (χ) & & & %

(16)

which, in this case, is independent of the input vector u. This property becomes useful when T calculating the second derivative of [ χ1, χ 2 , χ 9 ] : ! χ!! $ # 1 & d fˆ ( χ ) ∂ fˆ ( χ ) ∂ fˆ ( χ ) ∂ fˆ ( χ ) # χ!!2 & = = χ! = f (χ) + g(χ )u dt ∂χ ∂χ ∂χ & # #" χ!!9 &% ∂ fˆ ( χ ) , the previous dynamics can be written as: Denoting the Jacobian matrix with J ( χ ) = ∂χ ! χ!! $ # 1 & # χ!!2 & = J ( χ ) f ( χ ) + J ( χ ) g ( χ ) u & # #" χ!!9 &% " ±J l % r C χ1 '' # 2 Jp d &

It can be proved that matrix J ( χ ) g ( χ ) is invertible if and only if χ 2 ≠ arctan $$ " ±J l χ1 ≠ arctan $$ r # 2 Jp d

(17)

(18)

and

% '' . In most practical scenarios this condition is satisfied. In our case, with the &

parameters adopted for the simulation, the invertibility condition is guaranteed as long as the pitch and roll angles are limited to 80° which is a value never reached by the quadrotor during non-aerobatic flight. Let χ id , χ! id , χ!!id be the desired values for χ i , χ! i , χ!!i and define the ith error as ei = χ i − χ id . If the control inputs are chosen as: u* = − ( J ( χ ) g ( χ ) )

−1

(" *$ −1 * J ( χ ) f ( χ ) + (J ( χ ) g( χ ) ) $ *$ *$# )

" 1 χ!!1d % $ Ci1 e1 ' χ!!2 d ' − $ Ci21 e2 ' $ χ!!9 d '& $ Ci91 e9 #

% " %+ 2 ' $ Ci1 e!1 '' − $ Ci 2 e! '' $ 2 2 '' $ Ci92 e!9 '& # &,

(19)

where Ciij are positive constants, then the error dynamics can be written as ! e!! $ ! Ci1 e $ ! Ci 2 e! $ ! $ # 1 & # 1 1 & # 1 1 & # 0 & # !! e2 & + # Ci21 e2 & + # Ci22 e!2 & = # 0 & & # & & # # e9 &% # Ci91 e9 & # Ci92 e!9 & #" 0 &% #" !! % " % "

(20)

which yields stable second-order dynamics providing exponential decay of ei for i=1, 2, 9 (respectively the roll, pitch and altitude errors). 3.2.2. Outer Control Loop An outer control loop's task is to generate the desired values for the roll and pitch angles, in a way to reach a desired position in the horizontal plane. The constants Ci and Co represent the natural frequency of the subsystem's dynamics, subsystems described by the state variables χ1, χ 2 , χ 9 and χ 7, χ 8, χ10 χ11 . To avoid compromising the stability of the system, we choose Co