In this ebook, you will learn how motion planning works and how it can be applied to a wide range of autonomous systems. You will also learn about the motion planning algorithms available in MATLAB^{®} and Navigation Toolbox™ and how to select the best algorithm for your application.
Introduction
The days when a vehicle or machine required a driver or physical controller are gone—today’s selfdriving cars are smart enough to change lanes, allow pedestrians to pass, or even park themselves in a parallel spot.
Motion planning is one of the three components that make systems like selfdriving cars, robot manipulators, UGVs, and UAVs autonomous. The other two are perception and control.
Much like humans, autonomous systems explore a new environment by scanning it to know where they are and what is around them. Once they have a map of the environment, motion planning algorithms plan an obstaclefree path to a given destination. Based on the decision made about the next step on this path, a controller helps to move the system by sending commands to its actuators.
Common applications of motion planning include:
What Is Motion Planning?
Motion planning is a computational problem to find a sequence of actions that moves a robot or vehicle from an initial state to a goal state. “Motion planning” and “path planning” are often used interchangeably, but there is a key difference. Motion planning generates the vehicle’s motion as it changes position over time, whereas path planning only generates a path for the vehicle. With motion planning, the motion of the vehicle can change over the course of the path it is following, as in these two scenarios involving a selfdriving car:
Scenario 1: The car decelerates before stopping at a red signal and continues when the signal turns green—a change in motion but not in the planned path.
Scenario 2: The car changes lane on a highway—a change in both path and motion.
State Space and Other Motion Planning Key Concepts
For practical applications, motion planning needs several functional pieces to work. These include an environment map, generated using a simultaneous localization and mapping (SLAM) algorithm, and the state (position and orientation) of the robot or vehicle. The robot’s transformation from state to state defines its motion. The set of transformations that could be applied to a robot is called the state space or the configuration space (C_{space}). The configuration space can include free spaces (in which the robot states are considered to be valid) and obstacle spaces (in which the robot states are considered to be invalid).
For example, in a selfdriving car, the position of the car and its heading or orientation collectively represent its state. For automated parking of a selfdriving car, a map of the parking lot identifies the free and obstacle spaces, and the state space represents the set of all possible forward and reverse maneuvers defined using a motion model.
Path Cost, Optimality, and Completeness
Path Cost
While a robot or vehicle is finding a path, each step it takes can be associated with a cost. The cost of traveling through free space is usually set as zero, and the cost of traveling through a space containing obstacles as infinity.
Optimality
A path planning algorithm is called optimal if it always finds an optimal path. For a path to be optimal, the sum of its transition costs (edge costs) must be minimal across all possible paths leading from the initial position to the goal position.
Completeness
A path planning algorithm is called complete if it takes a finite amount of time to find a path (when one exists) and reports if no path exists.
The path provided by an optimal and complete path planning algorithm may not be the shortest, but it will incur the least cost. In some specific cases—for example, moving an indoor robot along a hallway— you can define the cost of moving the robot down the center of the hallway as less than the cost of moving it closer to the walls. The optimal path in this scenario will make the robot travel down the center, reducing the chance of collisions with the walls.
Common Types of Motion Planning
There are many different types of approaches to motion planning. The most common are:
 Searchbased planning and samplingbased planning approaches, which are based on the way the search tree or graph is created
 Global and local path planning approaches, which are based on whether the planning is done in the entire map or in a subset
