Создайте двунаправленный планировщик RRT для геометрического планирования
The plannerBiRRT объект является планировщиком с одним запросом, который использует двунаправленный быстро исследуемый алгоритм случайного дерева (RRT) с необязательной эвристикой соединения для увеличения скорости.
Двунаправленный планировщик RRT создает одно дерево с корневым узлом в заданном начальном состоянии и другое дерево с корневым узлом в заданном целевом состоянии. Чтобы расширить каждое дерево, планировщик генерирует случайное состояние и, если он действителен, делает шаг от ближайшего узла на основе свойства MaxConnectionDistance. Начальное и целевое деревья чередуют этот процесс расширения до тех пор, пока оба дерева не будут соединены. Если включено свойство EnableConnectHeuristic, процесс расширения игнорирует свойство MaxConnectionDistance. Недопустимые состояния или соединения, которые конфликтуют со окружением, не добавляются в дерево.
planner = plannerBiRRT(stateSpace,stateVal)stateSpace, и объект валидатора состояний, stateVal. Пространство состояний stateVal должно быть то же, что и stateSpace. The 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 seed для повторяемости.
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] Kuffner, J. J., and S. M. LaValle. «RRT-Connect: эффективный подход к планированию пути с одним запросом». В Труде 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по робототехнике и автоматизации. Материалы симпозиумов (кат. № 00CH37065), 2:995–1001. Сан-Франциско, Калифорния, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.