В этом примере показано, как запланировать путь, чтобы переместить большую мебель в ограниченное пространство, избегающее полюсов. Этот пример показывает рабочий процесс "проблемы Двигателя Фортепьяно", которая используется в тестировании алгоритмов планирования пути с ограниченными пространствами состояний. Этот пример использует plannerRRTStar возразите, чтобы реализовать пользовательское оптимизированное быстро исследующее дерево (RRT*) алгоритм. Если помощники в качестве примера иллюстрируют, как задать пользовательские пространства состояний и валидацию состояния для любого приложения планирования движения.
Чтобы помочь визуализировать и решить эту задачу планирования пути, два класса помощника обеспечиваются, ExampleHelperFurniture и ExampleHelperRoom. ExampleHelperFurniture класс собирает мебель путем соединения трех прямоугольных полей столкновения. Мебель настраивается как указано ниже:
ExampleHelperRoom задает размерность комнаты и обеспечивает функции, чтобы вставить мебель в комнату и проверять, является ли мебель в столкновении со стенами или полюсами. Эти два класса используются в блоке проверки допустимости состояния планировщика.
Покажите комнату с данной длиной и шириной. Добавьте предмет мебели с данным положением.
len = 6;
wid = 2;
room = ExampleHelperRoom(len, wid);
chair = ExampleHelperFurniture;
addFurniture(room,chair,trvec2tform([0 3.5 0]));
show(room,gca)
axis equal
Создайте statestateSpaceSE2 объект для мебели. Установите границы на основе размерностей помещения. Выборки пространства состояний случайные состояния в пространстве состояний. В этом примере состояние мебели является вектором с 3 элементами, [x y theta], для координат xy и угла вращения в радианах.
bounds = [-0.8 0.8; [-1 5]; [-pi pi]]; ss = stateSpaceSE2(bounds); ss.WeightTheta = 2;
Планировщик требует, чтобы индивидуально настраиваемый блок проверки допустимости состояния включил проверку столкновения между мебелью и фиксаторами в комнате. Обеспеченный класс, ExampleHelperFurnitureInRoomValidator, проверяет валидность состояний мебели на основе попарных выпуклых функций проверки столкновения многоугольника. Класс автоматически создает комнату и помещает мебель странной формы в нее при конструкции.
% Set the initial pose of the furniture initPose = trvec2tform([0 3.5 0]); % Create a customized state validator sv = ExampleHelperFurnitureInRoomValidator(ss, initPose); % Reduce the validation distance % Validation distance determines the granularity of interpolation when % checking the motion that connects two states. sv.ValidationDistance = 0.1;
Используйте plannerRRTStar как планировщик и задают пользовательское пространство состояний и утверждают блок проверки допустимости. Задайте дополнительные параметры для планировщика. GoalReached функция помощника в качестве примера возвращает true когда выполнимый путь приближается достаточно к цели в пороге. Это выходит из планировщика.
% Create the planner rrt = plannerRRTStar(ss, sv); % Set ball radius for searching near neighbors rrt.BallRadiusConstant = 1000; % Exit as soon as a path is found rrt.ContinueAfterGoalReached = false; % The motion length between two furniture poses should be less than 0.4 m rrt.MaxConnectionDistance = 0.4; % Increase the max iterations rrt.MaxIterations = 20000; % Use a customized goal function rrt.GoalReachedFcn = @exampleHelperGoalFunc;
Установите запуск и положение конца для мебели. Этот пример перемещается от одного полюса до другой и вращает стул pi радианы. Запланируйте путь между положениями. Визуализируйте дерево поиска и итоговый путь.
% Set the init and goal poses start = [0 3.5 0]; goal = [0 -0.2 pi]; % Set random number seed for repeatability rng(0, 'twister'); [path, solnInfo] = plan(rrt,start,goal); hold on % Search tree plot(solnInfo.TreeData(:,1), solnInfo.TreeData(:,2), '.-'); % Interpolate path and plot points interpolate(path,300) plot(path.States(:,1), path.States(:,2), 'r-', 'LineWidth', 2) hold off
Помощнику в качестве примера предоставляют для сглаживания пути путем сокращения углов пути, если это возможно. Анимируйте движение мебели от запуска до целевого положения. График анимации показывает промежуточные состояния, когда мебель перешла к целевой позиции.
f = figure; % Smooth the path, cut the corners wherever possible pathSm = exampleHelperSmoothPath(path, sv); interpolate(pathSm,100); animateFurnitureMotion(sv.Room,1,pathSm.States, axes(f)) % show the trace of furniture skip = 6; states = pathSm.States([1:skip:end, pathSm.NumStates], :); exampleHelperShowFurnitureTrace(sv.Room.FurnituresInRoom{1}, states);
