Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

plan

Planificar ruta entre dos estados

Desde R2021a

Sintaxis

path = plan(planner,startState,goalState)

[path,solnInfo] = plan(planner,startState,goalState)

Descripción

path = plan(planner,startState,goalState) devuelve una ruta de árbol aleatorio de exploración rápida (RRT) bidireccional desde el estado inicial hasta el estado objetivo como un objeto navPath.

[path,solnInfo] = plan(planner,startState,goalState) también devuelve la información de la solución de la planificación de ruta.

Ejemplos

Planificar ruta entre dos estados utilizando RRT bidireccional

Utilice el objeto plannerBiRRT para planificar una ruta entre dos estados en un entorno con obstáculos. Visualice la ruta planificada con estados interpolados.

Crea un espacio de estados.

ss = stateSpaceSE2;

Cree un validador de estado basado en occupancyMap utilizando el espacio de estado creado.

sv = validatorOccupancyMap(ss);

Cree un mapa de ocupación a partir de un mapa de ejemplo y establezca la resolución del mapa en 10 celdas por metro.

load exampleMaps
map = occupancyMap(ternaryMap,10);

Asigne el mapa de ocupación al objeto validador de estado. Especifique el intervalo de distancia de muestreo.

sv.Map = map;
sv.ValidationDistance = 0.01;

Actualice los límites del espacio de estados para que sean los mismos que los límites del mapa.

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

Cree el planificador de rutas y aumente la distancia máxima de conexión.

planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 0.3;

Especifique los estados de inicio y objetivo.

start = [20 10 0];
goal = [40 40 0];

Planifica una ruta. Debido a la aleatoriedad del algoritmo RRT, establezca el valor inicial rng para la repetibilidad.

rng(100,'twister')
[pthObj,solnInfo] = plan(planner,start,goal);

Muestra el número de iteraciones necesarias para que el árbol converja.

fprintf("Number of iterations: %d\n",solnInfo.NumIterations)

Number of iterations: 346

Visualizar los resultados

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

Replanifique la ruta con la propiedad EnableConnectHeuristic establecida en verdadera.

planner.EnableConnectHeuristic = true;
[pthObj,solnInfo] = plan(planner,start,goal);

Muestra el número de iteraciones necesarias para que el árbol converja. Observe que el planificador requiere significativamente menos iteraciones en comparación con cuando la propiedad EnableConnectHeuristic está establecida en falsa.

fprintf("Number of iterations: %d\n",solnInfo.NumIterations)

Number of iterations: 192

Visualizar los resultados

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

Planifique la ruta a través del mapa de ocupación 3D utilizando el planificador RRT bidireccional

Cargue un mapa de ocupación tridimensional de una manzana de la ciudad en el espacio de trabajo. Especifique el umbral para considerar las celdas libres de obstáculos.

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

Infle el mapa de ocupación para agregar una zona de amortiguamiento para una operación segura alrededor de los obstáculos.

inflate(omap,1)

Cree un objeto de espacio de estados SE(3) con límites para las variables de estado.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

Cree un validador de estado de mapa de ocupación 3D utilizando el espacio de estado creado. Asigne el mapa de ocupación al objeto validador de estado. Especifique el intervalo de distancia de muestreo.

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

Cree un planificador de rutas RRT bidireccional con una distancia máxima de conexión aumentada y un número máximo reducido de iteraciones. Establezca la propiedad EnableConnectHeuristic en verdadero.

planner = plannerBiRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          EnableConnectHeuristic = true);

Especifique las poses de inicio y objetivo.

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Configure el generador de números aleatorios para obtener resultados repetibles.

rng(1,"twister");

Planifique la ruta.

[pthObj,solnInfo] = plan(planner,start,goal);

Visualice la ruta planificada.

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)

Argumentos de entrada

planner — Planificador de rutas

objeto plannerBiRRT

Planificador de rutas, especificado como un objeto plannerBiRRT.

startState — Estado inicial de la ruta

vector de valor real de N-elemento

Estado inicial de la ruta, especificado como un vector de valor real de elementos N. N es el número de dimensiones en el espacio de estados.

Ejemplo: [1 1 pi/6]

Ejemplo: [40 180 25 0.7 0.2 0 0.1]

Tipos de datos: single | double

goalState — Estado objetivo de la ruta

vector de valor real de N-elemento

Estado objetivo de la ruta, especificado como un vector de valor real de elementos N. N es el número de dimensiones en el espacio de estados.

Ejemplo: [2 2 pi/3]

Ejemplo: [150 33 35 0.3 0 0.1 0.6]

Tipos de datos: single | double

Argumentos de salida

path — Información de ruta planificada

objeto navPath

Información de ruta planificada, devuelta como un objeto navPath.

solnInfo — Información de la solución

estructura

Información de la solución, devuelta como una estructura. La estructura contiene estos campos:

Campo	Descripción
IsPathFound	Indica si se encuentra una ruta. Devuelve 1 (true) si se encuentra una ruta. De lo contrario, devuelve 0 (false).
ExitFlag	Indica la causa de terminación del planificador, devuelta como: 1 — El planificador alcanza la meta. 2 — El planificador alcanza el número máximo de iteraciones. 3 — El planificador alcanza el número máximo de nodos.
StartTreeNumNodes	Número de nodos en el árbol de búsqueda inicial cuando finaliza el planificador, excluyendo el nodo raíz.
GoalTreeNumNodes	Número de nodos en el árbol de búsqueda de objetivos cuando finaliza el planificador, excluyendo el nodo raíz.
NumIterations	Número de iteraciones combinadas tanto del árbol inicial como del árbol objetivo.
StartTreeData	Colección de estados explorados que reflejan el estado del árbol de búsqueda inicial cuando finaliza el planificador. Tenga en cuenta que los valores NaN se insertan como delimitadores para separar cada borde individual.
GoalTreeData	Colección de estados explorados que reflejan el estado del árbol de búsqueda de objetivos cuando finaliza el planificador. Tenga en cuenta que los valores NaN se insertan como delimitadores para separar cada borde individual.

Tipos de datos: structure

Historial de versiones

Introducido en R2021a

Consulte también

Objetos

• plannerBiRRT | navPath

Funciones

• copy | shortenpath