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

Transcription

1 J. Cent. South Univ. (2012) 19: DOI: /s 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 , 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 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. Foundation item: Project supported by the Second Stage of Brain Korea 21 Projects Received date: ; Accepted date: Corresponding author: CHUNG Won-jee, Professor, PhD; Tel: ; wjchung@changwon.ac.kr

2 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 J. Cent. South Univ. (2012) 19: 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

3 J. Cent. South Univ. (2012) 19: 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 T, as 6 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

4 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: nx sx a x R ny sy (1) nz sz a z cos cos cos sin +sin sin cossin sin cos +cos sin cos cos sincos cos +sin sinsin sin cos +cos sin sin sin sin coscos cos (2) where arccos( az ) arctan 2( ax, ay ) arctan 2( nz, sz) J. Cent. South Univ. (2012) 19: And n x, n y, and a z 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 R graphical program of Fig. 5 as follows [7]: r0 ( 0, 0, 0), r1 ( 1, 1, 1) (4) LinEuler( r, r, t) r (1 t) rt(1 t 1) (3) 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) Fig. 5 LabVIEW source program of linear Euler interpolation

5 J. Cent. South Univ. (2012) 19: 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 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 [11] : 2 2 n x sx a y z xy wz xz wy x 2 2 ny sy ay 2xy 2wz 1 2x 2z 2yz 2wx 2 2 nz sz a z 2xz 2wy 2yz 2wx 12x 2y q( w,( x, y, z)) (6) where nx sy az 1 w (7) 2 sz ay ax nz,, ny x y x sx (8) 4w 4w 4w In this work, to develop orientation interpolation program, we have used Spherical Linear interption, i.e. SLERP proposed by Shoemaker, one of quaternionbased 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 q1 and q 2 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 cos q1q2 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): q1 w( x1i y1jz1k) q2 w( x2i y2jz2k) q1sin((1 t) ) q2sin( t ) slerp( tq ; 1, q2) sin (9) cos ( q q ) (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 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 R 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

6 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 J. Cent. South Univ. (2012) 19: 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 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. 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

7 J. Cent. South Univ. (2012) 19: 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. Fig. 8 View of developed dynamic simulation using RecurDyn R based on Lab-VIEW R 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 Table 1 Maximum torque of each joint Joint No. Maximum torque/(nmm) Euler SLERP Fig. 9 Calculated torque profiles of each joint using RecurDyn R : (a) Axis 1; (b) Axis 2; (c) Axis 3; (d) Axis 4; (e) Axis 5; (f) Axis 6

8 3414 J. Cent. South Univ. (2012) 19: 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 Fig. 10 Velocity of RS2 end effector 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 ( ). References Fig. 11 Flowchart of implementation of two orientation controls on RS2 Table 2 Maximum joint torque voltage Voltage/mV Joint No. Euler SLERP 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 [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: [3] JONES E M, FJELD P. Gimbal Angles, Gimbal Lock, and a Fourth Gimbal for Christmas [EB/OL]. < History/alsj/gimbals.html>, [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: [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, [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: [7] DAM E B, KOCH M, LILLHOLM M. Quaternions, interpolation and animation. DIKU-TR9815 [R]. Copenhagen: department of Computer Science, University of Copenhagen, [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: [9] SPONG M, VIDYASAGAR M. Robot dynamics and control [M]. Hoboken: John Wiley and Sons, 1989: [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: [11] SHOEMAKE K. Animating rotation with quaternion curves [C]// Proceedings of the 12th Annual Conference on Computer Graphics and Interactive Techniques, San Francisco, 1985: [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: vl. (Edited by HE Yun-bin)