KINEMATIC CONTROL OF A SEVEN-JOINT MANIPULATOR WITH NON-SPHERICAL WRIST

Fabrizio Caccavale Pasquale Chiacchio Stefan0 Chiaverini Dipartimento di Informatica e Sistemistica

Universith degli Studi di Napoli “Federico II” via Claudio 21,80125 Napoli, Italy

E-mail: { caccavale,chiacchio,chiaverini} @disna.dis.unina.it

ABSTRACT

This paper focuses on real-time kinematic control of a seven-joint industrial robot manipulator having a non- spherical wrist. Despite the structure of the wrist, a two- stage inverse kinematics algorithm is devised which solves for the arm’s inner five joints in the first stage and for the outer two joints in the second stage. A damped least- squares technique with varying damping factor is adopted to provide robustness to kinematic singularities. An addi- tional constraint is introduced and a task priority strategy is adopted to solve redundancy. Experimental results are reported in a case study.

1. INTRODUCTION

Finding inverse kinematics solutions is of the utmost im- portance in order to transform motion specification as- signed by the user in the operational space into joint ref- erences for the robot control system.

First solving inverse kinematics and then performing joint space control is commonly known in the literature as kine- matic control, as opposed to operational space control where the control loop is closed directly on end-effector variables. One advantage of the former is the possibility of exploiting the joint servos of industrial robots which normally are not accessible to the user.

