Поступательная кинематика на пятипланочном роботе

В этом разделе показано, как выполнить 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. Изображение представляет собой эскиз пятипланкового робота. Углами активных соединений являются ${}_{\mathit{\lambda \; а}}$ и (${}_{\mathit{b)}}$, а пассивных соединений являются (${}_{\mathit{c}}$) и (${}_{\mathit{d}}$). Использование вложенного for цикл для вычисления рабочего пространства пера для набора активных углов соединения, которые варьируются между [95, 150] и [30, 85] градусами соответственно. Чтобы убедиться, что робот находится в +- режим, используйте [50 -50 -110] градусов в качестве начальных значений для переменных приближения (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z), а затем используйте последние значения переменных приближения, чтобы направить следующее решение 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. Постройте график углов пассивных соединений, чтобы проверить, находится ли пятипланковый робот в +- режим. Если робот находится в +- При этом, в зависимости от режима, значения, выраженные («${}_{\mathit{с}}$») и («${}_{\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')

Сюжет показывает, что пятибарковый робот находится в +- режим. Кроме того, вычисляемая рабочая область не имеет сингулярности, так как значения ${}_{\mathit{startc}}$ и ${}_{\mathit{startd}}$ удалены от нуля.

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 открывается средство просмотра решателя кинематики, которое показывает анимацию процесса. Анимация также показывает, что результат не желателен, поскольку пятипланковый робот отсутствует в +- режим. На изображении показан снимок анимации.

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] градусов в качестве начальных значений для переменных приближения (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z) и используйте последние значения переменных приближения для управления следующим решением 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')