Improving Performance of Task Space Controllers using an Inner PD Joint Controller: An Application to Visual Sevoing. 1 Centro

R. Garrido1 , E. Canul1 , A. Soria1 . de Investigación y de Estudios Avanzados del IPN, D.F. México

Abstract— This paper presents a comparative study of two task space control approaches. The first approach, similar to most of task space controllers, makes use of an inner velocity loop. The second proposed approach employs an inner proportional derivative (PD) joint position-velocity loop. A stability proof for the second apprach is provided together with experiments using a visual servoed robot. It is shown that the proposed control law needs less gain at the task space level then precluding amplification of the measurement noise; moreover, it is shown that the first approach produces uncontrolled movements when the task space sensor fails whereas the proposed controller avoids uncontrolled behavior

Fig. 1.

Block Diagram of (1)

I. INTRODUCTION Since its very first apparition [1], the task space control approach has gained more interest between researchers. Theoretical research has increased and applications developed. It has been treated to deal mainly with stability under several assumptions such as the robot is not redundant and its Jacobian matrix is nonsingular at the desired position [1], subsequently in [3],[4] a variant of such controller is proposed under similar assumptions. In [5] a novel approach uses an energy shaping plus damping injection approach and achieves local asymptotic stability despite of the singularities on the task function Jacobian. Further contributions in [6], [7] are the inclusion of uncertainties on the robot Jacobian matrix and gravity torques are considered, then, provided that a bound on the estimated robot Jacobian matrix is known asymptotic stability is assured. In [7] an integral action is embedded in the control law. Other contributions [8],[9] consist on task space feedback controllers using the transpose robot Jacobian matrix instead of its generalized inverse. Finally, the visual servoing approach in [8], [10] as a particular case of the task space approach is considered for solving the position measurement problem in task space, in [10] the author proves asymptotic stability and robustness of the proposed controller under uncertainties in camera orientation and lens radial distortion. Notice that all the aforementioned approaches employ joint velocity and position task feedback and do not take into account joint position measurements. This paper proposes a task-space robot controller using two nested loops, an inner joint PD loop and an outer PI loop. Stability of the closed loop system is proved through the Lyapunov method. Experimental results using a robot controlled through visual feedback asses the performance of the proposed approach.

II. P RELIMINARIES A. Previous Task Space Controllers From the first work on task space control [1] to the last ones [6],[7], there is a common structure: an inner loop and an outer loop connected in cascade. The structure of most task space controllers is as follows [1]:  = M W (q) Ns y ˜  Ny q˙ + j (q)

(1)

