Планирование движения для дерева жесткого тела с использованием двунаправленного RRT
manipulatorRRT объект является планировщиком с одним запросом для манипуляторов, который использует двунаправленный алгоритм быстрого исследования случайных деревьев (RRT) с дополнительной эвристикой соединения для потенциального увеличения скорости.
Двунаправленный планировщик RRT создает два дерева с корневыми узлами в указанных конфигурациях начала и цели. Для расширения каждого дерева планировщик создает случайную конфигурацию и, если она допустима, делает шаг от ближайшего узла на основе свойства MaxConniceDistance. После каждого внутреннего абонента плановик пытается соединить два дерева с помощью нового внутреннего абонента и ближайшего узла на противоположном дереве. Недопустимые конфигурации или подключения, которые сталкиваются с средой, не добавляются в дерево.
В случае поиска с использованием greedier включение свойства EnableStartHeuristic отключает ограничение для MaxConnectionDistance свойство при соединении между двумя деревьями.
Установка EnableConnectHueristic свойство для false ограничивает протяженность при соединении между двумя деревьями значением MaxConnectionDistance собственность.
Объект использует rigidBodyTree модель робота для генерации случайных конфигураций и промежуточных состояний между узлами. Объекты столкновения задаются в модели робота для проверки конфигураций и проверки на наличие столкновений с окружающей средой или самим роботом.
Чтобы спланировать путь между конфигурацией начала и цели, используйте plan объектная функция. После планирования можно выполнить интерполяцию состояний вдоль траектории с помощью interpolate объектная функция. Чтобы попытаться укоротить контур путем обрезки кромок, используйте команду shorten объектная функция.
Чтобы указать область для выборки концевых эффекторных позиций рядом с конфигурацией цели, создайте workspaceGoalRegion и укажите его как goalRegion входные данные для plan объектная функция. Чтобы изменить вероятность выборки дополнительных конфигураций целей, укажите свойство рабочей области GoalRegionBias.
Дополнительные сведения о сложности вычислений см. в разделе Сложность планирования.
rrt = manipulatorRRT( создает двунаправленный планировщик RRT для указанного robotRBT,{})rigidBodyTree модель робота. Пустой массив ячеек указывает на отсутствие препятствий в среде.
rrt = manipulatorRRT( создает планировщик для модели робота с объектами столкновения, размещенными в среде. Плановик проверяет наличие конфликтов с этими объектами.robotRBT,collisionObjects)
plan | Планирование пути с использованием RRT для манипуляторов |
interpolate | Интерполяция состояний по пути от RRT |
shorten | Обрезка кромок для укорочения траектории от RRT |
Используйте 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 затравка для повторяемости.
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-положения и ориентации Euler 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
Сложность планирования
При планировании движения между узлами в дереве генерируется и проверяется набор конфигураций. Это время вычисления плановика пропорционально количеству сгенерированных конфигураций. Количество конфигураций между узлами определяется соотношением свойств MaxConniceDistance и ValidationDistance. Чтобы увеличить время планирования, рассмотрите возможность увеличения расстояния проверки или уменьшения максимального расстояния соединения.
Проверка каждой конфигурации имеет сложность O (mn + m2), где m - количество тел столкновения в rigidBodyTree object и n - количество объектов-коллизий в worldObjects. Использование большого количества сетей для представления робота или среды увеличивает время проверки каждой конфигурации.
Бесконечные пределы соединения
Если ваш rigidBodyTree модель робота имеет пределы соединения, которые имеют бесконечный диапазон (например, поворотное соединение с пределами [-Inf Inf]), manipulatorRRT объект использует пределы [-1e10 1e10] для выполнения равномерной случайной выборки в пределах соединения.
[1] Куффнер, Дж. Дж., и С. М. ЛаВалле. «RRT-Connect: эффективный подход к планированию пути с одним запросом». В трудах 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по робототехнике и автоматизации. Материалы симпозиумов (Cat. № 00CH37065), 2:995–1001. Сан-Франциско, Калифорния, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.