Let’s look at each type of approach in more detail.
SearchBased Planning
Searchbased planning creates a searchable graph, with each vehicle state or configuration identified as a node. The graph expands from start to goal node using cost and heuristicbased approaches to find the shortest path. Searchbased planning is commonly performed on a discretized map, where the map is subdivided into grid cells and the number of states is finite or countably infinite (each state can be assigned a unique integer).
The discrete state space is often represented in a 2D grid map where the centers of cells are the states to search upon. One common map representation is the occupancy grid map.
The A* algorithm is a commonly used searchbased method for finding paths in discrete grid maps.
Searchbased planning on a grid map is often appropriate when the vehicle or robot can be considered as a point and no motion model or kinematic equation is involved in the planning stage. Once the path planning algorithm provides waypoints for the robot to follow, a control algorithm can be used to take the kinematic constraints into account.
SamplingBased Planning
In samplingbased planning, a search tree or road map is created by randomly adding nodes in the state space. Possible collisionfree paths are found using a continuous motion model. Samplingbased planning often uses heuristics to explore the search space and bias the search direction. Once created, the tree or road map uses collisionchecking or search methods to find the shortest path to goal.
The RRT algorithm is a commonly used samplingbased method for finding paths in a continuous state space.
Samplingbased motion planning can be suitable for highdimensional search spaces such as those used to find a valid set of configurations for a robot arm to pick up an object. The suitability of samplingbased planning for various practical applications makes it popular even though it does not provide a complete solution. The probability of finding a solution (if one exists) converges to 1 if the density of the search tree brings the samples close enough together. This makes some samplingbased planners such as RRT and RRT* probabilistically complete.
Global and Local Path Planning
Global path planning, or mapbased planning, involves finding an optimal path based on a priori knowledge of the environment. Global planning algorithms plan an initial path that avoids known and static obstacles in the environment. For example, an autonomous mobile robot may plan a global path to deliver a book from one office to another on a corridor with static obstacles such as walls.
Local path planning (or dynamic replanning) recalculates the path to avoid unknown and dynamic obstacles. Local planning algorithms track the global plan and create local trajectories while avoiding the newly introduced obstacles. For example, a selfdriving car may plan local trajectories to change lanes to avoid other vehicles and merge back to the global path to reach the destination.
FourStep Motion Planning Workflow with MATLAB
Navigation Toolbox provides classes for implementing various planning algorithms, including common searchbased planners such as A* and samplingbased planners, such as RRT and RRT*. The toolbox also provides path metrics to evaluate the planned path for obstacle clearance and smoothness.
In addition, Navigation Toolbox provides an interface that lets you implement a samplingbased motion planning algorithm in a systematic, fourstep workflow:
 Represent the state space.
 Define a state validator.
 Sample new states and check for validity.
 Represent a collection of valid states as a path.
Representing the State Space
The custom state space class nav.StateSpace
lets you define a state space that contains the possible states or configurations for any application. For example, stateSpaceDubins
and stateSpaceReedsShepp
support planning for automated parking by connecting any two states in the state space so that the state space mimics the motion of a carlike robot or a robot with Ackermann steering.
Navigation Toolbox provides the following readytouse state spaces.
State Space  Configuration  Environment  Application 

stateSpaceSE2 
(x, y, θ)  2D  Holonomic robots 
stateSpaceSE3 
(x, y, z, qw, qx, qy, qz)  3D  Robot manipulators 
stateSpaceDubins 
(x, y, θ)  2D  Nonholonomic vehicle with minimum turning radius 
stateSpaceReedsShepp 
(x, y, θ)  2D  Nonholonomic vehicle with minimum turning radius 
Defining a State Validator
A state validator is based on the state space and corresponds to the map obtained from the SLAM algorithm. It checks the validity of a state or the motion between two sampled states. For example, a collision checker is a type of state validator that indicates when a robot state or configuration is in collision with an obstacle.
Navigation Toolbox provides the following state validators for validating states and discretized motions in 2D and 3D occupancy maps.
State Validator  Type  Application 

validatorOccupancyMap 
occupancyMap, binaryOccupancyMap 
2D occupancy map 
validatorVehicleCostmap 
vehicleCostmap 
2D occupancy map 
validatorOccupancyMap3D 
occupancyMap3D 
3D occupancy map 
These state validators are derived from the custom state validator available in the toolbox, nav.StateValidator
, which can be used to determine the validity of a state or the motion between any two states.
Sampling New States and Checking for Validity
Samplingbased planning algorithms randomly sample states in the defined state space and create an obstaclefree path from start to goal using the state validator. Algorithms such as RRT and PRM use different sampling schemes to sample the states and create a search tree or road map.
To sample the states inside the given map (obtained from the SLAM algorithm), state space bounds corresponding to the map’s outer limits are applied.
Representing a Collection of Sampled States
You can use the plan
function in Navigation Toolbox to consolidate the planning algorithm output in a tree data structure. You can use the class navPath
to store the collection of states in a given state space and interpolate them to obtain the path.
Selecting a Motion Planning Algorithm
The following motion planning algorithms are available in Navigation Toolbox.
Algorithm  Type and Scope  Benefits 

Gridbased A*  Global planning 

Hybrid A*  Global planning 

Probabilistic road maps (PRMs)  Global planning 

Rapidly exploring random tree (RRT)  Global planning 

RRT*  Global planning 

Trajectory optimal Frenet  Local trajectory generator 

