Contenido principal

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

plannerMPNET

Cree un planificador de rutas bidireccional basado en MPNet

Desde R2024a

Descripción

El objeto plannerMPNET almacena una red de planificación de movimiento (MPNet) entrenada previamente para usar en la planificación de rutas. El planificador de rutas MPNet es un planificador bidireccional y converge más rápidamente hacia la solución óptima. El planificador de rutas MPNet utiliza dos enfoques de planificación para calcular rutas sin colisiones: 1) Planificación de trayectoria neuronal bidireccional y 2) Planificación de trayectoria clásica. El método de planificación de ruta neuronal utiliza una MPNet previamente entrenada para calcular los estados de una ruta libre de colisiones. Los estados calculados mediante la planificación de rutas neuronales se denominan estados aprendidos. El método clásico de planificación de rutas utiliza algoritmos de planificación de rutas, como el método de árbol aleatorio de exploración rápida (RRT), para calcular los estados de una ruta libre de colisiones. Los estados calculados utilizando la planificación de trayectoria clásica se denominan estados clásicos. De forma predeterminada, el planificador de rutas MPNet utiliza la planificación de rutas neuronales para calcular la ruta óptima entre dos estados. El planificador cambia a un enfoque de planificación de ruta clásica solo si el método de planificación de ruta neuronal no puede encontrar una ruta entre dos estados. Para obtener información sobre cómo funciona el planificador de rutas MPNet, consulte la sección Algoritmos.

Creación

Descripción

planner = plannerMPNET(stateValidator,mpnet) crea un planificador de ruta MPNet bidireccional utilizando un validador de estado y un MPNet entrenado previamente.

  • El argumento stateValidator establece la propiedad StateValidator del planificador de rutas MPNet.

  • El argumento mpnet establece la propiedad MotionPlanningNetwork del planificador de rutas MPNet.

ejemplo

planner = plannerMPNET(___,Name=Value) especifica las propiedades MaxLearnedStates y ClassicalPlannerFcn del planificador de rutas MPNet usando argumentos de nombre-valor.

Nota

Para ejecutar esta función, necesitará Deep Learning Toolbox™.

Propiedades

expandir todo

Esta propiedad o parámetro es de solo lectura.

Validador de estado para el planificador de rutas MPNet, especificado como un objeto validatorVehicleCostmap, un objeto validatorOccupancyMap o un objeto de la subclase de la clase nav.StateValidator.

Esta propiedad o parámetro es de solo lectura.

MPNet preentrenado para la planificación de rutas neuronales, especificado como un objeto mpnetSE2. El planificador de rutas MPNet converge más rápidamente hacia la solución óptima si la red entrenada previamente se entrena en rutas óptimas. Para obtener más información sobre MPNet, consulte Comience con las redes de planificación de movimiento.

Número máximo de estados aprendidos que generará el planificador de rutas MPNet, especificado como un entero escalar positivo. Si este valor se establece en cero, el planificador de rutas MPNet utiliza de manera predeterminada un método de planificación de rutas clásico, tal como se especifica en la propiedad ClassicalPlannerFcn.

Tipos de datos: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Planificador clásico para planificación, especificado como un identificador de función para llamar a cualquiera de estos planificadores de rutas: objeto plannerRRT, plannerBiRRT o plannerRRTStar. El planificador de rutas recurre a un planificador clásico en los siguientes casos:

  1. El número máximo de estados aprendidos se establece en 0.

  2. El número máximo de estados aprendidos es insuficiente para calcular una ruta entre los puntos de inicio y destino.

  3. Uno o más estados consecutivos en la ruta global, devuelta por MPNet, chocan con un obstáculo. Para obtener más información sobre cómo el planificador vuelve a calcular la ruta en presencia de colisiones, consulte la sección Algoritmos.

De forma predeterminada, el planificador utiliza el algoritmo de planificación de ruta RRT* si el MPNet entrenado previamente no logra generar una ruta. Esta propiedad solo es visible cuando se especifica ClassicalPlannerFcn como entrada.

Tipos de datos: function_handle

Funciones del objeto

planEncuentra un camino sin obstáculos entre dos estados
copyCrear una copia profunda del objeto del planificador de rutas de MPNet

Ejemplos

contraer todo

Cargar MPNet previamente entrenado

