A New Nonlinear PI/PID Controller for Quadrotor ... - IEEE Xplore

10 downloads 253 Views 280KB Size Report
Salvador González-Vázquez and Javier Moreno-Valenzuela. Centro de Investigación y Desarrollo de Tecnologıa Digital. Tijuana B.C. ; México. Av. del Parque ...
2010 Electronics, Robotics and Automotive Mechanics Conference

A new nonlinear PI/PID controller for Quadrotor posture regulation Salvador Gonz´alez-V´azquez and Javier Moreno-Valenzuela Centro de Investigaci´on y Desarrollo de Tecnolog´ıa Digital Tijuana B.C. ; M´exico Av. del Parque No. 1310, Mesa de Otay, C.P. 22510 {sgonzalez, moreno}@citedi.mx Abstract

1 Introduction

Based on the classic scheme of proportional , integral and derivative (PID) control, we present the design of a nonlinear controller, which aims to regulate the posture (position and orientation) of a 6 d.o.f. (degrees of freedom) aerial vehicle called Quadrotor. In particular, the Quadrotor horizontal position is controlled by PI actions, while the Quadrotor orientation and vertical position are controlled by PID algorithms. The controller shows robustness to aircraft systems effects, such as Coriolis forces and aerodynamic drag, although only the effect of gravitational forces is compensated. The controller performance is verified by numerical tests.

Several Quadrotor designs have been presented in [4, 5, 14, 15]. These belong to the so-called Mini/UAVs RW/VTOL (Mini / Unmanned Aerial Vehicles - Rotatory Wing / Vertical Take Off and Landing), which generally are used in different areas and social approaches described by [3, 13]. Applications are from supervisor system in disaster zones, traffic monitoring, wildlife monitoring, topographic mapping, fire detection, crops supervision, coordination of ground military units. Concerning the 3 d.o.f. Quadrotor (mounted on a static structure), in [2] was mentioned PD and PID actions, although in detail, only the PD action is explained under a linear approach. In [17, 18], a PD2 controller for the Quadrotor orientation was proposed. That controller compensates the gyroscopic and Coriolis terms, and global asymptotic stability arguments were presented. Also in [17, 18] it was mentioned the possibility to prove the global asymptotic stability without model compensation (gyroscopic and Coriolis terms). In relation to the control of 6 d.o.f. Quadrotor (freely moving), there are studies in the literature focused on the proposal and formal analysis of PD and PID control actions. In [11], PD actions are incorporated into a controller designed via exact feedback linearization, noting that the controller is efficient in quasi-stationary flight. A PD/PID-Backstepping controller was proposed in [12], which solves the problem of Quadrotor posture regulation (position and orientation). In that scheme, a PD term regulates the Quadrotor position, and a PID term regulates the Quadrotor orientation. Considering that the Quadrotor is in quasi-stationary flight, stability arguments were presented. PD-Backstepping controller for Quadrotor posture was proposed in [1], where the position is estimated through vision. However, there are not arguments to assure the closed-loop system stability.

Keywords: Control Systems, PID, Aerial Vehicles.

Notation Throughout this article, we will adopt the following notation. The scalar variables and constants are represented by serif lowercase letters, roman letters, and greek characters in normal format, i.e., x , θ ∈ R. Vectors of dimension n are defined by bold lowercase letters and bold italic greek letters, i.e., x , θ ∈ Rn . The matrices of size n × m are defined by italic capital letters, i.e., X ∈ Rn×m . To refer the element i of a vector x ∈ Rn the upper left superi script is used, i.e., x ∈ R. In ∈ Rn×n denotes the identity matrix of size n. on ∈ Rn denotes the vector of dimension n having ones as elements. To refer to the element i, j of a matrix X ∈ Rn×m the upper left superscript is used, i,j i.e., X ∈ R. An over dot placed on a variable, vector or matrix, indicates differentiation with respect to time, i.e., d x. A null vector or matrix will be denoted by 0. x˙ = dt For x ∈ R, we have that cx = cos (x), sx = sin (x) and −1 tx = tan (x), while c−1 x = 1/ cos (x), sx = 1/ sin (x) and −1 tx = 1/ tan (x) denote the respective reciprocal. 978-0-7695-4204-1/10 $26.00 © 2010 IEEE DOI 10.1109/CERMA.2010.78

