Проверяйте валидность запланированного пути к автомобилю
isValid = checkPathValidity(refPath,costmap)isValid = checkPathValidity(refPoses,costmap)isValid = checkPathValidity(refPath,costmap)refPath, против автомобиля costmap. Используйте эту функцию, чтобы протестировать, если путь допустим в изменяющей среде.
Путь допустим, если следующие условия верны:
Путь имеет по крайней мере одно положение.
Путь без коллизий и в рамках costmap.
Запланируйте путь к автомобилю через парковку при помощи оптимального быстро исследующего случайного дерева (RRT*) алгоритм. Проверяйте, что путь допустим, и затем постройте положения перехода вдоль пути.
Загрузите costmap парковки. Постройте costmap, чтобы видеть парковку и раздутые области для автомобиля, чтобы избежать.
data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)
Задайте запускаются и целевые положения для автомобиля как [x, y, Θ] векторы. Мировые модули для (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);
Постройте запланированный путь и положения перехода на costmap.
hold on plot(refPath,'DisplayName','Planned Path') scatter(transitionPoses(:,1),transitionPoses(:,2),[],'filled', ... 'DisplayName','Transition Poses') hold off
Чтобы проверять, допустим ли путь к автомобилю, функция checkPathValidity дискретизирует путь. Затем функция проверяет, что положения в дискретизированных точках без коллизий. Порог для положения без коллизий зависит от разрешения, в котором checkPathValidity дискретизирует.