Successive dynamic programming and subsequent spline optimization for smooth time optimal robot path tracking

The time optimal path tracking for industrial robots regards the problem of generating trajectories that follow predefined end-effector (EE) paths in shortest time possible taking into account kinematic and dynamic constraints. The complicated tasks used in industrial applications lead to very long EE paths. At the same time smooth trajectories are mandatory in order to increase the service life. The consideration of jerk and torque rate restrictions, necessary to achieve smooth trajectories, causes enormous numerical effort, and increases computation times. This is in particular due to the high number of optimization variables required for long geometric paths. In this paper we propose an approach where the path is split into segments. For each individual segment a smooth time optimal trajectory is determined and represented by a spline. The overall trajectory is then found by assembling these splines to the solution for the whole path. Further we will show that by using splines, the jerks are automatically bounded so that the jerk constraints do not have to be imposed in the optimization, which reduces the computational complexity. We present experimental results for a six-axis industrial robot. The proposed approach provides smooth time optimal trajectories for arbitrary long geometric paths in an efficient way.


Introduction
Highly automated production lines with efficient exploitation of the available resources become increasingly important for high wage countries to stay competitive.The utilization of present hardware plays a central role.This can be achieved by an intelligent path planning, using mathematical models of the mechanics to consider technical constraints in an optimization for the determination of optimal motions.It is an established approach to divide the problem into the geometric path planning using a scalar path parameter s and optimization (Bobrow et al., 1985;Pfeiffer and Johanni, 1987).The path planning can for instance be done by combining lines and circles via clothoids.A popular and comfortable way is the definition of the path using polynomials or splines (Geu Flores and Kecskemethy, 2012;Gattringer et al., 2014).In Quang-Cuong (2014) a classification is proposed to divide optimization strategies deriving an optimal trajectory in the 2-D-phase space with coordinates (s, ṡ) into three families: 1. Dynamic programming (DP): the phase space is divided into a discrete grid.Based on Bellman's optimality principle (Bellman and Dreyfus, 1962) an optimal trajectory is derived on this grid (Shin and McKay, 1985).
2. Numerical integration: the optimal solution for the path parameter time evolution is obtained by integrating maximum and minimum accelerations and finding optimal switching points for this acceleration and deceleration periods in the phase plane (Bobrow et al., 1985;Pfeiffer and Johanni, 1987).Geu Flores and Kecskemethy (2012) used this method to solve the socalled generalized waiter motion problem.
3. Convex optimization: the third method is based on a convex formulation of the optimization problem that can be solved with efficient optimization packages (Verscheure et al., 2009;Ardeshiri et al., 2011).
Published by Copernicus Publications.
The tasks to be performed in industrial applications often contain long paths with rough as well as fine contours.Furthermore, the movement should be precise in order to comply with required accuracies on the one hand, and the motor torques should be smooth in order to avoid vibrations and to protect the hardware.Short optimization times are also crucial in order that the saving in execution time is not annihilated by increased calculation times.
To overcome these demands, one may start to optimize the whole path using one approach mentioned above considering kinematic (joint velocity and acceleration) and dynamic (torque) constraints.Smooth trajectories can be achieved by taking jerk or torque rate restrictions into account.In Constantinescu and Croft (2000) and Oberherber et al. (2014) methods to consider them in phase space are presented, while Debrouwere et al. (2013) proposes a sequential convex scheme to solve a nonlinear program.However, for standard six axis industrial robots and long geometric paths the calculation effort is enormous due to the high number of optimization variables and restrictions.This is also caused by the fine discretization, required for a detailed implementation of fine structures of the geometric path.A non-equidistant discretization, where only certain regions of the path are discretized finely, would be a first choice to reduce the size of the problem.But, if the path is very long and contains many finely discretized regions, this approach is not feasible.Another way to reduce the number of optimization variables is a decomposition of the path into segments, performing the optimization for the segments and assembling the solutions to the whole optimal trajectory.In order to achieve a continuous trajectory, terminal conditions have to be defined at the intersection points and regarded in the optimization.We use a DP approach to calculate optimal trajectories for each segment and subsequently for the entire path.For this task we consider restrictions of path velocity, joint velocity and acceleration and also torque limits.Neglecting the jerk restrictions is reflected in a bang-bang behavior of the motor torques.
To obtain smooth robot movements in short optimization times, we approximate the solution provided by the DP approach for each segment of the path with a spline.Subsequently, this approximation is used as initial guess for the optimization of the spline using an active-set solver (Nocedal and Wright, 2006), considering the same restrictions as before.The degree of the splines and the number of control points determine the smoothness of the solution.Since the initial state is near to the global optimum, the calculation times remain low.
As the initial state for the spline optimization is derived by a DP approach, the first part can be assigned to category one.A categorization of the spline optimization is not really possible, since we use an active set solver to optimize the non-convex problem.
The paper is organized as follows: in Sect. 2 the geometric path, describing the task the robot should perform, is defined.
Section 3 treats the time optimal path planning problem in the parameter space in a general way, Sect. 4 introduces our solution strategy.Section 5 addresses the used optimization based on DP in detail.An approach to attain smooth trajectories with short calculation times is introduced in Sect.6. Experimental results, realized on a Stäubli RX130L -a sixaxis industrial robot, are presented in Sect.7.
In this paper we use the abbreviation ( ) = d dt for the time derivative of a quantity.The derivative with respect to the path parameter is denoted with ( ) = d ds .Bold lower case letters characterize vectors, while capital letters are used for matrices with the exception of commonly used notations in mechanics.The euclidean norm of the vector x is denoted with x .

