Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 284 CONTROL OF FLEXIBLE ROBOT USING VISION SENSOR MEASUREMENTS Mohammad Najeh Nemah AL-Furat AL-Awsat Technical University, Engineering Technical College-Najaf \ Iraq mohammed.najeh85@yahoo.com Received 8 March 2015 Accepted 5 July 2015 ABSTACT The control of lightweight flexible robot using delayed measurements (i.e. vision sensor measurements) and moving along predefined paths is the focus of this work. The flexible robot dynamics is derived on the basis of a Lagrangian-assumed modes method. Noised and delayed tip deflection vision measurements are used beside the base tracking position for state variables estimation process. In order to generate the required control inputs a special state estimation approach is proposed to overcome noise and time delay and noise problems in the measurements. Two state estimators are suggested for each of the measurements, and the states resulted from these two estimators are combined in order for the end effector to follow the desired response. The one link flexible arm prototype dynamic model is chosen for developing a case study. Extensive simulation results are illustrated and discussed. Keywords: Flexible robot, state estimation, vision measurements. قياسات حساس الرؤيا السيطرة على روبوت مرن خفيف الوزن باستخدام محمد ناجح نعمة نجف / العراقال -جامعة الفرات االوسط التقنية/ الكلية التقنية الهندسية الخالصة مسار محدد مسبقا يا( و حركته حول ؤالسيطرة على روبوت مرن خفيف الوزن باستخدام قياسات التأخير )مثل قياسات حساس الر . (Lagrangian-assumed modesهو محور هذا العمل. معادالت حركة الروبوت المرن اشتقت باالعتماد على طريقة ) للتشوه في الطرف العلوي للروبوت استخدمت بجانب اساس عمله في تعقب الموقع و تأخير الوقت في قياسات الرؤيا الضجيج وقت ،الضجيج للتغلب على حالة خاص تقدير اسلوبلغرض خلق مدخالت السيطرة المطلوبة اقترح لعملية تقدير متغيرات الحالة. متغيران لكل نوع من القياسات، و المتغيرات الناتجة من هذا المقترح جمعت تم اقتراح و مشاكل الضجيج في القياسات. التأخير دراسة حالة. وبعدها وضحت و لتطويرذراع روبوت مرن واحدة فقط اختيرت كموديل ديناميكي .االستجابة المطلوبة لمتابعة نوقشت نتائج المحاكاة الواسعة النطاق. .حساس الرؤيا، حالة تقدير اسلوب، روبوت مرن الكلمات المفتاحية: mailto:mohammed.najeh85@yahoo.com Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 285 INTRODUCTION The design of mechanical arms results in a high weight to payload ratio. A successful method to achieve advantages such as lower cost, higher speeds, better energy efficiency and improved mobility; can be fulfilled by using of lightweight robots. In this case, the control is one of the critical points to an effective use of flexible robot arms. As a matter of fact in flexible robots more complex dynamics involved by the flexibility distributed along a lightweight flexible links. In order to achieve a successful control synthesis; an accurate dynamic model for the flexible manipulator is required. The recursive Lagrangian assumed modes method proposed by (Jean, 1998) [1] is an efficient and complete modeling technique. The result of this method is a number of generalized coordinates, and then state variables will be used for control purposes. The control objective is not just to drive the manipulator to a specific point and stabilize the vibrations. The controller should concern about the specified path to follow. For a single link flexible arm, an optimal control approach is candidate to succeed (Luca, 2003)[2]. The non- minimum phase property of flexible robot have not addressed in pseudo link concept associated with the tip output which is introduced in (Luca, 2003)[2]. In order to deal with the exact tracking problem many researchers have been proposed the noncausal controllers for the purpose of trajectory tracking. The goal of this paper is to present an approach to the control of flexible manipulators moving along predefined paths, based on state feedback theory using visual measurements. A similar approach has been proposed by several researchers. The early experimental work in this area (Rush et. al, 2002)[3], among others, aim at the end-point regulation problem based on the vision measurements. (Victor et. al, 2000)[4] studied the input-state feedback linearization problem and showed that the system is not in general linearizable, however it is input output linearizable. To this end, the tip positions cannot be selected as the visual outputs due to the instability of the unobservable dynamics associated with such choice of outputs. (Mallikarjunaiah et. al, 2013)[5] focused on the end-point control of a single flexible link which rotates in the horizontal plane by keeping the rotate angle of the flexible link at desired position and eliminate the oscillation angle of end effectors, the dynamic model is derived using a Lagrangian assumed modes method based on Euler–Bernoulli beam theory. (Rasheedat et. al, 2012)[6] developed a simple and efficient adaptive control scheme to automatically tune PD control gains for two-link flexible manipulator, the manipulator is modeled using Lagrange and assume mode method. The adaptive algorithm is developed for hybrid PD-PID controller in which the PD controller is for rigid body motion control and the PID is for end-point vibration suppression. The model of Linear Quadratic Controller Design Technique was controlled by using state space approach and the performance of single link manipulator in dynamic non- linear torque condition was studied in (Gamasu, 2014)[7]. A sensor fusion method for state estimation of a flexible industrial robot is developed in (Axelssona et. al, 2012)[8], By measuring the acceleration at the end- effector, the accuracy of the arm angular position, as well as the estimated position of the end- effector was also improved, the Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 286 extended Kalman filter and the particle filter are proposed as a solutions to the Bayesian problem estimation formulated. It must be mentioned for control purposes that the full state availability is assumed. In fact the flexible link variables can be measured using strain gauge, but the variables derivatives’ cannot be measured but derived through a reconstructing another dynamic system as in (Somolinos et. al, 2002)[9]. In this contribution it is assumed that one of the output signals of the system is suffered from noise and time delay measurement (e.g. camera measurements). In order to estimate the states based on two different types of measurements, two state estimators are designed. The first one is Leunberger observer to estimate states using base tracking motor signal and the second is a Kalman filter for the tip deflection noised and delayed signal. The paper is organized as follows: the mathematical model derivation will be presented, in the next section. The state estimation process will be detailed later. After that the control approach is presented. Then the case study parameters and simulations will be discussed in details. Finally, some conclusion regarding this work addressed in the last section. MATHEMATICAL MODELING OF FLEXIBLE ROBOT The dynamic model of flexible link with a tip mass robot shown in Figure (1) is derived in this section, where the motion of the robot is assumed as a rotation on a horizontal plane and the deflection occurs due to the movement of the link. Nonlinear equations of motion for a flexible manipulator can be successfully derived using the recursive Lagrangian approach outlined by (Luca, 2003)[2]. This can be done by computing the kinetic energy, and the potential energy. The total kinetic energy and total potential energy of the system can be found by adding up those of various components of the system as:                       P l h KE T p KE l T KE h T ht rrmdxxpxpJrrmKE 22 0 2 111 )()( 2 1  (1) Y X Figure (1) : kinematics descriptions for a flexible one-link. manipulator. O Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 287            l t dx dx xWd EIPE 0 2 2 2 )( 2 1 (2) Where lPlh PEandKEKEKE ,,, represent the kinetic energy of hub, link, payload, and elastic energy of link respectively, and hhp JandrrWmmIE ,,,,,,,, 21  represent mass per unit length of link, modulus of elasticity of material, moment of inertia about z-axis, payload mass, hub mass, deflection of the link, components of kinematics vectors and hub inertia respectively. Having the kinetic and potential energies of a typical element of an arbitrary link mass and stiffness matrices and load vector of the flexible link can be found by applying Lagrange's equations. (3) (4) N is the number of links, and ei n is the number of flexible modes included. For a flexible one link manipulator the dynamic equation of motion is formulated by using Eqs. (3 and 4) as: (5)      f K qqFqqM i         0 ,  (6) where     fqqFKqM ,,,,  are the mass, stiffness matrix, coriolus and centrifugal vector, and load vector respectively. q is the rigid and flexible displacements variables, and     n mmmm 11211 ,  . The components of mass matrix  qM , stiffness matrix K , and load vector in Eq. 6 are functions of elastic deformations, elastic velocities, and nonlinear terms including rigid body degrees of freedom and their time derivatives. Therefore, the dynamic equations of motion of one-link flexible manipulators are nonlinear. Two modes of vibration are used in this work (i.e. 2n ) to describe the def1ection of link; so in this work T q ][ 21  . The summation of 21 and,  with each of Nif LL t i ii ,,1                   Ninj LL t ei ijij ,,1,,10                                                                         00 00 ,,, ,,, ,, ,,              i TT KF F Mm mm     Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 288 coincides mode shape 21 and,  represents the value of W at each time instant. The dynamic model of the one link flexible arm with reference to the Lagrangian dynamic equations is presented. DYNAMIC STATE VARIABLES ESTIMATION Flexible Robot Linear Dynamic Model. The nonlinear flexible-link system is not in general input-state feedback Linearizable, however the system is locally input-output linearizable. For the sake of observer and controller design; the dynamic model is linearized about the operating point of the robot system. Hence for designing state space model state variables based on Eq.(6) assumed as, T ][ 2121   , and the dynamic state space model can be reformulated as: ),( )()( uh ugf     (7) Taylor series expansion is used to linearize the state space dynamic model according to these conditions for an autonomous system )(so,0  fu  . The model is linearized about the operating point which is found out by solving the following equations 0)(0   f . After some mathematical simplifications for Eq.(7), the linear state space model becomes (Tewari, 2002)[10] uDC uBA     (8) Using the Eq.(5), the linear state-space matrices are found out to be,         0 0 1 KM I A ,           QM B T 1 000 ,  TQ 001 , 0D The base motor angle and tip deflection of the link are taken as output,               0000 000001 212 1 lxlx C C C  In this paper, it is assumed that the first output is free of noise and time delay. On the contrary, the second output is disturbed by noise and time delay (camera measuremnts), based on these assumptions the output equation can be rewritten as:                          )(0000 000001 )( 212 1 tt lxlx , here  , represent the time delay and the noise in the measurements respectively. Observer Design. For this study, it is suggested that the flexible robot system is observed with two types of sensor: motor sensor the base position, camera sensor to observe the tip position of the robot. The vision Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 289 data, which provides direct measurements of the deflection with respect to inertial coordinate, is proved to be a good substitute for strain gauges. Related to the Eq.(8), two different measurements are used. Practically there will be time delay and noise in the all types of measurements; here the delay and the noise for the motor signal are neglected in comparison with the camera signal. Due to the limitations of the sampling rate, and the resolution of the camera to detect all modes of the system; two different observers are designed to estimate the dynamical behavior of the system. The first one is a standard one, while the second one is used not only to estimate the states, but also for time delay compensation. This contributes significantly to the beam tip deflection estimation using the vision sensor. For the non-delayed measurement, the states are estimated using classical observer approach (Heijden et. al, 2004)[11] as   111111 ˆˆ  KuBCKA  (9) with TT RCPK 1111  and 1 P as the solution of the Riccati equation described as 0 111 1 11111   PCRCPAPPA TT (10) Here 1  and 1 R are positive definite weighting matrices for the non-delayed states and measurements respectively. Kalman Filter Design. The advantage of using the camera as a tip sensing device is the direct inertial measurement. The disadvantage is a delayed and noisy measurement signal. The delay is due to the time used in the vision processing and video signal transmission. In this section the method of defining states using an augmented predictor for the delay and noise compensation is described. Here, an augmented Kalman filter is proposed for the delayed estimation. According to (Heijden et. al, 2004) [11], and assuming that the estimated states are delayed by  . The Kalman filter equation can be written as   222222 ˆˆ  KuBCKA  (11) where TT RCPK 2222  and 2 P is the solution of the Riccati equation described as 0 222 1 22222   PCRCPAPPA TT (12) Here 2  and 2 R are positive definite covariance matrices for the noised-delayed measurement. To remove the delay effect from the estimated states, a function g is defined as Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 290 uBgAg  (13) and the non-delayed state estimate can now be found (Roberts, 1986)[12] as )]()(ˆ[ˆ 22    tgteg A (14) However, in practical implementation the states of the system can be estimated from different measurements by combining all of the corresponding state variable estimates. The states from the second estimator are combined with the states of the state observer, which are estimated using the full observer using the minimum mean-squared error i ̂ with ni pq pq iiii iiii i 1 ˆˆ ˆ ,, 2,1,       . (15) Here n denotes the number of modes. An optimal estimation can be achieved, when they are combined properly. The derivation process of ii q , and ii p , are explained in detail in (Roberts, 1986)[12]. Note, that the subscripts '1' and '2' in the states denote cases that the states are estimated based on measurements '1' and '2', respectively. The schematic diagram for the estimation approach is shown in Figure (3). CONTROLLER DESIGN For a single flexible link the mass matrix is only a function of deflection variables, which are quadratic type nonlinearities. Thus a single link may well approximate a linear system, while this is not true in the multi-link case. In spite of this fact, the simulations conducted with a single link case provide a basis for multi-link investigations, since both cases suffer from the undesirable non- minimum phase property. This property shows up when the controlled output is the end-effector position. The less difficult problem of end-point stabilization may also become troublesome, although not impossible, because of the non-minimum phase nature. the controller design for single link flexible manipulator describe in the Figure (2). There are two output feedback signals and return to the observer and kalman filter respectively. After the observer processing the input signal generate the first combiner input signal ̂ , while the kalman filter output signal is the second combiner input signal ̂ . �̂� 𝒕 �̂� 𝒕 𝝉 𝒚𝟏 𝒕 𝒚𝟐 𝒕 𝝉 �̂� 𝒖 Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 291 Figure ( 2): controller design for single link flexible manipulator. The linearized model Eq. (8) is used in this work to design the state feedback controller for the flexible robot system. The control input based on the linear quadratic regulator can be written as: ̂Gu  . (16) Here c T c PBRG 1  is designed (Tewari, 2002)[10] to minimize the quadratic cost function:     0 uRuQJ c T c T  . (17) where cc RQ , are positive semi-definite matrices and cP is the solution of the associated Riccati equation 0 1   ccc TT ccc T QPBRBPAPPA (18) SIMULATION AND RESULTS The mathematical dynamical model of the lightweight flexible robot as well as the controllers have been developed in Matlab \ Simulink for simulation the structure shown in Figure (2). The desired output trajectory used this work is higher order polynomials are sometimes used for path segments, this polynomial is quintic polynomial:   5 5 4 4 3 3 2 210 tatatatataat  , the solution is provided from robotics toolbox (Corke)[13]. The initial conditions of the trajectory are chosen equal to zero in the start of the motion. The initial condition of the states are as follows  T000001.005.000  . It is assumed that the link are deformed before the start of the robot motion in order to ensure the capability of the estimation process in detecting the change in the states even if the estimator has no information about the initial conditions of the system. The flexible link physical parameters are as follows: 1 m length, 1x10 -4 m 2 cross sectional area, the first and second natural frequencies are: 2.12, 14.3 Hz, EI=54.6 N.m 2 , ρ=7850 Kg/m 3 . Two set of results are presented in this section, first include the simulation when the time of the robot motion to reach the desired angle is 2 sec, the second set when the time is 3 sec. The set of the estimators’ gains resulting from the design process are:  3463 1 10644.101059.941035.15445.47581.40501025.14 K  013.81012810799003.2108740.1257 646 2   K . The state feedback gains according to procedure addressed earlier are  2076.06108.04597.00578.11208.11G . The desired motion of the link is addressed in Figure (3); start from zero and tend to 90 degree in 2 and 3 seconds respectively. In order to compare the behavior of the designed system, a PD controller is also designed and simulated for the single flexible link robot. Due to the nature of the flexible robot dynamics (i.e. a minimum phase system), the uncontrolled motion results are not presented. The time delayed and the noised signals can be seen in Figure (4). This signal and the link rotation angle output signal are used for estimation process. The delay in the tip deflection output signal is variable value, in practical Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 292 applications the delay can be assumed as constant. The error in the estimation process due to the unknown initial conditions are shown in Figures (5), for the motion time 2 sec. It can be easily noted that the estimators can construct the states of the system based on the output signals very quickly. The tip displacement and the input torque for proposed state feedback and PD controlled motion are shown in Figures (6-9). In Figure(6) the motion time is 2 sec; the tip deflection suppressed effectively after 1 sec. The same situation for the tip deflection in Figure(8), although it is smaller but it required the same time for removing of the vibration from the tip. For the torque inputs and due to the change in the time of operation the required torque in 2 sec case is larger than the required torque for the 3 sec case Figures (7 and 9). Consequently the tip deflection and the input torque using the proposed state feedback control gave better performance in comparison with PD controller. CONCLUSIONS In this work the estimation of the state variables of single flexible link robot model for control based on different types of measurements was studied. The dynamic model of flexible robot system is designed using assumed mode method. In order to deal with the different properties of those measurements, two state estimators are designed. The Leunberger observer for the base angle of rotation measurements while the Kalman filter is used for tip displacement noisy and delayed measurements, the two estimators work well. The states from the noisy delayed measurement are estimated with a good accuracy. The control of single link flexible robot based on the estimated states are simulated, and the results show good performance for the controlled behavior. The proposed estimation process will be implemented later experimentally for more complicated flexible systems. REFERENCES [1] P. Jean-Claude, Recursive modeling of serial flexible manipulators, Journal of Astronautcal sciences, 46-1(1998) 1-24, 1998. [2] A. De Luca, Robots with Flexible Links: Modeling and Control, outlined, Roma University, Department of informatics and systematic (DIS), Roma, 2003, online avialable: www.prisma.unina.it/cira03/TA_DeLuca2.pdf, 2003. [3] D. Rush, D. Robinett Clark, E. Richard, F. John, G. Gordon, G. David, and S. Dennis, Flexible Robot Dynamics and Control, IFAC, International Series on systems and Engineering, 19, 2002. [4] O. Victor and R. Gamarra, A planar flexible Robotic manipulator, Division of Mechanical Engineering, College of Engineering UNESP, Guaratingueta, Brazil, Cybernetic, 29, 5/6 (2000)787-796, 2000. [5] S. Mallikarjunaiah and S. Narayana Reddy, Design of PID Controller for Flexible Link Manipulator, International Journal of Engineering Research and Applications (IJERA) ISSN: 2248- 9622 www.ijera.com Vol. 3, 2013. http://www.prisma.unina.it/cira03/TA_DeLuca2.pdf Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 293 [6] Rasheedat and M. Mahamood, Direct Adaptive Hybrid PD-PID Controller for Two-Link Flexible Robotic Manipulator, Proceedings of the World Congress on Engineering and Computer Science 2012 Vol II WCECS 2012, October 24-26, 2012, San Francisco, USA, 2012. [7] R. Gamasu, Pre-Load Torque Responses for Flexibility in Single Link Manipulator, M.Tech Research Scholar, Department of EEE, SACET, Cherala (A.P), India, 2014. [8] P. Axelssona, R. Karlssonb and M. Norrl, Bayesian State Estimation of a Flexible Industrial Robot, Patrik Axelsson, Rickard Karlsson and Mikael Norrlöf, Bayesian State Estimation of a Flexible Industrial Robot, 2012. [9] J. A. Somolinos, V. Feliu, and L. Sanchez, Design, dynamic modeling and experimental validation of a new three-degree-of-freedom flexible arm, Journal of Mechatronics, 12 (2002) 919– 948,2002. [10] A. Tewari, Modern Control Design with MATLAB and SIMULINK. John Wiley and Sons, 2002. [11] F. Heijden, R. Duin, D. Ridder, and D. Tax, Classification, Parameter Estimation and State Estimation, An Engineering Approach using MATLAB. John Wiley and Sons, 2004. [12] A. P. Roberts, State estimation when some measurements are delayed, IMA Journal of Mathematical Control and Information, 3 (1986) 299–310, 1986. [13] P. Corke, robotics Toolbox for Use with Matlab, more information http://www.petercorke.com. NOMENCLATURE h KE kinetic energy of hub. l KE kinetic energy of link. P KE kinetic energy of payload. l PE Elastic energy of link.  Mass per unit length of link. E Modulus of elasticity of material. I Moment of inertia about z-axis. p m Payload mass. h m Hub mass. W Deflection of the link. 1 r , 2 r Components of kinematics vectors. h J Hub inertia. N Number of links. ei n Number of flexible modes included.  Time delay in the measurements.  Noise in the measurements. Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 294 Figure (3): the desired base angle. 0 0.5 1 1.5 2 2.5 3 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 sec. motion 3 sec. motion Ɵ (t ) (r a d ) Time (sec) Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 295 Figure (4): tip deflection signal; a) 3 sec. motion, b) 2 sec. motion. 0 1 2 3 4 5 6 7 8 9 10 -0.015 -0.01 -0.005 0 0.005 0.01 0.015 (a) 0 1 2 3 4 5 6 7 8 9 10 -0.01 -0.005 0 0.005 0.01 0.015 (b) Figure (5): displacement variables error q(t). Time (sec) Ɵ e ( ra d ) δ 1 e (m ) δ 2 e (m ) Ψ 2 ( m ) Ψ 2 ( m ) Time (sec) Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 296 Figure (6): tip deflection (controlled motion) 2 sec. motion. Figure (7): tip deflection (controlled motion) 3 sec. motion. 0 1 2 3 4 5 6 -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0.015 0.02 Proposed controller PD-Controller 0 1 2 3 4 5 6 -0.01 -0.008 -0.006 -0.004 -0.002 0 0.002 0.004 0.006 0.008 0.01 Proposed Controller PD-Controller T ip -d is p la c e m e n t (m ) Time (sec) T ip -d is p la c e m e n t (m ) Time (sec) Al-Qadisiyah Journal For Engineering Sciences, Vol. 8……No. 3 ….2015 297 Figure (8): input torque 2 sec. motion. Figure (9): input torque 3 sec. motion. 0 1 2 3 4 5 6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 PD-Controller Proposed Controller 0 1 2 3 4 5 6 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 PD-Controller Proposed Controller f ( N .m ) Time (sec) Time (sec) f ( N .m )