# Motion Planning with MATLAB

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 self-driving 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 self-driving cars, robot manipulators, UGVs, and UAVs autonomous. The other two are perception and control.

Autonomous systems workflow.

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 obstacle-free 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:

Autonomous mobile robot navigation in a warehouse.

Last-mile drone delivery.

Pick and place with a robot manipulator.

## 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 self-driving 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 1: motion planning.

Scenario 2: The car changes lane on a highway—a change in both path and motion.

Scenario 2: path planning.

### 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 (Cspace). 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 self-driving car, the position of the car and its heading or orientation collectively represent its state. For automated parking of a self-driving 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:

• Search-based planning and sampling-based 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.

### Search-Based Planning

Search-based 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 heuristic-based approaches to find the shortest path. Search-based 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 search-based method for finding paths in discrete grid maps.

Search-based planning.

A* algorithm applied to a grid map.

Search-based 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.

### Sampling-Based Planning

In sampling-based planning, a search tree or road map is created by randomly adding nodes in the state space. Possible collision-free paths are found using a continuous motion model. Sampling-based planning often uses heuristics to explore the search space and bias the search direction. Once created, the tree or road map uses collision-checking or search methods to find the shortest path to goal.

The RRT algorithm is a commonly used sampling-based method for finding paths in a continuous state space.

Search-based planning.

Sampling-based motion planning can be suitable for high-dimensional 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 sampling-based 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 sampling-based planners such as RRT and RRT* probabilistically complete.

### Global and Local Path Planning

Global path planning, or map-based 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 self-driving car may plan local trajectories to change lanes to avoid other vehicles and merge back to the global path to reach the destination.

Local path planning for lane change maneuver of a self-driving car.

## Four-Step Motion Planning Workflow with MATLAB

Navigation Toolbox provides classes for implementing various planning algorithms, including common search-based planners such as A* and sampling-based planners, such as RRT and RRT*. The toolbox also provides path metrics to evaluate the planned path for obstacle clearance and smoothness.

Path metrics: smoothness.

Path metrics: clearance.

In addition, Navigation Toolbox provides an interface that lets you implement a sampling-based motion planning algorithm in a systematic, four-step workflow:

1. Represent the state space.
2. Define a state validator.
3. Sample new states and check for validity.
4. 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 car-like robot or a robot with Ackermann steering.

Navigation Toolbox provides the following ready-to-use 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 Non-holonomic vehicle with minimum turning radius
stateSpaceReedsShepp (x, y, θ) 2D Non-holonomic vehicle with minimum turning radius

State space SE2.

State space Dubins.

### 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

Sampling-based planning algorithms randomly sample states in the defined state space and create an obstacle-free 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
Grid-based A* Global planning
• Customizable cost and heuristics
• Optimality if heuristics are consistent and admissible
Hybrid A* Global planning
• Supports differential constraints
Probabilistic road maps (PRMs) Global planning
• Customizable
• Multi-query
Rapidly exploring random tree (RRT) Global planning
• Customizable
RRT* Global planning
• Customizable
• Asymptotically optimal
Trajectory optimal Frenet Local trajectory generator
• Customizable collision checking
• Self-defined optimality

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 x-y 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.

### 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.

See A* example

### Applications

The A* algorithm is directly applicable to omnidrive wheeled robots. A* is also popular for computer-based applications such as pathfinding in video games.

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.

See Hybrid A* example

### Applications

Unlike the traditional A*, hybrid A* is suitable for vehicles with nonholonomic constraints, such as self-driving cars. It guarantees kinematic feasibility and takes differential constraints—the orientation and velocity of the vehicle—into account.

Hybrid A* for automated parking.

Search-based 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 sampling-based 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 planner.

### 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 obstacle-free path, you can inflate the map by the vehicle’s radius by using the inflate function.

See PRM example

### 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.

PRM planner.

RRT is a sampling-based algorithm suitable for non-holonomic constraints. The RRT algorithm efficiently searches nonconvex high-dimensional 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 Xstart following these steps:

1. The planner samples a random state Xrand in the state space.
2. The planner finds a state Xnear that is already in the search tree and is closest to Xrand (based on the distance definition in the state space).
3. The planner expands from Xnear toward Xrand, until a state Xnew is reached.
4. The new state Xnew is added to the search tree.

This process is repeated until the tree reaches Xgoal. Every time a new node Xnew is sampled, its connection with other nodes is checked for collision. To realize a drivable path from Xstart to Xgoal, you can use motion primitives or motion models such as Reeds-Shepp.

### RRT in MATLAB

Navigation Toolbox provides plannerRRT, which follows the customizable sampling-based 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 high-dimensional spaces. It is available as manipulatorRRT in Robotics System Toolbox.

See RRT example

### 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 path-smoothing algorithm to compensate for these irregularities.

PRM planner.

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 Xnew.
• RRT* examines the decrease in the cost of the nodes and rewires the search tree to obtain a shorter and smoother path.

RRT* planner.

### RRT* in MATLAB

plannerRRTStar in Navigation Toolbox is suitable for solving geometric planning problems.

See RRT* example

### Applications

RRT* gives an asymptotically optimal solution, which makes it particularly useful for high-dimensional 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

Parallel parking using RRT* and MPC tracking controller.

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 fifth-order polynomial, which attempts to minimize jerk and guarantees continuity along states. Sampled trajectories are evaluated for kinematic feasibility, collision, and cost.

Trajectory planning for highway driving.

### 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 lane-changing maneuvers and adaptive cruise control (including velocity keeping). It can also be used with mobile robots and other Ackermann type vehicles for dynamic replanning.