Forward and Inverse Kinematics Analysis of Denso Robot

Home , Forward kinematics, Robot kinematics

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

Forward and Inverse Kinematics Analysis of Denso Robot Mehmet Erkan KÜTÜK 1*, Memik Taylan DAŞ2, Lale Canan DÜLGER1 1*Mechanical Engineering Department, University of Gaziantep Gaziantep/ Turkey E-mail: [email protected] 2 Mechanical Engineering Department, University of Kırıkkale

Abstract used Robotic Toolbox in forward kinematics analysis of A forward and inverse kinematic analysis of 6 axis an industrial robot [4]. DENSO robot with closed form solution is performed in This study includes kinematics of robot arm which is this paper. Robotics toolbox provides a great simplicity to available Gaziantep University, Mechanical Engineering us dealing with kinematics of robots with the ready Department, Mechatronics Lab. Forward and Inverse functions on it. However, making calculations in kinematics analysis are performed. Robotics Toolbox is traditional way is important to dominate the kinematics also applied to model Denso robot system. A GUI is built which is one of the main topics of robotics. Robotic for practical use of robotic system. toolbox in Matlab® is used to model Denso robot system. GUI studies including Robotic Toolbox are given with 2. Robot Arm Kinematics simulation examples. Keywords: Robot Kinematics, Simulation, Denso The robot kinematics can be categorized into two Robot, Robotic Toolbox, GUI main parts; forward and inverse kinematics. Forward kinematics problem is not difficult to perform and there is no complexity in deriving the equations in contrast to the 1. Introduction inverse kinematics. Especially nonlinear equations make the inverse kinematics problem complex. They may also Robot kinematics specifies the analytical study of the be coupled and have not got unique solutions. Thus motion of a robot manipulator. Formulating the reasonable solutions obtained mathematically may not solve the kinematics models for a robot mechanism is very problem physically [5]. Liu et al. applied geometric important in order to investigate the behaviour of approach for inverse kinematics analysis of a 6 dof robot industrial manipulators. There are two different spaces [6]. Qiao et al. used double quaternions to get solution for used in kinematics modelling of manipulators. They are inverse kinematics problem [7]. Nubiola and Bonev called Cartesian space and Quaternion space. The offered a simple and efficient way to solve inverse transformation between two Cartesian coordinate systems kinematics problem for 6R robots [8]. It is noticed that, can be decayed into a rotation and a translation. There are Artificial Intelligence (AI) methods are frequently used in a lot of approaches to represent rotation like Euler angles, inverse kinematics problem [9, 10, 11] in recent years. Gibbs vector, Cayley-Klein parameters, orthonormal matrices etc. Homogenous transformations based on 4x4 2.1 Forward Kinematics Analysis real matrices have been used dominantly in robotics [1]. Robotics Toolbox contains a lot of functions that are The forward kinematics problem is related between demanded in robotics and addresses fields such as the individual joints of the robot manipulator and the kinematics, dynamics, and trajectory generation. The position and orientation of the tool or end-effector. The Toolbox is convenient for simulation besides analyzing joint variables are the angles between the links for results from experiments with real robots. It is also an revolute or prismatic joints, and the link extension in the effective tool for education. The Toolbox is organized on prismatic or sliding joints [12]. A systematic way of a very common method of representing the kinematics and describing the geometry of a serial chain of links and dynamics of serial-link manipulators. It is described by joints was proposed by Denavit and Hartenberg and is the matrices. These include, in the basic case, the Denavit known today as Denavit-Hartenberg (DH) notation [2]. and Hartenberg parameters of the robot [2]. Any serial- The matrix A representing four movements is found by link manipulator can be configured by the user. A number postmultiplying the four matrices giving four movements of examples are provided for well-known robots such as to reach frame {j-1} to frame {j} in Figure 1. the Puma 560 and the Stanford arm [3]. Constantin et al.

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

The following table shows DH parameters of the Denso robotic arm necessary to derive the kinematics of the robot. Gripper is not included in the analysis.

