We want to determine (X~, Y[) and (2~, ~r) from (X~ 'j, y uj) and ( 2"ji , ~"J), the measurement obtained purely from the i th camera. In this section, since all the derivations are with respect to the same camera, the subscripts "i" and "ci" can be dropped for notational brevity. We further simplify the notation for the reference point by using (X, Y) and (x, y, z) in place of (X ~, Y~) and (x ~, y~, zr), respectively. Moreover, we denote T v by T.
To reformulate the reference point problem in the 2-D image space, we want to represent the equations involved in Definition 1 in the 2-D image coordinates. To this end, we can rewrite J2 in (4.27) as follows. By (4.31),
x f = X z , y f = Yz (4.33)
Differentiating these equations yields
2 f = X z + X ~
:vf = i~z+ Y~
(4.34) (4.35) Noticing that 5(1) = 5(2) in (4.26), we have
z(1)z(1) + Y(1)5(1) Iz(2)z(2) + Y(2)~(2)) 2
+ " z(1) - 5(2)
Denoting p = 5/z, J2 can be written as
J2 = (X(1) - X(2) -~ X(1) X'(2)/2 ( Y(1) g(2) t2 (4.36)
p(1) p(2) + Y(1) -- Y(2) -~ P(1) p(2)
Here p(1) and p(2) are assumed to be nonzero. The treatment of the degenerate cases when p(1) = p ( 2 ) = 0 can be found in [18].
Consider J1 in (4.28). The location of the centroid of an object is generally not known.
The centroid can be approximated by
1 -~ 1 ~ 1
xC ._ j= ~ l XuJ yC __ 7 j=~l yuj' zC --" J~j=I~Z"J (4.37) Denote d "j = z"J/z. By (4.31),
= - - . = X"Jd "j (4.38)
Z ~ j = l ZuJ Z j j=
yCf__ 1 L yujf- z'j Z 6 f~ j = 1 ZuJ Z
1 J
= ~' yuJduJ (4.39)
J j="l z ~ f _ 1 ~ z"Jf
/..,
Z J j = l Z - ~ d "j (4.40)
- f J j = - 5 Therefore, J1 can be rewritten as
- xC(k) ~ (y(k) - / ( k ) )+ ) ~ + or, in the 2-D image coordinates,
Jl=k=lL I(X(k) - - 1 ~1 X~J(k)dU~(k))2 + (Y(k)- 1 L YUJ(k)dUJ(k)) 2-+- f2 1-- 1 L duJ(k) ( )21
Y j=l 7 j=l
(4.42)
3 ESTIMATION OF THE MOTION FIELD OF THE REFERENCE POINT 125
Equation (4.29) can be written in the 2-D image coordinates as follows. From Eq. (4.26), multiplying both sides of (4.29) by ffi:(2) and f/~(1), respectively, yields
Ix(2) f/k(2) x(1) f/k( 1)- I2(1) f/,~( 1)1 y(2) f /~(2) = y(1)f /~(1) + TI).v(1)f /~(1)l z(Z) f /~.(2) z(1)f /k(1) [_~(1)//~(1)_]
(4.43)
Using (4.31), (4.34), and (4.35), Eq. (4.43) becomes
I X(2)/p(2) IX(1)/p(1) Y(Z)/p(2) = I Y(1)/p(1) f /p(2) ~ f /p(1)
X(1)/p(1) + X(1)I
}'( f 1
+ T 1)/p(1) + Y(1) (4.44)
Finally, we want to represent Eq. (4.24) in the 2-D image coordinates. To this end, multiplying both sides of (4.24) by f / z uJ yields
9 o
5c'qf. = X f /dUJ- 603(Y uj - Y/d 'q) + 602(1 -- 1/d'q)f (4.45)
Z ul Z
pu, f ~ f /duj 603( xuj X/duj) + 5)1(1 1/d'~ f
Z ul Z (4.46)
~:uj f = _?" f /duj _ (o2( Xuj _ X/duj) + 6)1( Yuj
Z ud Z - y/duJ)f (4.47)
Differentiating (4.32) and using (4.31), (4.45), (4.46), and 4.47), we have
zuJ zuJ
z - - f x u J x + xuJ + &~ r - & ~ f d"~
(2)2 )2
"71" T ( X u j - - X u j 71- 603 y u j + 602f
~u) = J2uJf yul ~uj
zUJ Z u
(4.48)
= _ _+_ yuj y _ Y"J nt- (5) 3 X n t- 60 i f duJ
~Y + ( Y - Y"J) z --f-
+ yuj + 603 xuJ----7(YuJ) 2 - - 6 0 1 f (4.49)
Rewriting (4.48) and (4.49) yields
x + x - - X "j (! + 6~ X - 6 ~ Y +60 Y + eU~ d'q = 602f
z T -7 (4.50)
where
~y + y z" y.S + X - Y -
z - / 7 &3 X + c} jduj= - - & i f
,j A x , j + (x,J)2 x"S + & 3 Y"j + ( o z f
CX .-- m T
uj A __ ~,ruj -71- y u j _jr_ Co 3 x u J ---7 ( Y"S _ Co I f C y - -
(4.51)
Note that Eqs. (4.50) and (4.51) hold for any time, particularly for k = 1,2. Because (X"S(k), YUS(k)) and (X"J(k), Y"J(k)) are quantities that can be measured, Eqs. (4.50) and (4.51) for j = 1, 2 , . . . , or are constraints on the image of the reference point. In the following, time k is added to the equations whenever it is needed to represent the two adjacent frames. We have
Definition 2 Consider the minimization problem with the objective function (3.9) subject to the constraints 9iven by (4.44), (4.50), and (4.51), where J1 and J2 are 9iven by (3.21) and (3.15) respectively. The unknown variables are )((k), eY(k), X(k), Y(k), p(k), and d"J(k), for k = 1,2.
The 2-D reference point problem is to find the 2-D images (X(k), Y(k)) and ()((k), ~Y(k)) for k = 1, 2 of a 3-D point such that the minimization problem is solved. (X(k), Y(k)) is called the 2-D reference point.
The following lemma is useful for solving the 2-D reference problem.
pu 1 ,.,uj u 1 ~uj
Lemma I Consider the feature points uj (j = 1, 2 , . . . , J ) in the object. I f ,~x ,~y - ,~y '~x ~ 0 for j :t= 1, then
d "j = aj p + --f- X - 7 - Y (4.52)
for j = 1, 2 , . . . , J , where as 4 = 0 can be computed from the image of feature uj.
Proof Subtracting Eq. (4.50) with feature uj (j 4 = 1) from Eq. (4.50) with feature ul yields (_7) 2 (-73 1 )
" l d U l - c ~ j d"j = ( X"I - x " J ) P + - 7 X - 7 Y
C X J J
(4.53) Similarly, from (4.51),
ua,ul c~Jd,J y , l ( 6 9 2 dg~ )
. - = ( _ ru ) p + - f - x - - f - r (4.54)
ul uj __ p u l . , u j
If Cx cy ,~Y ,~x :/= O, then d "j (j = 1, 2 , . . . , J ) can be uniquely represented by (4.52), where
a l m
c~J(x .1 _ x,J) _ c ~ ( y ul - y,J) ul gul
" ~ ( x ~ - x "j) - c ~ ( - g " s ) C y
aj = cxUlc~J -- Cy"l cx, J (j 4 = 1)
d "j, by definition, is a positive quantity. Hence as 4 = 0. D
3 ESTIMATION OF THE MOTION FIELD OF THE REFERENCE POINT 127
Remark A robust way to compute aj is to use the least squares solution of the following equations"
- ul u2
Cx - C x 0 ... 0
ul u2
cr - c r 0 ... 0
ul u3
Cx 0 - c x ... 0
u 3 . .
c r ~ 0 - c r 9 0
c x ul 0 0 . . . . Cx j' cr ul 0 0 . . . . c} t
:al 1
! a 3 = a 4
_ a c J
X ul _ X u2 y u l y u 2
XUl _ XU3 yul yu3
X ul _ X U J
y u l _ y . ~
_
(4.55)
Theorem 1 Assume that the angular velocity of the moving object is known. Then the 2-D reference point (X(k), Y(k)) together with p(k) for (k = 1, 2) satisfies
C~x~(1)X(1) + c~2(1)Y(1) + c~13(1)p(1) -c52fb~(1 )
( ]211 ]221 ) 2 (1 + Tfl 1(1)) /~2 T f l 2 (1)
\p gl + - 1 1 - 1 0
c~2(1)X(1 ) + c~22(1)Y(1 ) + c~23(1)p(1 ) + 6)~fbx(1)
(712 722 ) /~ rf121(1 ) 22(1 + Tfi22(1)) = 0 + 8 \p(l) gl -[- p---~- g2_ -- 1
cr + ~2a(1)Y(1) + 0~33(1)p(1) - f2b~(1)
f?laX(1) + 712Y(1) + 713 721X(1) + 722Y(1) + 723
L p-~ii g~ + p2(1) g2 )
+ 2~T(X(2) -fla3(1)) + 22T(Y(2) -/323(1)) + 23(Tp(2 ) - 1 ) = 0 c~(2)X(2) + cr ) + cr ) -c52fb1(2) + 21(1 + Tp(1)) = 0 r ) + ~22(2)Y(2) + ~z23(2)p(2) + 6Jxfb~(2 ) + 22(1 + Tp(1)) = 0 cr ) + cr ) + CCaa(2)p(2 ) - f 2 b ~ ( 2 ) + 23(1 + Tp(1)) = 0
h~(X(1), Y(1), p(1), X(2)) = 0 h2(X(1), Y(1), p(1), Y(2)) = 0
h3(p(1), p(2)) - 0
(4.56)
(4.57)
(4.58) (4.59) (4.60) (4.61) (4.62) (4.63) (4.64) where
1 ~ X,~(k)aj(k ) J
bx(k) = J j=x
J
1 ~ Y"J(k)aj(k) br(k) = J j=l
1 ~ aj(k) J
bl(k ) = -~ j= ,
~ll(k) - - (1 - (02/fbx(k)) 2 + ((02/fbr(k)) 2 + ((02b~(k)) 2
~ 2 ( k ) = (01/f(1 - (02/fbx(k))bx(k) - (02/f(1 + (01/fbr(k))b r(k) - (0~(02b2(k)
~13(k) = - ( 1 - (02/fbx(k))b x(k) + (02/fbZr(k) + f (02b2(k) a22(k) = ((01/fbx(k)) 2 + (1 + (0~/fbr(k)) 2 + ((0ibx(k)) 2
~23(k) = -(0~/fb~c(k) - (1 + (0x/fbr(k))b r(k) - f(0xb~(k)
~33(k) - b2(k) + b2(k) + f2bZ~(k)
J
1 ~ (XUJ(k) _ aj(k)c ~(k)) flax(k) = ( 0 2 / f f113(k)
fix 2(k) = - (0x I f fix 3(k) - (03
1 J
f123(k)~ ~ (Y"J(k) - aj(k)c"~(k))
ar j=l
f121(k) = ( 0 2 / f f123(k) + (03 fl22(k) = - (0x/ff123(k)
7.1 = fl.x(1) - fl.x(2)(1 + rflxx(1)) - fl.2(2)rfl2x(1) 7.2 = ft.2(1) - fl.x(2)Tflx2(1) - fl.2(2)(1 + Tf122(1)) 7.3 = - f l . x ( 2 ) r ( 0 2 f + f l . 2 ( 2 ) r ( 0 x f
7.4 = - f l . x ( 2 ) r f l x 3 ( 1 ) - fl.2(2)Tf123(1) + ft.3(1) - fl.3(2) where n = 1, 2, and the functions gl, g2, ha, h2, and h 3 are defined by
gl = (7i1X(1) -k- 712Y(1) + 7,3)/P(1) -k- ~ 1 4 - T ( 0 2 f
g 2 = (721X(1) -k- 722Y(1) + ~23)/P(1) + Y24 + Tco~f
h, = (1 + Tp(1))X(2) - ( 1 + rf111(1))X(1 ) - Tf112(1)Y(1 ) - Tfl~3(1)p(1 ) - T ( 0 2 f h 2 = (1 + T p ( 1 ) ) Y ( 2 ) - Tfl2~(1)X(1 ) - ( 1 + Tf122(1)Y(1 ) - Tf123(1)p(1 ) + r ( 0 ~ f h 3 = (1 + Tp(1))p(2) - p(1)
(4.65) (4.66) (4.67) (4.68) (4.69) P r o o f The p r o o f consists of three parts.
(1) P i c k i n g i n d e p e n d e n t e q u a t i o n s from (4.44), (4.50), a n d (4.51).
T h e r e are 2 J e q u a t i o n s in (4.50) a n d (4.51) for j = 1, 2 , . . . , J . H o w e v e r , these e q u a t i o n s are not i n d e p e n d e n t . F r o m L e m m a 1, we have J i n d e p e n d e n t e q u a t i o n s given by (4.52) for j --- 1, 2 , . . . , J . S u b s t i t u t i o n of these e q u a t i o n s into (4.50) a n d (4.51) yields
(-~)2 (01 )
2 + X p = ( X "i - ajc~) p + --f- X - --f-- Y - (03 Y + (02f
~Y+ Yp = (Y"J - ajc~ j) p + - - f - X - - - f - Y +(03 X - (01f
(4.70) (4.71)
3 ESTIMATION OF THE MOTION FIELD OF THE REFERENCE POINT 129
Summing (4.70) for each j from 1 to J and then dividing the summation by or yields (~)2 6)1 r t
~2 + X p = p + - - f - X - 7 - fl13 --(b3Y nt- 6)2f (4.72)
Similarly by (4.71),
c~ 6)1 )
} ' + Yp = p + --f- X - --f- g ~23 + 6) 3 X - 6) l f (4.73) Since p is assumed nonzero, dividing (4.72) and (4.73), respectively, by p yields
2 /~11X + /~12 Y "-[- 6)2f
- - n t- X -- +- fl13 (4.74)
P P
}r f121X -[- fl 2 2 Y - - 6 ) 1 f
- - - { - Y - - q- fl23 (4.75)
P P
So the equations in (4.50) and (4.51) are equivalent to those in (4.52), (4.74), and (4.75).
Finally, using (4.74) and (4.75), the equations in (4.44) can be written as X(2) (1 + Tfill(1))X(1 ) + Tfi12(1)Y(1 ) + T 6 ) 2 f
p(2) p(1) -+- Till3(1 ) (4.76)
Y(2) Tf121(1)X(1 ) + (1 + Tf122(1))Y(1) - T 6 ) l f
p(2) p(1) -+- Tf123(1 ) (4.77)
1 1
- ~ + T (4.78)
p(2) p(1)
Therefore, the constraints in the 2-D reference point problem can also be equivalently given by Eqs. (4.52), (4.74), (4.75), (4.76), (4.77), and (4.78).
(2) Eliminating variables X, Y, d uj (j - 1, 2 , . . . , J ) from the 2-D reference point problem.
Using (4.52), J1 in (4.42) can be written as
k= x - --f- bx(k ) X(k) + --f- bx(k) Y (k) - bx(klp(k ) + - --f- br(k)X(k) + 1 + --f- b r (k) Y(k) - br(k)p(k ) + f 2 6)2 (k)X(k) + b l ( k ) Y ( k ) - bl(k)p(k) + 1
- - - f - b 1 - 7 (4.79)
Substitution of (4.74) and (4.75) into J2 in (4.36) yields
J2 = g2 + g22 (4.80)
where
flll(1)X(1) -I- f112(1)Y(1) + 692f fill(2)X(2) + f112(2)Y(2) + 592f
gl = p(1) p(2)
~zl(1)X(1) + fl2z(1)Y(1) + Oalf fi21(2)X(2) + flzz(2)Y(2) - 69~f
g2 = p(1) p(2)
+ fi13(1) - fl13(2) (4.81) + fl23(1) - fl23(2)
(4.82) It is now clear that the 2-D reference point problem becomes the minimization problem with the objective function (4.30), with J1 and J2 being defined in (4.79) and (4.80), respectively, subject to the constraints given by Eqs. (4.76), (4.77), and (4.78).
(3) Solving the constrained minimization problem.
Functions gl and g2 in (4.81) and (4.82) can be simplified by substituting Eqs. (4.76), (4.77), and (4.78) into them. The results are given by Eqs. (3.65) and (3.66). Furthermore, from (4.78), we have
1 _ 1 -4- Tp(1) (4.83)
p(2) p(1)
Substitution of (4.83) into (4.76) and (4.77) yields
h 1 = 0 ( 4 . 8 4 )
h 2 -- 0 (4.85)
where h a and h 2 are defined by Eqs. (4.67) and (4.68), respectively. Equation (4.83) can also be written as
h 3 = 0 (4.86)
where h 3 is given by (4.69).
Define Lagrangian
L = J1 + 8J2 + 221hl + 222h2 + 223h3 (4.87) Then Eqs. (4.56), (4.57), (4.58), (4.59), (4.60), and (4.61) can be obtained by setting
~L
8(I-X(1) Y (1)p(1)X(2)Y (2)p(2)-]T) = 0 (4.88)
4 THE CONTROL DESIGN FOR TRACKING AND GRASPING
We have modeled the motion of the object as the motion of the reference point coupled with the change in orientation of the object. Because of the decoupled nature of the robot dynamics, the control for tracking and grasping the object can be designed in two separate
4 THE CONTROL DESIGN FOR TRACKING AND GRASPING 131
steps. To grasp the moving object, the center of the robot gripper is commanded to track the motion of the reference point. In the meantime, aligning the orientation of the gripper with that of the object can also be posed as an orientation tracking problem. However, since a dynamic model for the motion of the reference point is required in the tracking control design, we will first discuss the problem of fitting the data of the reference point to a dynamic system.