Proceedings of the 2(X)0 IEEE International Conference on Robotics & Automation
San Francisco, CA • April 2000
On-Orbi t Cal ibrat ion of the S P D M F o r c e - M o m e n t Sensor
g
u
F. ASpace System, Cana
Saint-Hubert, Q
Abstract
A six-axes force/moment sensor can be completely characterized by a 6 x 6 gain matrix whose elements can be estimated from a sequence of sensor outputs and applied forces. Unlike the conventional calibra- tion procedure that uses an external hardware to pro- duce the load forces, we present a calibration procedure that applies inertial forces as a result of movement of a manipulator payload in free space. The subsequent inertial forces and moments are calculated by using the Newton-Euler formulation assuming known iner- tial parameters (mass, center of mass, inertia tensor) of the payload. An extended Kalman filter is employed to perform the estimation of the sensor gain matrix based on the measurements of the manipulator veloc- ity and the sensor outputs. The performance of the proposed estimator is evaluated by simulation.
1 I n t r o d u c t i o n
The Special Purpose Dexterous Manipulator (SPDM) plays an important role in maintenance of the Interna- tional Space Station. It has been understood tha t the capability of the manipulator to perform contact tasks is totally dependent on an adequate force/moment feedback. Since the force feedback is established through the manipulator six-axes force/moment sen- sor, an accurate calibration of the sensor is crucial to the overall functionality of the SPDM. In order to convert the sensor readings into the actual forces and moments applied to the sensor, a correct calibration matr ix is required [9]. It has been understood that the calibration matr ix has to be updated frequently to cope with the proper ty variation of the sensor. In essence, any calibration procedure for force/moment sensor requires a sequence of sensor readings in re-
sponse to various load cases where the applied forces and moments are accurately known. The common procedure is to use a system of pulleys and weights to apply three orthogonally oriented components of
force and moment at the center of the sensor. In the zero-gravity environment, tha t is not a viable proce- dure, and using any other device to generate the ex- ternal forces demands its own calibration means. This work reports a novel procedure for force /moment sen- sor calibration that produces the required forces and moments as a result of general movement of the ma- nipulator end effector grasping a payload. The obvious advantages of this approach is tha t there is no need for external hardware. We considered the SPDM payload mechanism as a rigid-body whose inertial parameters, i.e. mass, center of mass, inertia tensor, are assumed to be known. The procedure for identification of the inertial parameters is documented in [2].
This paper is organized as follows. A modeling of a six-axes force/moment sensor is briefly explained in Section 2. In Section 3, the Newton-Euler formula- tion is used to express the equation of motion of the manipulator payload in a linear regression equation adequate for a least square estimator. Section 4 is devoted to plan an adequate motion t ra jec tory along which the subsequent force signals are persistently ex- citing - the persistent excitation condition ensures the convergence of the est imated parameters to their t rue values. In Section 5, the estimation problem is recast in the context of extended Kalman filtering, to cope with sensor noises and to relieve the need for accelera- tion measurement. A computer simulation developed in MATLAB is presented in Section 6 to evaluate the performance of the proposed estimator.
2 F o r c e / M o m e n t Sens ing
A six-axes force-moment sensor calculates applied forces and moments by measurement of induced
strains or displacements at some points [9]. Assum- ing elastic deformation of the sensor body, we can re- late the vector of measured strain e E ~n, n > 6, to the vector of generalized force-moment, which is also
0 3
known as wrench vector,
wig.] ns
by the sensor compliance matrix C8 E Nnx6 as
(1)
e = C s w .
The sensor output , @ E ~s, is converted from strain measurement through its calibration matrix, C E ~6Xn
= Ce. (2)
Thus, from the above we have
= C w, where C = CC8. (3)
C E ~6x6 is called sensor gain matrix. Ideally C = I, where ! is the identity matrix. This equality can be achieved if the nominal calibration matr ix (3 is any generalized inverse of the compliance matr ix C , . How- ever, since (3 is a constant matrix, any variation in the sensor compliance causes deviation of C from the identity. Hence C should have the form
c = i + A , (a)
where ~ is an additive per turbat ion matrix. The C matr ix characterizes the sensor, because when the ma- trix is identified, the actual force-moment w can be resolved from the sensor readings @ by
w = C - I & .
As a consequence, in order to extract the t rue values of the force-moment, the matr ix C must be invert- ible. By virtue of (4) one can show that the matr ix C is invertible if I1~11~ < 1 (H" II denotes the induced norm). This is a reasonable assumption to make be- cause II ~11 ~ indicates the error of the nominal calibra- tion which is typically less than 100%. The following sections shall describe a procedure to est imate matr ix C.
3 Force /Moment Est imat ion Using the Newton-Euler For- mulation
Since the force/moment sensor is located between the manipulator wrist and payload, the sensor can detect the reaction forces and moments due to the movement of the payload in free space. We can calculate the force
3 6
Figure 1: Coordinate system of interest with the ma- nipulator force /moment sensor.
and moment disturbances from the motion of the ma- nipulator end effector by making use of the payload forward dynamics. The following assumption is made for the calculation; (i) the payload is a rigid-body, (ii) the inertial parameters of the payload (mass, center of mass, inertia tensor) are precisely known, (iii) the measurement of the manipulator 's end-effector Carte- sian velocity is available. A schematic kinematics of the manipulator force /moment sensor along a payload is illustrated in Figure 1. Three different coordinate systems are used to describe vectors; {O} is the iner- tial coordinate system while {S} and {C} are parallel coordinate systems at tached to the sensor's center and the payload's center-of-mass, respectively. Note that {S} and {C} are oriented along the sensor axes but are not necessarily oriented along the principle axes of the payload inertia tensor. Henceforth, all vectors are expressed in the coordinate system {C} unless oth- erwise specified. T h a t is because the components of the measured force-moment and the inertial forces are naturally expressed on this coordinate system. Let v and ~ be the linear and angular velocities of the pay- load center-of-mass all are expressed in {C}. Also let cl E ~m denote the vector of joint angular velocities w h e r e t o > 6 - for t h e S P D M m = 7. Then, given the manipulator Jacobian 1 Jc(q) E ~6×~ writ ten in
1In order to change the Jacobian reference frame from {O} to {C}, we have
0 R Jo(q).
R is rotation matrix associated with the orientation of the end effector with respect to {O}.
0 4
frame {C}, we have [4]
f~ = v = Jc(q)cl (5)
where v E ~6 represents the general velocity. Since the payload is considered to be a rigid-body, its dy- namics can be fully captured by the Newton-Euler for- mulation. The equation of motion of the payload writ- ten in the moving frame {C} is
fc = m ( 9 + 1 2 x v ) ,
n~ = I ~ + f t × ICft. (6)
In the above, fc and nc are the inertial force and mo- ment, expressed in {C}, acting on the center-of-mass of the payload while m and Ic denote the payload's mass and inertia tensor, respectively. The following transformation is needed to express the inertial force and the moments in the sensor coordinate system {S},
- 5 = - n , = c × f c + n ~ , (7)
where c denotes the center-of-mass. Note that the neg- ative sign in the left-hand-side of the above equation is due to the fact that the reaction of the inertial force- moment is seen by the sensor in the opposite direction. Substituting fc and n¢ from (6) into (7) readily gives
[ m('i, + 12 x v) ] mc x ~ + me x ( f tx v) + Ic~ + fi x I~fl
(s) Recall from (1) that the wrench vector w includes the actual force and moment applied to the sensor. Equation (8) shows how to calculate the vector of generalized force-moment disturbances seen by the force/moment sensor when the manipulator maneu- vers in free space. It is evident that the values of the Cartesian velocity and acceleration are the required inputs to complete the calculation. The SPDM joints are instrumented with tachometers for measurement of joint velocity, hence the linear and angular veloci- ties can be computed from the joint velocities by using the Jacobian mapping (5). However, the manipulator is not equipped with acceleramometer for acceleration measurement. In this section we assume that the ve- locity signal is noiseless so that the acceleration in- formation can be faithfully derived by using numer- ical differentiation. This assumption will be relieved in the next section where acceleration is treated as a state vector to be estimated.
Equation (3) can now be re-written in the following linear regression form
= @(v,/s)O (9)
3 6
where • E ~sx36 and O E ~36 are defined as
W T 0 " ' " 0
0 W T ' ' " 0 I I J = : : " . . :
0 0 ..- w T
(10)
O ---- I t 1 , " " " , C61 T , ( 1 1 )
and ci is the ith row vector of the C matrix. Now the standard least-squares algorithm [5] can be applied to (9) to estimate the desired parameters ®. Note that O and C contain the same set of parameters but with different arrangement.
4 P a t h P l a n i n g
It is understood that the exponential convergence of the estimated parameters to the true values depends upon the excitation of regressor [1, 6, 8]. Therefore, it becomes necessary to maneuver the manipulator in such a way that the induced input signals contains rich information about the system modes - for instance, moving along a straight line without changing orien- tation cannot produce any moment. It has been shown that [8] a regressor is persistently exciting if there ex- ists ~ > 0 and ~- > 0 such that Vt _> 0
• t+~" ~ T ~ d t ' > a I (12)
Replacing (10) into (12) readily gives
• T ~ d t ' = 6 w w T d t ' (13) J~ Jt
In the following, we shall verify that by choosing a set of distinct orthogonal functions as the elements of w, the condition on (12) can be satisfied. Herein, we picked a set of sinusoidal functions as
f, = [ f l sin ~ r f2 sin - ~ / f3 sin ~ r ]T,
. , = [ .,cos . cos n3cos ]T T T ' (14)
where ~ 1 , " ' , ~ 3 are any integer numbers. By mak- ing use of the orthogonal properties of the sinusoidal functions in the interval [t, t + r], we have
Jt =T . : 2 2 n 2 n22,n32]. (15) f t + r w w T d t ' ~dlag[f~, f~, f~, 1,
By virtue of (15) and (13), one can conclude that the condition on (12) is fulfilled. Now, the next prob- lem is to define a motion trajectory along which the
0 5
manipulator produces the desired force and moment reactions. Tha t is equivalent to the solution of the inverse dynamics problem. The equation of motion of the payload can be derived from (8) as
~ = _i21(f~xicf~)+iZ1(cxfs_n,) (16)
The solution of the above differential equation with re- spect to the desired force-moment given in (14) yields the manipulator velocity command - note tha t due to the high nonlinearity of the above set of equation it becomes virtually impossible to have an explicite ex- pression of velocity, hence we need to resort to numer- ical methods. Since SPDM operates in the velocity control mode, the velocity trajectories obtained from (16) can be commanded to the SPDM control sys- tem. However, the feasibility of executing the velocity commands depends on the condition that the subse- quent displacement of the SPDM end-effector is en- closed within the operational workspace of the SPDM. The remainder of this section shall address this issue by calculating the absolute position t ra jectory of the manipulator 's end-effector so that one can validate the planed trajectory. Let vector r represent the position of the payload center-of-mass expressed in the inertial coordinate system. Then
= rtv, (17) RT = _[f x]R r,
where R is a rotat ion matr ix associated with orien- tat ion of the end-effector with respect to the inertial frame, and matr ix [12 x] represents the cross product operator defined in the matr ix form as
[0 _o. [nxl o .
-~2~ f~= 0
The above differential equations (17) can be solved numerically with respect to v( t ) and f~(t) to yield r(t) .
5 K a l m a n E s t i m a t o r
Implementat ion of an estimator for the deterministic model (10) demands actual values of the velocity, the acceleration, and the force/moment sensor outputs. However, in practice, only noisy measurements of ve- locity and sensor output are available tha t can dete- riorate estimation performance. Moreover, due to the lack of acceleration measurement, the acceleration has
3
to be derived by the finite difference method that re- sults in a poor estimation and can cause a drift in estimated parameters. In this section, we recast the estimation problem in the context of the Kalman fil- ter which is an optimal est imator by almost any of the known criteria [7]. The parameter vector of the sen- sor gain e is t reated as a s tate vector to be estimated. The parameters are assumed to be time-invariant dur- ing the estimation process, i.e. O = 0. The system dynamics is modeled as a double-integrator. There- fore the process and measurement models are to be in the form of
i [o] d
o o o o 0 + u (18) dt 0 0 0 0 0 0
B
[12 / =h(O, i>)+ (19) "l
Y = 12, e J
In the above, u E N6 is the process noise which can be described in terms of a process noise covariance matr ix o, E N6×6. The process noise is assumed to be Gaussian, zero-mean, and mutually uncorrelated. Thus, the covariance is a diagonal matr ix of the form o" = E [ u u T] = diag[a~zI, a~I]. a~ 2 and a~ are the variances associated with the linear and angular mo- tion of the rigid-body whose values are t reated as ad- justable tuning parameters of the filter. The measure- ment vector y E ~12 consists of the vectors of gen- eralized velocity and of the sensor output . The error vector e T E ~12 is the measurement noise assumed to be white, zero-mean, and Gaussian. Since there is no cross-corelation between the output noise of the veloc- ity and the force-moment sensors, the corresponding covariance matr ix § must be of the following form
§= E[eeT]= [ CE[e~e,]OT 0 ] 0 JcE[e4e~]Jc T "
(20) where E[e~e~ T] and E[e4e~" ] are the noise characteris- tics of the velocity and the force-moment sensors, and
and Jc are previously defined in (2) and (5).
Since the implementation is on digital computer, the continuous system (18) should be converted to a discrete-time system in the common form as
x (k + 1) = Vx(k) + z(k), (21)
where the state vector is defined as
X T = [vT , /~T , o T ] .
Let the observation of the process be occurred at dis- crete points in t ime in accordance with At. Then, the
6 0 6
required parameters to specify the discrete-time sys- tem (21) are • and Q = E[zz T] which are related to the parameters of the continuous system as following o]
~ = e A~t = 0 I 0 , (22) 0 0 I
and Q can be wri t ten formally in integral form as [3],
The matrix E[u(()uT(rl)] = o'5(( -- rl) is a diagonal matrix of Dirac delta function that is known from the continuous model. Thus
[ ~l~t30" ½ ~t20" 0 ] Q = ~At2a A t a 0 . (23)
0 0 0
The measurement vector for the discrete-time system is the same as for the continuous system (19). Since our system is composed of linear dynamics and non- linear measurement, the extended Kalman filter [7] should be employed. To this end, the observation function h(x) must be linearized around the current estimate of the states given by :k(k)
H(k) - c0h(x) I (24) 0x .x=~(k)
[ o o]1 = C Ow
Recall tha t C and • can be reconstructed from equa- tions (11) and (10). In the following, the " s u n Jacobians" in (24) are carried out assuming tha t the sensor axes are oriented along the principal axes of payload, i.e. I~ = diag[I==, I ~ , I , , l and e = [e, O, 01 T,
- m 0 0 0 0 0 0 - m 0 0 0 0
0w 0 0 - m 0 0 0 Os) 0 0 0 - I=~ 0 0
0 0 mc 0 - I ~ 0 0 - m c 0 0 0 - - I ~
m
~W 0 v
0 ~2z - ~ y 0 - v = v v - ~ 0 ~2= v= 0 - v = ~v -~2= 0 - v u v= 0 0 0 0 0 P=~z P=~v
- c ~ v c~2= 0 cv v +pv~z - c v x pv~=
3 6
and
Ivy - Iz~ I ~ - I=x I=x - Iuu P x = , Pv = , P ~ -
m m m
Now we can apply the extended Kalman algorithm [7] to the discrete model (21) with the linearized obser- vation vector (24). Kalman filter adaptively tunes its gains to minimize the mean square of the estimation error based on observation sequence y. The state and the filter gains update takes place in two steps [7],
Prediction: x(k + l [ k ) = ~ ( k l k )
P(k + l l k ) = ~ P ( k l k ) ~ T + Q (25)
Estimation:
~(k + l l k + 1) = ~ ( k + l l k + 1) + K[y(k + 1) - h]
P(k + l l k + 1) = P(k + l l k ) - KI-I]P(k + l l k )
K = P(k + 1]k)H T [HP(k + l l k )H T + §]-1 (26)
In the above, Q and § are the statistical specifications of process and measurement noises given by (23) and (20), and • and H are previously defined in (22) and (24).
6 S i m u l a t i o n R e s u l t s
We resorted to simulation to evaluate the performance of the proposed estimator. The inertial parameters of the payload are listed in Table 1. The a priori data P(0) = I, £,(0) = ~,(0) = 0, and G = I are set for the estimator. The sensor gain matrix C is randomly selected. The objective of the simulation is to esti- mate the sensor gain matrix C by using the proposed procedure. The Cartesian trajectory of the manipulator is shown in Figure 2. Figure 3 demonstrates the closeness of the estimated gain matrix and the actual one in the sense of infinity norm, i.e. the plot shows IIC - GIIoo. The history of the sensor reading error when its outputs is corrected continously by G-1 is shown in Figure 4.
parameter value unit Ixx 1.5 k gm 2 Ivy 1.5 k gm 2 Izz 0.4 k gm 2 m 50 kg c 0.2 rn
Table 1: Payload parameters used for simulation.
0 7
-1 ~ 0 Y (m) -2 -1 X(m)
Figure 2: The trajectory of robot end-effector.
2
1.5[
%______ 0 2 4 6 8 10
Time (sec)
Figure 3: The estimation error of sensor gain matrix measured by ~:>norm.
7 C o n c l u s i o n
A novel procedure for the characterization of a ma- nipulator wrist force-moment sensor with application to the SPDM has been presented. The method re- lies on applying inertial force and moment by maneu- vering manipulator payload in free space. Therefore, unlike conventional characterization procedures, there is no need for external hardware to produce the ref- erence forces and moments. The estimator is basi- cally a Kalman filter which takes the measurements of velocity and force/moment outputs and gives an optimal estimate of the sensor gain matrix. The es- timator also needs an envelop of inertial parameters associated with the payload. A simulation results has demonstrated the convergence property of the estima- tor.
A c k n o w l e d g m e n t s
The author wish to acknowledge fruitful discussion with Mr. Todd Miller and Mr. Patrick Assouad form Canadian Space Agency.
3 6
4
2;
oi a i
-4
i 2
0
.E
--4 2 4 6 8 10 0 2 4 8 8 10
"lime (He) "rime (He)
Figure 4: The history of force/moment error when the sensor readings are continously corrected by C-1. 5
R e f e r e n c e s
[1]
[2]
[3]
[4]
[5]
[6]
[7]
[8]
[91
B.D.O. Anderson. Exponential stability of linear systems arising from adaptive identification. IEEE Trans. On Automatic Control, 22(2), 1977.
C. G. Atkeson, C. H. An, and J. M. Hollerbach. Es- timation of inertial parameters of maipulator loads and links. Int. J. Robotics Research, 5(3):101-119, Fall 1986.
R. G. Brown and P. Y. C. Hwang. Introduction to Random Signals and Applied Kalman Filtering. John Wiley & Sons, Inc., 1997.
J.J. Craig. Introduction to Robotics. Addison Wes- ley, 1989.
L. Ljung and T. Soderstrom. Theory and practice of recursive identification. MIT press, Cambridge, 1983.
A.P. Morgan and K.S. Narendra. On the uni- form asymtotic stability of certain linear nonau- tonomous differential equation. S.LA.M. Journal of Control and Optimization, 15, 1977.
G. M. Siouris. Optimal Control and Estimation Theory. John Wiley & Sons, Inc., 1996.
J. E. Slotine and W. Li. Applied nonlinear control. Prentice Hall, 1991.
M.M. Svinin and M. Uchiyama. Optimal geomet- ric structures of force/torque sensors. The Inter- national Journal of Robotics Research, 14(6):560- 573, 1995.
