occupancyMap3D объект сохраняет препятствия в 3-D пространстве, используя наблюдения датчиков для отображения среды. Создайте карту и добавьте точки из облака точек для определения препятствий. Затем надувайте препятствия на карте, чтобы обеспечить безопасное рабочее пространство вокруг препятствий.

Создание occupancyMap3D объект с разрешением карты 10 ячеек/метр.

map3D = occupancyMap3D(10);

Определение набора точек 3-D как наблюдения из позы [x y z qw qx qy qz]. Эта поза предназначена для датчика, который наблюдает за этими точками и центрируется по началу координат. Определите два набора точек для вставки нескольких наблюдений.

pose = [ 0 0 0 1 0 0 0];

points = repmat((0:0.25:2)', 1, 3);
points2 = [(0:0.25:2)' (2:-0.25:0)' (0:0.25:2)'];
maxRange = 5;

Вставка первого набора точек с помощью insertPointCloud. Функция использует позу датчика и заданные точки для вставки наблюдений на карту. Отображаемые цвета соотносятся с высотой точки только в иллюстративных целях.

insertPointCloud(map3D,pose,points,maxRange)
show(map3D)

Вставьте второй набор точек. Луч между позой (началом координат) датчика и этими точками перекрывает точки предыдущей вставки. Поэтому свободное пространство между датчиком и новыми точками обновляется и помечается как свободное пространство.

insertPointCloud(map3D,pose,points2,maxRange)
show(map3D)

Надуть карту, чтобы добавить буферную зону для безопасной работы вокруг препятствий. Определите радиус транспортного средства и расстояние безопасности и используйте сумму этих значений для определения радиуса накачки для карты.

vehicleRadius = 0.2;
safetyRadius = 0.3;
inflationRadius = vehicleRadius + safetyRadius;
inflate(map3D, inflationRadius);

show(map3D)