Download - Time-Optimal Path Tracking for Robots: A Convex Optimization Approach

2318 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 54, NO. 10, OCTOBER 2009

Time-Optimal Path Tracking for Robots:A Convex Optimization Approach

Diederik Verscheure, Bram Demeulenaere, Jan Swevers, Joris De Schutter, and Moritz Diehl

Abstract—This paper focuses on time-optimal path tracking, asubproblem in time-optimal motion planning of robot systems.Through a nonlinear change of variables, the time-optimal pathtracking problem is transformed here into a convex optimalcontrol problem with a single state. Various convexity-preservingextensions are introduced, resulting in a versatile approach foroptimal path tracking. A direct transcription method is presentedthat reduces finding the globally optimal trajectory to solving asecond-order cone program using robust numerical algorithmsthat are freely available. Validation against known examples andapplication to a more complex example illustrate the versatilityand practicality of the new method.

Index Terms—Second-order cone program (SOCP)-based solu-tion method.

I. INTRODUCTION

T IME-OPTIMAL motion planning is of significant impor-tance for maximizing the productivity of robot systems.

Solving the motion planning problem in its entirety, however, isin general a highly complex and difficult task [1]–[6]. Therefore,instead of solving the entire motion planning problem directlyin the system’s state space, which is called the direct approach[5], the decoupled approach [1], [5], [7]–[9] is often preferredfor its lower computational requirements. The decoupled ap-proach solves the motion planning problem in two stages. In thefirst path planning stage, a high-level planner determines a geo-metric path thereby accounting for task specifications, obstacleavoidance and other high-level—usually geometric—aspects[1], [8], [9], but ignoring lower-level—often dynamic—aspects

Manuscript received November 20, 2007; revised October 24, 2008. Firstpublished September 22, 2009; current version published October 07, 2009.This work was supported by K.U. Leuven’s Concerted Research ActionGOA/05/10, K.U. Leuven’s CoE EF/05/006 Optimization in EngineeringCenter (OPTEC), the Prof. R. Snoeys Foundation, and the Belgian Programon Interuniversity Poles of Attraction IAP VI/4 DYSCO (Dynamic Systems,Control and Optimization) initiated by the Belgian State, Prime Minister’sOffice for Science, Technology and Culture. Recommended by AssociateEditor C.-Y. Su.

D. Verscheure, B. Demeulenaere, J. Swevers, and J. De Schutter are withthe Division PMA, Department of Mechanical Engineering, KatholiekeUniversiteit Leuven, Belgium. They are also with the Optimization inEngineering Center (OPTEC), Katholieke Universiteit Leuven (e-mail:[email protected]; [email protected]; [email protected]; [email protected]; [email protected]).

M. Diehl is with the Division SCD, Department of Electrical Engineering(ESAT), Katholieke Universiteit Leuven, Belgium. He is also with the Optimiza-tion in Engineering Center (OPTEC), Katholieke Universiteit Leuven (e-mail:[email protected]).

Color versions of one or more of the figures in this paper are available onlineat http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/TAC.2009.2028959

such as the dynamics of the robotic manipulator. In the sub-sequent path tracking or path tracking stage, a time-optimaltrajectory along the geometric path is determined, whereby themanipulator dynamics and actuator constraints are taken intoaccount [7], [10]–[23].

The path tracking stage constitutes the focus of this paper.Time optimality along a predefined path implies realizing ashigh as possible a velocity along this path, without violatingactuator constraints. To this end, the optimal trajectory shouldexploit the actuators’ maximum acceleration and decelerationability [10], [13], such that for every point along the path, atleast one actuator saturates [15].

Methods for time-optimal robot path tracking subject to ac-tuator constraints have been proposed in [7], [10]–[24]. Whilethese optimal control methods can roughly be divided into threecategories, most exploit that motion along a predefined path canbe described by a single path coordinate and its time derivative

[7], [10], [13]. Hence, the multi-dimensional state space of arobotic manipulator can be reduced to a two-dimensional statespace. The curve, sometimes referred to as the switchingcurve [10], [14], unambiguously determines the solution of thetime-optimal path tracking problem.

The first category of methods are indirect methods, whichhave have been proposed in [7], [10], [13] and subsequentlyrefined in [14], [16], [19], [21], [22]. For the sake of brevity,only [14] is discussed here, which reports to be at least an orderof magnitude faster than [7], [10], [13]. This method conductsone-dimensional numerical searches over to exhaustively de-termine all characteristic switching points, that is, points wherechanges between the active actuator constraints can occur.Based on these points, the switching curve, and hence thesolution to the overall planning problem, is found numericallythrough a procedure that is based on forward and backwardintegrations.

The second category consists of dynamic programmingmethods [11], [13], [24], while the third category consists ofdirect transcription methods [18], [23]. In contrast with most ofthe indirect methods, except [19] which considers time-energyoptimality, the methods in the second and third category areable to take into account more general constraints and objectivefunctions, such that time-optimality can be traded-off againstother criteria such as energy, leading to less aggressive use ofthe actuators.

