Trajectory Planning

Abstract

Trajectory planning consists in finding a time series of successive joint angles that allows moving a robot from a starting configuration towards a goal configuration, in order to achieve a task, such as grabbing an object from a conveyor belt and placing it on a shelf. This trajectory must respect given constraints: for instance, the robot should not collide with the environment; the joint angles, velocities, accelerations, or torques should be within specified limits, etc. Next, if several trajectories are possible, one should choose the one that optimizes a certain objective, such as the trajectory execution time or energy consumption. This chapter reviews methods to plan trajectories with constraints and optimization objectives relevant to industrial robot manipulators.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

Introduction

For a robot to accomplish a given high-level task – such as grabbing an object on a conveyor belt and placing it on a shelf – this task must be translated into low-level commands understandable by the robot operating system. This whole process, known as “motion planning,” can be broken down into a number of steps, as illustrated in Fig. 1. Note that, for clarity, the different levels and steps were represented in a sequential, linear fashion. In practice, however, several levels or steps might sometimes be addressed at the same time: for instance, when planning at the “trajectory” level, one might have to take into account the robot torque bounds, which appear at the “actuator commands” level.

Fig. 1

From task to commands. The work-space task may consist in, e.g., bringing the end effector of the robot manipulator from a starting position and orientation in space, where it has just grabbed the object, towards a goal position and orientation, where it can safely release the object onto the shelf. Inverse kinematics finds the manipulator joint angles that allow achieving the desired end-effector position and orientation. The joint-space task would then consist in connecting the starting joint angles and the goal joint angles. Path planning finds a collision-free path, that is, a continuous set of intermediate joint angles between the starting and goal joint angles, such that the manipulator does not collide with the environment or with itself at any of the intermediate joint angles. Time parameterization consists in finding the time stamps for the joint angles along the path, while respecting, e.g., the torque bounds and/or optimizing the traversal time or the energy consumption. Finally, instruction synthesis translates the desired trajectory into a set of instructions in the robot language, and command synthesis converts these instructions into low-level commands, such as the electrical current fed into the motors. Usually, this last step is performed internally by the robot controller provided by the robot manufacturer and is not accessible to the end user

Until recently, motion planning has mostly been performed by human operators: for a given task, a skilled, experienced operator would manually find the commands (using, e.g., a teach pendant and visual feedback) that would allow the robot to achieve the task. However, this approach is time-consuming and usually produces suboptimal trajectories. Further, if the task changes (even slightly), the whole tedious teaching process has to be done over again.

Impressive theoretical advances in the field of motion planning in the past few decades have brought about a new picture: it is now possible for a computer to find, in minutes or seconds, optimal commands for a robot to achieve a given task, even in very challenging, cluttered environments. Several companies, some of them spinning off from academia (e.g., Siemens Kineo, France/Germany or Mujin Inc., Japan), are developing software solutions that are in use in actual factories.

The current chapter discusses trajectory planning, which is a part of motion planning. Specifically, we shall focus on the problem of finding a trajectory of the joint angles between given starting and goal joint angles, see Fig. 2. It is thus assumed that appropriate computations have been carried out before (e.g., grab synthesis, inverse kinematics, etc.) or will be carried out after (e.g., robot instruction synthesis, command synthesis, etc.) this stage.

Fig. 2

Trajectory planning for a two-link manipulator. (a) The manipulator and the obstacles (black rectangles) in the work space. (b) Trajectories connecting q _start and q _goal in the joint space

Two important concepts, namely, constraints and optimization, are essential in trajectory planning. Constraints restrict the range of motions that the robot can execute. One can classify them into two categories. First, geometric constraints are the constraints that can be expressed solely in terms of the robot joint angles: these include bounds on the joint angles, avoidance of self-collision and of collision with obstacles, etc. These constraints can thus be fully taken into account in the path planning step. Second, kinodynamic constraints are the constraints that include higher-order time derivatives of the joint angles, such as bounds on the joint velocities, accelerations, torques, or motor current inputs. These constraints cannot be taken into account by path planning alone and must be considered at the trajectory level.

