Plan vehicle path using RRT* path planner
Plan a vehicle path to a parking spot by using the RRT* algorithm.
Load a costmap of a parking lot. Plot the costmap to see the parking lot and inflated areas for the vehicle to avoid.
data = load('parkingLotCostmapReducedInflation.mat'); costmap = data.parkingLotCostmapReducedInflation; plot(costmap)
Define start and goal poses for the path planner as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the Θ orientation values are in degrees.
startPose = [11, 10, 0]; % [meters, meters, degrees] goalPose = [31.5, 17, 90];
Create an RRT* path planner to plan a path from the start pose to the goal pose.
planner = pathPlannerRRT(costmap); refPath = plan(planner,startPose,goalPose);
Plot the planned path.
planner— RRT* path planner
RRT* path planner, specified as a
startPose— Initial pose of vehicle
Initial pose of the vehicle, specified as an [x, y, Θ] vector. x and y are in world units, such as meters. Θ is in degrees.
goalPose— Goal pose of vehicle
Goal pose of the vehicle, specified as an [x, y, Θ] vector. x and y are in world units, such as meters. Θ is in degrees.
The vehicle achieves its goal pose when the last pose in the path is
GoalTolerance property of
refPath— Planned vehicle path
tree— Exploration tree
Exploration tree, returned as a
digraph object. Nodes within
tree represent explored vehicle poses. Edges within
tree represent the distance between connected
Usage notes and limitations:
tree output argument, a digraph
object, is not supported.