In this paper, the basic time-optimal path tracking problem,discussed in Section II, is transformed into a convex optimalcontrol problem with a single state through a nonlinear changeof variables introduced in Section III. Various convexity-pre-serving extensions, resulting in a versatile approach for optimal

0018-9286/$26.00 © 2009 IEEE

VERSCHEURE et al.: TIME-OPTIMAL PATH TRACKING FOR ROBOTS 2319

path tracking that goes beyond mere time-optimality, are pre-sented in Section IV. Section V shows that direct transcription[6], [25]–[27] by simultaneous discretization of the states andthe controls, results in a reliable and very efficient method to nu-merically solve the optimal control problem based on second-order cone programming. Section VI subsequently validates thisnumerical method against the examples introduced in [13]. Sec-tion VII illustrates the practicality and versatility of the gener-alized problem formulation through a more advanced exampleof a six-DOF KUKA 361 industrial robot carrying out a writingtask. Section VIII contrasts the proposed solution method withthe existing indirect methods [7], [10], [13], [14], [16], [19],[21], [22], dynamic programming methods [11], [13], [24] anddirect transcription methods [18], [23] and identifies aspects offuture work.

II. ORIGINAL PROBLEM FORMULATION

The equations of motion of an -DOF robotic manipulatorwith joint angles , can be written as a function of theapplied joint torques as [28]

(1)

where is a positive definite mass matrixand is a matrix accounting for Coriolisand centrifugal effects, which is linear in the joint velocities,

is a matrix of Coulomb friction torques, whichcan be joint angle dependent, while denotes thevector accounting for gravity and other joint angle dependenttorques. In this paper, similarly as in [13], viscous friction is notconsidered.1

Consider a path , given in joint space coordinates,2 as afunction of a scalar path coordinate . The path coordinate de-termines the spatial geometry of the path, whereas the trajec-tory’s time dependency follows from the relation betweenthe path coordinate and time . Without loss of generality, itis assumed that the trajectory starts at , ends at andthat . In addition, since this paperconsiders time-optimal path tracking or related problems, it isassumed that everywhere and almost every-where for .

For notational convenience, the time dependency of the pathcoordinate and its derivatives is omitted wherever possible.For the given path, the joint velocities and accelerations can berewritten using the chain rule as

(2)

(3)

where , , and. Substituting and based on (2),

(3) results in the following expression for the equations of mo-tion [10]:

(4)

1Similarly as in [13], viscous friction is ignored to allow the equations ofmotion to be reformulated as a linear set of equations in �� and �� .

2For a path given in operational space coordinates, inverse kinematics tech-niques can be used to obtain the corresponding path in joint space coordinates[14], [28].

where

(5)

(6)

(7)

and where is replaced by using (2) andthe assumption that almost everywhere.

Similarly as in [7], [10], [13], the time-optimal path trackingproblem for the robotic manipulator subject to lower and upperbounds on the torques, can be expressed as

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

where the torque lower bounds and upper bounds may de-pend on . In most cases, and can be taken equal to 0.

III. REFORMULATION AS A CONVEX OPTIMAL

CONTROL PROBLEM

From (8)–(15), it is not obvious to decide whether any localsolution to the problem is also globally time-optimal. In [19], thetime-energy optimal control problem, which features (8)–(15)as a special case, is reformulated as an optimal control problemwith linear system dynamics, differential state and con-trol input , subject to nonlinear state dependent control con-straints. Subsequently, the Hamiltonian is shown to be convexwith respect to the control input, which allows to conclude thatany local optimum of the problem is also globally optimal. Thispaper provides, thanks to the use of a nonlinear change of vari-ables, an appreciably different reformulation with a number ofattractive properties.

First, by changing the integration variable from to , theobjective function (8) is rewritten as

(16)

Second

(17)

(18)

are introduced as optimization variables and supplemented withan additional constraint

(19)

which follows from the observation that:

(20)


as well as

(21)

While the nonlinear transformation (17), (18) is already recog-nized in [13], where it is used for quadrature purposes and fora geometric characterization of the admissable area of motion,this paper instead uses the transformed variables directly as theoptimization variables, such that problem (8)–(15) can be refor-mulated as a convex problem

(22)

(23)

(24)

(25)

(26)

(27)

(28)

Problem (22)–(28) is convex since all constraints (23)–(28) arelinear, while the objective function (22) is convex.

Problem (22)–(28) can be regarded as an optimal controlproblem in differential algebraic form (DAE), with pseudo-time, control input , differential state , algebraic states

, linear system dynamics (26) and subject to linear statedependent constraints (23), (27), (28), as well as initial andterminal constraints (24) and (25) respectively.

In contrast with the reformulation in [19], the reformulatedproblem (22)–(28) has only one differential state, while the alge-braic states can easily be eliminated using (23). Moreover, timedoes not appear explicitly in the formulation anymore. However,the true merit of the reformulation (22)–(28), is that, unlike thereformulation in [19], first, it is clear without proof that problem(22)–(28) is convex, and second, it becomes very easy to deviseadditional objective functions and inequality constraints that canbe incorporated, such that the resulting optimal control problemis still convex, as illustrated in Sections IV-A and IV-B. In addi-tion, due to the particular structure of the reformulated problem,it is shown in Section V how a numerical solution can be ob-tained very efficiently and reliably using a direct transcriptionmethod [6], [25]–[27].

