To show the effectiveness of the nonlinear observer and controller, simulations and experi- ments are carried out. Figure 2.11 shows an experimental setup with a two-link direct drive robot.
A two-link planar robot illustrated in Figures 2.8 and 2.9 was designed and manufactured in our laboratory. Two direct drive motors (NSK RS0810, N S K AS0408) are used.
M a x i m u m rotational velocities are 1/3 and 1.5 (rpm), and m a x i m u m torques are 9000 and 1000 (kgf ram), respectively. The size and inertial parameters of this robot are 11 = 300, lcx = 177, lcy= 88, lol = 159, lo2 -- 53.7 (mm), m I = 11.1, m 2 - - 4.05 (kg), 11 = 8.96 x 105, I 2 ---=
5.20x 10 r (kg mm2), D 1 = 2050, D 2 - - 159 (kgf m m s ) .
6 EXPERIMENTS 77
FIGURE 2.11
Visual feedback control system.
S O N Y NSK
X C 7 7 C E
DD M O T O R
kl t
-q , 'j
C S C ~
PM-401 T R P - I M G
NEC MITSUBISH~
PC9801 F A 3 4 W E X
The host computer for the manipulator control is a personal computer ( N E C PC9801). A transputer board TRPM-401, made by Concurrent Systems Inc., Japan, is added to the PC.
The TRPM-401 board has 2 Mbytes of dynamic RAM and a transputer T800 [32], a 32-bit microprocessor with an on-chip floating-point processing unit. The vision processing board is Concurrent Systems T R P - I M G . The main processing unit of T R P - I M G is also a transputer. The T R P - I M G board has three digital signal processors IMS A l l 0 connected in a pipeline. These DSPs do image convolution operation in real time. The board has 3 Mbytes of video RAM and 1 Mbyte of dynamic RAM. The video R A M is organized in 48 planes of 256 x 256 pixels, one byte per pixel. A TS68483 graphic controller chip is used as a video timing generator. The video digitizer chip AD9502 is used to sample a C C I R standard video input signal at a rate of 12.5 M H z and provides digital video information with a resolution of 8 bits. The I/O boards consist of an interface board (Concurrent Systems TRP98-2) for communication with the transputer with DA and counter boards. The DA boards are used to output the command torque, and the counter boards are used to input the encoder reading values. A C C D video camera S O N Y XC77CE is mounted on the end effector of the robot. The internal calibration of the camera and the external calibration of the geometrical relationships between the camera and the end effector are carried out on the basis of the calibration algorithm proposed by Tsai and Lenz [44, 45].
The sampling period of the vision system is 33 ms. The sampling period of the manipulator control is fixed to 1 ms. The feature vector (image coordinates of the center of the object) is computed in the T R P - I M G board and is passed to the TRPM-401 board every 33 ms, where the joint servo computations and observer update are carried out with the sampling period 1 ms.
Straight Motion
Simulations and experiments for object motion in a straight line are described. The initial object position is Sobj = [282.5,--119, 1000] r (mm), initial joint angle is q = [--60, 80] r (deg), initial object image is ~ = [ 1 8 , - 2 0 ] r (pixel), and reference image is ~a = [0, 0] r (pixel). The camera looks upward and the object moves above the robot. The task is to track the object so that the object image is kept at the reference point (origin of the image plane).
The object starts to move in the Y direction at 4 0 m m / s e c at t = 6 sec and stops at t = 10.5; moves again in the X direction at 20mm/sec at t - 13.5 and stops at t = 17. A stroboscopic plot of the robot motion is shown in Figure 2.12. Each triplet of line segments
FIGURE 2.12 Straight motion.
1 5 0 . . .
too L ...
5 0
0
- 5 0 . . .
; ~ - 1 0 0 - 1 5 0 L.
- 2 0 0 - 2 5 0 -300
i I
i 1
- -5 . . . 4 . . . i
. . .
: . . . i . . . . i . . . . i . . . . i , , ,
0 xoo zoo 300 400
x [mm]
shows a configuration of the robot and the camera. The base of the robot is at (0, 0) and the line segments passing through the base denote the first link. The second link is connected to the first link. The third line segment denotes the offset Icy in Figure 2.9. The positions of the camera center are marked by + s.
Simulation The simulation results are shown in Figure 2.13. The horizontal axis is time; (a)
l 0
x 0
i - 1 0 . " ' " - . . . [
- 2 0
0 3 6 9 1 2 1 5 1 8 2 1
(a) T i m e [see]
4O
-~ zoE- r . . . . "---..
',,,._~
0 -~ . . . . . . . - - - -
0 3 6 9 1 2 1 5 1 8 2 1
( b ) Time [secl
E - l o
~ - 2 . 0 . - . - - .
- 3 0 ~ ~ ~
0 3 6 9 1 2 1 5 1 8 2 1
( C ) Time [sec]
6 0 "
g---2
i 2 O
" ~ 0 . . . . . . . . . . . . . . . . . . . . .
- 2 . 0
O(d ) 3 ~i Time9 [sec]l'2 ' " 1'5 1 8 2 1
F I G U R E 2 . 1 3
Simulation results (straight motion): (a) x error, (b) y error, (c) X estimated velocity, (d) Y estimated velocity;
with o b s e r v e r ' - - , without observer.
6 EXPERIMENTS 79
and (b) are feature errors in x and y coordinates in the image plane; (c) and (d) are estimates of the object velocity in X and Y coordinates in the base frame. A white Gaussian noise with variance 0.1 (pixel 2) was added to the feature position ~. The solid lines in (a) and (b) are the results with the observer and the dotted lines are the results without the observer, that A is, the controller (2.36) but 0 - 0. This controller
= M ( B r j ) - 1(/2 __ /~) .nL_ h
v = - K l z - K 2 B r J i l (2.63)
is referred to as t a s k - l e v e l i n v e r s e d y n a m i c s in [22]. The same gains as with the observer-based control are used. Plots of the first 3 seconds in (a) and (b) show the response for a step change of the reference point. Since the robot moves to track the reference change, the observer is excited and generates some object velocity estimation. However, the object does not actually move. Thus the object velocity estimation has a bad effect on the tracking performance (i.e., overshoot). On the other hand, once the object is tracked at the reference position, the observer estimates the object velocity accurately. Note that the error in the image plane is reduced considerably while the object is moving with high velocity. Since the convergence time of the observer used in this simulation is almost 1 second, the overshoot at the moment of velocity change is inevitable.
Experiment Experiments are carried out on a direct drive robot. The parameters are the same as in the simulation. Results are shown in Figure 2.14. The errors in the image plane
-I0 -. " "" ,
i .J ...
0 3 6 9 12 15 18 21
(a) T i m e [see]
4 0 . . . . . . . . . . . . . . .
' ~ ' 2 0 ~ ' W - ~ c ~
o ~ . . : _ . . _ ~ ... 7 ... ? ! : . , , . ; ~ - 7 . . . - . . A
- 1 0
0 3
(b)
10 f ' o
~ - 1 0
~ - 2 0
- 3 0 9 "
0 3
(c)
6O
~ o E 2 0 E
0 - 2 0
6 9 12 15 18 21
T i m e [secl
... .
T i m e [secl
3 6 9 12 15 18 21
T i m e [sec]
FIGURE 2 . 1 4
Experimental results (straight motion): (a) x error, (b) y error, (c) X estimated velocity, (d) Y estimated velocity;- , with observer;--, without observer.
are due to friction, but they are not larger than 10 pixels. There are couplings in the estimation of velocity. The estimates are oscillatory c o m p a r e d with the simulation, and the effect of oscillation is found in the controlled error. However, the controlled error is fairly reduced while the object is moving.
C i r c u l a r M o t i o n
The second case is a circular m o t i o n with radius 2 0 m m . The center of the circle is at ( 2 6 5 , - 120) in the base coordinate system. The object starts to move with angular velocity 2.8 rad/sec at t = 6.5, changes the speed to 1.4 rad/sec at t = 17.5, and stops at t = 23.5. The r o b o t m o t i o n is shown in Figure 2.15. The line segments show the r o b o t links and the + marks show the camera position.
S i m u l a t i o n The simulation results are shown in Figure 2.16. The horizontal axis is the time;
(a) and (b) are the feature errors; (c) is the estimate of the angular velocity. The speed of convergence is slow, but the observer estimates the object velocity. When the observer is used, the error in the image plane decreases as the estimated value converges. On the other hand, there are steady-state errors for the controller without an observer.
Experiment Experimental results are given in Figure 2.17. The estimated value is accurate and the controlled error is reduced, but it does not converge to zero because the information on the center of the circle used in the design of the observer includes measurement error.