Habilite la verificación de colisiones de vehículos para la planificación de rutas utilizando el híbrido A*
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 verificador de colisiones de vehículos
Cargue el archivo parkingLotCostVal.mat
en el espacio de trabajo de MATLAB® . El archivo MAT incluye un mapa de costes de estacionamiento, costVal
.
load parkingLotCostVal.mat
Representa el tamaño del vehículo utilizando un objeto vehicleDimensions
(Automated Driving Toolbox) con un largo y ancho de 5 metros y 3 metros, respectivamente. Luego, cree un objeto vehicleCostmap
(Automated Driving Toolbox) usando el mapa de costes 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")
Para evitar obstáculos y planificar rutas libres de colisiones a través del estacionamiento, infle los tamaños de los obstáculos usando 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
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")
Para validar los estados del vehículo con los obstáculos inflados, cree un objeto validatorVehicleCostmap
usando el mapa de costes 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 suelen tener un radio de giro en el rango de 4 a 7 metros, por lo tanto, especifique MinTurningRadius
como 4
. Especifique MotionPrimitiveLength
como 6
, ofreciendo 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);
Defina las poses de inicio y meta para el vehículo como vectores de la forma [x y theta], donde x e 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 usar 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)
Acérquese al vehículo con las estimaciones circulares, en la pose final. El vehicleCostmap
estima la geometría subyacente del vehículo lo suficientemente bien como para planificar una ruta libre de 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.