smoothness

Описание

пример

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

smoothness(pathMetricsObj,'Type','segments') возвращает отдельные вычисления гладкости между каждым набором трех положений на пути, в форме (n –2) - вектор элемента, где n является количеством положений.

Примеры

свернуть все

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

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

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

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

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

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

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

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

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

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

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

Figure contains an axes object. The axes object 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 object. The axes object 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 object. The axes object contains 29 objects of type image, patch, line. These objects represent Inflated Areas, Planned Path, Clearance of Path States.

Входные параметры

свернуть все

Информация для метрик пути в виде pathmetrics объект.

Ссылки

[1] Линдеманн, Стивен Р. и Стивен М. Лэвалл. "Простые и эффективные алгоритмы для вычисления сглаженных, законов об обратной связи без коллизий по данным разложениям ячейки". Международный журнал Исследования Робототехники 28, № 5. 2009, стр 600-621.

Смотрите также

Объекты

Функции

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