Proceedings of 2013 IEEE International Conference on Mechatronics and Automation August 4 - 7, Takamatsu, Japan
CYRUS Algorithms for Manipulators Inverse Kinematics Ali Pyambri Paramani MSc Student Stellenbosch University, South Africa
[email protected] Abstract - Accuracy, speed and applicable to general problems are among the most important fundamentals in all computational methods. Calculation methods for inverse kinematics of Manipulators are no exception of this rule. Kinematic equations of manipulators are solved using non-linear methods and the accuracy of each manipulator is less than its repeatability. The accuracy of parameters in the robot kinematic equation can affect the robot accuracy. An error in parameter calculations will cause errors in the calculation of joint angles from inverse kinematic equations. Although the repeatability of most computing methods is relatively good, their accuracy is usually very low and its level varies in different robots. Most conventional methods have been designed for specific situations, and certain types of manipulators. When changing Manipulators or joint type, the overall situation of the problem and its solution change completely. This article aims to introduce Cyrus Algorithm for calculating inverse kinematics for Manipulators in a 3D environment. The Cyrus is a general approach that can be used for a variety of Manipulators with any degrees of freedom and joints. In addition to applicable to general I.K, this method has a high speed and accuracy compared to similar methods. In this method, all solutions are calculated by determining spans (ranges) for the manipulator movement. The Cyrus Algorithm idea is based on one of the most basic engineering principles, intuitive understanding and argument computing which is defined with the aid of algebra and geometry with mathematical logics.
parameters of that robot [1]. Inverse kinematics of Manipulator is solvable when all the joint variable sets related to the given location and orientation can be determined by following a specific algorithm. [2] Most researchers resort to iterative methods for inverse kinematics using the Jacobian matrix to avoid the difficulty of finding a closed-form joint solution. Since a closed-form joint solution, if available, has many advantages over iterative Methods [3]. The Newton– Raphson method is another method used for numerical solution of IK. It requires the basic Jacobian matrix [4], where its efficient computation methods are already available [5], [6]. The NR method, however, succeeds if and only if the function of the original equation has the inverse function and the original equation is solvable. Another commonly adopted method for inverse kinematics is the geometric method [7], [8] however, the geometric method requires geometric intuition in solving the Joint solution of a manipulator and it may become more difficult to obtain the joint solution when more than four or five joints are involved. When the geometric approach is used to solve inverse kinematics equations, one can sometimes calculate next several solutions by performing simple operations on different angles obtained from the first solution. Thus the calculation of the first solution will be expensive computationally but subsequent solutions will be obtained very quickly with addition and subtraction of the obtained angles [9]. The problem of solvability is as severe as that of singularity. A more difficulty is that it is hard to know if the equation is solvable or not in advance in most situations—it is burdensome for operators to assign carefully the goal position and orientation of the link of interest to be within the workspace in order to guarantee the solvability in the motion design and remote control of large-scale structure-varying kinematic chains such as Hyper-redundant Robot [10]. Hyperredundant Manipulators have a very large relative degree of kinematic redundancy. These robots are analogous in design and operation to “snakes” or “tentacles. Traditionally, kinematic analysis and motion planning for redundant manipulators has relied upon a pseudo-inverse, generalized inverse or extended inverse of the manipulator Jacobian matrix [11].
Index – Inverse Kinematics, Cyrus Matrix, Interval, Condition, Hyper-redundant Manipulator.
I. INTRODUCTION Inverse kinematics calculation methods for Manipulators are one of the most important and interesting parts of the robotics science. Researchers are trying to increase the computing accuracy and speed by developing new methods or optimizing current computing methods. The objective of the problem is to find a robot configuration that satisfies specified constraints about the position and orientation of a link of interest (the end effector in typical cases). Kinematic equations of Manipulators are solved using nonlinear methods and the accuracy of each Manipulator is less than its repeatability. The accuracy of parameters in the robot kinematic equation can affect the robot accuracy. An error in parameter calculations will cause errors in the calculation of joint angles from inverse kinematic equations. Although the repeatability of most computing methods is relatively good, their accuracy is usually very low and its level varies in different robots .By applying various calibration methods, one
This paper will introduce the Cyrus Algorithm. Using this algorithm, non-linear equations found in inverse kinematics can be solved by following a general algorithm. This algorithm combines closed solving techniques, numerical methods and the geometric approach.
Can increase the robot accuracy through estimating kinematic
978-1-4673-5558-2/13/$31.00 ©2013 IEEE
1405
III. CYRUS FUNDAMENTAL MATRIX
II. THE ART OF INVERSE KINEMATICS
Generally, analytical methods can be used in position analysis to yield results with high degree of accuracy. This accuracy comes with a price in that the methods often become numerically intensive. Complex method involving higher order math, have been developed for position analysis. Using geometrical conceptions, complex methods can be expressed in a simpler way and algorithms can be reduced. Consider Points "D" and "G" are given in the reference coordinate system “Fig 1”. Point "D" is defined related to Point "G" as follow:
A. IK: One of the most important arts of Robotics Engineering is to simplify complex problems and solve them with the help of simple mathematical functions. Engineers have always tried to convert complex systems into simple and understandable ones so that they can be described simply and nicely, and also existing information can be used easily for intended purposes. Clearly, the purpose of reverse engineering is not to create a system equal to the studied system. Rather, it aims to create a system based on the modelled reference system that can meet the desired outputs using specific properties of the reference system. IK in Manipulators aims to find all possible and acceptable states, given a schematic profile of the Manipulators. In general, the best method to calculate IK in Manipulators must have the following characteristics: - It can be used for different Manipulators with different degrees of freedom. - It should have the least number calculations so that it has higher computing speed and accuracy and not take up a large space in the processor memory. There is also a third condition that has been given less attention: - The solutions obtained from inverse kinematics for a joint can be extended to another joint so that less calculation is required for the new joint. (Reduction of Calculation by Data overlapping and Transfer).
,
,
,
(1)(2) (3) (4)
Problem want to find all possible states of Point "D" for different values of LDG . New X`Y`Z` coordinate system can be defined in which X` axis is in line with LG and the origin is on the origin of the reference coordinate system. It is observed that Point D which is within the ODG triangle rotates around X` axis for different LDG values. The coordinates of Point D in the X`Y`Z` coordinate system is defined as follows. , 0,0 (5)
B. Cyrus Concept:
(6)
“If you can’t explain it simply, you don’t understand it enough “Albert Einstein. Kinematics Techniques for mechanical arms are like the "drill" of sculpture, and Manipulators are like a statue. If seeing the problem from this viewpoint, direct kinematics of mechanical arms is defined as follows: It is the mathematical method and art of assembling or forming the shape of skilled mechanical arms and it may be done in any size or material. The inverse kinematics of mechanical arm is defined as follows: Now, if we want to guess all the formed pieces (parts) with the help of "drills" of the direct kinematics method and depict the statue shape by putting them together, we will use the inverse kinematics technique. Given different thicknesses for different drills, there are always errors when putting together the obtained solutions (pieces). This article aims to introduce a new "drill" called CYRUS, which has minimum error. This drill can also be used for all statues (Manipulators) in all circumstances. The CYRUS algorithm's drills are obtained by cutting out (calculating) a large piece into smaller pieces. This technique replaces these small pieces with blanks of other pieces. This technique has not calculate small pieces. Mathematically, the resulting small pieces can be moved to the needed locations only with a small reorientation.
With Solving Equation System “(6),”: (7) .
(8) .
(9)
Fig. 1 the coordinates of Point D in the X'Y'Z' and XYZ
If Z D is selected from “(8),” then all Y D values are calculated for the constant value of X D . As can be seen, in all the above equations, the only variable value is the LDG value. In case of any changes in the LDG value all possible states can be
1406
obtained for Point D in the X`Y`Z` coordinate system. To move the obtained coordinates to the reference coordinate system, one can simply use the X-Y-Z Euler's angles to calculate rotation ratios. Equations “(10),” and “(11),” : 0, ,
0, , ) ,
(12)
,
(13)
0,0,
(10)
0, ,
.
·
.
.
·
.
“Fig 1” and “Fig 2” show work space of point D with Initialization Point G=(9,10,11) and LD 12.18 And 6 LDG 10, with the respect to “Fig 1”, It is observed that Point D which is within the ODG triangle rotates around X axis for different LDG values “Fig 1”. IV. CYRUS ALGORITHM
.
0
The previous section introduced basic conversion matrices used in the algorithms. The Cyrus algorithm was designed in such a way that reduces error rate in calculations and enhances accuracy. Based on the calculation of all possible states for inverse kinematics, Cyrus Algorithm aims to define a manipulator on an imaginary coordinate system (Size are defined based of workspace) and transfer it to the reference coordinate system. This algorithm was designed so that if the Arm has more than four joints, all calculations for joints after the fourth joint are performed based on the functions of previous four joints and only a minor change is made to the index shape of functions. Functions used in the algorithm were designed for a general state but given function types, in case of joint change; one can easily use it for a certain joint type by changing their function inputs. “Chart I” shows the algorithm stages. For calculating inverse kinematics and workspace of a Manipulator, the algorithm is based on Point G the objective or Goal point. Given the calculation method of trigonometric functions, no error is made due to the conversion of these functions into sine and cosine functions. Also, given the type of functions used in the algorithm, in each solution set, three variables are required for each joint. The X set is directly calculated. The Z solution set is determined without calculation and only by selecting from a span. Finally, the Y set is calculated based on the values obtained for Z and X .The inverse matrix equations will not be used in Cyrus Algorithms; however, positions of all the links are expressed based on the geometrical position of interfaces and through mathematical function. The connecting lines, based on which the analysis is carried out, are introduced in Algorithms.
(11)
Fig. 2 Work space of point D in X`Y`Z` Coordinate system
V. ATOSSA REFERENCE MANIPULATOR Consider the ATOSSA Manipulator “Fig 4”. Atossa is composed of (i) spherical joints. The ATOSSA profile table is defined as Table I. The algorithm is defined based on four imaginary lines connecting joints. The Algorithms are initialized based on imaginary connecting lines. The sizes of these imaginary lines are defined based on the system workspace.
Fig.3 Work space of point D in XYZ, reference Coordinate system
A. Definition of Cyrus Matrices: (11)
1407
TABLE I
The aim is to find:
ATOSSA Characteristics i
Number of Joint Length of Link (i) 3*1 CYRUS Matrix For Joint (i) 3*3 CYRUS Transfer Matrix For Transfer Data to Reference Coordinate System for Joint (i) , ,
3 , > L , +L If L , Sphere are separate.
Position of Joint (i) in Reference Coordinate Distance between to joint (0) and Point
(G)
, ,
then there are no solutions, the
= 0 and L , L , then the Sphere are If L , coincident and there are an infinite number of solutions. The workspace of joint (i-3) be expressed through the following formula in case the value of L , is specified. Equations “(27),” to “(30),” With respect to the constants values of L , and different values L , and L , . As stated in Equation “(31),” to “(33),”.
Distance between to Goal Point (G) and joint (i-1)
, ,
,
If L , < |L , – L , | then there are no solutions because one Sphere is contained within the other.
System .
,
Distance between to joint (0) and joint (i-1) Distance between to joint (0) and joint (i-3) Distance between to joint (i-3) and joint (i-1)
In the Algorithm, by making constraints and conditions for joints (i-1) and joints (i-3), all the needed data can be calculated with the algorithm functions. Here is how to calculate inverse kinematics of the ATOSSA Manipulator based the Cyrus Algorithm. Next, discuss how to use the Cyrus Algorithms in case of schematic change of the robot (arms and joints) as well as change in the degrees of freedom. Analysis is defined based on finding all possible states for the configuration of ATOSSA arm according to the G objective point. A. Stage 1 / Preliminary Analysis joint (i-1)
D. Stage4 / Preliminary Analysis joint (i-2) Joint (i-2) includes a point set caused by the intersection of Spheres: , , , , , , , Equations “(34),” to “(38),”. E. Algorithms Loop
The analysis is based on joint (i-1) and joint (i-3) which is indicating position and workspace of system. With respect to “Fig 4”, and Equations “(6),” it is obvious that the Position of point G (Goal Point) from joint (i-1) is LG, And joint (i-1) moves on Common Surface between two Sphere : 0,0,0, , , : , , , , . , , , are radius of spheres By calculating possible spans for values using Table II and Equations “(15),” to “(17),”, LG, , L , One can obtain all possible states for Joint (i-1) using Equations “(18),” to “(21),”. All the used functions and matrices were modeled using the concepts described in the Cyrus Fundamental Matrix section.
The algorithm was designed in such a way (see algorithm shape in” Chart I”) that inverse kinematics for joints after Joint (i-3) are calculated using existing and previous functions. In case of calculating the coordinates of Joint (i-4) one can view the ATOSSA robot by removing the obtained joints except Joint (i-4) as an arm whose objective points are 14 , , And is initialized to . This loop continues until the joint (1) is calculated. Equation “(39),”
B. Stage 2 / Preliminary Analysis joint (i) It is clear that joint (i) located on the curved caused by the intersection of two spheres: , , , , , , , To obtain inverse kinematics according to Table II and Equations “(22),” to “(26),” . One should use the solution sets obtain Cyrus Matrices Using value ai and g. C. Stage 3 / Preliminary Analysis joint (i-3) For design situation, these complex methods can be difficult to understand and impalement. This section describes how to find the intersection Curve between 2 Sphere on a plane with radius L , and L , with Centre J (0)=(0,0,0) and . 1 , ,
Fig. 4 Schematics of ATOSSA manipulator
1408
Chart. I CYRUS Algorithm
TABLE II CYRUS Algorithm Data Sheet 0, ∑ , ,
,
,
,
0
,
,
,
,
Stage 1 .
,
,
(18)
,
,
Stage 2 ,
,
,
,
(22) (23)
.
,
1
(15)(16)(17)
,
,
,
,
,
,
. ,
,
.
(19)
, .
,
,
· ·
1
(20) 0
,
,
,
,
,
,
,
,
,
,
,
,
,
0
1 ,
, ,
,
,
,
1
1 ,
,
,
,
,
,
,
,
, ,
,
,
; ;
,
,
,
∑
, ,
,
;
,
3
, ,
,
.
(26) (27)(28) 29 30
;
,
,
,
,
, ,
2
Stage 4 ,
,
,
35
,
(31)
, ,
(36)
.
. ,
.
· ,
,
,
,
·
2
0 ,
3
(34)
.
,
3
(25)
,
1
Stage 3
,
,
·
,
(21)
,
,
·
,
0 ∑ ∑
(24)
.
,
,
,
,
,
,
,
,
,
,
, ,
3 (33)
, ,
3
(32)
, ,
·
,
(37)
, ,
0
,
2
·
,
3
2
2
(38) ,
Algorithm Loop 1409
,
,
,
g= =ai-1
i=i-4 ;
39
VI. INITIALIZED CYRUS
L , 2.4 3.2 5 6 8
For initialized Atossa Manipulator with Cyrus Algorithms assume Input of Algorithms in are: 5, , ,
0 ,
,
,
0,0,0 , ,
9,10,11 , 4,4,5,4,4,6,17.37
(40)
7,
, ,
Then initialized Cyrus Matrix in Term of required Accuracy for Stage 1 to Stage 4 from Table III, select Imaginary line value from Equations “(15),” to “(17),” and “(27),” to “(30),” initialized Cyrus Function from equations “(18),” to “(26),” and from equations “(31),” to “(39),”. All outputs shown in “Fig 5 “ to “Fig 11”. for calculation steps see [12]. A. First initializations: In this section, the analysis basis will be initialized. Positions of all the links are expressed based on the geometrical position of interfaces and through mathematical function. The connecting lines, based on which the analysis is carried out, are introduced in Table II. Now, draw lines LG and L , the workspace of joint (4) is as follows. With respect to “Fig 6”. | 0,17 7.37,27.37 , 0 10 , 17.37 , , , , , 12.1890736 (41)(42)(43) , 0,8 (44) (45) , , , ,
L , 12.1890736 12.1890736 12.1890736 12.1890736 12.1890736
,
,
4 ,
,
TABLE III L ,
6.82,3.52,9.46 2.17,6.47,1.53 , 5 4
,
.
2
8
4
,
4 4
0.74 0.66 0 4
0.42 0.47 0.77 4
, ,
5 8
(53)(54) (55)(56)
0.45 0.88 0
.
7.96 |2.19| 0.68 0.35 0.63 2
(61) (62)
,
(64)(65) 3.149 (66)(67) 3
(69)(70)(71) (72)
H. Loop Stage 1, joint (1):
.
0.51 0.57 0.63
6.82 5
G. Algorithm Loop, Third initialization: 3.77,4.31,5.57 i=1; g=5; , , 0 0 0,0,0
,
,
L L
7.69
2 2 (63) F. Stage 4, Joint (3): 5, 2 3.77,4.31,5.57 , 3.04, 0.78,3.88 , , , , , , 3 3 2 (68)
.
,
,
0.56 0.28 0.77
2
. .
Select 9 9 9 9
5 (57) D. Second initialization: The workspace of joint (2) and joint (3) can be expressed through the following formula in case the value of L , is specified. The specified functions stated the positions of joint 2, 3 and 4 in terms of each line of the following Table III with respect to the constants values of L , and L , and different values of L , . As stated in analytical analysis section. 0 8 (58) , 12.1890736 9, (59) , 12.1890736 ; 9; , , , 12.1890736 9, (60) , 12.1890736 ; ; , , , E. Stage 3, joint (2):
12.1890736 9, , , 9; (46) 12.1890736 ; , 12.1890736 9, , 12.1890736 ; (47) , , , B. Stage 1, joint 4: Now, in case of selecting L , value from the interval and putting it in the equation “(21),” the workspace of joint (4) will be calculated correctly. In other words, all the positions that joint (4) can be placed are calculable based on different values of LG, . 12.1890 (48) 13.45 (49) , ,
,
8.989073598 7.189073598 6.189073598 4.189073598
,
.
L
3.43
1
(50) (51) (52)
1
C. Stage 2, joint (5): By initialized the following system of equations, a specific and unique coordinates can be obtained for joint (4) and for each of the obtained coordinates of joint (5):
1 (75)
1410
4 0.47 0.53 0.69 0
0.75 0.65 0
3.43 |2.04| 0.45 0.52 0.71 1
.
(73)(74) 1
Fig. 5 Schematics Of Atossa Manipulator With 5 Sphherical Joint
Fig.7 Work Space of Joint (5) for X , Y , Z
6.82,3.52,9.46)
Fig. 6 Atossa manipulator with Imaginaryy line Fig. 8 Work Space of Joint (2) for , , ,
Fig. 9 Work space of Joint (4) for L
,
7.37,27.37 , 0
LG,
,
,
.
, .
,
10 Fig. 11 Work Space of Joint (1) and Jo oint (3) for X , Y , Z 3.77,4.31,5.57 7
View Fig. 10 Workspace of Joint (4) BOTTOM V
1411
, .
)
VI. COMPUTATIONAL CONSIDERATIONS The calculation sequence for inverse kinematics of joints in the algorithm is as follows: 1 3 2 0 1 . All the values obtained from CYRUS matrices are a continuous series of numbers and are directly calculated using X numerical values by selecting imaginary line values. Z Values are calculated as a symmetric set of real numbers only by calculating its Extremum and it has not to calculate Z for all points. An important point is that imaginary line values are determined using the "ATOSSA" schematic profile. So no error is made in the function inputs. Also, inverse Kinematics of joints is made pairwise so that in case of errors for any reason, such as rounding error, it will not affect the calculation of the next joint pair. Since the values of CYRUS Matrices in the second matrix pair depend directly on the values selected for imaginary lines, which will not cause error in the initialization of the second pair matrices, given that they are schematic features of the robot. A. Change In The Joint Type Consider the Joint (i) in Atossa Manipulator described in the Atossa Reference Manipulator section. If the manipulator joints (unlike previous states which were all spherical) are designed in such a way that cannot rotate around the X` axis, then the CYRUS matrix is changed as follows.
,
(81)(82)
, ,
,
(83) ,
;
(84)
; SUMMERY: The Cyrus Algorithm is simple and fast. The functions used in this algorithm are limited, the range of output solutions is wide and one can extend the calculated solutions of a joint to other joints. When using the CYRUS approach, solutions are calculated in parallel rather than series. One can calculate next several solutions by performing simple operations on the first solution. Also, it takes up little space in the robot processor due to low amounts of computing. In many cases, it is necessary to solve the inverse kinematics equations of manipulators with at least 30 Hz speed. As a result, computational efficiency will be very important. The Algorithm accuracy is high. This algorithm can be used specifically for the inverse kinematics calculation of Snake Robots. Finally, this algorithm can be optimized for different joints and conditions and it can be implemented by the simplest available software . REFERENCES
,
[1]
S.Hayati, “Robot arm geometric link Parameter Estimation, “ on Decision and Proceeding of the 22nd IEEE Conference Control, Dec 1983 . [2] B. Roth “Performance Evaluation of Manipulator From a Kinematics View Point “Performance Evaluation of Manipulator, National Bureau of Standards, special publication 1975. [3] M. A. Ali, H. A. Park, and C. S. G. Lee, "Closed-Form Inverse Kinematic Joint Solution for Humanoid Robots," in the IEEE/RSJ Conference on Intelligent Robots and Systems, Taipei, Taiwan, Oct, 2010. [4] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation,” Int. J. Robot, vol. RA-3, no. 1, pp. 43–53, 1987. [5] D. E. Whitney, “The mathematics of coordinated control of prosthetic arms and manipulators,” Trans. ASME, J. Dyn. Syst., Meas., Control, vol. 94, no. 4, pp. 303–309, 1972. [6] D.E.Orinand W.W.Schrader,“ Efficient computation of the Jacobian for robot manipulators,” Int. J. Robot. Res., vol. 3, no. 4, pp. 66–75. [7] Zannatha and R. C. Limon, “Forward and inverse kinematics for a small-sized humanoid robot,” in Proc. of the IEEE Intl. Conf. on Electrical, Communications, and Computers, 2009. [8] D. Tolani, A. Goswami, and N. I. Badler, “Real-Time Inverse Kinematics” vol.62, no.5, pp.353, Sep.2000. [9] Craig, Introduction to Robotics: Mechanics and Control, Pearson Prentice Hall, 2005,pp.80-110.2000 [10] Y. Nakamura and K. Yamane, “Dynamics computation of structure- varying kinematic chains and its application to human figures,” IEEE Trans. Robot. Autom. vol. 16, no. 2, pp. 124–134, Apr. 2000. [11] C. A. Klein and C. H. Huang, “Review of the pseudo inverse for control of kinematically redundant manipulators,” IEEE Trans. Syst., Man, Cyber., vol. SMC-13, no. 2, pp. 245-250, Mar. 1983. [12] A. P. Paramani, “Optimum Inverse Kinematic Method For 12 DOF Manipulator” Proceeding of the 8nd International Conference on Mechatronics and Automation (ICMA), Aug 2011.
,
0 .
(76) Also, if one of joints is prismatic, one should assume the constant a values in the matrix as variable. Avoiding making repetitive calculations is one of the methods for reducing calculations in the numerical methods. In Cyrus Algorithm, some possible solutions for the configuration of the some Joint can be calculated by calculating inverse kinematic of the Other Joint and only by using previous data. Numbers of these solutions, with respect to the specifications of the robot are different. Define {k} are inputs of all CYRUS matrices. Its mean for Reduction of Calculation by Data overlapping if the input of Cyrus were equal for two different Joint with Same type (spherical, Revolute or Prismatic) user can replace data of First Cyrus Matrix to second one: , , 1,2,3, … ,
,…,
(77) (78) , ,
(79)(80)
,
1412
