Проверяйте автомобиль costmap на занятые положения или точки
Функция checkOccupied проверяет, заняты ли положения автомобиля или точки препятствиями на автомобиле costmap. Алгоритмы планирования пути используют checkOccupied, чтобы проверять, пригодны ли положения автомобиля кандидата вдоль пути для навигации.
Чтобы упростить проверку столкновения на положение автомобиля, vehicleCostmap раздувает препятствия согласно InflationRadius автомобиля, как задано свойством CollisionChecker costmap. Средство проверки столкновения вычисляет радиус инфляции путем включения автомобиля в набор перекрывающихся кругов радиуса R, где центры этих кругов простираются вдоль продольной оси автомобиля. Радиусом инфляции является минимальный R, должен был полностью заключить автомобиль в эти круги. Положение автомобиля без коллизий, когда ни один из центров этих кругов не лежит на расширенной ячейке сетки. Для получения дополнительной информации см. алгоритм на странице с описанием vehicleCostmap.
occ = checkOccupied(costmap,vehiclePoses)occ = checkOccupied(costmap,xyPoints)occMat = checkOccupied(costmap)Загрузите costmap с парковки.
data = load('parkingLotCostmap.mat');
parkMap = data.parkingLotCostmap;
plot(parkMap)Создайте положения автомобиля после прямолинейного пути. x и y (x, y) координаты задней оси автомобиля. theta является углом задней оси относительно оси X. Обратите внимание на то, что размерности автомобиля хранятся в свойстве vehicleDimensions costmap, и что существует смещение между задней осью автомобиля и его центром.
x = 6:0.25:10; y = repmat(5,size(x)); theta = zeros(size(x)); vehiclePoses = [x',y',theta']; hold on plot(x,y,'b.')
Проверяйте, заняты ли положения.
occ = checkOccupied(parkMap,vehiclePoses)
occ = 17x1 logical array
0
0
0
0
0
1
1
1
1
1
⋮
Положения автомобиля заняты, начавшись с шестого положения. Другими словами, центр автомобиля в шестом положении находится в радиусе инфляции занятой ячейки сетки.