メインコンテンツ

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

inverseKinematics

逆運動学ソルバーの作成

説明

inverseKinematics System object™ は、指定された剛体ツリー モデルに基づいて目的のエンドエフェクタ姿勢のジョイント コンフィギュレーションを計算するための、逆運動学 (IK) ソルバーを作成します。rigidBodyTree クラスを使用して、ロボットの剛体ツリー モデルを作成します。このモデルは、ソルバーが強制するすべてのジョイント拘束を定義します。求解が可能な場合、ロボット モデルで指定されているジョイント制限に従います。

照準拘束、位置の範囲、方向ターゲットなど、エンドエフェクタ姿勢以外の拘束をさらに指定するには、generalizedInverseKinematics の使用を検討してください。このオブジェクトにより、複数拘束の IK の解を計算できます。

IK の閉形式解析解の詳細については、analyticalInverseKinematics を参照してください。

目的のエンドエフェクタ姿勢に対するジョイント コンフィギュレーションを計算するには、以下を実行します。

  1. inverseKinematics オブジェクトを作成して、そのプロパティを設定します。

  2. 関数と同様に、引数を指定してオブジェクトを呼び出します。

System object の機能の詳細については、System object とはを参照してください。

作成

説明

ik = inverseKinematics は、逆運動学ソルバーを作成します。このソルバーを使用するには、RigidBodyTree プロパティで剛体ツリーモデルを指定します。

ik = inverseKinematics(PropertyName=Value) は、オプションである 1 つ以上の名前と値の引数を使用して、逆運動学ソルバーのプロパティを指定します。たとえば、SolverAlgorithm="fminconsqp" は逆運動学ソルバーとして fmincon SQP ソルバーを使用します。

プロパティ

すべて展開する

特に指定がない限り、プロパティは "調整不可能" です。つまり、オブジェクトを呼び出した後にプロパティ値を変更することはできません。オブジェクトは呼び出し時にロックされ、関数 release でロック解除されます。

プロパティが "調整可能" な場合は、いつでも値を変更できます。

プロパティ値の変更の詳細については、System object を使用した MATLAB でのシステム設計を参照してください。

剛体ツリー モデル。rigidBodyTree オブジェクトとして指定します。剛体ツリー モデルを変更したら、その剛体ツリーをこのプロパティに再割り当てします。次に例を示します。

IK ソルバーを作成し、剛体ツリーを指定します。

ik = inverseKinematics('RigidBodyTree',rigidbodytree)

剛体ツリー モデルを変更します。

addBody(rigidbodytree,rigidBody('body1'),'base')

剛体ツリーを IK ソルバーに再割り当てします。剛体ツリー モデルを変更する前にソルバーまたは関数 step が呼び出された場合、release を使用して、プロパティの変更を許可します。

ik.RigidBodyTree = rigidbodytree;

逆運動学の求解アルゴリズム。"BFGSGradientProjection""LevenbergMarquardt"、または "fminconsqp" として指定します。各アルゴリズムの詳細については、逆運動学のアルゴリズムを参照してください。

"fminconsqp" アルゴリズムには Optimization Toolbox™ が必要です。

指定したアルゴリズムに関連付けられたパラメーター。構造体として指定します。構造体のフィールドはアルゴリズムに固有です。ソルバー パラメーターを参照してください。

使用法

説明

[configSol,solInfo] = ik(endeffector,pose,weights,initialguess) は、指定したエンドエフェクタ姿勢を達成するジョイント コンフィギュレーションを探します。コンフィギュレーションの初期推定と、pose の 6 成分に対する許容誤差の重みの目標値を指定します。アルゴリズムの実行に関連する解の情報 solInfo が、ジョイント コンフィギュレーションの解 configSol とともに返されます。

入力引数

すべて展開する

エンドエフェクタ名。文字ベクトルとして指定します。エンドエフェクタは、inverseKinematics System object 内に指定された rigidBodyTree オブジェクト上の座標系でなければなりません。

エンドエフェクタの姿勢。4 行 4 列の同次変換として指定します。この変換は、endeffector プロパティで指定される、剛体の位置と向きの目標値を定義します。

姿勢の許容誤差の重み。6 要素ベクトルとして指定します。最初の 3 要素は、目的の姿勢に対する向きの誤差の重みに対応します。最後の 3 要素は、目的の姿勢に対する xyz 位置の誤差の重みに対応します。

ロボット コンフィギュレーションの初期推定。構造体配列またはベクトルとして指定します。この初期推定を使用して、目標とするロボット コンフィギュレーションにソルバーを誘導するのに役立てます。解がこの初期推定に近いことは保証されていません。

ベクトル形式を使用するには、RigidBodyTree プロパティで割り当てられているオブジェクトの DataFormat プロパティを "row" または "column" に設定します。

