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

shortenpath

Acortar la ruta eliminando nodos redundantes

Desde R2024b

Sintaxis

outputPath = shortenpath(inputPath,sv)

Descripción

outputPath = shortenpath(inputPath,sv) acorta la ruta entre una pose inicial y una pose final eliminando nodos redundantes a lo largo de la ruta. La función utiliza el validador de estado de entrada sv para validar la ruta acortada devuelta en la salida. Eliminar nodos redundantes no siempre reduce la longitud de la ruta.

Ejemplos

Acortar el camino de Dubins

Establezca la semilla aleatoria para garantizar la repetibilidad de los resultados.

rng(100,"twister")

Crear un objeto de espacio de estados de Dubins. Establezca el radio de giro mínimo para el vehículo Dubins en 0,2.

ss = stateSpaceDubins;
ss.MinTurningRadius = 0.2;

Cargue una matriz binaria que especifique una cuadrícula de ocupación.

load("exampleMaps.mat","simpleMap")

Cree un mapa de ocupación 2-D a partir de la cuadrícula y configure la resolución del mapa en 10 celdas/metro.

map = occupancyMap(simpleMap,10);

Cree un validador de estado utilizando el espacio de estado de Dubins. Especifique el mapa y la distancia requerida para validar los segmentos de ruta dentro del mapa.

sv = validatorOccupancyMap(ss);
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 un planificador de ruta RRT y establezca la distancia de conexión máxima en 0,3. Especifique una función de objetivo personalizada que determine que una ruta llega al objetivo si la distancia euclidiana al objetivo está por debajo de un umbral de 0,1 metros.

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

planner = 
  plannerRRT with properties:

               StateSpace: [1×1 stateSpaceDubins]
           StateValidator: [1×1 validatorOccupancyMap]
             StateSampler: [1×1 stateSamplerUniform]
          MaxNumTreeNodes: 10000
            MaxIterations: 10000
    MaxConnectionDistance: 0.3000
           GoalReachedFcn: @(~,s,g)(norm(s(1:3)-g(1:3))<0.1)
                 GoalBias: 0.0500

Establezca los estados de inicio y objetivo.

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

Calcula la ruta entre la pose inicial y la pose objetivo utilizando el planificador de ruta RRT. Tenga en cuenta el número de estados en la ruta calculada.

inputPath = plan(planner,start,goal)

inputPath = 
  navPath with properties:

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

Elimine los nodos redundantes a lo largo de la ruta calculada utilizando la función shortenpath. Tenga en cuenta el número de estados en la ruta de salida.

outputPath = shortenpath(inputPath,sv)

outputPath = 
  navPath with properties:

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

Calcular las longitudes de la ruta original y la ruta acortada.

lenPath = pathLength(inputPath)

lenPath = 
4.3670

lenShortened = pathLength(outputPath)

lenShortened = 
3.7560

Tenga en cuenta que la eliminación de nodos redundantes ha disminuido la longitud de la ruta.

Si desea aumentar la resolución de la ruta, puede realizar una interpolación para obtener la cantidad deseada de estados. Aumente el número de estados en la ruta acortada a 50 y calcule la longitud de la ruta.

interpolatedPath = copy(outputPath);
interpolate(interpolatedPath,50)
lenInterpolated = pathLength(interpolatedPath)

lenInterpolated = 
3.7560

Grafique la ruta original, la ruta acortada y la ruta interpolada.

show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
legend(Location="bestoutside")
line(inputPath.States(:,1),inputPath.States(:,2),LineStyle="-",LineWidth=1.5,Color="red",Marker="o")
line(outputPath.States(:,1),outputPath.States(:,2),LineStyle="-",LineWidth=1.5,Color="blue",Marker="o")
line(interpolatedPath.States(:,1),interpolatedPath.States(:,2),LineStyle="--",LineWidth=0.5,Color="green",Marker="o")
legend("Start","Goal","Original Path","Shortened Path","Interpolated Path")
title("Path Comparison")
drawnow
hold off

Acortar ruta en el entorno del mapa de ocupación 3D

Establezca la semilla aleatoria para garantizar la repetibilidad de los resultados.

rng(1,"twister")

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");
map = mapData.omap;
map.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(map,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=map,ValidationDistance=0.1);

Crear un planificador de ruta RRT.

planner = plannerRRT(ss,sv);

Especifica las poses de inicio y de destino.

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

Planifique la ruta.

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

Acortar el camino.

outputPath = shortenpath(inputPath,sv);

Visualice la ruta original y la ruta acortada.

show(map)
axis equal
view([30 55])
hold on
s1 = scatter3(start(1,1),start(1,2),start(1,3),[],"g","filled",DisplayName="Start");
s2 = scatter3(goal(1,1),goal(1,2),goal(1,3),[],"m","filled",DisplayName="Goal");
p1 = plot3(inputPath.States(:,1),inputPath.States(:,2),inputPath.States(:,3),"r-",LineWidth=2,DisplayName="Original Path");
p2 = plot3(outputPath.States(:,1),outputPath.States(:,2),outputPath.States(:,3),"y-",LineWidth=2,DisplayName="Shortened Path");
legend([s1 s2 p1 p2],Location="bestoutside")
title("Path Comparison")

Argumentos de entrada

inputPath — Ruta de entrada

objeto navPath

Ruta de entrada, especificada como un objeto navPath. La ruta de entrada debe contener estados válidos. Puede utilizar cualquiera de los planificadores de rutas para obtener la ruta de entrada. Sin embargo, si la ruta de entrada ya es óptima, es posible que la función no la acorte significativamente.

sv — Validador de estado

objeto validatorOccupancyMap | objeto validatorVehicleCostmap | objeto validatorOccupancyMap3D | objeto de una subclase de nav.StateValidator

Validador de estado, especificado como un objeto validatorOccupancyMap, un objeto validatorVehicleCostmap, un objeto validatorOccupancyMap3D o un objeto de una subclase de la clase nav.StateValidator. El espacio de estado asociado con el validador de estado y el espacio de estado utilizado para calcular la ruta de entrada deben ser el mismo. La configuración del validador de estado determina si la función devolverá una ruta libre de colisiones.

Argumentos de salida

outputPath — Camino acortado

objeto navPath

Ruta acortada, devuelta como un objeto navPath.

Historial de versiones

Introducido en R2024b

Consulte también

navPath | isPathValid | pathLength | smoothness