Optimum Path Planning for

Mechanical Manipulators1
To assure a successful completion of an assigned task without interruption, such as
the collision with fixtures, the hand of a mechanical manipulator often travels along
a preplanned path. An advantage of requiring the path to be composed of straight-
line segments in Cartesian coordinates is to provide a capability for controlled
J.Y. S. Luh interaction with objects on a moving conveyor. This paper presents a method of
Professor. obtaining a time schedule of velocities and accelerations along the path that the
manipulator may adopt to obtain a minimum traveling time, under the constraints
C. S. Lin of composite Cartesian limit on linear and angular velocities and accelerations.
School of Electrical Engineering,
Because of the involvement of a linear performance index and a large number of
Purdue University, nonlinear inequality constraints, which are generated from physical limitations, the
West Lafayette, Ind. 47907 "method of approximate programming (MAP)" is applied. Depending on the
initial choice of a feasible solution, the iterated feasible solution, however, does not
converge to the optimum feasible point, but is often entrapped at some other point
of the boundary of the constraint set. To overcome the obstacle, MAP is modified
so that the feasible solution of each of the iterated linear programming problems is
shifted to the boundaries corresponding to the original, linear inequality con-
straints. To reduce the computing time, a "direct approximate programming
algorithm (DAPA)" is developed, implemented and shown to converge to op-
timum feasible solution for the path planning problem. Programs in FORTRAN
language have been written for both the modified MAP and DAPA, and are
illustrated by a numerical example for the purpose of comparison.

I Introduction
The advancement of technology makes it possible for a
computer to perform on-line control of mechanical
manipulators [1,2]. However, the path traveled by the hand of
a manipulator is still preplanned to assure a successful
completion of a mission, such as getting in and out of a
fixture, and avoiding the collision with obstacles. To facilitate
the controlled interaction with objects on a moving conveyor
[3,4,5], the preplanned, traveled path is composed of straight-
line segments in Cartesian coordinates connected by smooth
arcs. The manipulator rests at its initial location before it
moves, and stops at its final location. The joints of the
manipulator are driven by electical motors and controlled by
servo systems which are designed for the worst case. In actual
operation, the manipulator thus performs below its
capability. In addition, some conventional controls are
designed in such a way that the manipulator stops at the end
of each line segment of the path. Consequently, the time
interval of the entire execution of the manipulator is not
efficiently utilized. It is seen that the elimination of the stop at
the end of each line segment and the use of the maximum
velocity under physics', constraints will allow the manipulator
to move more rapidly and hence the total execution time is
1
Supported by NSF Grants ENG 76-18567 and APR 77-14533.
Contributed by the Dynamics Systems and Control Division for publication
in the JOURNAL OF DYNAMIC SYSTEMS, MEASUREMENT, AND
Manuscript received by the Dynamic Systems and Control Division, June 5,
1980.
reduced. A preliminary investigation of this problem has been
reported earlier [6].
The real, physical constraints on the manipulator are the
applied torques/forces at the joints. Because a serial link
manipulator is a highly nonlinear system and all its joints are
coupled, it is very difficult to convert the bounds on
torques/forces into one on accelerations and velocities of each
joint [7,8]. In the references, bounds on joint velocities and
accelerations are assigned based on experimental data for a
specific manipulator. It seems that this assignment does not
have an advantage of accuracy and efficiency over the
assignment of the bounds on hand velocities and accelerations
using experimental data. In addition, there are constraints
which are naturally specified in Cartesian coordinates, such as
interaction between the manipulator and objects on a moving
conveyor, transportation of liquid in open containers, etc.
One may suggest that the Cartesian path could be trans-
formed into joint coordinates and then the constraints on
joint velocities and accelerations could be imposed since
Jacobian transformations are well developed [2,3,4,7].
However, the transformations are valid on a point-to-point
basis. No transformations of functions of time between the
two coordinates are known. An attempt has been made to
approximate the Cartesian path of the hand in joint coor-
dinates by means of spline functions and least square error fit
[9], but the approximation error overshadows the merit of the
CONTROL.
minization of traveling time.
Based on the reasons indicated above, the velocities and

1 4 2 / V o l . 102, JUNE 1981 Transactions of the A S M E

Copyright © 1981 by ASME

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms
 cosa COS7 — sina cos/3 sin7

sina COSY + c o s a cos

0sm7
BASE COORDINATES
sin^sinY
Pit) J"0
— cosa sin7 — sina cos/3 COSY

- sina sin7 + cosa cos/3 COS7

sin/3 COS7
n°(t)
sina sin/3 Px
(a) (b)
Fig. 1 (a) Position and orientation vectors of the hand - cosa sin/3 , P = Py (2)
(b) Eufer angles of orientation
cos/3 Pz

accelerations are constrained in Cartesian path of the hand in
this paper. The position and orientation errors on the arc of
the actual path from the corner of two adjacent straight-line
segments must be kept sufficiently small so that the actual
path does not significantly deviate from the preplanned path.
Otherwise it may have a risk of running into obstacles. These
requirements can be expressed by a set of linear and nonlinear
inequalities. As a result, one may formulate the minimum
traveling time problem as a nonlinear programming problem.
Because of the involvement of a large number of the nonlinear
inequality constraints, a feasible approach is to apply the
"method of approximate programming (MAP)" [10,11,12]
which reformulates and solves a sequence of linear
programming problems with all the nonlinearities being
iteratively linearized. Although MAP has been successfully
applied to a large class of nonlinear programming problems
[11,12], no proof of convergence has ever been published in
literature. For the path planning problem, however, it is
observed that the iterated feasible solution, which depends on
the choice of initial feasible solution, is often entrapped at
different points on the boundary of the constraint set. To
overcome this difficulty, the "lines of indifference" are in-
troduced along which the feasible solution from each iteration
may be moved away from the trap and toward the boundaries
corresponding to the original, linear inequality constraints.
By extending the method of large-step-gradient [12], a
"direct approximate programming algorithm (DAPA)" has
been developed and implemented. The technique differs from
MAP in that the restrictions on the distances between com-
ponents of two consecutively iterated feasible solutions are
not imposed in DAPA. The feasibility is, however, achieved
by adjusting the iterated solution along the direction of its
evolution. Thus each iteration produces a feasible solution.
As a result, the average computing time for implementing
DAPA is approximately 60 percent of that for MAP. Because
of the geometrical property of the constraint set, DAPA is
shown to be convergent for the path planning problem.
II Position and Orientation of the H a n d
The position and orientation of the hand are briefly
sketched in Fig. 1(a), and can be represented by a 4 x 4 matrix
[13]:
"n(0 s(0 a ( / ) p(f)
H(0
0 0 0 1
where, as indicated in Fig. 1(a), p is the position vector of the
hand; n, s and a are the unit normal, unit slide, and unit
approach vectors of the hand, respectively. The last three
orthonormal vectors specify the orientation of the hand. If
the orientation is defined in terms of Euler angles of rotation
[14] with respect to the base coordinates (x 0 , y 0 , z 0 ), then [2],
since (x 0 , y 0 , z 0 ) will coincide with (n, s, a) if: (1) first x0 is
rotated a radians about z0 so that x 0 aligns with n° where n°
is a vector lying in the (x 0 , y 0 )-plane and being normal to a;
(2) next z 0 is rotated /3 radians about n° to align with a; (3)
finally n° is rotated 7 radians about a to align with n. For
convenience, let
0' p'
H' = (3)
0 0 0 1
where
0' = [ n ' s ' a ' ] = [ n ( / , ) s ( / , ) B ( f , ) ] (4)
and
P' = P('/) (5)
For / = 0,1,. . .,m, H' are given. They specify the corners of
the desired path traveled by the hand, as shown in Fig. 2.
I l l P a t h Traveled by the H a n d
The path traveled by the hand consists of a sequence of
straight-line segments connected by smooth arcs. Let m be the
total number of segments. The (m — 1) intersection points of
every two adjacent segments are denoted by hand matrices H'
for / = 1,2, . . .,m — \. Accordingly H° and H m denote the
initial and final points, respectively. During the execution of
segment /, the position and orientation of the hand can be
either in transition from segment / — 1, or on segment /, or in
transition to segment / + 1 . Figure 2 depicts the situations. The
changes of the position and orientation of the hand are related
to, respectively, the linear velocity v' and angular velocity u'
of the hand with respect to base coordinates. In the middle
portion of segment /, v1 and a>' are assigned to be constants.
The nonzero constant accelerations V and u>' are only applied
in transition from segment / to segment i + l . For the con-
venience of discussion, let
v*=0, a)*=0forA:<l,Ar>OT, (6)
to indicate that the hand rests at its initial position and
orientation before it moves, and stops at its final position and
orientation. The condition for v* in (6) can also be expressed
by
r
p°, whenever k< 1,
(1) p* = (7)
jp m , whenever k>m.
Because of the basic difference between the descriptions of
changes in position and in orientation, it is convenient to
discuss the translation and rotation separately. As shown in
Fig. 2, Tj is the amount of time required to move the hand
through segment /, and 2T,- is the amount of time required to
transit from segment / to segment j + 1. Thus, if the zero-

Journal of Dynamic Systems, Measurement, and Control JUNE 1981, Vol. 102/143

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 Transition from

tth to l+lst segment

Segment i + t
Segment

Segment i-1 i-1

Segment 2 Segment m-1

Segment m

H " tn(t,) s_(t,) a(t.)J

1 1 £ - £(t )
non-zero constant constant v_ , o>
.0 .0 ,, J are applled
are applled
L1*L
0 I I I -0,1,

Fig. 2 Straight-line segments of traveling path

reference time is set at the initial point (i.e. at t = 0, the hand where ( )' = transpose of ( ). The angular velocity of the
starts to leave position p°), the velocity and acceleration are hand, to' with respect to the hand coordinates, is assigned to
chosen as be constant in the middle portion of segment i. Thus
i-l

v / =(p''-p'- I )/7 , ,-,forr 0 + YJ r* + T,_, <r<r 0 wi=(di/Ti)pi,ioTT0+ En

]_, T + k ri_l<t<T0
*=i *=i

