manipulatorRRT

Планируйте движение для древовидного твердого тела с помощью двунаправленного RRT

развернуть все на странице

Описание

The manipulatorRRT объект является планировщиком с одним запросом для рук манипулятора, который использует двунаправленный быстро исследуемый алгоритм случайных деревьев (RRT) с необязательным эвристическим соединением, чтобы потенциально увеличить скорость.

Двунаправленный планировщик RRT создает два дерева с корневыми узлами в заданных начальных и целевых строениях. Чтобы расширить каждое дерево, планировщик генерирует случайное строение и, если это допустимо, делает шаг от ближайшего узла на основе свойства MaxConnectionDistance. После каждого расширения планировщик пытается соединиться между двумя деревьями с помощью нового расширения и ближайшего узла на противоположном дереве. Недопустимые строения или соединения, которые конфликтуют со окружением, не добавляются в дерево.

Для более зеленого поиска включение свойства EnableConnectHeuristic отключает предел на MaxConnectionDistance свойство при соединении двух деревьев.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is true, the connection steps are not limited by the max connection distance.

Установка EnableConnectHueristic свойство к false ограничивает расстояние удлинения при соединении двух деревьев значением MaxConnectionDistance свойство.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is false, the connection steps are limited by the max connection distance.

Объект использует rigidBodyTree модель робота, чтобы сгенерировать случайные строения и промежуточные состояния между узлами. Объекты столкновения заданы в модели робота, чтобы подтвердить строения и проверить на столкновения со окружением или самим роботом.

Чтобы спланировать путь между начальными и целевыми строениями, используйте plan функция объекта. После планирования можно интерполировать состояния вдоль пути с помощью interpolate функция объекта. Чтобы попытаться сократить контур путем обрезки ребер, используйте shorten функция объекта.

Чтобы задать область для выборки положений end-effector около строения цели, создайте workspaceGoalRegion и задайте его как goalRegion вход plan функция объекта. Чтобы изменить вероятность дискретизации дополнительных строений цели, задайте свойство WorkspaceGoalRegionBias.

Для получения дополнительной информации о вычислительной сложности см. «Сложность планирования».

Создание

Синтаксис

rrt = manipulatorRRT(robotRBT,{})
rrt = manipulatorRRT(robotRBT,collisionObjects)

Описание

пример

rrt = manipulatorRRT(robotRBT,{}) создает двунаправленный планировщик RRT для заданного rigidBodyTree модель робота. Пустой массив ячеек указывает, что в окружении нет никаких препятствий.

rrt = manipulatorRRT(robotRBT,collisionObjects) создает планировщик для модели робота с объектами столкновения, размещенными в окружении. Плановик проверяет наличие столкновений с этими объектами.

Свойства

расширить все

Максимальная длина между запланированными строениями, заданная как положительная скалярная величина. Объект вычисляет длину движения как евклидово расстояние между два строений соединений. Различия между двумя положениями соединений для шарнира вращения вычисляются с помощью angdiff функция. В процессе расширения это максимальное расстояние, которое может измениться строение.

Если на EnableConnectheuristic для свойства задано значение trueобъект игнорирует это расстояние при соединении двух деревьев во время этапа соединения.

Типы данных: double

Разрешение расстояния для проверки движения между строениями, заданное как положительная скалярная величина. Расстояние валидации определяет количество интерполированных узлов между двумя смежными узлами в дереве. Объект проверяет каждый интерполированный узел путем проверки на наличие конфликтов с роботом и средой.

Типы данных: double

Максимальное количество сгенерированных случайных строений, заданное в виде положительного целого числа.

Типы данных: double

Непосредственно соедините деревья во время фазы соединения планировщика, заданной как логическое 1 (true) или 0 (false). Установка значения свойства true заставляет объект игнорировать MaxConnectionDistance свойство при попытке соединения двух деревьев.

Типы данных: logical

Вероятность выборки состояния цели из области цели рабочей области, заданная как положительное значение в области значений [0,1). Смещение задает вероятность добавить дополнительные состояния цели в дерево из workspaceGoalRegion объект. Когда это значение установлено в нуль, workspaceGoalRegion объект все еще отсчитывает одну цель для планирования плановика.

Увеличение этого значения увеличивает вероятность достижения состояния цели в области цели, но может привести к увеличению времени планирования, потому что каждое новое состояние цели добавляет дополнительную сложность для планирования.

Зависимость

Вы должны использовать goalRegion вход при вызове plan функция объекта.

Типы данных: double

Функции объекта

planПланируйте путь, используя RRT для манипуляторов
interpolateИнтерполяция состояний по пути из RRT
shortenОбрезка ребер для укорочения пути из RRT

Примеры

свернуть все

Открыть Live Script

Используйте manipulatorRRT объект для планирования пути для модели древовидного твердого тела в окружении с препятствиями. Визуализируйте запланированный путь с интерполированными состояниями.

Загрузите модель робота в рабочую область. Используйте руку манипулятора KUKA LBR iiwa ©.

robot = loadrobot("kukaIiwa14","DataFormat","row");

Сгенерируйте окружение для робота. Создайте объекты столкновения и задайте их положения относительно основы робота. Визуализация окружения.

env = {collisionBox(0.5, 0.5, 0.05) collisionSphere(0.3)};
env{1}.Pose(3, end) = -0.05;
env{2}.Pose(1:3, end) = [0.1 0.2 0.8];

show(robot);
hold on
show(env{1})
show(env{2})

