Plan Path to Custom Goal Region for Mobile Robot

This example shows how to plan a path for a mobile robot to a goal region using a rapidly exploring random tree (RRT) path planner. In this example, you can define a custom goal region as a 2-D polygon, and then plan a path to it.

When using domestic helper robots or package delivery robots, it might not be possible to define the goal as a single point in a dynamic environment. For example, if the goal point is temporarily occupied, this can cause the planner to fail. In such scenarios it can be more efficient to plan to reach a point in an acceptable goal region instead. You can leverage the GoalReachedFcn property of the plannerRRT and plannerRRTStar objects to plan a path to a goal region. This example shows you how to define a goal region as a polygon using the polyshape function, and then plan a path to it using, plannerRRT path planner.

For this example, consider the use case of a warehouse package delivery robot. The robot picks up a package from the warehouse and drops it in a specified area near the entrance of the warehouse, rather than at a particular point.

Set Up Environment and Start Position of Robot

Create an occupancyMap object for a warehouse map, and specify the initial position of the robot.

% Load the map and create an occupancy map with resolution of 1 cell/meter.
load wareHouseMap.mat
map = occupancyMap(logicalMap);

% Visualize the map.
show(map)
hold on

% Set the initial position of the robot.
initialPos = [10 40 -pi/4];
helperFunctionPlotRobot(initialPos)
hold off

Define Goal Region and Custom Goal-Reached Function

Define a goal region near the entrance of the warehouse using the polyshape function. The polyshape function enables you to define an n-sided polygon by specifying the vertices of the polygon. You can also use the nsidedpoly function to create regular polygons.

% Specify vertices of the goal Region.
vertices = [1 17;
            1 15;
            25 15;
            25 17];

% Create goal region using the vertices.
goalRegion = polyshape(vertices);

Visualize the robot and the goal region on the warehouse map.

hold on
plot(goalRegion,FaceColor="red",FaceAlpha=0.2)
hold off

Create a custom anonymous function to evaluate whether a particular point is within the goal region.

goalReachedRegionCheck = @(planner,currentPos,goalPos) isinterior(goalRegion,currentPos(1:2));

Define State Space and State Validator

Create a Dubins state space.

ss = stateSpaceDubins;

Create a state validator using the created state space and the map.

sv = validatorOccupancyMap(ss,Map=map);

Set the distance for interpolating and validating path segments.

sv.ValidationDistance=0.1;

Update the state space bounds to be the same as the map limits.

ss.StateBounds=[map.XWorldLimits; map.YWorldLimits; [-pi pi]];

Plan Path

Create the path planner, reduce the number of iterations, and increase the maximum connection distance. Specify your custom goal-reached function to check if the goal is reached.

planner = plannerRRT(ss,sv, ...
                     MaxIterations=2500, ...
                     MaxConnectionDistance=5, ...
                     GoalReachedFcn=goalReachedRegionCheck);

Plan the path from the current robot position to the goal region on the map. Consider the centroid of the goal region as the goal position. In some cases the centroid may be occupied. In such a case, take any other obstacle-free location in the region as the goal position.

% Set the goal position to the centroid of the goal region.
[goal_x,goal_y] = centroid(goalRegion);
goal = [goal_x goal_y 0];

% Plan the path.
rng(10,"twister") % for repeatable result
[pthObj,solnInfo] = plan(planner,initialPos,goal);

Visualize Path

Plot the path computed by the planner from the current robot position to the goal region on the warehouse map.

hold on
plot(pthObj.States(:,1),pthObj.States(:,2),"r-",LineWidth=1)
hold off

Optimize Path

Optimize the path to obtain a smooth path with sufficient clearance for the robot to navigate the map.

Create a configuration options object for optimization.

options = optimizePathOptions;

Maintain a safe distance of 2 meters from the obstacles.

options.ObstacleSafetyMargin = 2;

Optimize the path generated by the planner.

optPath = optimizePath(pthObj.States,map,options);
hold on
plot(optPath(:,1),optPath(:,2),"m-",LineWidth=1)
legend("","Planned Path","Optimized Path")
hold off