Module2 - mtrl - Motion Planning

----- Suggestion on Material ----------

Most of the material has been put together by Björn Olofsson, with important input also from Jana Tumova and Joshua Haustein. Thanks to you.

Learning Outcomes

The objective of this part of Module 2 is to get an introduction to motion planning and some common algorithms and computer tools for performing this kind of planning in practice. The module also intends to describe how motion planning could be a component in an autonomous system and its relation to the other components.

We have included more material in this module than we realize most of you will have time to read. Use your own judgement and interest to decide which parts of the material marked in red to read.

 

Motion Planning and its Relation to Autonomous Systems

Motion planning is an essential component in many autonomous systems. Consider, for example, an autonomous car driving in an urban environment or UAVs performing different tasks. In these systems, a component is needed that computes a plan for how the desired tasks should be executed. The planner is often divided into different parts, doing planning at different levels. In the module on Task Planning (Module2 Task Planning), different strategies to compute a plan for solving a task were investigated. The motion planning component often acts based on the results from a high-level task planner. In the UAV example, the task planner could compute how the specific vehicles should move from one place to another and in what sequence (without details on how each vehicle should move in-between while taking physical constraints and obstacle avoidance into account). It is then the task of the motion planner to do a more detailed plan on how each vehicle should move from its start position to the desired goal position, considering all required constraints on the motion and avoiding obstacles and other vehicles in the same area, for example.

Motion Planning Criteria

In very general terms, the motion planning task could be described as transferring the state (such as position and velocity) of a moving system from an initial state to another desired goal state. During the motion, there are often certain criteria that need to be fulfilled for feasibility of the plan:

  • The motion needs to adhere to the system dynamics, encoded by a model describing the outcome of a certain action.
  • Obstacles, humans, and other vehicles (that could be stationary or time-varying) need to be avoided.
  • Physical limits on control inputs (for example, motor power in a car or maximum torque in a robot motor).
  • Constraints on internal states (for example, velocity or available friction between the tires and the road).
  • Path constraints.

In addition to these criteria, there are sometimes additional specifications in terms of optimality of a computed motion plan. These criteria could be in terms of minimizing the time or path length required for performing a certain task or minimizing the energy consumed.

Path and Trajectory Planning

Fundamental concepts in motion planning are paths and trajectories. A path is a parametrized curve in space defining the geometric motion of the system. A trajectory is time-parametrized, thus specifying the relation between path and time during the course of the motion (or equivalently the velocity). The motion planning problem is sometimes divided into a path planning stage (where the system dynamics sometimes are ignored) and a subsequent trajectory planning stage for tracking the computed path (optional reading Section 14.6 Links to an external site. in "Planning Algorithms" by LaValle for further details on decoupled approaches to motion planning). The intention is to reduce the computational complexity of the complete task. With increased computing power, direct integrated approaches to motion planning are also tractable, ultimately leading to higher performance.

Reactive Sensor-Based Motion Planning

Motion planning in uncertain, unstructured, or unknown environments needs to be reactive. Inputs from sensors provide information about the ego system and the environment (see Module 1 - Sensing). In a self-driving car, this could be LIDARs and IMUs providing data to the motion planning and control system. Based on the data acquired from these sensors, the path and the associated trajectory often need to be replanned online as a results of new information arriving. Considering that this could be a system moving with potentially high velocity (such as a car driving on a highway), the replanning in many cases needs to be done with real-time computational constraints and there must always be a trajectory for bringing the system to a complete stand still for safety. A YouTube video illustrating a motion planner acting in an autonomous vehicle, comprising many of the features described in the text above, is available here: A* in Action - Artificial Intelligence for Robotics Links to an external site.A* in Action - Artificial Intelligence for Robotics

Motion Planning and Feedback Control

Under ideal conditions, the path or trajectory computed by the motion planner will lead exactly to the desired behavior of the autonomous system when executed. However, because of uncertainty in the model and the parameters of the system as well as the environment, the motion planned often needs to be accompanied by feedback control (see Module 2 - Control) in order for the execution to be robust and lead to the intended motion. There is also a trade-off between when to do replanning and when to let the feedback controller handle local deviations from the planned path and trajectory.

Suggested Reading Material

The following book chapter and survey article provide an introduction, an overview, and many examples of motion planning in different fields:

 

Algorithms and Methods

We will investigate three common classes of motion planning algorithms. There are many variants and extensions of the fundamental algorithms discussed. The provided references provide more details on these aspects.