642

In this paper, an alternative PI/PID design for a 6 d.o.f. Quadrotor is proposed. The new design incorporates PI or PID control actions for each d.o.f. of the Quadrotor. Unlike the PD actions, the integral action allows to decrease the steady-state error. Besides, the PI/PID actions allow better perfomance on Quadrotor movements. On the other hand, the proposed controller does not compensate the Coriolis forces, which minimizes its complexity.

The elements of the model (1)-(2) are described in Appendix A. For the underactuated part (1), Mu , Fu , Bu (qa ) ∈ R2×2 are the matrices of inertia, fiction and input, respectively, and uu (ua ) ∈ R2 is the input force vector, while, for actuated part (2), Ma (qa ) , Ca (qa , q˙ a ) , Fa (qa ) , Ba (qa ) ∈ R4×4 are the matrices of inertia, Coriolis, friction and input, respectively, ga ∈ R4 is the vector of gravitational forces and ua ∈ R4 is the input force vector. The expression (3) represents the vector of underactuated states (Quadrotor displacements with respect to the Earth’s plane), while the expression (4) represents the vector of actuated states (Quadrotor altitude and angular displacements). It is worth mentioning that the matrix Ba (qa ) is invertible under the restriction i   η  < 90o , i = 1, 2, 3, (5)

2 Quadrotor Model The Quadrotor system is described in [4, 5, 9, 10, 14, 16, 17, 18] and is shown in Figure 1, where B and E denotes the non inertial frame and inertial frame, respectively. Let us  T ∈ R3 the vector of angular denote by ω = p q r  T velocities, ν = u v w ∈ R3 the vector of linear  T ∈ R4 the vector velocities, and ub = f1 f2 f3 f4 of lift force delivered by the propellers.

being i η ∈ R the i-th element of the vector η ∈ R3 .

3 Proposed controller 3.1 Control objective The control objective will be established by using the following variables ˜qu = ˜qa = where

Figure 1. Quadrotor reference frame

Consequently, the movements in B can be transferred to a fixed reference frame E, resulting in three rotational  T φ θ ψ movements η = ∈ R3 called Euler angles, and three traslational movements denoted by  T ξ= x y z ∈ R3 .

(1)

qa + Ca (qa , q˙ a ) q˙ a + Ma (qa ) ¨ +Fa (qa ) q˙ a + ga = Ba (qa ) ua ∈ R4 ,

(2)

where qa =



z η

qu = T T



T x y ∈ R2 ,  T = z φ θ ψ ∈ R4 .

˜x ˜y

T

˜ θ˜ ψ ˜ ˜z φ

= qdu − qu T

∈ R2 ,

= ∗ qda − qa ∈ R4 ,

T  qdu = xd yd ∈ R2 , T  ∗ qda = zd ∗ η d T ∈ R4 ,  T ∗ η d = ∗ σ d T ψd ∈ R3 , T  ∗ σ d = ∗ φd ∗ θd ∈ R2 ,

(6) (7) (8) (9) (10) (11)

being ˜qu ∈ R2 and q˜a ∈ R4 the vector of errors for underactuated and actuated part, respectively, qdu ∈ R2 the vector of Quadrotor desired position on the plane, while ∗ qda ∈ R4 is the vector of Quadrotor desired altitude and angular movements. The superscript ∗ means that the vector depends of the system states, and therefore also depends of time. Moreover, xd , yd , zd and ψd ∈ R are defined to be constants.

In [9] the Quadrotor dynamic model is presented, which can be rewritten as proposed in [7], that is, Mu ¨ qu + Fu q˙ u = Bu (qa ) uu (ua ) ∈ R2 ,





By designing control inputs ua ∈ R4 for the system (1)(2), the control objective consist in guaranteeing that the vectors (6) and (7) satisfy the limits

(3)

lim ˜q t→∞ u

(t) = 0

∈ R2 ,

(12)

(4)

lim ˜q t→∞ a

(t) = 0

∈ R4 .

(13)

643

