A Full Analytical Solution to the Direct and Inverse Kinematics of the Pentaxis Robot Manipulator

Transcription

1 A Full Analtical Solution to the Direct and Inverse Kinematics of the Pentais Robot Manipulator Moisés Estrada Castañeda, Luis Tupak Aguilar Bustos, Luis A. Gonále Hernánde Instituto Politécnico Nacional Centro de Investigación Desarrollo de Tecnología Digital Av. Instituto Politécnico Nacional No. 1310, Mesa de Ota, Tijuana, B.C., Méico, 510. alumno. ipn. m laguilarb@ ipn. m lgonal@ citedi. m 014 Published b DIF U 100 ci@ http: // nautilus. ua. edu. m/ difu100cia Abstract In this paper, the kinematics of a 5-DOF robot manipulator was obtained. The direct kinematics was presented using the Denavit-Hartenberg convention and method. The inverse kinematics was obtained analticall and all possible solutions were found for each joint. The kinematic solution was verified eperimentall b programming a kinematic solver for the MoveIt! ROS R platform. Kewords: Kinematics, Robot Manipulator, 5-DOF, ROS 1. Introduction The PENTAXIS robot was developed in Centro de Investigación Desarrollo de Tecnología Digital (CITEDI-IPN in 1994 for industrial and educational purposes and since then has been acquired b other research Institutes in the region. Computation of direct and inverse kinematics plas an important role in trajector generation, path or motion planning, position, and motion control of full-actuated robots [1, ]. The direct-kinematics consists in finding the basecoordinates of the end effector of the robot given the joint variables. The inverse-kinematics problem is opposite to the direct-kinematics problem, that is, it consists in determining the joint variables of a robot given the orientation and posture for the end effector in base coordinates [3]. There eist man methods to find the direct kinematics. For a robot with few joints (up to three, the direct kinematics can be analticall obtained. For a robot with higher degrees-of-freedom, the Denavit-Hartenberg method [4] becomes suitable. On the other hand, the computation of the inverse kinematics is more complicated since several solutions (posture can be obtained. There eist several softwares that find or solve the inverse-kinematics problem as the default kinematic solver in Moveit! from ROS R [5] however, a solution cannot be found for certain Cartesian trajectories. The contribution of this work is towards a construction of a software platform for the PENTAXIS under ROS R where it will help to program and record industrial tasks as path planning contouring, and peak-and-place opera- DIF U 100 ci@. Revista electrónica de Ingeniería Tecnologías, Universidad Autónoma de Zacatecas

