Marco Faroni
email: marco.faroni@stiima.cnr.it

Manuel Beschi
email: manuel.beschi@unibs.it

Antonio Visioli
email: antonio.visioli@unibs.it

Nicola Pedrocchi
email: nicola.pedrocchi@stiima.cnr.it

A real-time trajectory planning method for enhanced path-tracking performance of serial manipulators

Keywords: Motion planning, trajectory scaling, path following, joint constraints, robot manipulators

published or not. The documents may come

A real-time trajectory planning method for enhanced path-tracking performance of serial manipulators

Introduction

Recent advances in industrial robotics require robots to work in unstructured environments or to cooperate with other agents (e.g., human operators). This has renewed the interest in motion planning algorithms that cope with dynamic situations. Motion planning of industrial manipulators is typically structured as follows:

Step 1: A collision-free path that connects the initial and the final (and possible intermediate) configurations is computed.

The path has to be consistent with the joint range limits and the robot reachable workspace. The path can be defined manually in the case of point-by-point programming, but it is more often computed by means of path-planning algorithms. Among them, sampling-based path planners are the most widespread [START_REF] Elbanhawi | Sampling-based robot motion planning: A review[END_REF][START_REF] Gammell | Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[END_REF].

Step 2: A time parametrization is assigned to the path. The result of this step is a velocity profile along the path. Such profile should respect the robot limits (e.g., joint maximum velocity, acceleration, and/or torque) and process requirements (e.g., constant-speed traits in gluing and laser-cutting operations).

Step 3: An open-loop controller modifies the nominal trajectory based on real-time information from the process during the execution of the motion. This step may include real-time trajectory re-planning [START_REF] Zhang | Real-time trajectory planning based on joint-decoupled optimization in human-robot interaction[END_REF][START_REF] Zhang | Dynamic trajectory planning of a 3-dof under-constrained cable-driven parallel robot[END_REF], safety speed modulation for human-robot collaboration [START_REF] Byner | Dynamic speed and separation monitoring for collaborative robot applicationsconcepts and performance[END_REF][START_REF] Marvel | Implementing speed and separation monitoring in collaborative robot workcells[END_REF] or speed coordination with conveyors and other robots.

The resulting motion is then tracked by a closed-loop controller. Note that this paradigm is not strict. For example, Steps 1 and 2 may merge if the path planner also takes into account the robot velocity and acceleration limits (these planners are usually referred to as kinodynamic [START_REF] Elbanhawi | Sampling-based robot motion planning: A review[END_REF]). Also, Step 3 may not be present in simple applications.

Steps 1 and 2 are computed before the execution of the motion. In static applications (i.e., without cooperation with other agents and in structured environments), they may be performed offline by using, for example, optimization-based algorithms [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF][START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF][START_REF] Gallant | Extending the capabilities of robotic manipulators using trajectory optimization[END_REF] or by shaping high-order polynomials [START_REF] Huang | Optimal time-jerk trajectory planning for industrial robots[END_REF][START_REF] Boryga | Planning of manipulator motion trajectory with higher-degree polynomials use[END_REF][START_REF] Dinc ¸er | Improved trajectory planning of an industrial parallel mechanism by a composite polynomial consisting of bézier curves and cubic polynomials[END_REF]. In the presence of dynamic obstacles and goals, they need to be performed just before the robot movement and computational time is therefore a key issue. Step 3 runs during the execution of the motion; computing time requirements are therefore strict as the algorithm has a sampling period comparable with the robot controller's one.

As mentioned, the output of Step 2 is a velocity profile along the path that respects the robot limits. However, finding an optimal solution to the path-parametrization problem in Step 2 is time consuming and not compatible with many applications that do not allow for offline trajectory planning. For example, in many manipulation scenarios, grasping poses of the objects are not fixed, so they are calculated based on a vision system. Feasible trajectories are therefore planned on the fly, just before their execution, and planning time is a critical point. Similar issues arise in human-robot collaboration and multi-agent systems, where online re-planning is often required. A common approach is to use approximated methods in Steps 1 and 2 and to refine the motion online in Step 3. In this way, part of the computational burden can be moved to

Step 3, reducing latency of the trajectory execution. In other words, Steps 1 and 2 compute an approximated trajectory that does not take into account kinodynamic constraints. The algorithms in Step 3 are then in charge of modifying the trajectory iteratively at runtime, to make it compliant with the robot limits at the current sampling period.

Another issue addressed in Step 3 is the fast reaction to the behavior of other agents (e.g., human co-workers) and changes in the environment. Real-time path re-planning may be adopted, but the resulting planning problem is usually too complex to be solved with small latency. In this regard, anytime sampling-based motion planners allow for collision-free online path re-planning, but they are far from being implementable with sampling periods in the order of few milliseconds [START_REF] Gammell | Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[END_REF][START_REF] Abbasi-Yadkori | Extending rapidly-exploring random trees for asymptotically optimal anytime motion planning[END_REF]. Another approach consists in the modification of the velocity profile along the same path. Preserving the nominal path leads to two advantages: i) the technological application is not jeopardized1 ; and ii) there is no need for re-computing a collision-free path, which was the computational bottleneck of the aforementioned approach. For these reasons, online speed reduction is also the preferred way to ensure safety in human-robot collaboration. For example, Speed and Separation Monitoring and Power Force Limitation modes, as defined in the safety standard ISO/TS 15066 [START_REF]2016 Robots and robotic devices -Collaborative robots, Standard, International Organization for Standardization[END_REF], are implemented by slowing down the robot speed according to safety constraints from the standard. This has usually been implemented by means of danger zones where the robot speed is limited [START_REF] Matthias | Example application of iso/ts 15066 to a collaborative assembly scenario[END_REF], but recent more advanced strategies tend to modulate the speed dynamically depending on the human-robot distance and the safety requirements defined by the standard (namely, the minimum protective distance and the maximum impact force) [START_REF] Byner | Dynamic speed and separation monitoring for collaborative robot applicationsconcepts and performance[END_REF]. These advanced approaches might take advantage of algorithms capable of taking into account robot physical limitations during the deceleration phase.

Related works

In many cases, it is desirable to preserve the nominal geometrical path defined by the technological process the robot is performing. Real-time algorithms can modify the nominal velocity profile to preserve the path when the trajectory in execution is expected to exceed the robot physical limitation or to avoid collisions with humans and other robots. The methods devoted to this purpose are referred to as online trajectory scaling methods.

Different approaches have been proposed in the literature. Most of them approximate the planning problem by only taking into account the desired motion at the current time instant to reduce the computational effort as much as possible. For this reason, they are referred to as local or non-look-ahead. Examples can be found in [START_REF] Antonelli | A new on-line algorithm for inverse kinematics of robot manipulators ensuring path tracking capability under joint limits[END_REF], in which a scaling factor that multiplies the nominal velocity is calculated at each iteration of the task. Such scaling factor can slow down the trajectory execution in case it does not satisfy the joint velocity or acceleration limits. In [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF], this principle is extended to redundant manipulators under joint kinematic limits. Another strategy is proposed in [START_REF] Lo Bianco | Online trajectory scaling for manipulators subject to high-order kinematic and dynamic constraints[END_REF][START_REF] Lo Bianco | A discrete-time filter for the generation of signals with asymmetric and variable bounds on velocity, acceleration, and jerk[END_REF][START_REF] Lo Bianco | A scaling algorithm for the generation of jerk-limited trajectories in the operational space[END_REF], where the scaling architecture is based on a nonlinear filter that modifies the longitudinal velocity profile based on the activation of the joint constraints. In case of infeasibility, path deformations arise and the filter is in charge of converging to the original task in minimum time. The main disadvantage of the non-look-ahead methods is that they only take into account information about the current state of the system. As a consequence, it is often impossible to prevent saturation of the robot joint controller. This results in path-following errors. Heuristic strategies were devised in [START_REF] Lo Bianco | Techniques to preserve the stability of a trajectory scaling algorithm[END_REF] to partially address this problem in the methods described in [START_REF] Lo Bianco | Online trajectory scaling for manipulators subject to high-order kinematic and dynamic constraints[END_REF]. Such rules need, however, to be tuned empirically and their effectiveness strongly depends on the trajectory to be performed.

Look-ahead strategies have been also proposed [START_REF] Faulwasser | Implementation of nonlinear model predictive path-following control for an industrial robot[END_REF][START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF][START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF] to overcome this issue. They typically set up a constrained nonlinear optimal control problem to minimize the path deviation along a predictive time horizon. The pathparametrization variable is used as an additional control variable to determine the optimized timing law. They give better results if compared with non-look-ahead techniques, but they require much higher computational effort to solve the optimal control problem online. In fact, their implementation can be troublesome on industrial controllers because of the limited computing resources and mathematical software tools.

Online trajectory scaling has also been adopted for safe human-robot collaboration. In [START_REF] Zanchettin | Safety in human-robot collaborative manufacturing environments: Metrics and control[END_REF], linear safety constraints in the robot velocity are designed to avoid human-robot collisions; in [START_REF] Lippi | Safety in human-multi robot collaborative scenarios: a trajectory scaling approach[END_REF], a safety function is related to the scaling of the trajectory; in [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF], a receding-horizon approach scales the trajectory based on the human-robot relative distance. In these works, trajectory scaling is a tool used to slow down the trajectory to a safe speed according to the human proximity.

The trajectory scaling algorithms used in these works trace back to those described in the aforementioned approaches. In particular, [START_REF] Zanchettin | Safety in human-robot collaborative manufacturing environments: Metrics and control[END_REF] and [START_REF] Lippi | Safety in human-multi robot collaborative scenarios: a trajectory scaling approach[END_REF] use non-look-ahead methods; [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF] uses the look-ahead MPC-based approach described in [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF].

Contribution