Table 1. DH Parameters of the Denso Robotic Arm J o i n t L i m i t s Joint i i di ai i ( d e g r e e s )

1 q1 d1 0  /2 1 6 0 , 1 6 0

q 1 2 0 , 1 2 0 Fig. 1. DH representation of a general purpose joint-link 2 2 0 a2 0 combination q Transformation between two joints in a generic form [3] is 3 3 0 a3  /2 20 , 160 given in Eq. (1).

4 q4 d4 0  /2 160, 160

cossincossinsincosjjjjjjj  a  sincoscoscossinsin  a 5 q5 0 0  /2 1 2 0 , 1 2 0 j1 jjjjjjj (1) Aj  0sincos jjj d 6 q6 d6 0 0 3 6 0 , 3 6 0 0001

where d 1 = 0.125m, a2 =0.21m, a3 =-0.075m, d4 = 0.21m Denso Robot is a 6 Degrees-of-Freedom (DOF) robotic and d6 = 0.07m. manipulator. The link lengths are given in Figure 2(a). World frame and joint frames used in calculations and Transformation matrix for each joint can be obtained home position of Denso robot are shown in Figure 2b and by using Eq. (1). The parameters given in Table 1 are Figure 2c, respectively. substituted into Eq. (1) to find each of them. Six transformation matrices are presented in Eq. (2).

CS1100 CSa2222 C 0 SC00 SCa S 0 11 2222 A1  A 2  010 d 0010 1  0001 0001

CS00 CSa C0  44 3333  SC4400 SCa3333 S0   A4  A3  0100 010 d4  0001 0001

CS00 (a) Link lengths (b) World frame and joint frames 55 CS 00 66 SC5500  SC6600 A5  A   0100 6 0 0 1 d6  0001 0 0 0 1 (2)

where Cos and Sin are abbreviated to C and S, respectively. The total transformation between the base of the robot and the hand is; (c) Home position R TAAAAAAH  1 2 3 4 5 6 (3) Fig.2. General overview of DENSO

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

Transformation matrices for six axes given in Eq. (2) are To find the inverse kinematics solution for the 1st joint th postmultiplied in an order which is given in Eq. (3). This 1 as a function of the known elements, the 6 link equality is shown in Eq. (4). transformation inverse is postmultiplied as follows in Eq. noaPxxxx (9). noaP (4) yyyy noaPxxxx  A123456 AAAAA  noaPzzzz noaPyyyx  A AA AA AAxA1  1 (9) 0001 123466 5 6 noaP  zzzx 0001 where n (normal), o (orientation), a (approach) elements are for orientation and P (position) elements are position 1 where AA  I . I is identity matrix. In this case, the elements relative to the reference frame [12]. The 6 6 above equation is resulted in Eq. (10). elements of the matrix shown in left hand side of Eq. (4) are given in Eqs. (5, 6, 7 and 8). noaPxxxx  nSC  SSC(())((( C CC S SCCS SCC C C noaPyyyx 1 x 641412312365144123 A AAAAxA   12346 5 noaP ))())C1235123132 S SSC C SC C S zzzx nSC CSS(())((( S SC C SCCC SCS S S 0001 y 614412323165144123 C))()) C SSC S SC S S (10) 2315213312 nCSC CS(()())() SC CC SC SS SC SC S z 652323452332462332 The required multiplication in Eq. (10) is carried out and (5) it yields as Eq. (11).

