Motion Planning

Path metrics, RRT path planners, MPNet path planner, path following

Use motion planning to plan a path through an environment. You can use common sampling-based planners like RRT, RRT*, and Hybrid A*, deep-learning-based planner, or specify your own customizable path-planning interfaces. Use path metrics, state space sampling, and state validation to ensure your path is valid and has proper obstacle clearance or smoothness. Follow your path and avoid obstacles using pure pursuit, vector field histogram (VFH), and timed elastic band (TEB) algorithms.

Functions

expand all

Path Planners

`plannerRRT`	Create an RRT planner for geometric planning
`plannerRRTStar`	Create an optimal RRT path planner (RRT*)
`plannerBiRRT`	Create bidirectional RRT planner for geometric planning (Since R2021a)
`plannerControlRRT`	Control-based RRT planner (Since R2021b)
`plannerAStar`	Graph-based A* path planner (Since R2023a)
`plannerAStarGrid`	A* path planner for grid map (Since R2020b)
`plannerHybridAStar`	Hybrid A* path planner
`plannerPRM`	Create probabilistic roadmap path planner (Since R2022a)
`plannerMPNET`	Create MPNet based bidirectional path planner (Since R2024a)
`plannerBenchmark`	Benchmark path planners using generated metrics (Since R2022a)
`navGraph`	Create `navGraph` object (Since R2023a)

Paths and Path Segments

`navPath`	Planned path
`navPathControl`	Path representing control-based kinematic trajectory (Since R2021b)
`dubinsConnection`	Dubins path connection type
`dubinsPathSegment`	Dubins path segment connecting two poses
`reedsSheppConnection`	Reeds-Shepp path connection type
`reedsSheppPathSegment`	Reeds-Shepp path segment connecting two poses

Path Metrics and Path Optimization

`pathmetrics`	Information for path metrics
`optimizePath`	Optimize path while maintaining safe distance from obstacle (Since R2022a)
`optimizePathOptions`	Create optimization options for optimizePath function (Since R2022a)
`shortenpath`	Shorten path by removing redundant nodes (Since R2024b)

Path Following

`controllerVFH`	Avoid obstacles using vector field histogram
`controllerPurePursuit`	Create controller to follow set of waypoints
`controllerTEB`	Avoid unseen obstacles with time-optimal trajectories (Since R2023a)
`headingFromXY`	Compute heading angle from XY-points of path (Since R2023a)
`velocityCommand`	Retrieve velocity command from time series of velocity commands (Since R2023a)

State Spaces

`nav.StateSpace`	Create state space for path planning
`stateSpaceSE2`	SE(2) state space
`stateSpaceSE3`	SE(3) state space (Since R2020b)
`stateSpaceDubins`	State space for Dubins vehicles
`stateSpaceReedsShepp`	State space for Reeds-Shepp vehicles

Collision Checking

`checkCollision`	Check if two geometries are in collision
`checkMapCollision`	Check for collision between 3-D occupancy map and geometry (Since R2022b)
`nav.StateValidator`	Create state validator for path planning
`validatorOccupancyMap`	State validator based on 2-D grid map
`validatorOccupancyMap3D`	State validator based on 3-D grid map (Since R2020b)
`validatorVehicleCostmap`	State validator based on 2-D costmap
`dynamicCapsuleList`	Dynamic capsule-based obstacle list (Since R2020b)
`dynamicCapsuleList3D`	Dynamic capsule-based obstacle list (Since R2020b)

Collision Objects

`collisionBox`	Create box collision geometry
`collisionCapsule`	Capsule primitive collision geometry (Since R2022b)
`collisionCylinder`	Create collision cylinder geometry
`collisionMesh`	Create convex mesh collision geometry
`collisionSphere`	Create sphere collision geometry
`geom2struct`	Convert collision geometry objects to structure array (Since R2024a)
`collisionVHACD`	Decompose mesh into convex collision meshes using V-HACD (Since R2023b)
`showCollisionArray`	Show array of collision objects in figure (Since R2023b)

State Sampling

`nav.StateSampler`	Create state sampler for path planning (Since R2023b)
`stateSamplerGaussian`	Gaussian state sampler for sampling-based motion planning (Since R2023b)
`stateSamplerUniform`	Uniform state sampler for sampling-based motion planning (Since R2023b)
`stateSamplerMPNET`	MPNet state sampler for sampling-based motion planning (Since R2023b)
`sampleStartGoal`	Sample start and goal states for motion planning (Since R2024a)