Создайте планировщик RRT для модели робота.

rrt = manipulatorRRT(robot,env);

Задайте начальные и целевые строения.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Планируйте путь. Из-за случайности алгоритма RRT установите rng seed для повторяемости.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Визуализируйте путь. Чтобы добавить больше промежуточных состояний, интерполируйте путь. По умолчанию в interpolate объект использует значение ValidationDistance свойство для определения количества промежуточных состояний. The for цикл показывает каждый 20-й элемент интерполированного пути.

interpPath = interpolate(rrt,path);
clf
for i = 1:20:size(interpPath,1)
    show(robot,interpPath(i,:));
    hold on
end
show(env{1})
show(env{2})
hold off

Открыть Live Script

Задайте область цели в рабочей рабочей области и планируйте путь в этих границах. The workspaceGoalRegion объект задает границы XYZ-положения и ориентации ZYX Euler эффектора конца робота. The manipulatorRRT объект планирует путь, основанный на этой целевой области, и дискретизирует случайные положения в границах.

Загрузка существующей модели робота в rigidBodyTree объект.

robot = loadrobot("kinovaGen3", "DataFormat", "row");
ax = show(robot);

Figure contains an axes. The axes contains 25 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Создайте Планировщик Пути

Создайте быстро исследуемый планировщик пути (RRT) для робота. Этот пример использует пустое окружение, но этот рабочий процесс также хорошо работает с загроможденными окружениями. Можно добавить объекты столкновения в окружение, такую как collisionBox или collisionMesh объект.

planner = manipulatorRRT(robot,{});

Определите область цели

Создайте область цели рабочей области, используя имя тела робота с конечным эффектором.

Определите параметры области цели для вашей рабочей области. Область цели включает положение ссылки, границы XYZ-положения и пределы ориентации углов Эйлера ZYX. Этот пример задает границы XY-плоскости в метрах и позволяет вращать вокруг оси Z в радианах.

goalRegion = workspaceGoalRegion(robot.BodyNames{end}); 
goalRegion.ReferencePose = trvec2tform([0.5 0.5 0.2]);
goalRegion.Bounds(1, :) = [-0.2 0.2];    % X Bounds
goalRegion.Bounds(2, :) = [-0.2 0.2];    % Y Bounds
goalRegion.Bounds(4, :) = [-pi/2 pi/2];  % Rotation about the Z-axis

Можно также применить фиксированное смещение ко всем положениям, выбранным в области. Это смещение может учитывать инструменты захвата или изменения размерностей в рабочем рабочая область. В данном примере примените фиксированное преобразование, которое помещает концевой эффектор на 5 см выше рабочей области.

goalRegion.EndEffectorOffsetPose = trvec2tform([0 0 0.05]);
hold on
show(goalRegion);

Figure contains an axes. The axes contains 35 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Планируйте путь к области цели

Планируйте путь к области цели из домашнего строения робота. Из-за случайности в алгоритме RRT, этот пример устанавливает rng seed для обеспечения повторяемых результатов.

rng(0)
path = plan(planner,homeConfiguration(robot),goalRegion);

Показать робота, выполняющего путь. Чтобы визуализировать более реалистичный путь, интерполируйте точки между строениями пути.

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot,interpConfigurations(i,:),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1],...
        'CameraViewAngle',5)
  
    drawnow
end
hold off

Figure contains an axes. The axes contains 35 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Настройте положение End-Effector

Заметьте, что рука робота приближается к рабочей области снизу. Чтобы развернуть ориентацию конечного положения, добавьте pi вращение к оси Y для ссылки положения. 

goalRegion.EndEffectorOffsetPose = ... 
    goalRegion.EndEffectorOffsetPose*eul2tform([0 pi 0],"ZYX");

Повторите путь и снова визуализируйте движение робота. Теперь робот приближается с верхней частью.

hold on
show(goalRegion);
path = plan(planner,homeConfiguration(robot),goalRegion);

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot, interpConfigurations(i, :),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1])
    drawnow;
end
hold off

Figure contains an axes. The axes contains 45 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Совет

Комплексность планирования

  • При планировании движения между узлами в дереве генерируется и проверяется набор строений. Это время расчета плановика пропорционально количеству сгенерированных строений. Количество строений между узлами определяется отношением свойств MaxConnectionDistance и ValidationDistance. Чтобы улучшить время планирования, рассмотрите увеличение расстояния валидации или уменьшение максимального расстояния соединения.

  • Валидация каждого строения имеет сложность O (mn + m2), где m количество сталкивающихся тел в rigidBodyTree object и n - количество объектов столкновения в worldObjects. Использование большого количества сеток для представления вашего робота или окружения увеличивает время для проверки каждого строения.

Бесконечные Пределы Соединений

  • Если ваш rigidBodyTree модель робота имеет пределы соединений, которые имеют бесконечную область значений (например, шарнирное соединение с пределами [-Inf Inf]), а manipulatorRRT объект использует пределы [-1e10 1e10] для выполнения равномерной случайной выборки в пределах соединений.

Ссылки

[1] Kuffner, J. J., and S. M. LaValle. «RRT-Connect: эффективный подход к планированию пути с одним запросом». В Труде 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по робототехнике и автоматизации. Материалы симпозиумов (кат. № 00CH37065), 2:995–1001. Сан-Франциско, Калифорния, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

.

См. также

Объекты

Функции

Темы

Введенный в R2020b

Документация по Robotics System Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте