To see the procedure for designing the observer-based controller introduced in this section, an example of a planar two-link robot is given. Let us consider the robot shown in Figures 2.8 and 2.9. Figure 2.8 is the side view (from the + Yw direction). Figure 2.9 is the top view.
The camera is mounted on the second link and looks upward. The object position is higher than the camera position. When the joint angle vector q equals zero, the robot is stretched out and the links are aligned with the X w axis.
Robot Model
The robot model is given by (2.2) with
m I J1 -Jr- J2 -Jr- 2b cos q2 J2 + b cos q2
J2 -k- b cos q2-]
J2 ]
h = [-b(20102 + ,~2)sin q 2 + D10,]
bO 2 sin q2 --k- D2q 2 (2.40)
Object
9 P -
C & m e r a
Motor2
Zc 11 . J .
i :l I!o,orl
FIGURE 2.8
Two-link direct drive robot (side view).
where
J1 = 11 + rnllql + 1172 + ll J2 = 12 + m21.2
h = m2111:l 2 (2.41)
In these equations, I i and D1 i are the m o m e n t of inertia and the mass of the ith link; loi is the length between the ith joint and the mass center of the ith link" D i is the m o t o r friction coefficient of the ith joint.
C a m e r a M o d e l
Let l 1 be the length of link 1 and [/,,x 1,.:,] T be the c a m e r a position with respect to link 2.
Object
Yobj - " "Q y Camera
! . . . . . . . .
Yc x- c . ~
FIGURE 2.9
Two-link direct drive robot (overview).
4 NONLINEAR CONTROL LAW 73
Then we have the camera position and orientation
Scare
11C 1 Jr- lcxC12 -- lcyS12 11Sl + lcxs12 n t- lcyc12
a Gain 0 0 01 + 0 2
t position
t orientation
(2.42)
where s 1 = sin ql, cl = cos ql, $12 = sin(q1 + q2), C12 = cos(q 1 + q2)" Let the object position be Sob j = [_Xob j Yoba Zobj 0 0 0] r (the orientation has no meaning because the object is a point). Assume that the depth Z = Zob a -- Zca m is known. Let the feature vector ~ = Ix y ] r be the position of the object in the image plane, f be the focal length of the lens, and r i be the ith element of r. Then the object position with respect to the camera coordinate system is given by
X - cos r 6 -- sin r6 0 r 1 zY = sin r6 -- cos r6 0 r2
0 0 - 1 r 3
(2.43)
Therefore we have the camera model
(2.44) It is straightforward to compute (2.44), and we obtain
I lcx -+- 12C2 -- X~ -- Y~ 1
= F -lcy -+- 11S2 XobjS12 oF YobjC12 (2.45)
f m
where F = Z ' $2 m sin q2, c2 "-- COS q2"
lacobians
For this two-link robot example, the derivative of (2.6) is given by (2.8) with
J = F[ X~ Y~ --llS2 -qt- X~ -- Y~ L - - F
- - X o b j C 1 2 - YobjS12 11C2- X o b j C 1 2 - YobjS12 '
I c12 --$12] (2.46) --$12 C12
It is easy to see that
det J = F211(XobjS1 -- YobjCl), det L = - - f 2 (2.47) Thus, J becomes singular only if the object is on the line connecting the first and second joints (just above the first link and its extension), and L is always nonsingular. To avoid the
singular configuration, one may select more features than necessary. An example with redundant features may be found in [23].
Object Motion Model
In the previous example, the object height is constant. Thus the object degree of freedom is 2 and the object position is uniquely defined by p = [Xob j Yobj] r.
Straight Motion As shown in Figure 2.10(a), if the object motion is straight and the object velocities in the X and Y directions are v x and Vy, respectively, then we have (2.3) with
W(p) = I, O* = [v x Vy] r (2.48)
Circular Motion If the object motion is circular with constant velocity e) as depicted in Figure 2.10(b), the object position and its time derivative are described by
[ X ~ Y,,bj ] rc~ r sin cot ' ~ drX~162 L Yobj _] r~o COS o)t (2.49)
where r is the radius of the circle. Thus the object velocity is given by (2.9) with
- Yobj] 0* = co (2.50)
W(p) = x,,~ J'
For the case of an unknown center of the circle, let the center be (c:,, Cy). Then the object becomes
Yohj L r sin r + c~, (2.51)
Since the object velocity is the same as (2.49), we have the following parameterization:
I (0 1
o lol
W ( p ) = X obj - - 1 ' =
L.X',,j
Figure 8 Motion For "figure 8" motion as shown in Figure 2.10(c), the object position
Y o~
obj"
1
FIGURE 2.10
Object motions: (a) linear, (b) circular, (c) figure 8.
4 NONLINEAR CONTROL LAW 75
becomes
Yobj _] sin 2cot d (2.53)
Then the m o t i o n is modeled by (2.9) with
0 X~ ] 0 " = (2.54)
w(p) = ~ Y L / '
where R = - R~
2R 2 Controller
Suppose that ~e = [0 0] r. If the distance from the object to the origin of the work space is close to 11, the second joint angle is close to re/2 and the configuration is far from singular.
F o r simplicity, we consider such a task and assume that J is invertible. Since n = m = 2, B = I and z = ~.
If the object motion is straight, the object m o t i o n model is (2.48) and we have
NE x
'~--- (01 -}- 02) 0 J + Fla01 N = 2F(c)1 + 02)[--C12 $12 --C121_$12 Thus we obtain the controller
c2]]
s 2
(2.55)
"r = M J - l ( - KlZ + h (2.56)
If the object motion is circular, the object m o t i o n model is (2.49) and we have
~" = Jg/" + /~ + N o + (1)602 C12 $12 Xobj A
N = 2(01 4- 02)(I ) (2.57)
where 2 is given by (2.55). Thus the controller is
"C -- M J - I ( - K 1 z - K 2 " 2 - 2 - No -(~co 2) + h (2.58)
5 LINEARIZED CONTROLLER
The controller given in the previous section is rigorous and useful for theoretical analysis.
However, the computational burden of the inverse dynamics becomes very large for robots that have m a n y degrees of freedom, and real-time implementation of inverse dynamics is not easy [20, 21].
On the other hand, m a n y general-purpose industrial robots are the velocity control type and the robot joint contains high-ratio gears. Thus the terms of acceleration, centrifugal force, and Colioris force can be neglected. F o r these robots the controller can be linearized.
Since the computational burden is reduced considerably by linearization, a linearized controller is useful for implementation on industrial robots.
The linearized dynamics of the visual servo system (2.24) at (q, p) = (q*, p*) is given by
= BrJil + B T L W O * (2.59)
where J = J(q*, p*) and L = L(q*, p*). Then the following control law:
~/= (BTj) - 1(/.) __ B T L W O , ) (2.60) linearizes the dynamics from v to z as :~ = v. Thus, with a positive definite gain matrix K, the feedback law v = - K z makes the equilibrium point z = 0 exponentially stable.
For the linearized system (2.59), a linearized observer similar to (2.31) can be used:
&
) = BTJiI + B T L W O + H(2 -- z), O = - - W T L T B p ( 2 -- z) (2.61) Then the observer-based controller becomes as follows:
(I = ( B r j ) - 1(_ K 2 - B T L W O ) (2.62) Note that B, J, and L are constant matrices but W may depend on the object position p.
Thus the observer-based controller is not strictly linear but the nonlinear dynamics of the robot are not considered. Some examples of the linearized controller with the P U M A robot will be described in the following section.
6 EXPERIMENTS