State Propagation

`nav.StatePropagator`	State propagator for control-based planning (Since R2021b)
`mobileRobotPropagator`	State propagator for wheeled robotic systems (Since R2021b)

Custom Path Planning Interfaces

`createPlanningTemplate`	Create sample implementation for path planning interface
`nav.StateSpace`	Create state space for path planning
`nav.StateValidator`	Create state validator for path planning
`nav.StateSampler`	Create state sampler for path planning (Since R2023b)

Trajectory Generation

`referencePathFrenet`	Smooth reference path fit to waypoints (Since R2020b)
`trajectoryGeneratorFrenet`	Find optimal trajectory along reference path (Since R2020b)
`trajectoryOptimalFrenet`	Find optimal trajectory along reference path

Motion Planning Networks

`mpnetLayers`	Create custom motion planning networks (Since R2024a)
`mpnetSE2`	Motion Planning Networks (Since R2023b)
`mpnetPrepareData`	Prepare training data for Motion Planning Networks (Since R2023b)
`bpsEncoder`	Basis point set encoder (Since R2024a)

Plotting Functions

`plannerLineSpec.goal`	Specifications for plotting goal state (Since R2023b)
`plannerLineSpec.goalTree`	Specifications for plotting search tree from goal to start (Since R2023b)
`plannerLineSpec.heading`	Specifications for plotting heading angle (Since R2023b)
`plannerLineSpec.path`	Specifications for plotting forward path (Since R2023b)
`plannerLineSpec.reversePath`	Specifications for plotting reverse path (Since R2023b)
`plannerLineSpec.reverseTree`	Specifications for plotting reverse search tree (Since R2023b)
`plannerLineSpec.start`	Specifications for plotting start state (Since R2023b)
`plannerLineSpec.state`	Specifications for plotting generic states (Since R2023b)
`plannerLineSpec.tree`	Specifications for plotting forward search tree (Since R2023b)

Blocks

Pure Pursuit	Linear and angular velocity control commands
Vector Field Histogram	Avoid obstacles using vector field histogram

Topics

Motion Planning in Offroad Environment

Offroad Navigation for Autonomous Haul Trucks in Open Pit Mine

This example series shows how to create a set of planners to enable autonomous haul trucks to navigate uneven terrain and avoid obstacles. (Since R2024a)
- STEP 1: Create Route Planner for Offroad Navigation Using Digital Elevation Data
- STEP 2: Create Onramp and Terrain-Aware Global Planners for Offroad Navigation
- STEP 3: Navigate Global Path Through Offroad Terrain Using Local Planner
- STEP 4: Create Path Following Model Predictive Controller
- STEP 5: Model and Control Autonomous Vehicle in Offroad Scenario

Motion Planning Topics

Get Started with Motion Planning Networks
Motion Planning Networks for state space sampling and path planning.
Choose Path Planning Algorithms for Navigation
Details about the benefits of different path and motion planning algorithms.
Optimal Trajectory Generation for Urban Driving
This example shows how to perform dynamic replanning in an urban scenario using trajectoryOptimalFrenet.
Motion Planning in Urban Environments Using Dynamic Occupancy Grid Map
This example shows you how to perform dynamic replanning in an urban driving scene using a Frenet reference path.
Path Following with Obstacle Avoidance in Simulink®
Use Simulink to avoid obstacles while following a path for a differential drive robot.
Obstacle Avoidance with TurtleBot and VFH
This example shows how to use ROS Toolbox and a TurtleBot® with vector field histograms (VFH) to perform obstacle avoidance when driving a robot in an environment.
Vector Field Histogram
VFH algorithm details and tunable properties.
Pure Pursuit Controller
Pure Pursuit Controller functionality and algorithm details.

Featured Examples

Plan Mobile Robot Paths Using RRT

Use the rapidly exploring random tree (RRT) algorithm to plan a path for a vehicle through a known map. Special vehicle constraints are also applied with a custom state space. You can tune your own planner with custom state space and path validation objects for any navigation application.

Open Live Script

Dynamic Replanning on an Indoor Map

Perform dynamic replanning on a warehouse map with a range finder and an A* path planner.

Open Live Script