The paper proposes a novel technique that can be embedded in non-look-ahead scaling techniques to improve their performance. The proposed method takes into account the robot velocity, acceleration, and torque limits and modifies the velocity profile (output of Step 2) based on an approximated look-ahead criterion. As a result, the maximum admissible robot speed is given as new reference to a non-look-ahead trajectory scaling method. This confers predictive features on the existing scaling algorithm. The advantages of the proposed approach compared to existing techniques are that:

• The proposed technique can be integrated with existing scaling algorithms as it only requires to modify the nominal velocity profile before using it in any scaling methods.

• The additional computation burdens with respect to non-look-ahead algorithm are negligible (as only few simple calculations are needed) and, thus, way lower than existing optimization-based look-ahead methods [START_REF] Faulwasser | Implementation of nonlinear model predictive path-following control for an industrial robot[END_REF][START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF][START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF].

• The performance in terms of path-following error and execution time are significantly better than non-look-ahead approaches (e.g., [START_REF] Antonelli | A new on-line algorithm for inverse kinematics of robot manipulators ensuring path tracking capability under joint limits[END_REF][START_REF] Lo Bianco | Online trajectory scaling for manipulators subject to high-order kinematic and dynamic constraints[END_REF][START_REF] Lo Bianco | A discrete-time filter for the generation of signals with asymmetric and variable bounds on velocity, acceleration, and jerk[END_REF][START_REF] Lo Bianco | A scaling algorithm for the generation of jerk-limited trajectories in the operational space[END_REF][START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF]), as proven by simulation and experimental results.

• The method is not optimization-based, which is an advantage with respect to existing look-ahead methods [START_REF] Faulwasser | Implementation of nonlinear model predictive path-following control for an industrial robot[END_REF][START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF][START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF] in terms of ease of implementation, especially on industrial controllers, often characterized by limited computational capabilities.

• Velocity, acceleration, and torque limits are taken into account at the same time (note that [START_REF] Faulwasser | Implementation of nonlinear model predictive path-following control for an industrial robot[END_REF] does not include velocity and acceleration limits and [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] does not include torque limits).

Simulation and experimental tests are presented to prove the effectiveness of the method. Complex operational-space tasks are performed by a 6-degree-of-freedom Universal Robot UR10 robot. A comparison with look-ahead and nonlook-ahead methods is also provided. A video also accompanies the paper to show the experimental tests and clarify the practical consequences. The paper is organized as follows. Section 2 introduces preliminary notions on online trajectory scaling. In Section 3, the method to compute the modified velocity profile is described. Both joint-and operational-space trajectories are addressed. Simulation and experimental results on a 6-degree-of-freedom manipulator performing a complex task are analyzed in Section 4 and Section 5 respectively. Finally, conclusions are drawn in Section 6.

Preliminaries

Consider an n-degree-of-freedom robot manipulator and denote with q, q, q, and τ ∈ R n its joint configuration, velocity, acceleration, and torque vectors respectively. Physical limitations of the robot typically regard the joint velocities, accelerations and torques, which can be expressed as:

-qmax ≤ q ≤ qmax (1) -qmax ≤ q ≤ qmax (2) -τ max ≤ τ ≤ τ max ( 3
)
where qmax , qmax , and τ max ∈ R n are the maximum joint velocity, acceleration, and torque vectors respectively. From a practical perspective, torque and velocity limits are typically physical constraints due to the limited motor effort and are usually given in the datasheet of the robot. Acceleration limits are usually software limits imposed to avoid abrupt motion and are typically set depending on the application/robot. Configuration limits are not considered in this work, as the path is assumed to be within the robot workspace, that is, the path generated by the path planner (Step 1 in Section 1) is reasonably contained in the configuration bounds (with a safety margin accounting for possible tracking errors).

The robot is required to perform a joint-space curve described by a vector function Γ(γ) defined as follows:

Γ : [0, γ f ] → R n γ → q d := Γ(γ) (4)
where γ ∈ R is the scalar variable chosen to parametrize the curve and γ f is its final value. For example, if γ is the arc-length along the path, then γ f is equal to the total length of the path. Now define the timing law γ(t)as:

γ : [0,t f ] → [0, γ f ] t → γ(t) (5)
where t f is the total traveling time and γ is non-decreasing in time to avoid inversions of the sense of motion.

Referring to Section 1, Γ is computed by a path planner at Step 1, and γ(t) should be computed at Step 2. Online scaling algorithms take the nominal trajectory computed at Step 2 as input and modify the timing law iteratively in Step 3. The scaling algorithm has to solve the following Optimal Control Problem (OCP) at each time instant t 0 :

minimize γ t f t 0 γ(t) -γ 0 (t) 2 dt subject to -qmax ≤ q(t) ≤ qmax -qmax ≤ q(t) ≤ qmax -τ max ≤ τ(t) ≤ τ max γ(t) ≥ 0 ∀t ∈ [t 0 , t f ] (6)
where γ 0 is the nominal timing law (i.e., the output of Step 2) and inequalities between vectors are intended as componentwise. OCP ( 6) aims to compute a timing law as close as possible to the nominal one γ 0 , but that is also compliant with the robot physical limitations. The requirement γ(t) ≥ 0 is necessary to discard solutions with motion direction opposite to the desired one. Limited computing resources do not permit to solve (6) within a reasonable sampling period (i.e., around 1-10 ms). Considering a shorter time horizon [t 0 ,t 0 + h], 0 < h < t ft 0 gives, however, satisfactory results, as proven in previous works [START_REF] Faulwasser | Implementation of nonlinear model predictive path-following control for an industrial robot[END_REF][START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF]. We will refer to such approach as look-ahead. In fact, most existing methods only consider the system at time t 0 in (6), to reduce the computing time as much as possible. We will refer to this last approach as non-look-ahead, as it is equivalent to setting h = 0. The resulting simplified OCP is:

minimize r r -γ 0 (t 0 ) 2 subject to -qmax ≤ q(t 0 ) ≤ qmax -qmax ≤ q(t 0 ) ≤ qmax -τ max ≤ τ(t 0 ) ≤ τ max ṙ ≥ 0 ( 7
)
where r is the decision variable, corresponding to the timing law at the current time instant. OCP ( 7) is a general formulation that applies to most of the existing non-look-ahead methods in the literature (with some minor modifications that, nonetheless, may significantly affect the performance). The main differences between existing methods are in the algorithm to solve the problem and in the constraints taken into consideration, as pointed out in Section 1. For example, [START_REF] Lo Bianco | Online trajectory scaling for manipulators subject to high-order kinematic and dynamic constraints[END_REF][START_REF] Lo Bianco | A discrete-time filter for the generation of signals with asymmetric and variable bounds on velocity, acceleration, and jerk[END_REF] find the optimal value of r in closed form by re-writing the robot constraints in the scalar variable r; [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF] approximates [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF] with a quadratic program in the joint velocity vector and in the timing law scaling factor (this approach is also used in Sections 4 and 5 to assess the performance of our proposed approach).

Please note that, according to the tiered planning framework described in Section 1, OCPs ( 6) and ( 7) are implemented similarly to a discrete-time controller with sampling period T . That is, the OCP is solved at each sampling time and the solution is sent as reference timing law to the robot controller; then the procedure is repeated at the next sampling time.

Method

Working principle

Existing scaling methods aim to compute a timing law that is as close as possible to the reference γ 0 . When the nonlook-ahead approach (7) is adopted, there is a loss of information about the future trajectory that may lead to poor control choices. The result is a large path-following error because of saturation of the joint variables. In this paper, we introduce a Timing-law Adaptation Module (TAM) that modifies the reference γ 0 if the future trajectory is expected to be infeasible with respect to the physical limits of the robot. A block diagram of the proposed algorithm is represented in Figure 1, where the joint-space case is illustrated. The bottom part of the figure represents a typical implementation of trajectory scaling algorithms. The nominal trajectory block computes the desired motion at each time instant. In practice, it can be implemented as an explicit function or as an interpolator between waypoints; the output is however the desired motion at a certain value of the parameter γ. The desired motion is, in turn, received as input by the scaling algorithm, which can modify it, if it is expected to violate joint limits or user-defined constraints (e.g., safety constraints in human-robot collaboration). The output of the scaling algorithm is the actual reference motion sent to the robot controller at each time instant. The scaling algorithm also updates the parameter γ that will be used to compute the new nominal motion at the next cycle. The Timing-law Adaptation Module (top part of Figure 1) is added to this framework to compute a modified scaling reference v 0 instead of γ 0 (t 0 ). To do so, joint configuration, velocities, accelerations, and torques expected at the end of a predictive window are considered to compute the maximum allowed scaling reference that would satisfy the robot physical limits (v vel , v acc , v tor ). If the nominal scaling γ 0 (t 0 ) is compliant with them, then it is not modified; otherwise the minimum value v lim is chosen as new candidate reference. This candidate reference is stored in a circular buffer of length equal to the predictive window. This circular buffer is necessary to keep track of the maximum allowed values along the whole window (and not only at the end of it). As a matter of fact, considering only the end point of the window would discard possible violation of constraints that were analyzed at previous iterations. The minimum value in the buffer is therefore chosen as the actual scaling reference v 0 and sent to the existing scaling algorithm. This procedure is repeated at each sampling time, with the look-ahead window (and the buffer) moving forward along the trajectory.

Timing-law Adaptation Module

Timing-law adaptation module

Consider an n-degree-of-freedom manipulator performing the desired joint trajectory (4) [START_REF] Byner | Dynamic speed and separation monitoring for collaborative robot applicationsconcepts and performance[END_REF]. The timing law γ(t) can be adjusted online by acting on a decision variable v s.t. γ = v. Our goal is to choose the proper value of v at each time instant, in such a way that the resulting motion along a look-ahead window of length h is compliant with the robot limits. To this purpose, consider a future time instant t p = t 0 + h, h ∈ R + , where t 0 is the current time instant. At t p corresponds γ p = γ(t 0 ) + t p t 0 v dt. The motion at time t p can be expressed as:

q(t p ) = q d (γ p ) (8) q(t p ) = q d (γ p ) v(t p ) (9) q(t p ) = q d (γ p ) v 2 (t p ) + q d (γ p ) v(t p ) (10)
where q d = dq d /dγ, q d = dq d /dγ and vp = dv dt . Variable γ p actually depends on the future trend of v, which is unknown, but it is reasonable to use the first-order approximation γ p ≈ γ(t 0 ) + hv(t 0 ). The proper value of v should be chosen in such a way that the resulting motion q(t p ), q(t p ), q(t p ) respects the robot constraints. We therefore analyze velocity, acceleration, and torque limits at time t p to find the maximum allowed value to satisfy them. Velocity limits on the n joints of the robot at time t p can be imposed as:

-qmax,i ≤ qi (t p ) ≤ qmax,i ∀ i = 1, . . . , n, (11)
which, using ( 9), becomes:

-qmax,i ≤ q d,i (γ p ) v ≤ qmax,i ∀ i = 1, . . . , n, (12)
from which:

v ≤ qmax,i |q d,i (γ p )| ∀ i = 1, . . . , n. (13)
Thus, the maximum value of v which permits to satisfy the velocity constraints of all joints is defined as:

v vel := min i=1,...,n qmax,i |q d,i (γ p )| (14) (in case q d,i (γ p ) = 0, we impose qmax,i /|q d,i (γ p )| = +∞).
Similarly, joint acceleration limits at time t p can be expressed as:

-qmax,i ≤ qi (t p ) ≤ qmax,i ∀ i = 1, . . . , n. (15)
By using [START_REF] Huang | Optimal time-jerk trajectory planning for industrial robots[END_REF], it results:

-qmax,i ≤ q d,i (γ p ) v 2 + q d,i (γ p ) v ≤ qmax,i ∀ i = 1, . . . , n. (16)
The size of the predictive window h is a tunable parameter of the method. Its value should be long enough for the system to reach the modified reference in a smooth manner. Thus, we assume that the term dependent on v is negligible compared with the one dependent on v 2 (this assumption will be verified empirically in Section 4.3.3). The acceleration bounds become:

-qmax,i ≤ q d,i (γ p ) γ2 p (t p ) ≤ qmax,i ∀ i = 1, . . . , n. (17)
In this way, the maximum value of γp that respects the acceleration bound is defined as:

v acc := min i=1,...,n qmax,i |q d,i (γ p )| (18)
(if q d,i (γ p ) = 0, we impose qmax,i /|q d,i (γ p )| = +∞ ).
155

Consider now the torque limits at time t p :

-τ max,i ≤ τ i (t p ) ≤ τ max,i ∀ i = 1, . . . , n. (19)
Torque limits are rewritten in the joint kinematic variables through the robot inverse dynamics function. We consider here the robot dynamic model in the well-known form:

τ = H(q) q +C(q, q) q + g(q) + f v q + f s sign( q) [START_REF] Lo Bianco | A scaling algorithm for the generation of jerk-limited trajectories in the operational space[END_REF] where H(q) ∈ R n×n is the symmetric positive-definite inertia matrix, C(q, q) ∈ R n×n accounts for centrifugal and Coriolis effects, g(q) ∈ R n represents gravity torques, and f v , f s ∈ R n model viscous and Coulomb friction. Torque τ at time t p can be re-written by using (8) [START_REF] Gallant | Extending the capabilities of robotic manipulators using trajectory optimization[END_REF], and [START_REF] Huang | Optimal time-jerk trajectory planning for industrial robots[END_REF] in [START_REF] Lo Bianco | A scaling algorithm for the generation of jerk-limited trajectories in the operational space[END_REF] as follows:

τ(t p ) = H q d (γ p ) q d (γ p ) v 2 + q d (γ p ) v +C q d (γ p ), q d (γ p ) q d (γ p )v 2 + + g q d (γ p ) + f v q d (γ p )v + f s sign q d (γ p ) . ( 21
)
By using the same approximation adopted from ( 16) to [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF], it is possible to drop the term q d (γ p ) v in [START_REF] Lo Bianco | Techniques to preserve the stability of a trajectory scaling algorithm[END_REF]. Then, the maximum value of v that satisfies [START_REF] Lo Bianco | A discrete-time filter for the generation of signals with asymmetric and variable bounds on velocity, acceleration, and jerk[END_REF] can be found by solving the resulting second-order equation, that is:

v tor := min i=1,...,n v ≥ 0 H i q d,i (γ p ) +C i q d,i (γ p ) v 2 + . . . • • • + f v,i q d,i (γ p ) v + g i + f s,i sign q d,i (γ p ) -τ lim,i = 0 , (22)
where H i , C i , g i , f v,i , and f s,i denote the ith row of the respective matrices/vectors (the dependencies on the joint variables have been omitted for the sake of clarity) and:

τ lim,i = -τ max,i if τ d,i (γ p ) < 0, τ max,i if τ d,i (γ p ) > 0. ( 23
)
where τ d,i (γ p ) is the nominal desired torque computed by using v = γ0 in [START_REF] Lo Bianco | Techniques to preserve the stability of a trajectory scaling algorithm[END_REF].

Once the limit values v vel , v acc , and v tor have been computed, the overall reference value is given by:

v lim = min v vel , v acc , v tor , γ0 (t p ) . (24)
At this point, v lim is inserted in circular buffer a of length l = h/T (rounded using ceiling function), where T is the sampling period at which TAM runs so that:

for j = 1 to l -1 do a( j) ← a( j + 1)
end for a(l) ← v lim Note that the buffer is necessary to keep track of the values of v lim calculated at the previous l iterations. Without it, possible constraints violations along the predictive window would be discarded, as only the end point of the window would be taken into account. The minimum value of the buffer is the maximum admissible value of v along the whole predictive window. By denoting the circular buffer with a ∈ R l , the reference value v 0 at time t 0 is:

v 0 (t 0 ) = min i=1,...,l a(i). (25)
TAM's output v 0 determines the velocity profile of the timing law to be given as reference to [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF]. The reference timing law γ 0 in ( 7) is therefore computed as γ 0 (t 0 ) = t 0 0 v 0 (t)dt.

Operational-space trajectories

165 Section 3.2 naturally applies to joint-space trajectories, but it can be extended to operational-space tasks as well. To this purpose, consider a Cartesian curve Γ cs (γ) and denote with f : R n → R m , q → f (q) (with 1 ≤ m ≤ n) the robot forward kinematics function. If f is invertible (i.e., there are no kinematic singularities on the path and the robot is not redundant), the problem traces back to that described in Section 3.2 by defining an equivalent joint-space desired motion as follows:

q d (γ p ) := f -1 Γ cs (γ p ) , (26)
q d (γ p ) := J -1 Γ cs (γ p ), (27)
q d (γ p ) := J -1 Γ cs (γ p ) - ∂ J ∂ q q d (γ p ) q d (γ p ) . ( 28
)
where f -1 : R m → R n denotes the inverse kinematics function of the manipulator and J = ∂ f /∂ q is its Jacobian. Then, the methodology derived from (8) on can be applied just alike.

Simulations

Setup

Simulations have been performed by using a 6-degree-of-freedom Universal Robots UR10 manipulator. The limits of the robot have been set to:

qmax = ( 2, 2, 3, 3, 3, 3 ) rad/s, (29)
Simulations use a virtual model of the robot, that is, the planned motion is tracked with no errors. This allows the evaluation of the performance of the planning methods, as the tracking error introduced by the controller is not present. Path following errors are therefore due to constrained solutions of the OCP. The sampling period of the online planning algorithm is 1 ms. The dynamics parameters needed to include the torque limits [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] are publicly available on the website of the robot manufacturer and are discussed in details in [START_REF] Gaz | A model-based residual approach for human-robot collaboration during manual polishing operations[END_REF].

Compared methods, tests, and settings

TAM can be applied to generic online trajectory scaling methods to improve their performance. In these tests, we use a Non-Look-Ahead method (NLA) based on [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF], with acceleration and torque limits added as described in [START_REF] Cheng | Resolving manipulator redundancy under inequality constraints[END_REF]. This NLA method approximates OCP [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF] with a quadratic program, where the optimization variables are the joint acceleration vector u and the timing law speed v, such that:

minimize u,v J q -x 0 (t 0 ) v 2 + λ γ0 (t 0 ) -v 2 subject to -qmax ≤ q ≤ qmax -qmax ≤ q ≤ qmax -τ max ≤ τ ≤ τ max τ = H q +C q + g + f v q + f s sign( q) q = u (32)
The cost function of ( 32) is an approximation of that of ( 7): the first term aims to minimize the difference between the computed Cartesian twist and the direction of the twist x 0 (t 0 ). In other words, it converts the path-following requirement into a soft-constraint in order to ensure the feasibility of the quadratic program. Note that the problem is formulated at velocity level. Because of this, if position errors occur (e.g., due to numerical drift), OCP (32) is not able to recover them.

