Моделируйте сцену

Чтобы помочь визуализировать и решить эту задачу планирования пути, предусмотрены два класса помощника 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);