Выведите и примените обратную кинематику к двухзвеннику руки робота

Этот пример выводит и применяет обратную кинематику к двухзвеннику робота при помощи MATLAB ® и Symbolic Math Toolbox™.

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

Шаг 1: Задайте геометрические параметры

Задайте длины ссылок, углы поворота шарнира и положения конечных эффекторов роботов как символьные переменные.

syms L_1 L_2 theta_1 theta_2 XE YE

Задайте значения для длин ссылки робота.

L1 = 1;
L2 = 0.5;

Шаг 2: Задайте координаты X и Y End Effector

Задайте координаты X и Y end-effector как функцию от углов поворота шарнира (θ1,θ2).

XE_RHS = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2)
XE_RHS = L2cos(θ1+θ2)+L1cos(θ1)L_2*cos (theta_1 + theta_2) + L_1*cos (theta_1)
YE_RHS = L_1*sin(theta_1) + L_2*sin(theta_1+theta_2)
YE_RHS = L2sin(θ1+θ2)+L1sin(θ1)L_2*sin (theta_1 + theta_2) + L_1*sin (theta_1)

Преобразуйте символические выражения в функции MATLAB.

XE_MLF = matlabFunction(XE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]);
YE_MLF = matlabFunction(YE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]);

Шаг 3: Вычислите и визуализируйте кинематику вперед

Прямая кинематика преобразует углы поворота шарнира в положения end-effector: (θ1,θ2)f(θ1,θ2)(XE,YE). Учитывая конкретные значения угла поворота шарнира, используйте кинематику вперед, чтобы вычислить местоположения конечного эффектора.

Задайте входные значения углов поворота шарнира как 0<θ1<90 и -180<θ2<180.

t1_degs_row = linspace(0,90,100);
t2_degs_row = linspace(-180,180,100);
[tt1_degs,tt2_degs] = meshgrid(t1_degs_row,t2_degs_row);

Преобразуйте угол модулей из степеней в радианы.

tt1_rads = deg2rad(tt1_degs);
tt2_rads = deg2rad(tt2_degs);

Вычислите координаты X и Y с помощью функций MATLAB XE_MLF и YE_MLF, соответственно.

X_mat = XE_MLF(L1,L2,tt1_rads,tt2_rads);
Y_mat = YE_MLF(L1,L2,tt1_rads,tt2_rads);

Визуализируйте координаты X и Y с помощью вспомогательной функции plot_XY_given_theta_2dof.

plot_XY_given_theta_2dof(tt1_degs,tt2_degs,X_mat,Y_mat,(L1+L2))

Figure contains 2 axes. Axes 1 with title X_E contains an object of type contour. Axes 2 with title Y_E contains an object of type contour.

Шаг 4: Найти обратную кинематику

Обратная кинематика преобразует положения end-effector в углы поворота шарнира: (XE,YE)g(XE,YE)(θ1,θ2). Найдите обратную кинематику из уравнений прямой кинематики.

Задайте уравнения прямой кинематики.

XE_EQ = XE == XE_RHS;
YE_EQ = YE == YE_RHS;

Решить для θ1 и θ2.

S = solve([XE_EQ YE_EQ], [theta_1 theta_2])
S = struct with fields:
    theta_1: [2x1 sym]
    theta_2: [2x1 sym]

Структура S представляет несколько решений для θ1 и θ2. Покажите пару решений для θ1.

simplify(S.theta_1)
ans = 

(2atan(2L1YE+σ1L12+2L1XE-L22+XE2+YE2)2atan(2L1YE-σ1L12+2L1XE-L22+XE2+YE2))where  σ1=-L14+2L12L22+2L12XE2+2L12YE2-L24+2L22XE2+2L22YE2-XE4-2XE2YE2-YE4[2*atan ((2*L_1*YE + sqrt (-L_1^4 + 2*L_1^2*L_2^2 + 2*L_1^2*XE^2 + 2*L_1^2*YE^2 - L_2^4 + 2*L_2^2*XE^2 + 2*L_2^2*YE^2 - XE^4 - 2*XE^2*YE^2 - YE^4)) / (L_1^2 + 2*L_1*XE - L_2^2 + XE^2 + YE^2)); 2*atan ((2*L_1*YE - sqrt (-L_1^4 + 2*L_1^2*L_2^2 + 2*L_1^2*XE^2 + 2*L_1^2*YE^2 - L_2^4 + 2*L_2^2*XE^2 + 2*L_2^2*YE^2 - XE^4 - 2*XE^2*YE^2 - YE^4)) / (L_1^2 + 2*L_1*XE - L_2^2 + XE^2 + YE^2))]

Покажите пару решений для θ2.

simplify(S.theta_2)
ans = 

