plan
Description
Examples
Plan Path Between Two States Using Bidirectional RRT
Use the plannerBiRRT
object to plan a path between two states in an environment with obstacles. Visualize the planned path with interpolated states.
Create a state space.
ss = stateSpaceSE2;
Create an occupancyMap
-based state validator using the created state space.
sv = validatorOccupancyMap(ss);
Create an occupancy map from an example map and set map resolution as 10 cells per meter.
load exampleMaps
map = occupancyMap(ternaryMap,10);
Assign the occupancy map to the state validator object. Specify the sampling distance interval.
sv.Map = map; sv.ValidationDistance = 0.01;
Update the state space bounds to be the same as the map limits.
ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];
Create the path planner and increase the maximum connection distance.
planner = plannerBiRRT(ss,sv); planner.MaxConnectionDistance = 0.3;
Specify the start and goal states.
start = [20 10 0]; goal = [40 40 0];
Plan a path. Due to the randomness of the RRT algorithm, set the rng
seed for repeatability.
rng(100,'twister')
[pthObj,solnInfo] = plan(planner,start,goal);
Display the number of iterations taken for the tree to converge.
fprintf("Number of iterations: %d\n",solnInfo.NumIterations)
Number of iterations: 346
Visualize the results.
show(map) hold on % Plot start pose and goal pose plot(start(1), start(2),plannerLineSpec.start{:}); plot(goal(1), goal(2), plannerLineSpec.goal{:}); % Start tree expansion plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ... plannerLineSpec.tree{:}) % Goal tree expansion plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ... plannerLineSpec.goalTree{:}) % Draw path plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:}) legend hold off
Replan the path with the EnableConnectHeuristic
property set to true.
planner.EnableConnectHeuristic = true; [pthObj,solnInfo] = plan(planner,start,goal);
Display the number of iterations taken for the tree to converge. Observe that the planner requires significantly fewer iterations compared to when the EnableConnectHeuristic
property is set to false.
fprintf("Number of iterations: %d\n",solnInfo.NumIterations)
Number of iterations: 192
Visualize the results.
figure show(map) hold on % Start tree expansion % Plot start pose and goal pose plot(start(1), start(2),plannerLineSpec.start{:}); plot(goal(1), goal(2), plannerLineSpec.goal{:}); plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ... plannerLineSpec.tree{:}) % Goal tree expansion plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ... plannerLineSpec.goalTree{:}) % Draw path plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:}) legend hold off
Plan Path Through 3-D Occupancy Map Using Bidirectional RRT Planner
Load a 3-D occupancy map of a city block into the workspace. Specify the threshold to consider cells as obstacle-free.
mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;
Inflate the occupancy map to add a buffer zone for safe operation around the obstacles.
inflate(omap,1)
Create an SE(3) state space object with bounds for state variables.
ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);
Create a 3-D occupancy map state validator using the created state space. Assign the occupancy map to the state validator object. Specify the sampling distance interval.
sv = validatorOccupancyMap3D(ss, ... Map = omap, ... ValidationDistance = 0.1);
Create a bidirectional RRT path planner with increased maximum connection distance and reduced maximum number of iterations. Set EnableConnectHeuristic
property to true.
planner = plannerBiRRT(ss,sv, ... MaxConnectionDistance = 50, ... MaxIterations = 1000, ... EnableConnectHeuristic = true);
Specify start and goal poses.
start = [40 180 25 0.7 0.2 0 0.1]; goal = [150 33 35 0.3 0 0.1 0.6];
Configure the random number generator for repeatable result.
rng(1,"twister");
Plan the path.
[pthObj,solnInfo] = plan(planner,start,goal);
Visualize the planned path.
show(omap) axis equal view([-10 55]) hold on % Start state scatter3(start(1,1),start(1,2),start(1,3),"g","filled") % Start tree expansion plot3(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2), ... solnInfo.StartTreeData(:,3),".-",Color="g") % Goal state scatter3(goal(1,1),goal(1,2),goal(1,3),"y","filled") % Goal tree expansion plot3(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2), ... solnInfo.GoalTreeData(:,3),".-",Color="y") % Path plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ... "m-",LineWidth=2)
Input Arguments
planner
— Path planner
plannerBiRRT
object
Path planner, specified as a plannerBiRRT
object.
startState
— Start state of path
N-element real-valued vector
Start state of the path, specified as an N-element real-valued vector. N is the number of dimensions in the state space.
Example: [1 1 pi/6]
Example: [40 180 25 0.7 0.2 0 0.1]
Data Types: single
| double
goalState
— Goal state of path
N-element real-valued vector
Goal state of the path, specified as an N-element real-valued vector. N is the number of dimensions in the state space.
Example: [2 2 pi/3]
Example: [150 33 35 0.3 0 0.1 0.6]
Data Types: single
| double
Output Arguments
path
— Planned path information
navPath
object
Planned path information, returned as a navPath
object.
solnInfo
— Solution Information
structure
Solution Information, returned as a structure. The structure contains these fields:
Field | Description |
---|---|
IsPathFound | Indicates whether a path is found. It returns 1
(true ) if a path is found. Otherwise, it returns
0 (false ). |
ExitFlag | Indicates the termination cause of the planner, returned as:
|
StartTreeNumNodes | Number of nodes in the start search tree when the planner terminates, excluding the root node. |
GoalTreeNumNodes | Number of nodes in the goal search tree when the planner terminates, excluding the root node. |
NumIterations | Number of combined iterations by both the start tree and goal tree. |
StartTreeData | Collection of explored states that reflect the status of the start search
tree when the planner terminates. Note that NaN values are
inserted as delimiters to separate each individual edge. |
GoalTreeData | Collection of explored states that reflect the status of the goal search
tree when the planner terminates. Note that NaN values are
inserted as delimiters to separate each individual edge. |
Data Types: structure
Version History
Introduced in R2021a
MATLAB コマンド
次の MATLAB コマンドに対応するリンクがクリックされました。
コマンドを MATLAB コマンド ウィンドウに入力して実行してください。Web ブラウザーは MATLAB コマンドをサポートしていません。
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)