Запланируйте движение дерево твердого тела использование двунаправленного RRT
manipulatorRRT объект является планировщиком единого запроса для рук манипулятора, который использует двунаправленные быстро исследующие случайные деревья (RRT) алгоритм с дополнительной эвристикой подключения, чтобы потенциально увеличить скорость.
Двунаправленный планировщик RRT создает два дерева с корневыми узлами в заданном запуске и целевых настройках. Чтобы расширить каждое дерево, планировщик генерирует случайную настройку и, если допустимый, получает шаг от самого близкого узла на основе свойства MaxConnectionDistance. После каждого расширения планировщик пытается соединиться между этими двумя деревьями с помощью нового расширения и самого близкого узла на противоположном дереве. Недопустимые настройки или связи, которые сталкиваются со средой, не добавляются к дереву.
Для более жадного поиска, включая свойство EnableConnectHeuristic отключает предел на MaxConnectionDistance свойство при соединении между этими двумя деревьями.
Установка EnableConnectHueristic свойство к false ограничивает дополнительное расстояние при соединении между этими двумя деревьями со значением MaxConnectionDistance свойство.
Объект использует rigidBodyTree модель робота, чтобы сгенерировать случайные настройки и промежуточные состояния между узлами. Объекты столкновения заданы в модели робота, чтобы подтвердить настройки и проверку на столкновения со средой или самим роботом.
Чтобы запланировать путь между запуском и целевой настройкой, используйте plan объектная функция. После планирования можно интерполировать состояния вдоль пути с помощью interpolate объектная функция. Чтобы попытаться сократить путь путем обрезки ребер, используйте shorten объектная функция.
Чтобы задать область к демонстрационным положениям исполнительного элемента конца около целевой настройки, создайте workspaceGoalRegion возразите и задайте его как goalRegion введите к plan объектная функция. Чтобы изменить вероятность выборки дополнительных целевых настроек, задайте свойство WorkspaceGoalRegionBias.
Для получения дополнительной информации о вычислительной сложности, смотрите Сложность Планирования.
rrt = manipulatorRRT( создает двунаправленного планировщика RRT для заданного robotRBT,{})rigidBodyTree модель робота. Массив пустой ячейки указывает, что нет никаких препятствий в среде.
rrt = manipulatorRRT( создает планировщика для модели робота с объектами столкновения, помещенными в среду. Планировщик проверяет на столкновения с этими объектами.robotRBT,collisionObjects)
plan | Запланируйте путь с помощью RRT для манипуляторов |
interpolate | Интерполируйте состояния вдоль пути от RRT |
shorten | Ребра для обрезки, чтобы сократить путь от RRT |
Используйте manipulatorRRT возразите, чтобы запланировать путь модель робота дерева твердого тела в среде с препятствиями. Визуализируйте запланированный путь с интерполированными состояниями.
Загрузите модель робота в рабочую область. Используйте LBR KUKA рука манипулятора 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 отберите для воспроизводимости.
rng(0) path = plan(rrt,startConfig,goalConfig);
Визуализируйте путь. Чтобы добавить больше промежуточных состояний, интерполируйте путь. По умолчанию, interpolate возразите, что функция использует значение ValidationDistance свойство определить количество промежуточных состояний. 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
Задайте целевую область в своей рабочей области и запланируйте путь в тех границах. workspaceGoalRegion объект задает границы на XYZ-позиционной и Эйлеровой ориентации ZYX исполнительного элемента конца робота. manipulatorRRT возразите планирует путь на основе той целевой области и выборок случайные положения в границах.
Загрузите существующую модель робота как rigidBodyTree объект.
robot = loadrobot("kinovaGen3", "DataFormat", "row"); ax = show(robot);
Создайте планировщика пути
Создайте планировщика пути к быстро исследующему случайному дереву (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);
Запланируйте путь к целевой области
Запланируйте путь к целевой области от домашней настройки робота. Из-за случайности в алгоритме RRT, этот пример устанавливает rng отберите, чтобы гарантировать повторяемые результаты.
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
Настройте положение исполнительного элемента конца
Заметьте, что манипулятор приближается к рабочей области от нижней части. Чтобы инвертировать ориентацию конечного положения, добавьте 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
Планирование сложности
При планировании движения между узлами в дереве набор настроек сгенерирован и подтвержден. Это время вычисления планировщика пропорционально количеству сгенерированных настроек. Количеством настроек между узлами управляет отношение свойств MaxConnectionDistance и ValidationDistance. Чтобы улучшить время планирования, рассмотрите увеличение расстояния валидации или уменьшение макс. расстояния связи.
Проверка каждой настройки имеет сложность O (mn+m2), где m является количеством тел столкновения в rigidBodyTree объект и n являются количеством объектов столкновения в worldObjects. Используя большие количества сеток, чтобы представлять вашего робота или среду увеличивает время для проверки каждой настройки.
Пределы соединения Бога
Если ваш rigidBodyTree модель робота имеет объединенные пределы, которые имеют бесконечную область значений (например, шарнирное соединение с пределами [-Inf Inf]), manipulatorRRT возразите использует пределы [-1e10 1e10] выполнять универсальную случайную выборку в объединенных пределах.
[1] Kuffner, J. J. и С. М. Лэвалл. “RRT-подключение: Эффективный Подход к Планированию пути Единого запроса”. В Продолжениях 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по вопросам Робототехники и Автоматизации. Продолжения симпозиумов (CAT. № 00CH37065), 2:995–1001. Сан-Франциско, CA, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.