Optimal Control for the Target-Tracking Problem using Three-Axis Camera Gimbals

Journal of Science & Technology 127 (2018) 035-039 35 Optimal Control for the Target-Tracking Problem using Three-Axis Camera Gimbals Do Dang Khoa*, Le Quang Duong Hanoi University of Science and Technology, No. 1, Dai Co Viet, Hai Ba Trung, Hanoi, Viet Nam Received: November 12, 2017; Accepted: June 25, 2018 Abstract In this paper, the target-tracking problem of a 3-axis camera gimbal mounted on a flying vehicle is considered. In order to keep the camera’s line of sight continuously pointing to a moving target, an optimal controller using LQR control techniques is applied. The motion equations of the gimbal system are derived by the Lagrangian approach considering the vehicle motion. The LQR controller is designed based on the system’s continuously linearized model. A tuning method for the LQR is also proposed to make the gimbal system point to a moving target in the shortest time. The feasibility of the proposed controller is shown by numerical simulations. Keywords: Optimal Control, LQR, Camera Gimbal, Line of Sight (LOS) 1. Introduction* Inertial stabilized platforms (ISPs) are mechanisms to control and stabilize the LOS of optical equipment. Recently, ISPs have been popularized in many civil and commercial applications (e.g. movies shootings, aerial photography). In such systems, the optical equipment, which is often mounted on a moving vehicle, must keep its optical sensor’s LOS pointing to a fixed or moving target. One of the most common types of ISPs is based on a gimballed structure [1]. The two main issues are raised as to build exact physical models and to develop good control algorithm to fulfill the target-tracking problems. Basically, there are two approaches to derive the gimbal mathematical models: one by Newton-Euler approach [2, 3] and the other by Lagrangian method [4, 5]. For gimbal control algorithms, many approaches have been applied such as robust control in [3], sliding mode control in [4], and conventional PID control in [5]. Most of the gimbal control challenges in the literature are related to dealing with two-axis gimballed configurations. In this paper, the LOS stabilization and target- tracking problems of a three-axis camera gimbal mounted on a flying platform is studied. The aim of the paper is to design an optimal controller to achieve good target-tracking performance as quick as possible under the dynamic disturbances from the flying platform. To fulfill this task, a nonlinear dynamic model of the three-axis gimbal is developed based on * Corresponding author: Tel.: (+84) 982.326.550 Email: khoa.dodang@hust.edu.vn the Lagrangian approach under the flying platform’s inertial effects and a linear quadratic regulator (LQR) is utilized. An offline-tuning procedure for LQR is proposed to find optimal values of state and control weight matrices to improve gimbal target-tracking performance. 2. Problem Formulation In this paper, a three-axis gimbal system illustrated in Fig.1 is considered. The gimbal system is assumed to be mounted on a flying platform at body 0. The camera fixed on the gimbal’s body 3 must keep its sensor’s LOS pointing to a moving object on the ground. To keep the object image stabilized in the camera frame of view, its sensor’s LOS must also be kept nonrotating in an inertial space under dynamic disturbances from the platform motion. Fig. 1. Model of 3-axis Gimbal In order to verify the proposed control algorithm, a mathematical model of the gimbal system needs to be derived. The gimbal system’s equations of motion are built based on three generalized coordinates as 1 , Journal of Science & Technology 127 (2018) 035-039 36 2 and 3 , which are the rotation angles (yaw, roll and pitch) of motors at each axis. To determine the gimbal system’s position, five reference frames are identified as in Fig. 1. The global frame OXYZ(g) is fixed to the ground. Local frames i i i iO x y z are attached to body i (i from 0 to 3) and 3 3O x is specified as the camera’s LOS. Those frames are choosen such that they are parallel to each other when 1 , 2 and 3 are all equal to zero. The camera LOS is determined by the transformation matrix method. Let’s define the transformation from frame a to from b by a 4 by 4 matrix a bT in the form as 1 a a a b b b T   =     R r T 0 (1) where a bR is a 3 by 3 rotation matrix and, a br is a 3 by 1 translation vector from frame a to frame b. The transformation matrix between the ground frame and the platform frame is specified as follows 0 0 0 0 0 0 0 1 g c c s c s c s s s c s c c c c c s s s c s s c s s c c c Y Zs X                             −     =      +  + − − T (2) where X0, Y0 and Z0 are the flying platform position of O0 in the ground frame; ,   , and  are roll, pitch and yaw angles of the flying platform (body 0). The terms , cs  stand for ( ) ( )sin , cos  and so on for , c , s  and , cs  . Other transformation matrices among the gimbal bodies are described as 1 1 1 1 1 1 1 10 1 1 0 0 0 0 1 0 0 0 1 c s l c s c l s h − −    −  =       T (3) 1 2 2 2 21 2 2 2 2 2 1 0 0 0 0 s 0 0 0 1 l c s b c s c b     − −  =  −     T (4) 3 3 3 3 3 3 32 3 3 3 3 3 3 3 0 s 0 1 0 0 c 0 0 0 1 c s l c h b s l s h c +     =  − − +     T (5) The terms 1 1, cs stand for ( ) ( )1 1sin , cos  and so on for 2 2, c , s and 3 3, cs . The direction of the camera LOS is calculated by the transformation matrix 3 g T as follows 0 1 2 3 33 0 1 2 3. . . 1 g g g g T   = =     R r T T T T T 0 (6) The LOS direction is specified by making the unit vector 3 3i of axis O3x3 same direction with vector of 3 3 O P . To keep the axis O3y3 in parallel to the ground, the term 3 (3, 2) g R , which is at the third row and second column of matrix 3 g R must be zero. Let’s assume the moving target’s position P in the ground frame is identified by the vector g Pr . As a result, the gimbal configuration ( )1 2 3, ,   to keep its LOS point to the moving target P while maintaining the stabilized image of P in the camera view of frame is determined by the following system of equations ( ) ( ) 3 3 3 3 3 3 3 3 3 (3,2) 0 . g T g g P g g T g g P −  = = −  T r r i 0 R T r r i 0 (7) The equations of gimbal motion in the frame 0, which are derived by the Lagrangian approach using the matrix method [8] has the form as *( ) ( , ) ( )+ + + =M q q C q q q Dq G q Q (8) where  1 2 3 T   =q , ( )M q is the 3 by 3 mass matrix, ( , )C q q is the 3 by 3 Coriolis and centrifugal matrix determined from the mass matrix, ( )G q is generalized forces due to the potential energy  , D is a damping matrix and Q* is the generalized forces due to motor torques and inertial forces and moments caused by the flying platform. The mass matrix ( )M q is calculated as follows ( ) ( ) 3 0 1 T T Ti i Ti Ri Ci Ri i m = = +M q J J J I J (9) where im is mass of body i and 0 CiI is inertia tensor around the centroid of body i in the frame 0. TiJ and RiJ are translational and rotational Jacobian matrices respectively. 0 0 , Ci iTi Ri   = =   r ω J J q q (10) where 0 Cir is a position vector of the centroid Ci of body i in frame 0, 0 iω is the angular velocity vector of body i in frame 0. The matrix ( , )C q q is derived as Journal of Science & Technology 127 (2018) 035-039 37 ( ) ( ) ( ) 1 ( ) ( , ) 2 T    =  −      M q M q C q q E q q E q q (11) where E is the 3 by 3 identity matrix and  is the Kronecker product [8]. The damping matrix D is determined from the dissipative function ( ) q as ( ) =  q Dq q (12) where 2 2 21 1 2 2 3 3 1 1 1 ( ) 2 2 2 b b b   = + +q and 1b , 2b , and 3b are damping coefficients of the gimbal motors. The vector ( )G q has the form as ( ) 3 0 0 1 , ( ) T i g Ci i m =   =  = −    G R g r q (13) where g is the vector of the form  0 0 T g=g , and g is the gravitational acceleration. The vector of generalized forces Q* is calculated as * * * * Fie Mie= + +Q Q Q Q (14) where * Q , * FieQ , and * MieQ are generalized forces corresponding to the gimbal motor torques, the resultants of inertial forces and inertial couples, respectively. The Coriolis effect is ignored due to assumptions of the platform’s small angular velocity. The generalized forces are defined as  * 1 2 3 T T    = =Q u (15) 3 * 0 1 T g T gFie Ti ci i= =Q J R F (16) 3 * 0 1 T g T gMie Ri ci i= =Q J R M (17) where g ciF and g ciM are the resultant of inertial force and couple at the centroid Ci of body i in the ground frame, respectively. ( )( )00 0 0 0 0 0 0 0 0 0 0 0 0 g g g g gci i ci g g g T g g g g T g ci Ci Ci m= − + + = − − F r α ω ω r M R I R α ω R I R ω (18) where 0 gω and is 0 g α are skew-symmetric tensors of angular velocity 0 gω and angular acceleration 0 g α of body 0 in the ground frame, respectively. Both 0 gω and 0 g α are assumingly known by sensor measurement. In the following section, the control torques in (15) need to be specified to force the equations (8) realize the conditions in (7). 3. Optimal Controller Design Generally, the gimbal nonlinear equations of motion (8) can also be converted into the form as ( ) 21 1 * 2 ie −    = =    − − − − +     xx x x M Cq Dq G Q u (19) where  1 1 2 3 T q q q=x ,  2 1 2 3 T q q q=x and, * * * ie Fie Mie= +Q Q Q . The measurable and controlled variables are 1( )t =y x (20) From (19) and (20), the gimbal system’s nonlinear model can be expressed as follows 0 0 0 0 0 0 ( ) ( ( ), ( ), ( ), ( ), ( )) ( ) ( ( ), ( ), ( ), ( ), ( )) g g g g g g t f t t t t t t h t t t t t  =  = x x u r ω α y x u r ω α (21) Where 0 0 0( ), ( ), ( ) g g gt t tr ω α are the platform’s acceleration, angular velocity and acceleration in the ground frame, respectively. To determine the motor torques u(t) for making the gimbal system’s LOS track a moving target, an optimal controller of LQR will be designed based on the continuously linearized model of the form ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) t t t t t t t t t t = + = + x A x B u y C x D u (22) Where ( ) ( ) ot t= −x x x , ( ) ( ) ( , )o ot t= −y y h x u , and ( ) ( ) ot t= −u u u . The matrices A(t), B(t), and C(t) D(t) are defined as follows , , ( , ) ( , ) , o o o o   = =  x u x u f x u f x u A B x u (23) , , ( , ) ( , ) , o o o o   = =  x u x u h x u h x u C D x u (24) where the operation point ( , )o ox u is determined from equation (7) at operating time to, its first derivative and the steady state equations as. 0 0 0( , , ( ), ( ), ( )) 0 g g g o o o o of t t t =x u r ω α (25) The system’s controllability and observability are satisfied. To apply LQR controller, the control signals u should have the form as [9] ( ) ( )t t= −u Kx (26) to minimize the cost function of the form as ( ) 0 1 2 T TJ dt  = + x Qx u Ru (27) Journal of Science & Technology 127 (2018) 035-039 38 Where Q and R are symmetric positive semi-definite and positive definite matrices, respectively. The optimal solution u is identified from the Hamiltonian approach as follows 1( ) ( )t t−= −u R BPx (28) Where P is the solution of the Riccatti equation as 1T T−+ + − =Q A P PA PBR B P 0 (29) As seen in (28), the LQR provides a negative feedback gain K with large stability margin [9]. The controller performance depends on the selection of the weight matrices Q and R. In this section, a practical method to select the weight matrices is introduced. Matrices Q and R are selected in the form as , T = =Q C C R I (30) where  is a tuning parameter to design the LQR such that the control signal u will drive the gimbal system point to the moving target in the shortest time. Let’s define ( )st  is the time period for the maximum norm of the state perturbations in (22) getting smaller than the predefined error 0.01 ( )rad = ( )st  x (31) The parameter * to make the gimbal system catch the moving target in the optimal time is the solution of the function * *( ) min(max( ( )))s st t     → = (32) 4. Gimbal System Simulation The 3D model of gimbal (Fig.1) was built using based on a real prototype. The gimbal parameters are measured and shown in Tables 1, 2 and 3 as follows: Table 1. Dimensions and Mass of the Gimbal Link l(m) b(m) h(m) ( )im kg 1 0.13 0 0.155 0.32341 2 0.125 0.072 0 0.32325 3 0.0325 0.049 0.01405 0.67008 Table 2. The Centroids of the Gimbal Links Link i Cix i Ciy i Ciz 1 0.01325 0 -0.07642 2 -0.05791 0.05261 0 3 -0.03237 0.02294 0.00578 Table 3. Moment of Inertia about the Centroids Link Ci i (xx) I Ci i (yy) I Ci i (zz) I 1 0.001396709 0.002011077 0.000675883 2 0.001289047 0.000817588 0.002076275 3 0.001682153 0.000614997 0.001274903 The tuning process from solving equation (32) is shown in Fig. 2, with * *0.005, ( ) 0.035( )st s = = . Fig. 2. Weight Parameter Tuning Process The flying platform’s position of O0 and roll, pitch and yaw angles are assumingly known as (Fig. 3) 0 0 0 sin(1.4 ) 60 sin(1.4 ) ; 0( ) 10( ) 6 sin( ) 4 sin(1.4 ) 6 t X Y t t s t s Z t t            = =    = =     = +  =  The moving target’s position P is defined as  0.2sin(1.2 ) 1 0 Tg P t t= +r Fig. 3. Trajectories of the drone and moving target Fig. 4. Gimbal Torques for Tracking Problem Journal of Science & Technology 127 (2018) 035-039 39 Fig. 5. Tracking Responses of the Gimbal Angles The pertubation results of motor torques and gimbal angles between two cases 1 = and * = are compared in Fig. 4 and 5. The optimal case tracks the object in much faster time with the trade off of higher motor torques. 5. Conclusion In the paper, the problem of controlling the gimbal camera’s LOS for tracking a moving target is studied. A dynamic model of a 3-axis gimbal system is built in consideration with the flying platform’s motion. A tuning algorithm for the LQR controller to find shortest tracking time is proposed and the numerical simulation shows that the designed controller meets the objective. Acknowledgments This work was supported by Hanoi university of Science and Technology under the research project T2016-PC-057. References [1] J.M. Hilkert, Inertially stabilized platform technology Concepts and principles, in IEEE Control Systems (2008), vol. 28, no. 1, 26-46. [2] B. Ekstrand, Equations of motion for a two-axes gimbal system, IEEE Transactions on Aerospace and Electronic Systems (2001), vol. 37, no. 3, 1083-1091. [3] S. B. Kim, S. H. Kim and Y. K. Kwak, Robust control for a two-axis gimbaled sensor system with multivariable feedback systems, in IET Control Theory & Applications (2010), vol. 4, no. 4, 539-551. [4] H. Özgür, E. Aydan, E. İsmet, Proxy-based sliding mode stabilization of a two-axis gimbaled platform, Proceedings of the World Congress on Engineering and Computer Science (2011), Vol 1. [5] M. Abdo, A. Toloei, A. R. Vali, & M. R. Arvan, Cascade control system for two axes gimbal system with mass unbalance. International Journal of Scientific & Engineering Research (2013), 4(9), 903- 913. [6] A. Cabarbaye, J. C. Escobar, R. Lozano, M. B. Estrada, Fast adaptive control of a 3-DOF inertial stabilised platforms based on quaternions, International Conference on Unmanned Aircraft Systems (ICUAS) (2017), 1463-1469. [7] A. Altan, R. Hacioğlu, Modeling of three-axis gimbal system on unmanned air vehicle (UAV) under external disturbances, 25th Signal Processing and Communications Applications Conference (SIU) (2017), 1-4. [8] Nguyen Van Khang, Dynamics of Multibody Systems (in Vietnamese), Science and Technology Publishing House, 2nd edition, 2017. [9] F.L. Lewis, D. Vrabie, V.L. Syrmos, Optimal Control, John Wiley & Sons, 3rd edition, 2012.