- Email: [email protected]
Robot trajectory planning with semi-infinite programming
European Journal of Operational Research 153 (2004) 607–617 www.elsevier.com/locate/dsw
Robot trajectory planning with semi-infinite programming A. Ismael F. Vaz
a,*
, Edite M.G.P. Fernandes a, M. Paula S.F. Gomes
b
a
b
Departamento de Producßa~o e Sistemas, Escola de Engenharia, Universidade do Minho, Campus de Gualtar, Braga 4710-057, Portugal Department of Mechanical Engineering, Mechatronics in Medicine Laboratory, Imperial College of Science, Technology and Medicine, London SW7 2BX, UK
Abstract In this paper we describe how robot trajectory planning can be formulated as a semi-infinite programming (SIP) problem. The formulation as a SIP problem allowed us to treat the problem with one of the three main classes of methods for solving SIP, the discretization class. Two of the robotics trajectory planning problems formulated were coded in the SIPAMPL environment which is publicly available. A B-Spline library was also created to allow the codification of the robotics trajectory problem. 2003 Elsevier B.V. All rights reserved. Keywords: Nonlinear programming; Robot trajectory planning; Semi-infinite programming; Discretization method; SIPAMPL database
1. Introduction Some engineering problems, like air pollution control [11], optimal signal sets design [6,14,27], production planning [15,28], Chebyshev approximation theory [9,10,20] and robot trajectory planning [7,16,23], can be formulated as semi-infinite programming problems. In this paper we will describe how two robot trajectory planning problems can be mathematically formulated as semiinfinite programming problems, coded in AMPL using a B-Spline library and efficiently solved by a
*
Corresponding author. E-mail addresses: [email protected] (A.I.F. Vaz), [email protected] (E.M.G.P. Fernandes), [email protected] (M.P.S.F. Gomes). URL: http://www.eng.uminho.pt/~dps/aivaz/.
discretization method. These formulations follow the ideas in [7,8,16,23]. In [21] an algorithm to minimize the trajectoryÕs time is proposed to solve a parametrization problem with a theoretical control method. In [4] the analysis of the optimal problem is done by the PontryaginÕs principle [18]. Both [4] and [24] use the bang–bang principle (see, for example, [13]) and numerical examples of dynamic equations for several robots are presented. In spite of the problem formulations in [8,23] being identical to the ones described herein, the problems were solved by a direct collocation method [22]. For the robot models see [17]. In [12] conditions for the weight joint torque matrices in the weight joint optimization problem are found. The found conditions guarantee that the local optimization of weighted joint torque with these matrices will have the same stability and
0377-2217/$ - see front matter 2003 Elsevier B.V. All rights reserved. doi:10.1016/S0377-2217(03)00266-2
608
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
optimality conditions as the local optimization of the weighted joint torque with the inertia inverse matrix. We start in Section 2 by introducing the reader to semi-infinite programming and to the main approaches used to solve semi-infinite programming problems. In Section 3 a brief introduction on trajectory definition is made. Section 4 will be devoted to presenting a robotics problem formulated as semi-infinite programming problem. In Section 5, the codified problems for which we present some numerical results are shown. The implemented algorithms and the numerical results obtained with the coded problems are shown in Section 6. Some conclusions are presented in the last section.
2. Semi-infinite programming A SIP problem is a mathematical program of the form minn x2R
f ðxÞ
s:t: ci ðx; tÞ 6 0;
i ¼ 1; . . . ; m;
set is a grid. The algorithm calculates a sequence of finite grids T0 T1 TN T . Finite nonlinear sub-problems are solved in each grid with a selected number of points. Let Tei Ti , i ¼ 0; . . . ; N, be such sets. The various discretization methods correspond to the various ways of selecting the points in Tei (for some examples see [9,10,16,19,20,25]). Exchange methods consist of computing approximations to the solutions of the problem max ci ðx; tÞ; t2T
i ¼ 1; . . . ; m;
ð2Þ
for a given x 2 Rn . Some of these approximations and other points define a finite set Te T and a finite minimization problem is solved on Te . The corresponding solution is set to x and the process is repeated with a new finite Te set, where some points could be added to and others dropped out from the previous set (exchange of points in the Te set). Reduction type methods calculate all the global (and some local) maxima to the problem (2). The infinite set is then replaced by a finite one that consists of the maxima found.
ð1Þ
8t 2 T ; where f ðxÞ is the objective function and ci ðx; tÞ, i ¼ 1; . . . ; m, are the infinite constraint functions. These problems are called semi-infinite programming problems due to the constraints ci ðx; tÞ 6 0, i ¼ 1; . . . ; m. We can think of T as an infinite index set and therefore (1) is a problem with finitely many variables over an infinite set of constraints. In fact, problem (1) could be described in a more general form (including equality and inequality constraints not depending on the variable t), but this formulation suits our purpose in this paper. During the last decades some efficient numerical methods have been proposed for solving SIP. The problems described in Section 4 were solved using a discretization method (see [11] for a review on SIP and numerical methods). There are three main classes of algorithms for SIP: discretization methods, exchange methods and reduction methods. A discretization method consists of replacing the infinite set T by a finite one. Usually the finite
3. Trajectory definition For a brief review of robotics mechanics and control we refer to [2]. A robot can be schematically represented as a connection of links. The robot represented in Fig. 1 has two links. Each link has a length and mass associated (d1 , d2 , m1 , m2 for the length and mass of the two links, respectively). Some mechanical devices provide movement to the links in some way. The ability that the robot has to position the links is called the degrees of freedom (d.o.f.) of the robot. In the case represented in Fig. 1 the robot has three d.o.f.: the ability to rotate around the y-axis, the ability to move link one and the ability to move link two. In robot joint space the specification of the values for the d.o.f. are enough to define the robot position in space. Since the robot position (d.o.f. values) varies we can define the path as a curve T
hðsÞ ¼ ½h1 ðsÞ; h2 ðsÞ; . . . ; hl ðsÞ ;
s 2 ½0; sf ;
ð3Þ
parametrized by s, where l is the number of d.o.f.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Y
m1
d2
assume that the given curve satisfies Eqs. (4)–(6) and without loss of generality that sf ¼ 1. Two optimization problems can be defined depending on the constraints used. The optimization problem results from minimizing a certain performance index (time, energy consumption, etc) while ensuring that the resulting trajectory can indeed be followed by the robot. We will look for a change of variables that relates the time t to the parameter s:
m2
θ3
d1 θ2
θ 1
X
t ¼ hðsÞ;
Fig. 1. Three degrees of freedom robot.
Natural constraints applied to the parametric curve are: 2 l X dhi > 0; s 2 ð0; sf Þ; ð4Þ ds i¼1 dh dh ð0Þ ¼ ðsf Þ ¼ 0 ds ds
ð5Þ
and 2
dh dh ð0Þ; ðsf Þ 6¼ 0: 2 ds ds2
ð6Þ
Eq. (4) means that at any time at least one joint in the trajectory must be in movement. Eq. (5) means that in the initial/final position the robot must be in rest (although any other value is acceptable) and Eq. (6) means that the robot in the initial/final position has to be in acceleration/deacceleration. Throughout the paper we will use the following notation for the derivatives: df ; f_ ¼ dt
f0 ¼
s 2 ½0; 1:
ð7Þ
This change of variables will be such that
Z
2
609
df : ds
4. Optimal parametrization of curves for robot trajectory design In this section the optimization of a given path in robot joint trajectory is described. Let us
• h0 ðsÞ > 0 for all s 2 ð0; 1Þ, meaning that h is strictly increasing; R1 • the total travel time tf :¼ hð1Þ ¼ 0 h0 ðsÞ ds is minimal; for Model 1 formulation, joint velocity, acceleration and jerk constraints are imposed: _ i ðtÞj 6 Ci;1 ; jH € i ðtÞj 6 Ci;2 ; jH ...
jHi ðtÞj 6 Ci;3 ; where Ci;1 , Ci;2 , Ci;3 are the constant bounds on velocity, acceleration and jerk on joint i, Hi ðtÞ :¼ hi ðh1 ðtÞÞ, i ¼ 1; . . . ; l and t 2 T :¼ ½0; tf . for a Model 2 formulation, torque constraints are imposed: j Fei ðtÞj 6 Ci ; where Fei ðtÞ :¼ F ðh1 ðtÞÞ and Ci are the joint torque and joint torque limit of the ith robot joint, respectively, i ¼ 1; . . . ; l and t 2 T :¼ ½0; tf . The described problems are in the generalized semi-infinite programming form, since the infinite index set depends on the control variables (the infinite set T depends on tf which is to be minimized). Using the fact that s ¼ h1 ðtÞ;
ð8Þ
we can define the problems in the standard semiinfinite programming form.
610
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 00 ðsÞÞ2 h0 ðsÞh000 ðsÞ
00
Model 1 can then be written as
...
ð9Þ
hð1Þ is minimum;
hi ðsÞ ¼
h ðsÞ 3ðh 00 0 h000 i ðsÞ 3hi ðsÞ h0 ðsÞ þ hi ðsÞ
0
hð0Þ ¼ 0;
ð10Þ
h0 ðsÞ > 0;
ð11Þ
jh_i ðsÞj 6 Ci;1 ;
ð12Þ
jh€i ðsÞj 6 Ci;2 ;
ð13Þ
...
jhi ðsÞj 6 Ci;3 ;
i ¼ 1; . . . ; l;
8s 2 ½0; 1:
ð14Þ
¼
where ui ðsÞ ¼
ð15Þ
The problem will be formulated in terms of the derivative of the unknown function h. Let gðsÞ ¼ h0 ðsÞ:
ð16Þ
The expression for the manipulatorÕs torques is 1 Fi ðsÞ ¼ Ji ni h€i ðsÞ þ Bi ni h_i ðsÞ þ ni þ
l X l X j¼1
l X
Iij ðhðsÞÞh€j ðsÞ
j¼1
!
Cijk ðhðsÞÞh_j ðsÞh_k ðsÞ þ di ðhðsÞÞ ;
2
00 ðsÞ
g3 ðsÞ
:
Using (18) and (19), Eq. (17) can then be rewritten as 0 1 0 0 h00i h0i gg A þ Bi ni hi Fi ðsÞ ¼ Ji ni @ g2 g 0 1 g0 1 @ ui v i g þ þ wi A; ð21Þ ni g2
jFi ðsÞj 6 Ci ;
8s 2 ½0; 1:
0
g ðsÞ 3ðg ðsÞÞ gðsÞg 00 0 h000 i ðsÞ 3hi ðsÞ gðsÞ þ hi ðsÞ g2 ðsÞ
ð20Þ
Model 2 is obtained by replacing Eqs. (12)–(14) with i ¼ 1; . . . ; l;
ðh0 ðsÞÞ2
3 ðh0 ðsÞÞ
9
Pl
00 > j¼1 Iij ðhðsÞÞhj ðsÞ > > P P = þ lj¼1 lk¼1 Cijk ðhðsÞÞh0j ðsÞh0k ðsÞ : Pl 0 > > > j¼1 Iij ðhðsÞÞhj ðsÞ ;
vi ðsÞ ¼ wi ðsÞ ¼ di ðhðsÞÞ
ð22Þ The Model 1 problem can then be rewritten in a SIP form as Z 1 minn gðsÞ ds x2R
s:t:
k¼1
0
gðsÞ > 0; jh_i ðsÞj 6 Ci;1 ;
ð17Þ
jh€i ðsÞj 6 Ci;2 ;
where for the ith robot joint: Ji ¼ motor inertia (Ji > 0, i ¼ 1; . . . ; l); ni ¼ gear ratio; Bi ¼ viscous damping coefficient (Bi > 0, i ¼ 1; . . . ; l); ðIij ðhðsÞÞÞi;j¼1;...;l ¼ inertia matrix (positive definite); ðCijk ðhðsÞÞÞi;j;k¼1;...;l ¼ Coriolis tensor; di ðhðsÞÞ ¼ gravitational torque. Using the chain rule, equalities (8) and (16) yield
jhi ðsÞj 6 Ci;3 ;
ð23Þ
...
d h0 ðsÞ h0i ðsÞ h_i ðsÞ ¼ fhi ðh1 ðtÞÞg ¼ i0 ; ¼ dt h ðsÞ gðsÞ 00
h€i ðsÞ ¼
ðsÞ h00i ðsÞ h0i ðsÞ hh0 ðsÞ
ðh0 ðsÞÞ2
ð18Þ 0
¼
ðsÞ h00i ðsÞ h0i ðsÞ ggðsÞ
g2 ðsÞ
;
ð19Þ
i ¼ 1; . . . ; l; 8s 2 ½0; 1; and Model 2 as Z 1 minn gðsÞ ds x2R
s:t:
0
gðsÞ > 0; jFi ðsÞj 6 Ci ;
ð24Þ i ¼ 1; . . . ; l;
8s 2 ½0; 1: For practical purposes, the gðsÞ function will be approximated by a B-spline ([1]). The objective function in Model 1 and Model 2 is then:
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Z
1
gðsÞ ds ¼
0
Z
1
Bk;n ðsÞ ds
ðsee 5:2Þ
Bk;n ðtÞ ¼
0
¼
Z
1
n X i¼1
¼
xi Bi;k;n ðtÞ;
ð25Þ
i¼1
0
¼
n X
611
n X
xi Bi;k;n ðsÞ ds
by ð25Þ
i¼1
Z
xi
1
Bi;k;n ðsÞ ds
0
n 1X xi ðniþk ni Þ: k i¼1
where Bi;k;n ðtÞ; for all i, are called the blending functions for the B-Spline. These blending functions are polynomials of degree k in one variable (t) and form a basis. xi are the coefficients of the BSpline and n is the knot vector. When using the B-Splines in a mathematical problem we may need to calculate the derivatives of the B-Splines (with respect to the t or xi variables). The formulae used are presented in the Appendix A.
5. Robotics problems coded in AMPL 5.3. The B-Splines library A library for AMPL to allow the codification of mathematical problems that use B-Splines is available and will be described in the following subsections. The B-Splines library is used to code the Model 1 and Model 2 optimal parametrization curve (SIP) problems that appear in robotics. In this section a brief introduction to AMPL and to B-splines will be given. We also describe the B-Splines library for AMPL and give an example on how this library is used for coding the robotics problems. In the Appendix A we show the BSplines formulae used in the library. 5.1. AMPL AMPL [3] is a modeling language that allows the codification of mathematical programming problems. AMPL is a descriptive language and does not allow the direct programming of functions (and no recursion is allowed). The automatic differentiation is only used to obtain the derivatives of the objective functions and constraints. Some problems in mathematical programming resort to function approximation by B-Splines [1]. The codification of such problems in AMPL can be very tedious if one needs to use several BSplines (degree of the B-Spline, number of coefficients, number of knots). 5.2. B-Splines A B-Spline is a linear combination of blending functions. A B-Spline can be represented by
AMPL has the ability to load a dynamic library. The B-Splines library and installation procedure are available from the internet 1 along with the SIPAMPL [24] package. This package contains a database with more than one hundred and thirty SIP problems coded in AMPL and an interface to any SIP solver (in particular to the NSIPS [26] solver). The library provides two functions bspline and dbspline. bspline is used to evaluate the B-Spline. dbspline is used to evaluate the BSpline derivatives with respect to the t parameter. The functions syntax is: bspline ðt; k; n; x1; x2; . . . ; xn; k1; k2; . . . ; knÞ; dbspline ðt; j; k; n; x1; x2; . . . ; xn; k1; k2; . . . ; knÞ; where t and x1; . . . ; xn are AMPL variables that correspond to the blending functions parameter and to the B-Spline coefficients respectively. k, n, k1; . . . ; kn are constants, where k is the B-Spline degree, n is the number of coefficients and k1; . . . ; kn are the knot points. j indicates the jderivative of the B-Spline with respect to t (oj B=otj ). bspline and dbspline return to AMPL the B-Spline value in t and, if requested, the first and second derivatives of the B-Spline with respect to the blending parameter and the coefficients. When
1
http://www.eng.uminho.pt/~dps/aivaz/.
612
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
using an external function the function AMPL command must be used, before using the function itself.
# to save some space we define a variable which is the B-Spline var g ¼ bspline(t,k,n,{i in 1. . .n} x[i],{i in 1. . .nk}knots[i]);
5.4. A robotics example Some problems in robotics can be formulated as a minimum time parametrization of a path in robot joint space [7,16]. This parametrization consists of finding a function that minimizes the total travel time of a robot for a given trajectory (in robot joint space). This function is usually approximated by a B-Spline. The problem can be equated in the following form (only one simple constraint is imposed, since the purpose is to illustrate how B-Splines can be used in AMPL, and not how to solve a real robotics problem. For a complete description see Section 4): Z 1 n 1X minn Bk;n ðtÞ dt ¼ xi ðniþk ni Þ x2R k i¼1 0 s:t: Bk;n ðtÞ P 0; 8t 2 ½0; 1; as a semi-infinite programming problem. Please note that the objective function does not depend on t. The constraint forces the B-Spline to be positive in the interval ½0; 1. A possible codification of this problem in AMPL is the following: # declare used external functions function bspline; #function dbspline; (not used in ex ample) # number of coefficients param n: ¼ 7; # degree of spline param k: ¼ 4; # number of knots param nk: ¼ 11; # knots vector param knots{1. . .nk}; # coefficients var x{1. . .n}; # blending parameter var t;
minimize obj: (1/k)*(sum {i in 1. . .n} (knots[i+k]-knots[i])));
(x[i]*
subject to tcons: g > ¼ 0; data; # add data to problem (knots) param knots : ¼ 1 0 2 0 3 0 4 0 5 0.25 6 0.5 7 0.75 8 1 9 1 10 1 11 1;
5.5. Coded SIP problems With the B-Splines library we were able to test three trajectories. The trajectories are those described by Haaren-Retagne in [7]. Example 1. A Model 1 formulation with the trajectory defined by h1 ðsÞ ¼ 1:5fðsÞ; h2 ðsÞ ¼ 0:5 sinð4:7fðsÞÞ; h3 ðsÞ ¼ 1:3fðsÞ þ 2:6; where fðsÞ ¼ s3 ð6s2 15s þ 10Þ. Example 2. A Model 2 formulation with the trajectory defined by
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 Table 3 Constants for Model 2 formulation
4 2 h1 ðsÞ ¼ pðfðsÞ 0:5Þ ; 3 p p h2 ðsÞ ¼ þ sinð2pfðsÞÞ; 2 4 p h3 ðsÞ ¼ fðsÞ; 2 where fðsÞ ¼ 2s3 þ 3s2 . Example 3. A Model 1 formulation where the robot coordinates are given through a spline representation with normalized B-splines of degree 6: hj ðsÞ ¼
11 X
aij Bi;6;n ðsÞ;
613
j ¼ 1; 2; 3;
i¼1
where the knots n are given by n1 ¼ ¼ n6 ¼ 0, n7 ¼ 0:1754653, n8 ¼ 0:3552259, n9 ¼ 0:5061700, n10 ¼ 0:6877756, n11 ¼ 0:8319530, n12 ¼ ¼ n17 ¼ 1. The coefficients aij are given by Table 1. The three different choices of derivatives bounds for Model 1 formulation are given in Table 2. The constants that define the robot for the Model 2 formulation are given in Table 3 and the
Table 1 Coefficients of the B-Splines robot coordinates i
j¼1
j¼2
j¼3
1 2 3 4 5 6 7 8 9 10 11
0.8353216 0.8353216 0.8353216 2.294404 1.506865 0.8491478 0.5452022 0.1477306 1.227333 1.227333 1.227333
)0.4971999 )0.4971999 )0.4971999 )1.289908 )0.2470876 0.1336621 0.7745534 )0.07365886 )0.4258067 )0.4258067 )0.4258067
2.803471 2.803471 2.803471 2.137097 1.881815 2.158783 1.821837 3.067521 2.340066 2.340066 2.340066
i
Ji
ni
Bi
Ci
1 2 3
1.98 104 1.98 104 1.98 104
62.5 62.5 62.5
1.744 103 1.744 103 1.744 103
2.090 0.523 1.045
manipulator masses and link lengths are m1 ¼ 5 kg, m2 ¼ 2 kg, d1 ¼ 1:5 m, d2 ¼ 0:5 m. For the sake of simplicity the dynamic equations for this robot are omitted, but they can be easily obtained from the coded problem. The combination of Examples 1–3 with the several constraints bounds gives seven robotics semi-infinite programming problems that were coded in AMPL. The problem characteristics are reported in Table 4. The problems were added to the SIPAMPL [24] database which can be obtained freely from the internet (see Section 5.3). In Table 4 ‘‘Problem’’ is the problem name (the filename has a .mod extension). The problems were solved with nine coefficients and one parameter in the B-Spline. The number of coefficients can be changed by introducing more knots to the B-Spline. ‘‘Model’’ indicates that the problem is a Model 1 (23) or Model 2 (24) formulation. ‘‘Bounds’’ indicates the different choices for the constraints bounds. The last column refers to the Table 4 Robotics problems coded in AMPL Problem
Model
Bounds
Terminology [7]
elke1 elke2 elke3 elke4 elke5 elke6 elke7
1 1 1 2 1 1 1
A B C – A B C
Ex1A Ex1B Ex1C Ex2 Ex3A Ex3B Ex3C
Table 2 Derivatives bounds for Model 1 formulation h1 A B C
h2
h3
C11
C12
C13
C21
C22
C23
C31
C32
C33
2 1 2
8 3 8
250 100 25
3 1 3
18 3 18
650 100 65
4 1 4
50 3 50
1000 100 100
614
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
terminology used in [7]. Five inner knots were used and the constraint gðsÞ > 0, in problems (23) and (24), was replaced by gðsÞ > e, with e ¼ 102 (as in [7]). 6. Numerical experiments The implemented versions of a discretization method were the ones described in [9,10] (Hettich modified version) and [20] (Reemtsen modified version) adapted to nonlinear semi-infinite programming (see [25] for more details). The following definitions are needed to describe the discretization algorithms. Let T be a Cartesian product of intervals ½a1 ; b1 ½a2 ; b2 ½ap ; bp . Definition 1. A grid is a set of the form T ½s ¼ ðs1 ; s2 ; . . . ; sp Þ ¼ T \ ft ¼ ðt1 ; t2 ; . . . ; tp Þ : ti ¼ ai þ jsi ; j ¼ 0; . . . ; ni ; i ¼ 1; . . . ; pg, where ni ¼ ðbi ai Þ=si . Definition 2. NLP(T ½s) is the following nonlinear programming sub-problem: minn x2R
f ðxÞ
s:t: ci ðx; tÞ 6 0;
i ¼ 1; . . . ; m;
ð26Þ
8t 2 T ½s;
Algorithm 1. (Hettich [9,10]) Step 0: Define T ½s0 , set Te ½s0 ¼ T ½s0 . Solve QP( Te ½s0 ) and let x0 be the solution. Define R ¼ Rðx0 Þ. Step k: If Sðxk1 Þ 6 Te ½sk1 then: Make Te ½sk1 ¼ R [ Sðxk1 Þ. Solve QP( Te ½sk1 ) and let xk1 be the solution. If Rðxk1 Þ 6 R then: Set R ¼ R [ Rðxk1 Þ. else: Add one point from Te ½sk1 n R to R. Continue with Step k. else: If k>r stop. Te ½sk ¼ R [ Sðxk1 Þ [N ðSðxk1 ÞÞ. Solve QP( Te ½sk ) and let xk be the solution. Set R ¼ R [ Rðxk Þ. Go to Step k þ 1, where r is the number of refinements and N ðSðxk1 ÞÞ contains the neighbours of points of Sðxk1 Þ in the full grid T ½sk that make an infinite constraint active or violated. The algorithm proposed by Reemtsen is presented below with some changes in the notation to meet our requirements: Algorithm 2. (Reemtsen [20])
ð27Þ
Step 0: Choose ee k 2 ð0; 1Þ and e d k P 0 ðk ¼ 1; . . . ; rÞ. Set D0 ¼ T ½s0 . Set also i ¼ 0 and k ¼ 1. Step 1: Solve NLP ðDi Þ and let xi be the solution. Step 2: If k > r stop. Set Biþ1 ¼ T ½sk , eiþ1 ¼ ee k and diþ1 ¼ e dk .
The following sets are crucial in the algorithm procedures. Rðxk Þ denotes the set of all points of the grid at iteration k that makes an infinite constraint active, i.e., Rðxk Þ ¼ ft 2 T ½sk : ci ðxk ; tÞ ¼ 0g. Sðxk Þ is the set of all points of the grid at iteration k that makes an infinite constraint active or violated, i.e., Sðxk Þ ¼ ft 2 T ½sk : ci ðxk ; tÞ P 0g. The algorithm proposed by Hettich for quadratic SIP is as follows:
where f ðxi Þ is the objective value in xi (solution of NLP(Di )). The first algorithm implemented in this paper is obtained from the Algorithm 1 where the QP subproblem is replaced by a NLP sub- problem.
Definition 3. QP(T ½s) is the quadratic programming sub-problem: 1 T x Qx þ cT x 2 s:t: AðtÞx bðtÞ 6 0; i ¼ 1; . . . ; m; 8t 2 T ½s: minn x2R
If xi solves the full grid, i.e., cj ðxi ; tÞ 6 diþ1 for all t 2 Biþ1 and j ¼ 1; . . . ; m then: Increment k by one and continue with Step 2. else: Set Diþ1 ¼ ft 2 Biþ1 : cj ðxi ; tÞ P eiþ1 f ðxi Þ; j ¼ 1; . . . ; mg. Increment i by one and go to Step 1.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Reemtsen showed numerical results, using Algorithm 2, for Chebyshev approximation problems, which are linear SIP. To solve nonlinear SIP, using Algorithm 2, the set Diþ1 was replaced by Diþ1 ¼ ft 2 Biþ1 : cj ðxi ; tÞ P eiþ1 jf ðxi Þj; j ¼ 1; . . . ; mg, as for our problems the objective function value could be no longer positive. The finite nonlinear sub-problems (NLP) were solved with the NPSOL ([5]) solver. These algorithms are implemented in the nonlinear semi-infinite programming solver (NSIPS [26]) which is publicly available from the internet (see Section 5.3). The notation used in Table 5 is Problem Problem name (filename with .mod extension in SIPAMPL). ACH/ACR Average number of constraints in the NLP solved in Hettich/Reemtsen version of the algorithm (the number of constraints in the first NLP solved is not counted). NH/NR Number of NLP solved in Hettich/Reemtsen version of the algorithm. fxH/fxR Objective function value in the solution of the final grid in Hettich/Reemtsen version of the algorithm. In all problems the step size in the first grid was 0.01, resulting in 100 points in the initial grid. Two refinements were made resulting in 600 points in the final grid. For problem elke2 neither method converges to a solution, since NPSOL could not find an admissible solution, as the limits on the velocity, acceleration and jerk are small. For problem elke7 neither method converges from the initial guess
615
x0 ¼ ð4:85386; . . . ; 4:85386Þ proposed by HaarenRetagne [7]. Other guess is used x0 ¼ ð2:85386; . . . ; 2:85386Þ. The numerical results were obtained on a Pentium III at 450 Mhz with 128 MB of RAM and a Linux Operating System (Red Hat 5.2) with AMPL Student Version 19991027 (Linux 2.0.18). User times, in seconds, for the solved problems are shown in Table 6 for both versions implemented (‘‘H’’––Hettich version, ‘‘R’’––Reemtsen version). The times reported by Haaren-Retagne in [7] were around 8 seconds and the problems were solved with a reduction type method and a sequential quadratic programming approach was used for the finite sub-problem. The numerical solutions obtained by Haaren-Retagne are similar to the ones obtained herein (see Table 7). Table 6 User times in solved problems (seconds) Problem
H
R
elke1 elke3 elke4 elke5 elke6 elke7
2.31 1.43 1.32 10.56 9.35 9.36
2.39 1.96 2.20 17.58 13.79 13.17
Table 7 Numerical results obtained by Haaren-Retagne in [7] Problem
Terminology [7]
Solution
elke1 elke2 elke3 elke4 elke5 elke6 elke7
Ex1A Ex1B Ex1C Ex2 Ex3A Ex3B Ex3C
1.08351 2.55783 1.52472 2.48325 2.39669 4.52595 2.91143
Table 5 Numerical results for the modified discretization algorithms Problem
ACH
ACR
NH
NR
fxH
fxR
elke1 elke2 elke3 elke4 elke5 elke6 elke7
150 – 116.7 50 130 122.5 130
545 – 540 606 675 755 455
7 – 4 5 4 5 5
3 – 3 3 3 3 3
1.08 – 1.52 2.50 2.40 4.49 2.91
1.08 – 1.52 2.50 2.40 4.52 2.91
616
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
7. Conclusions Robot trajectory planning problems can be posed as semi-infinite programming problems and efficiently solved by a number of available numerical methods. The robotics problems Model 1 and Model 2 presented herein are coded and publicly available in the SIPAMPL [24] database (see Section 5.3). Numerical results presented in the previous section are illustrative of the robustness of the discretization method when supported by a robust finite nonlinear solver. The two versions implemented differ in the average number of constraints and in the number of finite sub-problems solved but obtain similar solutions. In spite of the solutions found here (Table 5) and the ones obtained by Haaren-Retagne (Table 7) being similar, in all discretization methods an exact (or near exact) SIP solution is not guaranteed, but a solution on the finest grid is obtained.
Acknowledgements The authors are grateful to Denis Bouyssou, and two anonymous referees for their interest and their useful comments and suggestions.
Appendix A. B-Splines formulae The formulae used in the B-Splines library are: A.1. B-Splines blending functions Bi;k;n ðtÞ ¼
t ni Bi;k1;n ðtÞ niþk1 ni n t þ iþk Biþ1;k1;n ðtÞ: niþk niþ1
A.2. B-Splines functions derivatives w.r.t. the t parameter ! oj1 Biþ1;k1;n ðtÞ oj1 Bi;k1;n ðtÞ oj Bi;k;n ðtÞ otj1 otj1 ¼ ðk 1Þ þ otj niþk niþ1 niþk1 ni
with o0 Bi;k;n ðtÞ ¼ Bi;k;n ðtÞ; ot0 Pn nþj X oj Bk;n ðtÞ oj ðjþ1Þ i¼1 xi Bi;k;n ðtÞ ¼ ¼ xi Bi;kj;n ðtÞ j j ot ot i¼1 with ðjþ1Þ xi
( ¼
for j ¼ 0;
xi ðjÞ
ðk
ðjÞ
xi xi1 jÞ niþkj ni
for j > 0:
A.3. B-Splines first derivatives w.r.t. the coefficients Pn j oj xi Bi;k;n ðtÞ i¼1 o Bk;n ðtÞ o o otj otj oj Bl;k;n ðtÞ ¼ ¼ : otj oxl oxl A.4. B-Splines second derivatives w.r.t. the coefficients The B-Splines are a linear combination of blending functions whose variable is t and so the second derivatives w.r.t. the coefficients are zero. References [1] C. Boor, A Practical Guide to Splines, Springer-Verlag, 1978. [2] J.J. Craig, Introduction to Robotics, Mechanics and Control, second ed., Addison-Wesley, 1989. [3] R. Fourer, D.M. Gay, B.W. Kernighan, A modeling language for mathematical programming, Management Science 36 (5) (1990) 519–554. [4] H.P. Geering, L. Guzzella, S.A.R. Hepner, C.H. Onder, Time-optimal motions of robots in assembly tasks, IEEE Transactions on Automatic Control AC-31 (6) (1986) 512– 518. [5] P.E. Gill, W. Murray, M.A. Saunders, M.H. Wright, UserÕs guide for NPSOL: A fortran package for nonlinear programming, Stanford University, 1986. [6] M.S. Gockenbach, A.J. Kearsley, Optimal signal sets for non-gaussian detectors, SIAM Journal on Optimization 9 (2) (1999) 316–326. [7] E. Haaren-Retagne, A semi-infinite programming algorithm for robot trajectory planning, Ph.D. thesis, University of Trier, 1992. [8] A. Heim, O. Von Stryk, Trajectory optimization of industrial robots with application to computer-aided robotics and robot controllers, Optimization 47 (2000) 407–420.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 [9] R. Hettich, An implementation of a discretization method for semi-infinite programming, Mathematical Programming 34 (3) (1986) 354–361. [10] R. Hettich, G. Gramlich, A note on an implementation of a method for quadratic semi-infinite programming, Mathematical Programming 46 (1990) 249–254. [11] R. Hettich, K.O. Kortanek, Semi-infinite programming: Theory, methods, and applications, SIAM Review 35 (3) (1993) 380–429. [12] B. Hu, C.L. Teo, H.P. Lee, Local optimization of weighted joint torques for redundant robotic manipulators, IEEE Transactions on Robotics and Automation 11 (3) (1995) 422–425. [13] O.L.R. Jacobs, Introduction to Control Theory, Clarendon Press, Oxford, 1974. [14] C.T. Lawrence, A computationally efficient feasible sequencial quadratic programming algorithm, Ph.D. thesis, Institute for Systems Research, University of Maryland, 1998. [15] Y. Li, D. Wang, A semi-infinite programming model for earliness/tardiness production planning with simulated annealing, Mathematical and Computer Modelling 26 (7) (1997) 35–42. [16] S.P. Marin, Optimal parametrization of curves for robot trajectory design, IEEE Transactions on Automatic Control 33 (2) (1988) 209–214. [17] M. Oter, S. T€ urk The DFVLR models 1 and 2 of the manutec r3 robot, Tech. Report DFVLR-Mitt.88-13, Institute for Robotics and Systemdynamics, German Aerospace Research Establishment (DLR), Oberpfaffenhofen, 1992. [18] D.A. Pierre, Optimization Theory with Applications, Dover, 1986. [19] E. Polak, A.L. Tits, A recursive quadratic programming algorithm for semi-infinite optimization problems, Applied Mathematics and Optimization 8 (1982) 325–349.
617
[20] R. Reemtsen, Discretization methods for the solution of semi-infinite programming problems, Journal of Optimization Theory and Applications 71 (1) (1991) 85–103. [21] K.G. Shin, N.D. McKay, Minimum-time control of robotic manipulators with geometric path constraints, IEEE Transaction on Automatic Control AC-30 (6) (1985) 531–541. [22] O. Von Stryk, Numerical solution of optimal control problems by direct collocation, in: R. Bulirsch, A. Miele, J. Stoer, K.-H. Well (Eds.), Optimal Control––Calculus of variations, Optimal Control Theory and Numerical Methods, International Series of Numerical Mathematics 111 (1993) 129–143. [23] O. Von Stryk, M. Schlemmer, Optimal control of the industrial robot manutec r3, in: R. Bulirsch, D. Kraft (Eds.), Computational Optimal Control, International Series of Numerical Mathematics 115 (1994) 367–382. [24] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, SIPAMPL: Semi-Infinite Programming with AMPL, Technical Report ALG/EF/1-2000, Universidade do Minho, Downloadable from website http://www.eng.uminho.pt/ ~dps/aivaz/), in press. [25] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, Discretization methods for semi-infinite programming, Investigacß~ao Operacional 21 (1) (2001) 37–46. [26] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes NSIPS: Nonlinear Semi-Infinite Programming Solver, Technical Report ALG/EF/1-2001, Universidade do Minho, 2001 (Downloadable from website http://www.eng.uminho.pt/ ~dps/aivaz/). [27] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, Optimal signal sets via semi-infinite programming, Investigacß~ao Operacional 22 (1) (2002) 87–101. [28] D. Wang, S.-C. Fang, A semi-infinite programming model for earliness/tardiness production planning with a genetic algorithm, Computers and Mathematics with Applications 31 (8) (1996) 95–106.
Robot trajectory planning with semi-infinite programming A. Ismael F. Vaz
a,*
, Edite M.G.P. Fernandes a, M. Paula S.F. Gomes
b
a
b
Departamento de Producßa~o e Sistemas, Escola de Engenharia, Universidade do Minho, Campus de Gualtar, Braga 4710-057, Portugal Department of Mechanical Engineering, Mechatronics in Medicine Laboratory, Imperial College of Science, Technology and Medicine, London SW7 2BX, UK
Abstract In this paper we describe how robot trajectory planning can be formulated as a semi-infinite programming (SIP) problem. The formulation as a SIP problem allowed us to treat the problem with one of the three main classes of methods for solving SIP, the discretization class. Two of the robotics trajectory planning problems formulated were coded in the SIPAMPL environment which is publicly available. A B-Spline library was also created to allow the codification of the robotics trajectory problem. 2003 Elsevier B.V. All rights reserved. Keywords: Nonlinear programming; Robot trajectory planning; Semi-infinite programming; Discretization method; SIPAMPL database
1. Introduction Some engineering problems, like air pollution control [11], optimal signal sets design [6,14,27], production planning [15,28], Chebyshev approximation theory [9,10,20] and robot trajectory planning [7,16,23], can be formulated as semi-infinite programming problems. In this paper we will describe how two robot trajectory planning problems can be mathematically formulated as semiinfinite programming problems, coded in AMPL using a B-Spline library and efficiently solved by a
*
Corresponding author. E-mail addresses: [email protected] (A.I.F. Vaz), [email protected] (E.M.G.P. Fernandes), [email protected] (M.P.S.F. Gomes). URL: http://www.eng.uminho.pt/~dps/aivaz/.
discretization method. These formulations follow the ideas in [7,8,16,23]. In [21] an algorithm to minimize the trajectoryÕs time is proposed to solve a parametrization problem with a theoretical control method. In [4] the analysis of the optimal problem is done by the PontryaginÕs principle [18]. Both [4] and [24] use the bang–bang principle (see, for example, [13]) and numerical examples of dynamic equations for several robots are presented. In spite of the problem formulations in [8,23] being identical to the ones described herein, the problems were solved by a direct collocation method [22]. For the robot models see [17]. In [12] conditions for the weight joint torque matrices in the weight joint optimization problem are found. The found conditions guarantee that the local optimization of weighted joint torque with these matrices will have the same stability and
0377-2217/$ - see front matter 2003 Elsevier B.V. All rights reserved. doi:10.1016/S0377-2217(03)00266-2
608
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
optimality conditions as the local optimization of the weighted joint torque with the inertia inverse matrix. We start in Section 2 by introducing the reader to semi-infinite programming and to the main approaches used to solve semi-infinite programming problems. In Section 3 a brief introduction on trajectory definition is made. Section 4 will be devoted to presenting a robotics problem formulated as semi-infinite programming problem. In Section 5, the codified problems for which we present some numerical results are shown. The implemented algorithms and the numerical results obtained with the coded problems are shown in Section 6. Some conclusions are presented in the last section.
2. Semi-infinite programming A SIP problem is a mathematical program of the form minn x2R
f ðxÞ
s:t: ci ðx; tÞ 6 0;
i ¼ 1; . . . ; m;
set is a grid. The algorithm calculates a sequence of finite grids T0 T1 TN T . Finite nonlinear sub-problems are solved in each grid with a selected number of points. Let Tei Ti , i ¼ 0; . . . ; N, be such sets. The various discretization methods correspond to the various ways of selecting the points in Tei (for some examples see [9,10,16,19,20,25]). Exchange methods consist of computing approximations to the solutions of the problem max ci ðx; tÞ; t2T
i ¼ 1; . . . ; m;
ð2Þ
for a given x 2 Rn . Some of these approximations and other points define a finite set Te T and a finite minimization problem is solved on Te . The corresponding solution is set to x and the process is repeated with a new finite Te set, where some points could be added to and others dropped out from the previous set (exchange of points in the Te set). Reduction type methods calculate all the global (and some local) maxima to the problem (2). The infinite set is then replaced by a finite one that consists of the maxima found.
ð1Þ
8t 2 T ; where f ðxÞ is the objective function and ci ðx; tÞ, i ¼ 1; . . . ; m, are the infinite constraint functions. These problems are called semi-infinite programming problems due to the constraints ci ðx; tÞ 6 0, i ¼ 1; . . . ; m. We can think of T as an infinite index set and therefore (1) is a problem with finitely many variables over an infinite set of constraints. In fact, problem (1) could be described in a more general form (including equality and inequality constraints not depending on the variable t), but this formulation suits our purpose in this paper. During the last decades some efficient numerical methods have been proposed for solving SIP. The problems described in Section 4 were solved using a discretization method (see [11] for a review on SIP and numerical methods). There are three main classes of algorithms for SIP: discretization methods, exchange methods and reduction methods. A discretization method consists of replacing the infinite set T by a finite one. Usually the finite
3. Trajectory definition For a brief review of robotics mechanics and control we refer to [2]. A robot can be schematically represented as a connection of links. The robot represented in Fig. 1 has two links. Each link has a length and mass associated (d1 , d2 , m1 , m2 for the length and mass of the two links, respectively). Some mechanical devices provide movement to the links in some way. The ability that the robot has to position the links is called the degrees of freedom (d.o.f.) of the robot. In the case represented in Fig. 1 the robot has three d.o.f.: the ability to rotate around the y-axis, the ability to move link one and the ability to move link two. In robot joint space the specification of the values for the d.o.f. are enough to define the robot position in space. Since the robot position (d.o.f. values) varies we can define the path as a curve T
hðsÞ ¼ ½h1 ðsÞ; h2 ðsÞ; . . . ; hl ðsÞ ;
s 2 ½0; sf ;
ð3Þ
parametrized by s, where l is the number of d.o.f.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Y
m1
d2
assume that the given curve satisfies Eqs. (4)–(6) and without loss of generality that sf ¼ 1. Two optimization problems can be defined depending on the constraints used. The optimization problem results from minimizing a certain performance index (time, energy consumption, etc) while ensuring that the resulting trajectory can indeed be followed by the robot. We will look for a change of variables that relates the time t to the parameter s:
m2
θ3
d1 θ2
θ 1
X
t ¼ hðsÞ;
Fig. 1. Three degrees of freedom robot.
Natural constraints applied to the parametric curve are: 2 l X dhi > 0; s 2 ð0; sf Þ; ð4Þ ds i¼1 dh dh ð0Þ ¼ ðsf Þ ¼ 0 ds ds
ð5Þ
and 2
dh dh ð0Þ; ðsf Þ 6¼ 0: 2 ds ds2
ð6Þ
Eq. (4) means that at any time at least one joint in the trajectory must be in movement. Eq. (5) means that in the initial/final position the robot must be in rest (although any other value is acceptable) and Eq. (6) means that the robot in the initial/final position has to be in acceleration/deacceleration. Throughout the paper we will use the following notation for the derivatives: df ; f_ ¼ dt
f0 ¼
s 2 ½0; 1:
ð7Þ
This change of variables will be such that
Z
2
609
df : ds
4. Optimal parametrization of curves for robot trajectory design In this section the optimization of a given path in robot joint trajectory is described. Let us
• h0 ðsÞ > 0 for all s 2 ð0; 1Þ, meaning that h is strictly increasing; R1 • the total travel time tf :¼ hð1Þ ¼ 0 h0 ðsÞ ds is minimal; for Model 1 formulation, joint velocity, acceleration and jerk constraints are imposed: _ i ðtÞj 6 Ci;1 ; jH € i ðtÞj 6 Ci;2 ; jH ...
jHi ðtÞj 6 Ci;3 ; where Ci;1 , Ci;2 , Ci;3 are the constant bounds on velocity, acceleration and jerk on joint i, Hi ðtÞ :¼ hi ðh1 ðtÞÞ, i ¼ 1; . . . ; l and t 2 T :¼ ½0; tf . for a Model 2 formulation, torque constraints are imposed: j Fei ðtÞj 6 Ci ; where Fei ðtÞ :¼ F ðh1 ðtÞÞ and Ci are the joint torque and joint torque limit of the ith robot joint, respectively, i ¼ 1; . . . ; l and t 2 T :¼ ½0; tf . The described problems are in the generalized semi-infinite programming form, since the infinite index set depends on the control variables (the infinite set T depends on tf which is to be minimized). Using the fact that s ¼ h1 ðtÞ;
ð8Þ
we can define the problems in the standard semiinfinite programming form.
610
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 00 ðsÞÞ2 h0 ðsÞh000 ðsÞ
00
Model 1 can then be written as
...
ð9Þ
hð1Þ is minimum;
hi ðsÞ ¼
h ðsÞ 3ðh 00 0 h000 i ðsÞ 3hi ðsÞ h0 ðsÞ þ hi ðsÞ
0
hð0Þ ¼ 0;
ð10Þ
h0 ðsÞ > 0;
ð11Þ
jh_i ðsÞj 6 Ci;1 ;
ð12Þ
jh€i ðsÞj 6 Ci;2 ;
ð13Þ
...
jhi ðsÞj 6 Ci;3 ;
i ¼ 1; . . . ; l;
8s 2 ½0; 1:
ð14Þ
¼
where ui ðsÞ ¼
ð15Þ
The problem will be formulated in terms of the derivative of the unknown function h. Let gðsÞ ¼ h0 ðsÞ:
ð16Þ
The expression for the manipulatorÕs torques is 1 Fi ðsÞ ¼ Ji ni h€i ðsÞ þ Bi ni h_i ðsÞ þ ni þ
l X l X j¼1
l X
Iij ðhðsÞÞh€j ðsÞ
j¼1
!
Cijk ðhðsÞÞh_j ðsÞh_k ðsÞ þ di ðhðsÞÞ ;
2
00 ðsÞ
g3 ðsÞ
:
Using (18) and (19), Eq. (17) can then be rewritten as 0 1 0 0 h00i h0i gg A þ Bi ni hi Fi ðsÞ ¼ Ji ni @ g2 g 0 1 g0 1 @ ui v i g þ þ wi A; ð21Þ ni g2
jFi ðsÞj 6 Ci ;
8s 2 ½0; 1:
0
g ðsÞ 3ðg ðsÞÞ gðsÞg 00 0 h000 i ðsÞ 3hi ðsÞ gðsÞ þ hi ðsÞ g2 ðsÞ
ð20Þ
Model 2 is obtained by replacing Eqs. (12)–(14) with i ¼ 1; . . . ; l;
ðh0 ðsÞÞ2
3 ðh0 ðsÞÞ
9
Pl
00 > j¼1 Iij ðhðsÞÞhj ðsÞ > > P P = þ lj¼1 lk¼1 Cijk ðhðsÞÞh0j ðsÞh0k ðsÞ : Pl 0 > > > j¼1 Iij ðhðsÞÞhj ðsÞ ;
vi ðsÞ ¼ wi ðsÞ ¼ di ðhðsÞÞ
ð22Þ The Model 1 problem can then be rewritten in a SIP form as Z 1 minn gðsÞ ds x2R
s:t:
k¼1
0
gðsÞ > 0; jh_i ðsÞj 6 Ci;1 ;
ð17Þ
jh€i ðsÞj 6 Ci;2 ;
where for the ith robot joint: Ji ¼ motor inertia (Ji > 0, i ¼ 1; . . . ; l); ni ¼ gear ratio; Bi ¼ viscous damping coefficient (Bi > 0, i ¼ 1; . . . ; l); ðIij ðhðsÞÞÞi;j¼1;...;l ¼ inertia matrix (positive definite); ðCijk ðhðsÞÞÞi;j;k¼1;...;l ¼ Coriolis tensor; di ðhðsÞÞ ¼ gravitational torque. Using the chain rule, equalities (8) and (16) yield
jhi ðsÞj 6 Ci;3 ;
ð23Þ
...
d h0 ðsÞ h0i ðsÞ h_i ðsÞ ¼ fhi ðh1 ðtÞÞg ¼ i0 ; ¼ dt h ðsÞ gðsÞ 00
h€i ðsÞ ¼
ðsÞ h00i ðsÞ h0i ðsÞ hh0 ðsÞ
ðh0 ðsÞÞ2
ð18Þ 0
¼
ðsÞ h00i ðsÞ h0i ðsÞ ggðsÞ
g2 ðsÞ
;
ð19Þ
i ¼ 1; . . . ; l; 8s 2 ½0; 1; and Model 2 as Z 1 minn gðsÞ ds x2R
s:t:
0
gðsÞ > 0; jFi ðsÞj 6 Ci ;
ð24Þ i ¼ 1; . . . ; l;
8s 2 ½0; 1: For practical purposes, the gðsÞ function will be approximated by a B-spline ([1]). The objective function in Model 1 and Model 2 is then:
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Z
1
gðsÞ ds ¼
0
Z
1
Bk;n ðsÞ ds
ðsee 5:2Þ
Bk;n ðtÞ ¼
0
¼
Z
1
n X i¼1
¼
xi Bi;k;n ðtÞ;
ð25Þ
i¼1
0
¼
n X
611
n X
xi Bi;k;n ðsÞ ds
by ð25Þ
i¼1
Z
xi
1
Bi;k;n ðsÞ ds
0
n 1X xi ðniþk ni Þ: k i¼1
where Bi;k;n ðtÞ; for all i, are called the blending functions for the B-Spline. These blending functions are polynomials of degree k in one variable (t) and form a basis. xi are the coefficients of the BSpline and n is the knot vector. When using the B-Splines in a mathematical problem we may need to calculate the derivatives of the B-Splines (with respect to the t or xi variables). The formulae used are presented in the Appendix A.
5. Robotics problems coded in AMPL 5.3. The B-Splines library A library for AMPL to allow the codification of mathematical problems that use B-Splines is available and will be described in the following subsections. The B-Splines library is used to code the Model 1 and Model 2 optimal parametrization curve (SIP) problems that appear in robotics. In this section a brief introduction to AMPL and to B-splines will be given. We also describe the B-Splines library for AMPL and give an example on how this library is used for coding the robotics problems. In the Appendix A we show the BSplines formulae used in the library. 5.1. AMPL AMPL [3] is a modeling language that allows the codification of mathematical programming problems. AMPL is a descriptive language and does not allow the direct programming of functions (and no recursion is allowed). The automatic differentiation is only used to obtain the derivatives of the objective functions and constraints. Some problems in mathematical programming resort to function approximation by B-Splines [1]. The codification of such problems in AMPL can be very tedious if one needs to use several BSplines (degree of the B-Spline, number of coefficients, number of knots). 5.2. B-Splines A B-Spline is a linear combination of blending functions. A B-Spline can be represented by
AMPL has the ability to load a dynamic library. The B-Splines library and installation procedure are available from the internet 1 along with the SIPAMPL [24] package. This package contains a database with more than one hundred and thirty SIP problems coded in AMPL and an interface to any SIP solver (in particular to the NSIPS [26] solver). The library provides two functions bspline and dbspline. bspline is used to evaluate the B-Spline. dbspline is used to evaluate the BSpline derivatives with respect to the t parameter. The functions syntax is: bspline ðt; k; n; x1; x2; . . . ; xn; k1; k2; . . . ; knÞ; dbspline ðt; j; k; n; x1; x2; . . . ; xn; k1; k2; . . . ; knÞ; where t and x1; . . . ; xn are AMPL variables that correspond to the blending functions parameter and to the B-Spline coefficients respectively. k, n, k1; . . . ; kn are constants, where k is the B-Spline degree, n is the number of coefficients and k1; . . . ; kn are the knot points. j indicates the jderivative of the B-Spline with respect to t (oj B=otj ). bspline and dbspline return to AMPL the B-Spline value in t and, if requested, the first and second derivatives of the B-Spline with respect to the blending parameter and the coefficients. When
1
http://www.eng.uminho.pt/~dps/aivaz/.
612
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
using an external function the function AMPL command must be used, before using the function itself.
# to save some space we define a variable which is the B-Spline var g ¼ bspline(t,k,n,{i in 1. . .n} x[i],{i in 1. . .nk}knots[i]);
5.4. A robotics example Some problems in robotics can be formulated as a minimum time parametrization of a path in robot joint space [7,16]. This parametrization consists of finding a function that minimizes the total travel time of a robot for a given trajectory (in robot joint space). This function is usually approximated by a B-Spline. The problem can be equated in the following form (only one simple constraint is imposed, since the purpose is to illustrate how B-Splines can be used in AMPL, and not how to solve a real robotics problem. For a complete description see Section 4): Z 1 n 1X minn Bk;n ðtÞ dt ¼ xi ðniþk ni Þ x2R k i¼1 0 s:t: Bk;n ðtÞ P 0; 8t 2 ½0; 1; as a semi-infinite programming problem. Please note that the objective function does not depend on t. The constraint forces the B-Spline to be positive in the interval ½0; 1. A possible codification of this problem in AMPL is the following: # declare used external functions function bspline; #function dbspline; (not used in ex ample) # number of coefficients param n: ¼ 7; # degree of spline param k: ¼ 4; # number of knots param nk: ¼ 11; # knots vector param knots{1. . .nk}; # coefficients var x{1. . .n}; # blending parameter var t;
minimize obj: (1/k)*(sum {i in 1. . .n} (knots[i+k]-knots[i])));
(x[i]*
subject to tcons: g > ¼ 0; data; # add data to problem (knots) param knots : ¼ 1 0 2 0 3 0 4 0 5 0.25 6 0.5 7 0.75 8 1 9 1 10 1 11 1;
5.5. Coded SIP problems With the B-Splines library we were able to test three trajectories. The trajectories are those described by Haaren-Retagne in [7]. Example 1. A Model 1 formulation with the trajectory defined by h1 ðsÞ ¼ 1:5fðsÞ; h2 ðsÞ ¼ 0:5 sinð4:7fðsÞÞ; h3 ðsÞ ¼ 1:3fðsÞ þ 2:6; where fðsÞ ¼ s3 ð6s2 15s þ 10Þ. Example 2. A Model 2 formulation with the trajectory defined by
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 Table 3 Constants for Model 2 formulation
4 2 h1 ðsÞ ¼ pðfðsÞ 0:5Þ ; 3 p p h2 ðsÞ ¼ þ sinð2pfðsÞÞ; 2 4 p h3 ðsÞ ¼ fðsÞ; 2 where fðsÞ ¼ 2s3 þ 3s2 . Example 3. A Model 1 formulation where the robot coordinates are given through a spline representation with normalized B-splines of degree 6: hj ðsÞ ¼
11 X
aij Bi;6;n ðsÞ;
613
j ¼ 1; 2; 3;
i¼1
where the knots n are given by n1 ¼ ¼ n6 ¼ 0, n7 ¼ 0:1754653, n8 ¼ 0:3552259, n9 ¼ 0:5061700, n10 ¼ 0:6877756, n11 ¼ 0:8319530, n12 ¼ ¼ n17 ¼ 1. The coefficients aij are given by Table 1. The three different choices of derivatives bounds for Model 1 formulation are given in Table 2. The constants that define the robot for the Model 2 formulation are given in Table 3 and the
Table 1 Coefficients of the B-Splines robot coordinates i
j¼1
j¼2
j¼3
1 2 3 4 5 6 7 8 9 10 11
0.8353216 0.8353216 0.8353216 2.294404 1.506865 0.8491478 0.5452022 0.1477306 1.227333 1.227333 1.227333
)0.4971999 )0.4971999 )0.4971999 )1.289908 )0.2470876 0.1336621 0.7745534 )0.07365886 )0.4258067 )0.4258067 )0.4258067
2.803471 2.803471 2.803471 2.137097 1.881815 2.158783 1.821837 3.067521 2.340066 2.340066 2.340066
i
Ji
ni
Bi
Ci
1 2 3
1.98 104 1.98 104 1.98 104
62.5 62.5 62.5
1.744 103 1.744 103 1.744 103
2.090 0.523 1.045
manipulator masses and link lengths are m1 ¼ 5 kg, m2 ¼ 2 kg, d1 ¼ 1:5 m, d2 ¼ 0:5 m. For the sake of simplicity the dynamic equations for this robot are omitted, but they can be easily obtained from the coded problem. The combination of Examples 1–3 with the several constraints bounds gives seven robotics semi-infinite programming problems that were coded in AMPL. The problem characteristics are reported in Table 4. The problems were added to the SIPAMPL [24] database which can be obtained freely from the internet (see Section 5.3). In Table 4 ‘‘Problem’’ is the problem name (the filename has a .mod extension). The problems were solved with nine coefficients and one parameter in the B-Spline. The number of coefficients can be changed by introducing more knots to the B-Spline. ‘‘Model’’ indicates that the problem is a Model 1 (23) or Model 2 (24) formulation. ‘‘Bounds’’ indicates the different choices for the constraints bounds. The last column refers to the Table 4 Robotics problems coded in AMPL Problem
Model
Bounds
Terminology [7]
elke1 elke2 elke3 elke4 elke5 elke6 elke7
1 1 1 2 1 1 1
A B C – A B C
Ex1A Ex1B Ex1C Ex2 Ex3A Ex3B Ex3C
Table 2 Derivatives bounds for Model 1 formulation h1 A B C
h2
h3
C11
C12
C13
C21
C22
C23
C31
C32
C33
2 1 2
8 3 8
250 100 25
3 1 3
18 3 18
650 100 65
4 1 4
50 3 50
1000 100 100
614
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
terminology used in [7]. Five inner knots were used and the constraint gðsÞ > 0, in problems (23) and (24), was replaced by gðsÞ > e, with e ¼ 102 (as in [7]). 6. Numerical experiments The implemented versions of a discretization method were the ones described in [9,10] (Hettich modified version) and [20] (Reemtsen modified version) adapted to nonlinear semi-infinite programming (see [25] for more details). The following definitions are needed to describe the discretization algorithms. Let T be a Cartesian product of intervals ½a1 ; b1 ½a2 ; b2 ½ap ; bp . Definition 1. A grid is a set of the form T ½s ¼ ðs1 ; s2 ; . . . ; sp Þ ¼ T \ ft ¼ ðt1 ; t2 ; . . . ; tp Þ : ti ¼ ai þ jsi ; j ¼ 0; . . . ; ni ; i ¼ 1; . . . ; pg, where ni ¼ ðbi ai Þ=si . Definition 2. NLP(T ½s) is the following nonlinear programming sub-problem: minn x2R
f ðxÞ
s:t: ci ðx; tÞ 6 0;
i ¼ 1; . . . ; m;
ð26Þ
8t 2 T ½s;
Algorithm 1. (Hettich [9,10]) Step 0: Define T ½s0 , set Te ½s0 ¼ T ½s0 . Solve QP( Te ½s0 ) and let x0 be the solution. Define R ¼ Rðx0 Þ. Step k: If Sðxk1 Þ 6 Te ½sk1 then: Make Te ½sk1 ¼ R [ Sðxk1 Þ. Solve QP( Te ½sk1 ) and let xk1 be the solution. If Rðxk1 Þ 6 R then: Set R ¼ R [ Rðxk1 Þ. else: Add one point from Te ½sk1 n R to R. Continue with Step k. else: If k>r stop. Te ½sk ¼ R [ Sðxk1 Þ [N ðSðxk1 ÞÞ. Solve QP( Te ½sk ) and let xk be the solution. Set R ¼ R [ Rðxk Þ. Go to Step k þ 1, where r is the number of refinements and N ðSðxk1 ÞÞ contains the neighbours of points of Sðxk1 Þ in the full grid T ½sk that make an infinite constraint active or violated. The algorithm proposed by Reemtsen is presented below with some changes in the notation to meet our requirements: Algorithm 2. (Reemtsen [20])
ð27Þ
Step 0: Choose ee k 2 ð0; 1Þ and e d k P 0 ðk ¼ 1; . . . ; rÞ. Set D0 ¼ T ½s0 . Set also i ¼ 0 and k ¼ 1. Step 1: Solve NLP ðDi Þ and let xi be the solution. Step 2: If k > r stop. Set Biþ1 ¼ T ½sk , eiþ1 ¼ ee k and diþ1 ¼ e dk .
The following sets are crucial in the algorithm procedures. Rðxk Þ denotes the set of all points of the grid at iteration k that makes an infinite constraint active, i.e., Rðxk Þ ¼ ft 2 T ½sk : ci ðxk ; tÞ ¼ 0g. Sðxk Þ is the set of all points of the grid at iteration k that makes an infinite constraint active or violated, i.e., Sðxk Þ ¼ ft 2 T ½sk : ci ðxk ; tÞ P 0g. The algorithm proposed by Hettich for quadratic SIP is as follows:
where f ðxi Þ is the objective value in xi (solution of NLP(Di )). The first algorithm implemented in this paper is obtained from the Algorithm 1 where the QP subproblem is replaced by a NLP sub- problem.
Definition 3. QP(T ½s) is the quadratic programming sub-problem: 1 T x Qx þ cT x 2 s:t: AðtÞx bðtÞ 6 0; i ¼ 1; . . . ; m; 8t 2 T ½s: minn x2R
If xi solves the full grid, i.e., cj ðxi ; tÞ 6 diþ1 for all t 2 Biþ1 and j ¼ 1; . . . ; m then: Increment k by one and continue with Step 2. else: Set Diþ1 ¼ ft 2 Biþ1 : cj ðxi ; tÞ P eiþ1 f ðxi Þ; j ¼ 1; . . . ; mg. Increment i by one and go to Step 1.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
Reemtsen showed numerical results, using Algorithm 2, for Chebyshev approximation problems, which are linear SIP. To solve nonlinear SIP, using Algorithm 2, the set Diþ1 was replaced by Diþ1 ¼ ft 2 Biþ1 : cj ðxi ; tÞ P eiþ1 jf ðxi Þj; j ¼ 1; . . . ; mg, as for our problems the objective function value could be no longer positive. The finite nonlinear sub-problems (NLP) were solved with the NPSOL ([5]) solver. These algorithms are implemented in the nonlinear semi-infinite programming solver (NSIPS [26]) which is publicly available from the internet (see Section 5.3). The notation used in Table 5 is Problem Problem name (filename with .mod extension in SIPAMPL). ACH/ACR Average number of constraints in the NLP solved in Hettich/Reemtsen version of the algorithm (the number of constraints in the first NLP solved is not counted). NH/NR Number of NLP solved in Hettich/Reemtsen version of the algorithm. fxH/fxR Objective function value in the solution of the final grid in Hettich/Reemtsen version of the algorithm. In all problems the step size in the first grid was 0.01, resulting in 100 points in the initial grid. Two refinements were made resulting in 600 points in the final grid. For problem elke2 neither method converges to a solution, since NPSOL could not find an admissible solution, as the limits on the velocity, acceleration and jerk are small. For problem elke7 neither method converges from the initial guess
615
x0 ¼ ð4:85386; . . . ; 4:85386Þ proposed by HaarenRetagne [7]. Other guess is used x0 ¼ ð2:85386; . . . ; 2:85386Þ. The numerical results were obtained on a Pentium III at 450 Mhz with 128 MB of RAM and a Linux Operating System (Red Hat 5.2) with AMPL Student Version 19991027 (Linux 2.0.18). User times, in seconds, for the solved problems are shown in Table 6 for both versions implemented (‘‘H’’––Hettich version, ‘‘R’’––Reemtsen version). The times reported by Haaren-Retagne in [7] were around 8 seconds and the problems were solved with a reduction type method and a sequential quadratic programming approach was used for the finite sub-problem. The numerical solutions obtained by Haaren-Retagne are similar to the ones obtained herein (see Table 7). Table 6 User times in solved problems (seconds) Problem
H
R
elke1 elke3 elke4 elke5 elke6 elke7
2.31 1.43 1.32 10.56 9.35 9.36
2.39 1.96 2.20 17.58 13.79 13.17
Table 7 Numerical results obtained by Haaren-Retagne in [7] Problem
Terminology [7]
Solution
elke1 elke2 elke3 elke4 elke5 elke6 elke7
Ex1A Ex1B Ex1C Ex2 Ex3A Ex3B Ex3C
1.08351 2.55783 1.52472 2.48325 2.39669 4.52595 2.91143
Table 5 Numerical results for the modified discretization algorithms Problem
ACH
ACR
NH
NR
fxH
fxR
elke1 elke2 elke3 elke4 elke5 elke6 elke7
150 – 116.7 50 130 122.5 130
545 – 540 606 675 755 455
7 – 4 5 4 5 5
3 – 3 3 3 3 3
1.08 – 1.52 2.50 2.40 4.49 2.91
1.08 – 1.52 2.50 2.40 4.52 2.91
616
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617
7. Conclusions Robot trajectory planning problems can be posed as semi-infinite programming problems and efficiently solved by a number of available numerical methods. The robotics problems Model 1 and Model 2 presented herein are coded and publicly available in the SIPAMPL [24] database (see Section 5.3). Numerical results presented in the previous section are illustrative of the robustness of the discretization method when supported by a robust finite nonlinear solver. The two versions implemented differ in the average number of constraints and in the number of finite sub-problems solved but obtain similar solutions. In spite of the solutions found here (Table 5) and the ones obtained by Haaren-Retagne (Table 7) being similar, in all discretization methods an exact (or near exact) SIP solution is not guaranteed, but a solution on the finest grid is obtained.
Acknowledgements The authors are grateful to Denis Bouyssou, and two anonymous referees for their interest and their useful comments and suggestions.
Appendix A. B-Splines formulae The formulae used in the B-Splines library are: A.1. B-Splines blending functions Bi;k;n ðtÞ ¼
t ni Bi;k1;n ðtÞ niþk1 ni n t þ iþk Biþ1;k1;n ðtÞ: niþk niþ1
A.2. B-Splines functions derivatives w.r.t. the t parameter ! oj1 Biþ1;k1;n ðtÞ oj1 Bi;k1;n ðtÞ oj Bi;k;n ðtÞ otj1 otj1 ¼ ðk 1Þ þ otj niþk niþ1 niþk1 ni
with o0 Bi;k;n ðtÞ ¼ Bi;k;n ðtÞ; ot0 Pn nþj X oj Bk;n ðtÞ oj ðjþ1Þ i¼1 xi Bi;k;n ðtÞ ¼ ¼ xi Bi;kj;n ðtÞ j j ot ot i¼1 with ðjþ1Þ xi
( ¼
for j ¼ 0;
xi ðjÞ
ðk
ðjÞ
xi xi1 jÞ niþkj ni
for j > 0:
A.3. B-Splines first derivatives w.r.t. the coefficients Pn j oj xi Bi;k;n ðtÞ i¼1 o Bk;n ðtÞ o o otj otj oj Bl;k;n ðtÞ ¼ ¼ : otj oxl oxl A.4. B-Splines second derivatives w.r.t. the coefficients The B-Splines are a linear combination of blending functions whose variable is t and so the second derivatives w.r.t. the coefficients are zero. References [1] C. Boor, A Practical Guide to Splines, Springer-Verlag, 1978. [2] J.J. Craig, Introduction to Robotics, Mechanics and Control, second ed., Addison-Wesley, 1989. [3] R. Fourer, D.M. Gay, B.W. Kernighan, A modeling language for mathematical programming, Management Science 36 (5) (1990) 519–554. [4] H.P. Geering, L. Guzzella, S.A.R. Hepner, C.H. Onder, Time-optimal motions of robots in assembly tasks, IEEE Transactions on Automatic Control AC-31 (6) (1986) 512– 518. [5] P.E. Gill, W. Murray, M.A. Saunders, M.H. Wright, UserÕs guide for NPSOL: A fortran package for nonlinear programming, Stanford University, 1986. [6] M.S. Gockenbach, A.J. Kearsley, Optimal signal sets for non-gaussian detectors, SIAM Journal on Optimization 9 (2) (1999) 316–326. [7] E. Haaren-Retagne, A semi-infinite programming algorithm for robot trajectory planning, Ph.D. thesis, University of Trier, 1992. [8] A. Heim, O. Von Stryk, Trajectory optimization of industrial robots with application to computer-aided robotics and robot controllers, Optimization 47 (2000) 407–420.
A.I.F. Vaz et al. / European Journal of Operational Research 153 (2004) 607–617 [9] R. Hettich, An implementation of a discretization method for semi-infinite programming, Mathematical Programming 34 (3) (1986) 354–361. [10] R. Hettich, G. Gramlich, A note on an implementation of a method for quadratic semi-infinite programming, Mathematical Programming 46 (1990) 249–254. [11] R. Hettich, K.O. Kortanek, Semi-infinite programming: Theory, methods, and applications, SIAM Review 35 (3) (1993) 380–429. [12] B. Hu, C.L. Teo, H.P. Lee, Local optimization of weighted joint torques for redundant robotic manipulators, IEEE Transactions on Robotics and Automation 11 (3) (1995) 422–425. [13] O.L.R. Jacobs, Introduction to Control Theory, Clarendon Press, Oxford, 1974. [14] C.T. Lawrence, A computationally efficient feasible sequencial quadratic programming algorithm, Ph.D. thesis, Institute for Systems Research, University of Maryland, 1998. [15] Y. Li, D. Wang, A semi-infinite programming model for earliness/tardiness production planning with simulated annealing, Mathematical and Computer Modelling 26 (7) (1997) 35–42. [16] S.P. Marin, Optimal parametrization of curves for robot trajectory design, IEEE Transactions on Automatic Control 33 (2) (1988) 209–214. [17] M. Oter, S. T€ urk The DFVLR models 1 and 2 of the manutec r3 robot, Tech. Report DFVLR-Mitt.88-13, Institute for Robotics and Systemdynamics, German Aerospace Research Establishment (DLR), Oberpfaffenhofen, 1992. [18] D.A. Pierre, Optimization Theory with Applications, Dover, 1986. [19] E. Polak, A.L. Tits, A recursive quadratic programming algorithm for semi-infinite optimization problems, Applied Mathematics and Optimization 8 (1982) 325–349.
617
[20] R. Reemtsen, Discretization methods for the solution of semi-infinite programming problems, Journal of Optimization Theory and Applications 71 (1) (1991) 85–103. [21] K.G. Shin, N.D. McKay, Minimum-time control of robotic manipulators with geometric path constraints, IEEE Transaction on Automatic Control AC-30 (6) (1985) 531–541. [22] O. Von Stryk, Numerical solution of optimal control problems by direct collocation, in: R. Bulirsch, A. Miele, J. Stoer, K.-H. Well (Eds.), Optimal Control––Calculus of variations, Optimal Control Theory and Numerical Methods, International Series of Numerical Mathematics 111 (1993) 129–143. [23] O. Von Stryk, M. Schlemmer, Optimal control of the industrial robot manutec r3, in: R. Bulirsch, D. Kraft (Eds.), Computational Optimal Control, International Series of Numerical Mathematics 115 (1994) 367–382. [24] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, SIPAMPL: Semi-Infinite Programming with AMPL, Technical Report ALG/EF/1-2000, Universidade do Minho, Downloadable from website http://www.eng.uminho.pt/ ~dps/aivaz/), in press. [25] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, Discretization methods for semi-infinite programming, Investigacß~ao Operacional 21 (1) (2001) 37–46. [26] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes NSIPS: Nonlinear Semi-Infinite Programming Solver, Technical Report ALG/EF/1-2001, Universidade do Minho, 2001 (Downloadable from website http://www.eng.uminho.pt/ ~dps/aivaz/). [27] A.I.F. Vaz, E.M.G.P. Fernandes, M.P.S.F. Gomes, Optimal signal sets via semi-infinite programming, Investigacß~ao Operacional 22 (1) (2002) 87–101. [28] D. Wang, S.-C. Fang, A semi-infinite programming model for earliness/tardiness production planning with a genetic algorithm, Computers and Mathematics with Applications 31 (8) (1996) 95–106.