Efficient Closed-Form Solution of Inverse Kinematics for a Specific Six-DOF Arm

Transcription

1 International Journal of Control, Automation, and Sstems (1) 1 Efficient Closed-Form Solution of Inverse Kinematics for a Specific Si-DOF Arm Thanhtam Ho, Chul-Goo Kang*, and Sangoon Lee Abstract: Inverse kinematics solutions for multi-dof arms can be classified as analtical or numerical. In general, analtical solutions are preferable to numerical solutions because analtical ones ield complete solutions and are computationall fast and reliable. However, analtical closed-form solutions for inverse kinematics of -DOF arms rarel eist for real-time control purpose of fast moving arms. In this paper, we propose a fast inverse kinematics algorithm with a closed-form solution for a specific -DOF arm. The proposed algorithm is verified using simulation modules developed b us for demonstrations. Kewords: Robotic arm, Inverse kinematics, Closed-form solution, Simulation. 1. INTRODUCTION Inverse kinematics plas a ke role in robotics and computer animations. Given the pose (position and orientation) of the end-effector, inverse kinematics problem corresponds to computing joint variables for that pose. Inverse kinematics solutions for -dergee-of-freedom (DOF) arms ma be characterized as analtical or numerical[1]. Analtical solutions can be further subdivided into geometr-based closed-form solutions[] and algebraic-elimination-based solutions[]. In general, closed-form solutions can onl be obtained for or less than -DOF sstems with a specific structure. Solutions based on algebraic elimination epress joint variables as solutions to a sstem of multivariable polnomial equations, or epress a single joint variable as the solution to a ver high degree polnomial and determine the other joint variables using closed-form computations. In contrast to the analtical solutions, numerical approaches iterativel converge to a solution based on an initial guess[]. In numerical approaches, computation time to converge ma var, and thus numerical solutions are not appropriate to fast real-time control applications even if the are applicable to ill-posed sstems. In general, analtical solutions are preferable to numerical ones for real-time control applications because analtical ones ield all solutions (completeness) and are computationall fast and reliable. In some cases, partl closed-form and partl numerical solutions have been tried []. Manuscript received Sep., 1; revised Aug. 9, 11; accepted Jan., 1. Recommended b Editorial Board member Shinsuk Park under the direction of Editor-in-Chief Jae-Bok Song. This work was supported b Konkuk Universit in 1. Thanhtam Ho and Sangoon Lee are with Dept. of Mechanical Design and Production Engineering, Konkuk Universit, Gwangjin-gu, Seoul 1-71, Korea ( thanhtam.h@gmail.com). Chul-Goo Kang (corresponding author) is with Dept. of Mechanical Engineering, Konkuk Universit, ( cgkang@konkuk.ac.kr), Corresponding author The eistence of the closed-form solution depends on the kinematic structure of the arm. Pieper [] showed that the -DOF manipulator with a spherical wrist has a closedform solution. Man researchers have obtained closedform solutions for inverse kinematics of -DOF manipulators including Lee et al.[], Kang [7] and others [8-1] for -DOF PUMA robots, and Schilling [11] for a - DOF Intellede T robot. However, these solutions are ones for industrial manipulators that are different in configuration from the human-like arm shown in Fig. 1. Fast and efficient closed-form solution for the DOF arm such as Fig. 1 rarel eists for real-time control purpose of fast moving arms. Among earlier works on anthropomorphic arms, Tolani et al.[1] considered inverse kinematics of 7-DOF anthropomorphic limbs, and the simplified the arm structure to two segments including the upper arm and the forearm while shoulder blade is neglected. The simplification makes the inverse kinematics solvable but the solution is not able to appl to man robotic arms where shoulder blades are included. Also, Asfour et al.[1] derived a closed-form solution of the inverse kinematics for a 7-DOF arm of a humanoid robot ARMAR. After selecting a specific value of the z ais of the elbow, the determined joint angles b matri equations using decomposition approach. This paper presents an efficient (i.e., fast and reliable) closed-form solution of inverse kinematics for a -DOF arm similar to human arm structure. Previousl for the commercial arm b Robot and Design Co., Ltd. in Fig. 1, a pseudo-inverse numerical solution has been used for inverse kinematics of the arm, and average converging time was reported to be about 1 ms. This is a big computational burden for real-time control of the fast moving arm, and, consequentl, sampling frequenc of the feedback control sstem becomes quite low to control such a robot arm where high precision and fast response are the most primar requirements. Differentl from the work b Asfour et al.[1], we obtained joint angles b geometric relationships, and thus our solution is computationall faster than the Asfour s solution.

