Этот пример Simulink демонстрирует, как блок Inverse Kinematics может управлять манипулятором вдоль заданной траектории. Желаемая траектория задана как серия плотно распределенных положений для исполнительного элемента конца манипулятора. Генерация траектории и waypoint определение представляют много приложений робототехники как выбор и операция места, вычисляя траектории от пространственного ускорения и скоростных профилей, или даже имитируя внешние наблюдения за ключевыми кадрами с помощью камер и компьютерного зрения. Если траектория сгенерирована, блок Inverse Kinematics используется, чтобы перевести это в объединенную пространственную траекторию, которая может затем использоваться, чтобы симулировать динамику манипулятора и контроллера.
Загрузите модель, чтобы видеть, как она создается.
open_system('IKTrajectoryControlExample.slx');Модель состоит из четырех первичных операций:
Целевая генерация положения
Обратная кинематика
Динамика манипулятора
Изложите измерение
Эта диаграмма Stateflow выбирает, какой waypoint является текущей целью для манипулятора. График корректирует цель к следующему waypoint, если манипулятор добирается до в допуске текущей цели. График также преобразует и собирает компоненты waypoint в однородное преобразование через eul2tform функция. Однажды больше нет waypoints, чтобы выбрать, график отключает симуляцию.
Инверсная кинематика вычислила набор углов поворота шарнира, чтобы произвести желаемое положение для исполнительного элемента конца. Используйте Обратный Kinematicswith rigidBodyTree модель и задает целевое положение эффекта конца как однородное преобразование. Задайте серию весов для ограничений относительной погрешности на положение и ориентации решения, и дайте первоначальную оценку объединенных положений. Блок выводит вектор из объединенных положений, которые производят желаемое положение из rigidBodyTree модель задана в параметрах блоков. Чтобы гарантировать сглаженную непрерывность решений, предыдущее решение для настройки используется в качестве стартовой позиции для решателя. Это также уменьшает сокращение вычислений, если целевое положение не обновилось начиная с последнего шага времени симуляции.
Динамика манипулятора состоит из двух компонентов, контроллер, чтобы сгенерировать сигналы крутящего момента и модель динамики, чтобы смоделировать динамику манипулятора, учитывая эти сигналы крутящего момента. Диспетчер в примере использует компонент прямого распространения, вычисленный через обратную динамику манипулятора и контроллера PD обратной связи, чтобы откорректировать для ошибки. Модель манипулятора использует блок Forward Dynamics, который работает с rigidBodyTree объект. Для более сложной динамики и методов визуализации, считайте инструменты использования от Систем управления библиотекой Toolbox™ и Simscape Multibody™, чтобы заменить блок Forward Dynamics.
Измерение положения снимает показания угла поворота шарнира из модели манипулятора и преобразует их в однородное, преобразовывают матрицу, которая будет использоваться в качестве обратной связи в разделе Waypoint Selection.
Манипулятор, используемый для этого примера, является Переосмыслением манипулятор робота Sawyer™. rigidBodyTree объект, который описывает манипулятор, импортируется из URDF (объединенный формат описания робота) файл с помощью importrobot.
% Import the manipulator as a rigidBodyTree Object sawyer = importrobot('sawyer.urdf'); sawyer.DataFormat = 'column'; % Define end-effector body name eeName = 'right_hand'; % Define the number of joints in the manipulator numJoints = 8; % Visualize the manipulator show(sawyer); xlim([-1.00 1.00]) ylim([-1.00 1.00]); zlim([-1.02 0.98]); view([128.88 10.45]);
В этом примере цель манипулятора состоит в том, чтобы смочь проследить контуры монет, обнаруженных в изображении, coins.png. Во-первых, изображение обрабатывается, чтобы найти контуры монет.
I = imread('coins.png'); bwBoundaries = imread('coinBoundaries.png'); figure subplot(1,2,1) imshow(I,'Border','tight') title('Original Image') subplot(1,2,2) imshow(bwBoundaries,'Border','tight') title('Processed Image with Boundary Detection')
После обработки изображений ребра монет извлечены как пиксельные местоположения. Данные загружаются в из MAT-файла, boundaryData. boundaries массив ячеек, где каждая ячейка содержит массив, описывающий пиксельные координаты для одного обнаруженного контура. Более полное представление о том, как сгенерировать эти данные, может быть найдено в примере, "Трассировка контура в Изображениях" (требует Image Processing Toolbox).
load boundaryData.mat boundaries whos boundaries
Name Size Bytes Class Attributes boundaries 10x1 25376 cell
Чтобы сопоставить эти данные с мировой системой координат, мы должны задать, где изображение расположено и масштабирование между пиксельными координатами и пространственными координатами.
% Image origin coordinates imageOrigin = [0.4,0.2,0.08]; % Scale factor to convert from pixels to physical distance scale = 0.0015;
Углы Эйлера для желаемой ориентации исполнительного элемента конца в каждой точке должны также быть заданы.
eeOrientation = [0, pi, 0];
В этом примере ориентация выбрана таким образом, что исполнительный элемент конца всегда перпендикулярен плоскости изображения.
Если эта информация задана, каждый набор желаемых координат и Углов Эйлера может быть скомпилирован в waypoint. Каждый waypoint представлен как вектор с шестью элементами, чей сначала три элемента соответствуют желаемым xyz-положениям манипулятора в мировой системе координат. Последние три элемента соответствуют Углам Эйлера ZYX желаемой ориентации.
waypoints конкатенированы, чтобы сформировать n-6 массив, где n является общим количеством положений в траектории. Каждая строка в массиве соответствует waypoint в траектории.
% Clear previous waypoints and begin building wayPoint array clear wayPoints % Start just above image origin waypt0 = [imageOrigin + [0 0 .2],eeOrientation]; % Touch the origin of the image waypt1 = [imageOrigin,eeOrientation]; % Interpolate each element for smooth motion to the origin of the image for i = 1:6 interp = linspace(waypt0(i),waypt1(i),100); wayPoints(:,i) = interp'; end
Всего, существует 10 монет. Для simiplicity и скорости, меньшее подмножество монет может быть прослежено путем ограничения общего количества, переданного waypoints. Ниже, numTraces = 3 монеты прослеживаются в изображении.
% Define the number of coins to trace numTraces = 3; % Assemble the waypoints for boundary tracing for i = 1:min(numTraces, size(boundaries,1)) %Select a boundary and map to physical size segment = boundaries{i}*scale; % Pad data for approach waypoint and lift waypoint between boundaries segment = [segment(1,:); segment(:,:); segment(end,:)]; % Z-offset for moving between boundaries segment(1,3) = .02; segment(end,3) = .02; % Translate to origin of image cartesianCoord = imageOrigin + segment; % Repeat desired orientation to match the number of waypoints being added eulerAngles = repmat(eeOrientation,size(segment,1),1); % Append data to end of previous wayPoints wayPoints = [wayPoints; cartesianCoord, eulerAngles]; end
Этот массив является первичным входным параметром к модели.
Несколько параметров должны быть инициализированы, прежде чем модель может быть запущена.
% Initialize size of q0, the robot joint configuration at t=0. This will % later be replaced by the first waypoint. q0 = zeros(numJoints,1); % Define a sampling rate for the simulation. Ts = .01; % Define a [1x6] vector of relative weights on the orientation and % position error for the inverse kinematics solver. weights = ones(1,6); % Transform the first waypoint to a Homogenous Transform Matrix for initialization initTargetPose = eul2tform(wayPoints(1,4:6)); initTargetPose(1:3,end) = wayPoints(1,1:3)'; % Solve for q0 such that the manipulator begins at the first waypoint ik = inverseKinematics('RigidBodyTree',sawyer); [q0,solInfo] = ik(eeName,initTargetPose,weights,q0);
Чтобы симулировать модель, используйте sim команда. Модель генерирует выходной набор данных, jointData и показывает прогресс двух графиков:
X Графиков Y показывают нисходящее представление движений трассировки манипулятора. Линии между кругами происходят как переходы манипулятора от одной схемы монеты до следующего.
Waypoint, Отслеживающий график, визуализирует прогресс 3D. Зеленая точка указывает на целевое положение. Красная точка указывает на фактическое положение исполнительного элемента конца, достигнутое исполнительным элементом конца с помощью управления с обратной связью.
% Close currently open figures close all % Open & simulate the model open_system('IKTrajectoryControlExample.slx'); sim('IKTrajectoryControlExample.slx');
Выходные параметры модели два набора данных, которые могут использоваться для визуализации после симуляции. Объединенные настройки обеспечиваются как jointData. Положения исполнительного элемента конца робота выводятся как poseData.
% Remove unnecessary meshes for faster visualization clearMeshes(sawyer); % Data for mapping image [m,n] = size(I); [X,Y] = meshgrid(0:m,0:n); X = imageOrigin(1) + X*scale; Y = imageOrigin(2) + Y*scale; Z = zeros(size(X)); Z = Z + imageOrigin(3); % Close all open figures close all % Initialize a new figure window figure; set(gcf,'Visible','on'); % Plot the initial robot position show(sawyer, jointData(1,:)'); hold on % Initialize end effector plot position p = plot3(0,0,0,'.'); warp(X,Y,Z,I'); % Change view angle and axis view(65,45) axis([-.25 1 -.25 .75 0 0.75]) % Iterate through the outputs at 10-sample intervals to visualize the results for j = 1:10:length(jointData) % Display manipulator model show(sawyer,jointData(j,:)', 'Frames', 'off', 'PreservePlot', false); % Get end effector position from homoegenous transform output pos = poseData(1:3,4,j); % Update end effector position for plot p.XData = [p.XData pos(1)]; p.YData = [p.YData pos(2)]; p.ZData = [p.ZData pos(3)]; % Update figure drawnow end