The control objective (12)-(13) has been established in general form, because the vector ∗ σ d ∈ R2 and the allowable values for xd , yd , zd and ψd ∈ R have not been defined explicity yet. For the Quadrotor posture regulation problem, the proposed vector ∗ σ d ∈ R2 , must satisfy ∈ R2

lim ∗ σ d (t) = 0

t→∞

3.4 Underactuated part controller According to Section 3.1, to satisfy the control objective (12)-(13), we propose the vector ∗

(14)

3.2 About control actions According to [6], the underactuated part dynamics is dependent on the actuated inputs. For Quadrotor system (1)(2), the vector uu (ua ) is dependent on the input force vector ua , as described in [7], T  ∈ R2 , (15) uu (ua ) = ρ (ua ) ρ (ua ) ρ (ua ) =

4 

i

ua = oT4 ua

∈R,

(16)

a (qa ) =

where

[uapid (˜ea ) + ga ] ∈ R4 , (17)

˜eu =

4

being Ba (qa ) ∈ R and ga ∈ R , the kinematic compensation and gravitational forces compensation, respectively. The term uapid (˜ea ) ∈ R4 contains PID actions for the actuated part, which are described by ςa , uapid (˜ea ) = KP a ˜ q˙ a + KIa ˜ qa + KDa ˜ ˜ς˙ a = ˜ qa , where ˜ea =



˜ ς Ta

˜ qTa

˜ q˙ Ta

T

∈R

12

,

T

∈ R3 ,

(20)



0 −tφ tψ

T

∈ R2 ,

(24)

and β > 0 is a constant parameter. The vector uupi (˜eu ) ∈ R2 contains the PI actions for underactuated part, described as uupi (˜eu ) = KP u ˜qu + KIu ˜ς u , ˜ς˙ u = ˜qu , (25)

To satisfy the control objective (12)-(13), the following controller is proposed,

4×4

ψd

being i χ ∈ R the i-th element of the vector χ ∈ R2 , while remaining elements are ⎡ ⎤ −cψ sψ ⎦ ∈ R2×2 , A (qa ) = ⎣ (23) −1 −1 cφ cψ 0

3.3 Actuated part controller

−1

σd T

χ (˜eu , qa ) = β A (qa ) uupi (˜eu ) + a (qa ) ∈ R2 , (22)

 T being o4 = 1 1 1 1 ∈ R4 . The fact that ρ (ua ) can be expressed as the inner product oT4 ua simplifies the derivation of the closed-loop system, which is described later. In agreement to equations (15) and (16), the underactuated part of the Quadrotor dynamics (1) is controlled through the input force vector ua .

−1



where

i=1

ua (˜ea , qa ) = Ba (qa )



where ψd ∈ R is a constant value, while the function ∗ σ d is defined as ⎡

⎤ 1 arcsin χ (˜eu , qa ) ⎢ ⎥ σ d (˜eu , qa ) = ⎣

⎦ ∈ R2 , (21) 2 arcsin χ (˜eu , qa )

if the limits of (12) and (13) are satisfied. ∗ σ d ∈ R2 is a proposed function, this will be discussed in detail later.

where

ηd =



˜ς Tu

q˜˙ Tu

˜qTu

T

∈ R6 ,

(26)

KP u = diag {kpx , kpy } > 0 and KIu = diag {kix , kiy } > 0 ∈ R2×2 are the gains for proportional and integral control actions, respectively. The vector ˜eu ∈ R6 contains the states of (25), implying that uupi (˜eu ) : R6 → R2 .

(18)

4 Closed-loop system

(19)

According to Section 3.4, the vectors of desired positions (8) and (9), become

KP a = diag {kpz , kpφ , kpθ , kpψ } > 0, KDa = diag {kdz , kdφ , kdθ , kdψ } > 0 and KIa = diag {kiz , kiφ , kiθ , kiψ } > 0 ∈ R4×4 are the gains for proportional, derivative and integral control actions, respectively. The vector ˜ea ∈ R12 contains the states of (18), implying that uapid (˜ea ) : R12 → R4 .

qdu = ∗





xd

yd

T

∈ R2 ,

(27) T

∈ R4 , (28) with the vector σ d (˜eu , qa ) ∈ R2 described by (21), while xd , yd , zd and ψd ∈ R are constant values. From the vectors

644

qda := qda =

zd

T

σ d (˜eu , qa )

