Создание двунаправленного плановика RRT для геометрического планирования
plannerBiRRT объект - планировщик с одним запросом, использующий двунаправленный алгоритм быстрого исследования случайного дерева (RRT) с дополнительной эвристикой соединения для увеличения скорости.
Двунаправленный планировщик RRT создает одно дерево с корневым узлом в указанном начальном состоянии и другое дерево с корневым узлом в указанном целевом состоянии. Для расширения каждого дерева планировщик создает случайное состояние и, если оно допустимо, делает шаг от ближайшего узла на основе свойства MaxConniceDistance. Начальный и целевой деревья чередуют этот процесс расширения до тех пор, пока не будут подключены оба дерева. При включенном свойстве EnableStartHeuristic процесс расширения игнорирует свойство MaxConniceDistance. Недопустимые состояния или соединения, которые сталкиваются с средой, не добавляются в дерево.
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 свойство имеет значение true.
planner.EnableConnectHeuristic = true; [pthObj,solnInfo] = plan(planner,start,goal);
Отображение количества итераций для сходимости дерева. Обратите внимание, что планировщику требуется значительно меньше итераций по сравнению с EnableConnectHeuristic свойство имеет значение false.
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] Куффнер, Дж. Дж., и С. М. ЛаВалле. «RRT-Connect: эффективный подход к планированию пути с одним запросом». В трудах 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по робототехнике и автоматизации. Материалы симпозиумов (Cat. № 00CH37065), 2:995–1001. Сан-Франциско, Калифорния, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.