ACCOUNTING FOR FLEXIBILITIES IN VISUAL SERVOING

Preprints of the 5th IFAC Symposium on Robust Control Design

ROCOND'06, Toulouse, France, July 5-7, 2006

ACCOUNTING FOR FLEXIBILITIES IN VISUAL SERVOING Edouard Laroche, Lo˜ıc Cuvillon, Jacques Gangloff, Michel de Mathelin

Laboratoire des Sciences de l’Image de l’Informatique et de la T´el´ed´etection (UMR CNRS 7005), Bd S´ebastien Brant, F-67400, Illkirch, France

Abstract: In this paper is considered visual servoing of multi-link robotic arms including exibilities. It is shown that even slight exibilities in the structure make the classical strategy unproper. A H∞ controller is designed for a given working point that allows to handle properly the exibilities. It is shown that the control strategy allows to operate within a working domain that can be su–cient for some applications. As a demonstrative example, the case of a two-links arm is considered. c The model is obtained thanks to the DynaFlex toolbox. Copyright ° 2006 IFAC Keywords: H∞ control, Visual servoing, Flexible manipulator, Robotic arm.

1. INTRODUCTION Robotic manipulators are usually considered as rigid and their control algorithms are designed based on this assumption. They exhibit nonlinear dynamics, which make their analysis and control design not so simple. If the structure is light or if the bandwidth is high, the robotic arm may exhibit flexible modes that need to be accounted for. Many papers have been dedicated to the control of the joint positions in this context; the classical method consists in obtaining the control law by direct inversion of the model (de Luca and Siciliano, 1993). But the final goal is the control of the terminal efiector. This aspect have received some attention in the context of one-link arms (Tso et al., 2003). But the case of multi-link systems has been rarely considered (Theodore and Ghosal, 2003). The problem of controlling the tip of the instrument held by a multi-link arm undergoing some flexible modes can be encountered in real applications. For instance, a 6-DOF (degree of freedom) robotic arm (SCARA) was designed in

our laboratory for compensation of heart beating movements in robotic surgery using visual servoing (Ginhoux et al., 2005). For safety reasons and in order to achieve high dynamics, the arm had to be relatively light compare with an industrial arm. As the bandwidth had to be increased as much as possible in order to follow the dynamics of the heart beating movements, the flexibilities of the structure were excited and it has been shown that they were to be accounted for in order to attain the required bandwidth (Cuvillon et al., 2005). A simplifled linear model of the exible dynamics is identified from experimental data and the control strategy was validated on the experimental bench (Cuvillon et al., 2006). When developping a control law, it is well-advised to check its validity on a refined model accounting to all the significant phenomena before implementing it on the setup. This allows to check the robustness with respect to the assumptions done when establishing the synthesis model. This paper propose a method for developping such a validating model accounting for flexibilities. Thanks to this model, the destabilizing efiects

Preprints of the 5th IFAC Symposium on Robust Control Design

ROCOND'06, Toulouse, France, July 5-7, 2006

of the flexibilities on a classical visual servoing strategy are emphasized. Finally, a H∞ controller is designed and evaluated.

2. MODELING FLEXIBLE MANIPULATORS

X2 Y0

v2(l2, t) M

X10

X0 θ2

Image plane

2.1 State of the art Y10

Two general methods can be used for simulating flexible structures. The finite-element approach consists in discretizing the structure in several elements that can be considered as rigid and linked one to another by flexible joints (Meek and Liu, 1995). The dynamical model is then obtained by applying the Euler-Lagrange equations to the discretized system. The obtained dynamic model is generally of high order. The second approach, more classically used, is based on assumed deformation fields. P Deformation v(x, t) is then chosen as v(x, t) = k ˆk (x)Vk (t), where {ˆk (x)}k=1···n is the deformation base. The Vk are included as additional variables in the coordinate vector q = [µ T V T ]T with V = [V1 ...Vn ]T and a generalized model: M (q)¨ q = F (q, q, ˙ T)

Y0

v1(l1, t) X1 θ1

X0

O

Fig. 1. The flexible two-links arm P nv k+1 . Notice that for the transverse k=1 Vk (t)x deformation field, the first term is V1 x2 as the beam remains tangent at the origin to the X axis. Thanks to the virtual work principle, the toolbox provides the expressions of M and F of the dynamic model (1).

(1)

is obtained, T being the join torque vector. A famous reference developing such an approach is by De Lucas and Siciliano (de Luca and Siciliano, 1991) in which the deformation base is composed of the free modes of the system and the dynamic model is obtained with Euler-Lagrange equation.

