Application of Quaternion Interpolation (SLERP) to the Orientation Control of 6-Axis Articulated Robot using LabVIEW and RecurDyn

Transcription

1 Application of Quaternion Interpolation (SLERP) to the Orientation Control of 6-Axis Articulated Robot using LabVIEW and RecurDyn Jin Su Ahn 1, Won Jee Chung 1, Su Seong Park 1 1 School of Mechatronics, Changwon National University, South Korea Abstract - In general, the orientation interpolation of industrial robots has been done based on Euler angle system which can result in singular point (so-called Gimbal Lock). However, quaternion interpolation has the advantage of natural (specifically smooth) orientation interpolation without Gimbal Lock. This paper presents the application of quaternion interpolation, specifically Spherical Linear IntERPolation (in short, SLERP), to the orientation control of the 6-Axis articulated robot (RS2) using LabVIEW and RecurDyn. For the comparison of SLERP with linear Euler interpolation in view of smooth movement (profile) of joint angles (toqrues), the two methods are dynamically simulated on RS2 by using both LabVIEW and RecurDyn. Finally our original work, specifically the implementation of SLERP and linear Euler interpolation on the actual robot, i.e. RS2, is done using LabVIEW motion control tool kit. The SLERP orientation control is shown to be effective in terms of smooth joint motion and torque when compared to a conventional (linear) Euler interpolation. Keywords: Quaternion, Spherical Linear interpolation (SLERP), Euler Angle, Linear Euler Interpolation, 6-Axis Articulated Robot (RS2), LabVIEW, RecurDyn. 1 Introduction Nowadays, the performance of robot has been improved according to the development of robot control techniques. In some applications of robot, its performance is superior to human being s one. Even robots can be applied to the fields to which workers cannot be committed. For example, some welding robots can perform excellent welding better than human workers. These robots need accurate orientation interpolation with sooth movement. Besides welding, various tasks such as spray painting, sealing and handling require smooth orientation control. In general, the orientation interpolation of industrial robots has been done based on Euler angle system [1]. However, the orientation interpolation using Euler angles can result in singular point (so-called Gimbal Lock [2]), which can cause the malfunction of robots with systematic errors [3]. In addition, it can lead to undesirable results because it ignores interrelation between joint axes even in simple linear interpolation. However, quaternion interpolation has the advantage of natural (specifically smooth) orientation interpolation without singular point such as Gimbal Lock. Since Quaternion interpolation has been mostly used in 3-dimensional computer graphics, it has been applied to robot simulation instead of real robot control (or implementation) as shown in refs. [4-6]. In this paper, we will investigate on orientation control using quaternion interpolation for 6-Axis articulated robot (we will call it as RS2 hereinafter) which has been developed at our lab for research purpose. The robot control based on LabVIEW is briefly explained for the RS2 model. In addition, we will show our programming methods regarding both forward kinematics and inverse kinematics for RS2, which are needed for quaternion interpolation. In Section 3, Quaternion interpolation, specifically Spherical Linear IntERPolation (in short, SLERP) [7], is explained with linear Euler interpolation (which has been widely used for orientation control of industrial robots). For the comparison of SLERP with linear Euler interpolation in view of smooth movement of joint angles, the two methods are dynamically simulated on RS2 by using both LabVIEW and RecurDyn. In Section 4, our original work, specifically the implementation of SLERP on the actual robot, i.e. RS2, is done using LabVIEW motion control tool kit. Especially the linear Euler interpolation is also implemented on RS2, which is also compared with the SLERP in terms of torque. Finally Fig. 5 summarizes shows the structure of this paper. Concluding remarks will be made in Section 5. Fig. 1 Structure of this paper

