The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

Implementation of Resolved Motion Rate Controller with 5-Axis Robot Manipulator Arm
Pradya Prempraneerach1*, Pasan Kulvanit2
1

Department of Mechanical Engineering, Rajamangala University of Technology Thanyaburi, Pathumthani, Thailand 12110 2 Department of Science Service (DSS), Ministry of Science and Technology, Ratchatewi, Bangkok, Thailand 10400 *Corresponding Author: Tel: (662) 5493430, Fax: (662) 5493432, E-mail: [email protected]

Abstract In this research work, an industrial-process prototype, particularly pick-and-place of machined parts, is developed by using an OWI-535 5-axis robot manipulator arm and controlling with Faulhaber motion control systems. To make an end-effector of the OWI-535 robot arm following desired positions with specified joint velocities, the inverse kinematics technique, known as the resolved motion rate controller, can help generating motion trajectories automatically. This inverse kinematics technique can be implemented with the Jacobian pseudo-inverse or Jacobian singularity-robust inverse. This technique does not require to inversely solve algebraic or geometric kinematic equations. Computation of the robot inverse kinematics is simulated in Matlab and then a motion control of the OWI-535 robot arm is performed by the Faulhaber Motion Manager. The pick-and-place motion of the OWI-535 robot arm agrees with its kinematics simulation very well. Keywords: inverse kinematics, robot manipulator arm, Jacobian pseudo inverse, Jacobian singularityrobust inverse 1. Introduction Nowadays, robot manipulator arms with a computer control are extensively used in most industrial factories to increase efficiency and to deal with repeated and/or hazardous tasks, such as in semiconductor wafer production processes [3]. Performing repetitive tasks continuously and repeatedly in the production line, human fatigue can lead to accident/injury or might result in a long-term health problem. On the other hand, robot manipulator arm can repetitively carry huge and heavy machine parts from one production line to anothers. Redundancy of the robot manipulator can provide many advantages in the case when the degree of freedom of a given task is less than that of the manipulator. When a joint failure occurs, the robot manipulator is still able to perform its task reliably [7]. Moreover, singular configuration of the robot manipulator can be avoided by control of redundancy using the Jacobian singularity-robust inverse [4].

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

In this work, we apply the redundancy control to inverse kinematics problem of the OWI-535 5-axis robot arm so that it can perform a pick-and-place operation. In Section 2, the structure and forward kinematics formulation of this robot arm are introduced along with its hardware description. The resolved motion rate control, which is a technique to solve the inverse kinematics problem by incorporating either Jacobian pseudo-inverse or Jacobian singularityrobust inverse, describes in Section 3. Then, comparisons of kinematics simulations and experimental results of the pick-and-place operation of the OWI-535 robot arm is shown in Section 4. 2. Structure and Forward Kinematics of OWI535 Robot Arm The structure of the OWI535 robot arm consists of 4 revolute joints connecting by three linkages and an end-effector, which is composed of two 4-bar linkage mechanisms. The length between each joint is as show in Fig. 1. Fig. 2 shows a limitation of angular rotation of 4 revolute joints, which can specify a reachable workspace of the OWI535 robot arm.

(a)

(b)

(c)

(d)

Fig 2 Limitation of angular rotation of revolute joint 1(a), joint 2 (b), joint 3 (c), joint 4 (d) [5]. The joints rotation angle of each joint and frame assignment of the OWI535 robot arm, used in forward and inverse kinematic calculations are illustrated in Fig. 3. Advantage of this frame assignment is that the z axes of frame 2 to 4 are parallel; as a result, a Jacobian calculation is straightforward.

Fig. 1 Side view of the OWI535 robot arm structure.

Fig. 3 A frame assignment for the OWI535 robot arm.

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

2.1 Forward kinematics calculation of OWI-535 robot arm The Denavit-Hartenberg parameters [1] for each joint and link are shown in Table 1, where L1, L2, L3 and H1 values are given in Fig. 1. Table. 1 Denavit-Hartenberg parameters of the OWI-535 robot arm di ai-1 i i i-1
1 2 3 4 5 0 90 0 0 0 0 0 L1 L2 L3 H1 0 0 0 0 1 2 3 4 0