IV. GENERALIZED CONVEX OPTIMAL CONTROL PROBLEM

In Sections IV-A and IV-B, a number of practical objectivefunctions and constraints are given, which can be incorporatedto yield a more general, yet still convex optimal control problemas summarized in Section IV-C.

A. Objective Functions

In addition to time, other objectives can also be incorporatedto design a desirable relation between the path coordinateand time , such that time-optimality is traded-off against othercriteria.

1) Thermal Energy: The integral of the square of the torqueof joint is

(29)

This objective function is related to the thermal energy gener-ated by actuator . It can easily be shown [29] that is aconvex function of , for .

2) Integral of the Absolute Value of the Rate of Change of theTorque: The integral of the absolute value of the rate of changeof the torque of joint is

(30)

since for . This objective function is convex,since is a convex function of . While this objective functionhas no straightforward physical interpretation, incorporatingthis term reduces the rate of change of the torques. As explainedin Section VI, for some cases, this can be particularly useful toeliminate or reduce high rates of change of the torques, in otherwords, torque jumps.

B. Inequality Constraints

In addition to torque constraints, other constraints may alsobe useful.

1) Velocity Constraints: It is possible to incorporate velocitylimits, which may be inspired by task specifications, by im-posing symmetric lower bounds and upper boundson the velocity of joint as follows:

(31)

or by imposing symmetric lower bounds and upper boundson the translational components or the rotationalcomponents of the operational space velocity. Forexample

(32)

The operational space velocity components are related to thejoint velocities by the robot Jacobian [28] for

and , as follows:

(33)

Hence, (32) can be rewritten as

(34)


Note that the bounds defined by (31) and (34) can easily berewritten in the following form:

(35)

and that hence, they can be interpreted directly as upper boundson . Therefore, it is also sufficient to consider only the mostrestrictive upper bound for each .

2) Acceleration Constraints: It is also possible to incorporatelower bounds and upper bounds on the accelerationof joint as follows:

(36)

Similar as for velocity constraints, acceleration constraints canalso be applied to the components of the operational space ac-celeration. It can be shown that both joint space and operationalspace acceleration constraints can be rewritten in the followingform:

(37)

3) Rate of Torque Change Constraints: It can be shown thatconstraints on the rate of change of the torque , which arealso useful to impose, are not convex and are therefore less at-tractive from an optimization point of view.

C. Generalized Problem Formulation

Combining the objective functions (8), (29) and (30) in anaffine way3 and incorporating the constraints (9), (31), (32)yields the following generalized optimal control problem

(38)

(39)

(40)

(41)

(42)

(43)

(44)

(45)

(46)

where for are suitably chosen values, for ex-ample . This generalized optimal controlproblem is convex, due to the convexity of the objective functionand the inequality constraints, and the linearity of the system dy-namics and the equality constraints.

3Strictly speaking, the integral of the square of the torque for each actuatorshould be weighted with a factor which is proportional to the product of themotor resistance, the square of the motor gearing constant and the inverse of thesquare of the motor constant, as in [11]. Since these quantities are not alwaysreadily available, this paper adopts �� instead.

Fig. 1. �� is piecewise constant for � � �� .

Fig. 2. �� is piecewise linear for � � �� .

Fig. 3. � �� is piecewise nonlinear for � � �� .

V. NUMERICAL SOLUTION

There are three ways to solve the generalized optimal controlproblem (38)–(46), namely dynamic programming as adoptedin [11], [13], [24], indirect methods [19], or direct transcriptionas adopted in [18], [23] or direct single or multiple shooting[30]. In this section, a direct transcription method is proposedin Section V-A, which leads to a second-order cone program(SOCP) formulation in Section V-B.

A. Direct Transcription

The direct transcription method consists of reformulating theoptimal control problem (38)–(46) as a large sparse optimiza-tion problem. To this end, first, the path coordinate is dis-cretized4 on [0,1], which leads to grid points

, for . Second, the func-tions , and are modeled, by introducing a finitenumber of variables , , , which represent evaluations ofthese respective functions on the grid points or in between. Thechoice of the number of variables and the choice of the points atwhich they are evaluated characterize different direct transcrip-tion methods.

This paper proposes just one possibility. Since can be re-garded as the control input in problem (38)–(46), it is assumedto be piecewise constant (Fig. 1). Based on this assumption andfrom (26), it follows that is then piecewise linear (Fig. 2)and from (23), it follows that is then in general piecewisenonlinear (Fig. 3). From these observations, it is natural to as-sign on the grid points . In other words

(47)

4The grid size should be chosen such that � �� evaluated at the grid points,is a reasonable approximation for the continuous curve � �� for � � �� .


for and hence, . and areevaluated in the middle between the grid points , namely on

(Figs. 1 and 3). After introducingthe variables and , for

