Motion planning in high-dimensional spaces

Motion planning is a key tool that allows robots to navigate through an environment without collisions. The problem of robot motion planning has been studied in great detail over the last several decades, with researchers initially focusing on systems such as planar mobile robots and low degree-of-freedom (DOF) robotic arms. The increased use of high DOF robots that must perform tasks in real time in complex dynamic environments spurs the need for fast motion planning algorithms. In this overview, we discuss several types of strategies for motion planning in high dimensional spaces and dissect some of them, namely grid search based, sampling based and trajectory optimization based approaches. We compare them and outline their advantages and disadvantages, and finally, provide an insight into future research opportunities.


I. INTRODUCTION
A fundamental need in robotics is to have algorithms that convert high-level specifications of tasks into low level descriptions of how to move. The term motion planning is often used for these kinds of problems [1]. Motion planning is therefore an indispensable skill for robots that aspire to navigate through an environment without collisions. Motion planning algorithms attempt to generate trajectories through the robot's configuration space that are both feasible and optimal based on some performance criterion that may vary depending on the task, robot, or environment. There are two common paradigms for successful motion planning. The first paradigm separates motion planning into path planning and path execution. The second paradigm directly finds the trajectory of the robot -it takes into account dynamic constraints to plan the full robot motion from one point to the other. Path planning is used to find the shortest, or otherwise optimal, collision-free path between two points in environment while not taking into account the temporal component of robot motion. Path execution is then used to follow the planned path, leveraging methods from control theory. The initial attempts to solve path planning problems included grid search methods such as the A * algorithm [2], reactive planners such as bug algorithms [3], combinatorial planning methods originating from computational geometry such as cell decompositions [4], visibility graphs [5] and Voronoi diagrams [6], [7].
All of the mentioned methods are complete, meaning that they find a solution if it exists and report failure otherwise. They present elegant solutions for low dimensional configuration spaces with static obstacles. However, bug algorithms produce unnecessarily long paths, while grid search methods and combinatorial approaches suffer from the so-called curse of dimensionality, i.e. they quickly become computationally intractable with the increase of the configuration space dimension. That also means that replanning is possible, making those approaches ineffective in dynamic environments.
The increasing complexity of robots and the environments that they operate in has spurred the need for high-dimensional motion planning. Consider, for instance, a personal robot operating in a cluttered household environment or a humanoid robot performing navigation and manipulation tasks in an unstructured environment. Efficient motion planning is important to enable these high degree-of-freedom (DOF) robots to perform tasks, subject to motion constraints while avoiding collisions with obstacles in the environment. Processing time is especially important in dynamic environments where replanning is necessary. Those considerations lead to the development of grid-based methods with ameliorated efficiency and to the development of sampling-based motion planning algorithms which offer weaker guarantees than combinatorial methods but are more efficient. Sampling-based algorithms abandon the concept of explicitly charaterizing the configuration space -they use a collision detection algorithm to probe the configuration space to see whether some configuration lies in free space or not. Sampling-based algorithms are probabilistically complete, meaning that the probability they will produce a solution approaches one as more time is spent.
All of the discussed techniques so far aim at capturing the connectivity of free configuration space into a graph. The exigency for efficient and fast planning methods lead to development of the planning paradigm which directly finds the trajectory of the robot. Potential field methods were the initial approach of that paradigm. They model the robot, which is represented as a point in configuration space, as a particle under the influence of an artificial potential field. The artificial potential field is created by the attractive force from goal point and repulsive forces from obstacles in configuration space. To obtain the policy which moves the robot safely to the goal, one would simply perform gradient descent on the potential function. Main disadvantages of potential field methods are the lack of completness and optimality guarantees and the presence of local minima for a non point-mass robot.
Recently, the trajectory optimization methods that are appropriate for very high DOF robots have been proposed. The trajectory is encoded as a sequence of states and controls. Trajectory optimization approaches start with an initial trajectory and then minimize an objective function in order to optimize the trajectory according to some criterion. Those approaches are exceptionally fast, however, unlike samplingbased approaches, they only find locally optimal solutions.
The overview is organized as follows. Section II elaborates grid-based approaches to motion planning. In Section III, sampling based planning methods are presented. Afterwords, Section IV describes trajectory optimization methods. Lastly, conclusion is given in Section V with focus on importance of high-dimensional motion planning.

