SICE Annual Conference in Fukui, August 4-6, 2003

Fukui University, Japan

Robot Arm Trajectory Planning Using Missile Guidance Algorithm

DongKyoung Chwa, Junho Kang, Ki Hong Im, and Jin Young Choi

School of Electrical Engineering, ASRI, Seoul National University, Seoul, Korea, jychoi@ee.snu.ac.kr

Abstract dynamic, torque, and velocity constraints, and also

This paper presents a novel approach to on-line trajectory guarantees the position and velocity matching at the end
planning of robot arms for the interception of a fast- phase. The proposed method uses three types of axes namely
maneuvering object under torque and velocity constraints. joint axis, inertia axis, and body axis to describe the robot
arm to intercept a fast-maneuvering object, while only joint
Especially, the trajectory planning in the Y - direction
and inertia axes have been used for the interception of robot
employs a missile guidance algorithm to intercept the
arm in [4, 5].
maneuvering object. The proposed method yields that a
robot arm can smoothly grasp a maneuvering object.
Simulation results are included to demonstrate the proposed II. On-Line Trajectory Planning
method. Algorithm
Index terms: on-line trajectory planning, torque and
velocity constraints, guidance algorithm. G 2.1 Trajectory Planning Coordinates
Joint and inertia axes are usually used to describe the
position of the end-effector of a robot arm as shown in Fig. 1.
I. Introduction The angle position in joint axis and the plane position in
In this paper, a novel trajectory planning method is inertia axis can be transformed into each other through
presented for robot arms to track, intercept, and grasp a kinematics and inverse kinematics. While the guidance
moving object. Tracking and grasping a moving object is a algorithm is used in inertia axis in [4, 5] for the robotic
typical role of robot arms, which have many applications interception, it is only used in the initial part of the tracking
such as picking-up work-pieces from conveyer, playing table and is replaced later by a computed-torque control method,
tennis, catching mouse, and catching a ball. and also velocity constraint is not considered.
In case of a fast-maneuvering object, which move with
arbitrary random acceleration, visual feedback-based
tracking method [1], heuristic method for local minimum
time [2], and potential–field based method [3] have been
studied for the trajectory planning, where actual issues such
as dynamic constraints and computation problems have not
been considered.
Recently, trajectory planning methods have been
proposed in [4] and [5] using missile guidance algorithm
Proportional Navigation Guidance (PNG) and Augmented
Proportional Navigation Guidance (APNG), respectively;
guidance laws have been used for missiles tracking a
random-maneuvering object. While the position and velocity
matching conditions are satisfied under torque constraint, a
hybrid interception method is proposed where the velocity
constraint is not considered, and the guidance algorithm is
mŽUGXUGq–•›GO 41 , 4 2 PGˆ•‹G•Œ™›ˆGO X I , YI PGˆŸŒšUG
employed at the onset of its tracking and is replaced later by
a PD-type computed-torque control method. Thus, the Since the guidance law of missiles can be described in
advantage of the guidance-based approach is not fully inertial and body axes [6], body axis is newly introduced,
employed in these schemes. where X - direction of body axis is the moving direction of
In this paper, we present a novel method for intercepting the robot arm and Y - direction is orthogonal to X -
a fast-maneuvering object, which considers kinematic, direction to have the right-handed coordinate system. That is,

-859- PR0001/03/0000-0859 ¥400 © 2003 SICE