Highway Lane Change

Perceive surround-view information and use it to design an automated lane change maneuver system for highway driving scenarios.

Open Script

Highway Trajectory Planning Using Frenet Reference Path

Demonstrates how to plan a local trajectory in a highway driving scenario. This example uses a reference path and dynamic list of obstacles to generate alternative trajectories for an ego vehicle. The ego vehicle navigates through traffic defined in a provided driving scenario from a drivingScenario object. The vehicle alternates between adaptive cruise control, lane changing, and vehicle following maneuvers based on cost, feasibility, and collision-free motion.

Open Live Script

Reverse-Capable Motion Planning for Tractor-Trailer Model Using plannerControlRRT

Find global path-planning solutions for systems with complex kinematics using the kinematics-based planner, plannerControlRRT. The example is organized into three primary sections:

Open Live Script

Object Tracking and Motion Planning Using Frenet Reference Path

Dynamically replan the motion of an autonomous vehicle based on the estimate of the surrounding environment. You use a Frenet reference path and a joint probabilistic data association (JPDA) tracker to estimate and predict the motion of other vehicles on the highway. Compared to the Highway Trajectory Planning Using Frenet Reference Path example, you use these estimated trajectories from the multi-object tracker in this example instead of ground truth for motion planning.

Open Live Script

Optimization Based Path Smoothing for Autonomous Vehicles

Optimize the path for a car-like robot by maintaining a smooth curvature and a safe distance from the obstacles in a parking lot.

Open Live Script

Benchmark Path Planners for Differential Drive Robots in Warehouse Map

Choose the best 2-D path planner for a differential drive robot in a warehouse environment from the available path planners. Use the plannerBenchmark object to benchmark the path planners plannerRRT, plannerRRTStar, plannerBiRRT, plannerPRM, and plannerHybridAstar on the warehouse environment with the randomly chosen start and goal poses. Compare the path planners based on their ability to find a valid path, clearance from the obstacles, time taken to initialize a planner, time taken to find a path, length of the path, and smoothness of the path. A suitable planner is chosen based on the performance of each path planner on the above mentioned metrics.

Open Live Script

Offroad Planning with Digital Elevation Models

Process and store 2.5-D information, and presents various techniques for using it for an offroad path planner.

Open Live Script

Enable Vehicle Collision Checking for Path Planning Using Hybrid A*

Use a Hybrid A* planner to plan a path to a narrow parking space, while accounting for the shape of a car-like robot.

Open Live Script

Plan Path to Custom Goal Region for Mobile Robot

Plan a path for a mobile robot to a goal region using a rapidly exploring random tree (RRT) path planner. In this example, you can define a custom goal region as a 2-D polygon, and then plan a path to it.

Open Live Script

Hybrid Sampling Method for Motion Planning in Warehouse Environment

Combine uniform sampling and Gaussian sampling approaches for motion planning in narrow passages and wide spaces.

Open Live Script

New

Plan Path in Warehouse Scenario with Unseen Obstacle Avoidance

Plan path in a warehouse scenario by avoiding unseen obstacles using TEB algorithm.

Since R2024b
Open Live Script

Train Deep Learning-Based Sampler for Motion Planning

Create a deep learning-based sampler using Motion Planning Networks to speed up path planning using sampling-based planners like RRT (rapidly-exploring random tree) and RRT*. For information about Motion Planning Networks (MPNet) for state space sampling, see Get Started with Motion Planning Networks.

Open Live Script

Accelerate Motion Planning with Deep-Learning-Based Sampler

The example shows how to use sampling-based planners such as RRT (rapidly-exploring random tree) and RRT* with Motion Planning Networks (MPNet), deep-learning-based sampler to find optimal paths efficiently.

Open Live Script

Route Planning in Uneven Terrain Based on Vehicle Requirements

Use navGraph and plannerAStar to find a path through rough terrain while accounting for vehicle-based requirements and constraints.

Open Live Script

New

Path Planning Using MPNet for Automated Parking Valet System

Perform path planning for an automated parking valet system using a pretrained MPNet.

Since R2024b
Open Live Script

Offroad Navigation for Autonomous Haul Trucks in Open Pit Mine

Series shows how to create a set of planners to enable autonomous haul trucks to navigate uneven terrain and avoid obstacles.

Since R2024a
Open Live Script