    # Optimal time-complexity speed planning for robot manipulators

We consider the speed planning problem for a robotic manipulator. In particular, we present an algorithm for finding the time-optimal speed law along an assigned path that satisfies velocity and acceleration constraints and respects the maximum forces and torques allowed by the actuators. The addressed optimization problem is a finite dimensional reformulation of the continuous-time speed optimization problem, obtained by discretizing the speed profile with N points. The proposed algorithm has linear complexity with respect to N and to the number of degrees of freedom. Such complexity is the best possible for this problem. Numerical tests show that the proposed algorithm is significantly faster than algorithms already existing in literature.

## Authors

##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## 1 Introduction

For robotic manipulators, the motion planning problem is often decomposed into two subproblems: path planning and speed planning LaValle (2006).

The first problem consists in finding a path (i.e., the curve followed by the joints) that joins assigned initial and final positions. The second problem consists in finding the time-optimal speed law along the path that satisfies assigned velocity and acceleration constraints and respects the maximum forces and torques allowed by the actuators. In this paper we consider only the second problem. Namely, given a path in the robot configuration space, we want to find the optimal speed-law that allows following while satisfying assigned kinematic and dynamics constraints. More specifically, we consider the problem

 minq,τtfD(q)¨q+C(q,˙q)˙q+ℓ(q)=τ,(˙q2,¨q,τ)∈C(q),q∈Γ, (1)

where: is the travel time; is the generalized position;

is the generalized force vector;

is the mass matrix; is a matrix accounting for centrifugal and Coriolis effects; is an external force term (for instance gravity); is a set that represents the kinematic and dynamic limitations of the manipulator.

### 1.1 Related Works

There are mainly three different families of speed profile generation methods: Numerical Integration, Dynamic Programming, and Convex Optimization.

References Bobrow, Dubowsky, and Gibson (1985); Pfeiffer and Johanni (1987) are among the first works that study Problem 1 using the Numerical Integration approach. In particular, they find the time-optimal speed law as a function of arc-length and not as a function of time. This choice simplifies the mathematical structure of the resulting problem and has been adopted by most of successive works. In Bobrow, Dubowsky, and Gibson (1985); Pfeiffer and Johanni (1987) the optimization problem is solved with iterative algorithms. In particular, reference Bobrow, Dubowsky, and Gibson (1985) finds the points in which the acceleration changes sign using the numerical integration of the second order differential equations representing the motions obtained with the maximum and minimum possible accelerations. Reference Pfeiffer and Johanni (1987) is based on geometrical considerations on the feasible set. However, this approach has some limitations due to the determination of the switching points that is the main source of failure of this approach (see Slotine and Yang (1989); Shiller and Lu (1992)). For recent results on Numerical Integration see Pham (2014); Pham, Caron, and Nakamura (2013); Pham and Stasse (2015). For instance Pham and Stasse (2015) considers the case of redundant manipulators.

In the Dynamic Programming approach the problem is solved with a finite element approximation of the Hamilton-Jacobi-Bellman equation (see Shin and McKay (1986); Singh and Leu (1987); Oberherber, Gattringer, and Müller (2015)). The main difficultly with this approach is the high computational time due to the need of solving a problem with a large number of variables.

The Convex Optimization approach is based on the approximation of Problem (1) with a finite dimensional optimization problem obtained through spatial discretization. Reference Verscheure, Demeulenaere, Swevers, Schutter, and Diehl (2009) is one of the early works using this approach. It shows that Problem (1) becomes convex after a change of variables and that a discretized version of Problem (1) is a Second-Order Cone Programming (SOCP) problem. This approach has the advantage that the optimization problem can be tackled with available solvers (e.g., see Lipp and Boyd (2014); Gurobi Optimization (2016)). Moreover, differently from the Numerical Integration, this approach allows considering other convex objective functions. However, the convex programming approach could be inappropriate (see for instance Pham (2014)) for online motion planning since the computational time grows rapidly (even if still polynomially) with respect to the number of samples in the discretized problem. Subsequent works, starting from Verscheure, Demeulenaere, Swevers, Schutter, and Diehl (2009), extend the applicability of this approach to different scenarios (see Debrouwere, Van Loock, Pipeleers, Dinh, Diehl, De Schutter, and Swevers (2013); Csorvási, Nagy, and Vajk (2017)) and propose algorithms that reduce the computational time (see Hauser (2014); Nagy and Vajk (2018)). To reduce computational time, reference Hauser (2014) proposes an approach based on