To cope with this, the nominal twist is modified based on a proportional control law such that x 0 = Γ cs (γ) + Ke, where e = Γ csf (q) is the position Cartesian error, K is a proportional gain and f is the forward kinematics function of the robot (see [START_REF] Caccavale | The role of Euler parameters in robot control[END_REF] for details). The second term of the cost function minimizes the difference between the computed timing law v and the nominal one. OCP (32) also includes joint velocity, acceleration and torque bounds, just like [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF]. Torque bounds are considered through the inverse dynamics of the robot, H, C, g, f v , f s are the dynamics matrices as defined in (20) evaluated at q(t 0 ) and q(t 0 ). Similarly, J is the Jacobian of the manipulator at q(t 0 ). TAM is applied above NLA, in the sense that it modifies the nominal velocity profile to improve the feasibility chances. That is, when NLA is used, γ0 is the nominal velocity profile; when TAM is used, γ0 is the modified velocity reference obtained from [START_REF] Zanchettin | Safety in human-robot collaborative manufacturing environments: Metrics and control[END_REF]. In all tests, we set K = 100 and λ = 1 • 10 -3 in (32), and the look ahead window h = 0.2 s. The look-ahead method [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] is also considered for comparison. This look ahead method solves OCP (6) with a finite horizon and approximates the resulting nonlinear program with a quadratic program in order to reduce computing time, allowing for online implementation with high sampling rates. We implement the look-ahead method with a predictive horizon of 0.2 s, in order to have a fair comparison with the tuning of TAM. Moreover, the predictive horizon is discretized into a staircase function of 10 steps during the solution of the OCP. Details on setting and implementation issues of the method are discussed in [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF][START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF]. For this technique is based on Model Predictive Control, it will be referred to as MPC.

Position and orientation errors are evaluated. Position error is the Euclidean distance between the end-effector position and the closest point on the desired path. Orientation error is the absolute value of the angle between the end-effector reference frame and the desired reference frame. Another relevant parameter is the average slowdown introduced by the scaling method, computed as the ratio between the desired execution time and the time taken by the robot to perform the whole task (t real /t f ). The task is an example of manufacturing task, such as gluing or laser cutting, along complex shapes. The nominal path consists in the stylized profile of the word CNR in the yz-plain. The shape is a sequence of segments and circular arcs; quotes are shown in Figure 2. During the task, the end-effector orientation should be orthogonal to the yz-plain. The nominal timing law γ 0 is a seven-trait-acceleration function as shown in Figure 3. Except for the initial and final part of the trajectory, the longitudinal velocity of the timing law is constant, as typical of gluing and cutting. Different values of t f are evaluated to assess the performance of the algorithms under increasing severity of the desired trajectory (note that smaller values correspond to more demanding tasks, as the same path should be covered in less time). In a real-case scenario, the value of t f and the corresponding velocity profile should be chosen by the offline trajectory planner (Step 2 in Section 1). Ideally, the optimal value of t f is the minimum that does not lead to constraint violations but, especially when the available planning time is limited, an approximated solution is accepted. If the resulting trajectory is too slow, constraints will not probably be violated, but throughput of the process is not optimized. Vice versa, if the start •

• end resulting trajectory is too fast, quality of the process is lost because of saturation. By using an online scaling algorithm in Step 3, Step 2 is allowed to compute fast trajectories that will be adapted online to respect the constraints.

Results

Path-following errors

Maximum and mean path-following errors (e max and e mean ) are shown in Tables 1 and2. In all cases, TAM and MPC have similar path-following errors, and they both significantly outperform NLA. In the least demanding task (t f = 5 s), TAM and MPC's errors are around ten times smaller than NLA's error; in the most demanding task (t f = 2 s), this difference grows of three orders of magnitude. These results can be better visualized in Figure 4. For t f = 5 s, all algorithms follow the desired path; zoomed areas show that NLA slightly deviates from the path during narrow curves (Figure 4a). As the task becomes more demanding, NLA gives larger and larger deformations, while TAM and MPC are still able to preserve the desired geometry (Figures 4b and4c). For t f = 2 s, the shape drawn by NLA is unrecognizable, while TAM and MPC follow the nominal path with errors comparable to t f = 5 s. The large errors of NLA are due to the inability of the scaling algorithm to slow down the trajectory in advance. On the contrary, TAM and MPC foresee the presence of steep changes and narrow curves and slows down the task if necessary. We can therefore conclude that the use of TAM allows the original algorithm to achieve path-following performance that are similar to look-ahead methods.

Execution times

Regarding the execution time, results for all tasks are shown in Table 3. The percentage difference between TAM and NLA is also computed as

t TAM real -t NLA real t f • 100
where t TAM real and t NLA real are TAM and NLA's execution times. MPC gives the best results, as the look-ahead optimizationbased strategy is able to reduce the slowdown. Apparently, NLA outperforms TAM in terms of execution time for t f = 3, 4, 5 s. This is true for less demanding cases (e.g., t f = 5 s), for which the tracking error of all methods are satisfactory; for more demanding cases, however, the shorter execution time of NLA is jeopardized by significant path errors as visible from Figure 4. For very demanding tasks (e.g., t f = 2 s), NLA execution time even overcomes TAM's execution time, because of the long time taken to converge to the final position when the error is very large (see t f = 2 s). We can therefore conclude that the significant improvement of tracking accuracy provided by TAM partially comes at the cost of a slightly larger execution time with respect to NLA for less-demanding tasks. When the task is very demanding, however, TAM results are comparable with optimization-based look-ahead methods.

Effect of the length of the look-ahead window

The length of the look-ahead window h is a tunable parameter that should be chosen by the user. In Section 3, we assumed that its length was long enough for the system to reach the modified reference timing law with vp = 0. Moreover, the proposed method relies on the approximation γ p ≈ γ(t 0 ) + h γ0 (t 0 ) to compute the values of q d (γ p ), q d (γ p ), q d (γ p ) in ( 8), ( 9), [START_REF] Huang | Optimal time-jerk trajectory planning for industrial robots[END_REF]. It is therefore clear that the choice of h affects the performance of the method: in principle, if h is too small, the modified timing law will not be feasible; if it is too large, the aforementioned approximation might lead to worsened results. In this paper, the choice of the length of h was made empirically owed the application at hand, and a systematic tuning procedure is beyond the scope of this paper; however, we run several tests with different values of h to show how its choice affects the validness of the performance of the method. In particular, we consider the trajectory shown in Figure 2 with t f = 3 s and we plot the results for different values of h ranging from 0.01 to 1.6 s. Figure 5 shows the maximum error (blue bars) and the ratio between the execution and the nominal time t real /t f (yellow bars) for all values of h. We can observe a large error for short horizons (in principle, as h → 0, TAM is equivalent to NLA). As the horizon grows, the error converges to smaller values. On the other hand, the execution time tends to increase in h, as the scaling method acts too early in slowing down the trajectory. As a rule of thumb, the correct value of h is the smallest that guarantees the desired path-following performance. From our experience with industrial manipulators, values ranging from 100 to 200 ms usually lead to satisfactory results.

To motivate the poor performance obtained with small values of h, we need to verify whether the approximation made from ( 16) to ( 17) is valid or not. To do this, recall [START_REF] Huang | Optimal time-jerk trajectory planning for industrial robots[END_REF]:

q(t p ) = q d (γ p ) v 2 first term + q d (γ p ) v second term (33)
Our approximation is based on the assumption that the second term of the equation is negligible with respect to the first one if the scaling method acts sufficiently in advance so that v ≈ 0 before the acceleration limit is hit. To verify it empirically, we compute the two terms of (33) a posteriori for different values of h. Results are in Figure 6. The plots show the trend of the first and second terms (blue and yellow areas respectively) during the execution of the trajectory. It is possible to observe that for small values of h, the second term of (33) is relevant, and this motivates the bad performance of the method. For larger values of h, however, the first term is much more relevant than the second one, and the approximation is therefore valid. This leads to small path-following errors as shown by the simulation results of Section 4.3. It is worth noticing that the range of values suggested for h (100-200 ms) actually shows non-negligible values of the second term of (33) during brief portions of the trajectory; nonetheless, the method still has good performance. A possible reasons for this is that, during these transients, either the system is not in a critical condition with respect to the constraints, or the underlying non-look-ahead scaling method handles the constraint violation if it is not too severe.

Computing time

Limiting the computing time is key in online implementation, as the method iterates at each sampling time. A qualitative analysis of the computing times taken by the different approaches is provided. Please note that the information on computing times serve only for a qualitative, rough comparison between the approaches; actual computing times depend on implementation aspects, and a deep statistical analysis about this is not within the scope of this paper. The tests were performed on a standard laptop mounting an i7-8565U CPU. Quadratic programs were solved by the parametric active-set solver QpOASES [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF]. Table 4 shows the maximum, mean, and the standard deviation of the computing time taken to perform each cycle. As expected, NLA and TAM have comparable computing times. The reason is that most of the computing time of both methods is taken to solve OCP (32), while the growth in computation owed to the proposed reference modification is negligible. Compared to the look-ahead method (MPC) , NLA and TAM give significantly smaller computing times. This is an expected result, as the look-ahead OCP is larger in terms of number of optimization variables and number of constraints. (f) h = 0.8 s Figure 6: Evaluation of the approximation on (33); results for different length of the horizon h. First term of (33) (yellow area); sum of first and second terms of (33) (blue area). Results for h = 0.01 and 1.6 s are not shown for the sake of brevity.

Experimental results

In this section, we assess the algorithms' performance on a real robot with a (non-ideal) controller to verify whether the improvements given by the proposed method are still significant, despite the tracking error introduced by the motion controller. The experiments are shown also in the video accompanying this paper.

Setup