Cargue un archivo de datos que contenga un MPNet previamente entrenado en el espacio de trabajo de MATLAB® . El MPNet ha sido entrenado en varios mapas de laberintos 2-D con anchos y alturas de 10 metros y resoluciones de 2,5 celdas por metro. Cada mapa de laberinto contiene un ancho de paso de 5 celdas de cuadrícula y un espesor de pared de 1 celda de cuadrícula.

data = load("mazeMapTrainedMPNET.mat")
data = struct with fields:
      encodingSize: [9 9]
       lossWeights: [100 100 0]
        mazeParams: {[5]  [1]  'MapSize'  [10 10]  'MapResolution'  [2.5000]}
       stateBounds: [3×2 double]
    trainedNetwork: [1×1 dlnetwork]

Establezca el valor de semilla para generar resultados repetibles.

rng(10,"twister")

Cree un mapa de laberinto para la planificación del movimiento

Crea un mapa de laberinto aleatorio para planificar el movimiento. El tamaño de la cuadrícula (MapSize×MapResolution) debe ser el mismo que el de los mapas utilizados para entrenar MPNet.

map = mapMaze(5,1,MapSize=[10 10],MapResolution=2.5);

Crear validador de estado

Cree un objeto validador de estado para utilizar en la planificación del movimiento.

stateSpace = stateSpaceSE2(data.stateBounds);
stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

Seleccionar estados de inicio y de destino

Genere múltiples estados de inicio y objetivo aleatorios utilizando la función sampleStartGoal.

[startStates,goalStates] = sampleStartGoal(stateValidator,100);

Calcular la distancia entre los estados inicial y objetivo generados.

stateDistance= distance(stateSpace,startStates,goalStates);

Seleccione dos estados que estén más alejados entre sí como inicio y objetivo para la planificación del movimiento.

[dist,index] = max(stateDistance);
start = startStates(index,:);
goal = goalStates(index,:);

Visualice el mapa de entrada.

figure
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 3 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Start, Goal.

Calcular ruta usando el Planificador de rutas de MPNet

Configure el objeto mpnetSE2 para utilizar el MPNet entrenado previamente para la planificación de rutas. Establezca los valores de la propiedad EncodingSize del objeto mpnetSE2 en los valores utilizados para entrenar la red.

mpnet = mpnetSE2(Network=data.trainedNetwork,StateBounds=data.stateBounds,EncodingSize=data.encodingSize);

Cree un planificador de rutas MPNet utilizando el validador de estado y el MPNet entrenado previamente.

planner = plannerMPNET(stateValidator,mpnet);

Planifique una ruta entre los estados de inicio y destino seleccionados utilizando el planificador de rutas MPNet.

[pathObj,solutionInfo] = plan(planner,start,goal);

Mostrar ruta planificada

Muestra el objeto navPath devuelto por el planificador de rutas MPNet. El número de estados en la ruta planificada y los vectores de estado asociados se especifican mediante las propiedades NumStates y States del objeto navPath, respectivamente.

disp(pathObj)
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [5×3 double]
       NumStates: 5
    MaxNumStates: Inf

Establezca las propiedades de línea y marcador para mostrar los estados de inicio y objetivo utilizando las funciones plannerLineSpec.start y plannerLineSpec.goal, respectivamente.

sstate = plannerLineSpec.start(DisplayName="Start state",MarkerSize=6);
gstate = plannerLineSpec.goal(DisplayName="Goal state",MarkerSize=6);

Establezca las propiedades de la línea para mostrar la ruta calculada utilizando la función plannerLineSpec.path.

ppath = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Planned path");

Traza la ruta planificada.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Planned path, Start state, Goal state.

Mostrar datos adicionales

Muestra la estructura solutionInfo devuelta por el planificador de rutas MPNet. Esta estructura almacena los estados aprendidos, los estados clásicos y los estados de baliza calculados por el planificador de rutas MPNet. Si el planificador de rutas MPNet no calcula alguno de estos tres tipos de estados, el valor del campo correspondiente se establece como vacío.

