Создайте двунаправленного планировщика RRT для геометрического планирования
plannerBiRRT объект является планировщиком единого запроса, который использует двунаправленное быстро исследующее случайное дерево (RRT) алгоритм с дополнительной эвристикой подключения для увеличенной скорости.
Двунаправленный планировщик RRT создает одно дерево с корневым узлом в заданном начальном состоянии и другое дерево с корневым узлом в заданном целевом состоянии. Чтобы расширить каждое дерево, планировщик генерирует случайное состояние и, если допустимый, получает шаг от самого близкого узла на основе свойства MaxConnectionDistance. Запуск и целевые деревья чередуют этот дополнительный процесс, пока оба дерева не соединяются. Если свойство EnableConnectHeuristic включено, дополнительный процесс игнорирует свойство MaxConnectionDistance. Недопустимые состояния или связи, которые сталкиваются со средой, не добавляются к дереву.
planner = plannerBiRRT(stateSpace,stateVal)stateSpace, и объект блока проверки допустимости состояния, stateVal. Пространство состояний stateVal должен совпасть с stateSpace. stateSpace и stateVal аргументы также устанавливают StateSpace и StateValidator свойства, соответственно, планировщика.
Используйте plannerBiRRT возразите, чтобы запланировать путь между двумя состояниями в среде с препятствиями. Визуализируйте запланированный путь с интерполированными состояниями.
Создайте пространство состояний.
ss = stateSpaceSE2;
Создайте occupancyMap- основанный блок проверки допустимости состояния, использующий созданное пространство состояний.
sv = validatorOccupancyMap(ss);
Создайте карту заполнения из карты в качестве примера и установите разрешение карты как 10 ячеек на метр.
load exampleMaps
map = occupancyMap(ternaryMap,10);
sv.Map = map;Установите расстояние валидации для блока проверки допустимости.
sv.ValidationDistance = 0.01;
Обновите границы пространства состояний, чтобы совпасть с пределами карты.
ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];
Создайте планировщика пути и увеличьте максимальное расстояние связи.
planner = plannerBiRRT(ss,sv); planner.MaxConnectionDistance = 0.3;
Задайте запуск и целевые состояния.
start = [20 10 0]; goal = [40 40 0];
Запланируйте путь. Из-за случайности алгоритма RRT, набор rng отберите для воспроизводимости.
rng(100,'twister')
[pthObj,solnInfo] = plan(planner,start,goal);Отобразите количество проделанных итераций для дерева, чтобы сходиться.
fprintf('Number of iterations: %d\n',solnInfo.NumIterations)Number of iterations: 346
Визуализируйте результаты.
show(map) hold on plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2),'.-','color','b') % Start tree expansion plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2),'.-','color','g') % Goal tree expansion plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2) % draw path hold off
Повторно запланируйте путь с EnableConnectHeuristic набор свойств к истине.
planner.EnableConnectHeuristic = true; [pthObj,solnInfo] = plan(planner,start,goal);
Отобразите количество проделанных итераций для дерева, чтобы сходиться. Заметьте, что планировщик значительно требует меньшего количества итераций по сравнению с когда EnableConnectHeuristic свойство установлено в ложь.
fprintf('Number of iterations: %d\n',solnInfo.NumIterations)Number of iterations: 135
Визуализируйте результаты.
figure show(map) hold on plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2),'.-','color','b') % Start tree expansion plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2),'.-','color','g') % Goal tree expansion plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2) % draw path
[1] Kuffner, J. J. и С. М. Лэвалл. “RRT-подключение: Эффективный Подход к Планированию пути Единого запроса”. В Продолжениях 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по вопросам Робототехники и Автоматизации. Продолжения симпозиумов (CAT. № 00CH37065), 2:995–1001. Сан-Франциско, CA, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.