Motion Planning

Path metrics, RRT path planners, 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*, or specify your own customizable path-planning interfaces. Use path metrics 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 and vector field histogram algorithms.

Functions

expand all

Paths and Path Segments

`navPath`	Planned path
`navPathControl`	Path representing control-based kinematic trajectory
`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

`pathmetrics`	Information for path metrics
`clearance`	Minimum clearance of path
`isPathValid`	Determine if planned path is obstacle free
`smoothness`	Smoothness of path
`show`	Visualize path metrics in map environment

State Spaces

`stateSpaceSE2`	SE(2) state space
`stateSpaceSE3`	SE(3) state space
`stateSpaceDubins`	State space for Dubins vehicles
`stateSpaceReedsShepp`	State space for Reeds-Shepp vehicles

State Validation

`validatorOccupancyMap`	State validator based on 2-D grid map
`validatorOccupancyMap3D`	State validator based on 3-D grid map
`validatorVehicleCostmap`	State validator based on 2-D costmap
`isStateValid`	Check if state is valid
`isMotionValid`	Check if path between states is valid

State Propagation

`nav.StatePropagator`	State propagator for control-based planning
`mobileRobotPropagator`	State propagator for wheeled robotic systems
`distance`	Estimate cost of propagating to target state
`propagate`	Propagate system without validation
`propagateWhileValid`	Propagate system and return valid motion
`sampleControl`	Generate control command and duration
`setup`	Set up the mobile robot state propagator

`navGraph`

`navGraph`	Create `navGraph` object
`findlink`	Find IDs of links
`findstate`	Find IDs of states
`index2state`	Find state vectors of state indices
`state2index`	Find indices for queried state vectors
`successors`	Find successive state indices and costs

Motion Planning

`plannerRRT`	Create an RRT planner for geometric planning
`plannerRRTStar`	Create an optimal RRT path planner (RRT*)
`plannerBiRRT`	Create bidirectional RRT planner for geometric planning
`plannerControlRRT`	Control-based RRT planner
`plannerAStar`	Graph-based A* path planner
`plannerAStarGrid`	A* path planner for grid map
`plannerHybridAStar`	Hybrid A* path planner
`plannerPRM`	Create probabilistic roadmap path planner
`plannerBenchmark`	Benchmark path planners using generated metrics

Path Optimization

`optimizePathOptions`	Create optimization options for optimizePath function
`optimizePath`	Optimize path while maintaining safe distance from obstacle

Frenet Trajectory Generation

`referencePathFrenet`	Smooth reference path fit to waypoints
`trajectoryGeneratorFrenet`	Find optimal trajectory along reference path
`trajectoryOptimalFrenet`	Find optimal trajectory along reference path

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

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
`headingFromXY`	Compute heading angle from XY-points of path
`velocityCommand`	Retrieve velocity command from time series of velocity commands

Dynamic Capsule List

`dynamicCapsuleList`	Dynamic capsule-based obstacle list
`dynamicCapsuleList3D`	Dynamic capsule-based obstacle list
`addEgo`	Add ego bodies to capsule list
`addObstacle`	Add obstacles to 2-D capsule list
`checkCollision`	Check for collisions between ego bodies and obstacles
`egoGeometry`	Geometric properties of ego bodies
`egoPose`	Poses of ego bodies
`obstacleGeometry`	Geometric properties of obstacles
`obstaclePose`	Poses of obstacles

Blocks

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

Topics

Choose Path Planning Algorithms for Navigation
Details about the benefits of different path and motion planning algorithms.
Moving Furniture in a Cluttered Room with RRT
This example shows how to plan a path to move bulky furniture in a tight space avoiding poles.
Motion Planning with RRT for a Robot Manipulator
Plan a grasping motion for a Kinova Jaco Assistive Robotics Arm using the rapidly-exploring random tree (RRT) algorithm.
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 Example

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

Train Deep Learning-Based Sampler for Motion Planning

Demonstrates how to train a deep learning-based sampler to speed up path planning using sampling-based planners like RRT (rapidly-exploring random tree) and RRT*.

Open Live Script

Accelerate Motion Planning with Deep-Learning-Based Sampler

The example demonstrates how to augment sampling-based planners such as RRT (rapidly-exploring random tree) and RRT* with a 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