Main Content

Planning and Control

Vehicle costmaps, optimal RRT* path planning, lateral and longitudinal controllers

Automated Driving Toolbox™ provides several features that support path planning and vehicle control.

  • To plan driving paths, you can use a vehicle costmap and the optimal rapidly exploring random tree (RRT*) motion-planning algorithm. You can also check the validity of the path, smooth the path, and generate a velocity profile along the path.

  • To design vehicle control systems, you can use lateral and longitudinal controllers that enable autonomous vehicles to follow a planned trajectory.


expand all

vehicleCostmapCostmap representing planning space around vehicle
vehicleDimensionsStore vehicle dimensions
checkFreeCheck vehicle costmap for collision-free poses or points
checkOccupiedCheck vehicle costmap for occupied poses or points
getCostsGet cost value of cells in vehicle costmap
setCostsSet cost value of cells in vehicle costmap
inflationCollisionCheckerCollision-checking configuration for costmap based on inflation
pathPlannerRRTConfigure RRT* path planner
planPlan vehicle path using RRT* path planner
checkPathValidityCheck validity of planned vehicle path
driving.PathPlanned vehicle path
interpolateInterpolate poses along planned vehicle path
smoothPathSplineSmooth vehicle path using cubic spline interpolation
lateralControllerStanleyCompute steering angle command for path following by using Stanley method


Path Smoother SplineSmooth vehicle path using cubic spline interpolation
Velocity ProfilerGenerate velocity profile of vehicle path given kinematic constraints (Since R2019b)
Lateral Controller StanleyControl steering angle of vehicle for path following by using Stanley method
Longitudinal Controller StanleyControl longitudinal velocity of vehicle by using Stanley method