В этом примере Simulink показано, как блок обратной кинематики может управлять манипулятором по заданной траектории. Требуемая траектория определяется как ряд плотно разнесенных поз для концевого эффектора манипулятора. Генерация траекторий и определение ППМ представляют собой множество приложений робототехники, таких как управление захватом и размещением, расчет траекторий по профилям пространственного ускорения и скорости или даже имитация внешних наблюдений ключевых кадров с помощью камер и компьютерного зрения. Как только траектория сформирована, блок обратной кинематики используется для преобразования ее в траекторию совместного пространства, которая затем может использоваться для моделирования динамики манипулятора и контроллера.
Загрузите модель, чтобы увидеть, как она построена.
open_system('IKTrajectoryControlExample.slx');Модель состоит из четырех основных операций:
Формирование целевой позы
Обратная кинематика
Динамика манипулятора
Измерение позы
Эта диаграмма Stateflow выбирает, какой ППМ является текущей целью манипулятора. Диаграмма настраивает цель на следующий ППМ, как только манипулятор попадает в допуск текущей цели. Диаграмма также преобразует и компонует компоненты ППМ в однородное преобразование через eul2tform функция. Как только больше нет ППМ для выбора, диаграмма завершает моделирование.
Обратная кинематика рассчитала набор углов соединения для получения желаемой позы для концевого эффектора. Использовать обратную кинематику a rigidBodyTree и укажите целевую позу конечного эффекта в качестве однородного преобразования. Задайте ряд весов для зависимостей относительного допуска для положения и ориентации решения и дайте начальную оценку положений соединения. Блок выводит вектор положений соединения, которые создают требуемую позу из rigidBodyTree модель, указанная в параметрах блока. Для обеспечения плавной непрерывности решений в качестве начальной позиции решателя используется предыдущее конфигурационное решение. Это также уменьшает избыточность вычислений, если целевая поза не обновлялась с последнего временного шага моделирования.
Динамика манипулятора состоит из двух компонентов, контроллера для генерации сигналов крутящего момента и модели динамики для моделирования динамики манипулятора, заданной этими сигналами крутящего момента. Контроллер в данном примере использует компонент прямой передачи, вычисленный через обратную динамику манипулятора, и контроллер PD обратной связи для коррекции ошибки. Модель манипулятора использует блок Forward Dynamics, работающий с rigidBodyTree объект. Для более сложных методов динамики и визуализации рассмотрите возможность использования инструментов из блока Control Systems Toolbox™ и Simscape Multibody™ для замены блока Forward Dynamics.
Измерение позы берет показания угла соединения из модели манипулятора и преобразует их в однородную матрицу преобразования для использования в качестве обратной связи в разделе Выбор ППМ.
Манипулятором, используемым в этом примере, является робот-манипулятор Rethink 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 - массив ячеек, где каждая ячейка содержит массив, описывающий координаты пикселей для одной обнаруженной границы. Более полное представление о том, как генерировать эти данные, можно найти в примере «Трассировка границ в изображениях» (требуется панель инструментов обработки изображений).
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];
В этом примере ориентацию выбирают так, чтобы концевой эффектор всегда был перпендикулярен плоскости изображения.
Как только эта информация определена, каждый набор требуемых координат и углов Эйлера может быть скомпилирован в ППМ. Каждый ППМ представлен в виде шестиэлементного вектора, первые три элемента которого соответствуют требуемым позициям xyz манипулятора в мировом кадре. Последние три элемента соответствуют углам Эйлера ZYX нужной ориентации.
ППМ объединяются с образованием массива n-by-6, где n - общее число поз на траектории. Каждая строка в массиве соответствует ППМ на траектории.
% 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 монет. Для простоты и скорости меньшее подмножество монет можно проследить, ограничив общее количество переданных в ППМ. Ниже на изображении прослеживаются 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 показывает сверху вниз движения отслеживания манипулятора. Линии между окружностями возникают при переходе манипулятора от одного контура монеты к следующему.
График отслеживания ППМ отображает ход 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