II. GRID-BASED APPROACHES
Grid-based motion planning approaches overlay a grid on configuration space and assume each configuration is corresponding to a grid point. The robot is allowed to move to adjacent grid points as long as the line between them is collision-free, i.e. contained within free configuration space. This discretizes the set of actions, and search algorithms, for instance the A * algorithm [2], are used to find a path from the start to the goal. These approaches require setting a grid resolution. Search is faster with lower resolution, but the algorithm will fail to find paths through narrow portions of free configuration space. The D * algorithm [8], along with its improved variants Focussed D * [9] and D * Lite [10], is a commonly used algorithm based on the A * capable of planning paths in unknown, partially known, and changing environments in an optimal and complete manner.
Altough naive application of A * is typically unsuitable for high-dimensional planning problems, since number of points on the grid grows exponentially in the configuration space dimension, current research has made advances in applying forward search to systems with many DOFs. Until recently, a major problem with A * and related algorithms had been that admissible heuristics result in examination of prohibitively large portions of the configuration space, whereas inflated heuristics cause significantly suboptimal behavior. In [11], a framework for efficiently updating an A * search while smoothly reducing heuristic inflation is presented, allowing resolution complete search in an anytime fashion on a broader variety of problems than previously computable. It starts by finding a suboptimal solution quickly using a loose bound, then tightens the bound progressively as time allows. Given enough time it finds a provably optimal solution. Additional work has examined inducing smoother motion while reducing the cardinality of the action set [12], as well as reusing partial plans discovered by past searches [13]. Since gridbased methods are complete, they are often used for planning in environments featuring bottlenecks and other such narrow passages. The main drawback of grid-based approaches is their computational complexity, as even the state-of-the-art methods suffer from the curse of dimensionality, and become computationally intractable for very high DOF systems such as humanoid robots. Right: RRT exploring environment with one obstacle after 500 iterations [14]. The root of the tree is shown as green circle.

