ヒューマノイド ロボットの逆運動学の解析解
この例では、ヒューマノイド ロボットの頭部連鎖の逆運動学に対する解析解を導き出す方法を示します。
手順 1: パラメーターの定義
Kofinas らの研究 [2] に基づいた Denavit-Hartenberg (DH) パラメーターと表記法を使用して、NAO ヒューマノイド ロボット [1] の頭部連鎖のリンク (胴体と頭部との間のリンク) の運動学を説明します。以下の変換は頭部連鎖のリンクを定義します。
ここで
はベース (胴体) からジョイントまたは基準座標系 0 への変換
は 0 を基準とする参照 1 の向き
は 1 を基準とする参照 2 の向き
はロールの回転
はピッチの回転
は参照 2 からエンドエフェクタ点 (頭部) への変換
は頭部の座標 (、、) を定義します。
この例では、到達可能な空間内で頭部の座標 xc
、yc
、および zc
が与えられた頭部連鎖のリンクで個々のジョイントの向きをすべて返すことにより、逆運動学問題を解析的に解きます。次に、効率性を高めるため、解析結果を数値のみの関数に変換します。
解析的に解くことで (可能な場合)、リアルタイムに計算を実行し、特異性を回避できます。ただし、数値的な方法は難しくなります。
手順 2: DH パラメーターを使用した頭部連鎖の順運動学の定義
関数 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;
手順 3: と の代数の解を求める
方程式 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 =
この方程式系には、解を求めるための 2 つの変数 と が含まれています。ただし、この方程式は xc
、yc
、および zc
を任意に選択できないことも示しています。そのため、yc
も変数として考慮します。系のその他すべての未知なものは、シンボリック パラメーターです。
この例は、逆運動学問題を解くための一般的な代数アプローチに従います [3]。考え方は、各変数の式が、既に方程式を解いたパラメーターおよび変数として、解のコンパクトな表現を取得することです。最初のステップとして、 または をパラメーターとして求めます。次に、既知の変数およびパラメーターとしてその他の変数を表します。これを行うには、変数が 1 つだけ含まれている方程式を特定することから始めます。
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 =
3 番目の方程式には変数 が 1 つだけ含まれています。この方程式を最初に解きます。
[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 =
解は および の 2 つのパラメーターによって異なります。
yc_theta_1_params
yc_theta_1_params =
解には加法的な 項があります。汎化を失うことなく、 を theta_1_sol
の解および条件 yc_theta_1_conds
で 0 に等しくなるように設定できます。
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
手順 4: 解の検証
および に対する既知の数値の任意のセットから開始して、順運動学でエンドエフェクタの位置 xc
、yc
、および zc
の数値を計算します。次に、前の計算からの逆運動学式を使用して、xc
、yc
、zc
から逆方向に、 および に対して可能な数値をすべて計算します。逆の解の 1 つのセットが および に対して開始した数値セットと一致することを確認します。
ロボットの固定パラメーターを設定します。この例の値は説明目的のみで提供されています。
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]);
順運動学では、 および に対する任意の開始値のセットが含まれる 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_2
の解ごとに num_theta_1
の解がある可能性に注意してください。
次の一連の操作を使用して解を確認します。
(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
が有効かどうかを調べます。条件が有効な場合、条件cond_yc
を介した相対許容誤差と絶対許容誤差内でy_derived
が数値的にy_known
と一致するかどうかを調べます。この条件が有効である場合は、theta_1_derived
を計算します。表示目的で table
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_known,theta_2_known)
の各ペアに対し、逆運動学を使用して取得した 2 つの可能性のある解のペア (theta_1_derived,theta_2_derived)
があることを確認します。逆の解の 1 つのセットが開始角と一致します。
参考文献
SoftBank Robotics. NAO. https://www.aldebaran.com/en/nao.
Kofinas, N., E. Orfanoudakis, and M. G. Lagoudakis"Complete Analytical Inverse Kinematics for NAO"In 2013 13th International Conference on Autonomous Robot Systems.Lisbon, Portugal: Robotica, 2013
Kendricks, K."Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory."Ph.D. Thesis.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