2 O 1 d 0 d 1 a 1 O d d 3 a O 3 O 6 O 5 O 4 Table 1. The D-H parameters of the PENTAXIS robot. Frame i θ i (d i ± 0.05 cms (a i ± 0.05 cms α i deg θ θ θ θ θ tions. O 0 Figure 1. Coordinate frames for the PENTAXIS robot. Figure. The PENTAXIS 5-DOF manipulator.. Anatom of the Pentais Robot The PENTAXIS is a 5-DOF robot manipulator with its joints of tpe revolute. When the robot is at its vertical pose, it will reach a height of 106 ± 0.05 cms if measured from its base to the tip of its end-effector which is a gripper. Ever joint, including the end-effector, is driven b a stepper motor. The coordinate frames for the manipulator is shown in Fig Forward Kinematics The Denavit-Hartenberg (D-H convention was used to derive the direct kinematics. The direct kinematics is a set of equations which is used to calculate the position and orientation of the end-effector from a given joint values b appling the D-H method to the coordinate frames depicted in Fig. 1. The D-H parameters are shown in Table 1. The transformation matri between two consecutive frames, denoted as T, is composed of the multiplication of the following homogeneous transformations matrices, denoted as A i : A i = Rot(, θ i Trans(0, 0, d i Trans(a i, 0, 0 Rot(, α i cos(θ i sin(θ i cos(α i sin(θ i sin(α i a i cos(θ i sin(θ = i cos(θ i cos(α i cos(θ i sin(α i a i sin(θ i 0 sin(α i cos(α i d i (1 where Rot(, θ i denotes the rotation of θ i around the ais, Trans(0, 0, d i is the translation of d i length, Trans(a i, 0, 0 is the translation of a i length, and Rot(, α i denotes the rotation of α i around the ais. For the PENTAXIS robot, the transformation matri from the base (frame 0 to the final effector (frame 6 is specificall epressed as n o a p 6 n T 0 6 = n A i = o a p o a, n = n n i=0 o a p o a. n o a ( Here, p = [p p p ] T R 3 is the position vector and n is the 3 3 orientation matri of the end-effector. The following set of equations are regarded as the direct kinematics for the PENTAXIS robot: n = c 34 c 1 c 5 s 1 s 5 (3 n = c 34 s 1 c 5 + c 1 s 5 (4 n = s 34 c 5 (5 o = c 34 c 1 s 5 s 1 c 5 (6 o = c 34 s 1 s 5 + c 1 c 5 (7 o = s 34 s 5 (8 a = s 34 c 1 (9 a = s 34 s 1 (10 a = c 34 (11 p = c 1 [a c + a 3 c 3 + (d 5 + d 6 s 34 ] (1 p = s 1 [a c + a 3 c 3 + (d 5 + d 6 s 34 ] (13 p = d 0 + d 1 a s a 3 s 3 + (d 5 + d 6 c 34 (14 where c i = cos(θ i, s i = sin(θ i, c i j = cos(θ i + θ j, s i j = sin(θ i +θ j, c i jk = cos(θ i +θ j +θ k, and s i jk = sin(θ i +θ j +θ k.

3 4. Inverse Kinematics The analtic inverse kinematics consists of finding θ 1 to θ 5 algebraicall from the set equations (3 ( Solutions for θ First solution From equations (1 and (13 the following solution is obtained: ( p θ 1 = atan. ( Second solution A second solution for θ 1 can be obtained from (10 and (9, that is, ( a θ 1 = atan. ( Third solution Another solution ma be derived, from equations (3 and (7 we first solve them for s 1 s 5, equate them, then substitute c 34 from (11 and finall solve for c 1 c 5 we have: c 1 c 5 = o a n 1 a. (17 Similar to (17, it is possible to obtain the following relation b now taking into account (4 and (6: p a s 1 c 5 = o a n 1 a. (18 Therefore, dividing (18 b (17 and solving for θ 1 we have a third solution: ( o a n θ 1 = atan. (19 o a n Fourth solution A fourth solution ma be obtained b appling a similar method for this last solution. Starting from (7 and (3, solving them for c 1 c 5, equate them, substitute c 34 from (11 and finall solve for s 1 s 5 we have: s 1 s 5 = n a o a 1. (0 We derive a similar equation as (0, b now taking into account (6 and (4 we have: c 1 s 5 = n a o a. (1 1 Dividing (0 b (1 and solving for θ 1 we have the fourth solution: Fifth solution ( n a o θ 1 = atan. ( n a o From equations (3 and (6 we solve for c 34 and then equate them, thus: s 1 = s 5n c 5 o, (3 similarl from equations (4 and (7 we have: c 1 = s 5n + c 5 o. (4 Therefore from the previous two equations the fifth solution is obtained: ( s5 n c 5 o θ 1 = atan. (5 s 5 n + c 5 o In this last solution there is a dependenc on θ 5, however there are solutions for θ 5 in which there is no dependence on an other unknown variable, as it is shown further in this document Sith solution When the arm of the robot is at the upright position the above solutions will have a ero b ero division. To avoid such singularit we set θ 5 to ero, therefore another solution is derived. From Eqs. (6 and (7 we have the following: therefore solution si is: 4.. Solutions for θ First solution s 1 = o, c 1 = o, ( o θ 1 = atan. (6 o From equations (8 and (5 we easil find the first solution for θ 5 : ( θ 5 = atan o. (7 n

4 4... Second solution Equations (3 and (7 are solved for c 1 c 5, equating them, substituting c 34 from (11 and solving for s 1 s 5 we have: s 1 s 5 = o a n 1 a. (8 Analogousl to the last equation we derive a second one, b now taking into account (6 and (4 we have: s 1 c 5 = o a n 1 a. (9 Dividing (8 b (9 and solving for θ we have the second solution: ( o a n θ 5 = atan. (30 o a n Third solution The derivation of the third solution for θ 3 is ver similar from this last solution. From (7 we solve for s 1 s 5, substitute that result into (3 and finall solve for c 1 c 5, we have: c 1 c 5 = n a + o a 1. (31 From (6 we solve for s 1 c 5, substituting that result into (4 and finall solving for c 1 s 5, we have: c 1 s 5 = n a o a. (3 1 Dividing (3 b (31 and solving for θ 5 we finall have the third solution: ( n a o θ 5 = atan. (33 o + a n Fourth solution From equations (3 and (4 we solve for c 34 and then equate them, thus: ( c1 n s 1 n s 5 =, (34 similarl from equations (6 and (7 we have: ( c1 o s 1 o c 5 =. (35 Therefore solution four is derived: ( c1 n s 1 n θ 5 =. (36 c 1 o s 1 o 4.3. Solutions for θ 3 From Eqs. (1 (14 the following terms ma be derived: a c + a 3 c 3 = Γ, a s + a 3 s 3 = Ω, (37 where Γ is an of the following terms derived from the combination of (1 and (13 with (5, (8, (9, (10: Γ = p + (d 5 + d 6 n = p (d 5 + d 6 o c 1 c 5 c 1 s 5 = p (d 5 + d 6 a = p (d 5 + d 6 a c 1 c 1 c 1 s 1 = p + (d 5 + d 6 n = p (d 5 + d 6 o s 1 c 5 s 1 s 5 = p (d 5 + d 6 a = p (d 5 + d 6 a, s 1 c 1 s 1 s 1 and Ω is an of the following terms derived from the combination of (14 with (3, (4, (6, (7, (11: Ω = ɛ + (d 5 + d 6 n + s 1 s 5 c 1 c 5 = ɛ + (d 5 + d 6 n c 1 s 5 s 1 c 5, = ɛ + (d 5 + d 6 o s 1 c 5 = ɛ + (d 5 + d 6 o + c 1 c 5 c 1 s 5 s 1 s 5 = ɛ + (d 5 + d 6 a, here ɛ = d 0 + d 1 p. The square of the sum of Eqs. (37 will give us: a + a a 3 (c c 3 + s s 3 + a 3 = Γ + Ω. (38 Because c c 3 + s s 3 = c 3, we can write: Γ + Ω a c 3 = a 3 a a, s 3 = ± 1 c 3. 3 In the above equation, we interpret the positive square root for when the robot elbow (joint 3 is up and the negative for when the elbow is down. Thus we have: 4.4. Solutions for θ ± 1 c 3 θ 3 = atan ( Γ +Ω a a 3 a a 3. (39 From the preceding Subsection, Eqs. (37 ma be epressed as follows: (a +a 3 c 3 c (a 3 s 3 s = Γ, (a 3 s 3 c +(a +a 3 c 3 s = Ω. (40 From above equations, we ma find a solution for s and c 3, and the combination of those solutions ield: ( (a + a 3 c 3 Ω (a 3 s 3 Γ θ = atan. (41 (a + a 3 c 3 Γ + (a 3 s 3 Ω

5 Figure 3. Flowchart of the Inverse Kinematic algorithm Solutions for θ 4 The solution of θ 4 can be straightforwardl found from the set of Eqs. s 34 = s 3 c 4 + c 3 s 4, c 34 = c 3 c 4 s 3 s 4, (4 where variables θ, θ 3 are known. B means of the man algebraic methods for solving linear equations, we find an equation for s 4 and another for c 4, dividing them ields: ( c3 s 34 s 3 c 34 θ 4 = atan. (43 s 3 s 34 + c 3 c 34 Here, c 34 and s 34 is given b: c 34 = n + s 1 s 5 = n c 1 s 5 = o s 1 c 5 = a c 1 c 5 s 1 c 5 c 1 s 5 = o + c 1 c 5 = [p (d 0 + d 1 + a s + a 3 s 3 ], s 1 s 5 d 5 + d 6 s 34 = n c 5 = o s 5 = = a c 1 = a s 1 = 1 ( p a c a 3 c 3 d 5 + d 6 c 1 1 ( p a c a 3 c 3. d 5 + d 6 s 1 5. Implementation and eperimental results In order to test our results we created a MoveIt! Kinematics Plugin [6] for the ROS platform, then four different Cartesian paths was given for the robot s end effector to

6 follow. The flowchart that represents the implemented algorithm for the inverse kinematics is shown in figure 3. We have let out the forward kinematics flowchart algorithm since its implementation is straightforward. A video has been recorded showing the robot following four paths: a rectangle, a circle, an ellipse, and a sine function. These paths were not possible to be calculated b the MoveIt! Motion Planner while using the default kinematic solver (the KDL kinematics numeric solver. The video is available in the following URL: mzr0w Conclusions This document presents the forward and inverse kinematics of a 5-DOF robot manipulator. The forward kinematics was derived using the Denavit-Hartenberg notation, where a set of twelve equations were obtained. For the inverse kinematics man solutions were given for each joint, for which in some cases there will be redundanc and for other cases there is onl a unique solution. The implementation for the direct kinematics is straightforward. However there ma be man kinds of implementations for the inverse kinematics. In this paper, we used a backtrack kind algorithm, then from the set of solutions that were obtained, a comparison with the forward kinematics would indicate the correct one. This implementation of an analtic kinematic solver is novel in regards that it deals with redundanc and singularities, and also it is fast enough to come up with the solution so that the end user is able to interact with the ROS R s Rvi visualier in real time. References [1] Rea N. Jaar. Theor of Applied Robotics. Kinematics, Dnamics, and Control, Springer, nd Edition, 010. [] J. Q. Gan, E. Oama, E. M. Rosales and H. Hu. A complete analtical solution to the inverse kinematics of the Pioneer robotic arm, Robotica, Cambridge Universit Press, Vol. 3, pages 13 19, 005. [3] F. L. Lewis, D. M. Dawson and C. T. Abdallah, Robot Manipulator Control: Theor and Practice, Marcel Dekker, nd Edition, New York, 006. [4] L. Sciavicco and B. Siciliano, Modeling and Control of Robot Manipulators, Springer, nd Edition, 010. [5] I. A. Sucan and S. Chitta, MoveIt!, April 014. [6] I. A. Sucan and S. Chitta, MoveIt!, org/wiki/kinematics/custom_plugin, April 014. [7] D. Xu, C. A. Acosta Calderon, J. Q. Gan and H. Hu. An Analsis of the Inverse Kinematics for a 5-DOF Manipulator, International Journal of Automation and Computing, Vol., pages , 005.