G
body axis can be obtained by rotating inertia axis by the
angle between the robot arm velocity vector and X -
direction vector of inertia axis.
2.2 Generation of Acceleration Commands
Considering Velocity Constraint
Y - Acceleration Command for Direction
Planning
As the process where the robot’s end-effector approaches
a moving object is similar to the process where a missile is
guided to an object, the guidance algorithms can be used for
the robotic interception problem in generating Y -
acceleration command of body axis. Various guidance
algorithms have been developed for the missile system [6].
Among these guidance algorithms, the combination of
pursuit guidance and proportional navigation guidance are
adopted as in [7], since proportional navigation guidance is
accurate with respect to the object maneuver, but sensitive to
noise while pursuit guidance is robust against the noise, but
inaccurate with respect to the object maneuver. It is briefly
reviewed in the following.
The interception geometry is shown in Fig. 2. The
equation of planar pursuit-evasion can be given by
dR
VT cos(V  T T )  V M cos(V  T M ) (1)
dt
dV 1 acceleration until it reaches the maximum velocity. In the
[ VT sin(V  T T )  V M sin(V  T M )] (2)
dt R
where VT and VM are the velocities of the object (or
target) and robot’s end-effector (or missile), the elapsed time
t is used as an independent variable, R is a relative
distance between them, V is a line- of-sight (LOS) angle,
and TT and T M are (flight) path angles of the object and
the end-effector, respectively.
Fig. 2. Interception geometry
To guide the end-effector toward the interception point,
-860-
we use the following guidance law suggested in [15]:
dT M dV
a  b sin(V  T M ) (3) or
dt dt
dV
Ay a  b sin(V  T M ) (4)
dt
where A y is Y - acceleration command of body axis. The
first term on the right-hand side of eqn. (4) represents the
proportional navigation, and the second one represents the
pursuit guidance. Thus, this guidance law is constructed by
combining these two guidance laws through the constant
parameters a and b , by which the properties of each
guidance law is retained and the acceleration magnitude or
flight time can be adjusted. The effectiveness of the above
guidance law is numerically tested in [15].
X - Acceleration Command for Speed Planning
Using the guidance law for Y - acceleration command of
body axis in subsection 2.2.1, X - acceleration command is
designed to make the relative distance and velocity in the
X - direction to be zero in this section. Just as in [4, 5] for
the non-maneuvering object, the robot arm interception for
the fast-maneuvering object is composed of the following
phases: i) acceleration phase, ii) constant velocity phase, and
iii) deceleration phase. In the acceleration phase, X -
acceleration command of body axis starts with maximum
constant velocity phase, acceleration command is set to be
zero and the velocity keeps its limit value.
While these two phases can be readily designed, the
switching time from constant velocity phase to deceleration
phase should be chosen and the acceleration command in
deceleration phase should be generated to satisfy the
position and velocity matching conditions. First, when we
assume that there is no relative distance and velocity in the
Y - direction and no torque constraint as well, X -
acceleration command can be simply selected as
Ax  Ac (5)
where Ac ! 0 is a constant acceleration within the torque
constraint. In this case, the switching time from constant
velocity phase to deceleration phase can be determined as
follows.
Denoting R0 and V0 to be the relative distance and
velocity at the time when constant velocity phase to
deceleration phase, the relative distance and velocity can be
expressed as
G
t Ax k d v(t )  k p r (t )  AT (13)
r (t ) R0  ³ 0
v(W )dW
Ax 2 where k p , kd ! 0 are constants for more smooth
R0  V0 t  t (6)
2 interception, and position and velocity matching. Eqn. (13)
A reduces to r  k d r  k p r 0 , which results in the
R0  V0 t  c t 2
2 convergence of r and v to zero.

v(t ) V0  Ax t V0  Ac t (7) 2.3 Modification of Acceleration Commands

Considering Torque Constraint
where t is elapsed time after the switching time. From eqn. The dynamic equation of robot arm in joint axis is given by
V0
(7), v(t ) becomes zero after t . Since r (t ) also D (q )q  C ( q, q ) q  G ( q) W (14)
Ac
V0 where q is angle position vector.
becomes zero after t from eqn. (6), the switching If the position vector of the end-effector in inertia axis is
Ac
Z ee ( Z X , Z Y ) I , the relation between Z ee and q can
time can be chosen to be the time when R0 and V0
be shown as
satisfies
Z ee P (q ) , Z ee J (q )q , Zee J ( q )q  J ( q) q (15)
V02
R0 (8) where P ( ˜ ) is a transformation operator from joint axis to
2 Ac
inertia axis, and J ( ˜ ) is a Jacobian matrix of P ( ˜ ) .
Actually, since the interception time increases due to the Substituting eqn. (15) into eqn. (14), we obtain
object maneuver, actual relative distance and velocity
denoted by ra (t ) and v a (t ) are different from r (t ) and DJ 1 Zee  (C  DJ 1 J ) J 1 Z ee  G ( P 1 ( Z ee )) W (16)
v(t ) in eqns. (6) and (7), as
For W [W 1 W 2 ] , the torque constraint can be given as
( Ax  AT ) 2 W i ,min d W i d W i ,max , i 1, 2 . Then, using eqn. (16) we can
ra (t ) R0  V0 t  t (9)
2 have the acceleration constraint in inertia axis ( X  , Y ) and
I I
 , Y ) through rotation