'' '' '' C1   nCoSx 6 x 6 oC x 6  nS x 6 a x Pad x  x 6      oSx  C65((())()) SSC 1 441 CCCCSSS 2 31 2 351 2 31 CCSCCS 3 2 '' '' '' S1 nCoSy 6 y 6 oC y 6  nS y 6 a y Pad y  y 6 (11)     C(())64 C 141 SS 2C 31 C 2 CC 3 S S '' '' '' ''  nC oS oC  nS a Pad     z6 z 6 z 6 z 6 z z z 6  0 0 0 1 0 0 0 1 oCy  CCS61(())((( 44 SSSCCSS 1 2 32 3 1651 C 44 CSC 1 2 3 SSS    

))())C2 C 3 SS 152 C 1 33S SC 1 2 S S st th oz   SSCC6523( (  SS 23 )  CCCS 4523 (  CS 32 ))  CSCS 6423 (  CS 32 ) It is noticed that the elements located in 1 row & 4 column (abbreviated to (1, 4)) and 2nd row & 4th column (6) (abbreviated to (2, 4)) can be used in defining (these abbreviations are also used in the remaining part of the inverse kinematics analysis). All elements in the left-hand aSx  SSC51(())() 441 CCCCSSC 2 31 2 351 2 31 CCSCCS 3 2 a  SCS(  CSSS (  CCS ))  CCSS (  CSS ) side of the Eq. (11) are known. However, all of them are y 514 4123 231 5213 312 not used in calculation of  because of limitations of aCC CS()() SC SC SC S (7) 1 z 5232 34 52 33 2 space. Due to these reasons, they are not written. The Pd S( SSC (())())( CCCCSSC CCSCCSd CCS x 6 5 144 1231235 1231324 123 symbol ‘ '' ’ is used instead of them. From (1, 4) and (2, 4) )CCSaCCaCCCaCS S 1 322 1 23 1 2 33 123 elements, is found in Eq. (12).

Py  aCS221  dSCS 6514( (  CSSS 4123 (  CCS 231 ))  CCSS 5213 (  CSS 312 ))  1 Payy d 6 1 tan () pi (12) Paxx d 6 dCSS4 ( 213 CSS 312 ) aCCS 3231 aSSS 3123 P d  dCC()( ()( SS dCCC SS CSCS z 1 4 23 236 5 23 2345 23 The 1st link inverse transformation matrix is C)) Sa S a C S a C S 3 22 2 3 2 3 3 3 2 premultiplied by Eq. (10) to find the inverse kinematics (8) rd solution for the 3 joint (3 ) as a function of the known 2.2 Inverse Kinematics Analysis elements. It is given in Eq. (13).

n o a P The transformation process of the position and x x x x n o a P orientation of an end-effector from Cartesian space to A1 A A A A A A  1 xy y y x xA  1 (13) joint space is defined as inverse kinematics problem. 1 1 2 3 4 5 1n o a P 6 z z z x There are three solutions approaches; analytical, 0 0 0 1 numerical and semi analytical [5]. Analytical approach is

used herein.

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

1 where AA1 1  I . I is identity matrix. In this case the whree XRYRCos  Sin above equation is resulted in Eq. (14). 221  Y RXY Tan  noaPxxxx X  11noaPyyyx (14) AAAAAxxA  234516 noaP zzzx 0001 X and Y expressions are substituted in Eq. (21), it yields,