+ Er*-T„ (8) + En
Li*!*-* (12)
*=1 k=\
j for / = 1,2, . . ,,m. For / = 0 and m, second half of (7)
VJ=(yJ+* -1/J)/2Tj, for T0 + E Tk-Tj<t<T0 applies, i.e.
k=\
j (o*=0, or^=0forA-<l,A:>w. (13)
+ Ln + rj (9) During the transition phase between /th and f+ 1st line
k=\ segments, the angular velocity of the hand must change from
for (=1,2, . . .,m, and 7 = 0,1,2, . . .,m. During the entire o)' to &)'+1 in 2T, seconds. Thus the required angular ac-
time interval of travel, either one of the two equations is celeration with respect to the hand coordinates is
effective, but not both. Note the vJ will assure the hand to
j
reach both its required velocity and position after it exits +l J T
from the transition phase. wJ=(oii - to )/2TJ, for T 0 + E k-Tj<t<TQ
1 *=1
The orientation changes from fi'" to fl' as the joints of the
manipulator move through segment ;'. To express the change j
of orientation linearly in terms of a rotational variable, a unit (14)
vector p' with respect to the coordinates [n s a] is introduced k=\
(Fig. 3) such that the change from fl'-1 to 0' can be achieved fory = 0,1, . . .,m. Again, either (12) or (14), but not both, is
by rotating n' (or s' or a') an angle of 6, radians about p'. effective during the entire traveling time interval of the hand.
Thus p' is fixed while the hand coordinates rotate. In terms of
[n s a], 0, and />, can be computed from [15]: IV Problem of Optimum Path Planning Subject to
, / 1
0 / =cos- {[(n )'n''- +(s )'s''-/ 1 Inequality Constraints
1 Because of physical limitations, the magnitude of velocity
+ (a')'a'- -l]/2j (10) of the hand, v (/), has an upper bound Kv, and the magnitude
and of its acceleration, v(t), is bounded by Ka. That is, for all?,
0<llv(Oll=£A'„ (15)
1 1
(a')'s'- - (s')'a'- §<\\y{t)\<Ka (16)
1
(n')'a'- - (a')'n''- 1
(11)
Likewise the angular velocity and angular acceleration of the
Isindi hand are bounded by Ka and Ka, respectively:
(s ; )' n'"' - (n')'s'- 1 0<=llctf(/)ll<tf u (17)

144/Vol. 102, JUNE 1981 Transactions of the ASME

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 0 < llco(f) II <AT„, for all r. (18) \\6>i\\<Kaforj=0,\, . . .,m. (34)
If the time interval 2T, during which the velocities change, is By (12), and since p' is a unit vector, inequality (33) reduces to
too long, the path actually traveled by the hand may con-
siderably deviate from the preplanned path, especially around 1(9,- l<ATu7,i f o r / = 1,2, . . .,m. (35)
the neighborhood of H'. This phenomenon may result in a To reduce the inequality constraints (34), let
risk of colliding with obstacles. A possible way of avoiding
p' = lp'xp'yp'J' (36)
the excessive deviation is requiring a fixed ratio between 77
and T, since T,-_, + T, < Tt as seen from Fig. 2. Thus the and
constraints for the error in path traveled are chosen as Rj = ( ^ p>-' + pjpf-' + pfjtf-' )»,«,_, (37)
0 < T , < 7 ' , / ^ , / = 1,2 . . .,m, (19) Then by (12), (13), (14), (36), and (37), inequality (34)
0 < T y < r , + 1/A-,y = 0,l, . . . , / n - l , (20) becomes
V:r?0? + 1 +7? + 1 0 2 -2r,.r,. + l J R? + 1 < 2Ka r, T, TI+, (38)
where if is a positive constant whose value can be selected
with 0,6',+ , ^Rf+i for / = 1,2 m-l, and
according to how large the deviation of the path traveled is
permissible.The advantage of choosing (19) and (20) is that («" = 0:) \e,\^2KaT0Tu (39)
they are linear inequalities. (*' = « : ) \0m\<2KaT„,Tm. (40)
From Fig. 2 it is seen that the overall execution time for the
entire path is Consequently the optimum path planning problem can be
stated as a problem of minimizing the overall execution time
m
ET of (21) with respect to 7, and ry for / = 1,2, . . .,m andy
E T = T 0 + £ : T ; + T,„ (21) = 0,1, . . .,m, subject to constraints (19), (20), (25), (30),
/=i (31), (32), (35), (38), (39), and (40). In this problem, there are
Thus the optimum path planning problem is to minimize the (2w + l) variables and (8/W + 2) inequality constraints, where
performance index ET with respect to Tt and T,- for / = m is the number of straight-line segments of the path traveled.
1,2, . . .,m and j = 0,1, . . .,m, subject to the inequality From (21), the execution time ET to be minimized is linear
constraints (7), (13), and (15) through (20). in T, and r,. Now, K, Kv,Ka,Ka,Ka, Dh Cf, Rf, and 0, are
Before solving the problem, v ( / ) , v ( / ) , w(r) and 6i(t) in either given or computed quantities. If all the inequality
the inequalities must be converted into v', V', «', to', p', p', 6h constraints were also linear in T, and T, , then the optimization
Th and T,. For (15) it can be written as problem might be solved by the method of conventional linear
programming. However, constraints (30) through (32) and
IIv'\\<KV for ; = 1,2, . . .,m. (22) (38) through (40) are nonlinear. Thus it is a nonlinear
For convenience, let programming problem and a number of search techniques are
directly applicable. In view of the involvement of a linear
X>> = \p'xPyp'zV (23) performance index and a large number of inequality con-
and straints, piecewise linear approximations that are periodically
updated are preferable. This leads to the application of the
D, = V (p'x -px~' ) 2 + (pj, -p'f ! ) 2 + ( p i -p'z-' ) 2 (24) "method of approximate programming (MAP)" [10,11,12].

then by (8), (23), and (24), inequality (22) becomes

D,*KVT, (25) V Method of Approximate Programming (MAP)
for / = 1,2, . . .,m. Likewise, (16) can be written as [10,11] and the False Solution
W\\<Kaforj=Q,\, . . .,m. (26) The method of approximate programming (MAP) was
introduced by Griffith and Stewart [10]. It has been reported
By (8) and (9), one contains that the method is successful in dealing with a large class of
^' = [(P''+1 -9>)/TJ+l -<j>i-p>-l)/Tj]/(2Tj) (27) nonlinear programming problems. Its convergence, however,
Consequently (26) becomes is not proven [11,12]. Zoutendijk believed that the method
should converge for the convex problems by formalizing the
+
II W ' ' -9i)-Tj+,(9i-pl-i)\\/{2TjTjTJ+x)^Ka (28) choice of distance bound 5* as defined in [11]. To apply the
for/ = 0,1, . . .,/n, under the constraint condition (7). Again method, the path planning problem must be reformulated
for convenience, let according to the procedure outlined in [16]. A FORTRAN
program has been written and numerical problems have been
C}= (Pi-Pi-'XPT 1 -pir2) + (P,y-p'y-1M~1 -^^ executed with different initial feasible solutions. When the
present feasibility tolerance T 2 , as defined in [11], is suf-
+(Pi-p / r I Kp / r'-p / r 2 ) (29) ficiently small, MAP program terminates at different feasible
Then (26) or (28) together with the constraint (7), becomes: solutions after more than thirty iterations for different
initially chosen, feasible points. A close examination shows
V7?Z>?+, + 7?+,Z>? -2T,Tl+1Cf+l slK.nTtT^ (30) that these different feasible solutions are merely different
points on the boundary of the constraint set which are not
withDjDi+i >C? + 1 for/ = 1,2, . . . , m - l , a n d optimum points. Consequently the computed optimum
solution is false.
(i = 0:) As2ifoT0r„ (31)
To understand the cause of the problem, observe that the
(»' = «:) Dm<2KarmTm. (32) inequality constraints form a nonconvex constraint set, as
shown in Appendix, with no upper bounds on r , ' s and Ti+]'s.
The constraints on the angular velocity, (17), and angular Since the performance index ET is linear, the optimum
acceleration, (18), can be written as feasible solution must be on the boundary of the set. The
hyperplane tangent to the set at the optimum point must be
l l w ' l l s ^ u f o r / = l , 2 , . . .,m, (33) parallel to the isocontours of ET. For illustration, consider
and Fig. 4 which is essentially a reproduction of Fig. 7(a) of the

Journal of Dynamic Systems, Measurement, and Control JUNE 1981, Vol. 102/145

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 Appendix. It represents the projection of the constraint set
onto the (Th r,)-plane for a fixed value of Ti+l. The boundary
consists of both straight lines and curves. It is seen that if the
projection of the optimum feasible solution is on the curved
section of the boundary, it must be on the darkened section as
shown in Fig. 4. The reason is that if the projection of the
optimum solution, (7'/,f,-), were on the undarkened section of
the curved boundary, then the tangent line, P~, at that point
would intersect the boundary at another point, (T,, ?,), of the
boundary. By holding the values of all other variables con-
stant, the values of ET at these two points are the same. Since
Tj has no upper bound, then there would be a neighboring
boundary point of either (7s,,77) or (T^f,), at which the value
of ET would be lower. Thus CT,,f,) cannot be the projection
of an optimum solution.
Now let (77, rf) be the projection of the optimum solution
on the darkened section of the boundary as shown in Fig. 4,
and Pt be the tangent line at (77, rf). Let (7?, ij) be the
projection of the feasible solution from the kth iteration, and
r> be the linear approximation of the boundary which is
described by the first two terms of Taylor series expansion
Fig. 3 Change of orientation
/Q-
;
<Ti
yP/
/y/
p., \ \
(
\, V Ti'' sf <V *i>
\ 1 p.
1
Fig. 4 Geometrical interpretation of iterations for MAP
146/Vol. 102, JUNE 1981
Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms
about (7*, Tf). The rectangle surrounding the point is
specified by the distance bound &kv, which is required by MAP
to restrict the size of evolution of consecutively iterated
solutions. For the Ar+ 1st iteration, the projection of the
constraint set in (7",, T,)-plane is indicated by the shaded area
in Fig. 4. Let Q be a corner point of the set which represents
the shortest distance from the set to Pt. Since, as seen in Fig.
4, Q is feasible, then it is the projection of the feasible
solution from the k + 1st iteration, i.e., Q = (7t + 1 , T* + 1 ).
Since Q is close to the boundary, then for the £ + 2nd
iteration, the rectangle surrounding Q must be very small
(Fig. 4) so that (Tf+2, rf +2 ) will be feasible. Consequently the
distance between (Tf+l, rf +1 ) and (Tf+2, T? +2 ) is small. If the
distance between two consecutively iterated feasible solutions
is sufficiently small, then the difference between the values of
ET evaluated at these two solutions is insignificant. As a
result, the computational process of MAP is terminated and
the feasible solution from the k + 2nd iteration is designated
as the optimum solution, which is obviously false.
VI Modification of MAP Algorithm
As explained in the preceding section, the false solution
may take place if the iterated feasible solution is close to the
curved section of the boundary of the nonconvex constraint
set, along which the optimum solution lies. Thus a logical way
to overcome the obstacle is to move the feasible solution of
each iteration away from that section of boundary.
Observe that the linear performance index ET of (21) is
independent of T, for ( = 1,2, . . .,m-1. Therefore, as
depicted in Fig. 4, for every / and for every iteration k, there is
a line Lf along which r, may assume any values without
alternating the value of ET evaluated at that iteration. For
convenience call it the "line of indifference." Now extract
conditions from (19) and (20) to form:
T?=min(Tf/K, Tf+l/K),i=l,2 m-1. (41)
This is a straight-line section of the boundary of the constraint
set. It intersects Lf+' at (Tf+[, T?) (see Fig. 4). Since (Tf+1,
T/") is in the feasible region, it may replace the original
/ k
y_ . x. = mm Crf/K, t f + ,/K)
(T +
i ' • T!> /
_ L. , line of Ind i f ference
(Tf + ', z\* ')
F
'k + 1
f
k
i' «/f^Z • J
* / neignboring j
boundary 1
po i n t ,* 2
k + 2,
Transactions of the AS ME
 feasible solution (7"f+1, rf + 1 ) of the Ar-H 1st iteration. Such a
replacement does not change the value of ET of that iteration.
However the iterated feasible solution is moved away from
the curved section of the boundary to the straight-line section,
and hence the problem of reaching a false optimum solution is
avoided.
In MAP, a complete relinearization of the problem takes
place at each iteration. No link exists between subsequent
linear programming subproblems. Thus the feasible solution
from any iteration, can be viewed as an initially chosen
starting point for the next iteration. Now, in addition to (41),
inequalities (19) and (20) also allow the conditions TQ = T\lK
and Tk„ = Tk„/K. Thus, to move away from the curved section
of the boundary to the straight-line section, the starting point
for the A:+ 1st iteration is chosen to be (rf + 1 , T/1") for
; = 0,1 m.
VII Direct Approximate Programming Algorithm
(DAPA)
Although the constraint set for the path planning problem
is nonconvex, its geometrical property together with the linear
performance index restrict the optimum solution lying on
certain sections of the boundary as shown in Section V. By
taking the advantage of this property, a large-step gradient
technique [12] based on the cutting plane method [11] is at-
tempted to shorten the computing time. As a result, a direct
approximate programming algorithm (DAPA) is developed.
It differs from MAP in that the requirement of distance
bound 5* in MAP is omitted in DAPA. Thus the solution
from each iteration is allowed to be infeasible. The soluton is
then made feasible by contracting the distance between two
solutions from consecutive iterations along the direction
between them, and hence no iterative computations are
wasted on infeasible solutions. Consequently the com-
putational time for solving the path planning problem is
reduced.
To develop the computational scheme for DAPA,
inequalities (30) through (32) and (38) through (40) must be
linearized by a straightforward differentiation. The
linearized inequalities become, respectively,
m)2D2i+, + (7* + , )2Df - 2 7 / Tki+, C)+, - A{Karf Tf Tki+, ) 2 ] Let t* be the feasible solution of the path planning problem
+ [2T?D>i+l -2Tki+lC2i+i -MCArt^T?(T*,+ i)2]lT,-T,k] linear approximation of all nonlinear inequality constraints
+ [2Tki+]D?-2T,kC2i+i-8K2a(T?)HTh2Tki+i]
2 by using the method of linear programming. If t is feasible,
X[Ti+i - n + i l - I ^ ^ t r , * ) ^ ^ , ) ] ^ , - ^ ] ^ (42) then t* +1 = i is the feasible solution from k+lst iteration.
A -2KaT0kTk-2KaT0k[Tl - Tk]-2KaTnT0 - r0k] < 0 (43) for its feasibility. The testing process is applied repeatedly.
D,„ -2KaTk„Tkm -2KaTk„[Tm - 7*J -2K a T k „[r m - r k m \ < 0
(44)
[(Tf)262i+l +(Tki+i)29?-2TikTki+iR2,+ i -HKaT?T?1*,+ tf\
+ [2Tik62i+l -2Tki+lR2i+l -8n(rA)2^(H+1)2]
k k
X [T, -T, ] + [2T i+, 6) -2T?R i+, 2
-8ff*„ k 2
<r, ) (.T[) T
, r 2 2
x[Ti+] - n + 1 ] - [ 8 n T M 7 / ) ( n + , ) ] h - r A ] £ 0
10, I -2KaTQkTk -2KaT$[T, - Tk]-2KaTnr0 - r0k] < 0 (46)
and
Wm \-2Kark„Tkm -2KaTk„[Tm - 7*J -2KaTkm[rm
for / = 1,2 An —1.
Journal of Dynamic Systems, Measurement, and Control
Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms
Let X be a feasibility adjustment factor, 0 < X < 1. Then
the computational scheme for DAPA is as follows:
A. Input parameters are: K, Kv, Ka, K„, ka, number of
segments m, hand matrices H' for » = 0,1, . . .,m and
feasibility adjustment factor X for 0 < X < 1.
B. Computed parameters are: vectors p1, scalars Dh Cf, 6,,
and Rf f o r / = 1,2, . . .,m andy' = 2,3, . . .,m.
C. Output variables are 7", and T, for z'=l,2, . . .,m and
7 = 0,1, . . .,m.
StepO: SetA:=0.
Step 1: Choose a set of initial, feasible solution T\ and T° for
z'=l,2, . . ,,m, andy' = 0,l m.
Step 2: Set T, = 7? for #=1,2 m\ r 0 = T§; rm = T°„ ;
and compute ET of (21).
Step 3: Apply the method of linear programming to obtain a
new solution set of Tt and TJ for ( = 1,2, . . .,m and
j = 0, . . .,m by minimizing ET of (21) subject to
inequality constraints (19), (20), (25), (35), and (42)
through (47).
Step 4: Test whether the new set of T, and T, satisfies the
original constraints (19), (20), (25), (30) through (32),
(35), and (38) through (40). If not, set T, = [XT, +
(1 - X ) Tf], Tj = [X TJ = (1 - X ) if] and repeat Step
4. If yes, continue.
Step 5: Compute the new value of ET of (21) and test whether
the improvement on the value of ET is significant. If
yes,set7|+i = 7,and7J r + 1 = ry for /=1,2, . . .,m,
andy' = 0,l, . . .,m. Then set k = k+\ and return to
Step 3. If not, continue.
Step 6: Stop. The computer new set of T, and Tj for
i=\,2,...,m, amd j = 0,...,m, is designated as the
optimum feasible solution.
VIII Convergence of Direct Approximate Pro-
gramming Algorithm (DAPA)
Let t be a (2m + 1) dimensional vector defined by
t=[TxT2 . . . TmT0Tx . . . Tm}' (48)
Thus
ti = Tiandtm+j+l=TJ;i=l,2, . . .,m;j = 0,l, . . .,m. (49)
from kth iteration. For k+lst iteration, DAPA requires the
about t*; and then obtains a solution 1 that minimizes the
linear performance index ET over the linearized constraint set
Otherwise contract t to [Xf = (1 -X)t*], 0 < X < 1, and test
Whenever the feasibility requirement is satisfied, the resulting
vector is called t* +1 . For convenience, let ET (t) be the value
of the linear performance evaluated at t. Because of the
nature of DAPA as described above, it is obvious that
ET(t*) > ET(t* + ') >ET(t). (50)
By applying the same reason to all the preceding iterations,
2 k
one obtains
i+l ]
ET(t°)>ET(t')> . • .>ET(t*)>ET(t A : + 1 ) (51)
(45) where t° is the initially chosen, feasible solution.
Let t* be the optimum feasible solution such that ET(t*) =
min ET, eA (t), where A is the nonconvex constraint set
described in the Appendix. If t* lies on the originally linear
- T * J <0 sections of the boundary of A, then t* will be determined
whenever the linear programming routine is applied during
(47) the first iteration.
Now, supposing t* lies on the nonlinear sections of the
JUNE1981,Vol.102/147
 r , linear approximation
of the curved boundary

Fig. 5 Geometrical interpretation of iterations for DAPA

boundary of A, as depicted in Fig. 5. Let r/ be the unit inward secutively iterated solutions is almost tangent to the
gradient vector of, and Pt the hyperplane tangent to, the boundary. If the curvature of the boundary in that neigh-
boundary of A at t*. Then for every f ePtl. borhood is small, then the contraction of the iterated solution
ET(t")=ET(t*) (52) may be repeated many times to satisfy the feasibility test.
Consequently the distance between two feasible solutions
According to the discussions in Section V, t* may lie only on from two consecutive iterations is small. As a result, the
the darkened section of the curved boundary (Fig. 5). Thus Pt computational process may terminate with a relatively large
is a supporting hyperplane, i.e. A is at one side of Pt only, value for the performance index ET. To avoid this problem,
and A n P ( = t*. Therefore (51) becomes the algorithm is modified to include the use of the line of
ET(t°)>ET(t')> . . . >ET(t*)>ET(t* + 1 )^ET(t*) (53) indifference which was introduced in Section VI. This
modification requires a change in Step 5 only as follows:
It is seen, then, the distance from Pt to t* decreases
monotonically as k increases. Since, by (53), t* ^ t * + l and Step 5' Compute the new value of ET of (21) and test
(t*+i _ (*) cannot be normal to r/. Then the projection of whether the improvement on the value of ET is
(t*+i _ t*) onto i/is significant. If yes, set Tf+i = Th T§ + 1 = Tx/K,
0 > ( t * + l - t * ) ' 17 ' m = Tm/K, and *jjt+i mm(Tj/K, Tj+l/K) for
(54)
;=1,2, . . .,m, and y'=l,2, . . . , / w - l . Then set
where ( ) ' = transpose of ( ). Since (t* +l - t*) = (t* + 1 k = k +1 and return to Step 3. If not, continue.
- t*) - (t* - t*), then (54) implies
(t*-t*)')/>(t A r + 1 - t * ) ' r, (55) X An Illustrative Example
+I
Butt* eA, t* = A PIP,,. Thus Program in FORTRAN language have been writtten for
(t *+i _ t * ) ' , > o
solving the minimum traveling time problem of path planning
(56)
+1
by all three algorithms discussed in preceeding sections. The
and the equality holds only when t* t* If the same programs are applied to the following numerical example for
reason is applied to all the iterations, then one obtains. comparison. To implement the programs, the constant K in
the path error constraints, (19) and (20), must be reasonably
( t ° - t * ) ' » , > ( t l -t*)'ti> . . . >(tk~t*)'v assigned. From Fig. 2, it is seen that rj+] + r,- < Tt which can
> . . . >(tr-t*)'i,>0 (57) be satisfied by the choice of T, < r , / 2 and T,- < TJ+l/2 for i
for some integer r > k+l, and the equality in (57) holds only = 1,2, . . .,/nandy = 0,1, . . .,m — 1. By a comparison with
when tr = t*. Therefore the iterated solutions in DAPA (19) and (20), one conlcudes that for practical purpose, K >
converge for the path planning problem. 2. For this example, K = 4 is chosen. This value must be
increased if the simulation of the movement of the mechanical
IX D A P A with Reset on r's manipulator indicates an unsatisfactory path error.
DAPA has been written in FORTRAN for the path Consider a traveled path that consists of five straight-line
planning problem, and numerical examples have been segments, m = 5. This problem consists of eleven variables
executed with different initial feasible solutions. It is observed and forty-two inequality constraints. The path is specified by
that the iterated feasible solutions drift towards the boundary six hand matices H', / = 0,1, . . .,5. For simplicity, the
of the constraint set so that the direction between two con- orientation submatrices 0' in H' have already been converted

148/Vol. 102, JUNE 1981 Transactions of the ASME

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 into parameters p' and 0,-. The given input data are: K = 4, K„ being reset for each iteration needs approximately one-half of
= 40cm/s,,K:a = 20cm/s2,A:a) = 45deg/s,A:a = 15Vs2and the computing time on CDC, but the computed value of

Is)
i = 0 1 3 4 5 unit
P\ = 0 30 30 40 40 -15 cm
P\ = -40 -40 -20 5 65 30 cm
P\ = 30 30 20 4 4 15 cm
P\ = - 1 0 0 0 0.5547 cm
P'y = - 0 1 1 1 0.8321 cm
P\ = - 0 0 0 0 0 cm
= - 90 30 60 0 54.038 degrees
o,
The given initial feasible solution and the given bounds on the optimum ET is 3.1 percent higher than that by the modified
movement of solutions for each iteration for MAP are listed MAP.
together with the iterated solutions in Table 1, with the
distance bound 5* halved whenever the &th iterated solution is XI Conclusion
infeasible. It has been shown that by introducing the "lines of in-
For DAPA schemes, the same input data are used except difference," MAP may be modified to solve the optimum
that X = 2/3 and no bounds on the movement of solutions for path planning problem. In this problem, the physical con-
each iteration are needed. The criterion for terminating the straints on velocities, accelerations, and the error constraints
computations in all the schemes is that whenever the im- on the traveling path form a nonconvex constraint set. It has
provement on the performance index ET over the preceding also been shown, however, that the optimum feasible solution
iteration is no more than 1 percent of the difference between is restricted on certain sections of the boundaries of the set.
the values of ET of the very first two iterations that produce By taking advantage of this restriction, a direct approximate
feasible solutions. The computed results by using DAPA with programming algorithm (DAPA) has been developed and
no reset and with reset on r's are listed in, respectively, Tables shown to be convergent. Because every iteration results in a
2 and 3. feasible solution, DAPA requires less computing time on a
Table 4 shows a comparison along the three schemes based digital computer than the modified MAP does.
on the same numerical example. As far as this example is
concerned, the modified MAP yields a smallest value of
optimum ET but requires a longer time on CDC 6500 to References
compute the results. However, DAPA with the values of r's 1 Luh, J. Y. S., Walker, M. W., and Paul, R. P. C , "On-Line Corn-

Table 1 An illustrative example using modified MAP

* i 11 12
\ 0 3 4 5 6 7 8 9 10

8.0 8.n 8.(1 8.0 8.0 4.0 2.0 1.0 0.5 0.5 0.25 U.25
*1 2 1 I 1 1 4.0 2.0 1.0 i.n 1.0 0.5 0.5
(sec) 3 1 I 1 1 8.0 8.n 8.0 8.0 8.0
a.o
T.( + ) 5 t t t t t • t t t t
1.0 1.0 1.0 1.0 1.0 0.5 0.25 0.125 UJ1625 0.0625 0.03125 0.03125
1 1 I 1 1 0.5 0.25 0-125 0.125 0.125 (I.H625 0.0625
(sec) 2 [ I 1 1 1.0 1.0 1.0 1.0 1.0 1.0 1.0
for
..<•)
3 1 I ( I I I I 1 1 I
5 t V t
* t
* t t
* •
3.0 8.0 8.0 8.0 8.1) 4.0 2.0 1.0 1.0 1.0 0.5 0.5
*1
1 8.0 8.0
(II
2 B.n 8.0 8.0
(sec) 1 1 8.0 8.0
for
3
i 1 1 I 1 1 1 I
4 1 1 I
T.(-) 5
0
1
t
1.0
t
1.0
1
V
1.0 1.0
*
1.0
t
I
0.5
1
t
0.25
1
*
IT. 125 0.0625 U. 1)625
t t

«5 2
I t
I
1
1
I
I
0.5
1.0
0.25 0.125
1.0
0.125
1.0
u.125
1.0
0.0625
1.0
0.0625
1.0
(sec) I 1.0
for I I 1 1 1 1 I I I
4 I I 1 1 1 1 I I I
5 t t • t t V t t t
1 8.0 5.H41 4.352 3.880 3.631 4.696 4.696 4.305 3.805 3.562 3.619 3.557 3.557
2 1 5.041 4.352 3.880 3.631 2.683 2.683 2.986 3.373 3.562 3.506 3.557 3.557
(sec) 3 1 5.011 4.210 3. .77 i. 124 2.886 2.886 2.886 2.886 2.886 2.331 2.828
1 5.031 4.306 3.780 3.463 3.339 3.339 3.339 5.339 3.339 3.322 3.322 3.322
S * 5.031 4.306 3.780 3.463 3.339 3.339 3.339 3.339 3.339 3.322 3.322 3.322

1.0 0.745*1.260 0.76?-1.0B8 0.807-0.970 0.835*0.908 0.560 0.560 0.658 0.783 0.845-0.890 0.828 0.859-0.889 0.858
1 1 1.260 1.088 0.97U 0.908 0.671 0.671 0.747 0.843 0.890 0.R76 0-«89 0.889
2 1 1.2*46*1.253 1.053 0.823*0.894 0-781 0.671 0.671 U.722 0.722 U.722 0.708 0.708 0.707
*i 0.707
3 1 1.253 1.053 0.894 0.781 0.722 0.722 0.722 0.722 0.722 0.708 U.708
Uec) 4 1 1.258 1.076 0.945 0.866 O.R35 0.835 0.835 0.835 U.335 0.831 U.831 0.830
0.540-1.076 0.550*0.945 0.556*0.866 0.552 0.552 0.552 0.552 0.552*0.835 0.544 0.544*0.331 U.543
5 • 0.597*1.258

ET
Q)
42 26.495 22.832 20.254 18.703
" " ' " 18.085
" 17.991 17.985

SMutic
of Gwen
n i t i a l Feasible-Reset Feasit>Le*Reset feasible-Reset „..,„.-.« FeasioLe*Reset Feasible-Reset
Optimum
Feasible

fT. - T J , whenever T. - T* > Q

i I 0, otherwise.

10, whenever T. - T. :> 0

V-'M k
^-[Tj - T":, otherwise.

*.<+) and T.(-) are similarly defined.

Journal of Dynamic Systems, Measurement, and Control JUNE1981,Vol. 102/149

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 Table 2 An illustrative example using DAPA with no reset on T'S

X 0 1 2 3 4 5 6 7 8 9

Tk 1 8.0 5.041 4.352 3.880-4.142 3.757-3.971 3.671-3.911 3.644+3.-858 3.623+3.849 3.619-3.843 3.617-3.839
2 1 5.041 4.352 3.880+4.142 3.757+3.971 3.671+3.911 3.644+3.858 3.623+3.849 3.619+3.843 3.617+3.839
(sec) 1 1 5.011 4.210 3.577*3.928 3.367-3.678 3.192+3.582 3.129+3.493 3.074+3.476 3.064*3.465 3.058+3.458
4 1 5.031 4.306 3.780+4.072 3.628+3.874 3.512+3.803 3-474+3.738 3.442-3.726 3.436*3.719 3.433+5.714
5 t 5.031 4.306 3.780-4.072 3.628+3.874 3.512+3.803 3.474+3.738 3-442+3.726 3.436-3.719 3.433*3.714

k
0 1.0 0.745 0.A97 0.765+0.727 0.792+0.756 0.813*0.767 0.819-0.778 0.825*0.779 0.826*0.781 0.827-0.781
1 1 1.260 1.088 0.970+1.036 0.939+0.993 0.918+0.978 0.911*0.965 0.906-0.962 0.905*0.961 0.904+0.960
(sec) 2 1 1.253 1.010 0.894+0.959 0.739-0.861 0.798+0.849 0.641+0.808 0.609*0.800 0.603+0.795 0.599+0.791
3 1 1.253 1.053 0.894+0.982 0.842+0.920 0.798+0.896 0.782+0.873 0.768+0.R69 0.766+0.866 0.764-0.865
4 1 1.258 1.076 0.945+1.018 0.907+0.969 0.878+0.951 0.869*0.934 0.860-0.932 0.859+0.930 0.858+0.928
5 t 0.59? 0.444 0.473+0.457 0.493*0.473 0.510+0.480 0.516*0.487 0-571+11.488 0.522+0.489 0.522+0.490

ET = 42 26.495 22.666 - + 21.540 - + 20.598 - + 20.258 - + 19.950 - + 19.895 - + 19.859 - * 19.835

(sec)
Nature o f Given Feasible Feasible Contract Contract Contract Contract Contract Contract Optimum
Solution Initial Twice + Feasible Twice * Feasible 4 t i n e s * Feasible 4 times * F e a s i b l e 8 times - F e a s i b l e 9 t i n e s - F e a s i b l e 10 times * Feasible

Table 3 An illustrative example using DAPA with reset on T'S

0 1 2 3 4 5 6 7
\
Tk 1 8.0 5.041 4.352 3.880 3.631 4.696-4.105 3.517-3.843 3.545-3.784
1 2 5.041 4.352 3.880 3.631 2.683-3.209 3.517-3.346 3.545-3.385
(sec) 3 5.011 4.210 3.577 3.124 2.886-3.018 2.854-2.945 2.838-2.924
4 5.031 4.306 3.780 3.463 3.339-3.408 3.329-3.373 3.324-3.363
5 5.031 4.306 3.780 3.463 3.339-3.408 3.329-3.373 3.324-3.363
t
0 1.0 0.745-1.260 0.767-1.088 0.807-0.970 0.835-0.908 0.560-0.753-1.026 0.878-0.960-0.961 0.855-0.940
1 1.260 1.088 0.970 0.908 0.671-0.802 0.879-0.837 0.886-0.846
2 1.253 1.010-1.053 0.823-0.894 0.781 0.671-0.732-0.755 0.465-0.626-0.736 0.710-0.731
Z 1.253 1.053 0.894 0.781 0.722-0.755 0.713-0.736 0.710-0.731
4 1.258 1.076 0.945 0.866 U.835-0.852 0.832-0.843 0.831-0.841
5 Y 0.597*1.258 0.540-1.076 0.550-0.945 0.556-0.866 0.552-0.726-0.852 0.549-0.717-0.843 0.547-0.785

ET = 42 26.495 22.832 20.254 18.703 - - 18.627 - - 18.556 - - 18.543

(sec)
Nature of Giuen Contract Contract Contract Optimum
Solution Initial Feasibte*Reset Feasible-Reset Feasible-Reset Feasible-Reset Twice + Feasible - Reset Twice + Feasible + Reset 4 times + Feasible

Table 4 Comparison of algorithms based on an example 12 Zoutendijk, G., "Nonlinear Programming: A Numerical Survey," J.
SIAMon Control, Vol. 4, No. 1, 1966, pp. 194-210.
13 Markiewicz, B. R., "Analysis of the Computed Torque Drive Method
Algorithm Number o f Optimum Computing Time on
Iterations Value o f ET CDC 6500 w i t h FORTRAh and Comparison with Conventional Position Servo for Computer-Controlled
Manipulator," Technical Memorandum 33-601, Jet Propulsion Laboratory,
Mar. 1973.
M o d i f i e d MAP 12 17.985 s e c . 9.925 s e c .
14 Goldstein, H., Classical Mechanics, Addison-Wesley, 1959, pp. 14-22.
15 Paul, R., "The Mathematics of Computer Controlled Manipulators,"
DAPA U i t h o u t Proceedings of the 1977 Joint Automatic Control Conference, 77CH-1220-
Reset on T ' S 9 19.835 s e c . 6.554 sec.
3CS, Vol. l , p p . 124-131.
16 Cooper, L., and Steinberg, D., Introduction to Methods of Optimization,
DAPA With W.B.Saunders Co., 1970, pp. 311-318.
Reset on t ' s 7 18.543 s e c . 5.114 s e c .

APPENDIX
putational Scheme for Mechanical Manipulators," ASME JOURNAL OF
DYNAMIC SYSTEMS, MEASOREMENT, AND CONTROL, Vol. 102, No. 2, June 1980, Inequality Constraints for Path Planning Problem
pp.69-76.
2 Luh, J. Y. S., Walker, M. W., and Paul, R. P. C , "Resolved- The (8/M+2) inequality constraints for the path planning
Acceleration Control of Mechanical Manipulators," IEEE Transactions on problem may be divided into four groups:
Automatic Control, Vol. 25, No. 3, June 1980, pp. 468-474. (a) 0 < a,' < T,. This group consists of (25) and (35) which
3 Taylor, R. H., "Planning and Execution of Straight-Line Manipulator
Trajectories," Research Report RC 6657 (# 28633), IBM Thomas J. Watson
specifies the positive bounds for Tt.
Research Center, July 1977. (b) 0 < b,' T; < T,, 0 < b,'. This group consists of (19)
4 Paul, R., "Manipulator Cartesian Path Control," IEEE Transactions on and (20) which has a straight line in the (T,, T,) plane as a
Systems, Man, and Cybernetics, Vol. SMC-9, No. 11, Nov. 1979, pp. 702-711. boundary.
5 Snyder, W. W., "Microcomputer Based Path Control," Robotics Age,
Vol. 2, No. 1, Spring 1980, pp. 6-15.
(c) 0 < d< TjT,. This group consists of (31), (32), (39), and
6 Luh, J. Y. S., Walker, M. W., "Minimum-time Along the Path for a (40). Since 0 < T, and 0 < Th the boundary d = T,r, repre-
Mechanical Arm," Proceedings of the 1977 IEEE Conference on Decision and sents a hyperbola in the first quadrant whose asymptotes are
Control, Vol. 1, 77CH1269-OCS, Dec. 7-9, 1977, New Orleans, pp. 755-759. T,-and T,-axis. The set ((r l ,7 , / ) 1777} > d] is the collection of
7 Paul, R., "Modeling Trajectory Calculations and Serving of a Computer all the elements on the hyperbola and the inside the cup.
Controlled A r m , " Memo AIM-177, Stanford Artificial Intelligence
Laboratory, Stanford University, Nov. 1972. (d)0> Va7? + bTf+i - cTiTi+] < T,TiTi+i with 0 < c2
8 Finkel, R. A., "Constructing and Debugging Manipulator Programs," < 4ab. This group consists of (30) and (38). For convenience,
Memo AIM-284, ibid, Aug. 1976.
9 Lin, C. S., and Luh, J. Y. S., "Control and Computation for Mechanical
the analysis is divided into three parts:
Manipulators," Technical Report TR EE 80-25, School of Electrical (i) Constraint set at every fixed level of Ti+l, i.e. Ti+l = N
Engineering, Purdue University, July 1980. >0.
10 Griffith, R. E., and Stewart, R. A., "Nonliner Programming Technique
for the Optimization of Continuous Processing Systems," Management
Science, Vol. 7,1961, pp. 379-392.
^aTf+bW-cNTi < /W,- 7, (A.l)
11 Jacoby, S. L. S., Kowalik, J. S., Pizzo, J. T „ Iterative Methods for
Nonlinear Optimization Problems, Prentice-Hall, 1972, pp. 201-203. with c 2 < Aab. The equality of (A. 1) can be reduced to

150/Vol. 102, JUNE 1981 Transactions of the ASME

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms

 aT, + bN - cNT. - NT.T.

(a) Projection on (T|,T|)-plane with T| + i = N f i x e d

Fig. 6(a) Plot for V a 7 f +bN2 - c N T , = N T / T ,

(b) Projection on (T|, T, + 1 ) • plane with TI = M fixed

Fig. 7 Constraint set for path planning problem

^a1J+bn+i-cT,TM *MT,Tn (A.3)

with c2 < Aab. Since Tt > 0 and Ti+i > 0, the equality of
(A.3) can be reduced to:
(i/r/)2 (l/77 + ,) 2 = 1
+ (A.4)
Fig. 6(i>) Plot for Va7? + t r f + 1 - c T , r / + 1 =M7,-7 / + ,
(M*/A) {Afi/B)
where (1/77, l / 7 / + 1 ) represents the rotation of (1/7,, l / 7 , + l )
with an angle a about its origin, a = 1 / 2 tan _ ' ( - c/b -a); A
4a6 - c2 ' = fecos2a - ccos a sin a + osin 2 a; andB = 6 sin2 a — c
-^)-(r4)7(w)=""» sin a cos a + a cos 2 a. The plot of (A.4) is shown in Fig. 6(b),
2
1/(2 Va) and Va/c > l/(2Vfc).
since 7, + , = N > Oand 7,- > 0. Figure 6(a) shows a plot of since c < 4ab implies VZ>/c
For 7, > 0 and T • 0, the set {(M, T„ 7, + 1 )
(A.2). For r, > 0 and T, > 0, the set l+i
IVa7 2 +fe7? + 1 -c7,7,. MT,Ti+i, c2 < Aab] is the
( ( T ; , 7,- ,A0 MaTf+brf-cNT, 2
< N T , 7,-, c < 4afe) collection of all the elements on the boundary and inside the
cup in the first quadrant. This set is also nonconvex.
is the collection of all the elements on the boundary and inside By summarizing the characteristics of the inequality
the cup in the first quadrant. This set is nonconvex. constraints, the constraint set for the path planning problem
(ii) Constraint set at every fixed level of 7,-, i.e., T,• = L > can be represented by plots shown in Fig. 7 where the
0. Because of the symmetry between 7, and 7 , + l in the feasible regions are indicated by shaded areas. It is seen that
inequality constraint group (d), the constraint set has the the constraint set is nonconvex. For convenience, call the
exactly same form as that in Part (i). constraint set A. The variables T, 's are bounded above by 7,'s
(iii) Constraint set at every fixed level of T,-, i.e. r,- = M' > and Ti+i's while the latter two have no upper bounds. They,
however, approach infinity asymptotically.
0.

Journal of Dynamic Systems, Measurement, and Control JUNE1981, Vol. 102/151

Downloaded From: http://dynamicsystems.asmedigitalcollection.asme.org/ on 04/17/2015 Terms of Use: http://asme.org/terms