Fig. 4 Hardware setup 3. Inverse Kinematics using Resolved Motion Rate Control The Resolved Motion Rate Control (RMRC) using either Jacobian preudo-inverse and Jacobian singularity-robust inverse is a technique for solving the inverse kinematics of robot arm. In the first step of this algorithm, initial joint angles of the OWI535 robot arm must be specified, then we can calculate the Jocobian matrix (J), which relates the angular speed of i joint ( i or i ) to the end-effector or 4th-link velocity (V4), as shown in Eq. (1) below
& x &4 x 1 x 2 x 3 x 4 1 & 2 (1) & = y y y y y 4 1 2 3 4 & z 3 &4 z 1 z 2 z 3 z 4 & 4

The forward kinematics from frame 0 to frame 4 can be computed by a transformation matrix T04 = T01T12T23T34 . These transformation matrices (T01 , T12 , T23 , T34 ) are given in Appendix. Given joint rotation angle, we can calculate the distance from the base or frame 0 to the end-effector or frame 4 using T04 . 2.2. Hardware and motion control description The main improvement of the OWI535 robot arm is to modify gear boxes, which drive all links. All original dc motors are replaced by 2224SR dc motors and IE2-512 magnetic encoders from Faulhaber [2]. Five MCDC 3006/06S Faulhaber motion control systems [2] can control five motors incremental position and speed, as shown in Fig. 4. All Faulhaber motion controllers can be connected with a computer through RS232 interface.

where

x 3 = (L2 sin( 2 + 3 ) + L3 sin( 2 + 3 + 4 ))cos(1 ) x 4 = (L3 sin( 2 + 3 + 4 ))cos(1 )

x 2 = (L1 sin( 2 ) + L2 sin( 2 + 3 ) + L3 sin( 2 + 3 + 4 ))cos(1 )

x 1 = (L1 cos( 2 ) + L2 cos( 2 + 3 ) + L3 cos( 2 + 3 + 4 ))sin(1 )

y 2 = (L1 sin( 2 ) + L2 sin( 2 + 3 ) + L3 sin( 2 + 3 + 4 ))sin(1 ) y 3 = (L2 sin( 2 + 3 ) + L3 sin( 2 + 3 + 4 )) sin(1 ) y 4 = (L3 sin( 2 + 3 + 4 )) sin(1 )

y 1 = (L1 cos( 2 ) + L2 cos( 2 + 3 ) + L3 cos( 2 + 3 + 4 )) cos(1 )

z 2 = (L1 cos( 2 ) + L2 cos( 2 + 3 ) + L3 cos( 2 + 3 + 4 )) + H1 z 3 = (L2 cos( 2 + 3 ) + L3 cos( 2 + 3 + 4 )) + H1 z 4 = (L3 cos( 2 + 3 + 4 )) + H1

z 1 = 0

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