The required multiplication in Eq. (14) is carried out and RRZCos Cos( 33 )  Sin  Sin(   )  (23) it yields as Eq. (15). ''''''''''''()() a Cd Sa CCPa dSPl a 323423221615 xxyy Eq. (24) is obtained by using trigonometric addition '''''''''''' d Ca Sa SPdl a (15)  4233232215  zz formula in Eq. (23), ''''''''''''''''  Cos(3 )ZR / (24) 00010001

with the inverse cosine operation, SC a n d refer to Sin() and Cos() , 2323 2323  Cos(/)1 ZR equality is obtained. Then  is respectively. From (1, 4) and (2, 4) elements of the 3 3 equation, found as given in Eq. (25). Cos(/)1 ZR (25) a CdSa CCPa dSPl a ()() 3 323423221615 xxyy (16) d Ca Sa SPdl a 4233232215 zz The 2nd link inverse transformation matrix is premultiplied by Eq. (14) to find the inverse kinematics solution for the nd Right hand side of the Eq. (16) is known. They are 2 joint (2 ) as a function of the known elements in Eq. recalled as; (26). ACPa()() dSPl a noaP 1615 xxyy (17) xxxx noaP BPdlzz a 15 1111 yyyx (26) AA22 A 3 A 4 AAAxxA 5216  noaPzzzx Eq. (16) can be rewritten as below.  0001 a CdSa CA 32342322 (18) 1 dCa42332322 SaSB where AA2 2  I . I is identity matrix. In this case the above equation is resulted in Eq. (27). Having taken the squares of these expressions, they are noaPxxxx added to each other and it yields as;  111 noaPyyyx (27) 22222  aada a C CS Sa2() d 2() S CC SAB A345216 A AAAxxA 2342 32 232 232 4 2 232 23 noaP zzzx 0001

(19) Cos()3 Sin()3 The required multiplication in Eq. (27) is carried out, and then yields Eq. (28). The known parts are taken to the right hand side as shown in Eq. (20), '' '' ''''aC ''3 3 '' dSCCPad 432 ( () 16 ())() 16   2 SPa 2 x 16  xy d yz a  z Sd Pad 2 2 2 2 2 2a a Cos( )  2 a d Sin(  )  A  B  a  a  d (20) '' '' ''''dC '' ''( aSSCP () ())  ad ()  SP  ad   Cd P ad 2 3 3 2 4 3 2 3 4   4 3 3 32 16 16 2 16 x xy yz z '' '' '''''' '' ''''   Eq. (20) is then simplified and rewritten as Eq. (21), 0 0 0 10 0 01 XYZCos(33 )  Sin(  )  (21) (28) where

X 2 a23 a From (1, 4) element of the equation above,

(22) aCdS334321  CCPad( (x  x 61 )  SPa ( y  y  d 6221 ))  a  SdPad (  z  z 6 ) Y 2 a24 d Z A2  B 2  a 2  a 2  d 2 (29) 2 3 4 Eq. (29) can be rewritten as,

CKSKD()   (30) A triangle including X and Y can be formed to solve Eq. 2 1 2 2 (21) as an auxiliary technique. where

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

KCPadSPad11616(()()) xxyy noaPxxxx  KdPad() noaP 216 zz AAAA AAx ()1 yyyx (36) Da CdSa 456123 noaP 33432 zzzx 0001 A triangle including K1 and K2 can be formed to solve Eq. (30) as an auxiliary technique. The procedure applied in The required multiplication in Eq. (36) is carried out, and determination of 3 is carried out. So, all steps are not it is given in Eq. (37). explained. K '''''''''''''''' 2 2 1 2 where S K12  K   Tan   K '''''''''''''''' (37) 1   '''''''''''' Ca523231231 Ca SCa SS zxy  Then, 2 is found as given in Eq. (31)  00010001 1 2 Cos (DS / ) (31) From (3, 3) elements of the equation above,  expression can also be obtained from (2,4) elements 2 CaCaSCaSS (38) of Eq. (28), 523231231zxy

d433321616216 Ca SSCPa  dSPa(()())() dCdPaxxyyzz d Then,  is found as given in Eq. (39). (32) 5 1 (39) Eq. (32) can be rewritten as, 5 Cos (az C 23  a x S 23 C 1  a y S 23 S 1 )

SKCKE2324  (33) st nd rd The inverse transformation matrices of 1 , 2 and 3 where joints are premultiplied by Eq. (4) to find the inverse kinematics solution for the 4th joint ( ) as a function of KCPa31616(()()) dSPaxxyy d 4 KdPa() d the known elements. Multiplied matrix used in previous 416 zz step is also used in determination of  angle. It is given Ed Ca  S 4 4333 in Eq. (40). The elements (1, 3) and (2, 3) of the equation

are preferred in Eq. (42). A triangle including K3 and K4 can be formed to solve Eq. (33) as done in the previous steps. '''''''''''' C Sa Sa C Ca C S K 4 52323 123 1 zxy whereRKK Tan 221   3  34  '''''''''''' S4 Sa 511 Ca S yx K4   (40) '''''''''''''''' Then, 2 is found as given in Eq. (34).  00010001  1 Then,  is found as in Eq. (41). 2 Cos(/) ER (34) 4