Sequential Linear Programming

(SLP). Namely, the algorithm proposed in Hauser (2014) sequentially linearizes the objective function around the current point, while a trust region method ensures the convergence of the process. Further, Nagy and Vajk (2018) shows that, using a suitable discretization method, the time optimal velocity profile can be obtained by Linear Programming (LP) with the benefit of a lower computation time with respect to convex solvers.

A very recent, and very interesting, paper, closely related to our work, is Pham and Pham (2017). In Section 4.1, we will shortly describe the approach proposed there and compare it with our approach.

Our approach combines the ideas which we previously proposed in two other works. Namely, in Consolini, Locatelli, Minari, and Piazzi (2017a) we proposed an exact linear-time forward-backward algorithm for the solution of a velocity planning problem for a vehicle over a given trajectory under velocity, normal and tangential acceleration bounds. In Csorvási, Nagy, and Vajk (2017), a method based on the sequential solution of two-dimensional subproblems is proposed for the solution of the so-called waiter motion problem. The method is able to return a feasible, though not necessarily optimal, solution. In the current paper we merge the ideas proposed in the two above mentioned papers in order to derive an approach for the speed planning of robotic manipulators. This will be proved to return an optimal solution and to have linear time complexity both with respect to the number of discretization points and to the number of degrees of freedom of the robotic manipulator.

### 1.2 Main results

The purpose of this paper is to provide a speed planning method for robotic manipulators with optimal time complexity. With respect to the existing literature, the new contributions of this work are the following ones.

• We propose a new algorithm for solving a finite dimensional reformulation of Problem (1) obtained with discretization points.

• We show that if set in Problem (1) is defined by linear constraints, then the proposed algorithm has complexity , where is the number of discretization points and is the number of degrees of freedom. Moreover, such complexity is optimal.

• By numerical tests, we show that the proposed procedure is significantly faster than algorithms already existing in literature.

### 1.3 Paper Organization

In Section 2, we present the time-optimal control problem for robotic manipulators in continuous time. In Section 3, we present a class of optimization problems and an exact solution algorithm. We prove the correctness of the algorithm and compute its time complexity, showing that such complexity is optimal in case of linear constraints. In Section 4, we show that by suitably discretizing the continuous time problem, it is possible to obtain a finite dimensional problem with linear constraints that falls into the class defined in Section 3. Finally, we present an experiment for a 6-DOF industrial robotic manipulator and we compare the performance of the proposed approach with that of existing solvers (see Lipp and Boyd (2014); Gurobi Optimization (2016); Nagy and Vajk (2018)).

### 1.4 Notation

We denote with the set of nonnegative real numbers. For a vector , denotes the component-wise absolute value of and we define the norms , . We also set .

For , we denote by the set of continuous functions from to that have continuous first derivatives. For , denotes the derivative and notation is used if is a function of time. We set . We say that is bounded if there exists such that .

Consider . We say that , if there exists a positive constant such that, for all sufficiently large values of , .

## 2 Problem formulation

Let be a smooth manifold of dimension that represents the configuration space of a robotic manipulator with -degrees of freedom (-DOF). Let be a smooth curve whose image set represents the assigned path to be followed by the manipulator. We assume that there exist two open sets , and an invertible and smooth function . Function is a local chart that allows representing each configuration with coordinate vector .

The coordinate vector of a trajectory in satisfies the dynamic equation

 D(q)¨q+C(q,˙q)˙q+ℓ(q)=τ, (2)

where is the generalized position vector, is the generalized force vector, is the mass matrix, is the matrix accounting for centrifugal and Coriolis effects (assumed to be linear in ) and is the vector accounting for joints position dependent forces, including gravity. Note that we do not consider Coulomb friction forces.

Let be a function such that and () . The image set represents the coordinates of the elements of reference path . In particular, and are the coordinates of the initial and final configurations. Define as the time when the robot reaches the end of the path. Let be a differentiable monotone increasing function that represents the position of the robot as a function of time and let be such that . Namely, is the velocity of the robot at position . We impose () . For any

, using the chain rule, we obtain

 q(t)=γ(λ(t)),˙q(t)=γ′(λ(t))v(λ(t)),¨q(t)=γ′(λ(t))v′(λ(t))v(λ(t))+γ′′(λ(t))v(λ(t))2. (3)

