В этом примере показано, как планировать траекторию перемещения громоздкой мебели в узком пространстве, избегая опор. В этом примере показан рабочий процесс «Piano Mover's Problem», который используется для тестирования алгоритмов планирования пути с ограниченными пространствами состояний. В этом примере используется plannerRRTStar объект для реализации пользовательского оптимизированного быстро исследуемого дерева (RRT *) algoirthm. Приведенные в качестве примера помощники иллюстрируют определение пользовательских пространств состояний и проверки состояния для любого приложения планирования движения.
Для визуализации и решения этой проблемы планирования путей предусмотрены два вспомогательных класса: 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
Создать 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 в качестве планировщика и укажите пользовательское пространство состояния и средство проверки состояния. Укажите дополнительные параметры для плановика. 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);
