plan mobile robot paths using rrt -凯发k8网页登录

load occupancy map

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

define state space

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;

plan the path

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.

create a 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;

customize the 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);

plot the path

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

customize dubins vehicle constraints

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 dubinsconnection object.

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;

plan path

create a new planner object with the custom dubins constraints and a validator based on those constraints. specify the same goalreached function.

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);

plot path

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