and given the fact that is piecewise linear, thefirst two terms of the integral (38) can be approximated as

(48)

To handle integrable singularities, that is, where ,is treated separately. Using (47) to calculate the in-

tegral in (48) analytically, the right-hand side of (48) can berewritten as

(49)

where . The third term of the integral (38) canbe approximated for each as

(50)

where for . After introducingthe shorthand notation , the problem(38)–(46) can be rewritten in discretized form as a large scaleoptimization problem

(51)

(52)

(53)

(54)

(55)

(56)

(57)

(58)

(59)

(60)

Due to the convexity of problem (51)–(60), any local optimum isalso globally optimal. Hence, the problem may be solved usingany general purpose nonlinear solver. However, by rewritingproblem (51)–(60) as a second-order cone program (SOCP), it

can be solved even more efficiently, using a dedicated solver forthese types of problems.

B. Second-Order Cone Program Formulation

An SOCP has the following standard form [29]

(61)

(62)

(63)

Reformulating problem (51)–(60) in this form requires anumber of steps. First, (51)–(60) can be reformulated as anequivalent problem with a linear objective function, by intro-ducing variables for and for

, such that (51) can be rewritten as

(64)

where is a vector with all elements equal to 1. It is thennecessary to augment problem (51)–(60) with the inequalityconstraints

(65)

and

(66)

Second, to obtain an SOCP, constraints (65) can be replacedby two equivalent constraints by introducing variables for

as

(67)

(68)

Inequalities (67) and (68) can now be rewritten as two second-order cone constraints of the general form (63) as

(69)

(70)

(71)

and

(72)

(73)


Summarizing, an SOCP is obtained in standard form

(74)

(75)

(76)

(77)

(78)

(79)

(80)

(81)

(82)

(83)

(84)

(85)

(86)

where , , are defined for , , for, and for . While the reformu-

lation as an SOCP is not straightforward and requires the intro-duction of the auxiliary variables , and , it allows to usemature solvers [31] based on interior point methods [32], [33],that exploit the specific SOCP structure very efficiently. Thesededicated methods have a much better worst-case complexitythan methods for more general types of convex programs, suchas semidefinite programs [34].

The formulation (74)–(86) can be implemented directly andvery easilyusing the free high-level optimization modeling toolYALMIP [35], which also allows to test various solvers. Oncea solution is obtained for the variables , the relation be-tween the path coordinate and time can be constructed from theinverse relation , which is calculated as

(87)

VI. NUMERICAL BENCHMARK

To illustrate the validity of the method discussed in Sec-tion V, it is applied to a double parabola and a circular pathfor a three-DOF elbow manipulator as in [13]. Using YALMIPas a modeling tool [35] and the free optimization softwareSeDuMi [31] which uses a primal-dual interior point method[31], the method presented in Section V is used to determine

Fig. 4. Joint torques �� as a function of the path coordinate � for the parabolicpath for � � � and � � �.

Fig. 5. Joint torques �� as a function of the path coordinate � for the parabolicpath for � � � and � � �� .

the time-optimal solutions for the parabolic and circular paths,with .

The resulting torques as a function of the path coordinateare shown in Fig. 4 for the parabola path and for .These torques are equal to those produced in [13] apart from nu-merical jitter around and . This jitter is causedby singularities which are specific to the combination of manip-ulator and path [16]. In a singular point, one of the elements ofthe vector defined by (5) is equal to zero [16], and thesolution to the time-optimal trajectory is not uniquely defined[14]. A very effective way to remove this jitter, which hardlyaffects the overall solution, is to add a small amount of regu-larization to penalize torque jumps, or in other words, choose

. The resulting torques as a function of the path coor-dinate for the parabola path with regularizationare shown in Fig. 5. Similar results are obtained for the circularpath. The regularization approach is equally effective in dealingwith singular arcs and has also been validated for the examplepresented in [16], which focusses in detail on the problem ofsingular points and singular arcs.

The regularization illustrates very well the flexibility of thegeneralized problem formulation and the proposed solutionmethod. Unwanted behavior is easily eliminated using anappropriate choice for the objective function. Furthermore,the efficiency of the proposed solution method is evident fromthe solver times reported by YALMIP, namely 0.98 s for theparabola path without regularization, 1.29 s for the parabola


Fig. 6. Six-DOF KUKA 361 industrial manipulator performing a writing task.

path with regularization and 1.24 s for the circular path withregularization on an Intel Pentium 4 CPU running at 3.60 GHz.If are eliminated as optimization variables, the solver timesdecrease to 0.74 s, 0.93 s, and 0.89 s, respectively. To allow thereader to perform own optimization studies, a downloadableMatlab implementation of the examples in [13] and [16] hasbeen made available [36].

VII. NUMERICAL EXAMPLE: WRITING TASK

To illustrate the versatility and practicality of the method dis-cussed in Section V, it is applied to a more complex exampleinvolving a six-DOF manipulator. Section VII-A discusses themanipulator and the path, while the results are presented in Sec-tion VII-B.

A. Introduction

