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

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

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

Создайте пространство состояний Dubins.

statespace = stateSpaceDubins;

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

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

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

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

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

Присвойте парковку costmap объекту блока проверки допустимости состояния.

statevalidator.Map = costmap;

Запланируйте путь

Задайте запускаются и целевые положения для транспортного средства как [xY, ΘВекторы. Мировые единицы измерения для (xY) местоположения исчисляются в метрах. Мировые единицы измерения для углов ориентации Θ в градусах.

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 объект с помощью пространства состояний Dubins возражает и состояния, заданные poses.

path = navPath(statespace,poses);

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

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

pathMetricsObj = pathmetrics(path,statevalidator);

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

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