ayx Ca11 S st nd rd 1 The inverse transformation matrices of 1 , 2 and 3 4  tan  (41) a Sa CCa CS joints are premultiplied by Eq. (4) to find the inverse zxy 2323123 1 th kinematics solution for the 5 joint (5 ). It is given in Eq. st nd rd (35). The inverse transformation matrices of 1 , 2 and 3 joints are premultiplied by Eq. (4) to find the inverse nx o x a x P x  th n o a P kinematics solution for the 6 joint (6 ) as a function of ()()AAA11A AAAAA AAA x y y y x (35) 123 12345 6 123 n o a P the known elements. Multiplied matrix used in previous z z z x two steps ( and ) is also used in determination of  0 0 0 1 54 6 angle. It is given in Eq. (35). The elements (3, 1) and (3, 1 where ()AAAAA1 2 3AI 1 2 3  . I is identity matrix. The 2) of the equation are given in Eq. (42), above equation is resulted in Eq. (36).

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

'''''''''''''''' ''''''''''''''''   C SS Sn Cn S'''''''' Cn SSo Co S Co SS 6 55 62323 123 12323 123 1 zxyzxy 00010001 (42)

Then, 6 is found as given in Eq. (43). ()o C  o S C  o S S   tan1 z23 x 23 1 y 23 1 (43) 6  nz C23 n x S 23 C 1 n y S 23 S 1

Inverse problem solution does not always give one solution; the same end effector pose can be reached in many different configurations [13]. Previous positions of the motors are fed to the program in each step in order to offer a solution to this problem in Denso robot system. The difference between calculated position and previous Fig. 3. Robotics toolbox with Denso position are obtained. The solution having the minimum The joint space trajectories can be calculated by inverse difference is selected. So the robot will not try to jump to kinematics and a simulation for robot can be done to move far positions; it will go to the reach the nearest solution. robot from initial position to final position in Cartesian Each solution obtained by the inverse kinematics analysis space. should be tested in order to determine whether or not they A straight line whose initial and final coordinates in X, Y, bring the end-effector to the desired position. Z coordinates are [0.35 0 0.4] m. and [0.2 0.1 0.5] m. respectively is given in Figure 4. It is desired to plan the

® path composed of N number of points; N= 50 in this 3. Robotics Toolbox in Matlab example. The Toolbox performs many functions for analyzing and simulation of arm type robotics in fields of kinematics, dynamics, and trajectory generation. The Toolbox is based on a very general method of representing the kinematics and dynamics of serial-link manipulators. These parameters are included in MATLAB® objects [3]. Designed model by robotics toolbox is given in Figure 3.

Fig. 4. Planned path

The Cartesian coordinates (X, Y and Z) of the path is given in Figure 5.

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

Fig. 5. Coordinates of the path Transformation matrices are calculated by using the path data. Inverse kinematics analysis is applied. The angular displacements of the links of the robot are obtained as in (b) Figure 6. Fig. 7. Initial and final configurations of the robot

4. Guide User Interface (GUI) Graphical User Interface development environment offers a set of tools in order to generate graphical user interfaces (GUIs) [14]. They greatly facilitate the operation of designing and building GUIs. A GUI example has been prepared for Denso robot including the forward kinematics. GUI is given in Figure 8. Push buttons, sliders, axes etc. can be added on it. Additions are being visible on the m. file simultaneously as a function. Robotics Toolbox is embedded to GUI. The results are compared with the expressions obtained in the analytical solution. It is proved that same results are obtained by the robotic toolbox and the analytical solution.

Fig. 6. Angular displacements of the robot links Initial and final configurations of the robot are shown in Figure 7 (a, b), respectively.

Fig. 8. Designed GUI example I Figure 9 shows the designed simulation program by MATLAB/GUIDE to create serial link robot (Link 1-6) and control the joint angles (q1, q2, q3, q4, q5 and q6). DH parameters can directly be changed. Figure 9(a) and Figure 9(b) show the initial condition of the robot and the (a) path generated with the given coordinates, respectively.

Proceedings of the International Symposium of Mechanism and Machine Science, 2017 AzC IFToMM – Azerbaijan Technical University 11-14 September 2017, Baku, Azerbaijan

robotics issues. Studies on adapting Denso robot for upper extremity rehabilitation are going on in Mechatronics Laboratory at Gaziantep University. References [1]. J. Funda, R. H. Taylor and Paul R.P, On homogeneous transforms, quaternions, and computational efficiency, IEEE Trans. Robot. Automation 6, pages 382– 388. 1990. [2]. J. Denavit& R. S. Hartenberg, A kinematic notation for lower-pair mechanisms based on matrices, Journal of Applied Mechanics 1. pages 215-221, 1955. [3]. P. Corke. Robotics, Vision and Control, Fundamental Algorithms in Matlab®. Springer, 2011. [4]. D. Constantin, M. Lupoae, C. Baciu, D.Buliga, Forward kinematic analysis of an industrial robot, (a) Proceedings of the International Conference on Mechanical Engineering (ME 2015), 90-95, 2015. [5]. S. Kucuk & Z. Bingul, The Inverse kinematics solutions of industrial robot manipulators, IEEE Conferance on Mechatronics, pages 274-279, (2004). [6]. Y. Liu, D. Wang, J. Sun, L. Chang, C. Ma,Y. Ge and L. Gao, Geometric approach for inverse kinematics analysis of 6-dof serial robot, IEEE International Conference on Information and Automation, pages 852- 855, 2015. [7]. S. Qiao, Q. Liao, S. Wei, H.Su, Inverse kinematic analysis of the general 6R serial manipulators based on double quaternions, Mechanism and Machine Theory 45, 193-199, 2010. [8]. A. Nubiola & I.A. Bonev, Geometric approach to (b) solving the inverse displacement problem of Calibrated Fig. 9. Designed GUI example II decoupled 6R serial robots, Transactions of the Canadian Society for Mechanical Engineering 38, 31-44, 2014. 5. Conclusion [9]. A.R.J. Almusawi, L.C. Dülger, and S. Kapucu, A new In this study, it is focused on determining the artificial neural network approach in solving inverse analytical solution of forward & inverse kinematics of the kinematics of robotic arm (denso vp6242), Computational Denso 6 axis robot which is available in the laboratory. Intelligence and Neuroscience, 2016. The equations obtained are reported. [10]. R. Köker, A genetic algorithm approach to a Robotics toolbox is provided a great simplicity to us neural-network-based inverse kinematics solution of about kinematics of robots with the ready functions on it. robotic manipulators based on error minimization, However, making calculations in traditional way is Information Sciences 222, 528-543, 2013. important in order to control the robot and to form a [11]. A. V. Duka, Neural network based inverse background for further studies. Toolbox is then used to kinematics solution for trajectory tracking of a robotic verify the results obtained by analytical way. The results arm, Procedia Technology 12, 20-27, 2014. are the same. User interfaces are the effective tools to [12]. S. B. Niku . Introduction to Robotics: Analysis, show many works in a compact way. Due to this reason, Systems, Applications. Prentice Hall, 2001 Guide User Interface is performed. It is possible to reach [13]. D. Manocha & J. F. Canny, Efficient inverse simultaneous transformation matrix, position & kinematics for general 6r manipulators, IEEE orientation of robot hand when angular displacements of Transactions on Robotics and Automation. 10, 648-657, the motors are changed one by one. 1994. This study is contained a part of theoretical and [14]. Creating Graphical User Interfaces, (2015). numerical kinematics analysis of Denso robot. It is Mathwork. performed to build a background study for rehabilitation