В этом примере показано, как использовать алгоритм быстро исследующего случайного дерева (RRT), чтобы запланировать путь транспортное средство через известную карту. Ограничения специального транспортного средства также применяются с пользовательским пространством состояний. Можно настроить собственного планировщика с пользовательским пространством состояний и объектами валидации пути для любого приложения навигации.
Загрузите существующую карту заполнения пробела малого офиса. Постройте запуск и целевые положения транспортного средства сверху карты.
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
Чтобы задать пользовательские ограничения транспортного средства, настройте объект пространства состояний. Этот пример использует 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