ψd

of the Quadrotor model (1)-(2) are described explicitly in Appendix A. The gains for PI actions (18) and PID actions (25) are provided in Table 1. We have used β = 1.

(19) and (26), corresponding to the actuated and underactuated part, respectively, we define the overall vector of errors (state space vector of closed-loop system) T  T ˜e = ˜ ∈ R18 . (29) ς Ta ˜ ςu ˜ q˙ Tu ˜ q˙ Ta qTu ˜ qTa ˜

Table 1. PID controller gains

Taking the time derivative of (29) and substituting equations (1)-(2), (6), (7), (27), (28), the controller equations (15) and (17), the closed-loop system can be written as

¨ qda (˜ qu , qa ) =



¨ Td σ

0

(˜ qu , qa ) 0

T

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ∈ R18, ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (30)

3

4

∈ R , (31)

= ρ (˜ea , qa ) = 1

3,3

2,3

R (η) R (η) 1

−1

R (η)

P I D

KP a = diag {3, 20, 20, 20} KIa = diag {0.02, 0.1, 0.1, 0.1} KDa = diag {0.2, 1, 1, 1}

ρ (˜ea , qa )

∈ R2 ,

uapid (˜ea ) + mg



uapid (˜ea ) = kpz ˜z + kdz ˜z˙ + kiz ς˜z , ς˜˙z = ˜z ,

x

y

z

xd

yd

zd

2.5 2 1.5

up (˜ea , qa ) = Bu (qa ) uu (˜ ea , qa ) 1,3

KP u = diag {0.3, 0.3} KIu = diag {0.002, 0.002}

The Figure 2 shows the Quadrotor position behav T x (t) y (t) z (t) ior ξ (t) = for a sequence T  xd yd zd of desired position values ξ d = . Figure 3 shows the Quadrotor orientation η (t) =  T φ (t) θ (t) ψ (t) for the desired orientation func T . tion η d (˜eu , qa ) = φd (˜eu , qa ) θd (˜eu , qa ) ψd

[m]

