Main Content

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 R2019b

Descripción

path = plan(planner,startState,goalState) devuelve un path desde el estado inicial hasta el estado objetivo.

ejemplo

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

Ejemplos

contraer todo

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/metro.

load exampleMaps
map = occupancyMap(simpleMap,10);
sv.Map = map;

Establezca la distancia de validación para el validador.

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 = plannerRRT(ss,sv,MaxConnectionDistance=0.3);

Establezca los estados de inicio y objetivo.

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

Planifique una ruta con la configuración predeterminada.

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

Visualiza los resultados.

show(map)
hold on
% Tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 3 objects of type image, line.

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 con una distancia máxima de conexión aumentada y un número máximo reducido de iteraciones. Especifique una función de objetivo personalizada que determine que una ruta alcanza el objetivo si la distancia euclidiana al objetivo está por debajo de un umbral de 1 metro.

planner = plannerRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

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")
% 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)

Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 4 objects of type patch, scatter, line.

Argumentos de entrada

contraer todo

Planificador de ruta, especificado como un objeto plannerRRT o un objeto plannerRRTStar .

Estado inicial de la ruta, especificado como un vector de valor real de elemento N. N es la dimensión del espacio de estados.

Ejemplo: [1 1 pi/6]

Ejemplo: [40 180 25 0.7 0.2 0 0.1]

Tipos de datos: single | double

Estado objetivo de la ruta, especificado como un vector de valor real de elemento N. N es la dimensión del 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

contraer todo

Un objeto que contiene la información de la ruta planificada, devuelto como un objeto navPath .

Información de la solución, devuelta como una estructura. Los campos de la estructura son:

Campos de solutionInfo

CamposDescripción
IsPathFoundIndica si se encuentra una ruta. Devuelve como 1 si se encuentra una ruta. En caso contrario, devuelve 0.
ExitFlag

Indica el estado de terminación del planificador, devuelto como

  • 1 — si se alcanza el objetivo

  • 2 — si se alcanza el número máximo de iteraciones

  • 3 — si se alcanza el número máximo de nodos

NumNodesNúmero de nodos en el árbol de búsqueda cuando finaliza el planificador (excluyendo el nodo raíz).
NumIterationsNúmero de rutinas de "extensión" ejecutadas.
TreeDataUna colección de estados explorados que refleja el estado del árbol de búsqueda cuando finaliza el planificador. Tenga en cuenta que los valores NaN se insertan como delimitadores para separar cada borde individual.
PathCosts

Contiene el coste de la ruta en cada iteración. El valor de las iteraciones cuando la ruta no ha alcanzado el objetivo se indica mediante NaN. El tamaño del arreglo es NumIterations por 1. El último elemento contiene el coste de la ruta final.

Nota

Este campo es aplicable solo para el objeto plannerRRTStar .

Tipos de datos: structure

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2019b

Consulte también

Objetos

Funciones