2 2 Robot System (RS2) based on LabVIEW R 2.1 Introduction of RS2 Usually 6-axes manipulators which are widely used for welding, spray painting and so on, have payloads from 10 kg to 300kg. The payload over 500 kg belongs to Heavy Duty Handling Articulated Manipulator (abbreviated as HDHAM). In order to enhance both the control accuracy and the reliability of HDHAM, the synthetic technology including design, prototyping and control should be accompanied. For this purpose, in this paper, one fourth (1/4) model of HDHAM with 6 DOF (Degrees Of Freedom) (named as RS2) has been used as a preliminary step to manufacture the original model of HDHAM. The original HDHAM (2.4 m in height and 3.6 m in length) will be destined for handling payload of 600 kg. The RS2 shown in Fig. 2 is used for investigating the orientation control technology of original HDHAM in a laboratory. [8] For the control of RS2 system, LabVIEW is adopted as a graphical programming language that uses icons instead of lines of text to create applications. LabVIEW programs are called Virtual Instruments (VIs), because their appearance and operation imitate physical instruments, such as oscilloscopes and multimeters. LabVIEW contains a comprehensive set of tools for acquiring, analyzing, displaying, and storing data, as well as tools to help us troubleshoot code we write. Especially the LabVIEW hardware used in this paper is NI PXI-7350 Motion Controller, which sends commands to the servo drivers of Mitsubishi J2-Super series [9] for the motion control of RS2. position of the point are given and we have to calculate the angle of each joint. In our previous paper [11], we had solved forward and inverse kinematics solution for RS2. In this paper, we just show the LabVIEW R graphical program, which has developed in our lab, based on forward and inverse kinematics solutions for RS2. Forward kinematics program calculates the position and orientation of end-effector corresponding to input angle of each joint through the 0 homogeneous transformation matrix 6 T as shown in Fig 3. The advantage of developed program is that the homogeneous transformation matrix has been easily calculated only by modifying input angles. This forward kinematics routine of LabVIEW R is often called in the interpolation programs for RS2 which will be explained in the later section. Fig. 3 Forward Kinematics Program Fig. 2 RS2 System 2.2 Forward and Inverse kinematics of RS2 based on LabVIEW R In forward kinematics, the length of each link and the angle of each joint are given and we have to calculate the position of any point in the robot. Specifically, forward kinematics is computation of the position(x, Y, Z) and orientation(α, β, γ) of robot's end-effector. It is widely used in robotics. The orientation (α, β, γ) of robot means Euler angles [10]. In inverse kinematics, the length of each link and Fig. 4 Inverse Kinematics Program

3 In the meanwhile, inverse kinematics program calculates joint angles corresponding to input values of 6 DOF(X, Y, Z, α, β, γ). Fig. 4 shows a part of source routine of inverse kinematics for RS2, written in LabVIEW R graphical program. The inverse kinematics program is linked to interpolation programs as Sub VI type (in a format of subprogram LabVIEW R ) for both dynamic simulation of interpolation and real implementation of interpolation on RS2. In the interpolation program, the inverse kinematics program calculates the angle of each joint every sampling time (a few milliseconds). Especially the results of inverse kinematics program play an important role in generating command values of joint angles for NI PXI motion controller of LabVIEW R. 3 Dynamic Simulation using Linear Euler Interpolation and SLERP 3.1 Linear Euler interpolation The space of orientations can be parameterized by Euler angles. When Euler angles are used, a general orientation is written as a series of rotations about three mutually orthogonal axes in space. In general, Euler angles are widely used for orientation of robot. Using the equivalence between Euler angles and rotation composition, it is possible to change to and from matrix convention. In this paper, we used Z-Y-X Euler angle [12], where the rotation matrix R has been obtained from the homogeneous transformation matrix of forward kinematics program stated in section 2.2. In addition, the rotation matrix can be equivalently interchanged with Euler angles (α, β, γ) as follows: Fig. 5 LabVIEW R source program of linear Euler Interpolation Equation 4 shows that linear Euler interpolation program performs interpolation based on Euler angles of both start point r 0 and end point r 1.This program calculates the angle of each joint every sampling time by using the inverse kinematics program developed in section SLERP A quaternion has been introduced for the notation of orientation since it has simple notation of rotation as well as being convenient for the interpolation for orientation [7]. The quaternion can express itself into a rotational axis and rotational angle about the axis. The quaternion can be defined by Eq. (5) : q = w + (xi + yj + zk) (5) n x s x a x R = n y n z s y s z a y a z (1) Here x, y, z, w are real numbers while xi, yj, zk denote complex numbers. Due to the characteristics of complex numbers, it follows that where α = arccos(a z ), β = arctan2 a x, a y (3) γ = arctan2(n z, s z ) wherein nx, ny,. and az are asuumed to be given. The simple linear interpolation between two Euler angles is most obvious method. To develop interpolation program using Euler angles, linear Euler interpolation has been used in the LabVIEW R graphical program of Fig. 5 as follows [7] : i 2 = j 2 = k 2 = 1 ij = k, jk = i, ki = j, ijk = 1 Here it can be said that x, y and z denote the axis of rotation while w indicates the angle of rotation. Besides, any rotation matrix can be converted into a quaternion as follows [13] : n x s x a x 1 2y 2 2z 2 2xy 2wz 2xz + 2wy n y s y a y = 2xy + 2wz 1 2x 2 2z 2 2yz 2wx n z s z a z 2xz 2wy 2yz + 2wx 1 2x 2 2y 2 = q(w, (x, y, z)) (6) r 0 = (α 0, β 0, γ 0 ), r 1 = (α 1, β 1, γ 1 ) LinEuler(r 0, r 1, t) = r 0 (1 t ) + r 1 t (4) (0 < t < 1) Here w = n x + s y + a z (7)

