Прокрутка кинематики на пятизвеннике робота

В этом разделе показано, как выполнить анализ FK для вычисления рабочей области без сингулярности для пера, которое соответствует набору углов двигателя. В частности, пример присваивает активные углы поворота шарнира робота как целевые переменные, а координаты x и y пера как выходные переменные. Пример также присваивает пассивные соединения и ориентацию пера в качестве начальных переменных предположения, которые используются, чтобы помочь смещать решения к +- режим.

1. Чтобы выполнить анализ FK, сначала загрузите модель пятизвенника робота в память и создайте KinematicsSolver объект (fk) для модели.

mdl = 'sm_five_bar_robot';
load_system(mdl);
fk = simscape.multibody.KinematicsSolver(mdl);

2. Чтобы задать и изучить отношения между пером, мировой системой координат, активными шарнирами и пассивными шарнирами робота, необходимо создать переменные системы координат, которые соответствуют строению пера относительно мировой системы координат. Обратите внимание, что вам не нужно создавать переменные соединений, потому что они создаются автоматически при построении объекта.

Добавьте группу переменных поступательной и вращательной системы координат для пера в fk. Укажите лабораторную систему координат в качестве основы, а конечную систему координат в качестве последующей системы координат. Назовите группу переменных системы координат Pen. Объект имеет шесть переменных системы координат: три поступательные и три вращательные.

base = mdl + "/World Frame/W";
follower = mdl + "/Five Bar Robot/Pen/Pen/Tip";
addFrameVariables(fk,'Pen','Translation', base, follower);
addFrameVariables(fk,'Pen','Rotation', base, follower);

3. Присвойте основанные на положении переменные соединений активных соединений (j5.Rz.q и j4.Rz.q) как цели. Чтобы найти идентификаторы переменных соединений на основе положения, используйте jointPositionVariables функция объекта.

jointPositionVariables(fk)

ans=5×4 table
       ID           JointType                                  BlockPath                               Unit 
    _________    ________________    ______________________________________________________________    _____

    "j1.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_c"                "deg"
    "j2.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_d"                "deg"
    "j3.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_e"                "deg"
    "j4.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_a/Active_Joint_a"    "deg"
    "j5.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_b/Active_Joint_b"    "deg"

targetIDs = ["j5.Rz.q";"j4.Rz.q"];
addTargetVariables(fk,targetIDs);

4. Назначьте эти переменные в качестве выходов:

Чтобы найти идентификаторы переменных системы координат, используйте frameVariables функция объекта.

frameVariables(fk)

ans=6×4 table
            ID                           Base                                      Follower                       Unit 
    ___________________    _________________________________    ______________________________________________    _____

    "Pen.Translation.x"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.y"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.z"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Rotation.x"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.y"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.z"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"