Motion Planning Based on Graph Search

One class of motion planning algorithms is based on graph search for establishing a path or trajectory for the system to follow. The first step in such methods is to establish the graph, with its edges and vertices, to be used for the search. Each vertex in the graph corresponds to a state of the system, and if there is a way to go from a particular vertex to another vertex, an edge is added in the graph. Once the graph is established, different methods are available for searching effectively in the graph to find a solution from the initial state to the desired final goal. Each edge could also be associated with a cost, for example the length of that path segment or the time to go from vertex A to vertex B. An introduction to graph search in the context of motion planning is provided in the following chapter:

The infinite-dimensional continuous state space of a vehicle or robot needs to be discretized for graph search purposes. The discretized state space is referred to as the state-space lattice, and the motion of the vehicle or robot is then planned on this lattice.

When considering motion constraints, for example, implied by the system dynamics, the possible state transitions could be defined by so called motion primitives. These primitives are typically defined and computed a priori and describe the possible transition from one state to another state. In the example of a ground-vehicle with a constraint that it cannot move sideways, this motion restriction is implicitly encoded in the motion primitives. The motion primitives should span a sufficiently large part of the possible motions to do from a certain state such that the resulting state space lattice is sufficiently dense, but there is a trade-off between the number of primitives and the computational time of the motion planning algorithm. The concept and computation of motion primitives is described in the following paper:

A graph is constructed based on the motion primitives, possibly incrementally as new sensor data become available. Each edge corresponding to a certain motion segment could also be associated with a cost. In the graph construction, obstacles in the vicinity need to be considered, thus preventing application of certain motion primitives. The solution to the motion planning problem is then obtained as a sequence of motion primitives through the graph; the particular primitives are the results of the graph search. The following presentation and article describe motion planning based on motion primitives and graph search:

Suggested Additional Reading Material

Sampling-Based Motion Planning Algorithms

Another set of motion planning methods is based on sampling in the state space of the system to make a plan for (such as a vehicle or robot). Here, we consider Rapidly Exploring Random Trees (RRTs), which is one common class of sampling-based algorithms. The following article by LaValle presents the foundations of the method:

The method is based on incrementally constructing a tree in the state space, and here a summary of the key ideas is given: Random samples, possibly biased toward the goal state or toward other desirable directions, are generated iteratively. The point in the tree constructed so far that is closest to the random sample is used for expansion of the tree. From that closest point in the tree, the system is steered toward that random sample (in many cases by simulation of the model of the system to account for kinematic and possibly dynamic constraints) until an obstacle is detected or it comes sufficiently close to the random sample. The corresponding edge is added to the tree. This process is then repeated until a solution from the initial state to the desired goal state is found.

Suggested reading material for motion planning with RRTs is:

If optimal motion planning is to be performed, the RRT algorithm described in the previous paragraph could be extended to RRT*. RRT* aims to minimize the cost of the motion from the initial state to the desired goal state, by successive rewiring of the vertices in the constructed tree if a path that leads to a lower cost could be found when a new vertex is added. The RRT* algorithm is described in the following article, where it is also compared to RRT:

The RRT* algorithm is extended to planning for systems with differential constraints in the following paper:

 

Optimal Path and Trajectory Planning Using Dynamic Optimization

Yet another category of motion planning problems is conveniently formulated as optimal control problems and solved using dynamic optimization. This formulation enables comparably complex models of the system dynamics and constraints on the motion to be included in the formulation. In addition, a certain cost function (or objective function) to be minimized could be formulated within such a framework. Optimal control problems are often formulated in continuous time, since the model of the system is derived from physical principles. Occasionally, the optimal control problem could be solved with analytical methods, but often numerical optimization needs to be employed for computing a solution (see also Module 2 - Control for related approaches with MPC and convex optimization applied in receding horizon).

For transforming the infinite-dimensional optimization problem in continuous time to a finite-dimensional approximation that can be used in numerical optimization such as gradient descent or Newton methods, different approaches exist. Common methods are so called direct methods based on collocation or multiple shooting, which transform the optimization problem to a non-linear program that can be solved using different numerical solvers.  For an introduction to dynamic optimization, see the following chapter:

In the chapter in the previous reference, an extension of Modelica (see Module 2 - Modeling and Simulation) called Optimica for formulating optimization problems based on Modelica models are also described. Dynamic optimization has for a long time been used for optimal motion planning for vehicles and robots of different types. A survey article comprising many different applications and the employed methods is: