Task Space Robot Control using an inner PD loop. R. Garrido E. Canul. A. Soria Abstract— This paper presents a task-space robot controller composed of two nested loops. The inner loop corresponds to a Proportional Derivative joint controller and the outer loop consists of a Proportional Integral controller fed by task space measurements. Stability is studied by means of the Lyapunov method and a visual servoing application is considered for experimentally assessing the performance of the proposed controller.

I. INTRODUCTION Since Robotics appeared and took a good affinity with Automatic Control several years ago, there have been so far an increasing number of controllers to cope with two important problems in robot control: regulation and tracking. These controllers generally fall into two categories, namely joint or task space-based designs. However, since a task is specified in the robot task space, it seems more natural to develop control algorithms for this case. One of the first works was the seminal paper of Takegaki and Arimoto [1]. The authors assumed that the robot is not redundant and its Jacobian matrix is nonsingular at the desired position. This result was subsequently reported in [2]. References [3],[4] proposed a controller under the assumptions that the robot Jacobian matrix is full rank and the robot is non-redundant. The approach in [5] uses an energy shaping plus damping injection approach and achieves local asymptotic stability despite of the singularities on the task function Jacobian. Further contributions can be found in [6], [7] where the authors consider uncertainties on the robot Jacobian matrix and gravity torques. Asymptotic stability is assured provided that a bound on the estimated robot Jacobian matrix is known. The approach proposed in [7] uses a control law endowed with an integral action to compensate the robot uncertain gravitational torques. Later contributions [8],[9] propose task space feedback controllers using the transpose robot Jacobian matrix instead of its generalized inverse. As a particular case of the task space approach, visual servoing is considered for solving the position measurement problem in task space; therefore, the position is obtained directly from task space without requiring the robot kinematic inverse mappings. This approach is used in [8], [10]. In [10] the author proves asymptotic stability and robustness of the proposed controller under uncertainties in camera orientation and lens radial distortion. It is worth remarking that the aforementioned approaches rely on joint velocity and position task feedback and do not take into account joint position measurements. Considering robot joint position as feedback in task space controllers has several advantages. Firstly, 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. Secondly, 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 a task-space robot controller using two nested loops, an inner joint PD loop, and an outer PI loop. The Lyapunov method allows concluding stability of the proposed approach. Experimental results using a robot controlled through visual feedback asses the performance of the proposed approach. II. P RELIMINAIRES A. Review of Task Space Controllers Consider the controller proposed in [1] ˜  Ny q˙ + j (q)  = M W (q) Ns y

(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). Task Space controllers in [5], [8] and [10] shares the structure of controller (1). Note that if the task space position ˜ is not longer sensor fails, the proportional action M W (q) Ns y active and the robot motion may get unbounded.

Fig. 1.

Block Diagram of (1)

B. Robot Dynamics. In absence of friction and some other disturbances, the dynamics of a serial q-link rigid robot manipulator can be written as [11],[12]: P (q) q ¨ + F (q> q) ˙ q˙ + j (q) = 

Fig. 2.

(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  xW P (q) x  3. min {P (q)} kxk2 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 constan nF such that the following inequality is fulfilled [14]: 2 kF (q> q) ˙ qk ˙  nF kqk ˙

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  ¯ l (q) ¯ q maxlm ¯ CjCt ¯ . m C. Saturation Functions. In recent years the use of a special kind of saturation functions has become more common [2],[15]-[18]. The following 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.

Block Diagram of the proposed controller (8)

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 () for  6= 0. III. TASK -S PACE C ONTROLLER WITH AN I NTERNAL PD L OOP Consider the following control law 

= 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 Vl (hl ) and  l (l ) a scalar function and its time derivative respectively, according to Definition 1  (e) = ¢W ¡  1 (h1 ) = = =  p (hp ) is the saturation error vector.

Assume that an estimate MˆW (q) of the robot Jacobian matrix is available such that ° ° ° ° (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)

where: (7)

From (6) it follows that 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

0

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

h i 2 k (e)k2   W (e) Os Mˆ (q) Ol P (q) Ol MˆW (q) Os  (e) 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

(8)

Substituting (8) into (2) yields the closed loop system equation P (q) q ¨ + F (q> q) ˙ q˙ + j (q) = Ns MˆW (q) Os  (e) (9) Ng q˙ + j (qg )  Ns w Now, consider the following Lyapunov function candidate

1 Y (w>  (e) > q) ˙  min {P (q)} kyk2  ¸ 2 1 2 1 2 2 + 1   2 k (e)k + ns kwk 2 2 Hence, Y (w>  (e) > q) ˙ is definite positive if r 21 A 2

(13)

Taking the time derivative of (10) and using (9) yields i h Y˙ (w>  (e) > q) ˙ = ns q˙ W M W (q)  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)]

