In this section we discuss the problem of multiple sensor-based robot motion planning and control involved in performing tasks such as tracking and grasping a moving part. The problem we consider is to make a plan for a robot to track a target whose position, orientation, and velocity are measured by the estimation and calibration schemes discussed in Section 3. In doing so, we use the concept of parallel 9uidance. We propose that an error reduction term be added to the position and velocity of the target to form a desired position, velocity, and acceleration pofile for the robot. When the error reduction term is carefully planned, it guarantees a time optimal and robust robot motion with given bounded control.
The planner, we show, can be implemented as both time based and event based. This leads to a new event-based tracking scheme for a robot.
Robot Control
The dynamic model of a robot arm is given by
D(q)o" + C(q, O) + G(q) = z (5.14)
where q, D(q), C(q, 0), G(q) and z are respectively the joint angle vector, inertia matrix, load related to centripetal and Coriolis forces, load related to gravity, and joint torque vector. The joint torque has to satisfy the following constraints:
"Ci,min(q, q) ~ "Ci ~ "el,max(q, q) i = 1, 2 , . . . m (5.15) where m is the number ofjoints. The output is given by Y = H(q) = ( X , O) t where X E ] ~ 3 and 0 ~ R 3 represent the position and orientation of robot end effector. Let us consider the nonlinear feedback control law [41] given by
z = D ( q ) J - l ( q ) ( A d + K~(Va -- ~Y) + Kp(Yd - Y) - )(q)O) + C(q, il) + G(q) (5.16) where J(q) is the Jacobian of H(q) with respect to q; K v and Kp are the velocity and position feedback gains; A d, Vd, and Yd are the desired acceleration, velocity, and position vectors respectively. It is assumed that the robot has six joints, for otherwise a pseudoinverse is to be used in (5.16). It can be shown that if A a, V d, and Yd satisfy the constraints Yd = V~ and
= A d, then with proper choice of Kv and Kp, the closed-loop system is stable and the tracking error will vanish asymptotically provided the induced joint torque is within the limit specified by (5.15). The problem of robot motion planning considered here is to design A d, Vd, and Yd SO that the robot end effector will track the motion of a part.
Robot Tracking with Planned Error Reduction: Parallel Tracking
The main problem in robot tracking is to eliminate the position and velocity error between the robot end effector and the part by controlling the robot. An obvious tracking plan would be to choose
v~ = v.(t)
Aa= V~
(5.17)
which is to let the desired motion to be the motion of the part. However, if such a plan is used to control the motion of a robot, the required control will be large when the initial error between Ye, the position of the part, and Y, the actual position of the robot, is large.
A large value of the position error can easily cause the command torque to exceed the limit and the lower level controller to shut down. Usually, in a global tracking problem the initial value of the position error is very large. The tracking problem [ 4 2 - 4 4 ] studied in the literature has always been a local tracking problem, wherein it is assumed that the initial error is either zero or remains very small.
In order to circumvent the problem of dealing with a large value of the initial error, a new tracking plan is proposed in the form of
f Y. = Y.(t)+ Ye(t) V. = = V.(t) + ?e(t)
Aa = ~ = Ap(t) + Ye(t)
(5.18)
where Ye is an error reduction term that is set to Y(0) - Yp(0) at the time tracking starts and is gradually reduced to zero according to a plan. The proposed plan is based on using optimal control where the bounds on the control have also been taken into consideration.
The proposed plan controls the robot to move at the same speed as the part, while the position error is gradually reduced to zero according to an error reduction plan. This scheme is motivated by the parallel guidance in missile pursuit, hence the name parallel tracking. The difference, however, is that the velocity of the part also needs to be tracked for stable grasping.
After the error reduction term is added to the target position, the initial positional error is set to be zero to guarantee that the control will not be out of range when tracking starts.
The error reduction term will be planned so that the initial positional error will be reduced with a feasible control command.
The torque demand of a planned motion depends on the planned path, speed, and acceleration. It is usually difficult to obtain. Another method of motion planning often adopted is to specify a conservative constant speed and acceleration limit based on the off-line kinematic and dynamic work space analysis. In this way the motion planning problem is greatly simplified, making such planning schemes as time-optimal and minimum- energy planning possible [45, 46].
Assume the constraints on the desired motion are given by ][ Vail ~< Vm and IIA,~II ~< am. We also assume that by prior knowledge, the motion of the target satisfies the constraints [[ Vv[ [ ~ Vmp and lIAr[ [ ~ amp. It follows from (5.18) that a conservative constraint on the error reduction term planned error would be ~F e ~ v= - v,,,p and Ye [ ~ a,,, -- amp. It is reason- able to assume Vmv ~ v m and amp ~ v= for the robot to be able to keep track of the target.
Another side effect of adding the error reduction term is that the resultant small tracking control error makes it possible for us to ignore the fact that the space of orientation angles is not Euclidean and to treat the orientation tracking in the same way as position tracking is treated.
Time-Based Optimal Parallel Tracking
When the error reduction term is planned to reduce the initial error in a time-optimal way, the robot tracking plan in (5.18) is considered time optimal. The time-optimal planning
2 GRASPING 163
problem is stated as follows.
u min T = t ~ dt
= A
[[V][ ~< Vmye, [IA I ~ amye, g(o) = g ( r ) = O, ge(O)= Y ( O ) - gp(o), Y e ( r ) = 0
(5.19)
Let us define s = I[ Ye(t)[[ 9 The preceding time optimal problem can be stated as follows:
min T = dt
{~b = v - - a
[V[ ~-~ Umye, [a[ ~ a m y e , v(O) = v ( r ) = O, s(O) = Y(O) - - Y p ( O ) , s ( r ) = 0
(5.20)
This is a classical b a n g - b a n g time optimal control problem [47] and the solution is given by
a --
Umye > t >~ 0 amy e
amye
O, s(O) > t ~ Umye
Umye amye
s(O) amye, t >/
Umye
(5.21)
Let (m, n, p)r be a unit vector along the direction of the vector Y - Ye" The error reduction
, o
term Ye and its two derivatives Ye and Y~ are given by Ye = (m, n, p)ra(t)
~e = (m, ~, p)~(t) Ye = (m, n, p)rs(t)
(5.22)
where v(t) and s(t) are solution of (5.20) with a(t) as input. Note in particular that the vector (m, n, p)r is independent of t.
The initial velocity error between the robot and the target can sometimes cause the control to be out of range too. A similar technique can be used to overcome this problem by adding an error reduction term V~ in the velocity. The robot tracking plan in (5.18) can be rewritten as
L
= Y.(t)+ Ye(O + Vedt v~ = ~ = v~(t)+ Ye(t)+ Ve Ad = (/d = Ap(t) + Ye(t) n t- (Z e
(5.23)
where V e is an added term to cancel the initial velocity error between the robot and the target.
Ve is set to V(0) - Vp(0) at the time tracking starts and will be gradually reduced to zero based on a plan. The plan of Ve can be designed in the same way as the plan of Ye"
Event-Based Optimal Parallel Tracking
Event-Based Planning Traditionally, a motion plan for a robot is expressed in terms of time and is driven by time during on-line execution. That is, the control command is stored as a function of time and is indexed on the basis of the time passed during on-line execution. The problem with this kind of planning and control is that the progress of planned motion is not affected even when the robot fails to follow the planned motion for unexpected reasons such as the existence of an obstacle or malfunction of a device. Therefore the control error will become very big and cause the control to be out of range and the controller to shut down.
In order to overcome this problem, the concept of event-based planning and control for a robot following a given path has been introduced by Tarn, Bejczy, and Xi [46]. The problem they tried to solve was to design a motion plan for a robot following a given path which constant constraints assigned on the velocity and acceleration of the robot. Such a motion plan can be time optimal or of minimum energy [45].
The event-based planning and control scheme tries to parameterize the motion plan in terms of curve length s traveled along the prespecified path. Let v = ~, a = b, and w = v 2 and define u = dss" The motion-planning problem is now stated as follows. da
min T = dt = - ds
U
I d - ~ s 2 a =
l' -u
Iwl ~< Win, lal ~< am, lu[ ~< U m, w ( O ) -'- w ( S ) = a ( O ) = a(S) = 0
(5.24)
where wm and u m are, respectively, an equivalent speed limit and the jerk constraint to guarantee smoother robot motion. This is again a classical time-optimal b a n g - b a n g control problem. The solution of this problem is given in the form of u(s), a(s), and w(s) or v(s) = x ~ ( s ) [46].
F o r a straight-line path the vector motion plan is given by
= (m, n, p)Ta(s)
= (m, n, p)rv(s) Ya = (m, n, P) Ts + Yo
(5.25)
where Yo is the beginning point of the path and (m, n, p)r a unit vector along the path.
The motion reference s and desired position Y~ can be determined during actual execution by projecting the actual position of the robot onto the given path and then choosing the projection as Yd. The minimum positional control error is generated when Ya is chosen in this way. The parameter s is thus the curve length between this Ye and the beginning point I1o. It is easy to observe that the motion reference s reflects the actual execution of a planned motion. The basic concept of this entire approach is actually the well-established feedback control principle.
2 GRASPING 165
The main advantage of using event-based robot motion planning and control is that the motion planning becomes a closed-loop process. The plan is realized during execution based on sensory measurements. It provides an efficient closed-loop algorithm for implementing the tracking control in real time, since the location and motion of the part as a function of time are unknown prior to execution. An important contribution of this chapter is to develop such an event-based tracking scheme based on the multiple sensory integration to achieve (1) stable global tracking and (2) robust grasping control of a part with unknown orientation.
Event-Based Tracking The concept of event-based planning and control can be employed to plan the error correction Ye in (5.18). A straight line is chosen as the path for Ye, which is the same as in Section 3.3.
The robot tracking plan in Section 3.3 can be reparameterized in terms of s, which is the distance between the part and the robot. This new choice of the event-based motion reference reflects the special characteristic of the tracking problem; that is, the target path is unknown prior to the execution.
Using the new motion reference, the event-based error correction term Ye can be obtained as
amy e ,
a - - O,
amye,
Vmye < S ~ S(0) s(0) 1 2
2 amy e
1 2 Umye
< s ~ s(O) 2 amy e
1 2 1.)my e S ~ 2 amy e
1 2 IOmye 2 amy e
(5.26)
and
V
1 - - X / S a m y e ( S ( O ) -- S), s(O) -- 2
Vmy e < S ~-~ s(O)
1 2 1 2
Vmye < S ~ S(0) Vmye
l')mye' 2 amy e 2 amy e
1 Umy e 2
--N/SamyeS, S ~ ~ amye
(5.27)
The vector motion plan is obtained as in (5.22) except that a and v are now functions of s.
The motion reference s is generated by calculating the magnitude of Y - Yp.
Since the motion plan is now driven by an event-related or state of execution-related motion reference rather than time, this robot tracking scheme has all the advantages of original event-based motion planning and control. This new scheme is called event-based robot tracking. It is a new extension of the event-based robot motion planning and control strategy.