disp(solutionInfo)
        IsPathFound: 1
      LearnedStates: [50×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [9×3 double]

Establezca las propiedades de línea y marcador para mostrar los estados aprendidos, los estados clásicos y los estados de baliza mediante plannerLineSpec.state.

lstate = plannerLineSpec.state(DisplayName="Learned states",MarkerSize=3);
cstate = plannerLineSpec.state(DisplayName="Classical states",MarkerSize=3,MarkerFaceColor="green",MarkerEdgeColor="green");
bstate = plannerLineSpec.state(MarkerEdgeColor="magenta",MarkerSize=7,DisplayName="Beacon states",Marker="^");

Grafique los estados aprendidos, los estados clásicos y los estados de baliza junto con la ruta calculada. De la figura, se puede inferir que el enfoque de planificación de ruta neuronal no pudo calcular una ruta libre de colisiones donde hay estados de baliza. Por lo tanto, el planificador de rutas MPNet recurrió al enfoque clásico de planificación de rutas RRT*. Los estados finales de la ruta planificada constituyen los estados devueltos por la planificación de ruta neuronal y los enfoques de planificación de ruta clásica.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(solutionInfo.LearnedStates(:,1),solutionInfo.LearnedStates(:,2),lstate{:})
plot(solutionInfo.ClassicalStates(:,1),solutionInfo.ClassicalStates(:,2),cstate{:})
plot(solutionInfo.BeaconStates(:,1),solutionInfo.BeaconStates(:,2),bstate{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 7 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Planned path, Learned states, Classical states, Beacon states, Start state, Goal state.

Cargar MPNet previamente entrenado

Cargue un archivo de datos que contenga un MPNet previamente entrenado en el espacio de trabajo de MATLAB® . El MPNet ha sido entrenado en varios mapas de laberintos 2-D con anchos y alturas de 10 metros y resoluciones de 2,5 celdas por metro. Cada mapa de laberinto contiene un ancho de paso de 5 celdas de cuadrícula y un espesor de pared de 1 celda de cuadrícula.

data = load("mazeMapTrainedMPNET.mat")
data = struct with fields:
      encodingSize: [9 9]
       lossWeights: [100 100 0]
        mazeParams: {[5]  [1]  'MapSize'  [10 10]  'MapResolution'  [2.5000]}
       stateBounds: [3×2 double]
    trainedNetwork: [1×1 dlnetwork]

Establezca el valor de semilla para generar resultados repetibles.

rng(70,"twister")

Cree un mapa de laberinto para la planificación del movimiento

Crea un mapa de laberinto aleatorio para planificar el movimiento. El tamaño de la cuadrícula (MapSize×MapResolution) debe ser el mismo que el de los mapas utilizados para entrenar MPNet.

map = mapMaze(5,1,MapSize=[12 12],MapResolution=2.0833);

Crear validador de estado

Cree un objeto validador de estado para utilizar en la planificación del movimiento.

stateSpace = stateSpaceSE2(data.stateBounds);
stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

Seleccionar estados de inicio y de destino

Seleccione un estado inicial y objetivo utilizando la función sampleStartGoal.

[startStates,goalStates] = sampleStartGoal(stateValidator,500);

Calcular la distancia entre los estados inicial y objetivo generados.

stateDistance= distance(stateSpace,startStates,goalStates);

Seleccione dos estados que estén más alejados entre sí como inicio y objetivo para la planificación del movimiento.

[dist,index] = max(stateDistance);
start = startStates(index,:);
goal = goalStates(index,:);

Crear un planificador de rutas de MPNet

Configure el objeto mpnetSE2 para utilizar el MPNet entrenado previamente para la planificación de rutas. Establezca los valores de la propiedad EncodingSize del objeto mpnetSE2 en los valores utilizados para entrenar la red.

mpnet = mpnetSE2(Network=data.trainedNetwork,StateBounds=data.stateBounds,EncodingSize=data.encodingSize);

Cree un planificador de rutas MPNet utilizando el validador de estado y el MPNet entrenado previamente. Planifique una ruta entre los estados de inicio y destino seleccionados utilizando el planificador de rutas MPNet.

planner{1} = plannerMPNET(stateValidator,mpnet);
[pathObj1,solutionInfo1] = plan(planner{1},start,goal)
pathObj1 = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [6×3 double]
       NumStates: 6
    MaxNumStates: Inf

solutionInfo1 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [20×3 double]

Crear una copia del Planificador de rutas de MPNet

Cree una copia de la primera instancia del planificador de rutas MPNet.

planner{2} = copy(planner{1});

Modificar el enfoque clásico de planificación de rutas

Especifique el planificador RRT bidireccional (Bi-RRT) como el enfoque de planificación de ruta clásico para el planificador de ruta MPNet. Establezca el valor máximo de distancia de conexión en 1.

classicalPlanner = plannerBiRRT(stateSpace,stateValidator,MaxConnectionDistance=1);
planner{2}.ClassicalPlannerFcn = @classicalPlanner.plan;

Planifique una ruta entre los estados de inicio y destino seleccionados utilizando el planificador de rutas MPNet modificado.

[pathObj2,solutionInfo2] = plan(planner{2},start,goal)
pathObj2 = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [5×3 double]
       NumStates: 5
    MaxNumStates: Inf

solutionInfo2 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [7×3 double]

Visualizar resultados

Establezca las propiedades de línea y marcador para mostrar los estados de inicio y objetivo utilizando las funciones plannerLineSpec.start y plannerLineSpec.goal, respectivamente.

sstate = plannerLineSpec.start(DisplayName="Start state",MarkerSize=6);
gstate = plannerLineSpec.goal(DisplayName="Goal state",MarkerSize=6);

Establezca las propiedades de línea y marcador para mostrar los resultados calculados mediante la función plannerLineSpec.path.

ppath1 = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Path computed using RRT* for classical path planning");
ppath2 = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="red",DisplayName="Path computed using Bi-RRT for classical path planning");

Grafique las rutas calculadas. Se puede inferir que el planificador de rutas MPNet ofrece mejores resultados cuando se utiliza el planificador de rutas Bi-RRT para la planificación de rutas clásica.

figure
show(map)
hold on
plot(pathObj1.States(:,1),pathObj1.States(:,2),ppath1{:})
plot(pathObj2.States(:,1),pathObj2.States(:,2),ppath2{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="southoutside")
hold off

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Path computed using RRT* for classical path planning, Path computed using Bi-RRT for classical path planning, Start state, Goal state.

Cargue un archivo de datos que contenga un MPNet previamente entrenado en el espacio de trabajo de MATLAB® . El MPNet ha sido entrenado en un único mapa para un vehículo Dubins. Puede utilizar este MPNet previamente entrenado para encontrar rutas de cálculo entre cualquier estado inicial y estado objetivo en el mapa utilizado para el entrenamiento.

inputData = load("officeMapTrainedMPNET.mat")
inputData = struct with fields:
      encodingSize: 0
       lossWeights: [100 100 10]
        officeArea: [1×1 occupancyMap]
    trainedNetwork: [1×1 dlnetwork]

Lea el mapa utilizado para entrenar la red.

map = inputData.officeArea;

Especifique los límites de las variables del espacio de estados correspondientes al mapa de entrada.

x = map.XWorldLimits;
y = map.YWorldLimits;
theta = [-pi pi];
stateBounds = [x; y; theta];

Crea un espacio de estado Dubins y establece el radio de giro mínimo en 0.2.

stateSpace = stateSpaceDubins(stateBounds);
stateSpace.MinTurningRadius = 0.2;

Cree un objeto validador de estado para utilizar en la planificación del movimiento. Establezca la distancia de validación en 0,1 m.

stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

Establezca el valor de semilla para generar resultados repetibles.

rng(100,"twister");

Genere múltiples estados de inicio y objetivo aleatorios utilizando la función sampleStartGoal.

[startStates,goalStates] = sampleStartGoal(stateValidator,500);

Calcular la distancia entre los estados inicial y objetivo generados.

stateDistance= distance(stateSpace,startStates,goalStates);

Seleccione dos estados que estén más alejados entre sí como inicio y objetivo para la planificación del movimiento.

[dist,index] = max(stateDistance);
start = startStates(index,:);
goal = goalStates(index,:);

Configure el objeto mpnetSE2 para utilizar el MPNet entrenado previamente para predecir muestras de estado entre una pose inicial y una pose objetivo. Establezca el valor EncodingSize en 0.

mpnet = mpnetSE2(Network=inputData.trainedNetwork,StateBounds=stateBounds,EncodingSize=0);

Cree un planificador de rutas MPNet utilizando el validador de estado y el MPNet entrenado previamente.

planner = plannerMPNET(stateValidator,mpnet);

Planifique una ruta entre los estados de inicio y destino seleccionados utilizando el planificador de rutas MPNet.

[pathObj,solutionInfo]  = plan(planner,start,goal)
pathObj = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceDubins]
          States: [5×3 double]
       NumStates: 5
    MaxNumStates: Inf

solutionInfo = struct with fields:
        IsPathFound: 1
      LearnedStates: [23×3 double]
       BeaconStates: [4×3 double]
    ClassicalStates: [0×3 double]

Establezca las propiedades de línea y marcador para mostrar los estados de inicio y objetivo utilizando las funciones plannerLineSpec.start y plannerLineSpec.goal, respectivamente.

sstate = plannerLineSpec.start(DisplayName="Start state",MarkerSize=6);
gstate = plannerLineSpec.goal(DisplayName="Goal state",MarkerSize=6);

Establezca las propiedades de la línea para mostrar la ruta calculada utilizando la función plannerLineSpec.path.

ppath = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Planned path");

Muestre el mapa de entrada y trace las poses inicial y final.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Planned path, Start state, Goal state.

Muestra la estructura solutionInfo devuelta por el planificador de rutas MPNet. Esta estructura almacena los estados aprendidos, los estados clásicos y los estados de baliza calculados por el planificador de rutas MPNet. Si el planificador de rutas MPNet no calcula alguno de estos tres tipos de estados, el valor del campo correspondiente se establece como vacío.

disp(solutionInfo)
        IsPathFound: 1
      LearnedStates: [23×3 double]
       BeaconStates: [4×3 double]
    ClassicalStates: [0×3 double]

Establezca las propiedades de línea y marcador para mostrar los estados aprendidos y los estados de baliza mediante plannerLineSpec.state.

lstate = plannerLineSpec.state(DisplayName="Learned states",MarkerSize=3);
bstate = plannerLineSpec.state(MarkerEdgeColor="magenta",MarkerSize=7,DisplayName="Beacon states",Marker="^");

Grafique los estados aprendidos y los estados de baliza junto con la ruta calculada. De la figura, se puede inferir que el enfoque de planificación de ruta neuronal calculó con éxito una ruta libre de colisiones en dos lugares donde hay estados de baliza. Por lo tanto, el planificador de rutas MPNet no recurrió al enfoque clásico de planificación de rutas RRT*. Los estados finales de la ruta planificada son los devueltos por el enfoque de planificación de ruta neuronal.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(solutionInfo.LearnedStates(:,1),solutionInfo.LearnedStates(:,2),lstate{:})
plot(solutionInfo.BeaconStates(:,1),solutionInfo.BeaconStates(:,2),bstate{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 6 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Planned path, Learned states, Beacon states, Start state, Goal state.

Este ejemplo muestra cómo entrenar una MPNet en un conjunto de datos personalizado y luego usar la red entrenada para calcular rutas entre dos estados en un entorno de mapa desconocido.

Cargar y visualizar conjunto de datos de entrenamiento

Cargue el conjunto de datos desde un archivo .mat. El conjunto de datos contiene 400.000 rutas diferentes para 200 entornos de mapas de laberintos. El conjunto de datos se ha generado para los parámetros predefinidos de la función mazeMap. La primera columna del conjunto de datos contiene los mapas y la segunda columna contiene las rutas óptimas para los estados objetivo y de inicio muestreados aleatoriamente de los mapas correspondientes. El tamaño del conjunto de datos es de 75 MB.

% Download and extract the maze map dataset
if ~exist("mazeMapDataset.mat","file")
    datasetURL = "https://ssd.mathworks.com/supportfiles/nav/data/mazeMapDataset.zip";
    websave("mazeMapDataset.zip", datasetURL);
    unzip("mazeMapDataset.zip")
end

% Load the maze map dataset
load("mazeMapDataset.mat","dataset","stateSpace")
head(dataset)
             Map                  Path     
    ______________________    _____________

    1×1 binaryOccupancyMap    {14×3 single}
    1×1 binaryOccupancyMap    { 8×3 single}
    1×1 binaryOccupancyMap    {24×3 single}
    1×1 binaryOccupancyMap    {23×3 single}
    1×1 binaryOccupancyMap    {17×3 single}
    1×1 binaryOccupancyMap    {15×3 single}
    1×1 binaryOccupancyMap    { 7×3 single}
    1×1 binaryOccupancyMap    {10×3 single}

El conjunto de datos se generó utilizando la función auxiliar examplerHelperGenerateDataForMazeMaps. La función auxiliar examplerHelperGenerateDataForMazeMaps utiliza la función mapMaze para generar mapas de laberinto aleatorios de tamaño 10 x 10 y resolución de 2,5 m. El ancho y el espesor de las paredes de los mapas se establecieron en 5 my 1 m, respectivamente.

passageWidth = 5;
wallThickness = 1;
map = mapMaze(passageWidth,wallThickness,MapSize=[10 10],MapResolution=2.5)

Luego, los estados iniciales y objetivos se generan aleatoriamente para cada mapa. La ruta óptima entre los estados inicial y objetivo se calcula utilizando el planificador de rutas plannerRRTStar. Los parámetros ContinueAfterGoalReached y MaxIterations se establecen en true y 5000, respectivamente, para generar las rutas óptimas.

planner = plannerRRTStar(stateSpace,stateValidator); % Uses default uniform state sampling
planner.ContinueAfterGoalReached = true; % Optimize after goal is reached
planner.MaxIterations = 5000; % Maximum iterations to run the planner

Visualice algunas muestras aleatorias del conjunto de datos de entrenamiento. Cada muestra contiene un mapa y la ruta óptima generada para un estado inicial y objetivo determinado.

figure
for i=1:4
    subplot(2,2,i)
    ind = randi(height(dataset)); % Select a random sample
    map = dataset(ind,:).Map; % Get map from Map column of the table
    pathStates = dataset(ind,:).Path{1}; % Get path from Path column of the table
    start = pathStates(1,:);
    goal = pathStates(end,:);

    % Plot the data
    show(map);
    hold on
    plot(pathStates(:,1),pathStates(:,2),plannerLineSpec.path{:})
    plot(start(1),start(2),plannerLineSpec.start{:})
    plot(goal(1),goal(2),plannerLineSpec.goal{:})
end
legend(Location="bestoutside")

Figure contains 4 axes objects. Axes object 1 with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Path, Start, Goal. Axes object 2 with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Path, Start, Goal. Axes object 3 with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Path, Start, Goal. Axes object 4 with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Path, Start, Goal.

Puede modificar la función auxiliar para generar nuevos mapas y entrenar MPNet desde cero. La generación del conjunto de datos puede tardar algunos días dependiendo de la configuración de la CPU y la cantidad de mapas que desee generar para el entrenamiento. Para acelerar la generación de conjuntos de datos, puede utilizar Parallel Computing Toolbox™.

Crear redes de planificación de movimiento

Cree un objeto de redes de planificación de movimiento (MPNet) para el espacio de estado SE(2) utilizando mpnetSE2. El objeto mpnetSE2 carga un MPNet preconfigurado que puedes usar para entrenamiento. Alternativamente, puede utilizar la función mpnetLayers para crear un MPNet con diferentes cantidades de entradas y capas ocultas para entrenar en el conjunto de datos.

mpnet = mpnetSE2;

Establezca las propiedades StateBounds, LossWeights y EncodingSize del objeto mpnetSE2. Establezca StateBounds utilizando la propiedad StateBounds del objeto stateSpace.

mpnet.StateBounds = stateSpace.StateBounds;

Especifique los pesos para cada variable del espacio de estado utilizando la propiedad LossWeights del objeto mpnetSE2. Debe especificar los pesos para cada variable de espacio de estado x, y y θ del espacio de estado SE(2). Para un espacio de estados SE(2), no consideramos la cinemática del robot, como el radio de giro. Por lo tanto, puedes asignar un valor de peso cero para la variable θ.

mpnet.LossWeights = [100 100 0];

Especifique el valor de la propiedad EncodingSize del objeto mpnetSE2 como [9 9]. Antes de entrenar la red, el objeto mpnetSE2 codifica los entornos del mapa de entrada en una representación compacta de tamaño 9 por 9.

mpnet.EncodingSize = [9 9];

Preparar datos para el entrenamiento

Divida el conjunto de datos en conjuntos de tren y prueba en una proporción de 0,8:0,2. El conjunto de entrenamiento se utiliza para entrenar los pesos Network minimizando la pérdida de entrenamiento, el conjunto de validación se utiliza para verificar la pérdida de validación durante el entrenamiento.

split = 0.8;
trainData = dataset(1:split*end,:);
validationData = dataset(split*end+1:end,:);

Prepare los datos para el entrenamiento convirtiendo los datos sin procesar que contienen los mapas y rutas al formato necesario para entrenar MPNet.

dsTrain = mpnetPrepareData(trainData,mpnet);
dsValidation = mpnetPrepareData(validationData,mpnet);

Visualice el conjunto de datos preparado. La primera columna de muestra contiene los datos del mapa codificados, el estado actual codificado y los estados objetivo. La segunda columna contiene el siguiente estado codificado. El estado codificado se calcula como [x,y,cos(θ),sin(θ)] y se normaliza al rango de [0, 1].

preparedDataSample = read(dsValidation);
preparedDataSample(1,:)
ans=1×2 cell array
    {[0.2607 0.4112 0.6846 0.9647 0.9138 0.5757 0.4883 1.3733e-04 0.0549 0.1646 0 0.1646 0.1646 0.1646 0.1646 0.1646 0.0549 0.1646 0.8244 0.0870 0.9383 0.8244 0.8244 0.8244 0.8244 0.1646 0.1646 0.8244 0.0870 0.9020 0.0094 0.0870 0.0870 0.0870 3.9316e-16 0.1646 0.8244 0.0870 0.9020 0.0094 0.9020 0.9043 0.9383 0.1646 0.1646 0.8244 0.0870 0.9020 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 1 0.9043 0.9020 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 0.8313 0.0870 0.0870 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 0.9333 0.8244 0.8244 0.8244 0.9383 0.0870 0.8244 0.1646 0.0549 0.1646 0.1646 0.1646 0.1646 0.1646 2.6928e-16 0.1646 0.0549]}    {[0.2720 0.4130 0.6786 0.9670]}

Entrenar la red de aprendizaje profundo

Utilice la función trainnet para entrenar el MPNet. El entrenamiento de esta red puede llevar mucho tiempo dependiendo del hardware que utilice. Establezca el valor doTraining en true para entrenar la red.

doTraining = false;

Especifique trainingOptions (Deep Learning Toolbox) para entrenar la red de aprendizaje profundo:

  • Configure el optimizador "adam".

  • Establezca MiniBatchSize para el entrenamiento en 2048.

  • Baraja los dsTrain en cada época.

  • Establezca MaxEpochs en 50.

  • Establezca ValidationData en dsValidation y ValidationFrequency en 2000.

Puede considerar que la capacitación fue exitosa una vez que la pérdida de capacitación y la pérdida de validación converjan cerca de cero.

if doTraining
    options = trainingOptions("adam",...
        MiniBatchSize=2048,...
        MaxEpochs=50,...
        Shuffle="every-epoch",...
        ValidationData=dsValidation,...
        ValidationFrequency=2000,...
        Plots="training-progress");

    % Train network
    [net,info] = trainnet(dsTrain,mpnet.Network,@mpnet.loss,options);

    % Update Network property of mpnet object with net
    mpnet.Network = net;
end

Puede guardar la red entrenada y los detalles del entorno del mapa de entrada en un archivo .mat y usarlo para realizar la planificación del movimiento. En el resto del ejemplo, utilizará un MPNet previamente entrenado para realizar directamente la planificación del movimiento en un entorno de mapa desconocido.

Cargue un archivo .mat que contenga la red entrenada previamente. La red se ha entrenado en varios mapas de laberinto generados aleatoriamente y almacenados en el archivo mazeMapDataset.mat. El archivo .mat contiene la red entrenada y los detalles de los mapas de laberinto utilizados para entrenar la red.

if ~doTraining
    data = load("mazeMapTrainedMPNET.mat")
    mpnet.Network = data.trainedNetwork;
end
data = struct with fields:
      encodingSize: [9 9]
       lossWeights: [100 100 0]
        mazeParams: {[5]  [1]  'MapSize'  [10 10]  'MapResolution'  [2.5000]}
       stateBounds: [3×2 double]
    trainedNetwork: [1×1 dlnetwork]

Realizar planificación de movimiento utilizando MPNet entrenado

Cree un mapa de laberinto aleatorio para probar la MPNet entrenada para el desplazamiento de ruta. El tamaño de la cuadrícula (MapSize×MapResolution) del mapa de prueba debe ser el mismo que el de los mapas utilizados para entrenar MPNet.

Haga clic en el botón Run para generar un nuevo mapa.

 
mazeParams = data.mazeParams;
map = mapMaze(mazeParams{:});
figure
show(map)

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

Cree un objeto de validador de estado.

stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

Cree un planificador de rutas MPNet utilizando el validador de estado y el objeto MPNet como entradas.

planner = plannerMPNET(stateValidator,mpnet);

Genere múltiples estados de inicio y objetivo aleatorios utilizando la función sampleStartGoal.

[startStates,goalStates] = sampleStartGoal(stateValidator,500);

Calcular la distancia entre los estados inicial y objetivo generados.

stateDistance=distance(stateSpace,startStates,goalStates);

Seleccione dos estados que estén más alejados entre sí como inicio y objetivo para la planificación del movimiento.

[dist,index] = max(stateDistance);
start = startStates(index,:);
goal = goalStates(index,:);

Planifique la ruta entre los estados inicial y final utilizando la MPNet entrenada.

[pathObj,solutionInfo] = plan(planner,start,goal)
pathObj = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [6×3 double]
       NumStates: 6
    MaxNumStates: Inf

solutionInfo = struct with fields:
        IsPathFound: 1
      LearnedStates: [32×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [0×3 double]

Establezca las propiedades de línea y marcador para mostrar los estados de inicio y objetivo utilizando las funciones plannerLineSpec.start y plannerLineSpec.goal, respectivamente.

sstate = plannerLineSpec.start(DisplayName="Start state",MarkerSize=6);
gstate = plannerLineSpec.goal(DisplayName="Goal state",MarkerSize=6);

Establezca las propiedades de la línea para mostrar la ruta calculada utilizando la función plannerLineSpec.path.

ppath = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Planned path");

Traza la ruta planificada.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Planned path, Start state, Goal state.

Algoritmos

La función plan del objeto plannerMPNET implementa el algoritmo de planificación de ruta MPNet presentado en [1]. Esta sección ofrece una breve descripción general del algoritmo.

  • El planificador de rutas MPNet calcula una ruta aproximada que consta de estados aprendidos que se encuentran a lo largo de la ruta global entre los estados inicial y objetivo. Si la ruta que conecta cualquier estado aprendido no está libre de colisiones, el planificador de rutas MPNet realiza una replanificación solo para ese segmento en la ruta aproximada.

  • Dos estados aprendidos consecutivos que están en el espacio libre y no se pueden conectar sin colisionar con un obstáculo se denominan estados baliza. El planificador de rutas MPNet selecciona estos estados de baliza como nuevos puntos de inicio y destino e intenta encontrar una ruta libre de colisiones para conectarlos. Este paso se llama replanificación. El planificador de rutas realiza un número limitado de intentos para encontrar esta ruta. Si no puede encontrar la ruta dentro de un número específico de intentos, pasa a utilizar un planificador de rutas clásico. El planificador de rutas MPNet determina la cantidad máxima de intentos que puede realizar antes de cambiar a un planificador clásico mediante la propiedad MaxNumLearnedStates.

  • En cada paso de planificación y replanificación, el planificador aplica el enfoque de contracción de estados perezosos (LSC) para eliminar los estados redundantes calculados por el planificador. Esto genera una menor complejidad computacional y ayuda al planificador a encontrar la ruta más corta sin colisiones entre el estado inicial real y el estado objetivo.

Plot that shows the computed collision-free path between a start point and a goal pint on a 2-D map. The plot aslo demonstrates beacon states, learned states computed using MPNet path planner, and classical states computed using a classical planner. The plot shows two instances when the planner had to do replanning. In the first instance, the MPNet path planner successfully finds a collision-free path between two beacon states during replanning. In the second instance, the MPNet path planner is not able to find collision-free path between two beacon states. Hence, it switches to a classical planner for replanning.

Referencias

[1] Qureshi, Ahmed Hussain, Yinglong Miao, Anthony Simeonov, and Michael C. Yip. “Motion Planning Networks: Bridging the Gap Between Learning-Based and Classical Motion Planners.” IEEE Transactions on Robotics 37, no. 1 (February 2021): 48–66. https://doi.org/10.1109/TRO.2020.3006716.

Capacidades ampliadas

expandir todo

Historial de versiones

Introducido en R2024a