smoothness
Suavidad de la ruta
Descripción
smoothness( evalúa la suavidad de la ruta planificada. Los valores cercanos a pathMetricsObj)0 indican una ruta más suave. Las rutas en línea recta devuelven un valor de 0.
smoothness( devuelve cálculos de suavidad individuales entre cada conjunto de tres poses en la ruta, en forma de un vector de (n –2) elementos, donde n es el número de poses.pathMetricsObj,'Type','segments')
Ejemplos
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 un 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;
Define los estados de inicio y destino para el planificador de rutas como vectores [x, y, theta]. x y 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 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 suave. 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')
![Figure contains an axes object. The axes object with xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, patch. These objects represent Planned Path, Minimum Clearance.](../../examples/nav/win64/ComputePathMetricsExample_01.png)
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 un 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)')
Crea un objeto stateSpaceDubins y aumenta el radio de Turing mínimo a 4 metros.
statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % metersCrea 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
Define las posiciones de inicio y destino del 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 ruta del vehículo desde la posición inicial hasta la posición 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 de Dubins y los estados especificados por poses.
path = navPath(statespace,poses);
Calcular y visualizar métricas de ruta
Crea 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
Calcular y visualizar 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)')
Calcular y visualizar el smoothness de la ruta. Los valores cercanos a 0 indican una ruta más suave. 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)')
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 suave. 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)
Argumentos de entrada
Referencias
[1] Lindemann, Stephen R., and Steven M. LaValle. "Simple and efficient algorithms for computing smooth, collision-free feedback laws over given cell decompositions." The International Journal of Robotics Research 28, no. 5. 2009, pp. 600-621.
Historial de versiones
Introducido en R2019b