4 x = s z a y 4w, y = a x n z 4w, z = n y s x 4w (8) In this paper, to develop orientation interpolation program, we have used Spherical Linear interption, i.e., SLERP proposed by Shoemaker [14], one of quaternion-based interpolation methods. SLERP has a geometric formula independent of quaternions, and independent of the dimension of the space in which the arc is embedded. Let q 1 and q 2 be the first and last points (specifically quternions) of the arc, and let t be the parameter, 0 t 1. Compute θ as the angle subtended by the arc, so that cos θ = q 1 q 2, the 4-dimensional dot product of the unit quaternions from the start point to the end point. Then SLERP can be expressed by equations (9) and (10): 3.3 Dynamic simulation of linear Euler interpolation and SLERP for RS2 In order to compare linear Euler interpolation with SLERP for RS2 by using RecurDyn (multi-body dynamic simulation software), first of all, the two interpolation LabVIEW R programs developed in subsections 3.1 and 3.2 calculate joint angles according to every sampling time under the same conditions of start and end points. The simulation results are shown in Fig. 7 where the blue and red colors denote linear Euler interpolation and SLERP, respectively. q 1 = w + (x 1 i + y 1 j + z 1 k) q 2 = w + (x 2 i + y 2 j + z 2 k) slerp(t; q 1, q 2 ) = q 1 sin((1 t)θ) + q 2 sin(tθ) sin θ (9) θ = cos 1 (q 1 q 2 ) (10) Figure 6 shows the LabVIEW R graphical source program of SLERP, which is more complicated than that of linear Euler interpolation program. The inputs of linear Euler interpolation program are Euler angles, while the input to SLERP are quaternions. For the comparative analysis of linear Euler interpolation with SLERP, the SLERP program needs the same conditions as linear Euler interpolation as follows. First, the SLERP program converts input quaternions into their corresponding input Euler angles by using equations (2), (3) and (6). Then this program performs SLERP interpolation based on equation (10) which results in quaternion outputs. Then the quaternion outputs are converted into Euler angles in the similar manner to the input Euler angles. Finally the SLERP program shown in Fig. 6 calls the inverse kinematics routine (shown in Fig. 4) to obtain joint angles from the Euler angles, under the assumption that the positions of end-effector trajectory are given between start and end points. Fig. 7 Simulated angle of each Joint based on linear Euler interpolation and SLERP Then the joint angles are applied to the RS2 model of RecurDyn so that linear Euler interpolation can be compared with SLERP from the viewpoint of smoothness of both joint torques and end-effector velocity. The dynamic simulation of RecurDyn aims at testing the virtual performance of the two interpolation methods before their real implementation on RS2. The 3-dimenional (3-D) model of RS2 has been constructed in RecurDyn by importing the 3-D RS2 model of Solid Works, as shown in the upper right of Fig. 8. Table 1 shows the maximum torque of each joint for linear Euler interpolation and SLERP. It can be noticed that the magnitudes of the maximum torques for joint 1, 5 and 6 of SLERP are much smaller than those of linear Euler interpolation. Moreover SLERP can result in smooth joint torque profiles in comparison with linear Euler interpolation, as shown in Fig. 9. In addition, Fig. 10 shows that the endeffector velocity profile of SLERP is more smooth than that of linear Euler interpolation. Consequently it can be stated that SLERP has the advantage of natural (specifically smooth joint profile with less torque) orientation interpolation without singular point, compared with linear Euler interpolation. Fig. 6 Spherical Linear Interpolation Program Source

5 Fig. 10 The velocity of RS2 end effector Fig. 8 Dynamic Simulation using RecurDyn Table 1 Maximum Torque of Each Joint (Unit: N mm) Joint Euler SLERP 1 2, , , ,895-50, ,723-4, , , Implementation of SLERP on RS2 using LabVIEW R Motion Control In order to implement the orientation control of both linear Euler interpolation and SLERP on RS2, we have developed their orientation interpolation programs based on LabVIEW R graphical program as shown in Fig. 11. This figure shows that the implementation of two orientation control flows on RS2 is organized in three parts. The first part is the orientation interpolation routines of both linear Euler interpolation and SLERP based on LabVIEW R graphical programs developed in previous section. Fig. 11 Flow chart of implementation of two orientation controls on RS2 Fig. 9 Calculated torque profiles of each joint using RecurDyn In the meanwhile, the second part is the position control routine through NI PXI-7350 Motion Controller with Mitsubishi J2 series servo drives and HC-MFC servo motors. Specifically RS2 can be controlled according to pulses sent by NI PXI-7350 Motion Controller. Finally, the third part is for collecting torque voltage of each servo motor (specifically servo drive) using NI PXI-6133 DAQ equipment. As shown