(-σ1σ1)where  σ1=2atan(-L12+2L1L2-L22+XE2+YE2L12+2L1L2+L22-XE2-YE2-L12+2L1L2-L22+XE2+YE2)[-2 * atan (sqrt ((- L_1^2 + 2 * L _ 1 * L _ 2 - L_2^2 + XE ^ 2 + YE ^ 2) * (L_1^2 + 2 * L _ 1 * L _ 2 + L_2^2 - XE ^ 2 - YE ^ 2) )/( - L_1^2 + 2 * L _ 1 * L _ 2 - L_2^2 + Xe 2 * atan (sqrt ((- L_1^2 + 2 * L _ 1 * L _ 2 - L_2^2 + XE ^ 2 + YE ^ 2) * (L_1^2 + 2 * L _ 1 * L _ 2 + L_2^2 - XE ^ 2 - YE ^ 2) )/( - L_1^2 + 2 * L _ 1 * L _ 2 - L_2^2 + XE ^

Преобразуйте решения в функции MATLAB, которые можно использовать позже. Функции TH1_MLF и TH2_MLF представление обратной кинематики.

TH1_MLF{1} = matlabFunction(S.theta_1(1),'Vars',[L_1 L_2 XE YE]);
TH1_MLF{2} = matlabFunction(S.theta_1(2),'Vars',[L_1 L_2 XE YE]);
TH2_MLF{1} = matlabFunction(S.theta_2(1),'Vars',[L_1 L_2 XE YE]);
TH2_MLF{2} = matlabFunction(S.theta_2(2),'Vars',[L_1 L_2 XE YE]);

Шаг 5: Вычисление и визуализация обратной кинематики

Используйте обратную кинематику, чтобы вычислить θ1 и θ2 от координат X и Y.

Задайте точки сетки координат X и Y.

[xmat,ymat] = meshgrid(0:0.01:1.5,0:0.01:1.5);

Вычислите углы θ1 и θ2 использование функций MATLAB TH1_MLF{1} и TH2_MLF{1}, соответственно.

tmp_th1_mat = TH1_MLF{1}(L1,L2,xmat,ymat);
tmp_th2_mat = TH2_MLF{1}(L1,L2,xmat,ymat);

Преобразуйте угол модулей из радианов в степени.

tmp_th1_mat = rad2deg(tmp_th1_mat);
tmp_th2_mat = rad2deg(tmp_th2_mat);

Некоторые входные координаты, такие как (X, Y) = (1.5,1.5), находятся за пределами достижимой рабочей области конечного эффектора. Решения обратной кинематики могут сгенерировать некоторые мнимые значения theta, которые требуют коррекции. Исправьте мнимые значения theta.

th1_mat = NaN(size(tmp_th1_mat));
th2_mat = NaN(size(tmp_th2_mat));

tf_mat = imag(tmp_th1_mat) == 0;
th1_mat(tf_mat) = real(tmp_th1_mat(tf_mat));

tf_mat = imag(tmp_th2_mat) == 0;
th2_mat(tf_mat) = real(tmp_th2_mat(tf_mat));

Визуализируйте углы θ1 и θ2 использование функции helper plot_theta_given_XY_2dof.

plot_theta_given_XY_2dof(xmat,ymat,th1_mat,th2_mat)

Figure contains 2 axes. Axes 1 with title \theta_1 contains an object of type contour. Axes 2 with title \theta_2 contains an object of type contour.

Шаг 6: Вычислите систему якобиан

Определение системы якобиан:

J=d(X,Y)d(θ1,θ2)=(dXdθ1dXdθ2dYdθ1dYdθ2)

the_J = jacobian([XE_RHS YE_RHS],[theta_1 theta_2])
the_J = 

(-L2sin(θ1+θ2)-L1sin(θ1)-L2sin(θ1+θ2)L2cos(θ1+θ2)+L1cos(θ1)L2cos(θ1+θ2))[- L_2*sin (theta_1 + theta_2) - L_1*sin (theta_1), -L_2*sin (theta_1 + theta_2); L_2*cos (theta_1 + theta_2) + L_1*cos (theta_1), L_2*cos (theta_1 + theta_2)]

Можно связать скорость соединения со скоростью конечного эффектора, и наоборот, используя систему Jacobian:

(dXdtdYdt)=(dXdθ1dXdθ2dYdθ1dYdθ2).(dθ1dtdθ2dt)

(dXdtdYdt)=J.(dθ1dtdθ2dt)

(dθ1dtdθ2dt)=J+.(dXdtdYdt)whereJ+является ли Мур-Псевдоинверс Пенроуза   J

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

Вспомогательные функции

function plot_theta_given_XY_2dof(X_mat,Y_mat,theta_1_mat_degs,...
                                  theta_2_mat_degs)

xlab_str = 'X (m)';
ylab_str = 'Y (m)';

figure;
hax(1) = subplot(1,2,1);
   contourf(X_mat, Y_mat, theta_1_mat_degs);
      caxis(hax(1), [-180 180]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(1), '\theta_1', 'Interpreter', 'tex')
      axis('equal')
hax(2) = subplot(1,2,2);
   contourf(X_mat, Y_mat, theta_2_mat_degs);
      caxis(hax(2), [-180 180]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(2), '\theta_2', 'Interpreter', 'tex')
      axis('equal')

end


function plot_XY_given_theta_2dof(theta_1_mat_degs,theta_2_mat_degs,...
                                  X_mat,Y_mat,a_cmax)
                              
xlab_str = '\theta_1 (degs)';
ylab_str = '\theta_2 (degs)';

figure;
hax(1) = subplot(1,2,1);
   contourf(theta_1_mat_degs, theta_2_mat_degs, X_mat);
      caxis(hax(1), [0 a_cmax]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(1), 'X_E', 'Interpreter', 'tex')
hax(2) = subplot(1,2,2);
   contourf(theta_1_mat_degs, theta_2_mat_degs, Y_mat); 
      caxis(hax(1), [0 a_cmax]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(2), 'Y_E', 'Interpreter', 'tex')

end
Для просмотра документации необходимо авторизоваться на сайте