Этот пример показывает, как вывести аналитические решения для обратной кинематики головной цепи человекоподобного робота.
Опишите кинематику ссылки головной цепи (ссылка между туловищем и головой) человекоподобного робота NAO [1] с использованием параметров Денавита-Хартенберга (DH) и обозначений на основе исследования Kofinas et al. [2]. Следующее преобразование задает ссылку головной цепи.
где:
- перемещение от основы (туловища) к шарниру или системе координат 0
- ориентация ссылки 1 относительно 0
- ориентация ссылки 2 относительно 1
- вращение крена
- тангаж вращения
- перемещение от ссылки 2 к точке с конечным эффектором (головке)
определяет координаты головы, которые , , .
В этом примере вы аналитически решаете задачу обратной кинематики, возвращая все ориентации отдельных соединений в звене цепи головы с заданными координатами головы xc
, yc
, и zc
в пределах достижимого пространства. Затем вы преобразуете аналитические результаты в чисто числовые функции для эффективности.
Решение аналитически (когда это возможно) позволяет вам выполнять расчеты в реальном времени и избегать особенностей, которые вызывают трудности для численных методов.
Функция getKinematicChain
возвращает определенную кинематическую цепь для робота NAO с точки зрения символьных переменных. Для получения дополнительной информации о getKinematicChain
, см. Раздел «Вспомогательные функции».
kinChain = 'head';
dhInfo = getKinematicChain(kinChain);
T = dhInfo.T
T =
Выразите матрицу преобразования прямой кинематики T
как следующая последовательность продуктов: T = ABase0*T01*T12*Rx*Ry*A2Head
. Задайте отдельные матрицы следующим образом.
Задайте матрицу преобразования из туловища (основы) в соединение 0.
ABase0 = dhInfo.ABase0
ABase0 =
Задайте матрицу преобразования из соединения 0 в соединение 1.
T01 = dhInfo.T01
T01 =
Задайте матрицу преобразования из соединения 1 в соединение 2.
T12 = dhInfo.T12
T12 =
Задайте матрицу поворота крена.
Rx = dhInfo.Rx
Rx =
Задайте матрицу поворота тангажа.
Ry = dhInfo.Ry
Ry =
Задайте матрицу преобразования из соединения 2 в головку.
A2Head = dhInfo.A2Head
A2Head =
Известными параметрами этой задачи являются CameraX
, CameraZ
, NeckOffsetZ
, и положения xc
, yc
, и zc
. Неизвестные параметры: и . После нахождения и , можно вычислить отдельные преобразования T
. Затем робот может достичь желаемого положения xc
, yc
, zc
.
Хотя вы можете увидеть эти параметры в матрицах преобразования, они не существуют как переменные в базовом рабочем пространстве MATLAB. Это связано с тем, что эти параметры берутся из функции. Функции не используют базовое рабочее пространство. Каждое рабочее пространство функции отделено от базового рабочего пространства и всех других рабочих областей, чтобы защитить целостность данных. Таким образом, чтобы использовать эти переменные вне функции getKinematicChain
, использовать syms
создать их.
syms CameraX CameraZ NeckOffsetZ xc yc zc theta_1 theta_2 real;
Перепишите уравнение T = ABase0*T01*T12*Rx*Ry*A2Head
, разделяя термины, описывающие движения туловища и головы робота: inv(T01)*inv(ABase0)*T = T12*Rx*Ry*A2Head
. Упростите левую и правую стороны уравнения и задайте интересующие уравнения, соответствующие выражениям для координатных положений.
LHS = simplify(inv(T01)*inv(ABase0)*T); RHS = simplify(T12*Rx*Ry*A2Head); eqns = LHS(1:3,end) - RHS(1:3,end)
eqns =
Эта система уравнений содержит две переменные, для которых вы хотите найти решения, и . Однако уравнения также подразумевают, что вы не можете произвольно выбирать xc
, yc
, и zc
. Поэтому также рассмотрим yc
как переменная. Все другие неизвестные системы являются символьными параметрами.
Этот пример следует типичному алгебраическому подходу для решения задач обратной кинематики [3]. Идея состоит в том, чтобы получить компактное представление решения, где выражение каждой переменной в терминах параметров и переменных, для которых вы уже решили уравнения. В качестве первого шага найдите или или с точки зрения параметров. Затем выразите другую переменную в терминах известной переменной и параметров. Для этого начните с определения уравнения, которое имеет только одну переменную.
intersect(symvar(eqns(1)),[theta_1,theta_2,yc])
ans =
intersect(symvar(eqns(2)),[theta_1,theta_2,yc])
ans =
intersect(symvar(eqns(3)),[theta_1,theta_2,yc])
ans =
Третье уравнение содержит только одну переменную, . Сначала решите это уравнение.
[theta_2_sol,theta_2_params,theta_2_conds] = ... solve(eqns(3),theta_2,'Real',true,'ReturnConditions',true)
theta_2_sol =
theta_2_params =
theta_2_conds =
Решения имеют добавку термин с параметром . Без потери обобщения можно задать равным 0 в решениях и условиях.
theta_2_sol = subs(theta_2_sol,theta_2_params,0)
theta_2_sol =
for p = 1:numel(theta_2_conds) theta_2_conds(p) = simplify(subs(theta_2_conds(p),theta_2_params,0)); end
Теперь решите для переменных и yc
в терминах переменной и другие символические параметры.
[theta_1_sol,yc_sol,yc_theta_1_params,yc_theta_1_conds] = ... solve(eqns(1:2),theta_1,yc,'Real',true,'ReturnConditions',true);
Отобразите решения для и yc
.
theta_1_sol
theta_1_sol =
yc_sol
yc_sol =
Решения зависят от двух параметров, и .
yc_theta_1_params
yc_theta_1_params =
Решения имеют добавку срок. Без потери обобщения можно задать равным 0 в решении для theta_1_sol
и условия yc_theta_1_conds
.
theta_1_sol = simplify(subs(theta_1_sol,yc_theta_1_params,[0,0]))
theta_1_sol =
for p = 1:numel(yc_theta_1_conds) yc_theta_1_conds(p) = simplify(subs(yc_theta_1_conds(p),yc_theta_1_params,[0,0])); end
Аналогичная замена не требуется для yc_sol
поскольку нет зависимости от параметров.
intersect(symvar(yc_sol),yc_theta_1_params)
ans = Empty sym: 1-by-0
Начиная с произвольного набора известных числовых значений для и , вычислите числовые значения положений end-effector xc
, yc
, и zc
с кинематикой вперед. Затем работайте назад от xc
, yc
, и zc
для вычисления всех возможных числовых значений для и использование выражений обратной кинематики из предыдущего расчета. Проверьте, что один набор обратных решений совпадает с начальным набором числовых значений для и .
Установите фиксированные параметры для робота. Значения в этом примере предназначены только для рисунка.
CameraXValue = 53.9; CameraZValue = 67.9; NeckOffsetZValue = -5;
Используя прямые расчеты, найдите положения end-effector, соответствующие значениям и .
Tfk = ABase0*T01*T12*Rx*Ry*A2Head; xyz = Tfk(1:3,end);
Создайте символическую функцию для этих положений.
xyzFunc = symfun(subs(xyz,[CameraX,CameraZ,NeckOffsetZ], ...
[CameraXValue,CameraZValue,NeckOffsetZValue]),[theta_1,theta_2]);
Для кинематики вперед задайте две переменные theta_1_known
и theta_2_known
которые содержат произвольное стартовое множество значений для и .
theta_1_known = [pi/4,pi/6,pi/8,pi/10]; theta_2_known = [pi/8,-pi/12,pi/16,-pi/10]; numPts = numel(theta_1_known); num_theta_2 = numel(theta_2_sol); num_theta_1 = numel(theta_1_sol);
Обратите внимание, что потенциально существуют num_theta_1
решения для каждого num_theta_2
решение.
Используйте следующую последовательность операций для проверки решений.
Цикл (theta_1_known,theta_2_known)
. Для каждой точки вычислите конечные положения x_known
, y_known
, и z_known
, которые затем «известны».
Цикл над решениями для соответствующий x_known
и z_known
. Для каждого решения проверьте, соответствует ли соответствующее условие cond_theta_2
является допустимым. Если условие верно, вычислите соответствующее числовое решение theta_2_derived
.
Цикл над решениями для соответствующий x_known
, z_known
, и theta_2_derived
. Для каждого решения проверьте, соответствует ли соответствующее условие cond_theta_1
является допустимым. Если условие верно, проверьте, является ли y_derived
численно совпадает с y_known
в пределах относительных и абсолютных погрешностей через условие cond_yc
. Если это условие допустимо, вычислите theta_1_derived
.
Соберите результаты в таблицу T
в целях отображения.
T = table([],[],[],[],'VariableNames',{'theta_1_known','theta_2_known',... 'theta_1_derived','theta_2_derived'}); for ix1 = 1:numPts xyz_known = double(xyzFunc(theta_1_known(ix1),theta_2_known(ix1))); x_known = xyz_known(1); y_known = xyz_known(2); z_known = xyz_known(3); for ix2 = 1:num_theta_2 % theta_2 loop cond_theta_2 = subs(theta_2_conds(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]); if isAlways(cond_theta_2) % if theta_2 is valid theta_2_derived = subs(theta_2_sol(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]); theta_2_derived = double(theta_2_derived); for ix3 = 1:num_theta_1 % theta_1 loop cond_theta_1 = subs(yc_theta_1_conds(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); if isAlways(cond_theta_1) % if theta_1 is valid y_derived = subs(yc_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); y_derived = double(y_derived); cond_yc = abs(y_known - y_derived) < max(100*eps,1e-6*abs(y_known)); % check rounding if isAlways(cond_yc) % if yc is valid theta_1_derived = subs(theta_1_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); theta_1_derived = double(theta_1_derived); T = vertcat(T,table(theta_1_known(ix1),theta_2_known(ix1),theta_1_derived,... theta_2_derived,'VariableNames',T.Properties.VariableNames)); end end end end end end T
T=8×4 table
theta_1_known theta_2_known theta_1_derived theta_2_derived
_____________ _____________ _______________ _______________
0.7854 0.3927 0.7854 0.3927
0.7854 0.3927 -2.3562 -1.7346
0.5236 -0.2618 0.5236 -0.2618
0.5236 -0.2618 -2.618 -1.0801
0.3927 0.19635 0.3927 0.19635
0.3927 0.19635 -2.7489 -1.5383
0.31416 -0.31416 0.31416 -0.31416
0.31416 -0.31416 -2.8274 -1.0278
Заметьте, что существует две возможные пары решений (theta_1_derived,theta_2_derived)
полученный с использованием обратной кинематики для каждой пары стартовых углов (theta_1_known,theta_2_known)
. Один набор обратных решений совпадает с начальными углами.
SoftBank Robotics. НАО. https://www.softbankrobotics.com/emea/en/nao.
Кофинас, Н., Э. Орфанудакис, и М. Г. Лагудакис. «Полная аналитическая обратная кинематика для NAO». В 2013 году 13-я Международная конференция по автономным Роботам системам. Лиссабон, Португалия: Robotica, 2013.
Кендрикс, К. «Решение обратной кинематической робототехнической задачи: сравнительное исследование матрицы базиса Денавита-Хартенберга и Гребнера». Доктор философии. Дипломная работа. Auburn University, Auburn, AL, 2007.
function kinChain = getKinematicChain(link) % This function returns the kinematic chain of the NAO humanoid robot % represented using Denavit-Hartenberg (DH) parameters. % The function uses as a reference the paper: Complete analytical inverse % kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G., % 2013 13th International Conference on Autonomous Robot Systems % (Robotica), Publication Year: 2013. if nargin < 1 link = 'head'; end % Notation: A, R, and T are the translation, rotation, and DH % transformation matrices, respectively. % Specify DH parameters for the desired end configuration. syms r11 r12 r13 r21 r22 r23 r31 r32 r33 xc yc zc real; R = [r11 r12 r13;r21 r22 r23;r31 r32 r33]; kinChain.T = [R,[xc;yc;zc];0,0,0,1]; switch link case 'head' % Kinematic chain from torso (base) to head. syms CameraX CameraZ NeckOffsetZ real; % Translation from torso (base) to joint 0. ABase0 = getA([0 0 NeckOffsetZ]); % Translation from joint 2 to head. A2Head = getA([CameraX 0 CameraZ]); % Transformation from joint 0 to joint 1. % theta_1 is the rotation angle corresponding to the 0-1 link. syms theta_1 real; alpha1 = 0; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1); % Transformation from joint 1 to joint 2. % theta_2 is the rotation angle corresponding to the 1-2 link. syms theta_2 real; piby2 = str2sym('pi/2'); d2 = 0; a2 = 0; alpha2 = -piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2); % Rx is the roll rotation. Rx = getR('x',piby2); % Ry is the pitch rotation. Ry = getR('y',piby2); % Capture the kinematic chain as a string. % The transformation is from the base to the head. kinChain.DHChain = 'ABase0*T01*T12*Rx*Ry*A2Head'; kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.Rx = Rx; kinChain.Ry = Ry; kinChain.A2Head = A2Head; case 'lefthand' % Kinematic chain from torso to left hand. syms ShoulderOffsetY ElbowOffsetY ShoulderOffsetZ real; ABase0 = getA([0 (ShoulderOffsetY+ElbowOffsetY) ShoulderOffsetZ]); syms HandOffsetX LowerArmLength real; AEnd4 = getA([(HandOffsetX+LowerArmLength) 0 0]); piby2 = str2sym('pi/2'); syms theta_1 real; alpha1 = -piby2; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1); syms theta_2 real; d2 = 0; a2 = 0; alpha2 = piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2); syms UpperArmLength theta_3 real; d3 = UpperArmLength; a3 = 0; alpha3 = -piby2; T32 = getT(a3,alpha3,d3,theta_3); syms theta_4 real; d4 = 0; a4 = 0; alpha4 = piby2; T43 = getT(a4,alpha4,d4,theta_4); Rz = getR('z',piby2); kinChain.DHChain = 'ABase0*T01*T12*T32*T43*Rz*AEnd4'; kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.T32 = T32; kinChain.T43 = T43; kinChain.Rz = Rz; kinChain.AEnd4 = AEnd4; end end function A = getA(vec) % This function returns the translation matrix. A = [1 0 0 vec(1); 0 1 0 vec(2); 0 0 1 vec(3); 0 0 0 1]; end function R = getR(orientation,theta) % This function returns the rotation matrix. switch orientation case 'x' R = [1 0 0 0; 0 cos(theta) -sin(theta) 0; 0 sin(theta) cos(theta) 0 0 0 0 1]; case 'y' R = [cos(theta) 0 sin(theta) 0; 0 1 0 0; -sin(theta) 0 cos(theta) 0; 0 0 0 1]; case 'z' R = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0; 0 0 0 1]; end end function T = getT(a,alpha,d,theta) % This function returns the Denavit-Hartenberg (DH) matrix. T = [cos(theta),-sin(theta),0,a; sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d; sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d; 0,0,0,1]; end