Experimental tests were performed on a 6-degree-of-freedom manipulator. The setup consists of a Universal Robots UR10, version 3.5, controlled by means of a ROS-based control architecture. The controller consists of a cascade scheme: a position control loop runs in ROS Kinetic on Ubuntu 16.04 and communicates with the robot velocity controller via a TCP connection with sampling rate equal to 125 Hz. The proposed method could actually run at higher sampling rates (according to the computing times shown in Section 4.4), but 125 Hz is the maximum sampling rate accepted by TCP interface of the robot at hand, which therefore acts as a bottleneck for the whole system. The outer loop is a proportional controller with sampling rate equal to 125 Hz and gain equal to 7, plus a velocity feedforward term. The inner velocity loop is the built-in control system. The outputs of the tested methods are given as reference to the low-level controller with sampling period T = 8 ms (i.e., the sampling period of the outer loop). The measured robot configuration is acquired from the robot encoders with sampling rate equal to 125 Hz, and the velocity and acceleration are obtained a posteriori via numerical differentiation. Torques are estimated by multiplying the motor currents by the torque constant of each motor. Currents are measured through the robot built-in current sensor and communicated at 125 Hz through the TCP interface of the robot; the values of the torque constants of each motor can be found in [START_REF] Gaz | A model-based residual approach for human-robot collaboration during manual polishing operations[END_REF]. As already mentioned, the dynamics parameters needed in the torque limits [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] are publicly available on the website of the robot manufacturer and are discussed in details in [START_REF] Gaz | A model-based residual approach for human-robot collaboration during manual polishing operations[END_REF]. Velocity, acceleration, and torque limits have been set to (29).

Compared methods, tests, and settings

The proposed method (TAM), the non-look-ahead method (NLA), and the optimization-based look-ahead method (MPC) are compared as described in Section 4.2. Maximum and mean path-following errors (e max and e mean ) and the execution time (t real ) are evaluated. Path-following errors are computed for both the planned and measured trajectories. The former refers to the trajectory computed by the scaling algorithms and then given as reference to the robot controller.

The latter are computed from the actual joint coordinates measured by the joints' encoders. Cartesian positions and orientations are then computed by applying forward kinematics to the joint coordinates at each time instant. Definitions of e max , e mean , and t real are given in Section 4.2.

The robot task is a sine curve in the xz-plain defined as:

Γ xyz = π/3 + 0.6 -0.5 γ, 0.8, 0.4 + 0.2 sin(4πγ) m, (34)
where γ ∈ [0, 1]. During the task, the end-effector orientation should be orthogonal to the yz-plain. The nominal timing law γ 0 is a seven-trait-acceleration function as shown in Figure 3. Different values of t f are tested (note that the trajectory becomes more and more demanding as t f decreases).

Results

Results are shown in Tables 5 and6. The case t f = 5 s provides a nominal trajectory that is always within the robot limits. It is therefore taken as reference case to evaluate the performance of the following trajectories. For the less demanding case (t f = 3.5 s), all methods give similar results and are successful in slowing down the trajectory to preserve the geometrical path (errors are comparable to the non-saturated case t f = 5 s). As the nominal trajectories get more demanding, NLA's errors grow significantly, while TAM and MPC are still able to follow the path accurately. For example, for t f = 1.5 s, TAM's path-following error is in the order of 0.1 mm, while NLA drifts from the nominal path of about 5 cm. This can be noticed in Figure 7a, which shows the path performed by the three planners: TAM and MPC follow the curve accurately; NLA gives large overshoots during narrow curves, according to the behaviors analyzed in Section 4.3. It is worth stressing that the improvement due to the proposed method is evident also from the measured data (Tables 5b and6b). In other words, TAM's improvement is relevant not only under ideal conditions (i.e., with ideal motion control as in Section 4), but also when the motion tracking error is taken into account.

Regarding the execution times, simulation results are confirmed again. Results are shown in Table 7. MPC gives overall better results. TAM and NLA have comparable execution times in less demanding tasks, while TAM outperforms NLA as the task becomes more demanding (up to -60% for t f = 1.5 s). This is clear also from Figure 7b, which shows the resulting timing laws for t f = 1.5 s: MPC and TAM's timing law resembles the nominal one (dilated in time); MPC is the faster among the three; NLA has an irregular and slow trend. 6.1 • 10 -6 6.9 • 10 -6 5.9 • -6 3. Finally, as an example, the trends of the joint configuration, velocities, and accelerations for t f = 2 s are represented in Figure 8. Dashed gray lines are the nominal timing laws (γ 0 and its velocity and acceleration). From Figure 8c, it is interesting to point out that the nominal timing law significantly exceeds the acceleration limits (dashed blue lines). Nonetheless, thanks to the scaling algorithms, the joint accelerations and velocities are always kept within the bounds, except for few acceleration spikes due to measurement noise and the motion controller actions. It is also worth stressing that TAM's trends are much smoother than NLA's ones, with consequent minor stress of the robot actuators.

Conclusions

We have proposed a novel trajectory scaling method that modifies the reference timing law based on future information about the desired task. This can be used to confer a predictive feature on the existing trajectory scaling algorithms, but without falling into the computational disadvantages of model predictive control methods (i.e., higher computational burden and the need of solving nonlinear optimization problems). Simulation and experimental results have shown that the proposed method significantly improves the path-following error and the total execution time of the trajectories if compared with non-look-ahead methods, and it shows lower computing time compared to optimization-based look-ahead methods.

Figure 1 :

 1 Figure 1: Block scheme of the proposed scaling algorithm.

  qmax = ( 5, 5, 10, 10, 10, 10 ) rad/s 2 , (30) τ max = ( 200, 200, 100, 50, 50, 50 ) Nm.

Figure 2 :

 2 Figure 2: Drawing of the simulation task (quotes are in millimeters).

Figure 3 :

 3 Figure 3: Seven-trait-acceleration timing law along.

Figure 4 :

 4 Figure 4: Simulation results. Path performed by the different methods: nominal path (black dots); TAM (solid black lines); NLA (dotted red lines); MPC (dashed green lines).

Figure 5 :

 5 Figure 5: Results for different length of the horizon h. Maximum position error (blue bars); ratio between the actual and the nominal execution time (yellow bars).