Geometric path planning
The aim of path planning is the definition of the robot's task.There are several methods to define such a geometric path, like polynomials, combination of lines, circles and clothoids (Müller et al., 2007) or splines (Bobrow, 1988;Gattringer et al., 2014).The latter provide a comfortable way to define the robots motion whether in joint coordinates q or in Cartesian coordinates z E .To consider obstacles in the workspace, it is common to define the geometric path in the Cartesian space as shown in Fig. 1.
A three-dimensional spline curve, describing the endeffector position can be written as with the scalar path parameter s = [s B , s E ] (s B -begin of the path, s E -end of the path), and the n D control points d l defining the shape of the curve.These control points can either be defined directly or can be determined using interpolation points p l as shown in Gattringer et al. (2014).N d l (s) denote the B-spline basis functions of degree d.There are different ways to define this basis functions as local or global support functions.For details we refer to De Boor (1978) and Piegl and Tiller (1995).Unit Quaternions Q = e 0 , e T T (scalar part e 0 and vector part e) are used for the definition of the end-effector orientation Q E .The evolution of the separate coordinates e 0 , e x , e y and e z along the path is again defined via splines with a subsequent normalization.The angular velocity of the end-effector, represented in the end-effector frame, can be calculated to E ω E = 2 [e 0 ė − eė − e ė0 ].

Problem description
In Sect. 2 the geometric path is defined as a function of the scalar path parameter s.The goal of this section is to find an optimal relationship between the path parameter and time: , 6, 245-254, 2015 www.mech-sci.net/6/245/2015/q 1 q 2 q 3 q 4 q 5 q 6 d 0 Figure 1.Six-axis industrial robot with geometric path.
s(t).This is accomplished with an optimization considering technical constraints like motor-torque, velocity and acceleration restrictions.The general formulation of the path tracking problem is given by wherein τ denotes the vector of motor torques as a function of time.With the coefficients k 1 and k 2 a weighting between time and energy optimality can be achieved.Basically we are able to handle this general case, but in this paper we concentrate on time optimal solutions, therefore the factors k 1 = 1 and k 2 = 0 are used and the torques vanish from the cost function.The cycle time t E represents the solution of the optimization and is consequently an unknown quantity.
A change of the integration variable from t to s leads to and the optimization problem can be written as ds. (4)

Process constraints
For the trajectory optimization several technical constraints should be considered.The first one concerns the end-effector velocity v E = dr E dt , which is a process related restriction.Such constraints can be found in grinding or welding operations.By using the chain rule the path velocity follows as in the parameter range.