2 Submission to International Journal of Control, Automation, and Sstems. KINEMATIC STRUCTURE The robotic arm considered in this paper has -DOF motion as shown in Fig. 1. Even if the human arm has redundanc in motion, the basic -DOF motion of it is similar to the motion of this robot arm. The structure of the arm is simplified to four segments which represent the shoulder blade, the upper arm, the forearm and the hand. The base frame is attached at the origin of the shoulder blade while the last frame, the tool frame z of the hand, is attached at the joint between the forearm and the hand, the wrist. The chosen coordinate frames are peculiar in the sense that the origin o of the frame z lies at the same point with the origin o (this point is denoted b S), the 1 origin o lies at the same point with the origin o (this point is denoted b E), and the origin o lies at the same point with the origin o (this point is denoted b W). The forward kinematics problem for this robot arm is solved as usual using the DH parameters and homogeneous transformation matrices A i for each joint i. Since the whole si joints are revolute, the forward kinematics solution is given b r r r r r r r r T A1 A A A A A r1 r r r 1 (1) Fig. 1. Picture of the robot arm (Robot and Design Co, Ltd.) o z z 1 o o 1 H z 1 o1 o1 o S o o o o E z o o W z z o Fig.. The coordinate frames and the home configuration The actual motor positions of the arm are shown as clinders in Fig., but the coordinate frames and home configuration of the robot arm are chosen as the ones in Fig. to fit the Denavit-Hartenberg (DH) convention, in which the origin o and o are placed at o 1 and o respectivel. The chosen home configuration is natural since the shoulder lies in a horizontal line while the upper arm and the forearm lie in a vertical direction as in human arms. In the paper, l implies shoulder length, 1 l does upper arm length, and l does forearm length. z The position of the end-effector (i.e., wrist) is observed from the transformation matri T to be l c1c c s1s s c1sc lc1s o l s1c c c1s s s1sc ls1s () l s c s c c l c l 1 and the orientation of the hand is observed as rotation matri which is the first submatri of T. Notice that the notation s i means sin( i ) and c i means cos( i ). In the paper, lower-case bold face characters represent vectors epressed in component forms, and left superscript notation represents the coordinate frame with which the vector is epressed.. CLOSED-FORM SOLUTION Inverse kinematics problem is equivalent to the problem of solving the sstem of nonlinear equations obtained from the forward kinematics problem. Unfortunatel the general analtical solution for this problem does not eist. We present a closed-form solution that is computationall fast and reliable. Compared to the Asfour s work [1], we use different constraint equations to obtain the elbow pose, and also use geometric relationships to obtain and instead of matri inversions. The calculation process is composed of two steps. First the position of the elbow position E is determined b solving the geometric constraints, and then each joint angle is determined using the calculated elbow position E. Before the elbow position is computed, the position and orientation of the origin o at the wrist are etracted from

3 International Journal of Control, Automation, and Sstems (1) the given data. Let H be the position of the end point on the hand and R be the orientation of the coordinate frame z. From the geometric relationship, the position vector of the wrist epressed in the frame z is etracted from the given data to be as in (). W H W l W H z W z H j () where l is the length of the hand. In (), j is the unit vector of the ais epressed in the base frame. The unit vector j can be etracted as the second column of the orientation matri R. R i j k () The elbow point E is obtained b solving the sstem of constraint equations. Since the point S is fied and the lengths of the upper arm and forearm are constants, the point E can be placed on a circle as shown in Fig.. Due to the geometric relationships, one easil sees that the point E must satisf the set of constraints in (). k WE WE l SE l S () W k W k z zw kz W W z zw l S S z zs l To solve the simultaneous equations (7) convenientl, we define temporar variables t W t W tz z zw and a vector from W to S as T (7) (8) r a b c with r a b c (9) and another temporar variable k 1 as l k 1 l r. (1) Then one can epress the variable t and t in terms of t z. from the first and third equation of (7). Here we demonstrate the method to obtain a solution in one case where parameters t and a are not equal to zero. The solutions in other cases will be achieved in the same manner. With assumption that ak bk, t can be written in terms of t z (in case ak bk, the algorithm is done in the similar wa). t k k t z (11) W z z l Fig.. Geometric constraints of the elbow point E. Now let s define the position vector E epressed in the base frame, and the unit vector k as E z T T k k k k () z Then the constraints () can be written in terms of coordinates of elbow E. E l where k 1 z ak bk ak bk kk, k ak ck Also t can be epressed in terms of t z. t k k t z z k k (1) (1) where k k k k k k, k (1) In the net step, if one substitute t in (11) and t in (1) into the second equation of (7), a quadratic equation in terms of t z is obtained. Let s define k k kk k k 1 k k l (1)