outputIDs = ["Pen.Translation.x";"Pen.Translation.y";"j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addOutputVariables(fk,outputIDs);

5. Присвойте переменные соединений пассивных соединений (j1.Rz.q и j2.Rz.q) и вращательный z компонента пера (Pen.Rotation.z) как догадывается.

guessIDs = ["j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addInitialGuessVariables(fk,guessIDs);

6. Изображение представляет собой эскиз пятизвенника робота. Углы активных соединений ${\theta }_{\mathit{a}}$ и ${\theta }_{\mathit{b}}$, и пассивные соединения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$. Использование вложенного for цикл для вычисления рабочей области пера для набора активных углов поворота шарнира, которые варьируются между [95, 150] и [30, 85] степенями, соответственно. Чтобы убедиться, что робот находится в +- mode, используйте [50 -50 -110] степени в качестве начальных значений для переменных guess (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z) и затем используйте последние значения переменных guess, чтобы направить следующее решение FK.

guesses = [50 -50 -110]; % Initial guesses for theta_c, theta_d, and rotational z componenet of the pen
index = 1;

% Generate angles for theta_a and theta_b
theta_a = linspace(95,150,31);
theta_b = linspace(30,85,31);

% Allocate memory for output data
fk_data = zeros(length(theta_a)*length(theta_b),4);

% Calculate the robot's workspace for the given theta_a and theta_b
for i = 1:length(theta_a)
    for j = 1:length(theta_b)
        targets = [theta_b(j),theta_a(i)];
        [outputVec,statusFlag] = solve(fk,targets,guesses);  
        if statusFlag == 1 % Save a solution if its statusFlag is 1
            fk_data(index,1:4) = [outputVec(1),outputVec(2),outputVec(3),outputVec(4)];
            index = index + 1;
            guesses = [outputVec(3),outputVec(4),outputVec(5)]; % Update next guess with last solution
        end
    end
end

7. Постройте график углов пассивных соединений, чтобы убедиться, что пятизвенник робота в +- режим. Если робот находится в +- режим, значения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$ стабильно положительны и отрицательны, соответственно.

figure
plot(fk_data(1:index-1,3),'b','LineWidth',2); % Note that the total number of the solutions that satisfy statusFlag equals to index-1;
hold on
plot(fk_data(1:index-1,4),'k','LineWidth',2);
hold off


title('Passive Joint Angles')
xlabel('Index')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')

График показывает, что пятизвенник робот находится в +- режим. Более того, вычисленная рабочая область свободна от сингулярности, потому что значения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$ находятся вдали от нуля.

8. Постройте график данных рабочей области на изображении пятизвенника робота в виде сверху.

% Plot the top-view image of the five-bar robot
figure  
x = [-1 1];
y = [1 -1];
C = imread('top_view_five_bar_robot.png');
image(x,y,C)

hold on

% Plot the calculated workspace as blue dots
plot(fk_data(:,1),fk_data(:,2),'b.')
ylabel('y (m)')
xlabel('x (m)')
axis equal
axis([-1 1 -1 1]);
hold off

Запуск обратной кинематики на пятизвеннике робота

В этом разделе пример показывает, как выполнить IK-анализ, чтобы вычислить углы двигателя, которые соответствуют конечной эффекторной траектории. В частности, пример присваивает поступательные компоненты x и y пера как целевые переменные и присваивает углы двигателя как выходные переменные. Затем в примере вычисляются углы двигателя для траектории пера. Чтобы убедиться, что робот находится в +- режим, используйте пассивные соединения и ориентацию пера как догадки для смещения решений IK. Обратите внимание, что перед запуском этого раздела необходимо запустить раздел FK.

1. Задайте круговую конечную эффекторную траекторию в вычисленной ранее рабочей области. Сохраните координаты окружности для анализа IK.

figure
% Plot the calculated workspace as blue dots on the top-view image of the five-bar robot
image(x,y,C)
hold on
plot(fk_data(:,1),fk_data(:,2),'b.')

% Specify the center and radius of the circle
center_x = 0; % m
center_y = 0.3; % m
radius = 0.15; % m
th = 0:pi/50:2*pi; % radians

% Save the x and y coordinates of the circle for the IK analyses
coordinates_x = radius * cos(th) + center_x;
coordinates_y = radius * sin(th) + center_y;

% Plot the circle
plot(coordinates_x,coordinates_y,'r','LineWidth',2)
axis equal
axis([-1 1 -1 1]);
ylabel('Length (m)')
xlabel('Width (m)')
hold off

2. Чтобы выполнить анализ IK, сначала создайте KinematicsSolver объект (ik) для модели.

ik = simscape.multibody.KinematicsSolver(mdl);

3. Чтобы сформулировать анализ IK, создайте переменные системы координат для строения для пера относительно мировой системы координат и добавьте их к объекту (ik). Можно повторно использовать базовую и последующую переменные из раздела FK.

addFrameVariables(ik, 'Pen','Translation', base, follower);
addFrameVariables(ik, 'Pen','Rotation', base, follower);

4. Присвойте переменные, которые соответствуют поступательным x и y компонентам пера (Pen.Translation.x и Pen.Translation.y) как цели. Чтобы найти идентификаторы переменных системы координат, используйте frameVariables функция объекта.

frameVariables(ik)

ans=6×4 table
            ID                           Base                                      Follower                       Unit 
    ___________________    _________________________________    ______________________________________________    _____

    "Pen.Translation.x"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.y"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.z"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Rotation.x"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.y"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.z"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"

targetIds = ["Pen.Translation.x";"Pen.Translation.y"];
addTargetVariables(ik,targetIds);

5. Назначьте эти переменные в качестве выходов:

Чтобы найти идентификаторы переменных соединений на основе положения, используйте jointPositionVariables функция объекта.

jointPositionVariables(ik)

ans=5×4 table
       ID           JointType                                  BlockPath                               Unit 
    _________    ________________    ______________________________________________________________    _____

    "j1.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_c"                "deg"
    "j2.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_d"                "deg"
    "j3.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_e"                "deg"
    "j4.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_a/Active_Joint_a"    "deg"
    "j5.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_b/Active_Joint_b"    "deg"

outputIds = ["j5.Rz.q";"j4.Rz.q";"j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addOutputVariables(ik,outputIds);

6. Использование for цикл для реализации анализа IK. В частности, используйте for цикл, чтобы назначить координаты предопределенной траектории перу и вычислить соответствующие углы двигателя.

N = length(coordinates_x); % Number of points on trajectory
index_in_fk = 1;
fk_data = zeros(N,5); % Allocate memory for the variable

for ind = 1:N
    targets = [coordinates_x(ind), coordinates_y(ind)]; % Use the x and y coordinates of the predefined circle for the inverse kinematics problem
    [outputVec_ik,statusFlag_ik] = solve(ik,targets);
    if statusFlag_ik == 1 
        viewSolution(ik) % View the solution in the Kinematics Solver Viewer
        fk_data(index_in_fk,1:5) = outputVec_ik; % save the output data
        index_in_fk = index_in_fk+1;   
    else
        error('Did not hit targets');
    end
end
closeViewer(ik);

7. Постройте график пассивных соединений робота, чтобы увидеть, находится ли робот в +- режим.

% Plot the angles of the passive joints
figure
plot(fk_data(:,3),'b','LineWidth',2);
hold on
plot(fk_data(:,4),'k','LineWidth',2);
hold off
title('Passive Joint Angles')
xlabel('Point')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')

График показывает, что робот не в +- режим, поскольку значения обоих пассивных соединений положительны.

Во время анализа IK откроется Kinematics Solver Viewer и покажет анимацию процесса. Анимация также показывает, что результат не желателен, потому что пятизвенник робота не в +- режим. На изображении показан снимок анимации.

8. Чтобы помочь гарантировать, что все решения остаются в режиме + -, присвойте переменные соединений пассивных соединений (j1.Rz.q и j2.Rz.q) и переменной вращательного z компонента пера (Pen.Rotation.z) как догадки для смещения решений IK.

guessIds = ["j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addInitialGuessVariables(ik,guessIds);

Используйте [50 -50 -110] степени в качестве начальных значений для переменных guess (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z) и используйте последние значения переменных guess, чтобы направить следующее решение IK.

guesses = [50 -50 -110]; % Assign initial guesses for passive joints and z rotation component of the pen
N = length(coordinates_x);
index_in_fk = 1;
fk_data = zeros(N,5); % Allocate memory for the variable
for ind = 1:N
    targets = [coordinates_x(ind), coordinates_y(ind)]; % Use the x and y coordinates of the prescribed circle for the inverse kinematics problem
    [outputVec_ik,statusFlag_ik] = solve(ik,targets, guesses);
    if statusFlag_ik == 1 
        viewSolution(ik) % View the solution in the Kinematics Solver Viewer
        fk_data(index_in_fk,1:5) = outputVec_ik; % Save the rotations of the motors
        index_in_fk=index_in_fk+1;
        guesses = [outputVec_ik(3) outputVec_ik(4) outputVec_ik(5)]; % Update next guess with last solution
    else
        error('Did not hit targets');
    end
end
closeViewer(ik);

График указывает, что пассивные углы поворота шарнира всегда хорошо удалены от нуля и всегда находятся в +- режим. Поэтому этот набор углов двигателя соответствует концевому эффектору, перемещающемуся плавным движением по круговой траектории, не проходя через какие-либо кинематические особенности.

% Plot the angles of the passive joints
figure
plot(fk_data(:,3),'b','LineWidth',2);
hold on
plot(fk_data(:,4),'k','LineWidth',2);
hold off
title('Passive Joint Angles')
xlabel('Index')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')