where  is the q × 1 vector of applied joint torques, M (·) 5 Rp{q is the robot Jacobian matrix, Ns 5 Rp{p , Ny 5 Rq{q are positive definite feedback proportional and derivative gains respectively, y ˜ = yg  y 5 Rp is the task space error where yg and y are the desired and measured positions respectively expressed in task space coordinates, q˙ 5 Rq is the measured joint velocity and g (q) 5 Rq is the gravity compensation. The first element of the right hand side of equation (1) and the gravity compensation modify the potential energy in the robot and the term Ny q˙ injects damping. Notice that the control law (1) can be interpreted as an inner velocity loop connected in cascade with an external position loop.(Figure 1). Note that all the industrial robots are controlled using joint proportional integral derivative (PID) controllers. In order to put to work most of the task space controllers mentioned above, the joint PID position controllers should be reconfigured as joint velocity controllers and the loop is closed using task space position measurements, for instance, measurements from a vision system. In this case, failure of the task space sensor would produce unpredictable movement since the robot is controlled only by the joint velocity loops, the proportional action M W (q) Ns y ˜ is not longer

active and the robot motion may get unbounded. After, having a PID or proportional derivative (PD) joint controller allows compensating disturbances and nonlinear phenomena as stiction at the joint level and with a high bandwidth. Note that task space sensors have poor bandwidth compared with joint sensors. Typically, a vision system using a high-speed digital camera is able to capture 500 images per second and a data acquisition card may sample an optical encoder at 100 Nk}. Moreover, it is not always possible to reconfigure the joint controller in industrial robots. This paper proposes the inclusion of a proportional action through joint position measurements in the inner joint PD loop, it is explicitly considered for the Lyapunov analysis and the stability of the proposed approach is concluded. Finally, there is a comparison between the proposal and the traditional task space schemes such as [1]

definition [2] states properties of saturation functions useful for the stability analysis presented in the next section. Definition 1: Consider the scalar functions Vl (l ) and its derivative  l (l ) where l is a real number. Functios Vl (l ),  l (l ) fulfill the following properties. 1. Vl () A 0 for  6= 0 and Vl (0) = 0 2. Vl () is at least two times differentiable and the derivative  l (l ) = gg l [Vl (l )] is strictly increasing in  for || ?  l for some  l , and the saturation for ||   l , i.e.  l () = ± l for    l and  ?  l respectively with  l a positive constant. 3. There exists a constant f¯l A 0, such that: Vl ()  f¯l  2l ()

B. Robot Dynamics. for  6= 0.

In absence of friction and some other disturbances, the dynamics of a serial q-link rigid robot manipulator can be written as [11],[12]:

III. TASK -S PACE C ONTROLLER WITH AN I NTERNAL PD L OOP

P (q) q ¨ + F (q> q) ˙ q˙ + j (q) = 

The proposed controller in this paper is the following

(2)

Vectors t, t˙ and t¨ correspond to the joint position, velocity and acceleration respectively, P (q) 5 Rq×q is the robot inertia matrix, F (q> q) ˙ q˙ 5 Rq corresponds to the Coriolis and centripetal forces vector, g (q) 5 Rq is the gravity torque vector and  the input torque vector. Throughout this paper, the use of the notation min {·} and max {·} will indicate the smallest and largest eigenvalues of a symmetric positive bounded given matrix respectively and k·k is the norm of a given matrix or vector and h·> ·i is the scalar product. Model (2) considering revolute joint robots has the following properties. 1. P (q) = P W (q) 2. P (q) A 0 2 3. min {P (q)} kxk  xW P (q) x  2 max {P (q)} kxk where min {P (q)} A 0, max {P (q)} A 0 denotes the minimum and maximum eigenvalue of P (q) respectively. 4. The matrix Q (q> q) ˙ = P˙ (q)  2F (q> q) ˙ is skew˙ symmetric and P (q) = F (q> q) ˙ + F W (q> q). ˙ 5. There exists a constant nF such that the following inequality is fulfilled [13]: kF (q> q) ˙ qk ˙  nF kqk ˙ 2 6. The robot potential energy X (q) and gravity vector j (q) are bounded, i.e. kX (q)k  nx and 0 kj (q)k  nj . Furthermore, there exists a constant n 0 0 such h that¯ kj (x)¯i j (y)k  n kx  yk where n  ¯ Cjl (q) ¯ q maxlm ¯ Ctm ¯ . C. Saturation Functions. In recent years the use of a special kind of saturation functions has become more common [2],[14]-[17]. The following



= Ng q˙ + Ns (u  q)

u = MˆW (q) Os  (e) + Ol

(3) Zw

MˆW (q) Os  (e) g (4)

0

Equation (3) corresponds to the inner PD control law and (4) to the outer PI controller. Ns = ns L 5 Rq×q , ns A 0, Ng A 0 5 Rq×q are the proportional and derivative gain matrices respectively with L the identity matrix, Os A 0 5 Rp×p and Ol A 0 5 Rq×q are the proportional and integral gain matrices for the outer loop, e is the task space error defined as e = Xg X 5 Rp ; where X and Xg are the measured and desired positions and  A 0 is an arbitrary constant. Figure 2 shows the block diagram for this controller. Let be Vl (hl ) and l (l ) as scalar function and its time derivative respectively according to Definition 1,  (e) = ¢W ¡  1 (h1 ) = = =  p (hp ) is the saturation error vector. W ˆ Assume that an estimate M (q) of the robot Jacobian matrix is available such that [7] ° ° ° ° (5) °M (q)  Mˆ (q)°  u with u a positive constant. Let w (w) be defined as Zw w (w) =

Ns1

[j (qg ) + Ns q (0)] +

y () g

(6)

0

where: y = q˙  Ol MˆW (q) Os  (e)

(7)

where | is given in (7). Therefore, the Lyapunov function candidate (10) can be rewritten as 1 Y (w>  (e) > q) ˙ = yW P (q) y + X (q)  X (qg ) 2 h i 1 2 W ˆ    (e) Os M (q) Ol P (q) Ol MˆW (q) Os  (e) 2 1 (12) +ns hS (e) > Ls i + wW Ns w + qW j (qg ) 2 Since the inertia matrix is positive definite the following inequality is fulfilled [7],[9] h i 2 k (e)k2   W (e) Os Mˆ (q) Ol P (q) Ol MˆW (q) Os  (e)

Fig. 2.

n o where 2 = max Os Mˆ (q) Ol P (q) Ol MˆW (q) Os . As a consequence of the robot model properties, for Ls large enough there exists a positive constant 1 such that

Block Diagram of the proposed controller (8)

Clearly w ˙ = y. Then, using (6), control law (3,4) can be rewritten as  = Ns MˆW (q) Os  (e)  Ng q˙ + j (qg )  Ns w

1 k (e)k2  X (q)X (qg )+qW j (qg )+ns hS (e) > Ls i Then, (10) is lower bounded as follows 1 Y (w>  (e) > q) ˙  min {P (q)} kyk2  ¸ 2 1 2 1 2 2 + 1   2 k (e)k + ns kwk 2 2

(8)

To get the closed loop system equation, the substitution of (8) into (2) is carried out P (q) q ¨ + F (q> q) ˙ q˙ + j (q) = Ns MˆW (q) Os  (e) Ng q˙ + j (qg )  Ns w (9) The following Lyapunov candidate function is considered for the stability analysis

Hence, Y (w>  (e) > q) ˙ is definite positive if r 21 A 2

The time derivative of (10) is made element by element and using (9) yields i h Y˙ (w>  (e) > q) ˙ = ns q˙ W M W (q)  MˆW (q) Os  (e)

1 W q˙ P (q) q˙ 2 q˙ W P (q) Ol MˆW (q) Os  (e)

nsW  (e) Os Mˆ (q) Ol MˆW (q) Os  (e) W  (e) Os Mˆ (q) Ol [j (qg )  j (q)]

Y (w>  (e) > q) ˙ =

+X (q)  X (qg ) + qW j (qg ) 1 +ns hS (e) > Ls i + wW Ns w 2 where hLs > S (e)i =

LWs S (e)

=

p X

(13)

(10)

osl Vl (hl ), SW (e) =

£

l=1 ¤W S1 (e1 ) = = = Sp (ep ) , Ls = gldj {Os } and q = q  qg , with q and qg as the join position and desired joint position respectively. First, to verify that (10) is positive definite consider the following expression

1 W 1 y P (q) y = q˙ W P (q) q˙  q˙ W P (q) Ol MˆW (q) Os  (e) 2 2 1 2 W +   (e) Os Mˆ (q) Ol P (q) Ol MˆW (q) Os  (e) (11) 2

]2 (q> q> ˙  (e))  q˙ W Ng q˙

(14)

]2 (q> q> ˙  (e)) = ˙ (e)W Os Mˆ (q) Ol P (q) q˙ W + (e) Os M˙ (q) Ol P (q) q˙ © ª + (e)W Os Mˆ (q) Ol F W (q> q) ˙  Ng q˙

