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

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