Most manipulators have a simple kinematic geometry so that closed-form solutions can be found to the inverse kine- matics problem. This is the case of a class of manipulators having a spherical wrist, i.e. when the three axes of the wrist revolute joints intersect at a common point [ 11.

Constructing a spherical wrist may complicate the me- chanical design, and thus a few robotic manipulators have a non-spherical wrist; typically the wrist revolute joint axes intersect only two-by-two. In spite of the mechani- cal simplicity, the inverse kinematics problem in general does not admit closed-form solutions and numerical tech- niques [2] are to be sought.

A computationally efficient technique for solving inverse kinematics of this class of manipulators is the two-stage

0-7803-2§§9-1/9§ $4.00 0 1995 HEEE

algorithm proposed in [3], where inner joint variables are solved in the first stage and the outer joint variables are solved in the second stage. Thanks to its closed-loop fashion [4], the algorithm is endowed with good tracking and steady-state performance.

Whenever the manipulator has redundant degrees of mo- bility, the inverse kinematics problem admits infinite so- lutions and some criterion has to be established to select a solution. One kind of redundancy occurs when a six-joint manipulator is placed on a platform or a track to acquire greater mobility in a larger portion of space. Closed-loop redundancy resolution techniques have been developed to exploit the available degrees of freedom to fulfill an addi- tional task besides the end-effector task. Among these, the task-priority strategy defines an order of priority between the two tasks by projecting the secondary task in the null space of the primary task Jacobian [5,6,7].

The two COMAU SMART-3 S installed in our labora- tory are examples of six-joint industrial robot manipula- tors with a non-spherical wrist whose axes intersect two- by-two. One of them is mounted on a sliding track, and as such it presents one redundant degree of mobility with respect to six-degree-of-freedom tasks.

The goal of this paper is to report an experimental in- vestigation of kinematic control for the above seven-joint redundant robot manipulator with non-spherical wrist. A two-stage closed-loop inverse kinematics algorithm is de- veloped which is based on the pseudo-inverse of a reduced order Jacobian matrix. Robustness to motion in the neigh- borhood of kinematic singularities is gained by resorting to a damped least-squares technique [8,9], where the damp- ing factor is adjusted [lo] on the basis of the smallest singular value of the Jacobian [ 111. Redundancy resolu- tion is then embedded in the schemes by utilizing the task priority strategy so as to ensure that the constraint task does not interfere with the end-effector task.

Results are illustrated to demonstrate real-time feasibil- ity of the overall kinematic control scheme, i.e. on-line trajectory generation, inverse kinematics with singularity robustness and redundancy resolution, and joint servo rou- tines.

50

2. SEVEN-JOINT INDUSTRIAL MANIPULATOR form

The SMART-3 S robot is manufactured by COMAU, the leading national robotics and automation company. Typi- cal industrial applications are arc welding, light handling, sealant dispensing, and water jet machining. The maxi- mum stroke of the manipulator is 1.4 m. The robot avail- able in our laboratory is mounted on a sliding track which enlarges the manipulator workspace.

Figure 1 - Sketch of the serial kinematic chain of the COMAU SMART-3 S robot.

Joint a; d; 19; a; 1 0 41 0 -7d2

4 0.11 0 44 -Id2

2 0.15 0.85 42 - ~ / 2 3 0.61 0 43 0

5 0 0.61 45 ~ / 2 6 0 -0.113 f&j -TI2 7 0 0.103 47 0

Table I - Denavit-Hartenberg parameters of the COMAU SMART-3 S robot.

Direct kinematics of the robot manipulator can be found by adopting the classical Denavit-Hartenberg convention. A sketch of the serial kinematic chain is displayed in Fig- ure 1 and its Denavit-Hartenberg parameters expressed in SI units are listed in Table I. Homogeneous transforma- tion matrices expressing relative position and orientation between consecutive links can be computed and the re- sulting direct kinematics equations can be expressed in the

51

where q is the (7 x 1) vector of joint variables, n, s, a are the unit vectors of the end-effector frame and p is the position vector of the origin of such frame with respect to the origin of the base frame.

In order to devise an inverse kinematics algorithm, the ma- nipulator differential kinematics must be considered. This can be characterized in terms of either the geometric or analytical Jacobian. With the former, the relationship be- tween the end-effector linear velocity vector i, and angular velocity vector w and the joint velocity vector q is given by

where J is the geometric Jacobian matrix.

In view of development of a two-stage inverse kinematics algorithm, a relevant point of the arm structure is the inter- section of the joint axes 6 and 7, denoted by the position vector p', that can be computed as

p' = p - d7a . (3)

It can be recognized that p' depends only on the joint vari- ables qp = ( dl 9 2 d3 9 4 d5 )T. Such vector also determines the orientation of the unit vector of frame 5, namely z5, which is subject to the geometric constraint [3]

aT%g = a s Q6. (4)

Differential kinematics equations involving the above two vectors can be established in terms of suitable analytical Jacobians as follows:

where

where

Furthermore, by denoting with qo = ( 8 8 87 )T the vec- tor of the two remaining joint variables, the following analytical Jacobians are of interest:

(9)

and

3. INVERSE KINEMATICS ALGORITHM

A two-stage inverse kinematics algorithm can be devised by solving first for qp and then for qo following the guide- lines in [3].

The first stage of the algorithm must guarantee not only that p' coincides with p& but also that the sixth-link ori- entation t 5 is compatible with the assigned a d through the geometric constraint (4). To the purpose, define the task-space vector

which in view of (5) and (7) allows to derive the velocity mapping

(12) x p = J P k P ) G P + E where

and

E = ( a;z5)

If p d is the desired end-effector position and n d , S d , a d describe the desired end-effector frame, the desired task- space vector is

where p: is the desired position for p' that, in view of (3), can be computed as p d - d T a d , and cos a3 is constant and equal to - 1. Accordingly, the algorithm error is

The joint velocity solution for the first stage can be thus computed as

q p = J ; ( q p ) ( * p , d -I- K p e p - e ) (17)

where K p is a suitable positive definite gain matrix.

In view of the kinematic properties of s , a, Js , Ja, the solution for qo can be computed via [3]

40 = k ( J ? ( q p , q 0 ) s d -t J F ( q p , 4 o ) a d ) (18)

where IC, > 0.

Joint positions are then found by numerical integration of the joint velocities qp and qo.

In the neighborhood of singularities of Jp, the pseudo- inversion in (1 7) becomes ill-conditioned and high veloc- ities may occur. Numerical robustness can be gained by resorting to a damped least-squares of the Jacobian [8,9]

J,* = J,T(JpJ,T + X 2 1 ) - '

where X is a damping factor achieving a trade-off between solution accuracy and solution feasibility [ 121. The damp- ing factor can be suitably tuned by defining a singular region in the neighborhood of the singularity on the basis of an estimate of the smallest singular value of Jp [ l l ] , i.e.

where 1 3 ~ is the estimate of the smallest singular value, and E defines the size of the singular region; the value of

is at user's disposal to suitably shape the solution in the neighborhood of a singularity [ 101.

4. REDUNDANCY RESOLUTION

The presence of redundancy can be utilized to introduce an additional constraint task; for the manipulator at issue in the present work which has a single degree of redundancy this is expressed by a scalar function 2,.

As a consequence, the above inverse kinematics algorithm must be modified to allow redundancy resolution. In order to avoid conflicts between the end-effector task and the constraint task, it is worth choosing a task priority strat- egy [5,6] which consists in projecting the joint velocity vector satisfying the constraint task onto the null space of the primary task.

h terms of the above two-stage algorithm, the solution to the first stage (17) is modified into

G~ = J J ( k p , d + Kpep - 5) + k C ( l - JJ Jp) j cec (21)

where I C , > 0, T

is the constraint Jacobian. and

e c = x c d - z c ( q ) (23)

is the constraint error between the desired and actual con- straint value.

The second stage of the algorithm remains the same as (18).

5. EXPERIMENTAL RESULTS

Experiments have been performed on the six-degree-of- freedom robot SMART-3 S with a non-spherical wrist whose axes intersect two-by-two. The robot is mounted on

52

a sliding track so as to realize a hnematically redundant structure. Each joint is actuated by brushless motors via gear trains; shaft absolute resolvers provide motor position measurements. The robot is controlled by an open version of the C3G 9000 unit whch has a VME-based architec- ture with a bus-to-bus communication link to a standard personal computer [ 131. The open version of the C3G 9000 controller allows seven different operation modes, of which one is that available on the industrial version of the controller. To implement the proposed inverse kinematics algorithms, we have used the operation mode number 6 in which the PC takes care of the joint reference trajectory generation and the control unit closes its standard PID joint servos at a 2 ms sampling rate. In this operation mode the joint servos are not accessible, nevertheless they are factory tuned.

The inverse kinematics algorithms (17,18) and (21,18) have been implemented as C modules running on a 486DX2/66 PC. At each sampling interval, the PC com- putes the joint references by Euler integration of the joint velocities (ia, 4,; the references are then passed to the servos through the communication link. For logging pur- poses, resolver readings and motor currents are down- loaded to the PC at each sampling instant.

Computation of the joint references requires computation of the task-space references, direct kinematics functions, and a single step of the inverse kinematics algorithm. In the case of algorithm (17,18), this takes 780ps; if algo- rithm (21,18) is used, 8 3 0 p are needed. Remarkably, the extra time needed by the latter algorithm is due to the redundancy resolution and is very small (about 6%) in comparison to the time needed by the former algorithm. In any case, the implementation shows that relatively short computation times are obtained with currently available low-cost hardware.

To tune the algorithm gains preliminary experiments have been performed by running the software on the PC without activating the communication with the robot control unit. As a result of these preliminary tests, the gain K p has been chosen equal to 10001 while k , has been set equal to 500 and k , equal to 5. It should be pointed out that a low value of 1, has been selected to achieve a smooth recovery, thus avoiding high joint velocities which could be infeasible for the robot actuators.

To test the above inverse kinematics algorithms, a task- space motion is assigned from the starting location

1 T % = ( 0 -1 1.288

Tf = ( - s i l ~ / 4 ) C O S ( T / ~ ) 0 0.594

COS( - ~ / 3 ) sin( - ~ / 3 ) 0 0.975

- s i n ( - ~ / 3 ) cos(-?r/3) 0 2.194 0 0 0 1

C O S ( T / ~ ) s i n ( ~ / 4 ) 0 0.975 to the desired final location

0 -1 1.288

0 0 0 1

The initial and final location of the end effector are interpo- lated using 5th-order polynomial time laws such that null initial and final velocities and accelerations are obtained. This results in a straight-line motion for the end-effector position which corresponds to a displacement of - 1.6 m along the z axis with an end-effector rotation of 77r/12 rad around the y axis. The duration of the motion is 6 s. Figure 2 reports the results obtained by using the inverse kinematics algorithm (17,18). To evaluate the perfor- mance of the system, we have chosen to consider the errors obtained in the task space. In detail, if qr denotes the joint references output by the inverse kinematics algorithm, the algorithm error is given by

e a , p = Pd - P ( ~ P )

for the positional part, and

1 ea , , = z ( " ( q r ) x n d + s(%) x s d + 8(QT) x s d )

for the orientation. If q denotes the vector of actual joint variables, the servo ermr is given by

e s , p = 24%) - P ( q )

for the positional part, and

for the orientation. Notice that servo errors are read in the task space but derive from joint space control actions.

"0 2 4 6 [sec1

Figure 2 -Experimental results for the algorithm (17,18): a) norm of the position errors (algorithm error: dashed; servo error: solid); b) norm of the orientation errors (algo- rithm error: dashed; servo error: solid).

53

It can be recognized that the algorithm error for the po- sitional part is about two orders of magnitude below the servo errors, while algorithm and servo errors on the orien- tation are comparable. This is due to the higher accuracy of the velocity mapping in (17), which is based on a Jaco- bian pseudoinverse, in comparison to the mapping in (1 8) based on a Jacobian transpose, which is not a velocity mapping.

IO-^ a I I

Figure 3 -Experimental results for the algorithm (21,lS): a) norm of the position errors (algorithm error: dashed; servo error: solid); b) norm of the orientation errors (algo- rithm error: dashed; servo error: solid).

0.25 I

i.e. the center of the moving track; a linear interpolation has been used to achieve a smooth null-space motion. The du- ration of the constraint-task is the same as the end-effector task.

To evaluate the performance of the system, we must con- sider the constraint-task error besides the errors obtained in the task space. Since the inverse kinematics algorithm is tuned to ensure low tracking errors on the primary (end- effector) task, constraint-task errors are substantially al- gorithm errors and are computed according to (23).

It can be recognized that the final desired value of the constraint is achieved with primary-task errors comparable to those obtained without redundancy resolution. During the manipulator motion, however, the constraint error is large since IC, is low and the task is demanding.

To test the redundancy resolution capabilities of the inverse kinematics algorithm in the presence of kinematic singu- larities a second experiment has been developed. The starting location of the manipulator is

1 0 -1 0 -0.113 0.599 0 0.801 1.852

-0.801 0 0.599 1.050 0 0 0 1

Tz= ( that corresponds to a kinematically singular configuration, and the robot arm must be reconfigured so as to leave the singularity. Therefore, the desired final location Tf is equal to the initial one, and the algorithm (21,18) is used with a suitable choice of the constraint task. It can be recognized that increasing 43 would help to escape the singularity; thus, we have assigned as the constraint task q3 to reach the final value - ~ / 2 rad from its starting value of -2.500. A linear interpolation has been used to achieve a smooth null-space motion. The duration of the constraint-task is 6 s. Figure 5 and 6 report the results obtained. It can be rec- ognized that in the starting phase of the motion the end- effector error is larger due to the singularity. However, the reconfiguration due to the constraint task allows better mobility of the arm structure and the tracking accuracy improves on both the end-effector task and the constraint task.

6. CONCLUSIONS

Figure 4 -Experimental results for the algorithm (2 1,lS): norm of the constraint error.

Figure 3 and 4 report the results obtained by using the inverse kinematics algorithm (21,lS). In this case, a con- straint task must be assigned besides the above end-effector task. We have assigned q1 to reach the final value 1.05 m,

This paper has studied the implementation of a kinematic control scheme for the industrial manipulator COMAU SMART-3 S. This is a six-joint robot with a non-spherical wrist whose axes intersect two-by-two. In addition, the manipulator is mounted on a sliding track which enlarges its workspace and gives kinematic redundancy.

A closed-loop inverse kinematics algorithm has been de- veloped to manage both the presence of the non-spherical wrist and the available redundant degrees of freedom. Ro- bustness to kinematic singularities has also been provided

5 4

by resorting to a damped least-squares technique. &ne- matic properties of the geometric structure of the robot arm have been exploited to lighten the computational burden of the algorithm.

7. ACKNOWLEDGEMENTS

This work was supported by Minister0 dell’universith e della Ricerca Scientifica e Tecnologica under 60% and 40% funds.

x IO“ a

*- 8. REFERENCES

Figure 5 - Experimental results for the reconfiguration task: a) norm of the position errors (algorithm error: dashed; servo error: solid); b) norm of the orientation errors (algorithm error: dashed; servo error: solid).

