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

pathmetrics

Información para métricas de ruta

Desde R2019b

Descripción

El objeto pathmetrics contiene información para calcular las métricas de ruta. Utilice funciones de objetos para calcular la suavidad, el espacio libre y la validez de la ruta en función de un conjunto de poses y el entorno del mapa asociado.

Creación

Sintaxis

pathMetricsObj = pathmetrics(path)

pathMetricsObj = pathmetrics(path,validator)

Descripción

pathMetricsObj = pathmetrics(path) crea un objeto basado en la entrada navPath objeto. Se supone que el validador de estado es un objeto validatorOccupancyMap , si el espacio de estado del objeto navPath es un stateSpaceSE2, stateSpaceDubins o stateSpaceReedsShepp objeto. De lo contrario, se supone que el validador de estado es un objeto validatorOccupancyMap3D , si el espacio de estado del objeto navPath es un objeto stateSpaceSE3 . La entrada path establece el valor de la propiedad Path .

pathMetricsObj = pathmetrics(path,validator) crea un objeto basado en el objeto de entrada navPath y el validador de estado asociado para verificar la validez de la ruta. La entrada validator establece el valor de la propiedad StateValidator .

Propiedades

Path — Estructura de datos de ruta

navPath objeto

La estructura de datos de la ruta, especificada como un objeto navPath, es la ruta cuya métrica se va a calcular.

StateValidator — Validador de estados en ruta

validatorOccupancyMap objeto | validatorVehicleCostmap objeto | validatorOccupancyMap3D objeto

Validador de estados en la ruta, especificado como objeto validatorOccupancyMap, validatorVehicleCostmap o validatorOccupancyMap3D .

Para espacios de estado 2-D, el validador de estado predeterminado es validatorOccupancyMap(stateSpaceSE2,binaryOccupancyMap(10)).

Para espacios de estado 3-D, el validador de estado predeterminado es validatorOccupancyMap3D(stateSpaceSE3).

Funciones del objeto

clearance	Distancia mínima del camino
isPathValid	Determinar si la ruta planificada está libre de obstáculos.
show	Visualice métricas de ruta en el entorno del mapa
smoothness	Suavidad de la ruta

Ejemplos

Métricas de ruta de cálculo

Calcule la suavidad, el espacio libre y la validez de una ruta planificada en función de un conjunto de poses y el entorno del mapa asociado.

Cargar y asignar mapa al validador de estado

Cree un mapa de ocupación a partir de un mapa de ejemplo y establezca la resolución del mapa.

load exampleMaps.mat; % simpleMap
mapResolution = 1; % cells/meter
map = occupancyMap(simpleMap,mapResolution);

Crea un espacio de estados de Dubins.

statespace = stateSpaceDubins;

Cree un validador de estado basado en el mapa de ocupación para almacenar los parámetros y estados en el espacio de estados de Dubins.

statevalidator = validatorOccupancyMap(statespace);

Asigne el mapa al validador.

statevalidator.Map = map;

Establezca la distancia de validación para el validador.

statevalidator.ValidationDistance = 0.01;

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

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

Planificar ruta

Cree un planificador de rutas RRT* y permita una mayor optimización.

planner = plannerRRTStar(statespace,statevalidator);
planner.ContinueAfterGoalReached = true;

Reduzca el número máximo de iteraciones y aumente la distancia máxima de conexión.

planner.MaxIterations = 2500;
planner.MaxConnectionDistance = 0.3;

Defina los estados de inicio y objetivo para el planificador de ruta como vectores [x, y, theta] . x e y son las coordenadas cartesianas y theta es el ángulo de orientación.

start = [2.5, 2.5, 0]; % [meters, meters, radians]
goal = [22.5, 8.75, 0];

Planifique una ruta desde el estado inicial hasta el estado objetivo. La función de plan devuelve un objeto navPath .

rng(100,'twister') % repeatable result
[path,solutionInfo] = plan(planner,start,goal);

Calcular y visualizar métricas de ruta

Cree un objeto de métricas de ruta.

pathMetricsObj = pathmetrics(path,statevalidator);

Verifique la validez de la ruta. El resultado es 1 (true) si la ruta planificada está libre de obstáculos. 0 (false) indica una ruta no válida.

isPathValid(pathMetricsObj)

ans = logical
   1

Calcule la distancia mínima de la ruta.

clearance(pathMetricsObj)

ans = 1.4142

Evaluar la suavidad de la ruta. Los valores cercanos a 0 indican una ruta más fluida. Las rutas en línea recta devuelven un valor de 0.

smoothness(pathMetricsObj)

ans = 1.7318

Visualice el espacio libre mínimo de la ruta.

show(pathMetricsObj)
legend('Planned Path','Minimum Clearance')

