Моделирование управления траекторией с инверсной кинематикой

Этот пример использует:

Этот пример 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]);

Генерация Waypoint

В этом примере цель манипулятора состоит в том, чтобы смочь проследить контуры монет, обнаруженных в изображении, 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')

Figure contains 2 axes objects. Axes object 1 with title Original Image contains an object of type image. Axes object 2 with title Processed Image with Boundary Detection contains an object of type image.

После обработки изображений ребра монет извлечены как пиксельные местоположения. Данные загружаются в из 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 желаемой ориентации.

$Waypoint = [\begin{matrix} X & Y & Z & ϕ_{z} & ϕ_{y} & ϕ_{x} \end{matrix}]$

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

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

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

X Графиков Y показывают нисходящее представление движений трассировки манипулятора. Линии между кругами происходят как переходы манипулятора от одной схемы монеты до следующего.
Waypoint, Отслеживающий график, визуализирует прогресс 3D. Зеленая точка указывает на целевое положение. Красная точка указывает на фактическое положение исполнительного элемента конца, достигнутое исполнительным элементом конца с помощью управления с обратной связью.

% Close currently open figures 
close all

% Open & simulate the model
open_system('IKTrajectoryControlExample.slx');
sim('IKTrajectoryControlExample.slx');

Figure contains 2 axes objects. Axes object 1 with title Waypoint Tracking is empty. Axes object 2 contains 3 objects of type line.

Визуализируйте результаты

Выходные параметры модели два набора данных, которые могут использоваться для визуализации после симуляции. Объединенные настройки обеспечиваются как 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

Документация