Proceedings of the 2002 IEEE International Conference on Robotics & Automation Washington, DC • May 2002
Manipulator Kinematic Error Model In a Calibration Process Through Quaternion-Vector Pairs M.A. Pérez Rueda, A.Lara Feria, J. C. Fraile Marinero, J. Delgado Urrecho, J.L.González Sánchez. H θ θ H H Θ ={ q , t }= cos , sin u , t = 2 2
Abstract - Three-dimendional modeling of rotations and translations in kinematic calibration is most commonly performed using homogeneous transformations. This paper presents a new technique for the calibration of robots based on the quaternion-vector pair approach for the identification of geometrical errors. Keywords-Kinematic Calibration, Quaternion.
θ θ cos , sin < u x , u y , u z > , t x , t y , t z 2 2
where the unit quaternion q encodes the appropriate rotation and the vector t denotes the corresponding translation displacement. A quaternion q, is by definition [4] the sum of a scalar (s) H and a three-dimensional vector ( v ). H H q = [s, v ] = s + v = {s, [vx, vy, vz]} (2)
I. INTRODUCTION
T
he Identification Jacobian is a Jacobian matrix relating measurement residuals to parameter errors. The Identification Jacobian plays a central role not only in the identification of the unknown parameter vector, but also in the study of optimal selection of measurement configurations and accuracy compensation. It is shown in this work that the original robot identification Jacobian through the quaternion-vector pair as spatial operator represents the translations and rotations. Robot calibration is the process of enhancing the accuracy of a robot manipulator through modification of the robot control software [1]. This process may be considered to consist of several sequential steps: modeling, measurement, identification, and implementation. Calibration of the manipulator kinematic errors as presented here is based on the relationship between the differential position and rotation vectors and the kinematic parameter errors .
The conjugate of a quaternion q, denoted by ~ q , is H ~ defined by negating its vector : q = [s, - v ] Let q be a quaternion. If N(q) = 1, then q is referred to -1 as a unit quaternion. For a unit quaternion: q = ~ q A unit quaternion can be expressed as: q=
θ θ cos , sin < u x , u y , u z > 2 2
(3)
A. Basic spatial operations with the quaternion-vector pair Θ Spatial transformation of a point vector H H −1 H r'= q ∗ r ∗ q + t (4) Composition of spatial operator
II. METHOD OF THE QUATERNION-VECTOR PAIRS
H H H H (q1, t 1 ) ∗ (q2 t 2 )=(q1 ∗ q2 ,q1 ∗ t 2 ∗ q1-1 + t 1 ) Inverse of a spatial operator
This method has been proposed by Taylor [2] and Funda [3], and his theoretical basis lies in the fact that the operator responsible for the orientation will be able to be a quaternion. A translation and a rotation will be able to be carried out through the operator composed by a unitary quaternion and a vector:
(5)
Θ -1 = (q, t )-1=(q-1, -q-1 ∗ t ∗ q) (6) On the quaternion-vector side, we have let the symbol “ ∗ ” denote quaternion multiplication, which is defined as [4].
Let Θ be the quaternion-vector pair
H H q1 ∗ q2 = [s1, v 1] ∗ [s2, v 2] = [(s1 s2 - vH 1 ⋅ vH 2 ), (s1 vH 2 + s2 vH 1 + vH 1 ∧ vH 2 )] (7) The reader is refered to [4], [5] and [6] for a more detailed discussion of the above operations.
M.A.Pérez Rueda is Assistant Professor in Mechanics Engineering Departament. Technical School of Industrial Engineering Escuela Técnica Superior de Ingenieros Industriales de Valladolid. Spain. Email:
[email protected]
0-7803-7272-7/02/$17.00 © 2002 IEEE
(1)
135
wj is the value that represents the translation vector for the jth joint and wTCP represents the translation between the end effector frame and the reference frame in the zero position. - If the joint is revolute, the expression of wj according to Equation (8), is:
III. RELATIONSHIPS OF THE ORIENTATION AND POSITION OF THE END EFFECTOR WITH THE GEOMETRIC ERRORS The location of a point p'1 of the link after a rotation is calculated by a rotation and a displacement that are represented by the quaternion-vector operator: H θ θ H H Θ =[q, w ]=[(cos + sin u ), w ] = 2 2 ( q, p-q ∗ p ∗ ~ q) (8) being θ the rotation angle That is to say:
wj = p j - qj ∗ pj ∗ ~ (15) qj being pj the coordinates of the position of the joint j referred to the global reference system. - If the joint is prismatic, the expression of wj according to the equation (10) is:
q + (p - q ∗ p ∗ ~ q) (9) p’1=q ∗ p1 ∗ ~ where p’1 is the location of p1 before the rotation and point p may be any point on the axis of rotation. In the case of a prismatic joint, the displacement is represented by the quaternion-vector pair:
([
H Θ =(q , t ) = 1,0,0,0 , t ⋅ u x , t ⋅ u y , t ⋅ u z
]
)
wj = ( t · ux, t ·uy, t · uz)j
Below we will develop the differential relationships for the operating rotation (quaternion) and for the operating displacement (vector). A. Geometric errors in each link
(10) The material presented in this section indicates how small errors about the nominal manipulator kinematics produce end point position and orientation changes, and is developed into a form that allows a calibration methodology to be formulated. Many approaches to the problem have been proposed [7, 8, 9, 10, 11, and 12]. We will use the Quaternion-vector pair model to ilustrate this approach. We will examine the small change effects of the values of the geometric parameters for each link. In the first place it is assumed that all the variations in the geometric parameters of the robot may be accounted for by variations in d (joint displacement), ux, uy and uz (joint axis orientation), px, py, and pz (joint axis location).
The expression of p’1 will be: (11) p’1 = p1 + ( t · ux, t ·uy, t · uz) being t the displacement of the joint. In this case three independent quantities are necessary to define the operator quaternion vector of displacement. Let Θ denote the transformation quaternion that relates the tool frame of the manipulator to the world frame, n H (12) Θ =[Q, W ]= + Θ j ΘTCP j=1 Being: Θ j the quaternion-vector operator that represents the
displacement for the j th joint. ΘTCP represents the transformation between the system of the end effector frame and the global frame in the zero position. This expression, applied to a robot of N degrees of freedom, is defined as: - The global quaternion is: Q = q1 ∗ q 2 ∗ q3 ∗ LL ∗ q n ∗ q TCP
(16)
A.1 Total differential of the quaternion of transformation q, of each link In the case of a prismatic joint, the corresponding quaternion is q=[1, 0 ] therefore all the differentials of that quaternion will be zero. For that reason this paragraph is referred only and exclusively to the case in which the joint will be revolute, that is to say d= θ Beginning with Equation (3) the change in single transformation quaternion q is:
(13)
where: qj represents the quaternion of the jth joint qTCP represents the rotation between the end effector frame and the reference frame in the zero position. - The vector that represents the final position of the end effector, after the transformation is obtained through the expression: W = q1 ∗ ∗ q n ∗ w TCP ∗ ~ qn ∗ ∗ ~ q1 + (14) + + q1 ∗ w 2 ∗ ~ q1 + w1
dq =
∂q ∂θ
∆θ+
∂q ∂q ∂q ∆ux+ ∆uy+ ∆uz ∂ ux ∂ uy ∂ uz
(17)
To develop it, we should find the differentials that will be obtained in the following sections.
136
Quyx can be written as: θ 2 θ ) - (uy·sin )/(2·ux), (uyuz Quyx = {(0, uz·(sin 2 2
A.1.1. Differential of the quaternion q with respect to the rotation of the joint By differentiating Equation (3) we get:
θ θ θ θ = (-sin ,ux.cos ,uy.cos ,uz .cos ) (18) 2 2 2 ∂θ 2 2 Furthermore, a quaternion Qθ we can find, such that: ∂q = q·Qθ (19) ∂q
1
(sin
(20)
A.1.2 Differential of the quaternion q with respect to the direction of the rotation axis.
uy ∂ ux = ux ∂ uy
uy θ θ = (0, sin , sin , 0) x u 2 2 x ∂ uy
θ 2
)2)/ux)}
q (Qθ·∆θ + Qux ·∆ux + Quy ·∆uy + Quz·∆uz) Following [5] by defining an error dq such as:
(25)
(29)
dq = q·δq
(30)
δq = Qθ·∆θ +Qux ·∆ux+ Quy ·∆uy + Quz ·∆uz
(31)
we have:
A.2 Total differential of the vector w, that represents the translation corresponding to each joint For a revolute joint: (1-ux )·v θ·px-(uxy·vθ-uz·sθ)·py-(uxz·vθ+uy·s θ)·pz 2
(21)
w= -(uxy·vθ+uz·sθ)·px+(1-uy2)·vθ·py-(uyz·vθ-ux·s θ)·pz
(22)
-(uxz·v-uy·s θ)·px-(uyz·v θ+ux·s θ)·py+(1-uz2)·v θ·pz
Taking into account the expressions (21) and (22) and differentiating with respect to uy and uz the following expressions are obtained (the superscripts x will indicate that the joint rotates about that axis): Differentiating with respect to uy: ∂q
2
,-((ux2 + uy2)·(sin
dq = q·Qθ·∆θ + q·Qux ·∆ux + q· Quy·∆uy+q·Quz·∆uz =
In the diferentiation of the parameters that define the direction of the joint, one must take into account that only two of the three parameters are independent. When the joint is nominally oriented in parallel direction to some of the axes, if there is a certain misalignment with respect to this axis, the parameter related to this axis is going to suffer a negligible variation, therefore we will take the other two as parameters of the joint, where the parameter of the joint will be function of the two remaining ones and will be calculated through the equation u2x + u2y + u2z = 1 For a joint whose rotation axis is oriented in the direction of the x axis , one must eliminate ux. For this, we must take into account the following expressions ux = (1 - uy2 - uz2)1/2 ∂ ux u = - z ∂ uz ux
sinθ
(26) Qux = Y ·Quxy + Z ·Quxz (27) Quy = X ·Quyx + Z ·Quyz (28) Quz = X ·Quzx + Y ·Quzy Where X, Y and Z will be equal to 1 in the cases in which the rotation axis is in the direction of the corresponding letter and zero in the rest of the cases. Substituting these results back into Equation (17) yields:
Qθ can be written as: ux uy uz , , ) 2 2 2
2
)2)/ux +
This approach is repeated when the joint axis is oriented in the direction of the x and z axes. We group the different quaternions Q that have been obtained in the previous paragraph in the following way:
∂θ
Qθ= ( 0,
θ
the simplification v θ =(1-cosθ) has been used. For a prismatic joint : t·ux w=
(23)
t·uy t·uy
By representing ∂q ∂ uy
x x = q·Quy
Below, the total differential of the function “w” will be calculated. For this, we will take into account the rotation axis or of translation of the joint.
(24)
137
Differentiating the vector w we have: dw =
∂w ∂d
∂w
·∆d +
·∆px +
∂ px
∂w ∂ ux
∂w ∂ py
·∆ux +
·∆py +
∂w ∂ uy
∂w ∂ pz
Substituting Equation (37) into (35) we get :
·∆uy +
∂w ∂ uz
dQ = Q ∗
·∆uz +
k =1
(qk+1 ∗ ... ∗ qn ∗ qTCP) taking into account:
(32)
·∆pz
n
å (qk+1 ∗ ... ∗ qn ∗ q0)-1 ∗ δqk ∗
(qk+1 ∗ ... ∗ qn ∗ qTCP)-1= qTCP -1 ∗ qn-1 ∗ ... ∗ qk-1 ∗ qk+1-1
All the relationships found so far, for quaternions as well as for vectors, refer to just one joint that separates two consecutive links. But the objective that is pursued is to obtain the mathematical expression that contains the differential variations of the parameters of the model in the position of the end effector for a manipulator robot of n links. Sumarizing, we want to obtain the errors in the position of the end effector which are due to differential variations in the parameters d, ux, uy, uz, px, py and pz of the different joints.
dQ=Q ∗
n
-1 -1 -1 -1 å ( qTCP ∗ qn ∗ ....... ∗ qk ∗ qk+1 ) ∗
k =1
∗ δqk ∗ (qk+1 ∗ ... ∗ qn ∗ qTCP) = n k −1 −1 −1 q TCP ∗ ∏ q n − j ∗ δq k k =1 j=0 = Q ∗ δQ
Q
n ∗ ∏ q i ∗ q TCP i = k +1
Understanding that now the symbol product of quaternions. Since we can write dQ = Q ∗ δ Q
If we apply the analysis in the preceding section to a n link manipulator, we are able to derive the position and orientation errors at the tool frame. B.1 Relationships of the quaternion differential dQ with the geometric direction errors of the robot
(40)
∏ represents the (41)
We obtain : δQ = n n (42) k −1 −1 −1 q TCP ∗ ∏ q n − j ∗ δq k ∗ ∏ q i ∗ q TCP k =1 i=k +1 j=0
We can express the deviation from the expected end position by an error quaternion dQ: (33) Q + dQ =(q1+dq1) ∗ .... ∗ (qn + dqn) ∗ qTCP Expanding Equation (33) and ignoring second-order products we get:
∗ qk-1 ∗ dqk ∗ qk+1 ∗ ... ∗ qn ∗ qTCP) and knowing that
(39)
we obtain:
B. Geometric errors in the robot.
Q + dQ = q1 ∗ q2 ∗ ... ∗ qn ∗ qTCP +
(38)
According to the expression (31) we have: δqk = Qθk·∆θk + Quxk ·∆uxk + Quyk ·∆uyk + Quzk·∆uzk Following a different approach, which states that the error in Q may be defined as small rotations δθ xTCP , δθ yTCP and δθ zTCP about the x, y, and z axes
n
å (q1 ∗ ...
k =1
(34)
respectively:
Q = q1 ∗ q2 ∗ ... ∗ qn ∗ qTCP we have:
δ Q = Q(x, δθ xTCP )Q(y, δθ yTCP )Q(z, δθ zTCP )
(43)
it may be shown that [5]:
n
dQ= å (q1 ∗ ... ∗ qk-1 ∗ dqk ∗ qk+1 ∗ ... ∗ qn ∗ qTCP) k =1
(35) On the other hand, substituting Equation (30) into (35) we have:
δQ = [0,
δθ yTCP δθzTCP
δθxTCP
] (44) , 2 2 2 B.2 Relationships of the vector of differential displacement dw with the geometric errors of the robot ,
n
dQ= å (q1 ∗ .. ∗ qk ∗ δqk ∗ qk+1 ∗ ..qn ∗ qTCP)
(36)
The vector that represents the final position of the end effector is obtained through the expression (14). Let j be the number of addends . Where j=1,2,.....n+1 being n the number of degrees of freedom. Let K be the number of functions that there is in each addend. Where k=1,2,....2j-1
k =1
Furthermore: q1 ∗ ... ∗ qk =Q ∗ (qk+1 ∗ ... ∗ qn ∗ qTCP)-1
(37)
138
From the expressions (42), (44), (46) and (47) we obtain the relationships between the displacement and direction errors of the end effector (dxTCP, dyTCP, dzTCP, δθx TCP, δθy TCP y δθz TCP) with the geometric errors of
The expression of W, can be simplified through a function φ , that represents each one of the elements that form part of the expression of W, and that takes the value of a quaternion or of a vector according to: Let φ be:
the robot (∆dk, ∆uxk, ∆uyk, ∆uzk, ∆pxk, ∆pyk, ∆pzk). These expressions may be formulated as coventional Jacobian representation by
-If kjÞ Φ k = ~ q 2 j− k Þ The function will be a conjugate quaternion Thus the expression of vector W will be: n +1 2 j−1 W= å ∏ φ j=1 k =1 k Differentiating Equation (45) we get:
δPTCP = J·δk
∆d ∆ ux ∆ uy N7 ∆ uz 0 ∆p x ∆ py ∆ pz The Nj are matrices of three rows and the number of columns is equal to the dimension of their corresponding ∆ vector (for example N2 and N9 depend on the dimension of the vector ∆ux ). Equation (48) is the equation that relates small variations in the kinematic parameters, δk, to variations in the end effector pose as given by δPTCP. The Jacobian, J, is a function of the nominal kinematic parameters as well as the joint angles so the relationship may vary significantly over the workspace. dx TCP dy TCP dzTCP N1 N2 N3 N4 N5 N6 = δθxTCP N8 N9 N10 N11 0 0 δθ yTCP δθzTCP
(45)
n +12 j−1 f ∂φr r dW = å å å φ ∗ L ∗ r dpar s ∗L ∗ φ2 j−1 = 1 j=1 r =1 s =1 ∂pars
r −1 2j − 1 ∂φ r )( ∏ φ t ) dpar sr å å ( ∏ φt) ( ∂ par sr t = r + 1 r = 1 s = 1 t = 1
( n + 1) ( 2 j − 1)
å
j =1
f
(46) -If Φr is a quaternion, then f=3 since it will depend on three parameters, as the vector of the parameters of the quaternion Φr , is: parr =[ θr , u ar, u br]
IV. EXPERIMENTAL RESULT
If Φr is a vector the following cases are presented - If the joint is revolute, f=5 since it will depend on five parameters, as the vector of the parameters of Φr is:
We carried out simulations to verify the calibration algorithm [13] using the developed model in [5] and the developed differential relationships in this paper. The manipulator structure used for the simulations was that of a six-joint, PUMA 560 robot manipulator. Sets of 15, 30, and 50 calibration points (locations) from both left-and right-arm configurations of the PUMA robot were used in the experiment. The error model was applied for error parameter estimation. We developed an identification algorithm for robot geometric errors and the experiment data revealed that the advantages of quaternion-vector pairs in these respects may make them an attractive choice in robot calibration.
parr =[ θr , u ar, u br , p ar, p br] -If the joint is prismatic, f=3 since it will depend on three parameters, as the vector of the parameters of Φr is: parr =[tr, u ar, u br ] The expresssion of dW, is a vector that has three components, which are all meaningful. Because of this, multiplying transformations have not been used as in the case of the rotations provided by quaternions, therefore ∂Φ
the terms
r
∂par
r s
are obtained through an ordinary
A. Position and orientation error
Errors between the predicted pose and measured pose were quantified in the following manner. An orientation and position error is then defined as -for the TCP orientation: δθ = δθ2x + δθ2y + δθ2 z
differentiation Following a different approach, which states that the error in W may be defined as small displacements from W of dxTCP,dyTCP and dzTCP about the x, y, and z axes respectively: dW = (dxTCP,dyTCP,dzTCP)
(48)
Hence
-for the TCP position:
(47)
139
δr =
2 2 2 dx + dy + dz
directly but it is necessary to obtain the differential transformation matrix, which involves a high computacional cost. The main purpose of this work is to propose to the robot calibration that quaternions, coupled with a displacement vector, function as very elegant and efficient spatial transformation operators. With the additional advantage of being very compact, and therefore storage efficient, quaternion/vector pairs provide an attractive alternative to the homogeneous matrices. In addition, quaternions seem to offer a more intuitive framework for using and understanding the effects of three-dimensional rotations because the axis and angle of rotation appear explicitly in their definition.
These errors are used to evaluate the accuracy enhancement given by the identified model. Figure 1 and Figure 2 show us the position and orientation error for each location according to the number of iterations.
REFERENCES Figure 1. Position error for each location and for each iteration
[1] B.M. Mooring, Z.S Roth and M.R. Driels, Fundamentals of Manipulator Calibration, John Wiley New York, 1991. [2] R.H. Taylor, Planning and Execution of Straight Line Manipulator Trajectories. IBM, J. Res. Devel., 23, 4, pp. 424-436, 1979. [3] J. Funda, R.H Taylor and R.P. Paul, On Homogeneous Transforms, Quaternions, and Computational Efficiency. IEEE Transactions on Robotics and Automation, 6, nº3, pp.382-388, 1990. [4] W.R. Hamilton, Elements of Quaternions, I and II .New York: Chelsea, 1869. [5] M.A. Pérez Rueda Aplicación del álgebra de cuaternios a la calibración de robots, Thesis, Univ. Valladolid, 2000. [6] E. Pervin, and J. A. Webb, Quaternions in vision and robotics. Dept. Comput. Sci., Carnegie-Mellon Uiniv., Tech. Rep. CMU-CS82-150, 1982. [7] J.Chen and L.M.Chao. Positioning error analysis for robot manipulators with all rotary joints. In IEEE International Conference on Robotics and Automation, pp 1011-1016, 1986. [8] J.Chen and Y.F.Chen. Estimation of coordinate measuring machine error parameters. In Proceedings of 1987 IEEE International Conference on Robotics and Automation, pp 196-201, 1987. [9] S. Hayati and M.Mirmirani. A software for robot geometry parameter estimation.SME Paper #MS84-1052, presented at Robots West Conference, Anaheim, CA, 1984. [10] Samad A. Hayati. Robot arm geometric link parameter estimation. In Proceedings of the 22nd IEEE Conference on Decision and Control, pp.1477-1483, 1983. [11] Robert P. Judd and Al. B. Knasinki. A technique to calibrate industrial robots with experimental verification. In proceedings of 1987 IEEE International Conference on Robotics and Automation, pp.351-357, 1987. [12] W.K. Veitschegger and Chi-Haur Wu. Robot accuracy analysis based on kinematics. IEEE Journal of Robotics and Automation, RA-2(3): 171-179, 1986. [13] M.A. Pérez Rueda Geometrical parameter identification for robot calibration using quaternion algebra. Modeling, Identification and Control. MIC 2001. Austria, pp. 663-667, 2001.
Figure 2: Orientation error for each location and for each iteration. It can be observed how the error decreases while the number of iterations increases. Therefore, it is deduced that the algorithm is convergent. Furthermore these graphics permit us to select the locations in which the smaller error is produced in each iteration. V. CONCLUSION We have accomplished a novel development of the obtainment of the jacobian through the diferentiation of quaternion-vector pairs, for its application to the robots calibration. In this work, a procedure to identify both the axis orientation error and axis position error for a robot has been proposed. In the obtained results, we can observe that the orientation error depends on some errors on the geometric parameters ∆ θ , ∆ux, ∆uy, ∆uz while the position error depends on all the geometric parameters. The algorithm that uses the quaternion-vector pairs gives us directly the position error, while in the algorithm that uses matrices the position error is not obtained
Mª Angeles Pérez Rueda received the M.S. and Ph.D. degrees, both in mechanical engineering, from the University of Valladolid, in 1993 and 2000, respectively. She is currently Assistant Professor in the Mechanics Engineering Departament from the University of Valladolid (Spain). Since 1996 she is a member of the Institute I.T.A.P. (Instituto de las Tecnologías Avanzadas de la Producción). Her research interest are robot calibration, Graphical robot aplications simulation and industrial processes automation.
140
