J. Cent. South Univ. (2012) 19: 3407−3414 DOI: 10.1007/s11771-012-1422-6

Realization of orientation interpolation of 6-axis articulated robot using quaternion

AHN Jin-su, CHUNG Won-jee, JUNG Chang-doo

School of Mechatronics, Changwon National University, Changwon 641-773, Korea

© Central South University Press and Springer-Verlag Berlin Heidelberg 2012

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 work presents the application of quaternion interpolation, specifically Spherical Linear IntERPolation (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 the view of smooth movement (profile) of joint angles (torques), 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. Key words: quaternion; spherical linear interpolation (SLERP); Euler angle; linear Euler interpolation; 6-axis articulated robot

Foundation item: Project supported by the Second Stage of Brain Korea 21 Projects Received date: 2011−12−28; Accepted date: 2012−06−24 Corresponding author: CHUNG Won-jee, Professor, PhD; Tel: +82−55−213−3624; E-mail: wjchung@changwon.ac.kr

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 that of human being. 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 smooth 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

three-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 work, we will investigate the orientation control using quaternion interpolation for 6-axis articulated robot (called 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. Quaternion interpolation, specifically Spherical Linear IntERPolation (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 the view of smooth movement of joint angles, two methods are dynamically simulated on RS2 by using both LabVIEW and RecurDyn.

The 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. Figure 1 summarizes the structure of the procedure of this work.

J. Cent. South Univ. (2012) 19: 3407−3414

3408

Fig. 1 Structure of procedure

2 Robot system (RS2) based on LabVIEW

Usually, 6-axis manipulators which are widely used for welding and spray painting, have payloads from 10 to 300 kg. The payload over 500 kg belongs to heavy duty handling articulated manipulator (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 work, 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 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 of 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 work 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.

The forward kinematics deals with the computation, 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 tip is described with Euler angles (, , ) [10]. In inverse kinematics, the length of each link and position of the point are given and we have to calculate the angle of each joint.

Fig. 2 RS2 System

J. Cent. South Univ. (2012) 19: 3407−3414

3409

In our previous work [11], we had solved forward

and inverse kinematics solution for RS2. In this work, we just show the LabVIEW graphical program, which has been 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 homogeneous transformation matrix 0

6T , as shown in Fig. 3. The advantage of the developed program is that the homogeneous transformation matrix has been easily calculated only by modifying input angles. This forward kinematics routine of LabVIEW is often called in the interpolation programs for RS2.

Meanwhile, inverse kinematics program calculates joint angles corresponding to input values of 6 DOF (X, Y, , , ). Figure 4 shows a part of source routine of inverse kinematics for RS2, written in LabVIEW graphical program. The inverse kinematics program is linked to interpolation programs as Sub VI type (in a format of subprogram LabVIEW) 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.

Fig. 3 View of developed forward kinematics program

Fig. 4 View of developed inverse kinematics program

J. Cent. South Univ. (2012) 19: 3407−3414

3410

3 SLERP based robot dynamic simulation using linear Euler interpolation

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 work, 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. In addition, the rotation matrix can be equivalently interchanged with Euler angles ( , , ) as follows:

x x x

y y y

z z z

n s a

n s a

n s a

@R (1)

cos cos cos sin +sin sin cos sin sin cos +cos sin cos

cos sin cos cos +sin sin sin sin cos +cos sin sin

sin sin cos cos co

s

(2) where

arccos( )

arctan 2( , )

arctan 2( , )

z

x y

z z

a

a a

n s

(3)

And nx, ny, and az are assumed 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Ⓡ graphical program of Fig. 5 as follows [7]:

0 0 0 0 1 1 1 1

0 1 0 1

( , , ), ( , , )

LinEuler( , , ) (1 ) (1 1)

r r

r r t r t r t t

(4)

Equation (4) shows that linear Euler interpolation

program performs interpolation based on Euler angles of both start point r0 and end point r1. This program calculates the angle of each joint every sampling time by using the inverse kinematics program developed in Section 2. 3.2 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)

Fig. 5 LabVIEW source program of linear Euler interpolation

J. Cent. South Univ. (2012) 19: 3407−3414

3411

where x, y, z and w are real numbers while xi, yj, zk, denote complex numbers. Due to the characteristics of complex numbers, it follows that i2= j2= k2= –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 [11] :

2 2

2 2

2 2

1 2 2 2 2 2 2

2 2 1 2 2 2 2

2 2 2 2 1 2 2

( ,( , , ))

x x x

y y y

z z z

y z xy wz xz wyn s a

n s a xy wz x z yz wx

n s a xz wy yz wx x y

w x y z

q

(6) where

1

2

x y zn s aw

(7)

, ,4 4 4

z y y xx zs a n sa n

x y xw w w

(8)

In this work, to develop orientation interpolation

program, we have used Spherical Linear intERPtion, i.e. SLERP proposed by Shoemaker, 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 1q and 2q be the first and last points (specifically quaternions) of the arc, and let t be the parameter, 0≤t≤1. Compute as the angle subtenede by the arc, so that

1 2cos q q is four-dimensional dot product of the unit quaternions from the start point to the end point. Then SLERP can be expressed by Eqs. (9) and (10):

1 1 1 1

2 2 2 2

1 21 2

( i j k)

( i j k)

sin((1 ) ) sin( )slerp( ; , )

sin

q w x y z

q w x y z

q t q tt q q

(9)

1

1 2cos ( )q q (10)

Figure 6 shows the LabVIEWⓇ 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 inputs 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 inputs Euler angles by using Eqs. (2), (3) and (6). Then, this program performs SLERP interpolation based on Eq. (10) which results in quaternion outputs. And 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. 3.3 SLERP based robot dynamic simulation of linear

Euler interpolation 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Ⓡ programs developed in and end points. The simulation results are shown in Fig. 7. Then, the joint angles are applied to the RS2 model of

Fig. 6 Spherical linear interpolation program source

J. Cent. South Univ. (2012) 19: 3407−3414

3412

RecurDyn® subsections 3.1 and 3.2 calculate joint angles according to every sampling time under the same conditions of start so that linear Euler interpolation can be compared in SLERP from the light 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 three-dimensional (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 presents the maximum torque of each joint for linear Euler interpolation and SLERP. It can be noticed that the magnitudes of the maximum torques for joints 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 end-effector 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. 4 Implementation of SLERP on RS2 using

LabVIEWⓇ 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Ⓡ graphical program, as shown in Fig. 11.

It is shown that the implementation of two orientation control flows on RS2 is organized in three parts. The first part is the orientation interpolation routine of both linear Euler interpolation and SLERP

Fig. 7 Simulated angles of each Joint based on linear Euler interpolation and SLERP: (a) Axis 1; (2) Axis 4; (c) Axis 3; (d) Axis 4; (e)Axis 5; (f) Axis 6

J. Cent. South Univ. (2012) 19: 3407−3414

3413

Fig. 8 View of developed dynamic simulation using

RecurDynⓇ

based on Lab-VIEWⓇ graphical programs developed in previous section.

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 in Table 2, the maximum joint torques of SLERP are smaller than those of linear Euler interpolation, as expected.

Table 1 Maximum torque of each joint

Maximum torque/(Nmm) Joint No.

Euler SLERP

1 2 209 520

2 147 578 146 293

3 –42 895 –50 393

4 –4 723 –4 577

5 2 025 871

6 –4 591 –978

Fig. 9 Calculated torque profiles of each joint using RecurDynⓇ: (a) Axis 1; (b) Axis 2; (c) Axis 3; (d) Axis 4; (e) Axis 5; (f) Axis 6

J. Cent. South Univ. (2012) 19: 3407−3414

3414

Fig. 10 Velocity of RS2 end effector

Fig. 11 Flowchart of implementation of two orientation

controls on RS2

Table 2 Maximum joint torque voltage

Voltage/mV Joint No.

Euler SLERP

1 2 209 520

2 147 578 146 293

3 –42 895 –50 393

4 –4 723 –4 577

5 2 025 871

5 Conclusions

This work presents the real application of quaternion interpolation, specifically Spherical Linear IntERPolation, 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 the 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, the implementation of SLERP and linear Euler interpolation on the actual robot, RS2 has been 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 conventional linear Euler interpolation

Acknowledgment

The authors of this paper were partly supported by

the Second Stage of Brain Korea 21 Projects. This research was supported by Basic Science Research Program through the National Research Foundation of Korea (NRF) funded by the Ministry of Education, Science and Technology (2011-0013902).

References

[1] FU K S, GONZALEZ R C, LEE C S G. Robotics: Control, sensing,

vision and intelligence [M]. New York: McGraw-Hill, 1987:22.

[2] HOAG D. Apollo guidance and Navigation: Considerations of apollo

imu gimbal lock [M]. Canbridge: MIT Instrumentation Laboratory,

1963: 1–64.

[3] JONES E M, FJELD P. Gimbal Angles, Gimbal Lock, and a Fourth

Gimbal for Christmas [EB/OL]. <http://www.hq.nasa.gov/office/pao/

History/alsj/gimbals.html>, 2002.

[4] PURWAR A, JIN Z, GE Q J. Computer aided synthesis of piecewise

rational motions for spherical 2R and 3R robot arms [C]// Annual

Mechanisms and Robotics Conference, Las Vegas, 2006: 1209–1222.

[5] AHLERS S G, McCARTHY J M. The clifford algebra of double

quaternions and the optimization of Ts robot design, applications of

clifford algebras in computer science and engineering [M],

Birkhauser Inc, 2000.

[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 [J]. Systemics Cybernetics and Informatics 2005:

374–379.

[7] DAM E B, KOCH M, LILLHOLM M. Quaternions, interpolation

and animation. DIKU-TR9815 [R]. Copenhagen: department of

Computer Science, University of Copenhagen, 1998.

[8] AHN J S, CHUNG W J. On design prototype and gain optimization

for heavy duty handling articulated manipulator (hdham) with 6 DOF

[C]// The 14th World Multi-Conference on Systemics, Cybernetics

and Informatics: WMSCI 2010, Orlando, 2010: 174–179.

[9] SPONG M, VIDYASAGAR M. Robot dynamics and control [M].

Hoboken: John Wiley and Sons, 1989: 20–70.

[10] AHN J S, CHUNG W J. A Study on 6-axis articulated robot using a

quaternion interpolation [C]// KSMTE of Spring Conference 2010,

Seoul, 2010: 294–300.

[11] SHOEMAKE K. Animating rotation with quaternion curves [C]//

Proceedings of the 12th Annual Conference on Computer Graphics

and Interactive Techniques, San Francisco, 1985: 245–254.

[12] MEBIUS J E. Derivation of the Euler-rodrigues formula for

three-dimensional rotations from the general formula for

four-dimensional rotations [J]. arXiv General Mathematics, 2007:

0701659vl.

(Edited by HE Yun-bin)