$\newcommand{\d}{\mathrm{d}} \newcommand{\bff}{\boldsymbol{f}} \newcommand{\bfg}{\boldsymbol{g}} \newcommand{\bfp}{\boldsymbol{p}} \newcommand{\bfq}{\boldsymbol{q}} \newcommand{\bfx}{\boldsymbol{x}} \newcommand{\bfu}{\boldsymbol{u}} \newcommand{\bfv}{\boldsymbol{v}} \newcommand{\bfA}{\boldsymbol{A}} \newcommand{\bfB}{\boldsymbol{B}} \newcommand{\bfC}{\boldsymbol{C}} \newcommand{\bfM}{\boldsymbol{M}} \newcommand{\bfJ}{\boldsymbol{J}} \newcommand{\bfR}{\boldsymbol{R}} \newcommand{\bfT}{\boldsymbol{T}} \newcommand{\bfomega}{\boldsymbol{\omega}} \newcommand{\bftau}{\boldsymbol{\tau}}$

Time-parameterization

The paths found in Section Path planning are geometric objects devoid of any timing information and, as such, cannot be executed directly on the robot. One simple solution may consist in time-parameterizing the path using a constant path velocity. However, this solution is usually unsatisfactory: either the velocity is too high and the robot will violate kinodynamic constraints at some positions on the path, or the velocity is too low, hence inefficient. This section presents algorithms to find optimal time-parameterizations, i.e. which minimize traversal time while respecting given kinodynamic constraints.

Consider a path $P$ in the configuration space

The objective is to find a time-parameterization $s$

so that the retimed trajectory $\bfq(s(t))$ satisfies given constraints while minimizing trajectory duration $T$, which is the most important criterion in industrial robotics.

Time-parameterization of straight segments under velocity and acceleration bounds

Velocity and acceleration bounds are the most common constraints in industrial robotics. For a n-DOF robot, they have the form

Paths returned by motion planners (see Section Path planning) are usually composed of straight segments. A straight segment between $\bfq_a$ and $\bfq_b$ is given by $\bfq(s):= \bfq_a + s (\bfq_b-\bfq_a)$. Differentiating with respect to $t$, one has $\dot\bfq = \dot s (\bfq_b-\bfq_a)$ and $\ddot\bfq = \ddot s (\bfq_b-\bfq_a)$.

Therefore, the optimization problem becomes: find the function $(s(t))_{t\in[0,T]}$ such that $T$ is minimal and that

and $s(0) = 0$, $s(T) = 1$, $\dot s(0) = 0$, $\dot s(T) = 0$. Note that the initial and final velocities are constrained to be zero so as to ensure velocity continuity at the junction of different path segments.

This problem has a closed-form solution:

1. If $\dot s_{\max} \geq \sqrt{\ddot s_{\max}}$, then the optimal profile is composed of two segments:

   i. acceleration $\ddot s_{\max}$ between time instants 0 and $t_s$;

   ii. deceleration $-\ddot s_{\max}$ between time instants $t_s$ and $2t_s$, where $t_s := 1/\sqrt{\ddot s_{\max}}$;

2. Else, the optimal profile is composed of three segments:

   i. acceleration $\ddot s_{\max}$ between time instants 0 and $t_0$;

   ii. zero acceleration (constant velocity) between time instants $t_0$ and $t_0+t_1$;

   iii. deceleration $-\ddot s_{\max}$ between time instants $t_0+t_1$ and $2t_0+t_1$, where $t_0 := \dot s_{\max}/\ddot s_{\max}$ and $t_1 := 1/\dot s_{\max} - \dot s_{\max}/\ddot s_{\max}$.

Time-parameterization of arbitrary paths under general second-order bounds

Second-order (kinodynamic) bounds are constraints of the form

where $\bfA(\bfq)$, $\bfB(\bfq)$, and $\bff(\bfq)$ are, respectively, an M x n matrix, an n x M x n tensor, and an M-dimensional vector.

Inequality (*) is very general and may represent a large variety of second-order systems and constraints. As an example, consider an n-DOF manipulator with dynamics

Assume that the manipulator is subject to lower and upper bounds on the joint torques, that is, for any joint i and time t,

Clearly, these torque bounds can be put in the form of (*) with

Contrary to the case of velocity and acceleration bounds and linear paths, there is no closed-form solution in the general case of second-order constraints and arbitrary paths. However, there is a very efficient algorithm, first proposed by Bobrow in the 1980's and later perfected by many others, to numerically find the optimal time-parameterization, see Pham and Pham (2018). An implementation of the algorithm can be found at https://github.com/hungpham2511/toppra.

To learn more about this topic

• Hauser, K., & Ng-Thow-Hing, V. (2010). Fast smoothing of manipulator trajectories using optimal bounded-acceleration shortcuts. In Robotics and Automation (ICRA), 2010 IEEE International Conference on (pp. 2493-2498).

• H. Pham, Q.-C. Pham. A new approach to Time-Optimal Path Parameterization based on Reachability Analysis. IEEE Transactions on Robotics, vol. 34(3), pp. 645–659, 2018.