III. SAMPLING-BASED METHODS
Sampling-based approaches have become popular in the domain of high-dimensional motion planning, including manipulation planning. In a sense, these approaches attempt to capture the connectivity of the robot configuration space by sampling it [14]. Randomized approaches have its advantages in terms of providing efficient solutions for challenging problems [1]. The downside is that the solutions are widely regarded as suboptimal. Sampling based planners are understood to be probabilistically complete [15], a weaker notion of completeness that ensures a solution will be provided, if one exists, given sufficient runtime of the algorithm (even if it means infinite runtime). However, they cannot determine if no solution exists. Sampling-based approaches typically work in a two-step fashion. First, a collision-free path is discovered without regard for any measure of cost. Second, the obtained path is improved by applying certain heuristics. For the first step, a Monte-Carlo algorithm for path planning with many degrees of freedom [16] was a seminal work that demonstrated a solver for impressively difficult problems. Perhaps the most commonly used algorithms are the probabilistic roadmap (PRM) [17] and rapidly exploring random trees (RRT) [18]. The PRM method was shown to be well suited for path planning in configuration spaces with many DOFs, and with complex constraints, including kinodynamic [19], [20]. RRT has also been applied to differential constraints, and was shown to be successful for general high dimensional planning [21]. The intuitive implementation of both RRT and PRM, and the quality of the solutions, lead to their widespread adoption in robotics and many other fields. Even though the idea of connecting points sampled randomly from the state space is essential in both RRT and PRM, these two algorithms differ in the way that they construct a graph connecting these points [22]. Other sampling-based path planners of note include expansive space trees (EST) [23] and sampling-based roadmap of trees (SRT) [24]. The EST employs a function that sets the probability of node selection based on neighboring nodes, unlike RRT where sampling is uniform. The SRT combines the main features of multiple-query algorithms such as PRM with those of single-query algorithms such as EST and RRT. For improving the planned path, one popular approach is the shortcut heuristic, which picks pairs of configurations along the path and invokes a local planner to attempt to replace the intervening sub-path with a shorter one [25], [26]. Methods such as medial axis and partial shortcuts have also proven effective [27].
PRM implements two main procedures to generate a probabilistic roadmap. A learning phase occurs first, where the configuration space is randomly sampled for a certain amount of time. The sampled configurations are declared as vertices if they lie in free configuration space and are connected to nearby vertices with a local planner, while those in the obstacle space are discarded. This is followed by a query phase where the start and goal configurations are defined and connected to the roadmap. A graph search is then performed to find the shortest path through the roadmap between start and goal configurations. An example of a roadmap built in the PRM learning phase is depicted in Fig. 1. Roadmaps are sometimes referred to as forests, as an analogy to trees in RRT. PRM has the inherent ability to solve different instances of the planning problem in the same environment, which is a product of maintaining the roadmap and specifying start and goal configurations in a subsequent stage. It is therefore referred to as a multi-query planner. Planning time is invested in sampling and generating a roadmap, but queries are solved quickly. PRM was initially developed for articulated robots, but has been extended for non-holonomic car-like robots.
RRT represents another category of sampling based planners, which are single-query planners. A tree is incrementally grown from the start configuration to the goal configuration, or vice versa. A configuration is randomly selected in the configuration space. If it lies in the free space, a connection is attempted to the nearest vertex in the tree. As a result of uniform sampling, the RRT is more likely to select samples in larger Voronoi regions and the tree is incrementally and rapidly grown towards that free space [14]. An example of a tree generated by RRT algorithm exploring environment with one obstacle is shown in Fig. 1. For single query problems, RRT is faster compared to PRM, since it does not require a learning phase.
Sampling based approaches typically do not explicitly optimize an objective function, altough variants of PRM and RRT which are provably asymptotically optimal have been proposed in [22]. As the sampling-based planners became increasingly well understood in recent years, it was suggested that randomization may not, by itself, account for their efficiency [28]. It was shown that quasi-random sampling sequences can accomplish similar or better performance than their randomized counterparts [29]. The main disadvantage of sampling-based motion planning methods is that the obtained paths often manifest redundant and jerky motion and hence require post processing to smooth and shorten the computed trajectories. Furthermore, considerable computational effort is expended in sampling and connecting samples in portions of the configuration space that might not be relevant to the task.