then that in body axis ( X
v a (t ) V0  ( Ax  AT )t (10) B B
transformation.
where AT is x -component of the object acceleration in
body axis. Thus, the acceleration command in the X - X - and Y - acceleration commands must be constrained
direction in eqn. (5) need to be augmented as to satisfy the torque constraint. In the case of acceleration
phase, direction planning is more significant than speed
Ax  Ac  AT . (11) planning at the onset of the interception of the maneuvering
object; thus, Y - acceleration is first determined within the
Here, AT is assumed to be available as usually done in torque constraint, and then X - acceleration is determined
augmented proportional navigation guidance law for missile within the resulting constraint by Y - acceleration as in Fig.
systems [6]. While the choice of Ac to be the maximum 3 (a). In constant velocity phase, X - acceleration is set to
acceleration within the torque constraint can be good from be zero and, accordingly, Y - acceleration can be
the time optimal aspect, this may result in non-smooth constrained within the maximum possible value, as shown in
Fig. 3 (b). In deceleration phase, since the end-effector is
interception motion. So, we have chosen Ac to be a proper
kept toward the object by the guidance law during the
constant within the torque constraint. In addition, when acceleration and constant velocity phases, and furthermore
r (t ) and v(t ) satisfy the position and velocity matching conditions need to be
satisfied, X - acceleration is first decreased unlike the
k d v  k p r d ro (12) acceleration phase, and then Y - acceleration is determined
as in Figs. 3 (c) and (d).
for a proper constant r0 after the switching by eqn. (8) is
done, Ax is further set to be

-861-
G

Table 1. X - and Y - Acceleration Commands.

X- Y - acceleration command
Phase acceleration
( Ay )
command ( Ax )
I maximum value
guidance law in eqn. (4)
(Acceleration within torque
constrained as Fig. 3 (a)
phase) constraint
II
guidance law in eqn. (4)
(Constant 0
constrained as Fig. 3 (b)
velocity phase)
III eqn. (11)
(a) acceleration phase. guidance law in eqn. (4)
(Deceleration constrained as
constrained as Fig. 3 (c)
phase) Fig. 5 (c)
IV eqn. (13)
guidance law in eqn. (4)
(Deceleration constrained as
constrained as Fig. 3 (c)
phase) Fig. 5 (c)

Table 2. Switching Time Between Phases I, II, III, and

IV.
Phase Change Switching Time
when the velocity of end-effector reaches
I o II
maximum velocity
when the relative velocity and relative
II o III
distance satisfy eqn. (8)
when the relative velocity and relative
(b) constant velocity phase. III o IV
distance satisfy eqn. (12)

III. Numerical SimulationsG

The robot arm considered in this paper is a two-link planar
manipulator. The physical parameters of the robot arm are
shown in Table 3. Design parameters a and b in eqn. (4),
Ac in eqn. (11), r0 in eqn. (12), and k d and k p in eqn.
(13) are selected as
a 3, b 3, Ac 2, r0 10 ,k d 2[Z n
2
(17)
kp Z ,[
n 0.707, Z n 5
(c) deceleration phase.
The limit of velocity is set to be 1 m / sec as shown in Figs.
Fig. 3. Generation of acceleration commands considering
torque constraint. 8 (b) and 9 (b).
Scenario is given in Table 4, where the initial position and
X - and Y - acceleration commands for each phase and the velocity of the object and the end-effector in inertia axis are
switching time between these phases are arranged in Tables arranged. Profiles of object maneuver for the scenario are
1 and 2. It should be noted that there is a result on reference arranged in Table 5. The object maneuver model is chosen
governor in the control context for input saturation problem to be a low-pass filter with the time constant 0.3. The
[8], where the reference input is generated within the tolerances of position and velocity errors are selected as
maximum region to avoid the input saturation. In the same 1% and 2% of their initial errors, respectively.
sense, acceleration commands are generated within torque
constraints.

-862-
G
Table 3. Physical parameters of a robot arm velocity at the time of interception are satisfied altogether.
This was possible by introducing body axis as trajectory
l a m I W max planning coordinates and separating the trajectory planning
I1 problem into direction planning and speed planning of robot
a1 m1 W1 max
First l1 arms. Simulations show that the proposed trajectory
joint 0.5m 1kg 0.2kg ˜ m 2 20 N ˜ m planning method can have the satisfactory performance
1m
against the object maneuver. The experimental issues using
a2 I2 W 2 max computer vision for object information can be pursued as a
Second l2 m2
0.5m 0.2kg ˜ m 2
20 N ˜ m possible further work
joint 1m 1kg

References
Table 4. Initial position and velocity of the object and the
end-effector. [1] A. J. Koivo and N. Houshangi, “Real-Time Vision
Initial Initial
Feedback for Servoing Robotic Manipulator with Self-
Initial position of velocity of Tuning Controller,” IEEE Transactions on Systems,
Initial velocity of
position of
object (m/sec)
end- end- Man and Cybernetics, Vol. 2, No. 1, pp. 134-141, 1991.
object (m) effector effector [2] Z. Lin, V. Zeman and R.V. Patel, “On-Line Robot
(m) (m/sec)
X Y - X Y - X Y -
Trajectory Planning for Catching a Moving Object,”
-dir dir
X -dir Y -dir
-dir dir -dir dir Proceedings of the IEEE International Conference on
0.8 1 0.2954 0.0521 0.2 1.7 0 0 Robotics and Automation, AZ, pp. 1726-1731, May
1989.
Table 5. Profiles of object maneuver [3] A. A. Masoud and M. M. Bayoumi, “Intercepting a
Switching time Maneuvering Target in a Multidimensional Stationary
Object maneuver Environment Using a Wave Equation Potential Field
2
( m sec ) between each
maneuver (second) Strategy,” Proceedings of the IEEE International
First– Second– Symposium on Intelligent Control, Columbus, OH, pp.
First Second Third Second Third 243-248, August 1994.
Scenario 0.5 2 -3 1.0 1.5 [4] M. Mehrandezh, M. N. Sela, R. G. Fenton, and B.
Benhabib, “Proportional Navigation Guidance in Robot
The results of the proposed method are arranged in Table Trajectory Planning for Intercepting Moving Object,”
6, which shows the satisfactory performance against the Proceedings of the IEEE International Conference on
maneuvering object. Simulation results of Scenarios 1 and 2 Robotics and Automation, Detroit, Michigan, pp. 145-
are shown in Fig. 4 (a), the trajectory of the object and the 150, May 1999.
end-effector is shown and their relative distance becomes [5] M. Mehrandezh, N. M. Sela, R. G. Fenton and B.
sufficiently small. In Fig. 4 (b), the velocity of the end- Benhabib, “Robotic Interception of Moving Object
effector satisfies its velocity limit of 1 m / sec . Following Using an Augmented Ideal Proportional Navigation
the acceleration commands in body axis in Fig. 4 (c), the Guidance Technique,” IEEE Transactions on Systems,
torque remains within the limited values as in Fig. 4 (d). The Man, and Cybernetics, Vol. 30, No. 3, pp. 238-250, May
positions and velocities of the object and the end-effector in 2000.
the X - and Y - directions are shown in Figs. 4 (e), (f), (g), [6] C. F. Lin, Modern Navigation, Guidance, and Control
and (h). Processing, Prentice Hall, 1992UG
[7] T. Takehira, N. X. Vinh, and P. T. Kabamba, “Analytical
Table 6. Performance of the proposed method. Solution of Missile Terminal Guidance,” Journal of
Guidance, Control, and Dynamics, Vol. 21, No. 2, pp.
Final distance Final velocity Elapsed 342-348, 1998.
error(m) error(m/sec) time(t) [8] A. A. Rodriguez and Y. Wang, “Performance
Scenario 0.0004 0.0012 1.9290 Enhancement Methods for Unstable Bank-to-
Turn(BTT) missiles with saturating actuators,”
IV. ConclusionG International Journal of Control, Vol. 63, pp. 641-678,
1996.
This paper presents a novel approach to on-line trajectory
planning of robot arms for the interception of a fast-
maneuvering object. Unlike the previous methods, a
guidance law is employed throughout the tracking phase,
and dynamic constraints such as the torque and velocity
constraints and the matching condition of the position and

-863-
G

(a) Trajectory of the object and the end-effector (e) Position in the X - direction

(b) Velocity of the end-effector (f) Position in the Y - direction

(c) Acceleration commands in body axis (g) Velocity in the X - direction

(d) Torque commands in inertia axis (h) Velocity in the Y - direction

Fig. 4. Tracking performance (T: object, E: end-effector).

-864-