As stated in the Introduction, we shall focus here on minimizing trajectory execution time, which is the most common optimization objective in the industry. When planning at the path level, minimizing execution time can be equated to minimizing the path length, which is covered in section “Minimizing Path Length.” At the trajectory level, however, shortest paths may not correspond to minimum-time trajectories when kinodynamic constraints come into play. Section “Minimizing Trajectory Duration” reviews methods to specifically minimize trajectory duration.

Minimizing Path Length

Asymptotically Optimal Methods

While the roadmap-based methods presented in section “Grid Search and Probabilistic Roadmap (PRM)” address the feasibility problem, i.e., the problem of finding one feasible path, it is straightforward to modify them to include path length optimization. Indeed, once qstart and qgoal have been connected respectively to vertices u and v in G, classical graph algorithms, such as Dijkstra’s search (Dijkstra 1959) or A* search (Hart et al. 1968), can be applied to find the shortest path between u and v in G. In turn, the path lengths between qstart and qgoal are minimized.
It can be shown that these algorithms are asymptotically optimal in the sense that the path length of the solution returned by these algorithms converges to the minimal path length as the grid size – in the case of Grid Search – goes to 0, or as the number of sampled points goes to infinity – in the case of the Probabilistic Roadmap.
Regarding the single-query problem, it has been shown that the RRT method, if modified in the same way as above, would not yield an asymptotically optimal algorithm. However, RRT* – a modified version of RRT where the EXTEND function tries to extend from not only the nearest vertex but from a specific number of nearest vertices – possesses the asymptotic optimality property (Karaman and Frazzoli 2011).

Path Shortcutting

While the RRT* algorithm just mentioned has the nice property of being asymptotically optimal, it is too slow to be used in practice. For single-query problems, it turns out that a two-step approach consisting of (i) finding one path using RRT, followed by (ii) post-processing this trajectory by repetitively applying shortcuts realizes a good trade-off between path quality and computation time (Geraerts and Overmars 2007).
The shortcut method is presented in Algorithm 3. It is simple to implement, yet very effective. A modified version, called partial shortcut, consists in shortcutting one joint angle at a time, can yield even higher-quality paths but also requires a longer computation time (Geraerts and Overmars 2007).

Minimizing Trajectory Duration

Fixed-Path Time Minimization

Once a collision-free path has been found, one can give sample configurations along the path as input to the robot. However, for most modern robot manipulators, execution time can be greatly reduced if each sample configuration is accompanied with a time stamp, i.e., the time instant when the robot should reach that configuration. This requires time parameterizing the path, that is, transforming it into a trajectory.
More specifically, with the notations introduced in Definition 2 of section “Important Concepts,” minimizing the traversal time of a given path P is to find the time parameterization s such that T is minimal and that the parameterized trajectory (q(s(t)))t∈[0,T] satisfies given kinodynamic constraints.
When the constraints are bounds on the joint torques, a very efficient solution to this problem, based on Pontryagin’s maximum principle, was proposed in the 1980s (Bobrow et al. 1985; Shin and McKay 1985) and has been continuously improved until today (Pfeiffer and Johanni 1987; Slotine and Yang 1989; Shiller and Lu 1992; Pham 2013). This method can also be applied to other types of kinodynamic constraints such as gripper and payload constraints (Shiller and Dubowsky 1989) or bounds on the joint velocities and accelerations (Kunz and Stilman 2012). More recently, another family of algorithms to solve this fixed-path time minimization, based on convex optimization, was proposed in (Verscheure et al. 2009; Hauser 2013).

Global Time Minimization

A number of exact (Geering et al. 1985; Meier and Ryson 1990) and approximate (Yang and Slotine 1994) methods exist to directly find the time-optimal trajectory subject to torque bounds between two configurations. However, these methods are only practical for low-dimensional problems and cannot deal with geometric obstacles.
To take into account both geometric and kinodynamic (e.g., torque bounds) constraints, an effective approach consists of generating a large number of paths and on each path, apply the fixed-path time minimization described in the previous section. In (Bobrow 1988), the author considers a family of paths consisting of Bezier curves. A path of this family can be represented by a set of control points p = ( p1,. . ., pn). One can then define the cost C(p) by the duration of the time-minimal parameterization of the Bezier curve represented by p. Finally, one can search for the time-minimal trajectory by a gradient search, where the gradient dC/dp is evaluated numerically.
Another method (Shiller and Gwo 1991) consists in building roadmaps as in section “Minimizing Path Length,” but where the cost of an edge in the graph search would not be the distance between the adjacent vertices but a heuristic quantity related to the fixed-path time minimization algorithm of section “Fixed-Path Time Minimization.”

Shortcutting with Kinodynamic Constraints

As in the case of path planning (section “Path Shortcutting”), it turns out that shortcutting is the most effective method to obtain trajectories with short durations (Hauser and Ng-Thow-Hing 2010). There is, however, an important difference between trajectory and path shortcutting: in trajectory shortcutting, one usually needs to ensure that the new portion can be inserted into the original trajectory while preserving the smoothness properties of the original trajectory. For instance, if one wants to preserve the C1-continuity of the trajectory (i.e., the property that the trajectory is differentiable and that the derivative is continuous), then it is necessary to generate shortcuts that begin and end, not only at the same configurations q1, q2, but also with the same velocities q́1, q́2 as in the original portion.
Algorithm 4 presents shortcutting under velocity and acceleration (or pure kinematic) bounds (Hauser and Ng-Thow-Hing 2010). This algorithm is very effective thanks to the following property: given the beginning and ending configurations and velocities (q1, q́1), (q2, q́2), it is possible to compute analytically the time-optimal trajectory portion Πkin(q1, q́1, q2, q́2) under given velocity and acceleration bounds (Hauser and Ng-Thow-Hing 2010).
Algorithm 5 presents shortcutting under general kinodynamic bounds (Pham 2012). Contrary to the case of velocity and acceleration bounds, there is no analytic expression of the time-optimal trajectory for general kinodynamic constraints. One thus have to resort to the path decoupling approach presented earlier: (i) interpolate a path between (q1, q́1) and (q2, q́2) respecting C1-continuity, and (ii) time-parameterize the path optimally under the given kinodynamic constraints. Note that the heuristic to choose the path in step (i) is crucial for the performance of the algorithm.