4 Submission to International Journal of Control, Automation, and Sstems Then the solutions of the quadratic equation depend on the value of. In case of >, the value of t z is calculated as in (1). t, t k k k k 1 z1 z k k (1) With respect to each value of t z, the value of t and t are determined b (11) and (1). Eventuall, b using these variables and (8), the position of the elbow E = [ z] T is obtained. After the position of the elbow is obtained, the rotation angle of each joint in inverse kinematics problem is calculated graduall b geometric approach. First of all, the rotation angle of the fourth joint,, is computed independentl b the law of cosine. 1 l l r cos ll where r W S W S zw zs (17) The rotation angles of the first and second joints 1 and are computed directl from the elbow position. It is observed that the joint angle decides the z coordinate of the elbow while the angle 1 relates to the and coordinate of this point. Therefore 1 Atan E, and 1 1 E cos l E l z (18) (19) Since the position of the elbow is not affected b the joint angle, the position of the wrist should be considered in order to obtain. It is obvious that the wrist position is decided b the first four joint angles. Since 1, and are known, one can utilize the wrist position to obtain the angle. Being taken from the result of the forward kinematics, the position of the wrist is the fourth column in the homogenous transformation matri T. It is seen that 1 1 cos z l l c l c c W lss () under the assumption that the upper arm and forearm are not in line or (i.e., not a singular configuration). At the home configuration of the robot, the z aes of the frame z and the frame z are set parallel (see Fig. ). The misalignment of these aes during the operation of the robot is caused b the joint angle. This fact is applied for the calculation of the angle. 1 cos k k (1) where implies the inner product of two vectors. The unit vector k is identical to k, and the unit vector k is etracted from the homogenous transformation matri T of the frame z. Finall, the joint angle is calculated b considering the angle between the -ais of the last frame z and the vector r formed from the wrist to the elbow. 1 cos r j l () The advantage of the above algorithm calculating all joint angles is that each angle is obtained in onl one formulation. The computation time therefore is ver short while the result is reliable due to analtical solutions. The computation time of the algorithm in [1] is about 1 microseconds for each loop with C code in Intel Core Quad. GHz computer, while the computation time of the proposed solution is about 1. microseconds for each loop in the same condition. The computational fastness comes mainl from the usage of geometric propert instead of inversions of homogenous transformation matrices in [1]. Among several sets of inverse kinematics solutions, we select a solution at the current instant to be a neighboring solution of the previous one at the previous instant without an jumps. It is noted that the proposed algorithm for the inverse kinematics computation can be applied for an -DOF arm which has similar kinematics configuration. That is after appling the Denavit-Hartenberg notation to si joints of the arm, two coordinates are set at the shoulder, two at the elbow, and one at the wrist.. VALIDATION Several additional works are performed to verif and demonstrate the proposed algorithm. First, the algorithm is implemented b C++ programming language to verif the computation time and solution accurac. The test is performed with the Microsoft Windows XP operating sstem on a personal computer with.ghz Intel CPU. Onl microseconds are needed to implement whole algorithm calculation including the inverse kinematics for the -DOF arm, which is much faster than the pseudo-inverse algorithm used previousl. This is also faster than Asfour et al. s algorithm [1], and Badler and Tolani s algorithm [1] which was reported to take microseconds on a -MHz SGI workstation that corresponds to. microseconds in the present computer condition.

