Планирование пути в среде различной сложности

Этот пример демонстрирует, как вычислить путь без препятствий между двумя местами на данной карте с помощью Вероятностной Дорожной карты (PRM) планировщик пути. Планировщик пути PRM создает дорожную карту в свободном пространстве данной карты, использующей случайным образом произведенные узлы в свободном пространстве и соединяющей их друг с другом. Если дорожная карта была создана, можно запросить для пути от данного местоположения запуска до данного местоположения конца на карте.

В этом примере карта представлена как карта сетки заполнения с помощью импортированных данных. При выборке узлов в свободном пространстве карты PRM использует это бинарное представление сетки заполнения, чтобы вывести свободное пространство. Кроме того, PRM не учитывает размерность робота при вычислении пути без препятствий на карте. Следовательно, необходимо раздуть карту размерностью робота, для того, чтобы позволить расчет пути без препятствий, который составляет размер робота и гарантирует предотвращение столкновения для фактического робота. Задайте местоположения начала и конца на карте для планировщика пути PRM, чтобы найти путь без препятствий.

Импортируйте карты в качестве примера для планирования пути

load exampleMaps.mat

Импортированные карты: simpleMap, complexMap и ternaryMap.

whos *Map*
  Name              Size               Bytes  Class      Attributes

  complexMap       41x52                2132  logical              
  emptyMap         26x27                 702  logical              
  simpleMap        26x27                 702  logical              
  ternaryMap      501x501            2008008  double               

Используйте импортированный simpleMap данные и построение представление сетки заполнения с помощью binaryOccupancyMap объект. Установите разрешение 2 ячеек на метр для этой карты.

map = binaryOccupancyMap(simpleMap,2);

Отобразите карту с помощью show функция на binaryOccupancyMap объект

show(map)

Figure contains an axes. The axes with title Binary Occupancy Grid contains an object of type image.

Задайте размерности робота и раздуйте карту

Чтобы гарантировать, что робот не сталкивается ни с какими препятствиями, необходимо раздуть карту размерностью робота прежде, чем предоставить его планировщику пути PRM.

Здесь размерность робота может быть принята, чтобы быть кругом с радиусом 0,2 метров. Можно затем раздуть карту этой размерностью с помощью inflate функция.

robotRadius = 0.2;

Как упомянуто прежде, PRM не составляет размерность робота, и следовательно обеспечение расширенной карты к PRM учитывает размерность робота. Создайте копию карты перед использованием inflate функционируйте, чтобы сохранить исходную карту.

mapInflated = copy(map);
inflate(mapInflated,robotRadius);

Отобразите раздутую карту

show(mapInflated)

Figure contains an axes. The axes with title Binary Occupancy Grid contains an object of type image.

Создайте PRM и установленные параметры

Теперь необходимо задать планировщика пути. Создайте a mobileRobotPRM возразите и задайте связанные атрибуты.

prm = mobileRobotPRM;

Присвойте расширенную карту объекту PRM

prm.Map = mapInflated;

Задайте количество узлов PRM, которые будут использоваться во время конструкции PRM. PRM создает дорожную карту с помощью данного количества узлов на данной карте. На основе размерности и сложности входной карты, это - один из первичных атрибутов, чтобы настроиться для того, чтобы получить решение между двумя точками на карте. Большое количество узлов создает плотную дорожную карту, и увеличивает вероятность нахождения пути. Однако наличие большего количества узлов увеличивает время вычисления и для создания дорожной карты и для нахождения решения.

prm.NumNodes = 50;

Задайте максимальное позволенное расстояние между двумя связанными узлами на карте. PRM соединяет все узлы, разделенные этим расстоянием (или меньше) на карте. Это - другой атрибут, чтобы настроиться в случае больших и/или сложных входных карт. Большое расстояние связи увеличивает возможность соединения между узлами, чтобы найти путь легче, но может увеличить время вычисления создания дорожной карты.

prm.ConnectionDistance = 5;

Найдите выполнимый путь на созданном PRM

Задайте местоположения начала и конца на карте для планировщика пути, чтобы использовать.

startLocation = [2 1];
endLocation = [12 10];

Ищите путь между местоположениями начала и конца с помощью findpath функция. Решение является набором waypoints от местоположения запуска в конец местоположение. Обратите внимание на то, что path будет отличаться из-за вероятностной природы алгоритма PRM.

path = findpath(prm, startLocation, endLocation)
path = 7×2

    2.0000    1.0000
    1.9569    1.0546
    1.8369    2.3856
    3.2389    6.6106
    7.8260    8.1330
   11.4632   10.5857
   12.0000   10.0000

Отобразите решение PRM.

show(prm)

Figure contains an axes. The axes with title Probabilistic Roadmap contains 4 objects of type image, line, scatter.

Используйте PRM для большой и сложной карты

Используйте импортированный complexMap данные, которые представляют большой и сложный план здания и создают бинарное представление сетки заполнения с данным разрешением (1 ячейка на метр)

map = binaryOccupancyMap(complexMap,1);

Отобразите карту.

show(map)

Figure contains an axes. The axes with title Binary Occupancy Grid contains an object of type image.

Раздуйте карту на основе размерности робота

Скопируйте и раздуйте карту, чтобы включить размер робота для предотвращения препятствия

mapInflated = copy(map);
inflate(mapInflated, robotRadius);

Отобразите раздутую карту.

show(mapInflated)

Figure contains an axes. The axes with title Binary Occupancy Grid contains an object of type image.

Сопоставьте существующий объект PRM с новой картой и установленными параметрами

Обновите объект PRM с недавно расширенной картой и задайте другие атрибуты.

prm.Map = mapInflated;

Установите NumNodes и ConnectionDistance свойства.

prm.NumNodes = 20;
prm.ConnectionDistance = 15;

Отобразите график PRM.

show(prm)

Figure contains an axes. The axes with title Probabilistic Roadmap contains 3 objects of type image, line, scatter.

Найдите выполнимый путь на созданном PRM

Задайте местоположение начала и конца на карте, чтобы найти путь без препятствий.

startLocation = [3 3];
endLocation = [45 35];

Ищите решение между местоположением начала и конца. Для комплексных карт не может быть выполнимого пути для данного количества узлов (возвращает пустой путь).

path = findpath(prm, startLocation, endLocation);

Поскольку вы планируете путь на большой и сложной карте, большее число узлов может требоваться. Однако часто не ясно, сколько узлов будет достаточно. Настройте количество узлов, чтобы убедиться, что между местоположением начала и конца существует выполнимый путь.

while isempty(path)
    % No feasible path found yet, increase the number of nodes
    prm.NumNodes = prm.NumNodes + 10;
    
    % Use the |update| function to re-create the PRM roadmap with the changed
    % attribute
    update(prm);
    
    % Search for a feasible path with the updated PRM
    path = findpath(prm, startLocation, endLocation);
end

Отобразите путь.

path
path = 12×2

    3.0000    3.0000
    4.2287    4.2628
    7.7686    5.6520
    6.8570    8.2389
   19.5613    8.4030
   33.1838    8.7614
   31.3248   16.3874
   41.3317   17.5090
   48.3017   25.8527
   49.4926   36.8804
      ⋮

Отобразите решение PRM.

show(prm)

Figure contains an axes. The axes with title Probabilistic Roadmap contains 4 objects of type image, line, scatter.

Для просмотра документации необходимо авторизоваться на сайте