Optimization comes into play when there are more than one path or trajectory that allow achieving the task while satisfying the constraints. It is then interesting to select the path or trajectory that optimizes a given objective. At the path level, one might be interested in finding the shortest path in joint space or in the end-effector space. At the trajectory level, other optimization objectives, such as minimum time, maximum smoothness, minimum energy, etc., can be considered. Note that because of kinodynamic constraints, a shortest path might not correspond to a minimum-time or minimum-energy trajectory.

Scope of this Chapter From the above discussion, it is clear that there exists a very large variety of trajectory planning problems (Hwang and Ahuja 1992; LaValle 2006), in terms of robot structures, constraints, optimization objectives, etc. The present chapter focuses on problems arising from the industry: we shall specifically consider serial robotic manipulators, constraints arising from obstacle avoidance and from bounds on the joint angles, velocities, accelerations and torques, and optimization objectives related to the trajectory execution time. Also, we shall concentrate more particularly on the methods that are actually in use in the industry rather than cover the whole panorama of existing planning methods.

Organization The remainder of this chapter is organized as follows. In section “Path and Trajectory Planning,” some methods for geometric path planning and kinodynamic trajectory planning are presented. The focus of that section is on finding one feasible path or trajectory – optimization is not considered. In section “Path and Trajectory Optimization,” methods for path and trajectory optimization under geometric and kinodynamic constraints are reviewed. As stated above, we shall mostly consider the minimization of trajectory execution time. Finally, section “Conclusion” offers a brief conclusive discussion.

Path and Trajectory Planning

Important Concepts

We first introduce the important concept of configuration space (Lozano-Perez 1983) in the context of serial robotic manipulators.

Definition 1

(Configuration space ). A set of joint angles q = (θ ₁, …, θ _n ), where n is the number of joints of the robot manipulator, is called a configuration. The set of all possible joint angles is called the configuration space and denoted $\mathcal{C}$. For instance, $\mathcal{C}={\left[-\pi, \pi \right]}^n$ for a manipulator with n revolute joints. If a robot is in collision with the environment or with itself at configuration q, then q is called a colliding configuration. The subset of $\mathcal{C}$ consisting of all non-colliding configurations is called the free space and denoted $\mathcal{C}$ _free.

It is assumed that the starting configuration q _start and the goal configuration q _goal are given. The problem thus consists in finding a path or a trajectory (see definition below) connecting q _start and q _goal and respecting the constraints.

Definition 2

(Path and trajectory). Formally, a path can be defined as a continuous function P

$$ \begin{array}{l}{P}:\kern0.36em \left[0,1\right]\to \mathcal{C}\\ {}\kern2.45em s\ \mapsto\ {\bf q}\left(s\right).\end{array} $$

For a path connecting q _start and q _goal , one thus has

$$ \mathbf{q}(0)={\mathbf{q}}_{\mathrm{start}},\kern1.25em \mathbf{q}(1)={\mathbf{q}}_{\mathrm{goal}}. $$

A path can be regarded as a geometric object devoid of any timing information. Next, a time parameterization of P is a strictly increasing function

$$ \begin{array}{l}s:\kern0.36em \left[0,T\right]\to \left[0,1\right]\\ {}\kern2.36em t\ \mapsto\ s(t)\end{array} $$

with s(0) = 0 and s(T) = 1. The function s gives the position on the path for each time instant t.

The path P endowed with a time parameterization s becomes a trajectory П

$$ \begin{array}{l}{\prod}:\kern0.36em \left[0,T\right]\to \mathcal{C}\\ {}\kern1.38em t\ \mapsto\ {\bf q}\left(s(t)\right),\end{array} $$

with T being the duration of the trajectory. Note that the same path P can give rise to many different trajectories П.

Path Planning Under Geometric Constraints

There exists a large variety of approaches to path planning : combinatorial methods, potential field methods, sampling-based methods, etc.

Combinatorial methods make use of algebraic geometry and mathematical programming tools to provide complete algorithms to the path planning problems. “Complete” means that the algorithm can decide in finite time whether the problem has a solution and provide a solution when it exists. An example of a combinatorial method is the cell decomposition method: when the obstacles are polygonal, the free space $\mathcal{C}$ _free can be decomposed into cells (e.g., triangular cells by a Delaunay triangulation). Solving the problem then boils down to finding a sequence of adjacent cells such that q _start and q _goal belong to respectively the first and the last cell of the sequence (see, e.g., LaValle 2006, Sect. 6.3.2). While combinatorial methods are valuable to obtain theoretical results (for instance, they allow proving that the general motion planning problem is NP complete), they are too slow to be used in practice, especially in high-dimensional problems. Another issue is that these methods require an explicit representation of the obstacles, which is very difficult to obtain in most practical problems.