6 in Table 2, the maximum joint torques of SLERP are smaller than those of linear Euler interpolation, as expected. 5 Conclusion Table 2 Maximum Joint Torque Voltage Joint Euler SLERP (Unit: mv) In general, the orientation interpolation of industrial robots has been done based on Euler angle system which can result in singular point (so-called Gimbal Lock). However, it is well known that quaternion interpolation has the advantage of smooth orientation interpolation without Gimbal Lock. This paper presented the real application of quaternion interpolation, specifically Spherical Linear IntERPolation (in short, SLERP), to the orientation control of the 6-Axis articulated robot (RS2) using LabVIEW and RecurDyn. For the comparison of SLERP with linear Euler interpolation in view of smooth profiles of joint angles and torques, the two methods have been dynamically simulated on RS2 by using both LabVIEW and RecurDyn. Finally our original work, specifically the implementation of SLERP and linear Euler interpolation on the actual robot, i.e. RS2, has been done using LabVIEW motion control tool kit. The SLERP orientation control was shown to be effective in terms of smooth joint motion and torque when compared to a conventional (linear) Euler interpolation. 6 Acknowledgement The authors of this paper were partly supported by the Second Stage of Brain Korea21 Projects 7 References [1] K.S. Fu, R.C. Gonzalez and C.S.G. Lee, Robotics: Control, Sensing, Vision and Intelligence., McGraw-Hill (1987), pp 22 [2] Hoag, D. Apollo Guidance and Navigation: Considerations of Apollo IMU Gimbal Lock. Tech. Rep. E- 1344, MIT Instrumentation Laboratory, April ml [4] Purwar A. Jin Z. Ge Q. J."Computer Aided Synthesis of Piecewise Rational Motions for Spherical 2R and 3R Robot Arms," Annual mechanisms and robotics conference, DETC , pp , 2006 [5] Ahlers, S.G., and McCarthy, J.M., 2000, The Clifford Algebra of Double Quaternions and the Optimization of TS Robot Design, Applications of Clifford Algebras in Computer Science and Engineering, E. Bayro and G. Sobczyk,eds., Birkhauser. [6] Chung W.j, Kim K.J. Kim S.H."Steering Control Algorithm of an Inter-Block Locomotion Robot Using a Quaternion with Spherical Cubic Interpolation," Systemics cybernetics and informatics 2005, pp , 2005 [7] Dam B. Erik, Koch Martin, Lillholm Martin: Quaternions, interpolation and animation, Technical report DIKU-TR9815, Department of Computer Science, University of Copenhagen, 1998 [8] J. S. Ahn, W. J. Chung, OnDesign Prototype and Gain Optimization for Heavy Duty Handling Articulated Manipulator (HDHAM) with 6 DOF, The 14th World Multi-Conference on Systemics, Cybernetics and Informatics: WMSCI 2010, Volume 2 pp 174~179 [9] MITSUBISHI, General-Purpose Interface MR-J2S- A Servo Amplifier Instruction Manual. [10] Herbert Goldstein, Classical Mechanics (2nd ed.), Reading, MA: Addison-Wesley, ISBN [11] J. S. Ahn, W. J. Chung, "A Study on 6-Axis Articulated Robot Using a Quaternion Interpolation," KSMTE of Spring Conference 2010, pp 294~300, 2010 [12] Bonev I.A, Zlatanov D, Gosselin C.M, "Advantages of the Modified Euler Angles in the Design and Control of PKMs,"Parallel kinematics seminar, Development methods and application experience of parallel kinematics 2002, pp , 2002 [13] Mebius J.E, Derivation of the Euler-Rodrigues formula for three-dimensional rotations from the general formula for four-dimensional rotations, arxiv General Mathematics [14] Ken Shoemake, "Animating Rotation with Quaternion CurvesK," In Proceedings of the 12th Annual Conference on Computer Graphics and Interactive Techniques (SIGGRAPH '85), pp [3] Jones, E. M., and Fjeld, P. Gimbal Angles, Gimbal Lock, and a Fourth Gimbal for Christmas, Nov