Обзор модели

Загрузите модель, чтобы увидеть, как она построена.

open_system('IKTrajectoryControlExample.slx');

Модель состоит из четырех основных операций:

Генерация целевого положения

Эта диаграмма Stateflow выбирает, какая точка пути является текущей целью для манипулятора. График настраивает цель на следующую путевую точку, когда манипулятор попадает в допуск к текущей цели. График также преобразует и собирает компоненты путевой точки в гомогенное преобразование через eul2tform функция. Если больше нет путевых точек для выбора, график завершает симуляцию.

Обратная кинематика

Обратная кинематика вычисляла набор углов поворота шарнира, чтобы создать желаемое положение для концевого эффектора. Используйте Обратную Кинематику rigidBodyTree модели и определения целевого положения эффекта разрыва как гомогенного преобразования. Задайте серию весов для относительных погрешностей на положение и ориентацию решения и дайте начальную оценку положений соединений. Блок выводит вектор положений соединений, которые производят требуемое положение от rigidBodyTree модель, заданная в параметрах блоков. Чтобы гарантировать плавность непрерывности решений, предыдущее строение решение используется в качестве стартового положения для решателя. Это также уменьшает избыточность вычислений, если целевое положение не обновлялось с момента последнего временного шага симуляции.

Динамика манипулятора

Динамика манипулятора состоит из двух компонентов, контроллера для генерации сигналов крутящего момента и динамической модели для моделирования динамики манипулятора с учетом этих сигналов крутящего момента. Контроллер в примере использует компонент feedforward, вычисленный через обратную динамику манипулятора, и ПД-контроллер обратной связи, чтобы исправить ошибку. Модель манипулятора использует блок Forward Dynamics, который работает со rigidBodyTree объект. Для более сложной динамики и методов визуализации рассмотрите использование инструментов из Control Systems Toolbox™ blockset и Simscape Multibody™, чтобы заменить блок Forward Dynamics.

Измерение положения

Измерение положения берёт показания угла поворота шарнира из модели манипулятора и преобразует их в матрицу гомогенного преобразования, которая используется в качестве обратной связи в разделе Waypoint Selection.

Определение манипулятора

Манипулятор, используемый в этом примере, является манипулятором робота Rethink Sawyer™. The 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];

В этом примере ориентация выбирается таким образом, чтобы концевой эффектор всегда был перпендикулярен плоскости изображения.

Если эта информация задана, каждый набор желаемых координат и углы Эйлера могут быть скомпилированы в путевую точку. Каждая путевая точка представлена как вектор с шестью элементами, первые три элемента которого соответствуют желаемым xyz-положениям манипулятора в мировой системе координат. Последние три элемента соответствуют углам ZYX Эйлера требуемой ориентации.

Путевые точки сцеплены, чтобы сформировать массив n на 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

Этот массив является первичным входным параметром в модель.

Setup модели

Перед запуском модели необходимо инициализировать несколько параметров.

% 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 и показывает прогресс на двух графиках:

% 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