In the context of programming by human demonstration [37],it may be desirable for a manipulator to track a human-gener-ated path, but not necessary or even undesirable to enforce thepath-time relation established during the demonstration. In fact,time-optimal path tracking may speed up these types of tasksconsiderably.

Considered here is a six-DOF KUKA 361 industrial manipu-lator carrying out a complex writing task (Fig. 6). The objectiveis to write a text on a plane parallel to the XY-plane (Fig. 6),while keeping the end-effector oriented in the negative Z-direc-tion at a height of . The end-effector path parallel tothe XY-plane is shown separately in Fig. 7, where the value ofthe path coordinate along the path is shown in steps of0.05. This type of path features long smooth segments as wellas sharp edges at the transitions between different character seg-ments. Therefore, a lot of switching is expected to realize thetime-optimal trajectory, such that the grid on which the problemis solved needs to be sufficiently fine.

B. Results

The method presented in Section V results in a minimal tra-jectory duration of 9.73 s . The corresponding

Fig. 7. Path in the plane parallel to the XY-plane.

Fig. 8. Joint torques �� as a function of the path coordinate � for the first threeaxes for the path shown in Fig. 7 and for � � � and � � �.

Fig. 9. Joint velocities �� as a function of the path coordinate � for the first threeaxes for the path shown in Fig. 7 and for � � � and � � �.

torques for the first5 three axes are shown as a function of thepath coordinate in Fig. 8 . Fig. 8 indicatesthat a lot of switching is necessary and that the torque for axistwo, which is heavily loaded by gravity, is saturated for a con-siderable part along the path and therefore is the main limitationon the acceleration and deceleration ability of the manipulator.

Fig. 9 shows the joint velocities for the first three axes for. Although interpretation is in general difficult,

it can be intuitively understood that a low velocity is required

5The torques for the last three axes are omitted, because the first three axesare more heavily loaded.


Fig. 10. Normalized trajectory duration versus the normalized integral of thesquares of the torques.

for the sharp edge of the letter “c” at , while the sub-sequent long smooth arc can be executed at a much higher ve-locity. Despite the relative non-smoothness of the path, sharpsegments of the path pose no problem to the solution method.By solving the problem for , 1999 and 2999, it is ver-ified that the obtained solution is, aside from sampling effects,grid-independent.

When solving the problem on an Intel Pentium 4 CPU run-ning at 3.60 Hz and with eliminated as optimization vari-ables, YALMIP reports a solver time of 2.87 s. To allow thereader to perform own optimization studies or to allow a faircomparison with other algorithms, a downloadable Matlab im-plementation has been made available [36], as well as a soft-ware-rendered video showing the manipulator carrying out thetrajectory.

C. Time-Optimality Versus Energy-Optimality

The very limited solver times allow to calculate the solutionof (74)–(86) for a large number of different weighting factors

and , in order to investigate their effect on the time-opti-mality of the solution. Here, time-optimality and energy-opti-mality are traded-off by varying . One important motivationfor having a nonzero is to limit the rate of change ofthe torques [23], such that the actuators can better handle thetorque demand. Other approaches to limit the rate of change ofthe torques consist of directly imposing upper and lower boundson this rate of change [17], [23], or choosing a parameterizationfor the pseudo-acceleration that is at least piecewise contin-uous [23]. Another important motivation for choosing ,on the other hand, is to limit the thermal energy dissipated bythe actuators, so as to prevent actuator overheating if the task iscarried out repeatedly [38].

Fig. 10 shows the relation between the trajectory durationand the thermal actuator energy if ischosen equal to , where is varied between and 0.6,while keeping . Both objectives are normalized throughdivision by their values for . The resulting trade-off curvereveals that a 10% increase in trajectory duration, results in aspectacular 38% reduction of the thermal energy dissipated bythe actuators (labeled (1) in Fig. 10), while a further increase intrajectory duration to 20% results in only 50% reduction of thethermal energy (labeled (2) in Fig. 10). These results, as well

as the steep trade-off close to , numerically quantifythe engineering intuition that true time-optimality comes at asignificant energy cost.

Fig. 10 illustrates very well the practicality and the versatilityof the generalized optimal control formulation (38)–(46) and theusefulness of an efficient numerical algorithm to investigate theeffect of trading off time-optimality against other criteria.

VIII. DISCUSSION

The convex reformulation and accompanying SOCP-basedsolution method presented in Sections III and V, are very ap-pealing from both a theoretical and a numerical point of view:the global optimum of time-energy optimal planning is guaran-teed to be found within a few CPU seconds of computation time.

None of the methods which consider more than just time-op-timality [11], [13], [18], [23], [24], except [19], can provide atheoretical guarantee of finding the global optimum. The theo-retical merit of our formulation with respect to [19], however,is that first, the proof of global optimality is extremely simpleas it follows directly from the convexity of the optimal controlproblem. Secondly, the formulation allows to easily devise con-vexity-preserving extensions, as illustrated in Section IV-A andIV-B.