5 7 . 1 • 5 ( 6 •

 7156 10 -5 8.9 • 10 -5 6.8 • 10 -5 1.3 • 10 -5 2.6 • 10 -5 1.2 • -5 3.0 7.6 • 10 -5 1.0 • 10 -4 9.1 • 10 -5 1.5 • 10 -5 4.9 • 10 -5 2.2 • -5 2.5 9.2 • 10 -4 1.5 • 10 -2 1.2 • 10 -4 2.3 • 10 -5 7.5 • 10 -4 3.4 • -5 2.0 1.1 • 10 -4 4.1 • 10 -2 1.9 • 10 -4 1.1 • 10 -5 3.8 • 10 -3 1.3 • -5 1.5 1.3 • 10 -4 5.5 • 10 -2 2.1 • 10 -4 2.4 • 10 -5 5.1 • 10 -3 3.1 • -10 -3 1.4 • 10 -3 1.5 • 10 -3 2.5 • 10 -4 2.8 • 10 -4 2.5 • -4 3.5 2.6 • 10 -3 2.3 • 10 -3 2.3 • 10 -3 4.5 • 10 -4 4.5 • 10 -4 4.1 • -4 3.0 3.3 • 10 -3 2.0 • 10 -2 3.0 • 10 -3 6.1 • 10 -4 1.8 • 10 -3 5.9 • -4 2.5 3.7 • 10 -3 6.1 • 10 -2 3.6 • 10 -3 6.9 • 10 -4 9.3 • 10 -3 8.5 • -4 2.0 2.4 • 10 -3 6.1 • 10 -2 3.1 • 10 -3 4.5 • 10 -4 9.6 • 10 -3 4.4 • -4 1.5 2.3 • 10 -3 7.6 • 10 -2 4.0 • 10 -3 5.1 • 10 -4 9.6 • 10 -3 7.7 • -4

6 • 4 ( 6 •

 646 10 -6 2.6 • 10 -6 1.9 • 10 -6 1.0 • 10 -6 1.0 • 10 -6 7.1 • -7 3.[START_REF] Byner | Dynamic speed and separation monitoring for collaborative robot applicationsconcepts and performance[END_REF] 7.0 • 10 -6 8.6 • 10 -6 6.7• 10 -6 2.2 • 10 -6 3.6 • 10 -6 1.2 • -6 3.0 2.4 • 10 -5 4.3 • 10 -3 2.1 • 10 -5 3.2 • 10 -6 1.4 • 10 -4 1.1 • -6 2.5 2.5 • 10 -5 3.0 • 10 -2 6.0 • 10 -5 3.1 • 10 -6 3.7 • 10 -3 8.9 • -6 2.0 2.2 • 10 -5 1.2 • 10 -1 8.3 • 10 -4 3.0 • 10 -6 2.2 • 10 -2 1.8 • -4 1.5 2.4 • 10 -5 1.8 • 10 -1 2.3 • 10 -3 3.0 • 10 -6 3.4 • 10 -2 3.4 • -10 -3 3.6 • 10 -3 3.5 • 10 -3 7.0 • 10 -4 7.1 • 10 -4 6.7 • -4 3.5 7.7 • 10 -3 7.9 • 10 -3 6.8 • 10 -3 1.2 • 10 -3 1.3 • 10 -3 1.4 • -3 3.0 8.3 • 10 -3 1.7 • 10 -2 6.4 • 10 -3 1.9 • 10 -3 2.6 • 10 -3 2.0 • -3 2.5 1.3 • 10 -2 3.5 • 10 -2 9.9 • 10 -3 2.1 • 10 -3 4.5 • 10 -3 2.2 • -3 2.0 7.2 • 10 -3 6.3 • 10 -2 9.9 • 10 -3 1.3 • 10 -3 8.3 • 10 -3 3.0 • -3 1.5 7.0 • 10 -3 8.2 • 10 -2 1.2 • 10 -2 1.2 • 10 -3 1.1 • 10 -2 1.4 • -3

Figure 7 :

 7 Figure 7: Resulting path and timing law for t f = 1.5 s.

Figure 8 :

 8 Figure 8: Measured joint positions, velocities, and accelerations for t f = 2 s. Dashed gray line: nominal reference. Solid black line: TAM. Dotted red line: NLA. Dashed green line: MPC. Dashed blue line: acceleration limits.

Table 1 :

 1 Translation error for different nominal trajectory times t f . 10 -4 5.30 • 10 -3 1.55 • 10 -4 2.55 • 10 -5 2.98 • 10 -4 2.67 • 10 -5 4.0 1.71 • 10 -4 1.78 • 10 -2 1.98 • 10 -4 3.58 • 10 -5 1.95 • 10 -3 3.93 • 10 -5 3.0 2.29 • 10 -4 4.65 • 10 -2 2.01 • 10 -4 5.04 • 10 -5 7.44 • 10 -3 4.99 • 10 -5 2.0 3.85 • 10 -4 1.43 • 10 -1 2.99 • 10 -4 8.43 • 10 -5 4.05 • 10 -2 1.03 • 10 -4

	t f		e max [m]			e mean [m]
	[s]	TAM	NLA	MPC	TAM	NLA	MPC
	5.0	1.33 •

Table 2 :

 2 Orientation error for different nominal trajectory times t f .

	t f		e max [rad]			e mean [rad]
	[s]	TAM	NLA	MPC	TAM	NLA	MPC
	5.0	6.44 • 10 -5 7.62 • 10 -3 8.06 • 10 -5	6.79 • 10 -7 4.35 • 10 -4 1.02 • 10 -6
	4.0	7.82 • 10 -5 2.51 • 10 -2 8.98 • 10 -5	1.04 • 10 -6 3.05 • 10 -3 1.33 • 10 -6
	3.0	8.35 • 10 -5 6.37 • 10 -2 9.77 • 10 -5	1.85 • 10 -6 1.11 • 10 -2 1.55 • 10 -6
	2.0	8.63 • 10 -5 4.19 • 10 -1 9.91 • 10 -5	3.11 • 10 -6 1.11 • 10 -1 2.01 • 10 -6

Table 3 :

 3 Trajectory execution time for different nominal trajectory times t f .

	t f [s]	t real [s] TAM NLA MPC	t real /t f TAM NLA MPC	t TAM real -t NLA real t f	• 100
	5.0	5.79	5.42	5.25	1.15	1.08	1.05	+7.4
	4.0	5.22	4.81	4.54	1.30	1.20	1.14	+10.2
	3.0	4.88	4.36	3.94	1.63	1.45	1.31	+17.3
	2.0	4.71	5.15	3.85	2.35	2.58	1.93	-22.0

Table 4 :

 4 Comparison of computing times.

	Method t max [µs] t mean [µs] std dev. [µs]
	TAM	212	64	8
	NLA	201	61	8
	MPC	2320	560	55

Table 5 :

 5 Translation error for different nominal trajectory times t f . • 10 -5 5.7 • 10 -5 4.9 • 10 -5

			(a) From planned motion
	t f		e max [m]			e mean [m]
	[s]	TAM	NLA	MPC	TAM	NLA	MPC
	5.0	5.1

Table 6 :

 6 Orientation error for different nominal trajectory times t f .

Table 7 :

 7 Trajectory execution time for different nominal trajectory times t f .

	t f [s]	t real [s] TAM NLA MPC	t real /t f TAM NLA MPC	t TAM real -t NLA real t f	• 100
	5.0	5.00	5.00	5.00	1.00	1.00	1.00	0.0
	3.5	3.50	3.50	3.50	1.00	1.00	1.00	0.0
	3.0	3.16	3.09	3.06	1.05	1.03	1.02	+2.2
	2.5	3.04	3.20	2.84	1.22	1.28	1.14	-6.3
	2.0	3.03	3.70	2.78	1.51	1.85	1.39	-33.7
	1.5	2.94	3.84	2.63	1.96	2.56	1.75	-60.3

Consider, for example, a gluing or a cutting operation along a given path, where the dispensing of glue or the cutting speed/power automatically adapts to the speed of the robot; in this case it is possible to change the timing law to optimize the execution of the task.

Acknowledgments

This work was partially supported by ShareWork project (H2020, European Commission -G.A. 820807) and Pickplace project (H2020, European Commission -G.A. 780488).


Marco Faroni
email: m.faroni003@unibs.it

Domenico Gorni
email: domenico.gorni@unibs.it

Antonio Visioli
email: antonio.visioli@unibs.it

Energy Minimization in Time-Constrained Robotic Tasks via Sequential Quadratic Programming

Keywords: Industrial robotics, energy minimization, trajectory optimization, quadratic programming

Reduction of the energy consumption in robotized processes is a key issue in nowadays manufacturing. In this paper, we propose a simple approach to energy minimization of robotic tasks with assigned cycle time based on sequential quadratic programming. The method aims at re-shaping a given timing law in the sense of energy saving, without modifying the desired path and the given cycle time. Thanks to the iterative linearization of the nonlinear time-constraint, the resulting minimization problem is solved by only using common quadratic programming solvers, making the method suitable for a direct implementation in robot industrial controllers. At first, the method is devised by only considering the kinematics of the manipulator. The dynamic model is then straightforwardly included, without significantly increasing the complexity of the method. Validation in simulation environment is provided in order to show the effectiveness of the methodology.

I. INTRODUCTION

Nowadays energy efficiency as well as the conservation of energy and natural resources are key aspects in the economic sector. The target of reduction of CO 2 emission is strictly correlated to the reduction of energy waste. In fact, companies belonging to many different sectors have been stimulated by the European Energy Management Standard (EN 6001) to reduce and monitor their energy consumption. Furthermore, during the last years, the price of energy is continuously increasing [START_REF] Unander | Decomposition of manufacturign energy use in iea countries, how do recent development compare with historical long-term trandes[END_REF], [START_REF] Paryanto | Reducing the energy consumption of industrial robots in manifacturing systems[END_REF]. Companies therefore react moving from the reduction of production time to the optimal trade-off between energy consumption and time. Recent statistics show that one of the major consumers of energy is actually the manufacturing industry.

During the last decades, the number of robots implemented in manufacturing has exponentially increased, moving companies to optimize their energy consumption in order to reduce the total amount of consumed energy [3]. For example, measurements show that an average 200-kg-payload robot in body shop yearly consumes around 8 [MW h] and, obviously, the most part of this energy is spent during the movement [START_REF] Meike | Energy efficient use of robotics in the automobile industry[END_REF], [START_REF] Maimon | Energy analysis of robot task motions[END_REF]. In a typical automotive car body, all the sheet metal parts are joined together by up to 4000 weld spots. Furthermore, other joining methods are gluing, arc welding and stud welding. In this kind of plants hundreds of robots that work unceasingly are involved [START_REF] Björkenstam | Energy efficient and collision free motion of industrial robots using optimal control[END_REF], [START_REF] Meike | Energy efficient use of multirobot production line in the automotive industry: Detailed system modeling and optimization[END_REF]. These data point out that, in order to satisfy economic efficiency criteria, industries are more and more interested in robot with lower energy consumption for comparable process time and accuracy.

The problem of computing optimal energy consumption paths and trajectories has been subject of research since the late 1960s [START_REF] Field | Iterative dynamic programming: An approach to minimum energy trajectory planning for robotic manipulators[END_REF]. In the last years, many researches have been carried out trying to solve it, however there is still the lack of a simple methodology that can be easily implemented. First of all, the energy minimization problem can be addressed from two different points of view based on the robotic application. The first approach concerns about finding optimal geometrical paths which can globally decrease the total energy consumption. For instance, if the robot has to execute a task like pick and place, the only geometrical constrains are the starting and the ending positions (and a few intermediate waypoints at most). Therefore, the geometrical path can be devised in such a way that the energy consumption is minimized. The second approach is followed in case of predefined geometrical paths. In this case, a decrease in the energy consumption can be obtained by acting on the timing law applied to the manipulator actuators by means of change in jerk, acceleration and velocity reference values.

This paper deals with this second class of strategies. In this framework, most of the proposed approaches set up a nonlinear optimization problem, which minimizes a cost function proportional to the energy consumption and takes into account the manipulator limits (e.g., configuration, velocity, acceleration and torque limits) in the constraints [START_REF] Meike | Energy efficient use of robotics in the automobile industry[END_REF], [START_REF] Wigström | Highlevel scheduling od energy optimal trajectories[END_REF], [START_REF] Shin | A dynamic programming approach to trajectory planning of robotic manipulators[END_REF], [START_REF] Shiller | Time-energy optimal control of articulated system with geometric path constraints[END_REF]. However, these methodologies usually require heavy computational burden and their implementation may often result to be cumbersome, as also pointed out in [START_REF] Riazi | Energy optimization of multi-robot systems[END_REF]. Heuristic approaches are also often adopted by practitioners [START_REF] Ayten | Optimum trajectory planning for industrial robots through inverse dynamics[END_REF], [START_REF] Martin | Minimum effort motions for open chain manipulators with task-dependent end-effector constraints[END_REF]. In this case, the timing law is devised by using smooth functions and by using an iterative approach to minimize velocity or acceleration peaks given the total task time and checking that the resulting motion satisfies the robot limits. These approaches are easily implementable, computationally light, and do not require optimization solvers. However, compliance with the robot limits can be checked only a posteriori, and the resulting timing law is suboptimal with respect to the original problem. There is still the need of easily-implementable and effective techniques which can attract practitioners and boost the industrial application of optimization-based methods.

This was also pointed out in [START_REF] Riazi | Energy optimization of multi-robot systems[END_REF], where a nonlinear optimization approach was developed to minimize a simplified energy index (namely, the squared sum of the joint accelerations). Such approach still requires the use of nonlinear solvers and does not take into account the dynamics of the manipulator. Nevertheless, it gives very good results, with a reduction of the energy consumption in the order of 30%.

In this paper, we therefore follow a similar approach, aiming at tackling the need for nonlinear solvers in the resolution of the optimization problem and the lack of inclusion of the dynamics of the robot in the problem. First of all, the energy minimization problem is set up in a different way with respect to [START_REF] Riazi | Energy optimization of multi-robot systems[END_REF]: the energy consumption index is written as a quadratic cost function and all the robot limits are written as linear constraints in the optimization variable, which is given by the curvilinear abscissa along the nominal path. The only nonlinear constraint is therefore given by the imposition of the total execution time of the task. The optimization problem is then solved by means of a Sequential Quadratic Programming (SQP) approach [START_REF] Zhu | An efficient sequential quadratic programming algorithm for nonlinear programming[END_REF], which consists in iteratively linearizing the nonlinear constraint around the solution obtained at the previous iteration. The proposed method therefore presents the great advantage of requiring just the implementation of Quadratic Programming (QP) techniques, which could also be easily implemented on industrial robot controllers. Moreover, we address the inclusion of the dynamic model of the manipulator and we show that, by following the approach presented in [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF], this does not significantly increase the computational complexity of the problem.

The paper is organized as follows. In Section II, the basic energy-optimal time-constrained path tracking problem is defined. In Section III, a linearization approach, based on Taylor expansion, is applied to the presented problem in such a way that the original problem can be solved as a sequence of quadratic programs. Section IV extends the kinematicbased approach presented up to that point by including the dynamic model of the manipulator. Finally, in Section V, simulation results are presented to show the effectiveness of the methodology. Conclusions are given in Section VI.

II. PROBLEM FORMULATION

Consider a given trajectory in the joint space and a set of kinematic constraints for a robot manipulator. The strategy proposed in this paper aims at modifying such trajectory by minimizing the energy consumption, preserving the path tracking and the given execution time of the task. Therefore, considering a manipulator composed by n joints, the trajectory q(t) can be defined by means of the path-velocity decomposition approach as the composition of the following functions:

s : [0,t f ] → [0, s f ], t → s(t), q : [0, s f ] → R n , s → q(s), (1)
where s represents the Euclidean distance from the beginning of the path, and q is a vector that contains all the joint coordinates respect to time q(t) = [q 1 (t), . . . , q n (t)] T . Furthermore t f is the total traveling time and s f = s(t f ) is the total length of the path. As this paper deals with path tracking problems, we reasonably assume that ṡ(t) > 0 ∀ t ∈]0,t f [ and ṡ(t) ≥ 0 in the extreme points t = 0 and t = t f . For a given path the joints velocities and accelerations can be rewritten as a function of the defined scalar path coordinate trajectory, as follows:

q(t) = q (s(t)) ṡ(t), q(t) = q (s(t)) s(t) + q (s(t)) ṡ(t) 2 , (2)
where ṡ = ds/dt, s = d 2 s/dt 2 , q (s) = dq/ds, and q (s) = d 2 q/ds 2 .

In case the dynamic model of the manipulator is not available, it is possible to have an approximate measure of the energy consumption as a weighted sum of squared angular accelerations for all the joints [START_REF] Riazi | Energy and peak power optimization of time-bounded robot trajectories[END_REF]. Therefore the problem of minimizing the energy consumption can be written as:

minimize s t f 0 q(t) T q(t)dt subject to s(0) = 0, s(t f ) = s f ṡ(0) = ṡ0 ṡ(t f ) = ṡ f ṡ(t) ≥ 0 q ≤ q(t) ≤ q q ≤ q(t) ≤ q (3)
where ṡ0 and ṡ f are the initial and final velocities (typically equal to zero), while q, q, q, and q are, respectively, the lower and upper joints velocity constraints and the lower and upper joints acceleration constraints.

It is clear that the proposed performance index is nonlinear with respect to the path coordinate s, however it is possible to reformulate the optimal control problem (3) as an optimal control problem with linear system dynamics subject to nonlinear state dependent constraints. Indeed, a smart variable change has been proposed in [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF]. Even though this requires a further set of equality constraints, the nonlinearity is moved from the performance index to a constraint. Firstly, a change in the integration variable, from t to s is required, so that the total time of the trajectory is rewritten as:

t f = t f 0 1dt = s(t f ) s(0) 1 ṡ ds (4)
Secondly, the following change of variables is applied:

a(s) = s, b(s) = ṡ2 . (5)
The relation between the new optimization variables a(s) and b(s) is given by the following additional equality constraint:

ḃ(s) = b (s) ṡ = 2a(s) ṡ, (6)
which implies b (s) = 2a(s). [START_REF] Meike | Energy efficient use of multirobot production line in the automotive industry: Detailed system modeling and optimization[END_REF] It is now possible to rewrite Problem (3) in such a way that the new optimization variables are a(s) and b(s) instead of s, that is:

minimize a,b s f 0 (q (s)a(s) + q (s)b(s)) T (q (s)a(s) + q (s)b(s))ds subject to b(0) = ṡ2 0 , b(s f ) = ṡ2 f , b (s) = 2a(s), b(s) ≥ 0, (q (s)) 2 b(s) ≤ ( q) 2 , q ≤ q(s) a(s) + q(s) b(s) ≤ q. ( 8
)
The resulting optimization problem is therefore quadratic and can be easily solved by using conventional QP solvers. Note that the cost function in ( 8) is not exactly equivalent to the cost function in (3). In fact, the new cost function minimizes the integral of squared acceleration along the curvilinear abscissa s, while the original problem (3) minimizes the integral of the squared acceleration along time. However, in this way, the new cost function results to be quadratic, with significant benefits from the computational point of view. Furthermore, Problem ( 8) is not complete yet, as none of the stated constraints accounts for the total traveling time. Thus, a further constraint has to be added, that is,

t f = s(t f ) s(0) 1 b(s) ds. (9)
This equality constraint is nonlinear. The next section addresses the linearization of this constraint in order to solve the optimization problem by means of a sequence of quadratic programs.

III. LINEARIZATION OF THE PROBLEM AND TERMINAL

CONDITIONS

The longitudinal path length s ∈ [0, s f ] is discretized by means of a gridpoint {s k }, k = 0, . . . , K, and a(s) and b(s) are considered to be constant along the discretized intervals. Therefore, define a = [a 0 , . . . , a K ] and b = [b 0 , . . . , b K ] where

a k = a s k -s k-1 2 and b k = b s k -s k-1 2
. The constraint expressing the total traveling time in (9) can be rewritten as:

t f = s(t f ) s(0) 1 b(s) ds = K ∑ k=1 1 √ b k ∆s k , (10)
where ∆s k = s ks k-1 . Therefore, a single sample of time ∆t k , at each generic instant k, can be rewritten as a function of ∆s k and b k , as follows: Figure 1 shows the function that represents the nonlinear relation between ∆t k and b k (s). To overcome this nonlinearity, a linearization approach based on Taylor series expansion is applied. First of all, it is important to stress that this approach is based on the knowledge of the nominal timing law that has to be optimized. Therefore, the initial values of a k and b k on the whole path [0, s f ] are assumed to be known and they are denoted by a 0 and b 0 .

∆t k (b k ) = 1 √ b k ∆s k . (11)
By applying a first-order Taylor series approximation to [START_REF] Shiller | Time-energy optimal control of articulated system with geometric path constraints[END_REF], we obtain:

∆t k (b k ) = ∆s k b 0 k - 1 2
∆s k b 0 k b 0 k (b k -b 0 k ) (12)
where b 0 k is the kth element of the set of known initial values of the optimization variable b k and ∆t k (b k ) is the approximated value of ∆t k (b k ) in the sense of Taylor expansion. Such a linearization is, obviously, an approximation of the pseudohyperbole around each initial value of the optimization variable b k . However, it has to be pointed out that in many cases b 0 and b K are equal to zero, therefore (12) results impossible to be applied. To overcome this issue, the first and the last time intervals are approximated by making use of the acceleration variables, that is:

∆t k (a k ) = 2∆s k a k , for k = 0 ∧ k = K. (13)
Equation ( 13) is then linearized in the same way as [START_REF] Shiller | Time-energy optimal control of articulated system with geometric path constraints[END_REF] by means of its first-order Taylor expansion. It is clear that each b k is now constrained to move on a line instead of on the pseudo-hyperbole. Thus, to obtain a reliable solution, it is important to limit the admissible variation around the linearization point. This can be implemented by imposing a maximum admissible deviation between the pseudo-hyperbole and the line. This result in an additional constraint such that: where b k and b k are calculated by imposing that the relative deviation between the pseudo-hyperbole and the line is smaller than a maximum value δ , i.e.:

b k ≤ b k ≤ b k ∀ k = 1, . . . , K (14)
∆t k (b k ) -∆t k (b k ) ∆t k (b k ) ≤ δ ∆t k (b k ) -∆t k (b k ) ∆t k (b k ) ≤ δ (15)
The considered linearized optimal problem therefore results: minimize 16) is based on the linearization of ( 11) and [START_REF] Ayten | Optimum trajectory planning for industrial robots through inverse dynamics[END_REF]. The error introduced by the linearization is smaller when the approximated solution is close to the initial conditions. For this reason, we adopt a sequential programming approach, by solving ( 16) iteratively, and using the last solutions as new initial timing law for the next optimization, until the difference between successive solutions is smaller than a given threshold ε. This procedure is summarized in Algorithm 1.

a, b (q (s k )a k + q (s k )b k ) T (q (s k )a k + q (s k )b k ) subject to b 0 = ṡ2 0 , b K = ṡ2 f , b = 2a b ≥ 0 (q (s k )) 2 b k ≤ ( q(s k )) 2 q ≤ q(s k ) a k + q(s k ) b k ≤ q ∆t k (b k ) -∆t k (b k ) ∆t k (b k ) ≤ δ ∆t k (b k ) -∆t k (b k ) ∆t k (b k ) ≤ δ ∆t k (b k ) = ∆s k b 0 k -1 2 ∆s k b 0 k b 0 k (b k -b 0 k ) k = 2, ..., K-1 ∆t k (a k ) = 2∆s k a 0 k -1 2 √ 2∆s k a 0 k a 0 k (a k -a 0 k ) k = {1, K} Problem (

Algorithm 1 Computation of the optimal motion law

Input: a 0 , b 0 Output: a, b repeat ∆t 1 ← 2∆s 1 a 0 1 -1 2 √ 2∆s 1 a 0 1 √ a 0 1 (a 1 -a 0 1 ) ∆t K ← 2∆s K a 0 K -1 2 √ 2∆s K a 0 K √ a 0 K (a K -a 0 K ) for i = 2 to K -1 do ∆t k ← ∆s k b 0 k -1 2 ∆s k b 0 k b 0 k (b k -b 0 k )
end for a b ← argmin Problem ( 16)

max value ← max a b - a 0 b 0 a 0 ← a b 0 ← b until max value ≤ ε
Depending on the value of δ and ε, it is possible to obtain a faster or slower convergence of the recursive algorithm and of course, a bigger or a lower proximity to the global optimal minimum of the nonlinear problem.

IV. REFORMULATION OF THE PROBLEM GIVEN THE DYNAMIC MODEL

Consider the dynamic model of the manipulator in the form: τ(t) = M(q(t)) q(t) + C(q(t), q(t)) q(t) + f(q(t))sgn( q(t)) + g(q(t)) [START_REF] Riazi | Energy and peak power optimization of time-bounded robot trajectories[END_REF] where M(q(t)) is a positive definite inertia matrix and C(q(t), q(t)) is a matrix accounting for Coriolis effects. Then, f(q(t)) is a vector that describes the Coulomb friction forces while g(q(t)) denotes the vector accounting for gravity. By applying ( 2) into ( 17) we obtain:

τ(s(t)) = M s (s(t)) s(t) + C s (s) ṡ(t) 2 + g s (s) (18)
where M s (s(t)) = M(q(s(t)))q (s(t))

C s (s(t)) = M(q(s(t)))q (s(t)) + C(q(s(t)), q (s(t)))q (s(t))

g s (s(t)) = f(q(s(t)))sgn(q (s(t))) + G(q(s(t))) (19)
By using the change of variable described in [START_REF] Maimon | Energy analysis of robot task motions[END_REF], the torque τ(s) results to be linear in the optimization variables a(s) and b(s). This means that a quadratic cost function can be easily set up to minimize the energy consumption in terms of integral of the squared torques. Similarly to [START_REF] Field | Iterative dynamic programming: An approach to minimum energy trajectory planning for robotic manipulators[END_REF], the following optimization problem results:

minimize a, b s f 0 (τ(s(t))) T (τ(s(t)))ds subject to b(0) = ṡ2 0 , b(s f ) = ṡ2 f b (s) = 2a(s) b(s) ≥ 0 (q (s)) 2 b(s) ≤ ( q) 2 q ≤ q(s) a(s) + q(s) b(s) ≤ q τ ≤ M s (s(t))a(s) + C s (s(t))b(s) + g s (s(t)) ≤ τ (20)
where τ(s) and τ(s) are the torque lower and upper bounds. The solution of such problem is analogous to the above mentioned kinematic case as described in Section III.

V. RESULTS

In order to verify the performance of the proposed methodology, the algorithm has been used to plan the trajectory on a 6-DOF manipulator. In particular, we consider a six-axis Efort ER3A industrial manipulator that has to perform a trajectory defined in the joint space. The target is, as mentioned before, to complete a task preserving the time and the path tracking by consuming as less energy as possible.

The methodology described in this work is implemented offline, in the sense that the algorithm is completely executed before the start of the motion. Differently from many other nonlinear algorithms developed to solve the same problem, this methodology requires only a standard quadratic programming solver. This kind of solvers are simple and fast enough to be easily included directly in the robotic controller [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF], [START_REF] Hildreth | A quadratic programming procedure[END_REF]. This experimental case study aims at demonstrating some of the features presented in the previous sections. No dynamic model is used in order to show the effectiveness of the methodologies in usual industrial case, where the dynamic model is often unknown or unreliable. For the sake of simplicity the trajectory is defined in the joint space. In fact, even though the tasks in industrial robotics are mainly defined in the operational space, from a practical point of view the motion is controlled acting directly on each single joint.

First of all, in order to improve the performance of the algorithm, an additional assumption has to be done. During the motion planned by a standard industrial controller, the most used motion laws (for instance, the seven-segments or the cycloidal ones) present the maximum velocity in the central part of the movement. The maximum accelerations and decelerations are usually mainly concentrated at the beginning and at the end of the trajectory. Due to this fact, it is recommended, in order to obtain a better convergence of the algorithm, to have a thicker path sampling in these parts of the trajectory. In case of dense sampling, the number of optimization variables increases significantly, causing an increment of the required computational time. We therefore an irregular sampling of the path length {s k } by making use the well-known Chebyshev nodes, which gives:

s k = 1 2 s f + 1 2 s f cos 2k -1 2k π , k = 1, . . . , K. (21)
Here we consider a trajectory defined in the path coordinates s(t) and its time derivatives. The timing law applied to the manipulator is a seven-segments one (see Figure 3), which has been generated based on the following values:

• s f = 10 [rad]; • s 0 = 0 [rad]; • ṡ0 = 0 [rad/s]; • ṡ f = 0 [rad/s]; • ṡmax = 1 [rad/s]; • smax = 0.5 [rad/s 2 ]; • ... s max = 0.5 [rad/s 3 ];
The total execution time of the trajectory therefore results to be t f = 12.5 [s]

The path length has been discretized by using a number of gridpoints K = 200. Furthermore, the tolerance values δ and ε have been chosen respectively equal to 2% and 1.5%. The choice of these two values is very important as the convergence of the algorithm and the proximity of the linearized solution with respect to the global optimum one depends mainly on these ones. First of all, it is important to stress that the output tolerance is based on the maximum displacement value between the set of solution at the iteration i and those at the iteration i -1. Therefore, in order not to obtain a fake convergence it is recommended to choose the δ value bigger than the ε one. The optimization variables have to be free to move more than the output tolerance in order to be sure that the resulting set of optimal values has actually converged to the global optimal solution. Obviously, in order to have a better precision, the values of these two parameters have to be small. Furthermore, increasing too much these two values will cause a bad convergence due to the lost of the good approximation hypothesis. In practice, the values are moving on a straight line that is actually approximating, with a good precision, the pseudo-hyperbole only when it is close to the tangent point (see Figure 2). The nominal trajectory is defined as:

q =       0.4(1 -cos(s(t))) 0.8 sin(s(t)) 0.2 cos(s(t)) 1.1(1 -sin(s(t))) 0.3(1 -cos(s(t))) 1.4 sin(s(t))       (22)
where s(t) is given by the seven-segment timing law described in Figure 3. The new timing law obtained by means of the proposed method is shown in Figure 4. Furthermore, in Figures 5 and6, the resulting joint accelerations of the 6-DOF manipulator are presented.

Table I shows the energy consumption of each joint in case of the original and the optimized timing law. The energy consumption is calculated as:

K ∑ k=1 q(k) T q(k)∆t k . (23)
It is possible to notice that the optimized trajectory allows a reduction of the energy consumption of about 25%, which is indeed a significant improvement.

As regards the computational time required by the algorithm, the convergence time for the proposed example resulted to be equal to 29 seconds, taking 19 iteration to satisfy the exit tolerance (the simulations have been executed in Matlab, on a standard laptop mounting an i7-4500U processor).

VI. CONCLUSIONS

In this paper we have presented a very simple and linear methodology that can be used to solve an energy minimization in time constrained robotic tasks. This kind of problem is very popular in industries where the energy minimization problem is, at the present day, an important topic. This algorithm has many advantages, for example it can be used both with or without the knowledge of the dynamic model of the manipulator. Furthermore, it preserves the path tracking and it is constrained to re-shape the motion law without changing the total time of the task. The linearization and, therefore, the possibility to use simple linear quadratic programming solvers makes it suitable for a direct implementation in the robot motion controller. However, the linearization also introduces the disadvantage of tuning the values of two tolerance parameters, which can affect the convergence performance.

Energy is actually saved by modifying the original timing law by minimizing the sum of the squared acceleration values subject to many different equality and inequality constraints. A simple case study has shown that the application of the proposed methodology allows a significant reduction of the energy consumption.

Fig. 1 :

 1 Fig. 1: Pseudo-hyperbole representing the nonlinear relation between ∆t k and b k (s).

Fig. 2 :

 2 Fig.2: Linearization of the pseudo-hyperbole. The red dot represent the starting point given by a generic b 0 k ; the blue line is the pseudo-hyperbole that has to be approximated; the green line is the Taylor linearizing function, while the black dots represent the maximum admissible variation b 0 k and b 0 k .

Fig. 3 :

 3 Fig.3: Position s, velocity ṡ and acceleration s of the applied law of motion before the optimization is run.

Fig. 4 :

 4 Fig. 4: Red: timing law obtained by the proposed method. Blue: original timing law.

2 ]Fig. 5 :

 25 Fig.5: Blue: joint accelerations calculated by the proposed method. Green: original joint accelerations. The three plots are related to joint 1, 2, and 3 starting from the top to the bottom.

2 ]Fig. 6 :

 26 Fig.6: Blue: acceleration calculated by the proposed method. Green: original acceleration profile. The three plots are related to joint 4, 5, and 6 starting from the top to the bottom.

TABLE I :

 I Comparison of the energy consumption (computed as in (23)) for the original and the optimized timing law.

	Joint Original timing law Optimized timing law %saving
	1	0.72	0.51	29
	2	3.18	2.40	24.5
	3	0.181	0.13	29
	4	6.01	4.54	24.4
	5	0.41	0.29	29
	6	9.73	7.35	24.5
	TOT	20.23	15.21	24.8