Manipulator constraints
Restrictions imposed by the used hardware concern the motor torque, joint velocity, and joint acceleration.In Sect. 2 the path is determined in Cartesian coordinates whereas these restrictions are defined in joint coordinates.Since the endeffector coordinates are calculated with the forward kinemat- for desired joint positions, the inverse kinematics q(s) = f −1 (z E (s)) provides the joint angles for desired end-effector coordinates.There are different ways to solve this locally, but not globally unique problem.We use a numerical approach based on the relation with the end-effector velocities v E , ω E represented in the inertial frame and the Jacobian With the chain rule Eq. ( 5), the joint velocity and acceleration follow as q = dq ds ṡ = q ṡ (9) The end-effector prime quantities are calculated, using and the Jacobian J In order to formulate the torque restrictions, a dynamic model of the robot is necessary.It is derived with the help of the Projection Equation (Bremer, 2008) resulting in the Equations of Motion (EoM) wherein M(q) is the position dependent, symmetric and positive definite mass matrix.All other generalized forces like Coriolis-, gravitational-, centrifugal-and friction forces are contained in g.The motor torques are represented by τ .As well as the velocity and acceleration restrictions Eqs. ( 6), ( 11), ( 12), also the torque restrictions are required in terms of the path parameter.There exist different ways to express the EoM in terms of the path parameter as those proposed in Johanni (1988) and Geu Flores and Kecskemethy ( 2012).An analytical formulation is shown in Gattringer et al. (2014), where the parametrized EoM are written as The coefficients a, b, c and d v of the parametrized EoM follow directly by rewriting and parametrizing the Projection Equation.
By introducing the variable z = ṡ2 the optimization problem in parameter space follows as The values for the motor torque restrictions τ min , τ max and joint velocity restrictions qmin , qmax can be taken from the data sheets of the motors respectively gears.Generally the lower limits are equal to the negative values of the upper limits: τ min = −τ max and qmin = − qmax .The same applies to the acceleration restrictions qmin = − qmax , which are usually defined in the joint controllers.Depending on the process to be performed, the path velocity limit v E,max is set to the optimal working speed.
The better the robot model matches the real system, the better the limits can be exploited.Primarily parameters that are necessary to simulate the kinematics, like link lengths or distances between axis can be taken from CAD -data.The parameters for the dynamic model -masses, centers of gravity or inertia tensors can usually only be estimated with CAD models.For a good match of the derived robot model with the real system, a parameter identification as shown in Neubauer et al. (2014) and Swevers et al. (1996) is indispensable.

General solution strategy
The requirements of smooth time optimal trajectories for long geometric paths and short computation times were discussed in Sect. 1 as well as our idea to overcome this challenge.A reduction of optimization variables for a particular optimization is achieved by dividing the path into sections and performing the optimization for these segments.To get smooth trajectories in acceptable calculation times, we define the evolution of z(s) in the "phase space" s × z as a smooth spline curve and optimize its shape with an active-set solver.
For this kind of problem a suitable initial guess z 0 (s) is crucial but is not always easy to define.In Verscheure et al. (2009) a parabola is used.This approach works reasonably well for zero velocity at the begin and end of the path, but is not guaranteed to work in the general case of desired start and end velocities.The division of the path into segments makes their consideration mandatory to achieve a continuous overall trajectory.For that purpose we propose an elegant way to derive the initial states by using a DP approach considering the terminal conditions and approximate its solution with a spline curve.Since the DP approach provides the global optimum, it yields an excellent approximation.However, the discretization for this algorithm can be chosen coarse in order to save computation time, since it only provides the initial states for a further optimization of the spline curve.
The piecewise optimization procedure is sketched in Fig. 2. It starts with the definition of an optimization s o and a prediction horizon s p .From s B to s o,1 + s p,1 = s 1,e the optimization is performed and provides an optimal trend, that is split into z opt,1 and z pr,1 .Along the optimization horizon the optimal trajectory is stored and used as a part of the optimal trajectory of the whole path.The prediction horizon is necessary to determine the terminal conditions.At the end of the optimization horizon z(s 1 ) and z (s 1 ) are stored as terminal conditions.Afterwards the horizons are shifted forward by s o,1 and the next optimization starts at s 2,b and leads to s 2,b + s o,2 + s p,2 = s 2,e under consideration of z(s 1 ), z (s 1 ).This procedure is repeated till the end of the path is reached.In further consequence we will call the sequential execution of the algorithm successive dynamic programming (SDP).
The length of the horizons should be chosen carefully.A short optimization horizon in combination with a long prediction horizon leads to unnecessary long calculation times since the solution of the prediction horizon is discarded.Conversely a long optimization horizon and a short prediction . Graphical illustration of technical restrictions in the z×z plane.
horizon effects unnecessary deceleration phases.A general choice can not be proposed, rather they have to be chosen problem dependent.
5 Dynamic programming approach to determine an initial solution