The potential field method was introduced in (Khatib 1986). This method constructs a potential field which is high near the obstacles and low at the goal configuration. Steering towards the goal configuration while avoiding the obstacles can then be naturally achieved by letting the robot configuration evolve in that potential field. This method is interesting in that it allows real-time control. However, the issue of local minima – the robot may get trapped in the local minima of the potential field – prevents its use in highly cluttered environments.

Sampling-based methods are perhaps the most efficient and robust, hence probably the most widely used methods for path planning currently. The next two sections review these methods in detail.

Grid Search and Probabilistic Roadmap (PRM)

A first category of sampling-based methods requires building, in a preprocessing stage, a roadmap that captures the connectivity of $\mathcal{C}$ _free. A roadmap is a graph G whose vertices are configurations of $\mathcal{C}$ _free. Two vertices are connected in G only if it is possible to connect the two configurations by a path entirely contained in $\mathcal{C}$ _free. Once such a roadmap is built, it is easy to answer the path planning problem: if (i) q _start can be connected to a (nearby) vertex u and (ii) q _goal can be connected to a (nearby) vertex v and (iii) v and v are in the same connected component of G (in the graph-theoretic sense), then there exists a collision-free path between q _start and q _goal (see Fig. 3).

Fig. 3

Roadmap-based methods. (a) Grid search. The starting and goal configurations are represented by respectively the blue square and the yellow star. Green disks represent sampled configurations lying in $\mathcal{C}$ _free, while red disks represent obstacle configurations. (b) Probabilistic roadmap. Same legends as in (a)

Methods for building the roadmap fall into two families: deterministic or probabilistic. A typical deterministic method is the Grid Search , where the configuration space $\mathcal{C}$ is sampled following a regular grid, as in Fig. 3a. A sampled configuration is rejected if it is not in $\mathcal{C}$ _free. Next, one attempts to connect every pair of adjacent configurations (adjacent in the sense of the grid structure) to each other: if the straight segment connecting the two configurations is contained within $\mathcal{C}$ _free, then the corresponding edge is added to the graph G.

In the Probabilistic Roadmap method (Kavraki et al. 1996), instead of following a regular grid, samples are taken at random in $\mathcal{C}$ _free, see Fig. 3b. Since there is no a priori grid structure, several methods exist for choosing the pairs of vertices for which connection is attempted: for instance, one may attempt, for each vertex, to connect it to every vertices that are within a specified radius r from it.

Advantages of the Roadmap-Based Methods The strength of the roadmap-based methods (both deterministic and probabilistic) comes from the global/local decomposition – the difficult problem of path planning is solved at two scales: the local scale, where neighboring configurations (adjacent configurations in Grid Search, configurations within a small distance r of each other in the Probabilistic Roadmap) are connected by a simple straight segment; and the global scale, where the graph search takes care of the global, complex connectivity of the free space.

Note also that it is not necessary for these methods to have an explicit representation of $\mathcal{C}$ _free: one only needs an oracle which says whether a given configuration is in $\mathcal{C}$ _free. To check whether a straight segment is contained within $\mathcal{C}$ _free, it suffices to call the oracle on every configuration (or, in practice, on sufficiently densely sampled configurations) along that segment.

These methods also possess nice theoretical guarantees. Specifically, it can be shown that the Grid Search is resolution complete, which means that if a solution exists, the algorithm will find it in finite time and for a sufficiently small grid size. Similarly, it can be shown that the Probabilistic Roadmap method is probabilistically complete, which means that, if a solution exists, the probability that the algorithm will find it converges to 1 as the number of sample points goes to infinity (LaValle et al. 2004). However, the converge rate of both methods is difficult to determine on practical problem instances.