In the second step, instead of solving inverse kinematic equation mathematically from the geometric of this 5-axis robot arm, the RMRC technique can compute joint rotation speed ( i ) at each time step, given the endeffector desired position and velocity. Joint angular velocity can be written as a linear combination of desired end-effector velocity and end-effector position error multiplying with the Jacobian inverse, as shown in Eq. (2). & = J 1 ( x & d + K ( xd x )) (2) where K is a gain coefficient for adjusting a convergence speed of the actual robot arm position. The larger the K coefficient is, the faster the actual end-effector position converges to the desire end-effecotr position. Using the forward kinematics, the end effectors position (X4) at different time steps can be computed from joints rotational angle by integrating & from Eq. (2) using the Euler integration. The RMRC technique employs the Jacobian inverse, J 1 . In this work, two types of Jacobian inverse are used in the RMRC technique. The Jacobian pseudo-inverse ( J # ) can be implemented easily from Eq. (3), however a singularity of matrix inverse might occur in some configurations (3) To avoid the singularity in Jacobian pseudoinverse computation, the Jacobian singularityrobust inverse ( J ) [4] in Eq. (4) incorporates an additional diagonal gain matrix such that the trajectory will deviate from the singular configuration.
J = J T JJ T + k1 I J # = J T JJ T

2 k 0 1 if < 0 and where k1 = 0 0 if 0 = det (JJ T ) and (0 , k 0 ) are constants. When 0 = 0 or k 0 = 0 , the Jacobian

singularity-robust inverse becomes the Jacobian pseudo inverse. Using Matlab, the motion trajectories of the robot arm can be generated after solving the inverse kinematics problem. Then, the RMRC technique is implemented on the OWI-535 robot arm and a parameters effect is examined. 4. Simulation Results of RMRC Technique Combining the RMRC technique with Euler integration, the inverse kinematics in Eq. (2) can be solved for joint rotation angles ( i ). In the following examples, integration time step (dt) of the Euler integration is 0.01 second. In the pick-and-place simulation, a object is assumed to originally locate at position of (15,15,3) cm, then the robot arm moves this object to a new position of (15,-15,3) cm. The initial joint angle ( 1 , 2 , 3 , 4 ) = (0.01,/2,0,0).

( )

(4)

Fig. 5 The trajectory of robot arm using the RMRC technique using J # with K = 2 at every 0.1 second from position of (0,0,31.1) cm to position of (15,15,3) cm.

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

Fig. 6 The trajectory of robot arm using the RMRC technique using J # with K = 2 at every 0.1 second from position of (15,15,3) to position of (15,-15,3) cm. Fig. 5 and 6 show Matlab simulation results of the RMRC technique using J # to calculate joint rotation angles at every 0.1 second. Then, we substitute joint angles into Jacobian in Eq. (1) to compute joints position in forward kinematics equations. In the first section (0 t 5 second), the robot arm moves from the initial position (x01,y01,z01) = (0,0,31.1) cm to the first desired position (xd1,yd1,zd1) = (15,15,3) cm, as shown in Fig. 5 and 7. In the second section (5 t 10 second), the robot arm moves from the first desired position (xd1,yd1,zd1) = (15,15,3) to the second desired position (xd2,yd2,zd2) = (15,15,3) cm as shown in Fig. 6 and 8. Both trajectories in the first and second sections in Fig. 5 and 6 use the gain, K, of 2. To speed up motion trajectories from initial to desired positions, K is set to 10, as shown in Fig. 7 and 8, thus, the end effector reaches the desired position in fewer time steps. In addition, the initial position (x01,y01,z01) is a singular configuration because it locates at a boundary of the reachable workspace. As a result, the RMRC

technique using J # causes the robot arm to approach (xd1,yd1,zd1) with the full extension of link 2 and 3, which is close to a singularity. However, using J in the RMRC technique, the motion trajectory is deviated from the singularity because the gain k1 in Eq. (4) prevents the inverse of (JJ T + k1 I ) to approach infinity near the singularity. Moreover, in the first section, all joints velocity is much smaller when comparing Fig. 7 with Fig. 5.

Fig. 7 The trajectory of robot arm using the RMRC technique using J with K = 10 at every 0.1 second from position of (0,0,31.1) cm to position of (15,15,3) cm.

Fig. 8 The trajectory of robot arm using the RMRC technique using J with K = 10 at every 0.1 second from position of (15,15,3) to position of (15,-15,3) cm.

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

Fig. 9 Using J # , end-effector trajectory (top) and position error (bottom) of the 1st section with K = 2.

Fig. 12 Using J , end-effector trajectory (top) and position error (bottom) of of the 1st section with K = 10. We also examine the end-effector trajectory as well as x-,y-,z-axis position error in the first section using J # with K = 2 and 10, as shown in Fig. 9 and 10 respectively, and using J with K = 2 and 10, as shown in Fig. 11 and 12. With a large gain, K, the end-effector position error quickly converges to zero. The RMRC technique using J # has larger endeffectors and joints velocities than that using # J . Especially, using J with K = 10 in the first section, there exist a large oscillation when this robot arm is close to the singularity initially, as shown in Fig. 10. However, the motion trajectory as well as joints velocity becomes smoother in Fig. 11 and 12. In the second section, when the robot arm is away from the singularity configuration, the motion trajectories using both J # and J are continuous and very smooth, as seen in Fig. 13 and 14. Notice that the position errors in x-,y-and z-axis converge to zero eventually.

Fig. 10 Using J # , end-effector trajectory (top) and position error (bottom) of the 1st section with K = 10.

Fig. 11 Using J , end-effector trajectory (top) and position error (bottom) of the 1st section with K = 2.

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

effector position accurately. Motion trajectories starts from the same initial position (x01,y01,z01), as shown in Fig. 15, and moves to the first desired position (xd1,yd1,zd1), as shown in Fig. 16, and then moves to the second desired position (xd2,yd2,zd2), as shown in Fig. 17.

Fig. 13 Using J # , end-effector trajectory (top) and position (bottom) of of the 2nd section with K = 2.

Fig. 15 End effector of OWI robot arm located at the initial position (x01,y01,z01).

Fig. 14 Using J # , end-effector trajectory (top) and position error (bottom) of of the 2nd section with K = 10. 5. Experimental Results of RMRC Technique The pick-and-place operation of the OWI-535 robot arm, similar to the simulation scenario, is implemented with Faulhaber Motion Manager software in VBscript language. The RMRC technique using the Jacobian singularityrobust inverse combining with forward kinematic are developed in Motion Manager to control the end-effector position. Because of a limitation of Motion Manager software that operates each individual motor sequentially, the end effector can not follow the desired speed. Nonetheless, the robot arm approaches the desired endFig. 16 End effector of OWI robot arm located at the first desired position (xd1,yd1,zd1).

Fig. 17 End effector of OWI robot arm located at the second desired position (xd2,yd2,zd2).

The First TSME International Conference on Mechanical Engineering 20-22 October, 2010, Ubon Ratchathani

6. Summary The inverse kinematics problem of the OWI-535 robot arm has been implemented with the resolved motion rate control (RMRC) technique both in Matlab for pick-and-place simulations and in Faulhaber Motion Manager software for motion trajectory of the actual OWI535 robot arm. The joints position of simulation results agrees well with that of the actual OWI robot arm. In the RMRC method, the Jacobian pseudo inverse might lead to the singular configuration, but the Jacobian singularity-robust inverse prevents the robot arm to approach the singularity. 7. Acknowledgement This work is supported by A New Researcher Scholarship of CSTS, MOST 2551, Coordinating Center for Thai Government Science and Technology Scholarship Students, National Science and Technology Development Agency, Thailand. 8. References [1] Craig, J.J. (1989). Introduction to robotics: mechanics and control, 2nd edition, ISBN: 0-20109528-9, Addison-Wesley Publishing Company, Inc. [2] Faulhaber, Instruction Manual for Motion Controller Series MCDC 3003/06S, Version 4th edition (2009), URL: http://www.faulhaber.com, access on 15/07/2010 [3] Kim D.I., Kim J.K., Sung H.K., and Kim S. (1995), Sensor-Based Navigation Control and Calibration of a Wafer-Handling Mobile Robot, paper presented in Thirtieth IEEE Industry Applications Conference Annual Meeting 1995, Vol. 3, 1995, pp 1922-1929.

[4] Nakamura N., Hanafuse H. (1985), Inverse Kinematic Solutions with Singularity Robustness for Robot Manipulator Control, paper presented in 85 ASME Winter Annual Meeting, Miami Beach, 1985, Vol. 15, pp. 193-204. [5] OWI, Robotic Arm Edge, Instruction Manual [6] Snchez A.J. and Martnez J.M. (2000), Robot-arm Pick and Place Behavior Programming System Using Visual Perception, paper presented in Proceedings. 15th International Conference on Pattern Recognition, Vol. 4, 2000, pp. 507-510. [7] Yoshikawa, T. (1990). Foundations of robotics: analysis and control, ISBN: 0-26224028-9, The MIT Press, Massachusetts. 9. Appendix The transformation matrices from joint 0 to 1 (T01 ) , from joint 1 to 2 (T12 ) , from joint 2 to 3 (T23 ) , from joint 3 to 4 (T34 ) can be written below.
cos(1 ) sin ( ) 1 1 T0 = 0 0 cos( 2 ) 0 2 T1 = sin ( 2 ) 0 cos( 3 ) sin ( ) 3 3 T2 = 0 0 cos( 4 ) sin ( ) 4 4 T3 = 0 0 sin (1 ) cos(1 ) 0 0 sin ( 2 ) 0 0 1 0 0 0 0 ; H1 1 0 1 0 ; 0 0 0 1 0 L1 0 0 ; 1 0 0 1 0 L2 0 0 1 0 0 1

0 cos( 2 ) 0 sin ( 3 ) cos( 3 ) 0 0 sin ( 4 )