2.2 DynaFlex toolbox DynaFlex toolbox allows to model multi-link mechanical systems including serial and parallel connexions with all kinds of kinematic constraints; the description being given as a linear graph. The segments can be either rigid or flexible. The deformation field is considered as the resultant of four components: the axial deformation, the two lateral deflexions and the torsion. In a 3 dimensional space (O, X, Y, Y ), let us consider a beam in the X direction from x = 0 to x = L. For a point of the beam of coordinates (x, y, z) without deformation, its coordinates (˜ x, y˜, z˜) after deformation are:  Rx ˜ = x + u ¡ yv 0 ¡ zw0 ¡ 12 0 (v 02 + w02 )d» x y˜ = y + v ¡ zφ + y  z˜ = z + w + yφ + z (2) where u(x, t) is the elongation, v(x, t) and w(x, t) are the transverse deformations along respectively the Y and Z directions and φ(x, t) is the angle of twist ; v 0 standing for ∂v(x,t) (Shi et al., 2001). ∂x Each deformation u, v, w and φ is developed as a sum of polynomial term; for instance v(x, t) =

2.3 Modeling a two-segments arm As a case study, a planar arm with two flexible segments is considered, as represented in figure 1. De exions v1 (x, t) and v2 (x, t) in the plan of both arms are considered. For each deformation, one term is used: v1 (x, t) = V11 (t)x2 and v2 (x, t) = V21 (t)x2 . The (xM , yM ) position of the tip of the arm is given in the frame (O, X0 , Y0 ) by respectively the real and imaginary part of: ¢ ¡ 2 3 l1 zM = exp (jµ1 ) l1 + ¡jV11 l12 ¡ 32 V11 ¢ 2 3 + exp (j (µ1 + µ2 + 2V11 l1 )) l2 + jV21 l22 ¡ 23 V21 l2 . (3) The two segments are of same length l1 = l2 = 0.5 m, of square section of width r = 5 cm; the masses are m1 = 7.5 kg and m2 = 5 kg; the Young modulus is of 1 GPa. Damping ratios on the joints are assumed null. Resulting expressions of matrix M (q) and vector F (q, q, ˙ T ) are not given within the text for space reason but can be found at http://eavr.u-strasbg.fr/perso/edouard/ exrob/.

3. VISUAL SERVOING AND FLEXIBILITIES 3.1 Principles Let F denotes the vector of the coordinates of the features in the image and F ⁄ its reference value. For a rigid arm, the displacements in the image are linked to the displacements of the angular position and the relationship F˙ = J(µ)µ˙ can be obtained

Preprints of the 5th IFAC Symposium on Robust Control Design

ROCOND'06, Toulouse, France, July 5-7, 2006

0.06 0.04

M

M

x , y (m)

where J(µ) is the interaction matrix. Assuming that the angular velocities are perfectly controlled (µ˙ = µ˙⁄ with µ˙⁄ the joint speed reference vector), the classical control strategy is

x M y

0.02

M

reference

(4)