With respect to numerical efficiency, it is difficult to compareour method to the other methods [7], [10], [11], [13], [14], [16],[18], [19], [21]–[24]: unfortunately, to the best of the authors’knowledge, no detailed solution times and actual implementa-tions are publicly available for most of the methods, some ofwhich were published nearly two decades ago. Hence, it is notpossible to assess whether these methods also yield the sameoptimal solution for the complex example of Section VII, norwhether they give rise to shorter computational times. Even ifthese methods were faster, the authors would perceive this asonly a minor shortcoming of the present method, since the com-putational times reported here are sufficiently short to be prac-tical in academic and industrial practice. In order to make futurenumerical benchmarking possible and to allow use by practi-tioners, the authors have provided a freely downloadable Matlabimplementation under the GNU public license [36].

Two other key aspects of the presented method are the easeof implementation and flexibility. With regard to flexibility, theindirect methods [7], [10], [13], [14], [16], [21], [22], except[19] which considers time-energy optimality, can only take intoaccount time-optimality and torque constraints and therefore donot feature the same flexibility as any of the dynamic program-ming [11], [13], [24] and direct transcription methods [18], [19],[23]. The latter two categories of methods do not restrict thechoice of constraints nor objective function, except that the ob-jective is generally assumed to be an affine combination of tra-jectory duration with other criteria. The price to be paid is thatthe global optimum is not guaranteed to be found. Our methodis more restricted in that sense, since only a limited, yet, inour opinion, sufficiently rich set of objective functions and con-straints can be handled. In fact, this limited set constitutes thevery essence of the efficiency of the presented solution method,and, rather than perceiving it as restrictive, it can also be seenas a theoretical foundation to evaluate the tractability of moregeneral formulations.


With regard to ease of implementation, it is clear from thediscussion in Section I that the indirect methods [7], [10], [13],[14], [16], [19], [21], [22] are procedural in nature and requireimplementation of a number of substeps, all of which requirethe choice of a method, tolerances and stopping criteria. Thedynamic programming approaches [11], [13], [24] are alsoprocedural in nature although their implementation involvesless numerical choices since they are decision-based rather thangradient-based. However, because of their sequential natureand large amount of variables, an efficient implementationrequires careful thought. Direct transcription methods [18],[23] are more straightforward to implement because they arebased on solving nonlinear programs for which, theoretically,any nonlinear solver can be used. Numerical efficiency, how-ever, requires an in-depth numerical analysis of the problemstructure. Conversely, the method in Section V requires onlythe choice of a transcription scheme, and the main difficultylies in enforcing the SOCP structure in the resulting program.The actual implementation is, however, very easy. Moreover,SOCP solvers such as SeDuMi [31], are freely available.

A first part of future work will focus on improving the exper-imental applicability of the time-optimal path tracking methodand consists of mitigating three key issues. First, the limitedbandwidth of any physical actuator implies that infinitely fasttorque jumps cannot be realized and therefore, the applicationof the presented method to sequential convex approximationsof problems which take into account constraints on the rate ofchanges of the torques will be investigated. Second, the torquescalculated by the time-optimal path tracking algorithm arepurely feedforward and need to be implemented in conjunctionwith a feedback controller. To prevent the feedback controllerfrom becoming unstable, it is necessary to impose boundson the actuator torques which are lower than the actuallyallowable actuator torques. This consideration is especiallyimportant in the presence of significant modeling errors. Third,the application of the presented method to sequential convexapproximations of problems which take into account viscousand other joint velocity dependent friction phenomena will beinvestigated. Finally, the practical applicability of the presentedmethod will be demonstrated experimentally. A second partof future work will focus on the application of the presentedmethod to path tracking problems which consider, in additionto time-optimality, minimization of reaction forces at the robotbase.

REFERENCES

[1] C.-S. Lin, P.-R. Chang, and J. Y. S. Luh, “Formulation and optimiza-tion of cubic polynomial joint trajectories for industrial robots,” IEEETrans. Autom. Control, vol. AC-28, no. 12, pp. 1066–1074, Dec. 1983.

[2] O. von Stryk and R. Bulirsch, “Direct and indirect methods for trajec-tory optimization,” Annals Oper. Res., vol. 37, pp. 357–373, 1992.

[3] M. C. Steinbach, H. G. Bock, and R. W. Longman, “Time optimal ex-tension and retraction of robots: Numerical analysis of the switchingstructure,” J. Optim. Theory Appl., vol. 84, no. 3, pp. 589–616, 1995.

[4] V. H. Schulz, “Reduced SQP Methods for Large-Scale Optimal Con-trol Problems in DAE With Application to Path Planning Problems forSatellite Mounted Robots,” Ph.D. dissertation, Universität Heidelberg,Heidelberg, Germany, 1996.

[5] H. Choset, W. Burgard, S. Hutchinson, G. Kantor, L. E. Kavraki, K.Lynch, and S. Thrun, Principles of Robot Motion: Theory, Algorithms,and Implementation. Cambridge, MA: MIT Press, Jun. 2005.

