Optimal Measurement Configurations for Gough Platform Calibration David Daney LORIA, INRIA Lorraine, SPACES Team, 615, rue du Jardin Botanique F-54602 Villers-les-Nancy Cedex,
[email protected] Abstract The robustness of robot calibration with respect to sensor noise is sensitive to the manipulator poses used to collect measurement data. The present paper proposes an algorithm based on a constrained optimization method which allows one to choose an set of measurement configurations. It works by selecting iteratively one pose after another inside the workspace, and after a few steps a set of N configurations is obtained which maximizes an index of observability associated to the parameter Jacobian. Our algorithm converges to a set of poses that are localized at the boundary of the articular workspace. Finally, a validation through simulation of a calibration experience shows that selected configurations improve significatively the kinematic parameters identification by dividing by 10 to 15 the noise associated to the results.
1 Introduction The Gough/Stewart platform offers high accuracy, for the manipulation of heavy objects, even at large velocities. This explains its use in a wide range of industrial applications, from flight simulation to the more recent high-speed machine-tools. Due to defects in manufacturing or assembly, it is wellknown that the geometry of robotic manipulators does not exactly match the design goals. A direct consequence is reduced accuracy of the manipulator, since robot control heavily relies on a precise description of the kinematic model. One way to tackle this problem is by improving the theorical kinematic model by means of kinematic calibration. This procedure allows an identification of kinematic parameters through a redundant information on the state of the robot provided by measurement, which one can see as constraints on the kinematic parameters. The noise associated with sensors implies an error on the calibration results which has to be minimized. To improve the robustness of the calibration methods, several solutions are known: increase the number of constraint equations, take into account the knowledge on the noise distribution associated to sensors and/or act on the nu-
merical quality of measurements. As shown in [2], the best results are obtained through a judicious choice of the robot configurations used during the measurements. Several works in robotics have been presented to produce optimal experimentation plan. First, one has to define different indices, associated with a set of poses, and to check their effects on robusteness calibration result. In [2, 1, 4, 6], the authors propose to observe the parameters Jacobian JP (matrix of the derivatives of constraint equations with respect to the kinematic parameters P) through its singular values – see section 3. Then an algorithm is used to maximise the observability index as a function of some parameters defining a measurement pose. This leads to the selection of an optimal set of configurations. Many solutions have been proposed. Khalil [4] uses a conjugate-type optimization method. Zhuang [12] tries to avoid local minima by using a Simulated Annealing approch or a Genetic Algorithm [13]. Born and Menq [1] study the determinant of JPT JP to determine how one should modify pose parameters to maximize their index. In this paper, we propose to adapt an algorithm named DETMAX [9], widely used in the numerical community for the design of experiments. This algorithm is used to maximize the determinant of JPT JP . However, section 4 outlines a general version of this algorithm and the reader can adapt it to various observability indices. Then, its implementation for the specific case of the Gough platform calibration by an Inverse Method is described. Finally, a set of optimal configurations is proposed and validated by a realistic calibration.
2 Modelling and Kinematics of Gough platform This section describes the Gough platform and its kinematic models. The manipulator consists of two rigid bodies, the base and the mobile platform, connected by 6 legs or segments. The leg linear actuators provide 6 dof for the platform pose relative to the base, corresponding to position P and rotation matrix R. A configuration X = [P, R] is associated to 6 length variations Li mea-
sured by “internal” leg sensors, i = 1, . . . , 6. B1 B2 Li
Mobile Frame (C, xr , yr , zr )
B6 B5 B3
Mobile
B4
ρi
X = P, R Legs or segments
li
Base A3
A4 A5
A2 A1
Reference Frame (O, x, y, z)
kinematic parameters NP to be identified. Note that for a given pose ζ of the robot, the dimension of F , which measures the amount of redundant information on the state of the robot, is generally less than the number of unknowns P. Therefore, to obtain the minimal number of constraints necessary to identify the kinematic parameters, it is necessary to collect information from the robot in a variety of different poses. We thereby obtain a system of constraint equations of the following form:
A6
S=
(
F1 (P, M1 [ζ1 ]) ... FN (P, MN [ζN ])
(2)
Figure 1: Gough platform Each leg is attached to the base by a U-joint and to the platform by a ball joint, so 23 parameters are required to model each leg [10]. But as shown in [7], the principal source of error in positioning is due to limited knowledge of the joint centers and to the fact that part of the length is not given by the sensors. We thus use a simpler model with attachment points ai in the reference frame, bi in the mobile frame, and offset lengths li . This gives 7 parameters per leg, therefore 42 overall, denoted by P. Kinematic models are essential for controlling the robots, and yield the constraints used in calibration. Inverse kinematics express the length variation for the i-th leg as a function of X : kP + R.bi − ai k2 = Li + li , i = 1 . . . 6.
(1)
To describe the rotation matrix, we use Euler’s angles parameters q1 , q2 , q3 . For the Gough platform, the Forward Kinematics model is more difficult to compute since it consists in solving (1) for P and R given Li and P. It has been shown how techniques from computer algebra based on Groebner bases can be used in principle to solve this problem. From a practical point of view this is however much more tricky. Faug`ere et al. kindly gave us access to the implementation of their recent work [3] which addresses the practical issues and allows one to efficiently solve this problem.
3 Calibration and Observability index The aim of calibration is to get redundant information on the state of the robot. For this purpose, we may use measurements provided by internal sensors placed along various joints of the robot, external information obtained by measurement machinery and/or additional constraints on the end-effector or joints. These measurements M are related to the kinematic parameters via constraint equations F (P, M). The first problem is to obtain a number of constraints Nc greater than or equal to the number of
where Nc = N × dim(Fj ) ≥ NP , [ζ1 , . . . , ζj , . . . , ζN ] denote the poses in which the robot is observed, and Mj [ζj ] is the data set corresponding to the pose ζj . For the actual kinematic parameters Pˆ of the robot, the equations F1,...,N should be approximately zero. By solving the equations S we may identify the kinematic paramˆ Because of sensor noise and instability eters P = P. introduced by physical constraints, however, the measurements M1,...,N are inexact. Measurement errors are propagated in the resolution machinery and affect the resulting values obtained for the parameters P. One strategy for improving the robustess of the calibration with respect of measurment noise is to get an overconstrained system of equations by increasing N . In practice, this technique yields improved robustess only while the number N of configurations remains less than a certain threshold value (c.f. [5]). We may use the parameter Jacobian JP , defined as the derivative of the constraining equations Fj with respect to the kinematic parameter values P, to ensure that our calibration is robust with respect to measurement noise. To the parameter Jacobian, in turn, we may associate an index of observability O to characterize the sensitivity of constraint equations to variation in the kinematic parameters. The matrix JP is a function of the given measurement configurations; by choosing a pose inside the workspace for which the observability index O(JP ) is optimized, we may improve the robustness of the calibration method. Many indices of observability have been proposed. Born and Menq in [1] use the determinant of JPT JP , while in [2, 4], Driels and Khalil use the condition number of the parameter Jacobian. More generally, Nahvi and Hollerbach [6] define the following indices and others in terms of singular values (denoted by σL ≤ . . . ≤ σ1 ) obtained from a Singular Value Decomposition of JP . √
L
σ ...σ
1 √L where m is the number of measure• O1 = m ment poses and L is the number of singular values. This q index corresponds to the determinant of JPT JP ;
here det(JPT JP ) = σL . . . σ1 . The sensitivity of F with respect to variations of the P is increased when O1 is maximal.
• O2 = σσL1 , the inverse of condition number of J (c.f. [2]). This index has a maximal value equal to 1. Maximizing this index permits us to ensure that no singular value is favored over another or, in physical terms, that the errors incurred in determining the kinematic parameters are “homogenized”. 2 • O3 = σL and O4 = σL /σ1 are slight modifications of O1 and O2 introduced in [6].
+ 2.3 Determine ζN +1 as:
Maximum O(ΥN +1 ) +
(3)
ζN +1 =ζN +1
+ 3 Υ+ N +1 = add(ΥN , ζN +1 ) − + 4 Determine ζN +1 ∈ ΥN +1 as:
Maximum−i=1..N +1 O(rem(Υ+ N +1 , ζi ))
(4)
ζi =ζN +1
4 The algorithm 4.1 Description The following algorithm, used to maximize the observability index, is an adaptation of the standard DETMAX algorithm [9] for experimental design [11]. When using an optimal configuration search it is difficult in general to find all the parameters defining the N configurations of measurement in a single step. To circumvent this problem, we select one additional pose N +1 that will improve our criterion of observability. We then discard the configuration for which the resulting observability index is maximal. The process is iterated until a configuration is consecutively added and removed. We use the following conventions: • ζi denotes the vector of parameters defining the ith configuration of measurement. • ΥN = [ζ1 , . . . , ζN ] is the set of parameter configurations associated with N configurations of measurement. • O(ΥN ) is an index associated to the N measurement configurations to be maximized. • Ω is the workspace of all possible configurations of measurement. • add(ΥN , ζN +1 ) = ΥN +1 is the procedure that adds a measurement configuration ζN +1 to the set ΥN . • rem(ΥN +1 , ζN +1 ) = ΥN is the procedure that removes a measurement configuration ζN +1 from the set ΥN +1 . The following algorithm is used to determine N measurement configurations that maximize the observability criterion O: 1 Initialize ΥN + 2 Determine ζN +1
2.1 Initialize ζN +1 ∈ Ω. 2.2 Construct ΥN +1 = add(ΥN , ζN +1 )
5
− + – if ζN +1 = ζN +1 then STOP, − ΥN = rem(Υ+ N +1 , ζN +1 ) − – else if ΥN = rem(Υ+ N +1 , ζN +1 ) go to 2.
The algorithm is well-adapted to any of the observability indices defined in 3. The standard version of DETMAX is used to maximize the determinant of JPT JP (∼ O1 ). There, the operations of addition and removal of a measurement configuration yield reduced complexity and run time. The reader should consult [11] for more information. 4.2 Adaption and implementation First, we formally define the vector of configuration parameters ζ introduced previously. We could have chosen to use articular coordinates, but instead we prefer to encode configuration parameters using the position and the orientation (Euler’s angles) of the robot ζ = X = [Px , Py , Pz , q1 , q2 , q3 ]; c.f.(section 2). The reason is that generalized coordinates are well-suited for defining the parameter Jacobian associated with the calibration method presented in section 5 to test this algorithm. Our algorithm is implemented in Matlab. In step 1, the Jacobian JP is initialized using N randomly chosen configurations ΥN = [X1 , . . . , XN ]. In step 2.2 of 4.1, we add a new line to the Jacobian whose entries are functions of the configuration of measurement XN +1 initialized in step 2.1. We let JP∗ denote the resulting matrix. The procedure (3)(step 2.3) is then used to maximize the observability index (O1 , O2 , O3 or O4 ) of JP∗ as a function of the configuration parameters of XNc +1 , and to produce a new configuration XN+c +1 . To determine the latter configuration, we use Matlab’s constr function to implement a constrained optimization method based on Sequential Quadratic Programming. This function is sensitive to local maxima and so, to correct for this problem, we execute step 2 for Nb = 40 estimates of XN +1 . These Nb configurations XN +1 are uniformly distributed inside the workspace. The pose for which O(JP∗ ) is maximal is then kept. Using this strategy we obtain results that are less sensitive to local maxima, but which do not necessarily globally maximize our observability criterion. Another problem is that of guaranteeing that the configuration XN+c +1 determined by the procedure (3) lies within
the workspace. To correct for that, we follow the SQP method and posit articular constraints on the leg lengths of our robot. Here we use Inverse Kinematics to calculate the articular coordinates as functions of XNc +1 . Finally, to our list of constraints we add additional constraints in order to assure that all configurations obtained by our optimization procedure are physically realistic. In step 3 we add a line corresponding to the configuration XNc +1 to the Jacobian JP . Finally, in step 4 of 4.1, we con−[i] struct N + 1 matrices JP in succession, where, for each i = 1, . . . , N + 1, the corresponding configuration Xi has been removed from JP∗ . The pose Xk associated to the Ja−[k] −[k] cobian JP that maximizes O(JP ), is discarded. The algorithm stops, in step 5, when Xk = XN+c +1 .
The aim of this paper is to find the configurations which maximize an observability index of the matrix JPi for the calibration of one leg i of the Gough platform. This process will be repeated for all segments i = 1 . . . 6. In this case, we fix the values of the kinematic parameters to a given value, such as the one provided by the robot constructor. But, to observe the influence of the same set of poses to calibrate all robot legs in one step, we introduce the jacobian J where the diagonal is constitute by the parameter jacobian JPi of each leg. Each block matrix JPi are built for each vector of kinematic parameters Pi but with the same measurment datas, as : JP =
Ã
JP
0
1 .
0 0
. 0
0
.
0 JP
6
!
(7) 6Nc ×42
5 Inverse calibration method 6 Results We test the method of optimal configuration search on the parameter Jacobian provided by a basic calibration method. The Inverse Method [14] presents the more popular method to calibrate a parallel robot. It consists to quantifiate the pose error indirectly through the leg length error. The constraint equations are directly provided by the implicit inverse kinematic equations (5). FiIK = kP + R.bi − ai k2 2 − (Li + li )2 , i = 1, . . . , 6. (5) Where the unknowns are 7 × 6 kinematic parameters Pi = [ai , bi , li ] with i = 1 . . . 6 is the index of the segment and measurements are Mj = [Pj , Rj , Li,j ] with j = 1, . . . , N index of the configuration. Each constraint follows from (5) and depends only on the 7 parameters of the respective leg, so we can decouple the robot calibration in 6 subproblems and take N ≥ 7 measures. In the present paper, we shall use a least squares method (using function Leastsq of Matlab, implementing a LevenbergMarquart algorithm) to solve these equations in terms of the parameters. Namely, we solve the following minimization problem, for each leg i = 1..6 :
Ci,j =
PN
j=1 Ci,j IK T IK [Fi,j (Pi , Xj , Li,j )] [Fi,j (Pj , Xj , Li,j )]
M inimum
To find out in which direction we should look for the minimum, we compute the jacobian matrices corresponding to the sub-calibration of each leg i independently, namely IK ∂Fi,j=1..N
. Each line j = 1, . . . , N of this matrix J Pi = ∂Pi (see [14]) is done by: T
[−Pj + Rj .bi − ai T , (RjT (Pj + Rj .bi − ai )) , −(Li,j + li )] (6)
We will use the Inverse Method to identify the kinematic parameters of leg 1 of INRIA’s Left Hand Robot (see [8]). As preparation, we apply the algorithm 4.1 to the Jacobian matrix (6) to find poses that minimize the effect of measurement noise. First we use the configuration search method presented earlier to determine a maximal value of O1 – section 3. Note that at the end of the process 4.1 only a subset of the initial set of N random configurations will have been modified, corresponding to 7 lines of the N × 7 Jacobian JP . Here seven is the minimal number of configurations need to identify the kinematic parameters P of leg 1. As predicted, the observability indices associated with the new set of configurations have increased relative to the old values: O1 has increased by 900%, O2 by 11860%, O3 by 12580% and O4 by 1.5 × 106 %. Furthermore, the optimal poses obtained from algorithm 4.1 have the desirable property of tending to converge to optimal poses on the boundary of the articular workspace, where leg lengths are minimal or maximal. The figure 2 presents these poses in term of their articular coordinates. We now repeat the above procedure with O2 , O3 and O4 , respectively, in place of O1 (step 2.1 of 4.1). In each case, we observe an undesirable tendency among certain configurations to converge at poses near the initial ones that we attribute to the presence of many local maxima. When leg 1 is replaced by legs 2 through 6 in succession, similar results are obtained. We therefore use the maximization of O1 as a selection criterion for measurement poses of each of our robot legs. As stated, our search method produces poses for the calibration of a single leg. To calibrate our robot, therefore we could proceed by obtaining distinct sets of measurement configurations for legs 1 through 6. However, it is less costly to determine a set of measurement configurations that is simultaneously optimal for every leg. For that purpose, we begin by constructing the set of 26 = 64 articular configurations corresponding to all pos-
Normalized leg length
Maximal leg length =1 Minimal leg length =0
er mb Nu s
nt me seg of
s tion te ura rdina g i onf coo e c lar f tharticu o r e y mb n b Nu give
use Υr to obtain 6 × 7 = 42 kinematic parameters that we denote by Pr . In the second we use Υs to obtain Ps . Discrepancies between estimated and actual values for our 42 kinematic parameters are listed in figure 3. The error |Pe − P| obtained before calibration is in column 3, and the remaining columns give the errors associated with poses obtained as a result of our calibration procedure. The post-calibration error |Pr − P| associated to 18 randomly chosen poses is in column 2, while the error |Ps − P| associated to 18 poses chosen as in Section 6 is in column 1. As we see in Figure 3, selecting poses as in
Figure 2: Leg length of seven optimal configurations which maximize the criterium o1
sible poses with legs of minimal or maximal length. For each of these we then use Forward Kinematics to calculate all corresponding poses defined in terms of generalized coordinates. After eliminating unrealistic configurations, we are left with 50 configurations of measurement expressed in terms of generalized coordinates. From among these, we use the algorithm 4.1 to obtain the subset of configurations which optimize the observability O1 of the Jacobian J (7). We obtain 18 symmetric configurations. We will now show how to obtain more robust calibration by using these configurations, which lie on the boundary of the workspace.
Error on kinematic parameters [cm]
After calibration (18 selected poses) After calibration (18 random poses) Before calibration
0.6 0.4 0.2 40 rs e et
0
30 ram pa ic at
3
2 Magnitude of error on: − kinematic parameters estimation: 0.5 cm 1 − position / leg lengths measurement: 0.001 cm − orientation measurement: 0.01 degrees
Error on kinematic parameters Mean [cm] Standard deviation [cm]
Before calibration
10
20 em kin
42
After calibration using random selected poses poses
0.2705
0.0335
0.0023
0.1450
0.0444
0.0017
7 Validation To validate our choice of boundary configurations, in turn, we will use a simulation of the Inverse Method described in section 5. Here, we represent the robot design given by constructor in terms of the 42 kinematic parameters described in [8]. These are denoted by Pe , and serve as initial estimates. They are then perturbed by 0.5 cm to simulate a realistic robot design, which we will denote by P. We now substitute these values into equation (1) to determine the articular coordinates Li as functions of two sets of poses Υr and Υs expressed in terms of generalized coordinates. Υr consists of 18 randomly generated poses (O1 (J (Υr )) = 1.7, O2 (J (Υr )) = 0.001, O3 (J (Υr )) = 0.8, O4 (J (Υr )) = 0.0009) and Υs consists of the 18 selected poses (O1 (J (Υs )) = 7.5, O2 (J (Υs )) = 0.015, O3 (J (Υs )) = 11.15, O4 (J (Υs )) = 0.16) on the boundary articular workspace determined in the preceding section. To each of these sets of poses we now add a uniformly distributed error on measurement parameters to simulate realistic sensor noise. The amplitude of error on our measurement parameters is 0.001 cm for position, 0.01 degrees for the Euler angles defining orientation and 0.001 cm for leg length. Next we apply two different simulations of the Inverse Method to each leg i = 1, . . . , 6. In the first we
Figure 3: Comparison of Gough platform calibration with and without measurement configurations selection
Section 6 leads to kinematic parameter estimates whose errors that are 10 to 15 as small as those obtained from randomly chosen configurations. In practice, increasing the number of poses until a certain threshold may improve the robustness of calibration with respect measurement noise but increase the time needed for obtaining data necessary for calibration ([5]). To illustrate the above phenomenon, we simulate 43 successive calibrations under the same experimental conditions presented at the beginning of the section, while varying the number of measurement poses between 8 and 50. Figure 4 compares mean errors for kinematic parameter estimates obtained using configurations lying on the boundary of the workspace and those randomly chosen. Observe that the errors computed using randomly chosen poses decrease steadily as a function of the number of configurations, while the errors obtained from a calibration method using carefully chosen configurations are roughly constant for configurations of eighteen or more
0
Acknowledgments
Mean of error on kinematic parameters [cm]
10
Experiment conditions: Magnitude of error on: − kinematic parameters estimation: 0.5 cm − position / leg lengths measurement: 0.001 cm − orientation measurement: 0.01 degrees
The author would like to acknowledge Luc Pronzato, Jean-Pierre Merlet, Ethan Cotteril, Luc Rolland and Guillaume Hanrot for their varous suggestions and their helps.
−1
10
Before calibration Random poses After calibration
−2
10
Boundary workspace poses −3
10
0
20 30 40 8 10 Number of measurement configurations
50
Figure 4: Influence of the number of poses on calibration result
References [1] J.H. Borm and C.H. Menq. ”Determination of optimal measurement configurations for robot calibration based on observabililty measure.” J. of Robotic Systemes, 10(1):51–63, 1991. [2] M. R. Driels and U. S. Pathre. ”Significance of observation strategy on the design of robot calibration experiments”. J. of Robotic Systemes, 7(2):197–223, 1990. [3] J.C. Faug`ere, J.P. Merlet, F. Rouillier and L. Rolland, ”Forward position analysis for parallel robots”, draft.
poses. We may conclude that choosing configurations along the boundary of our workspace permits a robust calibration of the Gough platform. Using a few carefully chosen poses is more effective for calibration than using many random poses. Moreover, our claims have been tested for a wide variety of Gough platforms, with the same positive results.
8 Conclusion Our results show that by choosing measurement configurations that maximize the O1 observability index we may significantly increase the robustness of Gough platform calibration with respect to measurement noise. Note, however, that maximization with respect to different indices related to our parameter Jacobian has varying effects on calibration and that the appropriate choice of index depends on the effects intended. In our case, choosing O1 in place of O2 , O3 or O4 is done to reduce the sensitivity of our constrained optimization method to local maxima. The problem of sensitivity to local maxima could be avoided entirely by using a global optimization method in place of step 2.3 of 4.1, but the goodness of our calibration results suggest that this is unnecessary. By contrast, it would be interesting to explore further the relationship between the geometry of the poses obtained by our algorithm and the numerical properties of our constraint equations. Among the most intriguing properties of our poses are that they lie on the boundary of our articular workspace and that they are highly radially symmetric. It seems natural to conjecture that the symmetry of our Gough platform and the configurations we obtained are linked; more optimistically, we might hope to reduce the number of kinematic parameters needed to define a set of poses optimal for calibration.
[4] W. Khalil, M. Gautier and Ch. Enguehard. ”Identifiable parameters and optimum configurations for robots calibration.” Robotica, 9:63–70, 1991. [5] A. Nahvi, J.M. Hollerbach and V. Hayward. ”Calibration of a parallel robot using multiple kinematics closed loops.” In ICRA, 407–412, San Diego, 1994. [6] A. Nahvi and J.M. Hollerbach. ”The noise amplification index for optimal pose selection in robot calibration.” In ICRA, 647–654, Minneapolis, 1996. [7] O. Masory, J. Wang and H. Zhuang. ”Kinematic modeling and calibration of a Stewart platform.” Advanced Robotics, 11:519–539, 1997. [8] J-P. Merlet.”Parallel robots.”Kluwer, Dordrecht, 2000. [9] T.J. Mitchell. ”An algorithm for the construction of D-optimal exerimental designs.” Techometrics, 16:203– 210, 1974. [10] P. Vischer. Improve the accuracy of parallel robots. ´ PhD thesis, Ecole polytechnique f´ed´erale de Lausanne, 1996. [11] E. Walter and L. Pronzato. ”Identification of parametric models.” Springer, London, 1997. [12] H. Zhuang, K. Wang, and Z.S. Roth. ”Optimal selection of measurement configurations for robot calibration using simulated annealing” In ICRA, 393–398, 1994. [13] H. Zhuang, J. Wu, W. Huang, ”Optimal Planning of Robot Calibration Experiments by Genetic Algorithms”. In ICRA, 981–986, Minneapolis, 1996. [14] H. Zhuang, Y. Jiahua and O. Masory. ”Calibration of Stewart platforms and other parallel manipulators by minimizing inverse kinematic residuals.” J. Robotic Systems, 15:395–405, 1998.