Graphical illustration of restrictions
For a fix point s i on the path the restrictions in Eqs. ( 16)-( 19) can be graphically illustrated in the z × z plane.This is exemplary shown in Fig. 3 for k = 2 degrees of freedom and with the assumption that the lower restrictions are equal to the negative upper restriction values.
The velocity restrictions can be squared and combined to representing a vertical line in the z × z plane.For each joint the acceleration limits follow to and thus to linear functions in the z × z plane.Due to the consideration of viscous friction (coefficient d v ) each joints torque restriction follows to and thus to nonlinear functions in the z × z plane.The set of curves provide a feasible region (shaded in Fig. 3) for the states.Significant points are given by the minimum and maximum feasible values of z, denoted with z min (s i ) and z max (s i ).In Fig. 3 z min (s i ) is equal to zero.Since a numerical method is used to derive the feasible region it is also possible to consider values z min (s i ) > 0, as they occur for example at the so-called waiter motion problem in Geu Flores and Kecskemethy (2012).z max (s i ) can either be given by the intersection point of the lowest upper with the highest lower restriction or can concur with the velocity restriction z L .Within the range z = z min . ..z max (s i ) the feasible region defines the minimum z (z) and maximum z (z) allowed gradients.

Discretizing the problem
The DP approach is based on Bellman's optimality principle."[...]An optimal policy has the property that whatever the initial state and the initial decision are, the remaining decisions must constitute an optimal policy with regard to the state resulting from the first decision (Bellman and Dreyfus, 1962) With a process running backwards, these decisions can be picked up and optimized with respect to a desired optimality.For that purpose the first step is to discretize the whole path into n segments s = s B . ..s i . ..sE with a discretization step size s = s E −s B n and i = 0. ..n.Then the optimization horizon s o with n o discrete points and prediction horizon s p with n p discrete points are chosen as integral multiples of s.In the following only one segment s = s b . ..s e is considered, where the start point is denoted with s b and the endpoint with s e .Afterwards we evaluate the minimum z min (s i ) and maximum admissible values z max (s i ) at all discrete points as shown in Fig. 3 resulting in the limiting curves z min (s) and z max (s), that provide the base of operations.Further z is discretized with the step size z = z max (s i )−z min (s i ) m into m pieces in the range z = z min (s i ). ..z min (s i )+j z. ..z max (s i ) with j = 0. ..m.The treatment of the path sections leads to a discrete (n o + n p ) × m grid instead of a n × m grid (n o + n p < n) in the phase plane.Within this grid also the cost functional from Eq. ( 15) has to be discretized to with n b = s b / s and n e = s e / s.

Successive dynamic programming
The backwards running process starts at the paths end by initializing the cost function W . Starting from a desired end velocity represented by z e the gradients to all velocity points z e−1,j on the path point s e−1 are calculated with Subsequently they are compared to the minimum z e−1,j and maximum z e−1,j allowed gradients at these points, provided www.mech-sci.net/6/245/2015/Mech.Sci., 6, 245-254, 2015 by Fig. 4. The cost function is initialized with W e−1,j = s √ z j at points with valid gradients (green solid in Fig. 5) and with W e−1,j → ∞ for points with invalid gradients (green dashed).With the cost function on hand the proceeding can be continued for the path points s e−2 . ..s b .At these points the cost function has to be considered in the calculation of the optimal gradients.This is accomplished by determining the minimum and maximum allowed gradients z i,j and z i,j using Fig. 4 of the actual point z i,j and calculating the covered range [z i,j , z i,j ] at the following path point s i+1 .Within this range along the z axis the location of the cost functions minimum z * i,j has to be found.A popular procedure is a minimum search based on the golden ratio.However, we take advantage of its special shape for purely time optimal trajectories W = s √ z .A closer examination shows that the minimum is located at the highest value of z which leads to a feasible value different to ∞.This step is reflected in a strongly reduced computation time.With z * i,j the optimal gradient follows to z i,j = z * i,j −z i,j s and the cost function is adapted to W i,j = W i+1,j + s √ z i,j or W i,j → ∞ if it is out of range.The whole procedure has to be executed for every discrete point at the remaining n e − n b − 2 × m grid.After completion of the optimal gradient determination, the optimal evolution of z(s) can be calculated iteratively with z opt,i+1 = z opt,i + z i,j s (blue in Fig. 5), starting with a desired start value z opt (s b ) = z b and a valid gradient z (s b ) = z b provided by the terminal conditions.

