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

Открыть сценарий в реальном времени

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

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

Шаг 1: Определение геометрических параметров

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

syms L_1 L_2 theta_1 theta_2 XE YE

Укажите значения длин звеньев робота.

L1 = 1;
L2 = 0.5;

Шаг 2: Определение координат X и Y концевого эффектора

Определите координаты X и Y концевого эффектора в зависимости от углов стыка $_{}_{}$ (

XE_RHS = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2)

XE_RHS =  $L_{2} \cos (θ_{1} + θ_{2}) + L_{1} \cos (θ_{1})$

YE_RHS = L_1*sin(theta_1) + L_2*sin(theta_1+theta_2)

YE_RHS =  $L_{2} \sin (θ_{1} + θ_{2}) + L_{1} \sin (θ_{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. Расчет и визуализация кинематики вперед

Передовая кинематика преобразовывает совместные углы в местоположения концевого захвата: $(_{θ1},_{θ2}) ⟶f_{(} {θ1}_{,} θ2)_{⟶} (_{} КСЕНОН$ , ВЫ). Учитывая конкретные значения угла соединения, используйте прямую кинематику для вычисления конечных местоположений эффектора.

Задайте входные значения углов соединения как $^{}_{}^{0\circ<θ1<90\circ}$ и $-^{}_{}^{}$ 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: Поиск обратной кинематики

Обратная кинематика преобразует положения концевых эффекторов в углы стыка: $(_{XE},_{} YE) {⟶g}_{} (_{XE}, {YE}_{)} ⟶_{}$ ( Найдите обратную кинематику из уравнений прямой кинематики.

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

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

Решите проблемы для $_{"" "" "" "" "" ""}$ $_{""}$ "

S = solve([XE_EQ YE_EQ], [theta_1 theta_2])

S = struct with fields:
    theta_1: [2x1 sym]
    theta_2: [2x1 sym]

Структура S представляет собой множество решений, используемых для ("") $_{и}$ $_{("" "" "" "" "" "" ""}$ " Покажите пару решений для $_{start1}$ .

simplify(S.theta_1)

ans = 
 $\begin{array}{l} (\begin{array}{c} 2 atan (\frac{2 L_{1} YE + σ_{1}}{{L_{1}}^{2} + 2 L_{1} XE - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}}) \\ 2 atan (\frac{2 L_{1} YE - σ_{1}}{{L_{1}}^{2} + 2 L_{1} XE - {L_{2}}^{2} + {XE}^{2} + {YE}^{2}}) \end{array}) \\ where \\ σ_{1} = \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}} \end{array}$

Покажите пару решений для $_{start2}$ .

simplify(S.theta_2)

ans = 
 $\begin{array}{l} (\begin{array}{c} - σ_{1} \\ σ_{1} \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\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} + {YE}^{2}}) \end{array}$

Преобразуйте решения в функции 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. Расчет и визуализация обратной кинематики

Используйте обратную кинематику, чтобы вычислить $_{по}$ $_{}$ координатам X и Y.

Определите точки сетки координат X и Y.

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

Вычислите углы, $_{используя}$ $_{}$ функции 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), находятся за пределами достижимого рабочего пространства конечного эффектора. Решения обратной кинематики могут генерировать некоторые мнимые значения тета, которые требуют коррекции. Исправьте мнимые значения тета.

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));

Визуализируйте углы $_{start1}$ и $_{start2}$ с помощью вспомогательной функции 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: Вычислительная система Jacobian

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

$\frac{J=d (X, Y}{) d_{} (_{θ1},} θ2 \begin{array}{c} \frac{)}{=_{}} & \frac{}{_{}} \\ \frac{}{_{}} & \frac{}{_{}} \end{array}$ (dXdθ1dXdθ2dYdθ1dYdθ2)

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

the_J = 
 $(\begin{array}{c} - L_{2} \sin (θ_{1} + θ_{2}) - L_{1} \sin (θ_{1}) & - L_{2} \sin (θ_{1} + θ_{2}) \\ L_{2} \cos (θ_{1} + θ_{2}) + L_{1} \cos (θ_{1}) & L_{2} \cos (θ_{1} + θ_{2}) \end{array})$

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

$(\begin{array}{c} \frac{}{} \\ \frac{}{dXdtdYdt} \end{array}) = \begin{array}{c} \frac{(}{_{}} & \frac{}{_{}} \\ \frac{}{_{}} & \frac{}{_{}} \end{array} dXdstart1dXdü 2dYdü 1dYdü 2) \begin{array}{c} \frac{._{(}}{} \\ \frac{_{}}{} \end{array}$ d

$(\begin{array}{c} \frac{}{} \\ \frac{}{dXdtdYdt} \end{array}) = \begin{array}{c} \frac{J._{(}}{} \\ \frac{_{}}{} \end{array}$ d

$(\begin{array}{c} \frac{_{}}{} \\ \frac{_{}}{dstart1dtdstart2dt} \end{array}) =^{} J \begin{array}{c} \frac{+ .}{(} \\ \frac{}{} \end{array} dXdtdYdt)^{} где J + -$ псевдоинверсия Мура-Пенроуза J

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

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

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

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