Загрузить и назначить карту средству проверки состояния

Создайте карту занятости из примера карты и задайте разрешение карты.

load exampleMaps.mat; % simpleMap
mapResolution = 1; % cells/meter
map = occupancyMap(simpleMap,mapResolution);

Создание пространства состояния Дубиньша.

statespace = stateSpaceDubins;

Создайте средство проверки состояния на основе карты занятости для хранения параметров и состояний в пространстве состояния Дубиньша.

statevalidator = validatorOccupancyMap(statespace);

Назначьте карту средству проверки.

statevalidator.Map = map;

Задайте расстояние проверки для средства проверки.

statevalidator.ValidationDistance = 0.01;

Обновите границы пространства состояний так, чтобы они совпадали с границами карты.

statespace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

Путь к плану

Создайте планировщик пути RRT * и выполните дальнейшую оптимизацию.

planner = plannerRRTStar(statespace,statevalidator);
planner.ContinueAfterGoalReached = true;

Уменьшите максимальное число итераций и увеличьте максимальное расстояние соединения.

planner.MaxIterations = 2500;
planner.MaxConnectionDistance = 0.3;

Определите начальное и целевое состояния для плановика пути как [x, y, theta] векторы. x и y - декартовы координаты, а theta - угол ориентации.

start = [2.5, 2.5, 0]; % [meters, meters, radians]
goal = [22.5, 8.75, 0];

Запланируйте путь от начального состояния к целевому состоянию. Функция плана возвращает navPath объект.

rng(100,'twister') % repeatable result
[path,solutionInfo] = plan(planner,start,goal);

Вычислить и визуализировать метрики пути

Создайте объект метрик пути.

pathMetricsObj = pathmetrics(path,statevalidator);

Проверьте правильность пути. Результат: 1 (true) если запланированный путь свободен от препятствий. 0 (false) указывает недопустимый путь.

isPathValid(pathMetricsObj)

ans = logical
   1

Рассчитайте минимальный зазор траектории.

clearance(pathMetricsObj)

ans = 1.4142

Оцените гладкость траектории. Значения, близкие к 0 укажите более гладкий путь. Прямолинейные контуры возвращают значение 0.

smoothness(pathMetricsObj)

ans = 1.7318

Визуализируйте минимальный зазор траектории.

show(pathMetricsObj)
legend('Planned Path','Minimum Clearance')

Загрузить и назначить карту средству проверки состояния

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

load parkingLotCostmap.mat;
costmap = parkingLotCostmap;
plot(costmap)
xlabel('X (meters)')
ylabel('Y (meters)')

Создать stateSpaceDubins объект и увеличить минимальный радиус затягивания до 4 метров.

statespace = stateSpaceDubins;
statespace.MinTurningRadius = 4; % meters

Создать validatorVehicleCostmap с использованием созданного пространства состояний.

statevalidator = validatorVehicleCostmap(statespace);

Присвойте карту затрат парковки объекту проверки состояния.

statevalidator.Map = costmap;

Путь к плану

Определите стартовые и целевые позы для транспортного средства как [x, y, Θ] векторы. Мировые подразделения для (x,y) места в метрах. Мировые единицы измерения для углов ориентации Θ в градусах.

startPose = [5, 5, 90]; % [meters, meters, degrees]
goalPose = [40, 38, 180]; % [meters, meters, degrees]

Использовать pathPlannerRRT (Automated Driving Toolbox) объект и plan Функция (Automated Driving Toolbox) позволяет планировать путь движения транспортного средства от начальной позы до целевой позы.

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

Интерполировать вдоль пути на каждом метре. Преобразуйте углы ориентации из градусов в радианы.

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));

Создать navPath с использованием государственного космического объекта Дубиньша и состояний, указанных poses.

path = navPath(statespace,poses);

Вычислить и визуализировать метрики пути

Создать pathmetrics объект.

pathMetricsObj = pathmetrics(path,statevalidator);

Проверьте правильность пути. Результат: 1 (true), если запланированный путь свободен от препятствий. 0 (false) указывает недопустимый путь.

isPathValid(pathMetricsObj)

ans = logical
   1

Вычислите и визуализируйте минимум clearance пути.

clearance(pathMetricsObj)

ans = 0.5000

show(pathMetricsObj)
legend('Inflated Areas','Planned Path','Minimum Clearance')
xlabel('X (meters)')
ylabel('Y (meters)')

Вычислите и визуализируйте smoothness пути. Значения, близкие к 0 укажите более гладкий путь. Прямолинейные контуры возвращают значение 0.

smoothness(pathMetricsObj)

ans = 0.0842

show(pathMetricsObj,'Metrics',{'Smoothness'})
legend('Inflated Areas','Path Smoothness')
xlabel('X (meters)')
ylabel('Y (meters)')

Визуализируйте зазор для каждого состояния траектории.

show(pathMetricsObj,'Metrics',{'StatesClearance'})
legend('Inflated Areas','Planned Path','Clearance of Path States')
xlabel('X (meters)')
ylabel('Y (meters)')