IV. TRAJECTORY OPTIMIZATION
The key motivation for trajectory optimization is the focus on producing optimal motion: incorporating dynamics, smoothness, and obstacle avoidance in a mathematically precise objective. Despite a rich theorethical history and successful applications, most notably in the control of spacecraft and rockets, trajectory optimization techniques have had limited success in motion planning. Much of this may be attributed to two causes: the large computational cost fot evaluating objective functions and their higher-order derivatives in highdimensional spaces, and the presence of local minima when considering motion planning as a (generally non-convex) continuous optimization problem.
A significant amount of recent work has focused on trajectory optimization and related problems. The use of potential fields for avoiding obstacles, including dynamic ones, was first proposed in [30]. The methods sensitivity to local minima has been addressed in a range of related work. Analytical navigation functions that are free of local minima have been proposed for some specific environments [31]. A global potential field to push the robot away from configuration space obstacles, starting with a trajectory that was in collision was proposed in [32]. As a part of a local optimization that tries to shorten and smooth the trajectory, the free part of configuration space can be locally approximated as a union of spheres around the current trajectory [33]. The trajectory is modelled as a massspring system, an elastic band, and replanning is performed by scanning back and forth along the elastic, while moving one mass particle at a time. An extensive effort is made to construct a model of the free space, and the cost function contains terms that attempt to control the motion of the trajectory particles along the elastic. This method was further extended in [34] where the real-time application to a high-degree-offreedom humanoid robot was presented. The aforementioned approaches locally approximate the free space using a union of spheres, which is an overly conservative approximation and may not find feasible trajectories even if they exist.
More recent trajectory optimization methods play two important roles in robot motion planning. Firstly, they can be used to smooth and shorten trajectories computed by other planning methods such as sampling-based planners. Secondly, they can be used to compute locally optimal, collision-free trajectories from scratch starting from naive trajectory initializations that might be in collision with obstacles [35]. The state-of-the-art trajectory optimization methods all start with an initial (commonly straight-line) trajectory and then minimize an objective function in order to optimize the trajectory according to some criterion. An illustrative example of motion planning for mobile manipulator in simulation is shown in Fig.  2. The figure shows both the initial trajectory that is in collision with obstacles and the optimized, collision-free one. The goal of trajectory optimization for motion planning is to obtain trajectories θ(t) that minimize costs and satisfy constraints. Motion planning can thus be formalized as where the trajectory θ(t) is a continuous-time function that maps time t to states, which are usually robot configurations and perchance higher order derivatives. F[θ(t)] is a cost functional or objective that encodes collision costs that enforce the trajectory to be collision-free and the quality of a trajectory that usually corresponds to smoothness and is evaluated as a minimization of velocity or acceleration [36]. G i [θ(t)] represents inequality constraints, for example joint angle limits, and H i [θ(t)] represents task-dependent equality constraints, such as the desired start and end configurations and velocities or the desired end-effector orientation, i.e. holding a glass full of liquid upright. Depending on the specific problem, the number of inequality constraints may be zero. Collision cost may also appear as an obstacle avoidance inequality constraint [35], depending on the optimization technique used to solve (1). Most existing trajectory optimization algorithms in practice work with a finely discretized trajectory, which is good for reasoning about tight navigation constraints or thin obstacles, but can cause a large computational cost. Covariant Hamiltonian Optimization for Motion Planning (CHOMP) [37], [36] revived interest in trajectory optimization methods by demonstrating the effectiveness on several robotic platforms, including a mobile manipulation platform and a quadruped. The key feature of CHOMP is a formulation of trajectory costs that are invariant to the time parametrization of the trajectory. In order to produce smooth robot motion while avoiding obstacles, CHOMP uses two objective functionals: a smoothness term which captures dynamics of the trajectory and an obstacle functional which captures the requirement of avoiding obstacles and preferring margin from them. They define the smoothness functional in terms of a metric in the space of trajectories. The obstacle functional is developed as a line integral of a scalar cost field c -a precomputed signed distance field, defined so that it is invariant to retiming. Regardless of how fast or slow the arm moves through the field, it will accumulate the exact same cost. An example of a precomputed signed distance field used for the obstacle functional is shown in Fig. 3. The two functionals have complementary role: the obstacle functional governs the shape of the path, and the smoothness functional governs the timing along the path.
Several augmentations of CHOMP have been proposed. Multigrid CHOMP with local smoothing [38] which improves the runtime of CHOMP under constraints, whithout significantly reducing optimality. T-CHOMP [39] is a functional gradient algorithm that directly optimizes in space-time, thus being able to successfully incorporate constraints and cost functions that explicitly depend on time. Inceremental trajectory optimization algorithm (ITOMP) [40] enables real-time replanning in dynamic environments. In contrast to CHOMP, which exploits the availability of the gradient, Stochastic Trajectory Optimization for Motion Planning (STOMP) [41] samples a series of noisy trajectories to explore the space around an initial trajectory which are then combined to produce an updated trajectory with lower cost. The key trait of STOMP is its ability to optimize non-differentiable constraints.
An important shortcoming of CHOMP and related methods is the need for many trajectory states in order to reason about fine resolution obstacle representations or find feasible solutions when there are many constraints. The framework called TrajOpt [42], [35] formulates motion planning as sequential quadratic programming problem and features convex collision checking. Sequential convex optimization involves solving a series of convex optimization problems that approximate the cost and constraints of the original problem. The ability to add new constraints and costs to the optimization problem allowes TrajOpt to tackle a larger range of motion planning problems, including planning for underactuated, non-holonomic systems. For collisions, TrajOpt uses signed distances using convexconvex collision detection and takes into account continuoustime safety by considering the swept-out volume of the robot between time steps. An illustration of swept volume is shown in Fig. 3. This formulation allows for sparsely sampled trajectory, which makes TrajOpt very computationally efficient in practice. However, if the smoothness is required in the output trajectory, either a densely parametrized trajectory or post-processing of the trajectory might still be needed thus increasing computation time.
A continous-time trajectory representation can avoid the computational cost incurred by using large number of states and yield a more efficient approach, while maintaining the smoothness of trajectory. Linear interpolation, splines, and hierarchical wavelets have been used to represent trajectories in state estimation and filtering. Recently, kernel methods [43] and B-splines [44] have been used in a similar manner to represent trajectories with fewer states in motion planning problems.
The Gaussian process motion planning family of algorithms [45]- [49] uses continous-time trajectory representation; specifically, they view trajectories as functions that map time to robot state. The continuous-time trajectory is represented as a sample from a Gaussian process (GP) generated by a linear time-varying stochastic differential equation. They show that GPs inherently provide a notion of trajectory Fig. 3: Left: signed distance field for use in collision cost in CHOMP [36] and GPMP2 [46]. Right: illustration of swept volume for use in continuous collision cost in TrajOpt [35] optimality through a prior. Efficient structure-exploiting GP regression facillitates querying the trajectory at any time of interest in O(1) complexity [50]. Using this representation, they developed GPMP (Gaussian Process Motion Planner), a gradient-based optimization algorithm that can efficiently overcome the large computational costs of fine discretization while maintaining smoothness in the result.
Through the GP formulation, they view motion planning as probabilistic inference [51], [52]. Similarly to the notion of trajectory optimality being captured by a prior on trajectories, one can also view the notion of feasibility probabilistically, encoded in a likelihood function. Through the use of factor graphs [53], Bayesian inference can be used to compute a solution to the motion planning problem efficiently. The duality between optimization and inference allows performing efficient inference on factor graphs, thus exploiting the structure of the underlying system by solving sparse least squares problems. Similar approaches have been used to solve Simultanous Localization and Mapping (SLAM) problems [54]. With this key insight, preexisting efficient optimization methods developed by the SLAM community can be exploited for motion planning. The GPMP2 algorithm, which is more efficient than previous motion planning algorithms, stems from those considerations. A useful property of GPMP2 is its extensibility and applicability for wide range of problems. For example, combined learning from demonstration and motion planning [55] presented an efficient approach to skill learning and generalizable skill reproduction. In [56], authors provided a framework for avoidance of singular robot configurations as manipulability maximization, while a unified probabilistic framework for trajectory estimation and planning was provided in [57]. The main drawback of GPMP algorithms is that they are limited in their ability to work with nonlinear inequality constraints.

