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
rigid3d3-D rigid geometric transformation
quaternionCreate a quaternion array
distAngular distance in radians
rotateframeQuaternion frame rotation
rotatepointQuaternion point rotation
rotmatConvert quaternion to rotation matrix
rotvecConvert quaternion to rotation vector (radians)
rotvecdConvert quaternion to rotation vector (degrees)
partsExtract quaternion parts
eulerConvert quaternion to Euler angles (radians)
eulerdConvert quaternion to Euler angles (degrees)
compactConvert quaternion array to N-by-4 matrix
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
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


Rotations, Orientations, and Quaternions for Automated Driving

Quaternions are four-part hypercomplex numbers that are used to describe three-dimensional rotations and orientations. Learn how to use them for automated driving applications.

Lateral Control Tutorial

Control the steering angle of a vehicle following a planned path and perform lane changing.

Featured Examples