Local spline approximation
The solution provided by the DP approach in Sect. 5 leads to a bang-bang -behavior in the motor torques, resulting in heavy stress for the actuators and the mechanics.In Oberherber et al. (2014) an approach is proposed, that considers torque derivative and joint jerk restrictions in the optimization.This extension of the DP algorithm results in long calculation times and is thus not feasible for long paths.For this reason, we propose a different way in this paper to obtain smooth trajectories.We approximate the optimal evolution of z opt (s) derived by the dynamic programming algorithm, with a spline curve.
In a first step the trend of z(s) is expressed as a spline whose nD control points d0 follow from a least squares approximation minimizing the error between optimal and approximated trend at the discrete points s = s b , s b + s. ..s i , . ..s e of the DP algorithm.The spline ẑ0 (s) is discretized to the originally demanded fine discretization ŝ = s E −s B n with n > n.

Optimization problem
Since the approximation does not respect any restrictions, local violations are the consequence.Therefore an optimization of the control points, using d0 as initial states, is performed in order to satisfy the restrictions at all discrete points.The optimization problem that has to be solved to fulfill the restrictions is qmin ≤ q ẑ ≤ qmax (29) with nb = s b / ŝ and ne = s e / ŝ. Figure 6 shows the SDP solution z opt and the approximation ẑ0 (s) with the control points d 0 used as initial states for the optimization.The smooth time optimal trajectory ẑopt (s) follows from the optimal control points d provided by the optimization Eqs. ( 27)-(31).

Gradients
For a fine discretization, the restriction check at every discrete point is computationally expensive.A significant calculation time reduction can be achieved by providing analytical expressions for the gradients of the cost functional and restrictions with respect to the optimization variables.There are actually software tools to compute gradient and Hessians using automatic differentiation.Nevertheless, as the restrictions and the objective are given as analytical functions of the optimization variables, the gradients can easily be calculated analytically.By inserting the spline ẑopt (s) = nD l=1 N d l (s) dl into the discrete cost functional its gradient regarding the optimization variables dl follows to Inserting the spline and its derivative with respect to the path parameter ẑ opt (s) = nD l=1 N d l (s) dl into the restrictions, their gradients regarding the optimization variables dl follow to

Terminal conditions
To achieve a continuous trajectory a consideration of the terminal conditions for the spline optimization is necessary, as with the DP algorithm.For this purpose the first two control points have to be calculated separately.The first control point is defined by the terminal condition for z(s b ) while the second control point follows to for a transition gradient z (s b ) and the derivatives of the first two basis functions N d 1 and N d 2 with respect to the path parameter If also transition conditions z(s e ) = z e and z (s e ) = z e at the end of the path are required, the same procedure also works for the last and second last control point.
The definition of z(s) as spline entails a further advantage namely an easy way to achieve a smooth start and stop.Jerky accelerations at the beginning and decelerations at the end of the path lead to end-effector vibrations which are problematical especial for elastic systems since they need a long time to settle to the desired endpoint.The definition of z (s b ) = 0 leads to a smooth start while z (s e ) = 0 provides a smooth stop.

