Điều khiển tối ưu từng đoạn hệ Quadrotor

Bài báo giới thiệu phương pháp thiết kế bộ điều khiển tối ưu từng đoạn trên trục thời gian để điều khiển bám quỹ đạo đặt trước cho hệ robot bay bốn động cơ. Khác với những bộ điều khiển MPC tương tự mà ở đó luôn nhất quán sử dụng cửa sổ dự báo hữu hạn, phương pháp thiết kế giới thiệu trong bài báo này sử dụng cửa sổ dự báo vô hạn. Điều này giúp cho bộ điều khiển luôn đảm bảo được tính bám ổn định cho hệ kín mà không cần phải có thêm hàm phạt trong hàm mục tiêu.

Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa VCCA 2015 DOI: 10.15625/vap.2015.0001 Optimal Receding Horizon Control for a Quadrotor Điều khiển tối ưu đoạn hệ quadrotor Nguyen Doan Phuoc(*), Lai Viet Thang, Bui Dang Truong Hanoi University of Science and Technology (*) phuoc.nguyendoan899@gmail.com Abstract The paper proposes an approach to design optimal receding horizon controller for tracking control a quadrotor On the contrary to the other MPC controllers, which use consistently a finite horizon, the here proposed controller uses an infinite horizon Hence it always guarantees the tracking stability of obtained closed loop systems without using an additional penalty function in objective function as usual Keywords: tracking control, infinite horizon, MPC, optimal control Tóm tắt Bài báo giới thiệu phương pháp thiết kế điều khiển tối ưu đoạn trục thời gian để điều khiển bám quỹ đạo đặt trước cho hệ robot bay bốn động Khác với điều khiển MPC tương tự mà quán sử dụng cửa sổ dự báo hữu hạn, phương pháp thiết kế giới thiệu báo sử dụng cửa số dự báo vô hạn Điều giúp cho điều khiển đảm bảo tính bám ổn định cho hệ kín mà không cần phải có thêm hàm phạt hàm mục tiêu Từ khóa: điều khiển bám, cửa sổ dự báo vô hạn, MPC, điều khiển tối ưu Introduction The quadrotor is a small helicopter, which has four rapidly spinning rotors to create a thrust force for keeping the helicopter hover in the air as depicted in Fig.1 From the aspect of a mechanical EulerLagrange system, the quadrotor has six degrees of freedom, involving three configuration variables to describe its translational motion and three other configuration variables for the rotational motion [1] But the quadrotor has only four independent inputs, the rotor speeds Therefore, the quadrotor is an underactuated mechanical system and hence the control of a quadrotor is one of the most fascinating and attractive problems in control engineering In last few years the control problem of quadrotor has received many attentions of researchers as a fundamental application in control of under-actuated flying objects [2], [3] and [4] Subsequently, many different automatic control methods for quadrotor have been published, including the PD design method in [2], [3] nonlinear Lyapunov, such as backstepping controllers in [2], [4], LQR controller in [2], [3] and sliding mode controller in [3], [4] as well as the MPC controller introduced in [3] VCCA 2015 4  1 z z  x y 3 2 3 Fig.1: Construction principle of a quadrotor z 2 4 x y  1 In [2]-[4] two linear controllers PD and PID were investigated for the control of quadrotor based on its simplified linearization model It was shown there that these controllers could be well useful, when the flying is near hover and the disturbances are absent [2] But unfortunately, the control performance will be very poor, if the control parameters are not quite properly selected [3] An attempt of applying LQR techniques to control quadrotors had been investigated in [2], [3] and [5] But these techniques require linear models of controlled objects and the linearization around an equilibrium point will cause obtained linear models to be far from the reality, these techniques obviously can not be applicable for the freely flying control Nonlinear state feedback controllers obtained by applying Lyapunov backstepping method were proposed in [2], [4] It was shown there that these controllers had to be reactive, especially for the yaw angle control However, as claimed in [2], more than 50% of the attempts to fly freely with this controller had been failed That obviously for the requirement of freely flying ability of quadrotors can not be acceptable The investigation results about the applicability of sliding mode controller, which is given in [2], [6] have shown that although this technique is able to stabilize the roll and pitch angles, but the occurred chattering effect, even if had been reduced, disturbs much the measurements and the desired control performance of closed systems [2] Whereas all above mentioned control methods are unable to handle unavoidable system constraints, the MPC method proposed in [3] seems to be well applied for such constrained control problem However, this control technique requires a discrete time model and an obliquely additional penalty function for objective function in order to guarantee the stability of the Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa VCCA 2015 closed system Unfortunately the question how to choose this penalty function suitably is still open, even till today Starting from the short analysis above of recently published methods for control of a quadrotor and their main properties of each, this paper will propose other control approach for the quadrotor The proposed approach is derived from the idea of output tracking control via infinite horizon receding control technique introduced in [7] With this infiniteness of receding horizon the requirement of additional penalty function for the objective function to guarantee the stability of closed loop systems would not be needed any more as usual in MPC techniques with finite horizon Consequently, it avoids towards also the solving of still unsolved problem that how to choose a penalty function suitably for the design of a nonlinear MPC controller The organization of the paper is as follows The main results are presented in Section In Section are all simulation results presented Then some concluding remarks are given in Section Controller design 2.1 Mathematical model of quadrotor Fig.1 depicts a principle construction of a quadrotor The quadrotor uses four spinning rotors The speeds 1, 2 , 3 , 4 of these rotors create a thrust force for keeping the quadrotor aloft as well as to help it hover in the air by changing accordingly these four rotor speeds For designing any controller it is obligatory a mathematical model of the controlled object required To achieve a mathematical model of the quadrotor either Newton mechanical laws or Lagrange equations are often used [1], [3]-[5] The hereafter used mathematical model of quadrotor is obtained by using Lagrange equations, which is introduced in [1], [5] as follows: x  f (x , u )  (1)  y  g (x )  where x  x1 ,  , x12   T         x , x , y , y , z , z , , ,  ,  ,  ,   T 2 2 u1  b (1       )  2 u  bl (   )   2 u3  bl (  1 )  2 2 u  d (1       )  and (4) f (x , u )   f1 ,  , f12  T with   f1  x   f2  u1 (c sin  cos   sin sin  ) m  f3  y    f4  u1 (sin sin  cos   cos sin  ) m f  z  5  f6  u1 cos  cos  m  g    f7      f8  (I y  I z ) I x  u I x    f9      f10  (I z  I x ) I y  u3 I y f     11   f12  (I x  I y ) I z  u I z  (5) are all system functions for describing the dynamic behavior of quadrotor The model parameters b, l , d in (4) and m , g , I x , I y , I z in (5) are respectively the lift constant, the distance between the rotor and the center of mass of the quadrotor, the drag constant, the mass of quadrotor, the gravitational acceleration, as well as three components of the corresponding inertia matrix I  diag (I x , I y , I z ) 2.2 Two correlative bilinear models of a quadrotor To develop a proper method for tracking control the given nonlinear mathematical model (1)-(5) of the quadrotor will be now rewritten correlatively in a bilinear form as follows: x  A(x )x  B (x )u  (6)  y  C (x )x  Whereas the output matrix: (2) is the vector of all system states, y  (x , y , z ,  )T  (x1 , x , x , x11 )T is the vector of four outputs, DOI: 10.15625/vap.2015.0001 (3) u  u1 , u , u3 , u  T is the vector of four input signals, which are related with the speeds 1, 2 , 3 , 4 of four spinning rotors as follows [1], [5]: 1 0 0 0 0 0 0   0 0 0 0 0 0 C (x )   0 0 0 0 0 0   0 0 0 0 0 0 of (6) is directly received from the equation (3), there are many different ways to get both remaining matrices A(x ), B (x ) of (6) from its original nonlinear model (1)-(5) The most used transformation methods for getting A(x ), B (x ) are as follows: The model BL1: VCCA 2015 Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa VCCA 2015 Two state dependent matrices A(x ), B (x ) of the correlative bilinear model (6) could be obtained by using Taylor series extension as follows: f A(x v , u v )  Av  x x ,u v B (x v , u v )  Bv  v f u , cos v cos v , 0, 0, 0, 0, 0,  T b   0, 0, 0, 0, 0, 0, 0,1 I x , 0, 0, 0,  T  x v ,u v  T b   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1 I z  T f x u denote Jacobian derivations of a function vector f (x , u ) and the pair (x v , u v ) are satisfied:  x v  Av x v  Bv u v For the particular circumstance of the quadrotor dynamical model (5) there are obtained [8]: Av  a1 , a ,  , a12   0, cos v sin v cos  v  sin v sin  v , 0, m cos v sin v sin  v  sin v cos  v , 0, b  0, 0, 0, 0, 0, 0, 0, 0, 0,1 I y , 0, where f b1  DOI: 10.15625/vap.2015.0001  with a1  a  a   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 T The model BL2: Both state dependent matrices A(x ), B (x ) of the bilinear model (6) can be received directly by changing the model (5) In that way they are obtained as follows [8]: A(x )  a1 , a ,  , a12   with a1 ,  , a are the same vectors as in Ar , a   0, x , 0, x11 , 0, 0, 0, 0, 0, 0, 0,  T  a  0, 0, 0, 0, 0, 0,1, 0, 0, 0, 0, (I x  I y )x10 I z  T a  1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 a   0, x , 0, 0, 0, 0, 0, 0, 0, 0, 0,  a   0, 0,1, 0, 0, 0, 0, 0, 0, 0, 0, 0 a10  0, 0, 0, 0, 0, 0, 0, (I y  I z )x12 I x ,1, 0, 0, a   0, 0, 0, 0,1, 0, 0, 0, 0, 0, 0, 0 a11   0, 0, 0, x , 0, 0, 0, 0, 0, 0, 0, 0 T T  T T a7  u1v  0,  sin v sin v cos  v  cos v sin  v , m 0,  sin v sin v sin  v  cos r cos  v , T a   0,0,0,0,0,0,1,0,0,    v (I z  I x ) I y ,0, v (I x  I y ) I z  T u1v  0, cos v cos v cos  v , 0, m cos v cos v sin  v , 0, cos v sin v , 0, 0, 0, 0, 0, 0 T  1, 0, 0, (I x  I y )v I z  T u1v  0,  cos v sin v sin  v  sin v cos  v , m 0, cos v sin v cos  v  sin v sin  v , 0, 0, 0, 0, 0, 0, 0,     0, 0, 0, 0, 0, 0, 0, (I y  I z )r I x , T a12  T  0, (I z  I x )r I y ,1, and Bv  b1 , b , b3 , b  with VCCA 2015   a12  0, 0, 0, 0, 0, 0, 0, 0, 0, (I z  I x )x I y ,1, and B (x )  b1 , b , b , b   T  where b1   0, cos x11 sin x cos x  sin x11 sin x , m 0,sin x11 sin x cos x  cos x11 sin x , 0, cos x cos x , 0, 0, 0, 0, 0, 0 T and all remaining vectors bi , i  2,3, are the same as in Bv given above  a10   0, 0, 0, 0, 0, 0, 0, (I y  I z ) v I x , a11  T T  sin v cos v , 0, 0, 0, 0, 0,  a9   2.3 Optimal receding horizon control for continuous bilinear systems Consider the bilinear system described generally in (6), where all function matrices A(x ), B(x ), C (x ) are in x continuous Suppose that at the current time instant tk the vector of system states x k  x (tk ) is measurable Then at the same time instant tk and during a short time interval [tk ,tk   ) afterwards the bilinear system (6) can be approximated as a linear time invariant system (LTI): x  Ak x  Bk u   , tk  t  tk 1  tk   (7) y  C k x  where: Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa VCCA 2015 Ak  A(x k ), Bk  B (x k ), C k  C (x k ) (8) are all now determined Of course, the smaller  is, the better its approximation (7) will be To tracking control the linear time invariant system (7) asymptotically to any desired output y by using d the optimal variation technique, it is required now this tracking problem for (7) to be converted correspondingly in a stabilizing control problem Denote the steady state of closed loop system after tracking phase with x s [k ] and u s [k ] , then this steady state must be satisfied: 0  Ak xs [k ]  Bk u s [k ]  y  C x [k ] k s  d  (9)    A Bk   x s [k ]     k  y  C k    u [k ]   s   d  where  is a zero matrix Therefore, if the matrix:  A Bk  Fk   k (10) C k    is invertible, then x s [k ], us [k ] will be obtained by solving: Bk    (11) the original tracking control problem of LTI system (7) is converted correspondingly in the stabilization of the nominal system:  (12) z  Ak z  Bk v which is obtained by subtracting (9) from (7) For optimal stabilizing nominal system (12) the following cost-function of infinite horizon is used:    J k   zTQk z  vT Rk v dt  tk where Qk , Rk are any symmetric positive definite matrices Thence, based on the variation technique the optimal input v * is obtained as follows [9]:   v *  Rk 1Bk Lz  Rk 1Bk L x  x s [k ]   u *  v *  us [k ]  Rk 1Bk L x s [k ]  x   u s [k ] (13) with the symmetric positive definite matrix L , which is achieved by solving the algebraic Riccati equation:  T T LBk Rk 1Bk L  Ak L  LAk  Qk (14) * Finally, the obtained optimal vector u is now sent to the system (6) as control signals If the time instant, at which u * is sent to the controlled object, is denoted with tk 1  tk   , then  is essentially the time interval, which is needed to carry out all above calculating operations from (8) to (14) as depicted in VCCA 2015 Fig.2 Hence it is, in principle, very small and the approximation (7) and (8) is absolutely acceptable Of course the vector of optimal control signals u * , which is just sent to (6), is also valid only during the next time interval [tk 1, tk 2 ) until all calculating steps (8)-(14) above are completely executed again to achieve the next optimal control signal for the next time instant tk 2  tk 1   Fig.2 below depicts this repeatedly working performance of the proposed optimal receding horizon controller for bilinear system (6), where all calculating operations (8)-(14) are repeatedly executed with the time  tk 1 tk tk   Getting x (tk ) t  u* Calculation unit (8)-(14) 1 0 0 1     Fk   y  y   d  d Now, with the symbols: z  x  x s [k ] and v  u  us [k ]  x s [k ]   Ak    u s [k ]  C k DOI: 10.15625/vap.2015.0001 Fig.2: Repeatedly working performance of the proposed controller 2.4 Control algorithm for the quadrotor It is now ready to implement the proposed optimal receding horizon controller for tracking control the quadrotor In the following implementation, both correlative bilinear models BL1 and BL2 of the quadrotor are useable Set u v  Choose any two symmetric positive definite matrices Q, R Measure the current state vector x from system and then determine the matrices A, B , C as follows: A  A(x , u v ), B  B (x , u v ), C  C (x , u v ) if a) the model BL1 is used b) A  A(x ), B  B(x ), C  C (x ) if the model BL2 is used Determine the matrix  A B F  C   If F is singular, then go back to the step Otherwise go to the next step Calculate x s , u s from:  0  xs  1 u   F y   s  d and L  LT  from: LBR1BT L  AT L  LA  Q Determine Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa VCCA 2015 u v  R 1BL x s  x   u s DOI: 10.15625/vap.2015.0001 Conclusions and send it to the quadrotor as the optimal control input vector Then go back to the step Simulation results The following simulation is carried out with the correlative bilinear model BL1 and: k  2.98 106 , l  0.3, d  1.14 107 , m  0.05kg g  9.81m s , I x  I y  0.1, I z  0.02 Q  I1212 , R  I 44 for all k where I1212 and I 44 are all identity matrices Obtained simulation results of the quadrotor hover with four system outputs: y (t )  x (t ), y (t ), z (t ),  (t )  The paper has studied tracking control of a quadrotor via infinite horizon receding control techniques The proposed control approach is then verified by simulating the hover of a quadrotor with Matlab The obtained simulation results confirm the good tracking performance of closed loop system as expecting Moreover, a problem to further study arising from this paper is that how to choose Qk , Rk , k  1, 2,  efficiently Since the proposed controller has freely chosen matrices Qk , Rk , then it seems that for the scenario of constrained control problems u U with a certain subset U of the control space, they could be so selected appropriately that the required constraint will be satisfied T and their references respectively are exhibited in Fig.3 They show that the tracking behavior of closed loop systems, beside a little interconnection effect, could be approved, even if reference signals alter quickly References [1] Bangular, M and Mahony, R.: Nonlinear dynamic modeling for high performance control of a quadrotor Proceedings of Australasian on Robotics and Automation, Wellington New Zealand, 3-5 Dec 2012 [2] Bouabdallah, S.: Design and Control of Quadrotors with Application to autonomous Flying Ph.D Dissertation University Aboubekr Belkaid, Algerie, 2007 [3] Bouffard, P.: Onboard Model Predictive Control of a Quadrotor Helicopter: Design, Implementation and Experiments Technical Report No UCB/EECS-2012-241 Berkeley, 2012 [4] Luukkonen, T.: Modeling and Control of quadrotor Independent research project in applied mathematics Espoo, 2011 [5] Martinez, V.C.: Modeling of the Flight Dynamics of Quadrotor Helicopter Master Thesis Cranfield University, 2007 [6] Bresciani, T.: Modeling, Identification and Control of a Quadrotor Helicopter Master Thesis Lund University, Sweden, 2008 [7] Phuoc, N.D and Ha, L.T.T: Constrained Output Tracking Control for Time-Varying Bilinear Systems via RHC with Infinite Prediction Horizon Journal of Computer Science and Cybernetics, Vol.31, No.2, pp 97-106, 2015 [8] Trường, B.Đ Thắng, L.V.: Điều khiển hệ quadrotor điều khiển dự báo Đồ án tốt nghiệp, Đại học Bách khoa Hà Nội, 2015 [9] Phước, N.D.: Lý thuyết điều khiển nâng cao In lần thứ 4, NXB Khoa học Kỹ thuật, 2009 Fig.3: Four system outputs in comparison with their references VCCA 2015