Проверка достоверности планируемого пути транспортного средства
isValid = checkPathValidity(refPath,costmap)refPath, против карты расходов на транспортное средство. Эта функция используется для проверки допустимости тракта в изменяющейся среде.
Путь допустим, если выполняются следующие условия:
Путь имеет по меньшей мере одну позу.
Путь свободен от столкновений и находится в пределах costmap.
Планирование пути транспортного средства через парковку с использованием оптимального алгоритма быстрого исследования случайного дерева (RRT *). Проверьте правильность пути, а затем постройте график позиций перехода вдоль пути.
Загрузить карту стоимости парковки. Постройте график расходов, чтобы увидеть стоянку и раздутые зоны для транспортного средства, чтобы избежать.
data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)
Определите начальные и целевые позы для транспортного средства как векторы [x, y, Start]. Мировые единицы измерения для местоположений (x, y) находятся в метрах. Всемирные единицы измерения для углов ориентации, выраженные в градусах.
startPose = [4, 4, 90]; % [meters, meters, degrees]
goalPose = [30, 13, 0];Использовать pathPlannerRRT объект для планирования пути от начальной позы до целевой позы.
planner = pathPlannerRRT(costmap); refPath = plan(planner,startPose,goalPose);
Проверьте правильность пути.
isPathValid = checkPathValidity(refPath,costmap)
isPathValid = logical
1
Интерполировать переходные позы вдоль пути.
transitionPoses = interpolate(refPath);
Постройте запланированный путь и переходные позы на карте затрат.
hold on plot(refPath,'DisplayName','Planned Path') scatter(transitionPoses(:,1),transitionPoses(:,2),[],'filled', ... 'DisplayName','Transition Poses') hold off
Чтобы проверить правильность пути транспортного средства, checkPathValidity функция дискретизирует путь. Затем функция проверяет, что позы в дискретизированных точках свободны от коллизий. Порог для свободного от столкновений положения зависит от разрешения, при котором checkPathValidity дискретизирует.