Этот пример выводит и применяет инверсную кинематику к манипулятору 2D ссылки при помощи MATLAB® и Symbolic Math Toolbox™.
Пример задает объединенные параметры и местоположения исполнительного элемента конца символически, вычисляет и визуализирует решения для прямой и инверсной кинематики и находит системный якобиан, который полезен для симуляции движения манипулятора.
Задайте длины ссылки, углы поворота шарнира и местоположения исполнительного элемента конца роботов как символьные переменные.
syms L_1 L_2 theta_1 theta_2 XE YE
Задайте значения для длин ссылки робота.
L1 = 1; L2 = 0.5;
Задайте координаты X и Y исполнительного элемента конца как функция углов поворота шарнира .
XE_RHS = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2)
XE_RHS =
YE_RHS = L_1*sin(theta_1) + L_2*sin(theta_1+theta_2)
YE_RHS =
Преобразуйте символьные выражения в функции 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]);
Прямая кинематика преобразовывает углы поворота шарнира в местоположения исполнительного элемента конца: . Учитывая определенные значения угла поворота шарнира, используйте прямую кинематику, чтобы вычислить местоположения исполнительного элемента конца.
Задайте входные значения углов поворота шарнира как и .
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))
Инверсная кинематика преобразовывает местоположения исполнительного элемента конца в углы поворота шарнира: . Найдите инверсную кинематику от прямых уравнений кинематики.
Определите прямые уравнения кинематики.
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
представляет несколько решений для и . Покажите пару решений для .
simplify(S.theta_1)
ans =
Покажите пару решений для .
simplify(S.theta_2)
ans =
Преобразуйте решения в функции 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]);
Используйте инверсную кинематику, чтобы вычислить и от координат 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));
Визуализируйте углы и использование помощника функционирует plot_theta_given_XY_2dof
.
plot_theta_given_XY_2dof(xmat,ymat,th1_mat,th2_mat)
Определение системного якобиана:
the_J = jacobian([XE_RHS YE_RHS],[theta_1 theta_2])
the_J =
Можно связать объединенную скорость в-конец-исполнительного-элемента скорости, и наоборот, при помощи системного якобиана:
Можно также преобразовать символьное выражение якобиана к блоку MATLAB function. Симулируйте местоположения исполнительного элемента конца робота на траектории путем определения нескольких waypoints как входных параметров к модели Simulink. Модель Simulink может вычислить профиль движения на основе значений угла поворота шарнира, чтобы достигнуть каждого waypoint в траектории. Для получения дополнительной информации смотрите Инверсную кинематику Манипулятора с 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