GridBased A*
The A* algorithm is a discrete path planner that can work on grid maps by creating weighted graphs connecting the start and goal nodes. A* works on a discretized grid map and expands the search tree using xy linear connections. The nodes are explored one by one based on a cost function that estimates the cost to travel from node to node.
How It Works
To find the path with the least cost, A* works to minimize the cost function f(n):
f(n) = g(n) + h(n)
where
 n is the next node on the path
 g(n) is the cost of the path from the start node to n
 h(n) is a heuristic function that estimates the cost of the cheapest path from n to goal
A* Algorithm in MATLAB
plannerAStarGrid
in Navigation Toolbox provides most commonly used heuristics, such as Euclidean and Manhattan. You can use either the predefined cost functions or custom cost functions for h(n) and g(n). You can specify either a binaryOccupancyMap
or occupancyMap
object as an input to the planner.
Applications
The A* algorithm is directly applicable to omnidrive wheeled robots. A* is also popular for computerbased applications such as pathfinding in video games.
Hybrid A*
Hybrid A* is an extension of A*. Like A*, Hybrid A* works in a discretized search space, but it associates each grid cell with a continuous 3D state (x, y, θ) of a vehicle. It uses a continuous state space consisting of motion primitives to generate smooth and drivable paths.
How It Works
Hybrid A* uses efficient guiding heuristics that make the search tree expand in the direction of the goal. It also uses analytic expansion of the path, which improves accuracy and reduces planning time.
Hybrid A generates a smooth and drivable path for a vehicle based on the exact goal pose provided.
Start Node, S = (1.5, 2.5, pi/2)
Goal Node 1, G1 = (5.5, 5.5, 0)
Goal Node 2, G2 = (5.5, 5.5, pi/2
Hybrid A* in MATLAB
Navigation Toolbox provides plannerHybridAStar
, which uses a state validator such as validatorOccupancyMap
or validatorVechicleCostmap
for collision checking in the occupancy map and includes costs and parameters for tuning specific to your application.
Applications
Unlike the traditional A*, hybrid A* is suitable for vehicles with nonholonomic constraints, such as selfdriving cars. It guarantees kinematic feasibility and takes differential constraints—the orientation and velocity of the vehicle—into account.
PRM
Searchbased algorithms are not suitable for applications with high degrees of freedom or those in which the map size is very large. Storing data from a large grid map is computationally expensive. PRM is a samplingbased planning algorithm which is helpful in such cases.
How It Works
PRM is a network graph of different possible paths in a map. The graph is generated by a limited number of random points or nodes within a given area. After randomly sampling each node, the PRM algorithm creates multiple clusters of nodes by connecting all the nodes within a fixed radius.
Once the roadmap has been constructed, a path can be queried from a given start location to a given goal location on the map. Since PRM allows multiple queries in the same road map for different start and goal locations, it saves computation time if the map is static (does not change over time). PRM uses a graph search algorithm such as A* planner to search for the path inside the road map it creates.
PRM in MATLAB
Robotics System Toolbox™ provides mobileRobotPRM
, which takes a binaryOccupancyMap
as input and creates a road map in the free space of the map by connecting the randomly sampled nodes. You can use the findpath
function to query for a path from a start to a goal location. Since PRM does not consider the dimension of the vehicle while finding an obstaclefree path, you can inflate the map by the vehicle’s radius by using the inflate
function.
Applications
The PRM implementation in MATLAB is suitable for holonomic mobile robot applications such as a static warehouse environment with no unknown obstacles, where you want to change the start and goal locations without the robot’s having to relearn the whole map.
RRT
RRT is a samplingbased algorithm suitable for nonholonomic constraints. The RRT algorithm efficiently searches nonconvex highdimensional spaces. It creates a search tree incrementally by using random samples in a defined state space. The search tree eventually spans the search space and connects the start state to the goal state.
How It Works
The RRT planner grows the search tree rooted at the start state X_{start} following these steps:
 The planner samples a random state X_{rand} in the state space.
 The planner finds a state X_{near} that is already in the search tree and is closest to X_{rand} (based on the distance definition in the state space).
 The planner expands from X_{near} toward X_{rand}, until a state X_{new} is reached.
 The new state X_{new} is added to the search tree.
This process is repeated until the tree reaches X_{goal}. Every time a new node X_{new} is sampled, its connection with other nodes is checked for collision. To realize a drivable path from X_{start} to X_{goal}, you can use motion primitives or motion models such as ReedsShepp.
RRT in MATLAB
Navigation Toolbox provides plannerRRT
, which follows the customizable samplingbased planning interface described in Chapter 3.
Bidirectional RRT (biRRT) is a variant of RRT that creates two trees, starting from both the start and goal states simultaneously. biRRT is useful for robot manipulators as it improves the search speed in highdimensional spaces. It is available as manipulatorRRT
in Robotics System Toolbox.
Applications
RRTs are particularly suited to path planning problems that involve obstacles, differential constraints for robots with high degrees of freedom, such as robot manipulators, and mobile robot path planning. Note that RRT planning can result in paths with sudden turns. You can use a pathsmoothing algorithm to compensate for these irregularities.
RRT*
The RRT algorithm gives a valid path, but not necessarily the shortest path. RRT* is an optimized version of the RRT algorithm. While realistically unfeasible, in theory the RRT* algorithm can deliver the shortest possible path to the goal when the number of nodes approaches infinity.
How It Works
The basic principle of RRT* is the same as RRT, but it has two key additions that enable it to produce significantly different results:
 RRT* includes the cost of each node as defined by the distance relative to its parent node. It always looks for the node with the cheapest cost within a fixed radius in the neighborhood of X_{new}.
 RRT* examines the decrease in the cost of the nodes and rewires the search tree to obtain a shorter and smoother path.
RRT* in MATLAB
plannerRRTStar
in Navigation Toolbox is suitable for solving geometric planning problems.
Applications
RRT* gives an asymptotically optimal solution, which makes it particularly useful for highdimensional problems such as robot manipulators. It is also useful in dense environments containing many obstacles. While RRT* finds the shortest path with the fewest nodes, it is not suitable for nonholonomic vehicles. RRT, by comparison, can be used for nonholonomic vehicles and is capable of handling differential constraints
Trajectory Optimal Frenet
Trajectory optimal Frenet is a local planner that plans trajectories based on a reference global path. A trajectory is a set of states where the state includes variables that are functions of time. Trajectory planning is useful for situations in which velocity is a consideration.
How It Works
As a local planner, trajectory optimal Frenet requires a global reference path provided as a set of \([𝑥\,𝑦\,𝜃] \) waypoints. For planning along curved and continuous reference paths, such as highways, it uses Frenet coordinates consisting of run length and lateral distance from the reference path.
Trajectory optimal Frenet samples alternate trajectories from the initial state, deviating from the reference path by some lateral distance. It uses a Frenet frame of reference, and two kinds of states:
 Cartesian state: \([𝑥\,𝑦\, 𝜃\, 𝜅\, 𝑥̇ \,𝑥̈] \)
 Frenet state: \([𝑠\,𝑠̇ \,𝑠̈\, 𝑙 \,𝑙̇ \,𝑙̈] 𝑠=𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝑎𝑙𝑜𝑛𝑔𝑅𝑒𝑓𝑃𝑎𝑡ℎ, 𝑙=𝑙𝑎𝑡𝑒𝑟𝑎𝑙𝐷𝑒𝑣𝑖𝑎𝑡𝑖𝑜𝑛𝑤𝑟𝑡𝑅𝑒𝑓𝑃𝑎𝑡ℎ\)
The initial state is connected to sampled terminal states via a fifthorder polynomial, which attempts to minimize jerk and guarantees continuity along states. Sampled trajectories are evaluated for kinematic feasibility, collision, and cost.
Trajectory Optimal Frenet in MATLAB
trajectoryOptimalFrenet
in Navigation Toolbox finds optimal trajectories along reference paths where reference waypoints are generated by a global planner such as Hybrid A* or RRT. trajectoryOptimalFrenet
generates multiple alternate paths and evaluates them for cost based on the final state’s deviation from the reference path, path smoothness, time, and distance. It uses a state validator such as validatorOccupancyMap
to check the validity of the states.
Applications
Trajectory optimal Frenet can be used as a local planner between the global planner and the vehicle or robot controller. It is applicable to highway driving applications such as lanechanging maneuvers and adaptive cruise control (including velocity keeping). It can also be used with mobile robots and other Ackermann type vehicles for dynamic replanning.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
 América Latina (Español)
 Canada (English)
 United States (English)
Europe
 Belgium (English)
 Denmark (English)
 Deutschland (Deutsch)
 España (Español)
 Finland (English)
 France (Français)
 Ireland (English)
 Italia (Italiano)
 Luxembourg (English)
 Netherlands (English)
 Norway (English)
 Österreich (Deutsch)
 Portugal (English)
 Sweden (English)
 Switzerland
 United Kingdom (English)
Asia Pacific
 Australia (English)
 India (English)
 New Zealand (English)
 中国
 日本Japanese (日本語)
 한국Korean (한국어)