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

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

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

Планируйте путь от начального состояния до состояния цели. Функция plan возвращает 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)')