3.2 A visual servoing task In the considered case, the camera is positioned beside the arm in an ‘eye to hand’ conflguration (Hutchinson et al., 1996) and provides the position in the image of the tip of the arm (y = [xM yM ]T ). Two experiments are considered. In the flrst one (#1), a step of 5 cm of amplitude is requested simultaneously on x1 and x2 . In figure 2 are given the trajectories of respectively the displacements in the image in the upper part and the joint velocity references in the lower part. In a second experiment (#2), the trajectory in the

0.15

0.2

0.25

0.3

0.35

0.4

1

2

u , u (rad/s)

−2 u1 u2

−4 −6 0

0.05

0.1

0.15

0.2 temps (s)

0.25

0.3

0.35

0.4

Fig. 2. Experiment #1 with the classical control strategy on the rigid model (‚ = 35 s¡1 ) 0.375 0.37 0.365

yM (m)

0.36 0.355 0.35 reference λ = 20 λ = 35

0.345 0.34 0.335 0.33 0.12

0.125

0.13

0.135

0.14

0.145 0.15 xM (m)

0.155

0.16

0.165

0.17

Fig. 3. Experiment #2 with the classical control strategy on the rigid model 0.08 0.06 0.04

xM yM

M

M

x , y (m)

The efiects of the flexibilities of the structure have received few attention. In a previous work, we proposed to identify globally the flexibilities of the structure and to use multivariable control strategies for obtaining high bandwidth visual servoing around a working configuration (Cuvillon et al., 2005). The validation of the controllers were done directly on the test-bench and the individual contributions of all the flexible efiects were not assessed separately. Herein, we show that deflections of low amplitude, that could have been neglected for designing the controller, finally turns the visual servoing loop unstable.

0.1

0

0.02

reference 0 0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

2

2

If performance is the main goal then dynamics cannot be neglected anymore and the model must be improved. The delays due to image acquisition and processing can be considered, as in (Corke and Good, 1996). The dynamics of the speed control loops can also be identified by a linear model µ˙ = H(s)µ˙⁄ as in (Ganglofi and de Mathelin, 2003). In the case where the length of F is equal to the number of joints to be controlled, a multivariable controller K(s) can be designed with the control strategy µ˙⁄ = K(s)(F ⁄ ¡ F ). If the length of F it larger than the number of joints, the system is uncontrollable and some modifications have to be be included in order to consider only the controllable modes (Hashimoto et al., 1996).

0.05

2

1

where J † (µ) is the inverse or the pseudo-inverse of J, depending on its being square or not. The system is globally stable and the errors in the image decrease exponentially. The dynamics can be tuned with the scalar gain ‚ (Espiau et al., 1992). Notice that this approach neglects completely the dynamics of the robot, the model being composed of a variable matrix gain and integrators. The time-delays and discretization efiects are also neglected.

0 0

u , u (rad/s)

µ˙⁄ = ‚J † (µ)(F ⁄ ¡ F )

0 −2 u1 u2

−4 −6 0

0.05

0.1

0.15

0.2 temps (s)

0.25

0.3

0.35

0.4

Fig. 4. Experiment #1 with the classical control strategy on the exible model (‚ = 35 s¡1 ) image is a rectangle followed at a constant speed of 0.1 m/s. The corresponding results with the classical control strategy (4) are given in figure 3. The reference trajectory is well followed. But what about the behavior with the flexible modes? In figure 4 are given the time response trajectories obtained with the flexible model (‚ = 35 s¡1 ). One can notice that the system is unstable. The bandwidth has to be reduced and a stable trajectory could be obtained with ‚ = 20 s¡1 ; the

Preprints of the 5th IFAC Symposium on Robust Control Design

ROCOND'06, Toulouse, France, July 5-7, 2006

0.06 rigid robot

referance

0.02 0 0

flexible robot 0

−20

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0

1

2

u , u (rad/s)

1

0.05

0.1

0.15

0.2 temps (s)

0.25

0.3

0.35

−120 0 10

2

10

3

10 pulsation (rad/s)

4

10

5

10

6

10

Fig. 7. Frequency response of the transfer between torque and angular velocities (rigid and flexible case) The output equation is obtained from the geometric model; the output being · = ˆ(q) = [<(zM ); =(zM )]T (see eq. (3)), the linear output equation writes · = C´ with: • ‚ ∂ˆ(q0 ) C= 02£4 (8) ∂q

0.375 0.37 0.365 0.36

M

1

10

0.4

Fig. 5. Experiment #1 with the classical control strategy on the exible model (‚ = 20 s¡1 )

y (m)

−60

−100

u1 u2

−2

0.355 0.35

Linearization is done for the position µ2 = 3… 4 , all other entries being null. The frequency response of the transfer between torque and angular velocities is given in figure 7 with and without flexibilities.

0.345 reference λ = 20 λ = 35

0.34 0.335 0.33 0.12

−40

−80

−1

−3 0

singular values (dB)

M

M

x , y (m)

20

xM yM

0.04

0.125

0.13

0.135

0.14

0.145 0.15 xM (m)

0.155

0.16

0.165

0.17

Fig. 6. Experiment #2 with the classical control strategy on the exible model corresponding results are given in figures 5 and 6. The reference trajectories are followed but the behavior is completely altered.

In an experimental context, the linear model can be obtained by identification using the informations provided by the camera as in (Cuvillon et al., 2005). Notice that the model and the obtained dynamics are close to those obtained in the mentioned paper in an experimental context.

4.2 Controller design 4. H∞ CONTROL H∞ control allows to design a closed loop controller for multivariable systems. It is reputated for well handling flexibilities (Duc and Font, 1999; Zhou et al., 1996).

4.1 Linearized model The nonlinear model writes q¨ = φ(q, q, ˙ T ) with φ(q, q, ˙ T ) = M ¡1 (q)F (q, q, ˙ T ). The linear model around a given working position (q0 , q˙0 , T0 ) writes ´˙ = A´ + BT with: • ‚ q ´= (5) q˙ # " 04£4 I4 A = ∂φ(q0 ,q˙0 ,T0 ) ∂φ(q0 ,q˙0 ,u0 ) (6) ∂ q˙

∂q

B=

•

0 ∂φ(q0 ,q˙0 ,T0 ) ∂T

‚

(7)

The linear plant being augmented with weighting functions such as in figure 8, the controller is designed in order to minimize the H∞ norm of the transfer between r and z. Weighting functions W1 (s) allows to shape the frequency response of the sensitivity function S(s) = (I + G(s)K(s))¡1 , in order to tune the bandwidth and the modulus margin. Making transfer K(s)S(s) decrease in high frequency allows roll-ofi efiect necessary for robustness to neglected dynamics. With the following weightings W1 (s) = diag{W11 (s), W11 (s)} and W2 (s) = diag{W21 (s), W21 (s)} with W11 (s) = 0.5s+13.2 s −4 and W21 (s) = 2.2+2.2e , the obtained H norm ∞ s+10000 is ° = 0.99. The templates (°W1¡1 (s), °W2¡1 (s)) and the frequency response of the closed-loop transfers are given in figure 9. The trajectories of the system with the H∞ controller are given in figures 10 and 11. One can notice that the behavior is notably improved compared to the classical control strategy presented

Preprints of the 5th IFAC Symposium on Robust Control Design

ROCOND'06, Toulouse, France, July 5-7, 2006

0.06

W1(s)

z1 =z z2

W2(s)

xM, yM (m)

r

0.04 x M y

0.02

M

reference

G(s)

0 0

+

· -

u1, u2 (rad/s)

e K(s)

Sing. Value (dB)

0.15

0.2

0.25

0.3

0.35

0.4

0 −0.5 u 1 u2

−1 −1.5 0

Fig. 8. Generalized plant with weighting function diagram

0.05

0.1

0.15

0.2 temps (s)

0.25

0.3

0.35

0.4

Fig. 10. Experiment #1 with H∞ control on the flexible system

Transfer from r to e

0 0.375

−10 0.37

−20 0.365

−30 0 10

1 Transfer from r to (dq/dt)* 2 10 10

10

3

H∞ GS

0.355

M

60

reference H∞

0.36

y (m)

80

Sing. Value (dB)

0.1

0.5

θ˙ ∗

10

0.05

0.35

40 0.345

20 0

0.34

−20 0 10

0.335

1

10

2

10

10

3

0.33 0.125

Fig. 9. Frequency response of the closed-loop system with H∞ control (plain) and templates (dashed)

5. ROBUSTNESS ANALYSIS Modulus margin is one of the criteria considered in controller design (¢M = 1/ maxω ¾(S(jω))). In flgure 12 are given the variations of the modulus margin corresponding to the linearized model for each point of the trajectory of Experiment #2. The classical controller with ‚ = 35 shows unstability for most of the trajectory. The classical controller with ‚ = 20 remains stable but the

0.135

0.14

0.145 0.15 xM (m)

0.155

0.16

0.165

0.17

Fig. 11. Experiment #2 with H∞ control on the flexible system

in figure 4 to 6. The maximum de exions are respectively of 0.76 mm and 0.17 mm at the end of the segments.

0.6 0.5

modulus margin

In an attempt to improve the robustness, a Gain Scheduling controller (GS) can be designed, inspired from the classicel control law (4). The nominal controller being K(s), the gain scheduling controller can be either K1 (s)J ¡1 (q) with K1 (s) = K(s)J(q) or J ¡1 (q)K1 (s) with K1 (s) = J(q0 )K(s). The two versions of GS showed very similar results; only the flrst version was then reported. In flgure 6 are given results of the H∞ controllers with and without GS. The improvement is not so clear; this point will be emphasized in the next section.

0.13

0.4 0.3

λ = 35 λ = 20 H

0.2

∞

H GS

0.1 0 0

∞

0.2

0.4

0.6

0.8 1 time (s)

1.2

1.4

1.6

Fig. 12. Evolution of the robustness margin during trajectory progress modulus margin remains low. A real improvement is obtained with the H∞ controller with or without gain scheduling. Rather than checking stability or some performance index along a given trajectory, it is more attractive to be able to insure that the system remains stable over a given working domain. Consider that each state xk remains in the interval [xk0 ¡ wk ; xk0 + wk ], where the wk are collected from simulation and so deflning the nominal working domain. Let us call B(r) the box obtained from dilatation of the original domain by ratio r, corresponding to variations of xk over [xk0 ¡rwk ; xk0 + rwk ]. The minimum of a given performance index

Preprints of the 5th IFAC Symposium on Robust Control Design

0.6

λ = 35 λ = 20 H∞

0.5 modulus margin

ROCOND'06, Toulouse, France, July 5-7, 2006

H∞ GS

0.4 0.3 0.2 0.1 0 0

0.2

0.4

radius r

0.6

0.8

1

Fig. 13. Evolution of the robustness margin with respect to the variation domain size over B(r) is a non-increasing function. In flgure 13 are given the variations of the robustness margin with respect to the dilatation ratio. The minimum is evaluated on the 28 = 64 summits of B(r); rigourosly speaking, what is computed is an upper bound, due to the lack of convexity proof. With this method, stability is guaranteed for 80 % of the nominal variations. Notice that the classical tools available for robustness analysis are not well adapted to the system. Indeed, they require a Linear Fractional Representation (LFR) what is di–cult to implement in the present case due to the complexity of the dynamical equations. Moreover the model include trigonometric fonctions that are irrational; the LFR would then require approximations. 6. CONCLUSION In this paper, it was shown that exibilities of small amplitude (less than one minimeter of deexion on a half-meter segment) can drastically deteriorate the performances of a classical visual servoing strategy. H∞ methods are famous for controlling LTI systems including exibilities. In the present case, they allow to reach high performance arround a nominal position. It was also shown that the performances are preserved within a working domain. This can be enough for applications where high performances are request only within a limited working domain. This is notably the case for heart-beating movement compensation in the context of robotized surgery (Ginhoux et al., 2005). REFERENCES Corke, P. I. and M. C. Good (1996). Dynamic efiects in visual closed-loop systems. IEEE Trans. on Robotics and Automation 12(5), 671–683. Cuvillon, L., E. Laroche, H. Garnier, J. Ganglofi and M. de Mathelin (2006). Continuous-time model identiflcation of robot exibilities for fast visual servoing. In: IFAC Symposium on System Identification. Newcattle-Australia.

Cuvillon, L., E. Laroche, J. Ganglofi and M. de Mathelin (2005). GPC versus H∞ control for fast visual servoing of a medical manipulator including flexibilities. In: Int. Conf. on Robotics and Automation. Barcelona-Spain. de Luca, A. and B. Siciliano (1991). Closedform dynamic model of planar multilink lightweight robots. IEEE Trans. on System Man and Cybernetics 21(4), 826–839. de Luca, A. and B. Siciliano (1993). Inversionbased nonlinear control of robot arms with flexible links. AIAA J. of Guidance Control and Dynamics 16(6), 1169–1176. Duc, G. and S. Font (1999). Commande H∞ et „analyse. Hermes Science Publications. Paris. Espiau, B., F. Chaumette and P. Rives (1992). A new approach to visual servoing in robotics. IEEE Trans. on Robotics and Automation 8(3), 313–326. Ganglofi, J. and M. de Mathelin (2003). High speed visual servoing of a 6 DOF manipulator using multivariable predictive control. Advanced Robotics 17(10), 993–1021. Ginhoux, R., J. Ganglofi, M. de Mathelin, L. Soler, M. Arenas Sanchez and J. Marescaux (2005). Active filtering of physiological motion in robotized surgery using predictive control. IEEE Trans. on Robotics 21(1), 67–79. Hashimoto, K., T. Ebine and H. Kimura (1996). Visual servoing with hand-eye manipulator– optimal control approach. IEEE Trans. on Robotics and Automation 12(5), 766–774. Hutchinson, S., G. D. Hager and P. I. Corke (1996). A tutorial on visual servo control. IEEE Trans. on Robotics and Automation 12(5), 651–670. Meek, J. L. and H. Liu (1995). Nonlinear dynamics analysis of flexible beams under large overall motions and the flexible manipulator simulation. Computers & Structures 56(1), 1–14. Shi, P., J. McPhee and G.R. Heppler (2001). A deformation field for euler-bernouilli beams with application to flexible multibody dynamics. Multibody System Dynamics 5(1), 79–104. Theodore, R.J. and A. Ghosal (2003). Robust control of multilink flexible manipulators. Mechanism and Machine Theory 38, 367–377. Tso, S.K., T.W. Yang, W.L. Xu and Z.Q. Sun (2003). Vibration control for a flexible-link robot arm with deflexion feedback. Int. J. of Non-Linear Mechanics 38, 51–62. Zhou, K., J. C. Doyle and K. Glover (1996). Robust and Optimal Control. Prentice-Hall.