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

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

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

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

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

% Show 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 default

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

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

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

show(occGrid)
hold on

% Plot entire 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 start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
hold off

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

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

Создайте объект пространства состояний использование помощника в качестве примера. Задайте те же границы состояния и дайте новый булев параметр как 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 default
[pthObj2,solnInfo] = plan(planner,start,goal);

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

Чертите новый путь на карте. Путь должен только выполнить левые повороты достигнуть цели.

figure
show(occGrid)

hold on

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

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

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