⎢ ⎢ ⎢ ˜ q˙ u ⎢ ⎢ ⎢ ⎢ Mu−1 [ Fu q˙ u − up (˜ea , qa ) ] ⎢ ⎢ ˜e˙ = ⎢ ⎢ ˜ qa ⎢ ⎢ ⎢ ⎢ ˜ q˙ a ⎢ ⎢ ⎢ −1 ⎣ ¨ qu , qa ) + Ma (qa ) [ Ca (qa , q˙ a ) q˙ a qda (˜ +Fa (qa ) q˙ a − uapid (˜ea ) ] where

Gains

P I



˜ qu

ξ , ξd



Action

(32)

1

0.5 0 0

∈R, (33)

10

20

30

t[s]

40

50

60

50

60

Figure 2. Quadrotor position (34) 60 50 40 30 20 10 0 −10 −20 −30 0

[ degrees ]

¨ Td (˜ being σ qu , qa ) ∈ R2 the second time derivative of (21), 1 and uapid (˜ea ) ∈ R the first element of the vector (18).

θ

ψ

φd

θd

ψd

η , ηd

The origin of the state space ˜e = 0 ∈ R18 is an equilibrium of the closed-loop equation (30). By analysis based on Lyapunov’s stability theory described in [8], it can be shown that the origin of the state space is locally exponentially stable. Due to space limitations in this document, that analysis is omitted. Consequently, the controller ua (˜ea , qa ) ∈ R4 in (17) and the function σ d (˜eu , qa ) ∈ R2 in (21) guarantees the accomplishment of control objectives (12)-(13). In addition, note that the limit (14) is also satisfied.

φ

10

20

30

t[s]

40

Figure 3. Quadrotor orientation

5 Numerical tests

Solid lines represents the Quadrotor actual configuration, and dotted lines describes the Quadrotor desired configuration, respectively. The Figure 4 shows the Quadrotor path behavior with respect to the frame E.

Using Matlab, we have verified the Quadrotor behavior controlled by PI/PID actions. The parameters and elements

645

Table 2. Description of the model parameters

z[m]

4 2

Notation

10 < t ≤ 20

t > 40

20 < t ≤ 30

0 < t ≤ 10

0 30 < t ≤ 40

−2 2

t=0

g

Gravity. [m s

Total mass of Quadrotor. [kg]

0.141

ι

Distance between the Quadrotor center of mass and the rotation axis of propeller. [m]

0.175

mp , mq

Moment of inertia along φ, θ, respectively. [kg m2 rad−1 ]

2.1 × 10−3

mr

Moment of inertia along ψ. [kg m2 rad−1 ]

4.2 × 10−3



Propeller conversion factor (thrust force → torque force). [m]

10.2 × 10−3

fp , fq

Aerodynamic friction coefficient along φ, θ, respectively. [N m s rad−1 ]

6.5 × 10−2

fr

Aerodynamic friction coefficient along ψ. [N m s rad−1 ]

7.8 × 10−2

fu , fv , fw

Aerodynamic drag coefficient in direction of u, v, w, respectively. [N s m−1 ]

6.5 × 10−1

1 0.5

y[m]

0 0

x[m]

Figure 4. Quadrotor path

6 Conclusions An alternative design of a PI/PID controller for posture regulation of a Quadrotor has been proposed. In particular, each d.o.f. can be tuned by its own PI or PID scheme. In relation to the literature, we used a more complete Quadrotor model, considering inherent effects that belong to the system, such as Coriolis terms and aerodynamic drag. Notice that these effects were not compensated in the new controller, which represents an advantage in a real-time implementation. Through numerical tests, the exponential stability of the state space origin was verified.

−T

T

M W (η)

∈ R3×3

M = diag {mp , mq , mr } ∈ R3×3

⎤ cφ s θ cψ + s φ s ψ cφ s θ s ψ − s φ cψ ⎦ cφ cθ

Coriolis matrices:  Ca (qa , q˙ a ) =

0

03×1 −T

˙ = W (η) CΔ (η, η)

01×3 ˙ CΔ (η, η)



∈ R4×4 −1

˙ W (η) CW (η, η)

∈ R3×3

−1 ˙ T ˙ ∈ R3×3 ˙ = SMω (η, η) CW (η, η) − M W (η) W ⎡ ⎤ 0 −mr r mq q 0 −mp p ⎦ ∈ R3×3 SMω = ⎣ mr r 0 −mq q mp p

Vector of gravitational torques: 01×3

−1

MΔ (η) = W (η)

Appendix A: Elements of the dynamic model

mg

9.80665

Mu = mI2 ∈ R2×2   m 01×3 Ma (qa ) = ∈ R4×4 03×1 MΔ (η)

This work was supported by Secretar´ıa de Investigaci´on y Posgrado del IPN, grant SIP-20100553, and by CONACyT, M´exico.



]

Inertia matrices:

Acknowledgments

Transformation matrices: ⎡ cθ cψ s φ s θ cψ − cφ s ψ R (η) = ⎣ cθ sψ sφ sθ sψ + cφ cψ −sθ s φ cθ ⎡ ⎤ 1 s φ tθ c φ tθ cφ −sφ ⎦ W (η) = ⎣ 0 −1 0 s φ cθ cφ c−1 θ

Value −2

m 1.5

1

ga =

Description

∈ R4

˙ = ω (η, η)

In Table 2, the model parameters are described.

646



p

q

r

T

−1

= W (η)

η˙ ∈ R3

Friction matrices: considering that fu = fv = fw , then

[6] I. Fantoni and R. Lozano. Non-Linear Control for Underactuated Mechanical Systems. Springer-Verlag, London, 2001.

Fu = diag {fu , fv } ∈ R2×2   fw 01×3 Fa (qa ) = ∈ R4×4 03×1 FΔ (η) −T

FΔ (η) = W (η)

Fω W (η)

−1

[7] S. Gonz´alez-V´azquez and J. Moreno-Valenzuela. Modelling of a Quadrotor: a new parametrization. Technical report, Centro de Investigaci´on y Desarrollo de Tecnolog´ıa Digital, I.P.N., 2010.

∈ R3×3

[8] H. K. Khalil. Nonlinear Systems. Prentice-Hall, Upper Saddle River, N.J., 1996.

Fω = diag {fp , fq , fr } ∈ R3×3 Input matrices:  1,3 Bu (qa ) = 

0 2,3 R (η)

3,3

Ba (qa ) = ⎡

R (η) 0

1 ⎢ 0 Bf = ⎢ ⎣ −ι −kψ

R (η) 01×3 −T 03×1 W (η) 1 ι 0 kψ

1 0 ι −kψ

[9] D. Lee, T. C. Burg, B. Xian, and D. M. Dawson. Output feedback tracking control of an underactuated Quad-Rotor UAV. In Proceedings of the American Control Conference, pages 1775–1780, New York city, U.S.A., July11-13, 2007.



[10] T. Madani and A. Benallegue. Sliding mode observer and backstepping control for a Quadrotor unmanned aerial vehicles. In Proceedings of the American Control Conference, pages 5887–5892, New York, NY, July11-13, 2007.

∈ R2×2 

[11] A. A. Mian and D. Wang. Dynamic modeling and nonlinear control strategy for an underactuated Quad Rotor rotorcraft. Journal of Zhejiang University, Science A, 9(4):539– 545, 2008.

Bf ∈ R4×4

⎤ 1 −ι ⎥ ⎥ ∈ R4×4 0 ⎦ kψ

[12] A. A. Mian and D. Wang. Modeling and backstepping-based nonlinear control strategy for a 6 dof Quadrotor helicopter. Chinese Journal of Aeronautics, 21:261–268, 2008. [13] A. Ollero and L. Merino. Control and perception techniques for aerial robotics. Annual Reviews in Control, 28(2):167– 178, 2004. [14] P. Pounds, R. Mahony, and P. Corke. Modelling and control of a Quad-Rotor robot. In Proceedings of the Australasian Conference on Robotics and Automation, Auckland, New Zealand, Dec.6-8, 2006.

References [1] E. Altu˘g, J. P. Ostrowski, and R. Mahony. Control of a Quadrotor helicopter using visual feedback. In Proceedings of the IEEE International Conference on Robotics and Automation, volume 1, pages 72–77, Washington, DC, E.U.A., May11-15, 2002.

[15] P. Pounds, R. Mahony, P. Hynes, and J. Roberts. Design of a Four-Rotor aerial robot. In Proceedings of the Australasian Conference on Robotics and Automation, pages 145–150, Auckland, New Zealand, Nov.27-29, 2002.

[2] S. Bouabdallah, A. Noth, and R. Siegwart. PID vs LQ control techniques applied to an indoor Micro Quadrotor. In Proceedings of the IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS 2004), volume 3, pages 2451–2456, Sendai, Miyagi, Japan, Sept.28 / Oct.2, 2004.

