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

Habilite la verificación de colisiones de vehículos para la planificación de rutas utilizando el híbrido A*

Abrir script en vivo

Este ejemplo muestra cómo utilizar un planificador híbrido A* para planificar una ruta hacia un espacio de estacionamiento estrecho, teniendo en cuenta la forma de un robot similar a un automóvil.

Configurar el mapa y el detector de colisiones de vehículos

Cargue el archivo parkingLotCostVal.mat en el espacio de trabajo MATLAB ®. El archivo MAT incluye un mapa de costos de estacionamiento, costVal.

load parkingLotCostVal.mat

Representa el tamaño del vehículo utilizando un objeto vehicleDimensions (Automated Driving Toolbox) con largo y ancho de 5 metros y 3 metros, respectivamente. Luego, cree un objeto vehicleCostmap (Automated Driving Toolbox) utilizando el mapa de costos del estacionamiento, para representar el espacio de planificación alrededor del vehículo. Trazar el mapa de costes del vehículo.

vehicleDims = vehicleDimensions(5,3);
map = vehicleCostmap(costVal);
plot(map,Inflation="off")
title("Costmap of the parking lot")

Figure contains an axes object. The axes object with title Costmap of the parking lot, xlabel X, ylabel Y contains an object of type image.

Para evitar obstáculos y planificar rutas sin colisiones a través del estacionamiento, aumente el tamaño de los obstáculos utilizando un objeto inflationCollisionChecker (Automated Driving Toolbox). Crea el objeto inflationCollisionChecker que utiliza tres círculos para aproximar las dimensiones del vehículo.

ccConfig = inflationCollisionChecker(vehicleDims,3);

El mapa de costes del vehículo infla los tamaños de los obstáculos según las dimensiones del vehículo y la cantidad de círculos utilizados para estimar el vehículo. Visualice la configuración de verificación de colisiones del vehículo para ver cómo se aproxima a las dimensiones.

plot(ccConfig) % display the collision-checking configuration

Figure contains an axes object. The axes object with xlabel Longitudinal vehicle axis, ylabel Lateral vehicle axis contains 20 objects of type line, polygon, scatter, text. These objects represent Circle, Circle centers.

Asigne la verificación de colisión al objeto del mapa y muestre el mapa de costes inflado del estacionamiento.

map.CollisionChecker = ccConfig;
plot(map)
title("Inflated costmap of the parking lot")

Figure contains an axes object. The axes object with title Inflated costmap of the parking lot, xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

Para validar los estados del vehículo con los obstáculos inflados, cree un objeto validatorVehicleCostmap utilizando el mapa de costos del vehículo.

validator = validatorVehicleCostmap(stateSpaceSE2,Map=map);

Planificar y visualizar la ruta

Inicialice el objeto plannerHybridAStar utilizando el objeto validador de estado. Especifique el radio de giro mínimo y la longitud primitiva de movimiento utilizando las propiedades MinTurningRadius y MotionPrimitiveLength del planificador, respectivamente. Los vehículos del mundo real normalmente tienen un radio de giro de entre 4 y 7 metros, así que especifique MinTurningRadius como 4. Especifique MotionPrimitiveLength como 6, lo que proporciona un equilibrio decente entre maniobrabilidad y eficiencia de búsqueda, pero tenga en cuenta que otros valores pueden ser mejores para diferentes entornos.

planner = plannerHybridAStar(validator,MinTurningRadius=4,MotionPrimitiveLength=6);

Define las posiciones de inicio y destino del vehículo como vectores con la forma [x y theta], donde x y y especifican la posición en metros y theta especifica el ángulo de orientación en radianes. La pose inicial se encuentra cerca de la entrada del estacionamiento y la pose objetivo es el espacio de estacionamiento deseado.

startPose = [6 10 pi/2]; % [meters, meters, radians]
goalPose = [90 57 -pi/2];

Planifica una ruta desde la pose inicial hasta la pose objetivo. Para generar una ruta más suave, puede utilizar la función optimizePath. Para obtener más información, consulte Suavizado de ruta basado en optimización para vehículos autónomos.

refpath = plan(planner,startPose,goalPose);

Visualice la ruta con el modelo del vehículo.

vehiclePlotInterval = 10; % Draw a vehicle model every 10 states
exampleHelperVisualizePathWithVehicleModel(planner,refpath,ccConfig,vehiclePlotInterval)

Figure contains an axes object. The axes object with title Plan Collision-Free Path for Vehicles With Hybrid A* Planner, xlabel X, ylabel Y contains 25 objects of type image, patch, line, scatter, polygon. These objects represent Inflated Areas, Reverse Motion Primitives, Forward Motion Primitives, Forward Path, Path Points, Orientation, Start, Goal.

Acérquese al vehículo con las estimaciones circulares, en la pose final. vehicleCostmap estima la geometría subyacente del vehículo lo suficientemente bien como para planificar una ruta sin colisiones hasta la posición objetivo. Tenga en cuenta que, aunque las estimaciones de los círculos se superponen con el área inflada, el vehículo está libre de colisiones siempre que los centros de los círculos no estén en el área inflada.

Tenga en cuenta que, para un vehículo con un ancho o largo mucho mayor, como un camión grande, el radio de las estimaciones del círculo también es mucho mayor. Esto significa que el área inflada también aumenta y es posible que el planificador no pueda encontrar una solución viable. Para planificar una ruta con precisión para su vehículo, debe configurar el verificador de colisiones de vehículos correctamente para las dimensiones de su vehículo al habilitar esta función.