5 International Journal of Control, Automation, and Sstems (1) In order for the validation of the algorithm the demonstration software as shown in Fig. is developed in D OpenGL environment. The software is planned not onl for kinematics validation but also for control purpose of the robot arm. However, it is used in this paper onl for kinematics validation. As for the validation of the computation accurac, a closed-loop process is embedded in the software, as shown in Fig.. At the initial state, the position and the orientation of the hand are given. Using the input data, the inverse kinematics computation is done, and the outputs from the state are the rotation angles of arm joints. The forward kinematics takes the joint angles and produces the actual position and orientation of the hand. The actual position and orientation of the hand are compared to the given ones in the first step. The errors can be acquired as a result (refer to Table 1). as zero errors. These results are obvious since our solution of the inverse kinematics is analtic one instead of pseudoinverse or numerical solutions. Table 1. Coordinates of thee points and position errors Point 1 Point Point Coordinates z Position errors z.8e-1-1.e-1.e-1 -.8e-1.8e e-1.8e-1-1.e-1 The second demonstration module is developed with MSC VisualNastran Desktop and Matlab Simulink softwares to demonstrate the proposed inverse kinematics solution for motion tracking control of the - DOF arm. In this co-simulation work, the dnamic model of the robot is simulated b the VisualNastran Desktop while the controller is implemented in Simulink as shown in Fig.. The parameters such as mass and moment of inertia of each link is selected based on the given data from the humanoid arm manufacturer (Robot and Design Co, Ltd). Fig.. The robot model in kinematics simulation software. Fig.. Simulink and VisualNastran screens. Fig.. Validation process for the inverse kinematics algorithm. In the program of the proposed inverse kinematics, all kinematic parameters including arm lengths, joint angles and hand positions are epressed in the double tpe with at least 1 digits of precision. In the table 1 below, the position errors of the hand are calculated at three points are summarized. Note that all the results in the table are obtained when the orientation of the hand is set parallel to the vector rh [ / 1/ ]. It is found from the validation process and the table that the position error is less than 1-1 mm which is due to round-off errors. For the human scale robot arm, it can be actuall considered In the simulation, the inverse kinematics computation is set prior to the sstem controller. As shown in Fig. 7, the joint angles that are the results of the inverse kinematics are the inputs to the sstem controller. For applications where the speed and the precision are required, such as path tracking, fast computation of the inverse kinematics takes an important role. Here the path ma be divided into man tin segments and the inverse kinematics computation should be done for the distal end of each segment. As the length of segment gets smaller, the inverse kinematics calculation is required to be faster and more accurate. Fig. 7 Schematic diagram of a control sstem for robot arms.

6 Submission to International Journal of Control, Automation, and Sstems We eecuted computer simulation of path tracking to demonstrate the inverse kinematics algorithm. In this eample, the arm is controlled such that the finger tip can follow a sine curve. The path is divided into man small segments and the inverse kinematics is calculated for the distal end of each segment. Si independent PID controllers are designed for controlling si motors in the joints. Visual Nastran Desktop (VND) and Matlab Simulink are used for the co-simulation work. The inverse kinematics and the control algorithm are implemented in Matlab Simulink while the robot dnamics is calculated b the VND software. Fig. 8 displas the simulation results for the two inverse kinematics algorithms that are tested in the same condition. The sampling time for this simulation is set to.1 ms. The trajector s amplitude is 7mm and the frequenc is.hz. Simulation is performed for seconds. Initiall, the robot arm is set at the home configuration. Since the initial position and the destination is far, at the initial time the actual position of the end point is shot far over the desired one. However, after about 1. seconds, the robot tracks to the reference stabl. In actual tracking control of the arm, motion command of each motor can be precalculated from path planning result of the end-effector during remaining time of the previous sampling period as a background job, but to see the effect of computational time in this paper, we calculate motion commands (using inverse kinematics) and controller outputs consecutivel during the present sampling period, and simulate the dnamics of the robot arm. Fig. 8(a) shows the results using the proposed analtic inverse kinematics algorithm, and Fig. 8(b) shows the results using eisting slower pseudoinverse kinematics algorithm. Although the tracking performance mostl depends on the control algorithm, it can be also affected b the inverse kinematics computation as shown in Fig. 8. These demonstrations clarif that the proposed inverse kinematics algorithm is fast and accurate enough to implement in the actual feedback control sstem of the robot arm at high sampling frequenc. Recentl, another approach to avoid computational load of inverse kinematics has been tried for motion control of a robotic manipulator, which is doubtful of practicabilit for fast and accurate motion control [1]. (a) (b) Fig. 8 Motion tracking simulation results using (a) the proposed analtic inverse kinematics algorithm, and (b) the eisting slower inverse kinematics algorithm.. CONCLUSIONS We presented a fast closed-form solution for the inverse kinematics of a si-dof arm with a specific structure similar to human arm. A closed-form solution is obtained b geometric approach with a novel assignment of coordinate sstems and geometric relationships, and is verified b C++ programs. The calculation time of this inverse kinematics algorithm is about 1. microseconds under PC environment with the Microsoft Windows XP and.ghz Intel CPU, which is faster than the previous algorithms. The proposed inverse kinematics algorithm is computationall fast enough to be applied for real-time control with high sampling frequenc. The limitation of the proposed algorithm is onl to be applicable to si DOF robots with the specific configuration of Fig. 1. In this paper, furthermore, visualization modules for verifing inverse kinematics of the arm, and for demonstrating motion tracking control of the arm including arm dnamics are developed. Using these visualization modules, the validit of the proposed inverse kinematic solution (analtic) is demonstrated. REFERENCES [1] D. Tolani, A. Goswami, and N. I. Badler, Realtime inverse kinematics techniques for anthropomorphic limbs, Graphical Models, vol., pp. -88,. [] D. Pieper and B. Roth, The kinematics of manipulators under computer control, Proc. of the nd Int. Congress on Theor of Machines and Mechanisms, pp , 199. [] D. Manocha and J. F. Cann, Efficient inverse kinematics for general R manipulators, IEEE Transactions on Robotics and Automation, vol. 1 pp. 8-7, 199. [] C. Klein and C. Huang, Review of pseudoinverse control for use with kinematicall redundant

