Используйте 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