V. CONCLUSION AND FUTURE WORK
Safely navigating through an environment is one of the key tasks which an autonomous mobile robot or vehicle has to accomplish. Motion planning is an essential tool used to find trajectories of robot states that achieve a desired task. In this overview, we have presented some of the representative methods for high-dimensional motion planning. Gridbased approaches are resolution complete and often offer optimal solutions. However, the number of grid points grows exponentially in the configuration space dimension, which makes even the state-of-the-art methods inappropriate for very high-dimensional problems. Sampling-based approaches are efficient in most practical problems but offer weaker guarantees. They are probabilistically complete, however, they often require post-processing and can still be inefficient in very complex configuration spaces. Trajectory optimization approaches can solve high-dimensional motion planning problems quickly, but solutions are only locally optimal.
Grid-based and sampling-based approaches have been wellstudied. Most of the future research opportunities for highdimensional motion planning lie in the most recent areatrajectory optimization. One of the challenges is finding the right cost function to optimize. With current methods, it is possible for a trajectory that is in collision briefly but generally stays far away from obstacles to have lower cost than a trajectory that never collides. Other important challenge is finding a principled way to tackle the local minima problem, either by employing different optimization methods, or by investigating the effect of trajectory initialization. Sampling-based and gridsearch based methods could be used to provide a coarse initial trajectory which is then enhanced with trajectory optimization methods. Seeking a coherent way to handle constraints and guarantee safety is another open problem.