出力引数

すべて展開する

ロボット コンフィギュレーション。構造体配列として返されます。この構造体配列には次のフィールドが含まれます。

  • JointNameRigidBodyTree ロボット モデルで指定されたジョイント名の文字ベクトル

  • JointPosition — 対応するジョイントの位置

このジョイント コンフィギュレーションは、エンドエフェクタの目的の姿勢を解の許容誤差の範囲内で達成する、計算された解です。

メモ

回転ジョイントについて、ジョイント制限がジョイント位置のラッピングが発生する 2*pi の範囲を超えた場合、返されるジョイント位置はジョイントの下限に最も近いものになります。

ベクトル形式を使用するには、RigidBodyTree プロパティで割り当てられているオブジェクトの DataFormat プロパティを "row" または "column" に設定します。

解法情報。構造体として返されます。解法情報構造体には次のフィールドが含まれます。

  • Iterations — アルゴリズムによって実行される反復回数。

  • NumRandomRestarts — アルゴリズムが局所的最小値で行き詰ったことを原因とするランダム リスタートの回数。

  • PoseErrorNorm — 解と目的のエンドエフェクタ姿勢との比較による、姿勢誤差の大きさ。

  • ExitFlag — アルゴリズムの実行と、アルゴリズムが戻った原因に関する詳細を提供するコード。各アルゴリズム タイプの終了フラグについては、終了フラグを参照してください。

  • Status — 解が許容誤差以内に収まっているか ('success')、またはアルゴリズムが見つけられる最適解に達したか ('best available') を記述する文字ベクトル。

オブジェクト関数

オブジェクト関数を使用するには、System object を最初の入力引数として指定します。たとえば、obj という名前の System object のシステム リソースを解放するには、以下の構文を使用します。

release(obj)

すべて展開する

stepSystem object のアルゴリズムの実行
releaseリソースを解放し、System object のプロパティ値と入力特性の変更を可能にします。
resetSystem object の内部状態のリセット

すべて折りたたむ

ロボットを読み込み、その逆運動学ソルバーを作成します。ソルバーのアルゴリズムを fmincon SQP アルゴリズムに設定します。

robot = loadrobot("universalUR5",DataFormat="row");
ik = inverseKinematics(RigidBodyTree=robot,SolverAlgorithm="fminconsqp");

ロボットの最後のボディをエンドエフェクタとして設定し、ターゲット姿勢、重み、初期推定コンフィギュレーションを設定します。

ee = robot.BodyNames{end};
poseTarget = se3([0 pi/2 -pi/2],"eul","ZYX",[0 0.7 0.3]);
weights = [1 1 1 0.8 0.8 0.8];
initGuessConfig = [pi/2 0 0 0 0 0];

初期推定コンフィギュレーションのロボットを表示し、ターゲット姿勢をプロットします。

show(robot,initGuessConfig);
axis([-0.5 0.5 -0.1 0.9 -0.1 0.8])
hold on
plotTransforms(poseTarget,FrameSize=0.2);
title("Initial Guess Configuration and Pose Target")

Figure contains an axes object. The axes object with title Initial Guess Configuration and Pose Target, xlabel X, ylabel Y contains 32 objects of type patch, line. These objects represent world, base_link, base, shoulder_link, upper_arm_link, forearm_link, wrist_1_link, wrist_2_link, wrist_3_link, ee_link, tool0, base_link_mesh, shoulder_link_mesh, upper_arm_link_mesh, forearm_link_mesh, wrist_1_link_mesh, wrist_2_link_mesh, wrist_3_link_mesh.

指定した重みと初期推定コンフィギュレーションを使用して、姿勢ターゲットに達するコンフィギュレーションを求めます。

[config,solninfo] = ik(ee,tform(poseTarget),weights,initGuessConfig);
show(robot,config,PreservePlot=false);
title("End-Effector Target Pose Achieved")
hold off

Figure contains an axes object. The axes object with title End-Effector Target Pose Achieved, xlabel X, ylabel Y contains 32 objects of type patch, line. These objects represent world, base_link, base, shoulder_link, upper_arm_link, forearm_link, wrist_1_link, wrist_2_link, wrist_3_link, ee_link, tool0, base_link_mesh, shoulder_link_mesh, upper_arm_link_mesh, forearm_link_mesh, wrist_1_link_mesh, wrist_2_link_mesh, wrist_3_link_mesh.

solninfo.Status
ans = 
'success'

Robotics System Toolbox™ loadrobotから PUMA 560 マニピュレーターを読み込み、Figure にロボット モデルを表示します。

puma = loadrobot("puma560");
show(puma,homeConfiguration(puma),PreservePlot=false);
axis([-1 1 -1 1 0 2])
title("PUMA 560 Home Configuration")