7 International Journal of Control, Automation, and Sstems (1) 7 manipulators, IEEE Transactions on Sstems, Man, and Cbernetics, vol. 7, , [] M. Benati, P. Morasso, and V. Tagliasco, The inverse kinematic problem for anthropomorphic manipulator arms, J. of Dnamics Sstems, Measurement, and Control, vol. 1, pp , 198. [] C. G. S. Lee, and M. Ziegler, Geometric approach in solving the inverse kinematics of PUMA robots, IEEE Trans. Aerospace and Electronic Sstems, vol., pp. 9-7, 198. [7] C. G. Kang, Online trajector planning for a PUMA robot, Int. J. of Precision Engineering and Manufacturing, vol. 8, pp. 1-1, 7. [8] S. Elgazzar, Efficient kinematic transformations for the PUMA robot, IEEE J. Robotics and Automations, vol. 1, pp. 1-11, 198. [9] R. P. Paul, and H. Zhang, Computationall efficient kinematics for manipulators with spherical wrists based on the homogeneous transformation representation, The International Journal of Robotics Research, vol., pp. -, 198. [1] J. Cote, C. M. Gosselin, and D. Laurendeau, Generalized inverse kinematic functions for the Puma manipulators, IEEE Trans. Robotics and Automation, vol. 11, pp. -8, 198. [11] R. J. Schilling, Fundamentals of Robotics: Analsis and Control, Prentice Hall, 199. [1] T. Asfour, and R. Dillmann, Human-like motion of a humanoid robot arm based on a closed-form solution of the inverse kinematics problem, Proc. of Int. Conf. on Intelligent Robots and Sstems, pp ,. [1] N. I. Badler, and D. Tolani, Real-time inverse kinematics of the human arm, Presence (Cambridge, Mass.), vol., pp. 9-1, 199. [1] R. Garcia-Rodriguez, and V. Parra-Vega, Taskspace neuro-sliding mode control of robot manipulators under Jacobian uncertainties, International Journal of Control, Automation, and Sstems, vol. 9, no., pp. 89-9, 11. Thanhtam Ho received the B.S. degree in Mechatronics Dept. from Hochiminh Cit Universit of Technolog, Vietnam in and the M.S. degree in Mechanical Design and Production Engineering in 8 from Konkuk Universit, Seoul, Korea, where he is currentl pursuing the Ph.D. degree in Mechanical Engineering. His research interests consist of the humanoid robot arm, biomimetics robots, lateral position control for the roll-to-roll printing sstem and computational simulation. Chul-Goo Kang received B.S. and M.S. degree in mechanical design and production engineering from Seoul National Universit in 1981 and 198 respectivel. He received Ph.D. degree in mechanical engineering from Universit of California, Berkele in Currentl, he is a professor at Konkuk Universit in Seoul, Korea. He served as the General Chair of 11 8 th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 11). His research interests include intelligent motion and force control, force sensor, train brake sstems, and intelligent robots. Sangoon Lee received B.S. degree from Seoul National Universit in 199, M.S. degree from KAIST in 199, and the Ph.D degree in mechanical engineering from Johns Hopkins Universit in. Since then, he has been a professor at Konkuk Universit, Seoul, Korea. His research interests include robotics, automation, and robotics applications to bioengineering