[16] G. V. Raffo, M. G. Ortega, and F. R. Rubio. Backstepping/nonlinear H∞ control for path tracking of a Quadrotor unmanned aerial vehicle. In Proceedings of the American Control Conference, pages 3356–3361, Westin Seattle Hotel, Seattle, Washington, U.S.A., June11-13, 2008.

[3] A. Budiyono. Advances in unmanned aerial vehicles technologies. In Proceedings of the International Symposium on Intelligent Unmanned System (ISIUS 2008), pages 1–13, Nanjing, China, Oct.15-18 2008.

[17] A. Tayebi and S. McGilvray. Attitude stabilization of a FourRotor aerial robot. In Proceedings of the 43rd IEEE Conference on Decision and Control, volume 2, pages 1216–1221, Atlantis Paradise Islands, Bahamas, Dec.14-17, 2004.

[4] P. Castillo, P. Albertos, P. Garcia, and R. Lozano. Simple real-time attitude stabilization of a Quad-Rotor aircraft with bounded signals. In Proceedings of the 45th IEEE Conference on Decision and Control, pages 1533–1538, Manchester Grand Hyatt Hotel, San Diego CA, U.S.A., Dec.13-15, 2006.

[18] A. Tayebi and S. McGilvray. Attitude stabilization of a VTOL Quadrotor aircraft. IEEE Transactions on Control Systems Technology, 14(3):562–571, May 2006.

[5] P. Castillo, A. Dzul, and R. Lozano. Real-time stabilization and tracking of a Four-Rotor mini rotorcraft. IEEE Transactions on Control Systems Technology, 12(4):510–516, July 2004.

647