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.

plan

Encuentra un camino sin obstáculos entre dos estados

Desde R2024a

    Descripción

    path = plan(planner,start,goal) calcula una ruta sin obstáculos entre los estados inicial y final utilizando el planificador de rutas MPNet (Motion Planning Networks).

    ejemplo

    [path,solutionInfo] = plan(planner,start,goal) también devuelve información adicional sobre los estados en la ruta calculada como una estructura.

    ejemplo

    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.

    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.

    Argumentos de entrada

    contraer todo

    Planificador de rutas MPNet, especificado como un objeto plannerMPNET.

    Estado inicial, especificado como un vector de tres elementos con la forma [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes.

    Tipos de datos: single | double

    Estado objetivo, especificado como un vector de tres elementos con la forma [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes.

    Tipos de datos: single | double

    Argumentos de salida

    contraer todo

    Camino sin obstáculos, devuelto como un objeto navPath.

    Información sobre la ruta y los estados en la ruta calculada, devuelta como una estructura con los siguientes campos:

    Campos de solutionInfo

    CamposDescripción
    IsPathFoundIndica si se encuentra una ruta. El valor de este campo se devuelve como 1, si se encuentra una ruta. De lo contrario, el valor devuelto es 0.
    LearnedStatesContiene estados calculados mediante planificación de ruta neuronal bidireccional. Este campo se devuelve como una matriz M por 3. Cada fila de la matriz tiene el formato [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes. M denota la cantidad de estados calculados utilizando la planificación de rutas neuronales. Si no se calcularon estados, la función devuelve un valor vacío para este campo.
    BeaconStatesContiene estados consecutivos en el espacio libre que no fueron conectables debido a colisiones con un obstáculo. Este campo se devuelve como una matriz P por 3. Cada fila de la matriz tiene el formato [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes. Si no hubiera estados de baliza, la función devuelve un valor vacío para este campo. Para obtener información sobre los estados de las balizas, consulte la sección Algoritmos.
    ClassicalStatesContiene estados calculados utilizando la planificación de ruta clásica. Este campo se devuelve como una matriz N por 3. Cada fila de la matriz tiene el formato [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes. N denota el número de estados calculados utilizando la planificación de ruta clásica. Si no se calcularon estados, la función devuelve un valor vacío para este campo.

    Algoritmos

    La función plan implementa el algoritmo de planificación de rutas 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

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

    Historial de versiones

    Introducido en R2024a