Planificación de rutas de vehículos y cálculo de métricas en un entorno de mapas de costes 2-D

Planifique una ruta de vehículo a través de un estacionamiento utilizando el algoritmo RRT*. Calcule y visualice la suavidad, el espacio libre y la validez de la ruta planificada.

Cargar y asignar mapa al validador de estado

Cargar un mapa de costes de un estacionamiento. Traza el mapa de costes para ver el estacionamiento y las áreas infladas que el vehículo debe evitar.

load parkingLotCostmap.mat;
costmap = parkingLotCostmap;
plot(costmap)
xlabel('X (meters)')
ylabel('Y (meters)')

Cree un objeto stateSpaceDubins y aumente el radio de Turing mínimo a 4 metros.

statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % meters

Cree un objeto validatorVehicleCostmap utilizando el espacio de estado creado.

statevalidator = validatorVehicleCostmap(statespace);

Asigne el mapa de costes del estacionamiento al objeto del validador de estado.

statevalidator.Map = costmap;

Planificar ruta

Defina las poses de inicio y objetivo para el vehículo como vectores [x, y, Θ]. Las unidades mundiales para las ubicaciones (x, y) están en metros. Las unidades mundiales para los ángulos de orientación Θ están en grados.

startPose = [5, 5, 90]; % [meters, meters, degrees]
goalPose = [40, 38, 180]; % [meters, meters, degrees]

Utilice un objeto pathPlannerRRT (Automated Driving Toolbox) y la función plan (Automated Driving Toolbox) para planificar la trayectoria del vehículo desde la pose inicial hasta la pose objetivo.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

Interpolar a lo largo de la ruta cada metro. Convierta los ángulos de orientación de grados a radianes.

poses = zeros(size(refPath.PathSegments,2)+1,3);
poses(1,:) = refPath.StartPose;
for i = 1:size(refPath.PathSegments,2) 
    poses(i+1,:) = refPath.PathSegments(i).GoalPose; 
end
poses(:,3) = deg2rad(poses(:,3));

Cree un objeto navPath utilizando el objeto de espacio de estados Dubins y los estados especificados por poses.

path = navPath(statespace,poses);

Calcular y visualizar métricas de ruta

Cree un objeto pathmetrics .

pathMetricsObj = pathmetrics(path,statevalidator);

Verifique la validez de la ruta. El resultado es 1 (true) si la ruta planificada está libre de obstáculos. 0 (false) indica una ruta no válida.

isPathValid(pathMetricsObj)

ans = logical
   1

Calcula y visualiza el mínimo clearance de la ruta.

clearance(pathMetricsObj)

ans = 0.5000

show(pathMetricsObj)
legend('Inflated Areas','Planned Path','Minimum Clearance')
xlabel('X (meters)')
ylabel('Y (meters)')

Calcule y visualice el smoothness de la ruta. Los valores cercanos a 0 indican una ruta más fluida. Las rutas en línea recta devuelven un valor de 0.

smoothness(pathMetricsObj)

ans = 0.0842

show(pathMetricsObj,'Metrics',{'Smoothness'})
legend('Inflated Areas','Path Smoothness')
xlabel('X (meters)')
ylabel('Y (meters)')

Visualice el espacio libre para cada estado de la ruta.

show(pathMetricsObj,'Metrics',{'StatesClearance'})
legend('Inflated Areas','Planned Path','Clearance of Path States')
xlabel('X (meters)')
ylabel('Y (meters)')

Planificación de rutas y cálculo de métricas en un entorno 3D

Planifique una ruta a través de una manzana de la ciudad utilizando el algoritmo RRT. Calcule y visualice la suavidad, el espacio libre y la validez de la ruta planificada.

Cargar y asignar mapa al validador de estado

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);

Planificar ruta

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 = [50 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);

Calcular y visualizar métricas de ruta

Cree un objeto de métricas de ruta.

pathMetricsObj = pathmetrics(pthObj,sv);

Verifique la validez de la ruta. El resultado es 1 (true) si la ruta planificada está libre de obstáculos. 0 (false) indica una ruta no válida.

isPathValid(pathMetricsObj)

ans = logical
   1

Calcule la distancia mínima de la ruta.

clearance(pathMetricsObj)

ans = 10

Evaluar la suavidad de la ruta. Los valores cercanos a 0 indican una ruta más fluida. Las rutas en línea recta devuelven un valor de 0.

smoothness(pathMetricsObj)

ans = 0.0011

Visualice el espacio libre mínimo de la ruta.

show(pathMetricsObj)
axis equal
view([100 75])
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)

Historial de versiones

Introducido en R2019b

Consulte también

Objetos

• navPath | validatorOccupancyMap | validatorVehicleCostmap | validatorOccupancyMap3D