Проверяйте автомобиль costmap на положения без коллизий или точки
Функция checkFree проверяет, свободны ли положения автомобиля или точки от препятствий на автомобиле costmap. Алгоритмы планирования пути используют checkFree, чтобы проверять, пригодны ли положения автомобиля кандидата вдоль пути для навигации.
Чтобы упростить проверку столкновения на положение автомобиля, vehicleCostmap раздувает препятствия согласно InflationRadius автомобиля, как задано свойством CollisionChecker costmap. Средство проверки столкновения вычисляет радиус инфляции путем включения автомобиля в набор перекрывающихся кругов радиуса R, где центры этих кругов простираются вдоль продольной оси автомобиля. Радиусом инфляции является минимальный R, должен был полностью заключить автомобиль в эти круги.
Положение автомобиля без коллизий, когда следующие условия применяются:
Ни один из центров круга автомобиля не лежит на расширенной ячейке сетки.
Величина затрат на каждого содержащего центр круга является меньше, чем FreeThreshold costmap.
Для получения дополнительной информации см. алгоритм на странице с описанием vehicleCostmap.
free = checkFree(costmap,vehiclePoses)free = checkFree(costmap,xyPoints)freeMat = checkFree(costmap)free = checkFree(costmap,vehiclePoses)
Загрузите costmap с парковки.
data = load('parkingLotCostmap.mat');
parkMap = data.parkingLotCostmap;
plot(parkMap)Создайте положения автомобиля после прямолинейного пути. x и y (x, y) координаты задней оси автомобиля. theta является углом задней оси относительно оси X. Обратите внимание на то, что размерности автомобиля хранятся в свойстве CollisionChecker.VehicleDimensions costmap, и что существует смещение между задней осью автомобиля и его центром.
x = 4:0.25:6; y = 3:0.25:5; theta = repmat(45,size(x)); vehiclePoses = [x',y',theta']; hold on plot(x,y,'b.') hold off
Несколько первых (x, y) координаты задней оси в расширенной области. Однако это не подразумевает столкновение, потому что центр автомобиля может быть вне расширенной области. Проверяйте, без коллизий ли положения.
free = checkFree(parkMap,vehiclePoses)
free = 9x1 logical array
1
1
1
1
1
1
1
1
1
Все значения free 1 (true), таким образом, все положения без коллизий. Центр автомобиля не вводит расширенную область ни в каком положении.
Если вы задаете маленькое значение InflationRadius, который не полностью заключает автомобиль, то checkFree может сообщить занятый, изображает из себя без коллизий. Чтобы избежать этой ситуации, значение по умолчанию InflationRadius полностью заключает автомобиль.