(15)

where

Since the gravity torques are bounded, there exists a positive constant 2 [7],[8] such that for ns large enough the following inequality holds nsW  (e)W Os Mˆ (q) Ol MˆW (q) Os  (e) W 2  (e) Os Mˆ (q) Ol [j (qg )  j (q)]  2 k (e)k

Finally, due to the fact that the robot Jacobian matrix is bounded, it follows that 2

˙  (e))|  f0 kqk ˙ > ;f0 A 0  |]2 (q> q>

(16)

As a consequence, Y˙ is upper bounded as follows ˙ k (e)k Y˙ (w>  (e) > q) ˙  ns umax {Os } kqk  (min {Ng }  f0 ) kqk ˙ 2  2 k (e)k2

(17)

where u is defined in (5). Inequality (17) can be further rewritten as: 1 ˙ 2 Y˙ (w>  (e) > q) ˙   (min {Ng }  f0 ) kqk 2 1  2 k (e)k2 2 1p  [ (min {Ng }  f0 ) kqk ˙ 2 2 (uns min {Os }) p k (e)k]2 min {Ng }  f0 # " 1 (uns min {Os })2 2  2  (18) k (e)k 2 min {Ng }  f0 Then, Y˙ (w>  (e) > q) ˙  0 if: min {Ng } A f0 2 (min {Ng }  f0 ) A (uns min {Os })2

Fig. 3.

Proposed Controller without inner joint position measurements

B. Comparative Study on the performance of the proposed controller. The performance of the proposed controller (3), (4) is compared with the traditional schemes of task space controllers, that is, without inner proportional action through joint position measurements. Therefore a minor modification in the scheme was made, the inner position feedback is eliminated so that the proportional action rely only in the outer PI loop. Therefore (3) becomes:  = Ng q˙ + Ns u

(19)

Asymptotic stability follows directly by applying La Salle’s invariance theorem [11],[12],[18]. Proposition 1: Closed loop of dynamic system (2) and control law (3), (4) has a unique equilibrium point at t = 0>  (h) = 0> z = 0> and it is asymptotically stable if Mˆ (t) fulfills (5) and Ns > Ng > Os > Ol and  are chosen such that inequalities (13) and (19) are fulfilled. IV. E XPERIMENTAL R ESULTS A. Experimental Development Platform The experimental test bed is shown in Figure 3. It was used a planar 2-link revolute joint robot under visual feedback [19]. The links are driven by DC brushed motors through timing belts. The motors are subsequently driven by power amplifiers from Copley Controls model 413, working in current mode. Joint position feedback is performed by incremental encoders with 10,000 pulses per revolution. Data acquisition was performed through a MultiQ 3 card from Quanser Consulting. Image acquisition for task space position feedback was performed using a CCD-Dalsa camera connected to a National Instruments PCI-1422 card through an RS-422 protocol. The visual sample time for the outer loop was 7 pv and the inner sample time for the joint position and velocity loop was 1 pv.

(20)

Figure 3 depicts the block diagram with this modification. The manipulator Jacobian matrix for the visual servoing application is given by Mˆ (q) = U () Mˆd (q)

(21)

where U () is the rotation matrix between the cartesian and the visual space and is taken as the identity and Mˆd (q) is an estimate of the robot analytic Jacobian matrix. For the robot used in the experiments Mˆ (q) is expressed as  ¸ ˆo1 cos t1 ˆo2 cos t2 ˆ M (q) = ˆ (22) o1 sin t1 ˆo2 sin t2 t1 and t2 are the robot angular positions, o1 and o2 denote the lenghts of the first and second robot links. Their approximate values are ˆo1 = ˆo2 = 0=21 p. The gains of the proposed controller (3), were chosen as follows1 ¸  ¸ 10 0 0=18 0 , Ng = > sat(h) = ±9 0 10 0 0=2  ¸  ¸ 0=15 0 2=2 0 Os = , Ol = (23) 0 0=12 0 1=8 

Ns =

And the gains of the modified controller (20)were set up as follows: 1{

and | in pixeles units

Fig. 4.

Signal noise with the proposed controller (3)

Fig. 6.

Joint velocities when visual feedback fails in controller (3)

Signal noise with modified controller (20)

Fig. 7.

Joint velocities when visual feedback fails in controller (20)

Fig. 5.

 Ns =

1 0 0 1

¸



0=15 , Ng = 0  ¸  1 0 Os = , Ol = 0 1

¸

0 > vdw(h) = ±9 0=18 ¸ 0=3 0 (24) 0 0=05

Figure 4 shows the signal noise with the set of parameters (23) for the proposed controller and Figure 5 for the set of parameters (24) for the modified controller. It can be seen that due to the use of higher proportional gains at task level in the modified controller (20) the noise coming from the camera is amplified; therefore, it is more sensitive to the noise coming from the vision system; as a consequence noise from the task level sensor, i.e. in the present case a vision system, is not amplified. On the other hand, the proposed controller is well-behaved in a safety-sense. The experiments were performed as follows: with a fixed reference the visual feedback was suddenly interrupted. Both schemes lost the reference but whereas

the proposed controller (3) did it with a slow constant joint velocity (Figure 6), the modified controller (20) did it with an increasing joint velocity in most cases (Figure 7) and, in some times, the robot turned uncontrolled and moved with fast joint velocities (Figure 8, the corresponding joint position is shown in Figure 9). After a while the visual feedback was reestablished. An advantage of keeping an inner joint proportional action is precisely that when the vision feedback system fails the robot still is under a partial control through the inner PD loop. V. C ONCLUSION This work offers an alternative approach for robot manipulator control in task space. Its main feature is an inner joint PD position loop in cascade with an outer PI loop fed by task space measurements. This setting allows compensating disturbances at joint level and avoids uncontrolled robot motion if the task position sensor fails. Moreover,

Fig. 8. Uncontrolled fast moving when visual feedback fails in controller(20)

Fig. 9. Uncontrolled Joint position when visual feedback fails in controller (20)

the proposed approach can be applied to an industrial robot since in this case the robot native controller supplies the PD inner loop. This feature would allow to integrators of industrial equipment coupling advanced controllers to well tested industrial robotic platforms. Experiments executed in a laboratory prototype show the controller performance. R EFERENCES [1] M. Takegaki, S. Arimoto. A new feedback method for dynamic control of manipulators. Trans. ASME; J. Dyn. Syst., Meas., Ctrl., vol.102, pp.119-125, June 1981. [2] S. Arimoto. Control Theory on Non-Linear Mechanical Systems. Oxford, U.K.: Claredon, 1996. [3] L. Sciavicco and B. Siciliano, Modeling and Control of Robot Manipulators. New York: McGraw-Hill, 1996. [4] C. Canudas de Wit, B. Siciliano and G. Bastin, Theory of Robot Control. New York: Springer-Verlag, 1996. [5] R. Kelly. Regulation of manipulators in generic task space: An energy shaping plus damping injection approach. IEEE Trans. on Robotics and Automation. vol.15, no.2, 1999.

[6] C. C. Cheah, S. Kawamura and S. Arimoto. Feedback control for robotic manipulators with uncertain kinematics and dynamics. Proc. IEEE Int. Conf. Robotics and Automation, Leuven, Belgium, 1998, pp. 3607-3612. [7] C. C. Cheah, S. Kawamura, S. Arimoto and K. Lee. PID control for robotic manipulator with uncertain Jacobian matrix. Proc. IEEE Int. Conf. Robotics and Automation, Detroit, MI, May 1999, pp. 494-499. [8] C. C. Cheah, M. Hirano, S. Kawamura and S. Arimoto. Approximate Jacobian control for Robots with uncertain Kinematics and Dynamics. IEEE Trans. on Robotics and Automation, vol. 19, no. 4, August 2003. [9] C.Q. Huang, X.G. Wang and Z.G. Wang. A class of transposed Jacobian-based NPID regulators for robot manipulators with an uncertain Kinematics. Journal of Robotic Systems, 2002, pp.527-539. [10] R. Kelly. Robust Asymptotically stable visual servoing of planar robots. IEEE Trans. on Robotics and Automation. vol.12, no.5, October 1996. [11] M. Spong and M. Vidyasagar. Robot Dynamics and control. New York: Wiley 1989 [12] J.J. Craig. Introduction to Robotics: Mechanics and Control. New York: Addison-Wesley 2000. [13] R. Kelly and V. Santibáñez. Control de movimientos de Robots manipuladores. Pearson-Prentice Hall. Madrid c. 2003 [14] R. Kelly. Global positioning of robot manipulators via PD control plus a class of nonlinear integral actions. IEEE Transactions on Automatic Control, Vol 43, pp. 934-938, July 1998. [15] R. Kelly and V. Santibáñez. Global convergence of the adaptive PD controller with computed feedforward for robot manipulators. in Proc. IEEE Int. Conf. Robotics and Automation, Detroit, MI, May 1999, pp. 1831-1836 [16] R. Kelly and V. Santibañez. A class of nonlinear PID global regulators for robot manipulators. in Proc. IEEE Int. Conf. Robotics and Automat., Leuven Bélgica, May 1998, pp. 3601-3606. [17] J. Álvarez, R. Kelly and I. Cervantes. Semiglobal stability of saturated linear PID control for robot manipulators. Elsevier science Ltd. Automática no. 39, pp. 989-995, June 2003. [18] H. Khalil, Nonlinear Systems. 3rd. edition, Prentice Hall. USA c.2002 [19] A. Soria, R. Garrido, I. Vásquez, R. Vázquez. Architecture for rapid prototyping of visual controllers. Robotics and Autonomous Systems, Volume 54, Issue 6, 30 June 2006, Pages 486-495