Figure contains an axes object. The axes object with title PUMA 560 Home Configuration, xlabel X, ylabel Y contains 20 objects of type patch, line. These objects represent link1, link2, link3, link4, link5, link6, link7, link2_mesh, link3_mesh, link4_mesh, link5_mesh, link6_mesh, link7_mesh, link1_mesh.

ロボットのエンドエフェクタが到達する目的の位置は [-0.5 0.5 0.75] です。逆運動学では、目的の姿勢を使用してジョイント コンフィギュレーションの解を求めるため、まず目的の並進をもつ SE(3) 同次変換行列に位置を変換します。次に、姿勢変換をプロットします。

pos = [-0.5 0.5 0.75];
poseTF = trvec2tform(pos);
hold on
plotTransforms(pos,eul2quat([0 0 0]),FrameSize=0.2);
title("PUMA 560 and End-Effector Target Position")

Figure contains an axes object. The axes object with title PUMA 560 and End-Effector Target Position, xlabel X, ylabel Y contains 24 objects of type patch, line. These objects represent link1, link2, link3, link4, link5, link6, link7, link2_mesh, link3_mesh, link4_mesh, link5_mesh, link6_mesh, link7_mesh, link1_mesh.

puma ロボット モデルの inverseKinematics System object™ を作成します。姿勢の回転と位置の重みを指定します。目標はエンドエフェクタが方向の制約なしでその位置に到達することであるため、方向の解が IK ソルバーにとって意味をもたないように、方向角度の重みを 0 に設定します。ロボットのホーム コンフィギュレーションを初期推定として使用します。

ik = inverseKinematics("RigidBodyTree",puma);
weights = [0 0 0 1 1 1];
initialguess = homeConfiguration(puma);

ik System object を使用してジョイント位置を計算します。ロボット モデルの最後のリンク link7 をエンドエフェクタとして使用します。

[configSoln,solnInfo] = ik("link7",poseTF,weights,initialguess);

生成された解のコンフィギュレーションを表示します。これでゴール位置に到達しました。

show(puma,configSoln,PreservePlot=false);
title("End-Effector Target Position Achieved")

Figure contains an axes object. The axes object with title End-Effector Target Position Achieved, xlabel X, ylabel Y contains 24 objects of type patch, line. These objects represent link1, link2, link3, link4, link5, link6, link7, link2_mesh, link3_mesh, link4_mesh, link5_mesh, link6_mesh, link7_mesh, link1_mesh.

ほとんどの IK 問題では、目的の姿勢ターゲットに到達できるコンフィギュレーションが複数あることに注意してください。ソルバーは最適化ベースであるため、目的の姿勢に実際には到達しない解に近づくことがあります。これが発生した場合、ソルバーは自動的に再起動し、ランダムなコンフィギュレーションを初期推定として使用します。つまり、同じ姿勢ターゲットに対して関数を複数回実行すると、すべてが姿勢ターゲットに到達する異なるコンフィギュレーションが得られることがあります。ランダム性を回避するには、乱数発生器のシードを設定するか、AllowRandomRestart を無効にするとともに、解に近い初期推定を使用することができます。

ik.SolverParameters.AllowRandomRestart = false

可能なすべての解を求める必要がある場合は、analyticalInverseKinematicsオブジェクトを使用します。

参照

[1] Badreddine, Hassan, Stefan Vandewalle, and Johan Meyers. "Sequential Quadratic Programming (SQP) for Optimal Control in Direct Numerical Simulation of Turbulent Flow." Journal of Computational Physics. 256 (2014): 1–16. doi:10.1016/j.jcp.2013.08.044.

[2] Bertsekas, Dimitri P. Nonlinear Programming. Belmont, MA: Athena Scientific, 1999.

[3] Goldfarb, Donald. "Extension of Davidon’s Variable Metric Method to Maximization Under Linear Inequality and Equality Constraints." SIAM Journal on Applied Mathematics. Vol. 17, No. 4 (1969): 739–64. doi:10.1137/0117067.

[4] Nocedal, Jorge, and Stephen Wright. Numerical Optimization. New York, NY: Springer, 2006.

[5] Sugihara, Tomomichi. "Solvability-Unconcerned Inverse Kinematics by the Levenberg–Marquardt Method." IEEE Transactions on Robotics Vol. 27, No. 5 (2011): 984–91. doi:10.1109/tro.2011.2148230.

[6] Zhao, Jianmin, and Norman I. Badler. "Inverse Kinematics Positioning Using Nonlinear Programming for Highly Articulated Figures." ACM Transactions on Graphics Vol. 13, No. 4 (1994): 313–36. doi:10.1145/195826.195827.

拡張機能

すべて展開する

バージョン履歴

R2016b で導入

すべて展開する