Этот пример показывает, как планировать путь для перемещения громоздкой мебели в плотном пространстве, избегая полюсов. Этот пример показывает рабочий процесс «Задачи Piano Mover», которая используется для проверки алгоритмов планирования пути с ограниченными пространствами состояний. Этот пример использует plannerRRTStar объект для реализации пользовательского оптимизированного быстро исследующего дерева (RRT *) algoirthm. Предоставленные в качестве примера помощники иллюстрируют, как задать пользовательские пространства состояний и валидацию состояний для любого приложения планирования движения.
Чтобы помочь визуализировать и решить эту задачу планирования пути, предусмотрены два класса помощника ExampleHelperFurniture и ExampleHelperRoom. The ExampleHelperFurniture класс собирает мебель путем сложения трех прямоугольных коробок столкновения. Мебель установлена следующим образом:
The ExampleHelperRoom определяет размерность помещения и обеспечивает функции для вставки мебели в помещение и проверки столкновения мебели со стенками или столбами. Эти два класса используются для проверки состояния планировщика.
Показать комнату с заданной длиной и шириной. Добавить предмет мебели с заданным положением.
len = 6;
wid = 2;
room = ExampleHelperRoom(len, wid);
chair = ExampleHelperFurniture;
addFurniture(room,chair,trvec2tform([0 3.5 0]));
show(room,gca)
axis equal
Создайте stateSpaceSE2 объект для мебели. Установите границы в зависимости от размерностей помещений. Пространство состояний дискретизирует случайные состояния в пространстве состояний. В этом примере состояние мебели является вектором с 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 в качестве планировщика и задайте пользовательское пространство состояний и средство проверки состояний. Задайте дополнительные параметры для планировщика. The 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);