Substituting (3) into the dynamic equations (2) and setting , we rewrite the dynamic equation (2) as follows:

 d(s)v′(s)v(s)+c(s)v(s)2+g(s)=τ(s), (4)

where the parameters in (4) are defined as

 d(s)=D(γ(s))γ′(s),c(s)=D(γ(s))γ′′(s)+C(γ(s),γ′(s))γ′(s),g(s)=ℓ(γ(s)). (5)

The objective function is given by the overall travel time defined as

 tf=∫tf01dt=∫sf0v(s)−1ds. (6)

Let be assigned bounded functions and consider the following minimum time problem:

###### Problem 1.
 minv∈C1,τ∈C0 ∫sf0v(s)−1ds, (7) subject to (∀s∈[0,sf]) d(s)v′(s)v(s)+c(s)v(s)2+g(s)=τ(s), (8) γ′(s)v(s)=˙q(s), (9) γ′(s)v′(s)v(s)+γ′′v(s)2=¨q(s), (10) |τ(s)|≤μ(s), (11) |˙q(s)|≤ψ(s), (12) |¨q(s)|≤α(s), (13) v(s)≥0, (14) v(0)=0,v(sf)=0, (15)

where (8) represents the robot dynamics, (9)-(10) represent the relation between the path and the generalized position shown in (3), (11) represents the bounds on generalized forces, (12) and (13) represent the bounds on joints velocity and acceleration. Constraints (15

) specify the interpolation conditions at the beginning and at the end of the path.

The following assumption is a basic requirement for fulfilling constraint (12).

###### Assumption 1.

We assume that is a positive continuous function, i.e., () with .

Next assumption requires that the maximum allowed generalized forces are able to counteract external forces (such as gravity) when the manipulator is fixed at each point of .

###### Assumption 2.

We assume that , such that .

In fact for condition (11) reduces to () .

Problem 1 is nonconvex, but it becomes convex after a simple change of variables (as previously noted in Verscheure, Demeulenaere, Swevers, Schutter, and Diehl (2009)). Indeed, set

 a(s)=v′(s)v(s),b(s)=v(s)2, (16)

and note that

 b′(s)=2a(s). (17)

Then, Problem 1 becomes:

###### Problem 2.
 mina,τ∈C0,b∈C1 ∫sf0b(s)−1/2ds, (18) subject to (∀s∈[0,sf]) d(s)a(s)+c(s)b(s)+g(s)=τ(s), (19) γ′(s)a(s)+γ′′(s)b(s)=¨q(s), (20) b′(s)=2a(s), (21) |τ(s)|≤μ(s), (22) 0≤γ′(s)2b(s)≤ψ(s)2, (23) |¨q(s)|≤α(s), (24) b(0)=0,b(sf)=0, (25)

where the squares of the two vectors and in (23) are to be intended component-wise. Problem 2 is convex since the objective function (18) is convex and the constraints (19)-(25) are linear.

The following proposition (that will be proved in the appendix) shows that Problem 2 admits a solution.

###### Proposition 1.

Problem 2 admits an optimal solution , and moreover,

 ∫sf0b∗(s)−1/2ds≤U<∞,

where is a constant depending on problem data.

We do not directly solve Problem 2, but find an approximated solution based on a finite dimensional approximation. Namely, consider the following problem, obtained by uniformly sampling the interval in points from to :

###### Problem 3.
 minτ,a,b 2hn−1∑i=1⎛⎝1b1/2i+b1/2i+1⎞⎠. (26) subject to (i=1,…,n−1) (27) diai+cibi+gi=τi, (28) γ′iai+γ′′ibi=¨qi, (29) bi+1−bi=2aih, (30) |τi|≤μi, (31) |¨qi|≤αi (32) 0≤[γ′i]2bi≤ψ2i, (33) b1=0,bn=0. (34) b∈Rn a∈Rn−1,τi∈Rp, (35)

where , , , , , , , , and , with .

Thank to constraints (28)-(30), it is possible to eliminate variables and and use only , with as decision variables. The feasible set of Problem 3 is a non-empty set since is a feasible solution (in fact, it also has a nonempty interior).

Since Problem 3 is convex, we can easily find a solution with an interior point method (see Verscheure, Demeulenaere, Swevers, Schutter, and Diehl (2009)).