Figure 6 - Experimental results for the reconfiguration task: norm of the constraint error.

The inverse kinematics algorithm runs on a 486DX2/66 PC connected to the C3G 9000 control unit of the robot through a bus-to-bus communication link. The system realizes a kinematic control scheme at a 500 Hz rate.

Experimental results show the real-time feasibility of the kinematic control scheme. Remarkably, despite the com- plexity of the inverse kinematics algorithm developed - which both handles kinematic singularities and allows re- dundancy resolution-, relatively short computation times are obtained with currently available low-cost hardware.

[ 11 R.P. Paul and H. Zhang, “Computationally efficient kinemat- ics for manipulators with spherical wrists based on the homoge- neous transformation representation,” The International Journal of Robotics Research, vol. 5, no. 2, pp. 32-44 (1986).

[2] V.I. Lumelsky, “Iterative coordinate transformation procedure for one class of robots,” IEEE Transactions on Systems, Man, and Cybemetics, vol. 14, pp. 500-505 (1984).

[3] L. Sciavicco and B. Siciliano, “Coordinate transformation: A solution algorithm for one class of robots,” IEEE Transactions on Systems, Man, and Cybemetics, vol. 16, pp. 550-559 (1986).

[4] A. Balestrino, G. De Maria, L. Sciavicco, and B. Sici- liano, “An algorithmic approach to coordinate transformation for robotic manipulators,” Advanced Robotics, vol. 2, pp. 327-344 (1988).

[5] Y. Nakamura, H. Hanafusa, and T. Yoshikawa, “Task-priority based redundancy control of robot manipulators,” Int. J. Robotics Research, vol. 6, no. 2, pp. 3-15 (1987).

[6] A.A. Maciejewski and C.A. Klein, “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments,” The Intemational Joumal of Robotics Research. vol. 4, n. 3, pp. 109-117 (1985).

[7] P. Chiacchio, S. Chiaverini, L. Sciavicco, and B. Siciliano, “Closed-loop inverse kinematics schemes for constrained redun- dant manipulators with task space augmentation and task pri- ority strategy,” The Intemational Journal of Robotics Research, vol. 10, n. 4, pp. 410-425 (1991).

[8] Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control,” Trans- actions of the ASME Joumal of Dynamic Systems, Measurement, and Control, vol. 108, pp. 163-171 (1986).

[9] C.W. Wampler 11, “Manipulator inverse kinematic solutions based on vector formulations and damped least-squares meth- ods,” IEEE Transactions on Systems, Man, and Cybemetics, vol. 16, n. 1, pp. 93-101 (1986).

[lo] S. Chiaverini, 0. Egeland, and R.K. Kanestr@m, “Achiev- ing user-defined accuracy with damped least-squares inverse kinematics,” Proc. 5th Intemational Conference on Advanced Robotics (ICAR’91), Pisa, I, pp. 672-677, June 1991.

[ l l ] S. Chiaverim, “Estimate of the two smallest singular val- ues of the Jacobian matrix: application to damped least-squares inversekinematics,” Joumal ofRobotic Systems, vol. 10, pp. 991- 1008 (1993).

[12] S . Chiaverini, “Inverse differential kinematics of robotic manipulators at singular and near-singular configurations,” I992 IEEE Intemational Conference on Robotics and Automation - Tutorial on ‘Redundancy: Performance Indices, Singularities Avoidance, and Algorithmic Implementations, Nice, F, pp. 2-1-9, May 1992.

[13] E Dogliani, G. Magnani, and L. Sciavicco, “An open ar- chitecture industrial controller,” Newsletter of the IEEE Robotics and Automation Society, vol. 7, n. 3, pp. 19-21, 1993.

55