Запланируйте путь к автомобилю к узкому месту для парковки при помощи оптимизированного быстро исследующего случайного дерева (RRT*) алгоритм. Попробуйте различные проверяющие столкновение настройки в costmap, используемом RRT* планировщик пути.

Загрузите и отобразите costmap парковки. costmap является объектом vehicleCostmap. По умолчанию vehicleCostmap использует проверяющую столкновение настройку, которая раздувает препятствия на основе радиуса только одного круга, заключающего автомобиль. costmap сверхраздувает препятствия (контуры места для парковки).

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;

figure
plot(costmap)
title('Collision Checking with One Circle')

Используйте inflationCollisionChecker, чтобы создать новую проверяющую столкновение настройку для costmap.

Задайте проверяющую столкновение настройку в свойстве CollisionChecker costmap.

vehicleDims = vehicleDimensions(4.5,1.7);  % 4.5 m long, 1.7 m wide
numCircles = 3;
ccConfig = inflationCollisionChecker(vehicleDims,numCircles);
costmap.CollisionChecker = ccConfig;

Отобразите costmap с новой проверяющей столкновение настройкой. Расширенные области уменьшаются.

figure
plot(costmap)
title('Collision Checking with Three Circles')

Задайте проблему планирования: автомобиль запускается около левого входа парковки и концов в месте для парковки.

startPose = [11 10 0];   % [meters, meters, degrees]
goalPose = [31.5 17 90]; 

Используйте объект pathPlannerRRT запланировать путь к месту для парковки. Постройте запланированный путь.

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

hold on
plot(refPath)
hold off

Создайте проверяющую столкновение настройку для costmap. Вручную задайте центры круга так, чтобы они полностью заключили автомобиль.

Задайте размерности автомобиля при помощи объекта vehicleDimensions.

length = 5; % meters
width = 2; % meters
vehicleDims = vehicleDimensions(length,width);

Задайте три центра круга и радиус инфляции, чтобы использовать для проверки столкновения. Разместите один центр в средней точке автомобиля. Сместите другие два центра равной суммой на любом конце автомобиля.

distFromSide = 0.175;
centerPlacements = [distFromSide 0.5 1-distFromSide];
inflationRadius = 1.2;

Создайте и отобразите проверяющую столкновение настройку.

ccConfig = inflationCollisionChecker(vehicleDims, ...
    'CenterPlacements',centerPlacements,'InflationRadius',inflationRadius);

figure
plot(ccConfig)

В этой настройке углы автомобиля не заключены в кругах. Чтобы полностью заключить автомобиль, увеличьте радиус инфляции. Отобразите обновленную настройку.

ccConfig.InflationRadius = 1.3;
plot(ccConfig)

Используйте эту проверяющую столкновение настройку, чтобы создать 10 20 метр costmap.

costmap = vehicleCostmap(10,20,0.1,'CollisionChecker',ccConfig);