Regarding the comparative performances of the deterministic and probabilistic approaches, it has been shown both theoretically and practically that randomness is not advantageous in terms of search time. However, it can be argued that probabilistic methods are easier to implement (LaValle et al. 2004).

Rapidly Exploring Random Trees (RRT)

The methods just discussed require building the entire roadmap in the preprocessing stage before being able to answer any query [a query being a pair (q _start q _goal) to be connected]. In applications where only a single or a limited number of queries are needed, it may not be worthy to build the whole roadmap. Single query methods, such as the Rapidly Exploring Random Trees (RRT), are much more suited for these applications (Lavalle et al. 1998).

Specifically, RRT iteratively builds a tree (see Algorithm 1), which is also a roadmap, but which has the property of exploring “optimally” the free space. The key lies in the EXTEND function, which selects the vertex in the tree that is the closest to the randomly sampled configuration (see Algorithm 2 and Fig. 4). Thus, the probability for a vertex in the tree to be selected is proportional to the size of its Voronoi region, causing the tree to grow preferably towards previously under-explored regions (Lavalle et al. 1998).

Fig. 4

Illustration for the EXTEND function. The tree is rooted at the black disk. Red disks and plain segments represent respectively the vertices and edges that have already been added to the tree. EXTEND attempts at growing the tree towards a random configuration q _rand. For this, q _near is chosen as the vertex in the tree that is the closest to q _rand. The tree is then grown from q _near towards q _rand, stopping at q _new, which is at the specified radius r from q _near

Algorithm 1

BUILD_RRT

Input: A starting configuration q _start

Output: A tree $\mathcal{T}$ rooted at q _start

$\mathcal{T}$.INITIALIZE(q _start)

for rep = 1 to N _maxrep do

q _rand ← RANDOM_CONFIG()

EXTEND($\mathcal{T}$, q _rand)

end for

Algorithm 2

EXTEND

Input: A tree $\mathcal{T}$ and a target configuration q

Effect: Grow $\mathcal{T}$ by a new vertex in the direction of q

q _near ← NEAREST_NEIGHBOR ($\mathcal{T}$,q)

if q _new ← STEER (q _near , q) succeeds then

Add vertex q _new to $\mathcal{T}$

Add edge [q _near, q _new] to $\mathcal{T}$

end if

Note: STEER(q _near, q) attempts making a straight motion from q _near towards q. Three cases can happen: (i) q is within a given distance r of q _near and [q _rand, q] is collision-free, then q is returned as the new vertex q _new; (ii) q is farther than a given distance r of q _near, and the segment of length r from q _near and in the direction of q is collision-free, then the end of that segment is returned as q _new (see Fig. 4); (iii) else, STEER returns failure

Finally, to find a path connecting q _start and q _goal, one can grow simultaneously two RRTs, one rooted at q _start and the other rooted at q _goal, and attempt to connect the two trees at each iteration. This algorithm is known as bidirectional RRT (Kuffner and Lavalle 2000). In practice, bidirectional RRT has proved to be easy to implement, yet extremely efficient and robust: it has been successfully applied to a large variety of robots and challenging environments.

Trajectory Planning Under Kinodynamic Constraints

As mentioned earlier, kinodynamic constraints involve higher-order derivatives of the configuration and cannot therefore be expressed in the configuration space. One approach to kinodynamic planning thus consists of transposing the path planning methods (such as PRM or RRT) to the state-space $\mathcal{X}$, that is, the configuration space $\mathcal{C}$ augmented with velocity coordinates, where the kinodynamic constraints can be appropriately taken into account (Donald et al. 1993; LaValle and Kuffner 2001; Hsu et al. 2002). One drawback of this approach is that moving into the state space is associated with a twofold increase in the dimension of the search space: if $\mathcal{C}$ is of dimension n, then $\mathcal{X}$ is of dimension 2n. Since planning algorithms usually have complexities that scale exponentially with the dimension of the search space (Hsu et al. 2002), a twofold increase in the dimension may make state-space kinodynamic planning algorithms impractical even for relatively small values of n (Shiller and Dubowsky 1991).

