Трассировка формы манипулятора в MATLAB и Simulink

В этом примере используются:

В этом примере показано, как отслеживать предопределенную форму 3-D в пространстве. Во многих применениях робототехники, таких как сварка, производство или контроль, можно использовать плавный и четкий путь. Траектория 3-D решается в пространстве задач для отслеживания мембраны MATLAB ® и выполняется с помощью робота Сойера из Rethink Robotics ®. Цель состоит в том, чтобы создать гладкий путь для конечного эффектора робота, чтобы следовать на основе установленных ППМ.

Пример трассировки формы манипулятора в MATLAB и Simulink показывает, как создать точно дискретизированный набор сегментов, которые затем могут быть переданы в решатель обратной кинематики для решения с помощью итеративного решения. Однако в этом примере предлагается альтернативный подход к снижению сложности вычислений. В этом примере сегменты контура разделяются на несколько дискретных точек и используются функции сглаживания для интерполяции между ППМ. Такой подход должен обеспечить более плавную траекторию и повысить эффективность выполнения.

Загрузить робота

В этом примере используется робот Sawyer компании Rethink Robotics ®. Импортируйте файл URDF, определяющий динамику жесткого тела. Установите DataFormat использование векторов столбцов для определения конфигураций роботов. Simulink ® использует векторы столбцов. Пределы пространства задач определяются на основе эмпирических данных.

sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';
taskSpaceLimits = [0.25 0.5; -0.125 0.125; -0.15 0.1];
numJoints = 8; % Number of joints in robot

Создание набора задач-пространственных ППМ

В этом примере цель состоит в том, чтобы получить набор сегментов пути, которые отслеживают логотип мембраны MATLAB ®. Поверхность мембраны и сегменты пути генерируются в виде матриц ячеек с помощью вспомогательной функции.generateMembranePaths. Чтобы визуализировать контуры, наложенные на поверхность, постройте график поверхности с помощью surf и сегменты пути путем итерации через массив ячеек сегментов пути. Вы можете увеличить numSamples для более тонкой выборки по поверхности.

numSamples = 7;
[pathSegments, surface] = generateMembranePaths(numSamples, taskSpaceLimits);

% Visualize the output
figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth', 2);
end
hold off

Figure contains an axes. The axes contains 7 objects of type surface, line.

Чтобы робот мог отслеживать выходные данные, визуализируйте форму в рабочем пространстве робота. Показать sawyer и постройте график отрезков на том же рисунке.

figure
show(sawyer);
hold all

for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth',2);
end

view(135,20)
axis([-1 1 -.5 .5 -1 .75])
hold off

Создание решателя обратной кинематики

Создание обратной кинематики (IK) с использованием загруженного sawyer дерево жесткого тела. Первоначально он конфигурируется с однородным набором весов, используя домашнюю конфигурацию в качестве начального предположения. Установите начальное предположение на домашнюю конфигурацию и допуски позы с однородными весами. Конечным эффектором для решателя IK является 'right_hand' тело робота.

ik = inverseKinematics('RigidBodyTree', sawyer);
initialGuess = sawyer.homeConfiguration;
weights = [1 1 1 1 1 1];
eeName = 'right_hand';

Преобразовать путевые точки пространства задачи в пространство соединения с использованием обратной кинематики

Используйте решатель обратной кинематики для генерации набора ППМ пространства соединения, которые дают конфигурации соединения для робота в каждой точке генерируемого pathSegments. Каждый сегмент соединения-пространства заносится в матрицу, jointPathSegmentMatrix, который передается модели Simulink в качестве входных данных.

% Initialize the output matrix
jointPathSegmentMatrix = zeros(length(pathSegments),numJoints,numSamples);

% Define the orientation so that the end effector is oriented down
sawyerOrientation = axang2rotm([0 1 0 pi]);

% Compute IK at each waypoint along each segment
for i = 1:length(pathSegments)
    currentTaskSpaceSegment = pathSegments{i};
    currentJointSegment = zeros(numJoints, length(currentTaskSpaceSegment));
    for j = 1:length(currentTaskSpaceSegment)
        pose = [sawyerOrientation currentTaskSpaceSegment(j,:)'; 0 0 0 1];
        currentJointSegment(:,j) = ik(eeName,pose,weights,initialGuess);
        initialGuess = currentJointSegment(:,j);
    end
    
    jointPathSegmentMatrix(i, :, :) = (currentJointSegment);
end

Загрузить модель Simulink

Используйте shapeTracingSawyer модель для выполнения траекторий и моделирования их на кинематической модели робота.

open_system("shapeTracingSawyer.slx")

Модель Simulink состоит из двух основных частей:

В разделе Формирование траектории (Trajectory Generation) берется матрица сегментов пути соединения-пространства, jointStartSegiveMatrix, и преобразуются сегменты в набор дискретизированных ППМ соединения-пространства (конфигурации соединения) на каждом этапе моделирования с использованием функционального блока MATLAB. Блок полиномиальной траектории (Polynomial Trajectory Block) преобразует набор конфигураций соединения в сглаженную траекторию B-сплайна пространства соединения во времени.
Раздел «Моделирование кинематики робота» (Robot Kinematics Simulation) принимает ППМ совместного пространства от сглаженной траектории и вычисляет результирующее положение конечного эффектора для робота.

Формирование траектории

Моделирование кинематики робота

Выполнение траекторий совместного пространства в Simulink

Смоделировать модель для выполнения генерируемых траекторий.

sim("shapeTracingSawyer.slx")

Просмотр результатов создания траектории

Модель выводит конфигурации соединения робота и положения концевого эффектора вдоль каждой сглаженной траектории пути. Для упрощения работы с инструментами печати MATLAB измените форму данных.

% End effector positions
xPositionsEE = reshape(eePosData.Data(1,:,:),1,size(eePosData.Data,3));
yPositionsEE = reshape(eePosData.Data(2,:,:),1,size(eePosData.Data,3));
zPositionsEE = reshape(eePosData.Data(3,:,:),1,size(eePosData.Data,3));

% Extract joint-space results
jointConfigurationData = reshape(jointPosData.Data, numJoints, size(eePosData.Data,3));

Постройте график новых положений концевого эффектора на исходной поверхности мембраны.

figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
plot3(xPositionsEE,yPositionsEE,zPositionsEE)
grid on
hold off

Figure contains an axes. The axes contains 2 objects of type surface, line.

Визуализация движения робота

В дополнение к описанной выше визуализации поведение трассировки можно воссоздать с помощью модели робота Сойера. Выполните итерацию через конфигурации соединений в jointConfigurationData для визуализации робота с помощью show и непрерывно строят график положения концевого эффектора в 3-D пространстве.

% For faster visualization, only display every few steps
vizStep = 5;

% Initialize a new figure window
figure
set(gcf,'Visible','on');

% Iterate through all joint configurations and end-effectort positions
for i = 1:vizStep:length(xPositionsEE)
    show(sawyer, jointConfigurationData(:,i),'Frames','off','PreservePlot',false);
    hold on
    plot3(xPositionsEE(1:i),yPositionsEE(1:i),zPositionsEE(1:i),'b','LineWidth',3)
    
    view(135,20)
    axis([-1 1 -.5 .5 -1 .75])
    
    drawnow
end
hold off

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