[6] M. Diehl, H. G. Bock, H. Diedam, and P.-B. Wieber, “Fast direct mul-tiple shooting algorithms for optimal robot control,” in Fast Motionsin Biomechanics and Robotics Optimization and Feedback Control,ser. Lecture Notes in Control and Information Sciences, K. Mombaur,Ed. New York: Springer, 2006, pp. 65–94.

[7] K. G. Shin and N. D. McKay, “Minimum-time control of robotic ma-nipulators with geometric path constraints,” IEEE Trans. Autom. Con-trol, vol. AC-30, no. 6, pp. 531–541, Jun. 1985.

[8] Z. Shiller and S. Dubowsky, “Robot path planning with obstacles, ac-tuator, gripper and payload constraints,” Int. J. Robot. Res., vol. 8, no.6, pp. 3–18, 1989.

[9] Z. Shiller and S. Dubowsky, “On computing the global time-optimalmotions of robotic manipulators in the presence of obstacles,” IEEETrans. Robot. Autom., vol. 7, no. 6, pp. 786–797, Dec. 1991.

[10] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, “Time-optimal controlof robotic manipulators along specified paths,” Int. J. Robot. Res., vol.4, no. 3, pp. 3–17, 1985.

[11] K. G. Shin and N. D. McKay, “A dynamic programming approach totrajectory planning of robotic manipulators,” IEEE Trans. Autom. Con-trol, vol. AC-31, no. 6, pp. 491–500, Jun. 1986.

[12] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectory plan-ning,” in Proc. IEEE Int. Conf. Robot. Autom., San Francisco, CA, Apr.1986, vol. 3, pp. 1399–1405.

[13] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectory plan-ning,” IEEE J. Robot. Autom., vol. RA-3, no. 2, pp. 115–123, Apr.1987.

[14] J.-J. E. Slotine and H. S. Yang, “Improving the efficiency of time-optimal path-following algorithms,” IEEE Trans. Robot. Autom., vol.RA-5, no. 1, pp. 118–124, Feb. 1989.

[15] Y. Chen and A. A. Desrochers, “Structure of minimum-time controllaw for robotic manipulators with constrained paths,” in Proc. IEEEInt. Conf. Robot. Autom., Scottsdale, AZ, 1989, vol. 2, pp. 971–976.

[16] Z. Shiller and H.-H. Lu, “Computation of path constrained time optimalmotions with dynamic singularities,” J. Dyn. Syst., Meas., Control, vol.114, pp. 34–40, Mar. 1992.

[17] M. Tarkiainen and Z. Shiller, “Time optimal motions of manipulatorswith actuator dynamics,” in Proc. IEEE Int. Conf. Robot. Autom., At-lanta, GA, 1993, vol. 2, pp. 725–730.

[18] J. T. Betts and W. P. Huffman, “Path-constrained trajectory optimiza-tion using sparse sequential quadratic programming,” J. Guid., ControlDyn., vol. 16, no. 1, pp. 59–68, 1993.

[19] Z. Shiller, “Time-energy optimal control of articulated systems withgeometric path constraints,” in Proc. IEEE Int. Conf. Robot. Autom.,San Diego, CA, 1994, pp. 2680–2685.

[20] Z. Shiller, “On singular time-optimal control along specified paths,”IEEE Trans. Robot. Autom., vol. 10, no. 4, pp. 561–566, Aug. 1994.

[21] H. X. Phu, H. G. Bock, and J. Schlöder, “Extremal solutions ofsome constrained control problems,” Optimization, vol. 35, no. 4, pp.345–355, 1995.

[22] H. X. Phu, H. G. Bock, and J. Schlöder, “The method of orientingcurves and its application for manipulator trajectory planning,” Numer.Funct. Anal. Optim., vol. 18, pp. 213–225, 1997.

[23] D. Constantinescu and E. A. Croft, “Smooth and time-optimal tra-jectory planning for industrial manipulators along specified paths,” J.Robot. Syst., vol. 17, no. 5, pp. 233–249, 2000.

[24] S. Singh and M. C. Leu, “Optimal trajectory generation for robotic ma-nipulators using dynamic programming,” J. Dyn. Syst., Meas., Control,vol. 109, no. 2, pp. 88–96, 1987.

[25] L. T. Biegler, “Solution of dynamic optimization problems by suc-cessive quadratic programming and orthogonal collocation,” Comput.Chem. Eng., vol. 8, pp. 243–248, 1984.

[26] O. Stryk, “Numerical solution of optimal control problems by directcollocation,” in Optimal Control: Calculus of Variations, Optimal Con-trol Theory and Numerical Methods. Basel, Germany: Birkhauser,1993, vol. 129.

[27] J. T. Betts, Practical Methods for Optimal Control Using NonlinearProgramming. Philadelphia, PA: SIAM, 2001.

[28] L. Sciavicco and B. Siciliano, Modeling and Control of Robot Manip-ulators. New York: McGraw-Hill, 1996.

[29] S. Boyd and L. Vandenberghe, Convex Optimization. Cam-bridge, MA: Cambridge Univ. Press, 2004 [Online]. Available:http://www.ee.ucla.edu/~vandenbe/cvxbook.html

