Поиск точек пересечения лучей и занятых ячеек карты
intersectionPts = rayIntersection(map,pose,angles,maxrange)map. Лучи, исходящие от указанного pose и angles. Точки пересечения возвращаются в мировой рамке координат. При отсутствии пересечения до указанного maxrange, [NaN NaN] возвращается.
Создайте двоичную сетку занятости. Добавьте препятствия и раздувайте их. Карта с более низким разрешением используется для иллюстрации важности размера ячеек сетки. Показать карту.
map = binaryOccupancyMap(10,10,2); obstacles = [4 10; 3 5; 7 7]; setOccupancy(map,obstacles,ones(length(obstacles),1)) inflate(map,0.25) show(map)
Найдите точки пересечения занятых ячеек и лучей, которые излучают из данной позы транспортного средства. Укажите максимальный диапазон и углы для этих лучей. Последний луч не пересекается с препятствием в пределах максимального диапазона, поэтому он не имеет точки столкновения.
maxrange = 6; angles = [pi/4,-pi/4,0,-pi/8]; vehiclePose = [4,4,pi/2]; intsectionPts = rayIntersection(map,vehiclePose,angles,maxrange)
intsectionPts = 4×2
3.5000 4.5000
6.0000 6.0000
4.0000 9.0000
NaN NaN
Постройте график точек пересечения и лучей из позы.
hold on plot(intsectionPts(:,1),intsectionPts(:,2),'*r') % Intersection points plot(vehiclePose(1),vehiclePose(2),'ob') % Vehicle pose for i = 1:3 plot([vehiclePose(1),intsectionPts(i,1)],... [vehiclePose(2),intsectionPts(i,2)],'-b') % Plot intersecting rays end plot([vehiclePose(1),vehiclePose(1)-6*sin(angles(4))],... [vehiclePose(2),vehiclePose(2)+6*cos(angles(4))],'-b') % No intersection ray
binaryOccupancyMap | occupancyMap (Панель инструментов навигации)