A second approach avoids the complexity explosion by decoupling the problem: first, search for a path in the robot configuration space $\mathcal{C}$ under geometric constraints (using path planning methods discussed earlier) and, in a second step, find a time parameterization of that path that satisfies the kinodynamic constraints (Bobrow et al. 1985; Kuffner et al. 2002). The drawback here is that the path found in the first step may have no time parameterization at all that respects the kinodynamic constraints. This drawback may be addressed by introducing an Admissible Velocity Propagation scheme, which allows checking, at each PRM/RRT extension, the existence of an eventual admissible time parameterization (Pham et al. 2013).

Path and Trajectory Optimization

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 q _start and q _goal 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 q _start and q _goal 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).

Algorithm 3

PATH_SHORTCUT

Input: A collision-free path P

Output: A shorter collision-free path

for rep = 1 to N _maxrep do

Pick two random points q _1, q ₂ along the path

if [q _1, q ₂] is collision-free then

Replace the portion of P between q ₁ and q ₂ by [q ₁, q ₂]

end if

end for

Note: [q ₁, q ₂] denotes the straight segment between q ₁ and q ₂ in the joint space.

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 = (p ₁ ,…, p _n ). 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 C ¹-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 q ₁, q ₂, but also with the same velocities ${\dot{\mathbf{q}}}_1,{\dot{\mathbf{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 $\left({\mathbf{q}}_1,{\dot{\mathbf{q}}}_1\right),\left({\mathbf{q}}_2,{\dot{\mathbf{q}}}_2\right)$, it is possible to compute analytically the time-optimal trajectory portion ${\prod}^{\mathrm{kin}}\left({\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2\right)$ 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 $\left({\mathbf{q}}_1,{\dot{\mathbf{q}}}_1\right)$ and $\left({\mathbf{q}}_2,{\dot{\mathbf{q}}}_2\right)$ respecting C ¹-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.

Algorithm 4

TRAJ_SHORTCUT_KINEMATIC

Input: A collision-free, C ¹ trajectory ∏ satisfying velocity and acceleration constraints

Output: A collision-free, C ¹ trajectory satisfying the velocity and acceleration constraints and with shorter time duration

for rep = 1 to N _maxrep do

Pick two random points q ₁,q ₂ along ∏

if ∏ ^kin(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) is collision-free then

Replace the portion of ∏ between q ₁ and q ₂ by ∏ ^kin(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$)

end if

end for

Note: ∏ ^kin (${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) denotes the optimal trajectory between (${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1$) and (${\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) under given velocity and acceleration bounds (see text)

Algorithm 5

TRAJ_SHORTCUT_GENERAL

Input: A collision-free, C ¹ trajectory ∏ satisfying general kinodynamic constraints

Output: A collision-free, C ¹ trajectory satisfying the kinodynamic constraints and with shorter time duration

for rep = 1 to N _maxrep do

Pick two random points q ₁,q ₂ along ∏

Generate a path P ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$)

if P ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) is collision-free then

Time-parameterize P ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$)into ∏ ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$)

if ∏ ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) has shorter time duration than the original portion then

Replace the portion of ∏ between q ₁ and q ₂ by ∏ ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$)

end if

end if

end for

Note: P ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) denotes an interpolated path between (${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1$) and (${\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$).

∏ ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) denotes the time-optimal trajectory obtained by time-parameterizing

P ^int(${\mathbf{q}}_1,{\dot{\mathbf{q}}}_1,{\mathbf{q}}_2,{\dot{\mathbf{q}}}_2$) under given kinodynamic constraints (see text)

Summary

We have presented an overview of trajectory planning and optimization methods, with a special emphasis on those relevant to industrial robotic manipulators. It appears from this overview that very efficient methods exist for planning high-quality trajectories when the environment (consisting of the robot, the obstacles, etc.) is well defined and static. A typical work flow, may integrate some of these methods as sketched in Fig. 5. The main current challenge of trajectory planning in classical factory automation lies mainly in the development of robust software, as well as practical integration into the work place.

Fig. 5

Typical work flow as practiced in a company specialized in motion planning for industrial robots

The next major step in factory automation is to integrate the robot more tightly with human operators. For this, new methods must be developed, taking into account environments that are by nature time changing, and sometimes in an unpredictable way, because of the close, possibly physical interaction with human operators. In this context, other types of constraints and optimization objectives must also be considered, such as safety or compliance.