After solving Problem 3, it is possible to find an approximated solution of Problem 2. Indeed, by quadratic interpolation, we associate to a vector , solution of Problem 3, a continuously differentiable function such that the following relations hold for

 Ib(0)=b1,  Ib(sf)=bn,Ib((i−1/2)h)=bi+bi+12, i=1,…,n−1,I′b((i−1/2)h)=bi+1−bih, i=1,…,n−1. (36)

Namely, interpolates and at 0 and , respectively, and the average values of consecutive entries of at the midpoint of the discretization intervals. Moreover, the derivative of at the midpoints of the discretization intervals corresponds to the finite differences of .

We define the class of quadratic splines as the subset of , such that, for , is a quadratic polynomial. For , let be such that:

 p|[h(i−12),h(i+12)](t)=xi+yi(t−hi)+zi(t−hi)2,

and let be such that

 p|[0,h/2](t)=x1+y1t+z1t2

and

 p|[h(n−1/2),hn](t)=xn+yn(t−hn)+zn(t−hn)2.

For ease of notation, set, for ,

 bi−1/2=bi−1+bi2,   bi+1/2=bi+1+bi2δi+1/2=bi+1−bih,   δi−1/2=bi−bi−1h.

The following proposition, whose proof is presented in appendix, defines the interpolating quadratic spline fulfilling (36).

###### Proposition 2.

For any , there exists a unique element such that the following interpolation conditions hold

 p(h(i+12))=bi+1/2,i=1,…,n−1,p′(h(i+12))=δi+1/2,i=1,…,n−1,p(0)=b1,  p(sf)=bn. (37)

Note that is continuously differentiable and that (36) holds. Such element will be denoted by .

Note that by Proposition 2, there exists a unique function that interpolates the solution of Problem 3. Then and are computed from by using relations (17) and (19), namely we set

 a(s)=b′(s)2,τ(s)=d(s)a(s)+c(s)b(s)+g(s).

Functions , and are approximate solutions of Problem 2. Indeed, (26) and (30) are approximations of (18) and (21), moreover, functions , and satisfy, by construction, constraints (19)-(25) for and by continuity, (19)-(25) are also approximately satisfied for . By increasing the number of samples , the solutions of Problem 3 become better approximations of the solutions of Problem 2. It is reasonable to suppose that, as approaches to , the solutions of Problem 3 converge to the solutions of Problem 2. Anyway this convergence property is not proved in this paper being outside its scope. It can be proved on the lines of Consolini, Laurini, Locatelli, and Cabassi (2017b), that presents a convergence result for a related speed planning problem for an autonomous vehicle.

## 3 Solution algorithms and complexity issues for the generalized problem

In this section we present an optimal time complexity algorithm that solves a specific class of optimization problems. In the subsequent section we will show that Problem 2 can be approximated by a finite dimensional problem that belongs to this class.

### 3.1 Exact algorithm for the solution of some special structured problems

The problems under consideration have the form

 ming(v1,…,vn)vi≤fij(vi+1)i=1,…,n−1,j=1,…,rivi+1≤bik(vi)i=1,…,n−1,k=1,…,ti,0≤vi≤uii=1,…,n, (38)

where we make the following assumptions.

###### Assumption 3.

We assume:

• monotonic non increasing;

• , concave, increasing and ,
;

• , concave, increasing and ,
.

The constraints in (38) can be rewritten in compact form as follows:

 vi≤min{Bi(vi+1),ui}i=1,…,n−1,vi+1≤min{Fi(vi),ui+1}i=1,…,n−1,

where:

 Bi(vi+1)=minj=1,…,rifij(vi+1),Fi(vi)=mink=1,…,tibik(vi).

Note that and are both concave and increasing over , since they are the minimum of a finite number of functions with the same properties. We prove that the same holds for .

###### Proposition 3.

is increasing and concave over .

###### Proof.

The fact that is increasing follows immediately from the increasingness of and . For what concerns concavity, :

 Fi∘Bi(λx+(1−λ)y)=Fi(Bi(λx+(1−λ)y))≥Fi(λBi(x)+(1−λ)Bi(y))≥λFi∘Bi(x)+(1−λ)Fi∘Bi(y),

where the first inequality is a consequence of the concavity of and the fact that is increasing, while the second inequality comes from concavity of . ∎

It immediately follows that:

 Fi∘Bi(x)−x  concave,   Fi∘Bi(0)>0. (39)

Then, there exists at most one point such that . Similarly, there exists at most one point such that . Note that are the positive fixed points of and , respectively. Alternatively, is also the optimal solution of the following two-dimensional convex problem:

 maxvi+vi+1vi≤fij(vi+1)j=1,…,rivi+1≤bik(vi)k=1,…,ti.

