Планируйте движение для древовидного твердого тела с помощью двунаправленного RRT
The manipulatorRRT объект является планировщиком с одним запросом для рук манипулятора, который использует двунаправленный быстро исследуемый алгоритм случайных деревьев (RRT) с необязательным эвристическим соединением, чтобы потенциально увеличить скорость.
Двунаправленный планировщик RRT создает два дерева с корневыми узлами в заданных начальных и целевых строениях. Чтобы расширить каждое дерево, планировщик генерирует случайное строение и, если это допустимо, делает шаг от ближайшего узла на основе свойства MaxConnectionDistance. После каждого расширения планировщик пытается соединиться между двумя деревьями с помощью нового расширения и ближайшего узла на противоположном дереве. Недопустимые строения или соединения, которые конфликтуют со окружением, не добавляются в дерево.
Для более зеленого поиска включение свойства EnableConnectHeuristic отключает предел на MaxConnectionDistance свойство при соединении двух деревьев.
Установка EnableConnectHueristic свойство к false ограничивает расстояние удлинения при соединении двух деревьев значением MaxConnectionDistance свойство.
Объект использует rigidBodyTree модель робота, чтобы сгенерировать случайные строения и промежуточные состояния между узлами. Объекты столкновения заданы в модели робота, чтобы подтвердить строения и проверить на столкновения со окружением или самим роботом.
Чтобы спланировать путь между начальными и целевыми строениями, используйте plan функция объекта. После планирования можно интерполировать состояния вдоль пути с помощью interpolate функция объекта. Чтобы попытаться сократить контур путем обрезки ребер, используйте shorten функция объекта.
Чтобы задать область для выборки положений end-effector около строения цели, создайте workspaceGoalRegion и задайте его как goalRegion вход plan функция объекта. Чтобы изменить вероятность дискретизации дополнительных строений цели, задайте свойство WorkspaceGoalRegionBias.
Для получения дополнительной информации о вычислительной сложности см. «Сложность планирования».
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 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
Задайте область цели в рабочей рабочей области и планируйте путь в этих границах. The workspaceGoalRegion объект задает границы XYZ-положения и ориентации ZYX Euler эффектора конца робота. The 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 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
Настройте положение 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
Комплексность планирования
При планировании движения между узлами в дереве генерируется и проверяется набор строений. Это время расчета плановика пропорционально количеству сгенерированных строений. Количество строений между узлами определяется отношением свойств 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.