Карта занятости нагрузки

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

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 и задание границ состояния. Этот объект ограничивает отобранные состояния выполнимыми кривыми Дубина для управления транспортным средством в пределах состояний. Радиус поворота 0,4m обеспечивает плотные повороты в этой небольшой среде.

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