The following result holds.

###### Proposition 4.

Under Assumption 3, the optimal solution of (38) is the component-wise maximum of its feasible region, i.e., if we denote by the feasible region, it is the point such that for all .

###### Proof.

See Consolini, Locatelli, Minari, and Piazzi (2017a); Nagy and Vajk (2018). ∎

We consider Algorithm 1 for the solution of problem (38).

The algorithm is correct, as stated in the following proposition.

###### Proposition 5.

Algorithm 1 returns the optimal solution of problem (38).

###### Proof.

We first remark that at each iteration holds. If a fixed point for exists, then after the backward propagation, , and , where denotes the upper bound for after the forward phase. We show that

 Fi(¯ui)=min{Fi(¯uoldi),Fi∘Bi(¯ui+1)}≥¯ui+1. (40)

If this is true for all , then the the point at the end of the backward phase is a feasible solution of (38). Indeed, by definition of in the backward phase,

 ¯ui≤Bi(¯ui+1),

while by (40),

 ¯ui+1≤Fi(¯ui),

so that is feasible for (38). Since holds and is monotone non increasing, we have that is the optimal solution of (38). We only need to prove that (40) is true. Note that is the result of the first forward propagation, so that . Thus, we need to prove that with . In view of (39), if the fixed point for exists, it is the unique nonnegative root of and

 Fi∘Bi(x)−x>0   ∀x≤¯vi+1,

from which the result is proved. Otherwise, if no fixed point exists,

 Fi∘Bi(x)−x>0   ∀x∈R+,

form which the result is still proved. ∎

Under a given condition, Algorithm 1 can be further simplified.

###### Remark 1.

If and , , fulfill the so called superiority condition, i.e.

 Fi(x),Bi(x)>x   ∀x∈R+,

then and the forward phase can be reduced to

 ¯ui+1=min{¯ui+1,Fi(¯ui)}.

### 3.2 Solving the subproblems in the forward phase and complexity results

We first remark that

 ¯ui=min{¯ui,Bi(¯ui+1),¯vi},¯ui+1=min{¯ui+1,Fi(¯ui),¯vi+1},

defined in the forward phase of Algorithm 1 are the solution of the two-dimensional convex optimization problem

 max vi+vi+1vi≤fij(vi+1)j=1,…,rivi+1≤bik(vi)k=1,…,ti0≤vi≤firi+1(vi+1)≡¯ui0≤vi+1≤biti+1(vi)≡¯ui+1. (41)

Alternatively, can also be detected as fixed points of and , respectively, where

 F¯ui(x)=min{¯ui,Fi(x)},B¯ui(x)=min{¯ui+1,Bi(x)}.

Although any convex optimization or any fixed point solver could be exploited for detecting these values, we propose the simple Algorithm 2, which turns out to be quite effective in practice. We denote by

 [Fi]−1(x)=maxj=1,…,ri{[fij]−1(x)}.

Note that increasing and concave implies that is increasing and convex and, consequently, is increasing and convex .

We illustrate how the algorithm works through an example.

###### Example 1.

Let

 bi1(x)=32x+2,bi2(x)=x+3,bi3(x)=12x+5,bi4(x)=8.

and

 [fi1]−1(x)=x−1,[fi2]−1(x)=92x−8,[fi3]−1(x)=5x−10.

Moreover, let . Then, we initially set .
In the first iteration we have

 ¯y =B¯ui(¯x1)=mink=1,…,4{bik(¯x1)}=min{14,11,9,8}=8,

with , and

 ¯z =[Fi]−1(¯x1)=maxj=1,…,3{[fij]−1(¯x1)}=max{7,28,30}=30,

with . Then, is the solution of the equation , i.e., (See also Figure 1). Figure 1: The first step of Algorithm 2. The red lines represent the linear function [fi¯j]−1 while the blue ones are the functions b¯ki. The green line represents the solution of the one dimensional equation fi¯j(bi¯k(x))=x.

In the second iteration we have

 ¯y =B¯ui(¯x2)=mink=1,…,4{bik(¯x2)}=min{375,335,345,8}=335,

with , and

 ¯z =[Fi]−1(¯x2)=maxj=1,…,3{[fij]−1(¯x2)}=max{135,415,8}=415,

with . Then, is the solution of the equation , i.e., (See also Figure 2).