В этом примере показано, как вывести аналитические решения для инверсной кинематики главной цепи гуманоидного робота.
Опишите кинематику главно-цепочечной ссылки (ссылка между туловищем и головой) гуманоидного робота NAO [1] использование параметров Denavit-Hartenberg (DH) и обозначений на основе исследования Kofinas и др. [2]. Следующее преобразование задает главно-цепочечную ссылку.
где:
перевод из основы (туловище) к соединению или системе координат 0
ориентация ссылки 1 относительно 0
ориентация ссылки 2 относительно 1
вращение списка
вращение подачи
перевод из ссылки 2 точки в-конец-исполнительного-элемента (голова)
задает координаты головы, которые являются xc
, yc
, zc
.
В этом примере вы аналитически решаете задачу инверсной кинематики путем возврата всех ориентаций отдельных соединений в главно-цепочечной ссылке, данной главные координаты xc
, yc
, и zc
в достижимом пробеле. Затем вы преобразуете аналитические результаты в чисто числовые функции для КПД.
Решение аналитически (при выполнении так возможно) позволяет вам выполнить расчеты в режиме реального времени и избежать сингулярности, которая вызывает трудность для численных методов.
Функциональный getKinematicChain
возвращает определенную кинематическую цепь для робота NAO в терминах символьных переменных. Для получения дополнительной информации на getKinematicChain
, смотрите раздел Helper Functions.
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
Начиная с произвольного набора известных числовых значений для и , вычислите числовые значения положений исполнительного элемента конца xc
, yc
, и zc
с прямой кинематикой. Затем работайте назад от xc
, yc
, и zc
вычислить все возможные числовые значения для и использование выражений инверсной кинематики от предыдущего расчета. Проверьте, что один набор обратных решений совпадает со стартовым набором числовых значений для и .
Установите фиксированные параметры для робота. Значения в этом примере в целях рисунка только.
CameraXValue = 53.9; CameraZValue = 67.9; NeckOffsetZValue = -5;
Используя прямые расчеты, найдите положения исполнительного элемента конца, соответствующие значениям и .
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
isvalid. Если условие допустимо, вычислите соответствующее числовое решение theta_2_derived
.
Цикл по решениям для соответствие x_known
, z_known
, и theta_2_derived
. Для каждого решения проверяйте, чтобы видеть если соответствующее условие cond_theta_1
isvalid. Если условие допустимо, проверяйте, чтобы видеть если 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 numerical match 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. NAO. https://www.softbankrobotics.com/emea/en/nao.
Kofinas, N., Э. Орфэнудакис и М. Г. Лэгудакис. "Завершите аналитическую инверсную кинематику для NAO". На 2 013 13-х международных конференциях по вопросам автономных систем робота. Лиссабон, Португалия: Robotica, 2013.
Kendricks, K. "Решая обратную кинематическую задачу робототехники: исследование сравнения матрицы Denavit-Hartenberg и базисной теории Groebner". Кандидатская диссертация. Обернский университет, Оберн, 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