This example shows how to 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.
Load an existing occupancy map of a small office space. Plot the start and goal poses of the vehicle on top of the map.
load("office_area_gridmap.mat","occGrid") show(occGrid) % Set start and goal poses. start = [-1.0,0.0,-pi]; goal = [14,-2.25,0]; % Show start and goal positions of robot. hold on plot(start(1),start(2),'ro') plot(goal(1),goal(2),'mo') % Show start and goal headings. r = 0.5; plot([start(1),start(1) + r*cos(start(3))],[start(2),start(2) + r*sin(start(3))],'r-') plot([goal(1),goal(1) + r*cos(goal(3))],[goal(2),goal(2) + r*sin(goal(3))],'m-') hold off
Specify the state space of the vehicle using a
stateSpaceDubins object and specifying the state bounds. This object limits the sampled states to feasible Dubins curves for steering a vehicle within the state bounds. A turning radius of 0.4 meters allows for tight turns in this small environment.
bounds = [occGrid.XWorldLimits; occGrid.YWorldLimits; [-pi pi]]; ss = stateSpaceDubins(bounds); ss.MinTurningRadius = 0.4;
To plan a path, the RRT algorithm samples random states within the state space and attempts to connect a path. These states and connections need to be validated or excluded based on the map constraints. The vehicle must not collide with obstacles defined in the map.
validatorOccupancyMap object with the specified state space. Set the
Map property to the loaded
occupancyMap object. Set a
ValdiationDistance of 0.05 m. This validation distance discretizes the path connections and checks obstacles in the map based on this.
stateValidator = validatorOccupancyMap(ss); stateValidator.Map = occGrid; stateValidator.ValidationDistance = 0.05;
Create the path planner and increase the max connection distance to connect more states. Set the maximum number of iterations for sampling states.
planner = plannerRRT(ss,stateValidator); planner.MaxConnectionDistance = 2.0; planner.MaxIterations = 30000;
GoalReached function. This example helper function checks if a feasible path reaches the goal within a set threshold. The function returns
true when the goal has been reached, and the planner stops.
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;
function isReached = exampleHelperCheckIfGoal(planner, goalState, newState) isReached = false; threshold = 0.1; if planner.StateSpace.distance(newState, goalState) < threshold isReached = true; end end
Plan the path between the start and goal. Reset the random number generator for reproducible results.
rng default [pthObj, solnInfo] = plan(planner,start,goal);
Show the occupancy map. Plot the search tree from the
solnInfo. Interpolate and overlay the final path.
show(occGrid) hold on % Plot entire search tree. plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-'); % Interpolate and plot path. interpolate(pthObj,300) plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2) % Show start and goal in grid map. plot(start(1),start(2),'ro') plot(goal(1),goal(2),'mo') hold off
To specify custom vehicle constraints, customize the state space object. This example uses
ExampleHelperStateSpaceOneSidedDubins, which is based on the
stateSpaceDubins class. This helper class limits the turning direction to either right or left based on a Boolean property,
GoLeft. This property essentially disables path types of the
Create the state space object using the example helper. Specify the same state bounds and give the new Boolean parameter as
true (left turns only).
% Only making left turns goLeft = true; % Create the state space ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft); ssCustom.MinTurningRadius = 0.4;
Create a new planner object with the custom Dubins constraints and a validator based on those constraints. Specify the same
stateValidator2 = validatorOccupancyMap(ssCustom); stateValidator2.Map = occGrid; stateValidator2.ValidationDistance = 0.05; planner = plannerRRT(ssCustom,stateValidator2); planner.MaxConnectionDistance = 2.0; planner.MaxIterations = 30000; planner.GoalReachedFcn = @exampleHelperCheckIfGoal;
Plan the path between the start and goal. Reset the random number generator again.
rng default [pthObj2,solnInfo] = plan(planner,start,goal);
Draw the new path on the map. The path should only execute left turns to reach the goal.
figure show(occGrid) hold on % Show the search tree. plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-'); % Interpolate and plot path. pthObj2.interpolate(300) plot(pthObj2.States(:,1), pthObj2.States(:,2), 'r-', 'LineWidth', 2) % Show start and goal in grid map. plot(start(1), start(2), 'ro') plot(goal(1), goal(2), 'mo') hold off