pathmetrics
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
Descripción
crea un objeto basado en la entrada pathMetricsObj
= pathmetrics(path
)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 .
crea un objeto basado en el objeto de entrada pathMetricsObj
= pathmetrics(path
,validator
)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
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)