Загрузите карту заполнения

Загрузите существующую карту заполнения пробела малого офиса. Постройте запуск и целевые положения транспортного средства сверху карты.

load("office_area_gridmap.mat", "occGrid")
show(occGrid)

% Set the start and goal poses
start = [-1.0, 0.0, -pi];
goal = [14, -2.25, 0];

% Show the start and goal positions of the robot
hold on
plot(start(1), start(2), 'ro')
plot(goal(1), goal(2), 'mo')

% Show the start and goal headings
r = 0.5;
plot([start(1), start(1) + r*cos(start(3))], [start(2), start(2) + r*sin(start(3))], 'r-' )
plot([goal(1), goal(1) + r*cos(goal(3))], [goal(2), goal(2) + r*sin(goal(3))], 'm-' )
hold off

Задайте пространство состояний

Задайте пространство состояний транспортного средства с помощью stateSpaceDubins объект и определение границ состояния. Этот объект ограничивает произведенные состояния выполнимыми кривыми Dubins для регулирования транспортного средства в границах состояния. Поворачивающийся радиус 0.4 м допускает трудные повороты в этой небольшой среде.

bounds = [occGrid.XWorldLimits; occGrid.YWorldLimits; [-pi pi]];

ss = stateSpaceDubins(bounds);
ss.MinTurningRadius = 0.4;

Запланируйте путь

Запланировать путь, выборки алгоритма RRT случайные состояния в пространстве состояний и попытках соединить путь. Эти состояния и связи должны быть подтверждены или исключены на основе ограничений карты. Транспортное средство не должно сталкиваться с препятствиями, заданными в карте.

Создайте validatorOccupancyMap объект с заданным пространством состояний. Установите Map свойство к загруженному occupancyMap объект. Установите ValdiationDistance из 0.05 м. Это расстояние дискретизирует связи пути и проверяет препятствия в карту на основе этого.

stateValidator = validatorOccupancyMap(ss); 
stateValidator.Map = occGrid;
stateValidator.ValidationDistance = 0.05;

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

planner = plannerRRT(ss, stateValidator);
planner.MaxConnectionDistance = 2.0;
planner.MaxIterations = 30000;

Настройте GoalReached функция. Этот помощник в качестве примера функционирует проверки, если выполнимый путь достигает цели в пороге набора. Функция возвращает true когда цель была достигнута, и остановки планировщика.

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

function isReached = exampleHelperCheckIfGoal(planner, goalState, newState)
    isReached = false;
    threshold = 0.1;
    if planner.StateSpace.distance(newState, goalState) < threshold
        isReached = true;
    end
end

Запланируйте путь между запуском и целью. Из-за случайной выборки этот пример устанавливает rng отберите для сопоставимых результатов.

rng(0,'twister')

[pthObj, solnInfo] = plan(planner, start, goal);

Постройте путь

Покажите карту заполнения. Постройте дерево поиска от solnInfo. Интерполируйте и наложите итоговый путь.

show(occGrid)
hold on

% Search tree
plot(solnInfo.TreeData(:,1), solnInfo.TreeData(:,2), '.-');

% Interpolate and plot path
interpolate(pthObj,300)
plot(pthObj.States(:,1), pthObj.States(:,2), 'r-', 'LineWidth', 2)

% Show the start and goal in the grid map
plot(start(1), start(2), 'ro')
plot(goal(1), goal(2), 'mo')
hold off

Настройте ограничения транспортного средства Dubins

Чтобы задать пользовательские ограничения транспортного средства, настройте объект пространства состояний. Этот пример использует ExampleHelperStateSpaceOneSidedDubins, который основан на stateSpaceDubins класс. Этот класс помощника ограничивает поворачивающееся направление, чтобы или исправиться или оставленный на основе булево свойства, GoLeft. Это свойство по существу отключает типы пути dubinsConnection возразите использованию (см. dubinsConnection.DisabledPathTypes).

Создайте объект пространства состояний использование помощника в качестве примера. Задайте те же границы состояния и дайте новый булев параметр как true (только левые повороты).

% Only making left turns
goLeft = true;

% Create the state space
ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds, goLeft);
ssCustom.MinTurningRadius = 0.4;

Запланируйте путь

Создайте новый объект планировщика с пользовательскими ограничениями Dubins и блоком проверки допустимости на основе тех ограничений. Задайте тот же GoalReached функция.

stateValidator2 = validatorOccupancyMap(ssCustom); 
stateValidator2.Map = occGrid;
stateValidator2.ValidationDistance = 0.05;

planner = plannerRRT(ssCustom, stateValidator2);
planner.MaxConnectionDistance = 2.0;
planner.MaxIterations = 30000;
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

Запланируйте путь между запуском и целью. Сбросьте rng отберите снова.

rng(0, 'twister')
[pthObj2, solnInfo] = plan(planner, start, goal);

Постройте путь

Чертите новый путь на карте. Путь должен только выполнить левые повороты достигнуть цели. В этом примере показано, как можно настроить ограничения и все еще запланировать пути с помощью типового алгоритма RRT.

figure
show(occGrid)

hold on

% show the search tree
plot(solnInfo.TreeData(:,1), solnInfo.TreeData(:,2), '.-'); % tree expansion

% draw path (after the path is interpolated)
pthObj2.interpolate(300)
plot(pthObj2.States(:,1), pthObj2.States(:,2), 'r-', 'LineWidth', 2)

% Show the start and goal in the grid map
plot(start(1), start(2), 'ro')
plot(goal(1), goal(2), 'mo')
hold off