˙  (e))  q˙ W Ng q˙ (14) ]2 (q> q> 1 W W W Y (w>  (e) > q) ˙ = q˙ P (q) q˙  q˙ P (q) Ol Mˆ (q) Os  (e) 2 where +X (q)  X (qg ) + qW j (qg ) 1 W (10) +ns hS (e) > Ls i + wW Ns w ]2 (q> q> ˙  (e)) = ˙ (e) Os Mˆ (q) Ol P (q) q˙ 2 + (e)W Os M˙ (q) Ol P (q) q˙ p X © ª W where hLs > S (e)i = LWs S (e) = osl Vl (hl ), SW (e) = ˙  Ng q˙ + (e) Os Mˆ (q) Ol F W (q> q) (15) l=1 ¤W £ S1 (e1 ) = = = Sp (ep ) , Ls = gldj {Os } and q = Since the gravity torques are bounded, there exists a q  qg , with q and qg as the join position and desired joint positive constant 2 [7],[8] such that for ns large enough position respectively. First, to verify that (10) is positive the following inequality holds definite consider the following expression W nsW  (e) Os Mˆ (q) Ol MˆW (q) Os  (e) 1 W 1 W W W W 2 y P (q) y = q˙ P (q) q˙  q˙ P (q) Ol Mˆ (q) Os  (e)  (e) Os Mˆ (q) Ol [j (qg )  j (q)]  2 k (e)k 2 2 1 + 2 W (e) Os Mˆ (q) Ol P (q) Ol MˆW (q) Os  (e) (11) Finally, due to the fact that the robot Jacobian matrix is 2 bounded, it follows that where | is given in (7). Therefore, the Lyapunov function candidate (10) can be rewritten as  |]2 (q> q> ˙  (e))|  f0 kqk ˙ 2 > ;f0 A 0 (16) 1 As a consequence, Y˙ is upperbounded as follows 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 ˙ k (e)k Y˙ (w>  (e) > q) ˙  ns umax {Os } kqk 1 W W 2 +ns hS (e) > Ls i + w Ns w + q j (qg ) (12)  (min {Ng }  f0 ) kqk ˙  2 k (e)k2 (17) 2 Since the inertia matrix is positive definite the following where u is defined in (5). Inequality (17) can be further inequality is fulfilled [7],[9] rewritten as:

1 1 2 2 ˙  2 k (e)k Y˙ (w>  (e) > q) ˙   (min {Ng }  f0 ) kqk 2 2 1 p  [ (min {Ng }  f0 ) kqk ˙ 2 2 (uns min {Os }) p k (e)k]2 min {Ng }  f0 # " 1 (uns min {Os })2 2 k (e)k  2  (18) 2 min {Ng }  f0 Then, Y˙ (w>  (e) > q) ˙  0 if: min {Ng } A f0 2 2 (min {Ng }  f0 ) A (uns min {Os })

(19)

Asymptotic stability follows directly by applying La Salle’s invariance theorem [11],[12],[19]. 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.

Fig. 3.

Experimental Test Bed

IV. E XPERIMENTAL R ESULTS A. Experimental test bed To show the performance of the proposed controller, it was tested using a planar 2-link revolute joint robot under visual feedback. The robot is 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 loop was 1 pv. The experimental test bed is shown in Figure 3.

Fig. 4.

approximate values are ˆo1 = ˆo2 = 0=21 p. The gains of the proposed controller (3), (4) were chosen as follows1

B. Performance of the Proposed controller.

¸  ¸ 10 0 0=24 0 Ns = , Ng = 0 10 0 0=24  ¸  ¸ 0=15 0 1=6 0 , Ol = Os = 0 0=1 0 1=5  = 1 and vdw(h) = ±8 

The manipulator Jacobian matrix for the visual servoing application is given by Mˆ (q) = U () Mˆd (q)

(20)

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  ¸ ˆo cos t1 ˆo2 cos t2 Mˆ (q) = ˆ1 (21) 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

Desired position VS actual position in {=

(22)

Figure 4 shows the robot behavior with displacement on the { axis and Figure 5 depicts displacements on the | axis. V. C ONCLUSION This works presents a novel approach for robot manipulator control in task space. Its key feature is an inner joint 1{

and | in pixeles units

[14] R. Kelly and V. Santibáñez. Control de movimientos de Robots manipuladores. Pearson-Prentice Hall. Madrid c. 2003 [15] 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. [16] 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 [17] 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. [18] 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. [19] H. Khalil, Nonlinear Systems. 3rd. edition, Prentice Hall. USA c.2002 [20] A. Michele, C. Giorgio, C. Casalino. Task Space Robot Control: Convergence Analysis and Gravity Compensation via Integral Feeback. Proc. IEEE Int. Conf. on Decision and control, Kobe, Japan, December 1996, pp. 3032-3037. Fig. 5.

Desired position VS actual position in |=

PD position loop in cascade with an outer PI loop fed by task space measurements. This topology allows compensating disturbances at joint level and avoids uncontrolled robot motion if the task position sensor fails. Moreover, 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] P. Lancaster and M. Tismenetsky. The theory of matrices. Academic Press. Inc.