pathmetrics

Информация для метрик пути

Описание

The pathmetrics объект содержит информацию для вычисления метрик пути. Используйте функции объекта для вычисления гладкости, зазора и валидности пути на основе набора положений и связанного окружения карты.

Создание

Описание

pathMetricsObj = pathmetrics(path) создает объект на основе входа navPath объект. Валидатор состояния принят как validatorOccupancyMap объект. The path вход устанавливает значение свойства Path.

пример

pathMetricsObj = pathmetrics(path,validator) создает объект на основе входа navPath объект и связанный с ним агент проверки состояния для проверки валидности пути. The validator вход устанавливает значение свойства StateValidator.

Свойства

расширить все

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

Валидатор для состояний в пути, заданный как validatorOccupancyMap или validatorVehicleCostmap объект.

Функции объекта

clearanceМинимальный зазор пути
isPathValidОпределите, является ли запланированный путь свободным от препятствий
showВизуализируйте метрики пути в среде map
smoothnessПлавность пути

Примеры

свернуть все

Вычислите гладкость, клиренс и валидность запланированного пути на основе набора положений и связанного окружения карты.

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

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

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

Figure contains an axes. The axes contains 5 objects of type image, patch, line. These objects represent Planned Path, Minimum Clearance.

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

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

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

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

Figure contains an axes. The axes contains 2 objects of type image, patch. This object represents Inflated Areas.

Создайте 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)')

Figure contains an axes. The axes contains 6 objects of type image, patch, line. These objects represent Inflated Areas, Planned Path, Minimum Clearance.

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

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

Figure contains an axes. The axes contains 3 objects of type image, patch. These objects represent Inflated Areas, Path Smoothness.

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

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

Figure contains an axes. The axes contains 29 objects of type image, patch, line. These objects represent Inflated Areas, Planned Path, Clearance of Path States.

Введенный в R2019b
Для просмотра документации необходимо авторизоваться на сайте