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

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

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 seed для последовательных результатов.

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