[30] H. G. Bock and K. J. Plitt, “A multiple shooting algorithmfor direct solution of optimal control problems,” in Proc. 9thIFAC World Congress, Budapest, Hungary, 1984, pp. 243–247[Online]. Available: http://www.iwr.uni-heidelberg.de/groups/ag-bock/FILES/Bock1984.pdf, Pergamon Press


[31] J. F. Sturm, “Using SeDuMi: A Matlab toolbox for optimization oversymmetric cones,” Optim. Methods Software, pp. 11–12, 1999.

[32] S. J. Wright, Primal-Dual Interior-Point Methods. Philadelphia, PA:SIAM Publications, 1997.

[33] Y.-J. Kuo and H. Mittelmann, “Interior point methods for second-ordercone programming and OR applications,” Computat. Optim. Appl., vol.28, no. 3, pp. 255–285, 2004.

[34] M. Lobo, L. Vandenberghe, S. Boyd, and H. Lebret, “Applications ofsecond order cone programming,” Linear Algebra Appl., vol. 284, pp.193–228, 1998.

[35] J. Löfberg, YALMIP, Yet Another LMI Parser. Linköping,Sweden, Univ. Linköping, 2001 [Online]. Available: http://www.con-trol.isy.liu.se/~johanl

[36] D. Verscheure, Time-Optimal Path Tracking for Robots: A ConvexOptimization Approach 2008 [Online]. Available: http://homes.esat.kuleuven.be/~optec/software/timeopt/

[37] W. Meeussen, “Compliant Robot Motion: From Path Planning orHuman Demonstration to Force Controlled Task Execution,” Ph.D.dissertation, Dept. Mech. Eng., Katholieke Univ. Leuven, Leuven,Belgium, 2006.

[38] M. Guilbert, P.-B. Wieber, and L. Joly, “Optimal trajectory generationfor manipulator robots under thermal constraints,” in Proc. IEEE/RSJInt. Conf. Intell. Robots Syst., Beijing, China, 2006.

Diederik Verscheure received the M.Sc. degree inmechanical engineering from the Katholieke Uni-versiteit (K.U.) Leuven, Leuven, Belgium, in 2005where he is currently pursuing the Ph.D. degree.

His research focuses on contact modeling andidentification, and optimal robot motion planning.

Bram Demeulenaere received the M.Sc. degreein mechanical engineering and the Ph.D. degreein mechanical engineering from the KatholiekeUniversiteit (K.U.), Leuven, Belgium, in 1999 and2004, respectively.

From 2004 to 2008, he was a PostdoctoralFellow of the Research Foundation-Flanders(FWO) affiliated with UCLA, K.U.Leuven, and theRuprecht-Karls-Universität, Heidelberg, Germany.He currently works for the Airtec Division, AtlasCopco Airpower, Wilrijk, Belgium. His main re-

search interest concerns applications of convex optimization in mechatronicmotion system design and biomechanics.

Dr. Demeulenaere is a Postdoctoral Fellow of the Research Foundation-Flan-ders (FWO-Vlaanderen).

Jan Swevers received the M.Sc. degree in electricalengineering and the Ph.D. degree in mechanicalengineering from the Katholieke Universiteit (K.U.),Leuven, Belgium, in 1986 and 1992, respectively.

He is a Professor with the Department of Mechan-ical Engineering, Division Production Engineering,Machine Design and Automation (PMA), K.U.Leuven. His research interests include modeling,identification, control, and optimization of mecha-tronics systems.

Joris De Schutter received the M.Sc. degreein mechanical engineering from the KatholiekeUniversiteit (K.U.) Leuven, Leuven, Belgium, in1980, the M.Sc. degree from the MassachusettsInstitute of Technology, Cambridge, in 1981, and thePh.D. degree in mechanical engineering, from K.U.Leuven, in 1986.

Following work as a Control Systems Engineer inindustry, in 1986, he became a Lecturer with the De-partment of Mechanical Engineering, Division Pro-duction Engineering, Machine Design and Automa-

tion (PMA), K.U. Leuven, where he has been a Full Professor since 1995. Heteaches courses in kinematics and dynamics of machinery, control, and robotics,and has been the Coordinator of the study program in mechatronics, establishedin 1986. He has published papers on sensor-based robot control and program-ming (in particular, force control and compliant motion), position control of flex-ible systems, and optimization of mechanical and mechatronic drive systems.

Moritz Diehl received the Diploma degree in physicsand the Ph.D. degree in numerical mathematics fromthe University of Heidelberg, Heidelberg, Germany,in 1999 and 2001, respectively.

In 2001, he started working as a Scientific Assis-tant at the University of Heidelberg. In 2006, he be-came Associate Professor with the Electrical Engi-neering Department, Katholieke Universiteit (K.U.)Leuven, Leuven, Belgium, and the Principal Investi-gator of K.U. Leuven’s Optimization in EngineeringCenter OPTEC. His research interests include numer-

ical methods for optimization, in particular for dynamic systems described bydifferential equations, as well as real-time optimization, state and parameter es-timation, and convex optimization.