Robot Control System Configuration
Real-time experiments are carried out on the visual feedback control system depicted in Figure 2.18. The host c o m p u t e r is an N E C PC9801 with TRPM-401 added in. A parallel c o m p u t a t i o n scheme for joint level servo is implemented on a network of eight transputers (ADS TBE02). These transputers communicate with each other via a transputer link. The vision processing board is a T R P - I M G . The sampling periods of the vision system and the joint servo are 33 and 1 ms, respectively. See Section 6.1 for a detailed description of this equipment.
FIGURE 2.15
C i r c u l a r m o t i o n .
150 '
0
-5o y , - l o o
- 3 0 0 -, . . . i . . . . z . . . . I . . . . i , , ,
0 100 2,00 300 400
X [mm]
t i i I
. . . ! . . . . +_ .. . . ~ .. . . 4 . . .
i i i J
i i i r
... i ~ i i ~ " i ...
i - 4 - ' - - - ~
6 EXPERIMENTS 81
40. ..'! ti /i / ~i
2 0 ! i ,--.,
i T ",
9
~_ ao ' '
. . . l, m L : , : 9 . . . .
_1o- - 3 o ; . . i ! i / ' : ! i i i , , ,/ w
i l ! i "~
. . . i,,, .~_~. ,'._.,,.. ~i. ~i . . .
- 4 0 I:
0 3 6 9 1 2 1 5 1 8 2 1 2 4 2 7
( a ) T i m e [ s e e ]
I ! : ~ "' : ' f 1
" , i! "~ !~ ' t!
2 0
.2 7 ~ " ~ i ~ \ /~,
~, ~ . . . ) r M~,~j! i] i ~ ~ ! ~ f ' . . , i / i , ' ii/kj . . . . ~ ; r ... /
~-2o_ ii !" '..' il i/
- 4 0 . . , . , , . "i. , :d .. ff . . ..,' , . ;i . . . . . . . . . .
0 3 6 9 1 2 1 5 1 8 2 1 2 4 2 7
(b) Time [see]
- 4 l
o
- 4 i I , , i . . I . . . .
0 3 6 9 1 2 1 5 1 8 2 1 2 4 2 7
(C) T i m e [ s e e ]
FIGURE 2.16
Simulation results (circular motion): (a) x error, (b) y error, (c) estimated angular velocity;--, with observer;
without observer.
Linear Motion
Feature P o i n t s a n d I m a g e J a c o b i a n We have tested the largest singular values of the image Jacobian for three objects shown in Figure 2.19. They are white boards with three, four, and five black marks. Three points are arranged to m a k e a regular triangle with edge length 100 ram. F o u r points are on corners of a square with edge length 100 mm. F o r five points, an extra point with height 30 m m is added at the center of the square. The m a r k s are on a plane except for the one at the center of the square. The features are the x and y coordinates of the image center of each mark. To avoid the singular cylinder m e n t i o n e d in Section 3.2 and Figure 2.6, the reference c a m e r a position is located outside the cylinder. C o m p u t i n g the m i n i m u m singular values (ami,) of the image Jacobians J3, J4, and J5 (i.e., the image Jacobians for three, four, and five feature points, respectively) yields
~Tmin(J3) = 0.339, ami,,(J4) = 0 . 6 1 4 , ~Tmin(J5) = 3 . 5 5 (2.64) Thus five features are desirable to obtain accurate position control of the c a m e r a in the 3-D work space. Therefore we carry out the next experiment with five feature points.
Experimental Setup The b o a r d with five features are attached to a P U M A 550. P U M A 550 and P U M A 560 are the same size but the former has five degrees of freedom. It does not have wrist rotation, which corresponds to joint 4 of the P U M A 560. The world coordinate system is at the base of the P U M A 560, which holds the camera. A n o m i n a l c a m e r a position
40 ~ ~ ~ i'
! " 9 ~ , i ' : '~
R 0 . --:-:-;----;'--v'-~--':-4 ." . . .
3 0
~ 2o
'I~, i0
~ -I0 - 2 0 - 3 0 --4O
0 3 6 9 1 2 1 5 1 8 2 1 2 4 2 7
T i m e [ s e c ]
2o / \ : , ?',. /i .
~, 0 _at 9
i - 2 0
---40
0 3 6 9 1 2 1 5 1 8 2 1 2 4 2 7
T i m e [ s e e ] 4
o
~ - 2 .
0
- 4 ' '
0 3 6 9 12 1 5 1 8 2 1 2 4 2 7
T i m e [ s e e ]
FIGURE 2.17
Experimental results (circular motion): (a) x error, (b) y error, (c) estimated angular velocity;- without observer.
, with observer;--
FIGURE 2.18
Visual feedback control system.
SONY I ~
x ~ ~
PUMA ~ ( )
iPower [ 560 i k T ~ ]
[Amp. I
! J
B o a r d s H ADS TBE02 ~T'C'SRPM_401 H C S ] " I [TRP-,~ [G~
l . I
1PC98O q [FA34WEX |
is in front of the marks and the distance is a b o u t 1000 mm. The n o m i n a l positions of the object and camera are s h o w n in Figure 2.20. The X w - Yw-Zw coordinate system is the world c o o r d i n a t e system.
Object M o t i o n In this experiment, the object m o v e s up and down; that is, the object m o t i o n
* is the object
* 0 0 0 ] r, where v=
is translational in the Z w direction. Thus, ~o = [0 0 v=
velocity in the vertical direction Zw. Since the object m o t i o n is one dimensional, we can
6 EXPERIMENTS 83
~ r 100n~ 0 1
lOmm 1
9 9
height: JOmm 9
lOmm ~
FIGURE 2.19
Configuration of feature points. Features are selected as the x and y coordinates in the image plane of the center of each circle. The center mark of five marks has height 30 mm.
c h o o s e the generalized c o o r d i n a t e as p = Zob j, w h e r e Zob j is the object height in the w o r l d
* a n d the p a r a m e t e r i z a t i o n (2.3) is given by W = 1 c o o r d i n a t e system. T h e n we have p = vz
a n d 0 = v~. Since 8Sob~/Cp = [0 0 1 0 0 0] T, the m a t r i x L for the o b s e r v e r b e c o m e s as follows"
I 0 0
1
= (2.65)
L Ji'~ 0
0 0
* t h a t is, the object velocity, in the vertical direction. At t - 10 sec T h e o b s e r v e r estimates vz,
the object starts to m o v e with velocity - 2 0 m m / s e c , t h a t is, 20 m m / s e c in the d o w n w a r d ( - Z w direction), a n d stops at t = 15. After 10 s e c o n d s of pause, it m o v e s u p w a r d with velocity 10 m m / s e c a n d stops at t - 35.
E x p e r i m e n t T h e e x p e r i m e n t a l results are s h o w n in F i g u r e 2.21(a) a n d (b). In F i g u r e 2.21(a), the vertical axis shows the p o s i t i o n of the c a m e r a in the w o r l d c o o r d i n a t e system. T h e solid a n d b r o k e n lines are the results with a n d w i t h o u t the observer, respectively. T h e d o t t e d line is the reference t r a j e c t o r y of the camera. T h e c o n t r o l law with the o b s e r v e r is given by (2.62) a n d (2.61), a n d the c o n t r o l law w i t h o u t the o b s e r v e r is
il = - ( B T J ) - 1Kz (2.66)
SON'," EE~ ....
XC77CE ~ - ~
I I I C o o ~ t e
FIGURE 2 . 2 0
Robot configuration and object position.
1000ram obj~
[
2 0
e ~ - 2 0 - 4 0 - 6 0 - 8 0 - 1 0 0 - 1 2 0
1.5
. . . . , ~\ ..."" / . - . . .
-.,\ /-!,;
\ \ - \ .." /'t ,.
":. \ ,.."
- \ ,:\ ..-/ .:i/
"I ', / ' /
0 1 0 2 0 3 0 4 0 5 0
( a ) T i m e [ s e c l
, .. , , 9 ,
I O 5 o
- 1 0 - 1 5 - 2 0
j. . . ~
I I
, A i | 1 ,
0 1 0 2 0 3 0 4 0 5 0
(b) , T i m e [ s e c l
FIGURE 2.21
Step response. (a) Object position --, with observer;
velocity --, estimation;--, true value.
- - , without observer;.-, reference trajectory. (b) estimated
The matrix K is the gain, which is the same as the observer-based controller. By using the observer the tracking speed is improved and the tracking error is reduced considerably. As shown in the nonlinear case, overshoot is a shortcoming of the observer-based scheme.
Figure 2.21(b) shows the estimated velocity of the object. The broken line shows the true value and the solid line is the experimental result. The observer estimates the object velocity fairly accurately.
Another objective of the observer is interpolation of the visual data obtained with a very slow sampling rate. The values of z and its estimate ~ are plotted in Figure 2.22. The time t = 0 in Figure 2.22 corresponds to the time t = 10 in Figures 2.21. Delay of one visual sample is found in the period 0.1-0.4 because the velocity estimation is not correct during this period. Once the velocity estimation converges to the true value, the delay is canceled.
Tracking a Minirobot
Khepera Robot This section gives experimental results of visual tracking of Khepera, a mini two-wheeled mobile robot. Khepera was developed at the L a b o r a t o r y of Micro Information, E P F L , Switzerland. Khepera has an MC68331 C P U and 256 Kbyte R A M and can be p r o g r a m m e d in C. Figure 2.23 shows an overview of the Khepera robot. The wheel base is 53 mm.
Object Motion The camera tracks the Khepera robot as it moves on the floor. The motion is circular with radius 97 mm. We assume that the center of the circle is known. Two cases
6 EXPERIMENTS 85
FIGURE 2.22
Estimated velocity (~_).-
- 2 5 - 3 0 - 3 5 - 4 0 - 4 5 - 5 0 - 5 5 - 6 0 - 6 5 - 7 0
J J
u J
,7"
S
0 O.1 0 . 2 0 . 3 0 . 4 0 . 5 0 . 6 O,7 0 . 8
T i m e [ s e c ]
, measurements z; - - , interpolated value ?z.
with velocities 0.9 a n d 1.8 rad/sec are e x a m i n e d . T h e e x p e r i m e n t a l setup is d e p i c t e d in F i g u r e 2.24. T h e feature is the center of K h e p e r a in the image. T h e c a m e r a t r a c k s the object in a plane parallel to the floor; t h a t is, the o r i e n t a t i o n a n d the height of the c a m e r a are k e p t constant.
Let the object p o s i t i o n be [Xob ~ Yobj 0] r. O r i e n t a t i o n is n o t c o n s i d e r e d in this case. T h e n the generalized c o o r d i n a t e s of the object b e c o m e p = [Xob ~ Yobj] r. Since the center p o s i t i o n is a s s u m e d to be k n o w n , we can use (2.50) for the object m o t i o n m o d e l . U s i n g C~Sobj/~ p = [1 1 0 0 0 0] r yields
L - Jimo c Rw
1 1 0 0 0 0
(2.67)
T h e o b s e r v e r estimates the r o t a t i o n a l velocity of the object.
T h e K h e p e r a r o b o t starts to m o v e c o u n t e r c l o c k w i s e at t = 5 sec with r o t a t i o n a l velocity co = 0.9 rad/sec, c h a n g e s its velocity to co = 1.8 at t = 20; a n d stops at t = 35. After 5 seconds of rest, the K h e p e r a starts to m o v e a g a i n clockwise with co = - 0 . 9 , c h a n g e s the velocity to co = - 1 . 8 at t = 54, a n d stops at t = 69.
FIGURE 2.23
Overview of Khepera robot.
FIGURE 2.24 Experimental setup.
Y
kkl i l J/ i II 11 !1
I
PUMA 560
---X
~ _ ~ Camera Object z
~ 0.9, 1.8 [rad/sec]
i 9 | r - - I ~ .]k"
I ' Object
Control Law In this experiment, the controlled degree of freedom is two, that is, the X and Y coordinates of the camera position in the world coordinate system. Thus, to simplify the controller the controlled variable z is set to ~. In other words, we choose B = I. Also, since the optical axis of the camera is aligned with the Z w axis, which is orthogonal to the floor, the image Jacobian linearized at the reference position can be simplified to
J = - ~ CRwJro b f (2.68)
Z,
where Z, is the camera height at the reference position. Then the control law is given by
[C o, l -
0 (CRwJ,ob)- 1 m c
= , Vc,,,,, = - K _ ( Z - z , )
= J q + L W O + H ( ~ - ~), O = - - w T L T p ( ~ - - ~) (2.69)
A
where K and K_ are gain matrices and ~ is the observer estimation of the object features.
, - . - ,
3 2
1
0 -
- 1
-2
-3 0 10' 20 FIGURE 2.25
| i | ! , , t . . .
3 0 4 0 5 0 6 0 7 0
T i m e [ s e c ]
Estimated rotational velocity (69).--, Estimated;--, true.
7 CONCLUSIONS 87
2 0
1 5
I O
o - 5 - 1 0 15 -20
! , i ,
1
, I
( ,,v .
I
0 1 0 2 0
(a) T i m e [ S e c ]
2 0 . . . .
~, !, i ii
I 0 .q ~, i
-- 1~ .c,~ { { { i ~ :\
.[ i i {,i ' i ~ '~
- o
-~ ,,, .,~ i] I f
-~o t, 'i i, ,~
- 2 0 " * '
O(b ) 1 0 2 0
' " i ' L ' ~ ' " ' ' " . . . . '
, , 1~ I, *I l ~ I , I / i '~
~] tl !i '! .~-, ,~ l i '
g
! I i I, i ~, i ~ r l ~ / I1i il1 Ill 117
v'(l'r V~V, t
3 0 4 0 5 0 6 0 7 0
~i ~ i i tl
i t It i ' ' i ~ i ] itl t! I, I.I i ' ! ' i Ji
, t ' I ....
... ~ o ~o 5'o do ~o
T i m e [ s e e ] FIGURE 2.26
Controlled error. (a) x direction; (b) y direction.--, With observer"--, without observer.
Experiment The initial joint angles are q = [10.9 - 4 2 . 1 - 7 0 . 9 2.01 23.0 0.01] T and the initial camera position is Sc,,m = [803.6 1.588 1103.0]. Figure 2.26 shows the estimated value of co. The solid line is the estimated value and the dashed line is the true value. The observer estimates the velocity fairly accurately, but oscillations are found. The oscillations are due to the calibration error of the rotation center. Also, the arm configuration becomes almost singular, that is, the arm is almost stretched out, when the object is at the farthest position.
Thus the joint control accuracy is not very good around the farthest point. This singularity problem is another reason for oscillation.
Figure 2.26(a) and (b) show the error in the image plane for the x and y directions, respectively. The solid line is the result with the observer whose control law is given by (2.69).
The broken line is the result without the observer. The control law without the observer is similar to (2.69) but the estimated velocity and the estimated feature are replaced by zero and the measured features, that is,
['o]
0 = (CRwJ,.ob)-i m ,
- / r - z ~ ) / Note that the error is reduced for both directions by using the observer.
7 CONCLUSIONS
The vision sensor includes delay in its structure. Also, the sampling rate of the vision sensor is usually very slow. Thus, increasing the feedback gain yields oscillations. F o r these systems
1 0 0
5 0
0
- 5 0
- 1 0 0 t
i t
0 10 2 0
t t l
( a ) 3 0 4 0 5 0
T i m e [ s e c ]
1 0 0
5 0
- 5 0
- 1 0 0
i ,
6 0 7 0
i L i i i | ,. i
( b ) 0 1 0 2 0 3 0 4 0 5 0 6 0 7 0
T i m e [ s ~ ]
FIGURE 2.27
Control input. (a) Feedforward part - LWO~ (b) feedback part - K~.
a feedforward control scheme is effective. The results given in this chapter emphasize the usefulness of object velocity feedforward in the application of vision-based control.
We have introduced a visual feedback controller with a velocity observer. The observer compensates the delay by estimating the object velocity. Also, the observer provides intersample information to the joint servo by updating the visual information with the sampling rate of the joint servo. Thus the problems of slow sampling time and delay of the vision sensor are resolved. The tracking performance is improved by feedforwarding the object velocity. Stability of the observer-based control system is presented in a nonlinear form. Simulations and experiments with a two-link direct drive robot have exhibited the effectiveness of the observer-based control scheme. A linearized version suitable for industrial robot control is also presented. Experimental results with P U M P 560 have shown stable and accurate performance of the observer-based visual servo controller.
REFERENCES
[1] P. K. Allen, A. Timcenko, B. Yoshimi, and P. Michelman. Automated tracking and grasping of a moving object with a robotic hand-eye system. IEEE Trans. Robotics and Automation, 9(2):152-165, 1993.
[-2] F. Chaumette and A. Santos. Tracking a moving object by visual servoing. In 12th IFA C World Congress, Vol.
9, pages 409-414, Sydney, Australia, 1993.
[3] P. I. Corke. Video-rate robot visual servoing. In Visual Servoing, K. Hashimoto ed., World Scientific, pages 257-283, Singapore, 1993.
[4] P. I. Corke. Visual control of robot manipulators--a review. In Visual Servoing, K. Hashimoto ed., World Scientific, pages 1-32, Singapore, 1993.