Results
The experiments are realized with a Stäbli RX130L, a sixaxis industrial robot.It is controlled by a Bernecker und Rainer system with PD controller and torque feed forward control for each joint.We use a spline curve of degree d = 4 in form of our institute logo (a robin), shown in Fig. 7, as geometric path.It is about l ≈ 7.8 m long and is discretized into n = 2000 pieces to represent even the fine contours.The end-effector orientation is held constant equal to the initial orientation, so that an observer directly faces the robots endpoint.
For the calculation of the initial solution for the spline optimization with the SDP algorithm the velocity discretization amounts m = 300, while the path is discretized into n = 250 in the range s B = 0. ..sE = 1.As horizons s o = 0.2 (50 segments) and s p = 0.08 (20 segments) are defined which lead to a subdivision of the total path into five segments.
Figure 8 shows the phase plane s ×z with the liming curve z max , the optimal evolution z opt provided by the SDP approach and the smooth optimal trend ẑopt .This spline curve of degree d = 4 contains nD = 40 control points for each path segment.
A comparison of the motor torques, discontinuous (provided by the SDP approach) and smooth, is given by Fig. 9.The basic behavior looks very similar, with the exception of a smooth start and stop, where the torques run into the static torques.The difference becomes clearer by looking at the torque rates in Fig. 10 and the joint jerks in Fig. 11. Figure 10 shows, that the torque rates of the discontinuous trajectory are approximately ten times higher then the torque rates of the smooth solution with nD = 40 control points for each section.
The smoothness of the trajectory contrasts with an increasing execution time from t E ≈ 3.72 s for the bang bang solution to t E ≈ 4.02 s for the smooth solution with nD = 40 control points for each section.This smooth trajectory was successfully implemented in simulations as well as on the real system, a video clip of the implementation is available on https://youtu.be/c5jllkLE4oU.A reduction of the number of control points to nD = 25 leads to a smoother solution, but increases the execution time to t E ≈ 4.22 s. Figure 11  reduction of the number of control points increases the execution time but reduces the joint jerks.
For validation purposes we implemented an optimization with hard jerk constraints ... q max = 3000 rad s 3 , considered in the spline optimization.This optimization converges only for a low number of control points up to nD = 25.The calculation time rises to t CPU ≈ 65 s, while the execution time t E ≈ 4.13 s is nearly the same as without jerk restrictions.
Finally we tried to calculate a time optimal trajectory for the whole path in one go, with nD = 100 control points.For that purpose we implemented two different approaches to derive an initial guess.A coarse discretized DP approach along ... q in rad/s 3 time t in s ... q in rad/s 3 ... q 6 ... q 5 ... q 4 ... q 3 ... q 2 ... q 1 ... q in rad/s Both approaches require a coarse discretization of the path ( n = 1250) to achieve a convergence of the spline optimization.Despite the coarse discretization, the calculation times increase clearly.The slightly smaller execution times t E can be attributed to the coarser discretization.
The results of the different methods are listed in Table 1.In this table, SO is the abbreviation for spline optimization and g indicates the usage of analytical gradients.Jerk suggests the consideration of hard joint jerk restrictions in the spline optimization.The last two lines of Table 1 show the results for the optimization in one go with the DP and parabola approach for the initial guess.
The spline optimization was done with the active-set algorithm of the Matlab optimizer fmincon.With the time t CPU we indicate the computation time on a standard PC with a CPU clock of 2.83 GHz.The results in Table 1, clearly indicate the improvements of the presented approach compared to an approach with jerk constraints.Nevertheless, the calculation times are significantly higher compared to the execution times.An improvement could for example be achieved by implementing the optimization not in MATLAB, but in a C-based optimization toolbox.

Conclusions
This paper presents an approach to derive smooth time optimal trajectories for arbitrary long geometric paths.The main idea is to split the path into sections, to calculate optimal trajectories using terminal conditions, and to assemble the solutions for the individual segments.To achieve smooth trajectories in acceptable calculation times we propose a spline optimization in the phase space.The problem of convenient initial states for the optimization is solved with a DP approach in which terminal conditions can be considered in an easy way.With the spline optimization it is also simple to achieve a smooth start and stop of the robot.The presented approach may be interesting for robot manufacturers which already have algorithms for the path tracking problem and want to extend them to achieve smooth trajectories.Experiments to show the proper functionality of the method are realized on a six-axis industrial robot.An extension of the algorithm to consider jerk and torque rate restrictions in the optimization will be part of future work.

Figure 4 .
Figure 4. Feasible region in the z × z plane with max z j and min z j at z j .

Figure 7 .Figure 8 .
Figure 7. Geometric path -logo of the Institute of Robotics at the JKU Linz.

Figure 9 .Figure 10 .
Figure 9. Motor torques, above: torque trends for the SDP solution, below: torque trends for the smooth solution.

Figure 11 .
Figure11.Joint jerks, above: trend of joint jerks for the SDP approach, middle: trend of joint jerks for a smooth solution with nD = 40 control points for each section, below: trend of joint jerks for a smooth solution with nD = control points for each section.

Table 1 .
Trajectory execution times t E and calculation times t CPU for the different methods.