plan
Syntax
Description
returns an obstacle-free path as a path
= plan(planner
,startState
,goalState
)navPath
object
between the start state and the goal state within a roadmap that contains a network graph of
connected nodes.
[
also returns path
,solutionInfo
] = plan(planner
,startState
,goalState
)solutionInfo
as a structure that contains the solution
information of the path planning.
Examples
Plan Obstacle-Free Path Using Probabilistic Roadmap Path Planner
Create an occupancy map from an example map and set the map resolution as 10 cells/meter.
map = load("exampleMaps.mat").simpleMap;
map = occupancyMap(map,10);
Create a state space and update the state space bounds to be the same as the map limits.
ss = stateSpaceSE2; ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];
Create a state validator with stateSpaceSE2
using the map and set the validation distance.
sv = validatorOccupancyMap(ss,Map=map); sv.ValidationDistance = 0.01;
Create a plannerPRM
object.
planner = plannerPRM(ss,sv);
Retrieve graph as a digraph object.
graph = graphData(planner);
Extract nodes and edges from graph.
edges = table2array(graph.Edges); nodes = table2array(graph.Nodes);
Specify the start and goal states.
start = [0.5 0.5 0]; goal = [2.5 0.2 0];
Plot map and graph.
show(sv.Map) hold on plot(nodes(:,1),nodes(:,2),"*","Color","b","LineWidth",2) for i = 1:size(edges,1) % Samples states at distance 0.02 meters. states = interpolate(ss,nodes(edges(i,1),:), ... nodes(edges(i,2),:),0:0.02:1); plot(states(:,1),states(:,2),"Color","b") end plot(start(1),start(2),"*","Color","g","LineWidth",3) plot(goal(1),goal(2),"*","Color","r","LineWidth",3)
Plan a path with default settings. Set the rng
seed for repeatability.
rng(100,"twister");
[pthObj, solnInfo] = plan(planner,start,goal);
Visualize the results.
if solnInfo.IsPathFound interpolate(pthObj,1000); plot(pthObj.States(:,1),pthObj.States(:,2), ... "Color",[0.85 0.325 0.098],"LineWidth",2) else disp("Path not found") end hold off
Plan Path Through 3-D Occupancy Map Using Probabilistic Roadmap Planner
Configure the random number generator for repeatable result.
rng(1,"twister");
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 probabilistic roadmap path planner with increased maximum connection distance.
planner = plannerPRM(ss,sv);
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];
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") % Goal state scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled") % Path plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ... "r-",LineWidth=2)
Input Arguments
planner
— Path planner
plannerPRM
object
Path planner, specified as a plannerPRM
object.
startState
— Start state of the path
N-element real-valued row vector
Start state of the path, specified as an N-element real-valued row vector. N is the dimension of the state space.
Example: [1 1 pi/6]
Data Types: single
| double
goalState
— Goal state of the path
N-element real-valued row vector
Goal state of the path, specified as an N-element real-valued row vector. N is the dimension of the state space.
Example: [2 2 pi/3]
Data Types: single
| double
Output Arguments
path
— Planned path information
navPath
object
Planned path information, returned as a navPath
object.
solutionInfo
— Solution information
structure
Solution information, returned as a structure. The structure contains the field:
Field | Description |
---|---|
IsPathFound | Indicates whether a path is found. It returns as
|
Data Types: struct
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2022a
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)