前回はFABRIK法やCCD法でロボットアームの逆運動学を試してみましたが、今回はヤコビ行列を用いた方法を試してみます。
環境：Python3.8.5、Jupyter Notebook6.1.4
運動学：
運動学においては、各Jointの角度を入力すると、アーム先端のEnd-Effectorの座標が求まります。
3リンクアームの場合、Link1の角度θ1の時のx成分、Link2の角度θ1+θ2の時のx成分、Link3の角度θ1+θ2+θ3の時のx成分を単純に足し合わせればEnd-Effectorのx座標が得られます。y座標についても同様に求めると、End-Effectorの座標(x, y)は以下のように求まります。L1、L2、L3は各Link1、2、3の長さ。
x = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) y = L1sin(θ1) + L2sin(θ1+θ2) + L3sin(θ1+θ2+θ3)
変化量 / ヤコビ行列：
運動学の関数をf、出力座標P=[x, y]、入力角度Θ=[θ1, θ2, θ3]とすると、
P = f(Θ)
となります。
逆運動学では、出力と入力が逆になるため、逆運動学の関数をgとすれば、
Θ = g(P)
になります。ちなみにgはfの逆行列でg(Θ)=f-1(Θ)、最終的にこのような関係式を以下の手順で導いていきます。
まず運動学の式の両辺を時間tで微分し、単位時間における角度の変化量の関係式にすると
dP/dt = df(Θ)/dt
になります。これはプログラム上ではforやwhileループでの1ステップごとの変化量。
そして、df(Θ)をJと置き換えると、
ΔP = J(Θ)・ΔΘ
という角度変化量による位置変化量の関係式になり、この場合のJがヤコビ行列となります。
そしてJの中身は、先ほど運動学で求めたxとyをそれぞれの角度θ1、θ2、θ3で微分（偏微分）し、
J(Θ) = [[dx/dθ1, dx/dθ2, dx/dθ3],
[dy/dθ1, dy/dθ2, dy/dθ3]]
となります。
それぞれを偏微分した結果は、
dx/dθ1 = - L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3) dx/dθ2 = 0 - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3) dx/dθ3 = 0 - 0 - L3sin(θ1+θ2+θ3) dy/dθ1 = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) dy/dθ2 = 0 + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) dy/dθ3 = 0 + 0 + L3cos(θ1+θ2+θ3)
となります（ちなみにcosθの微分は-sinθ、sinθの微分はcosθ、0の部分は微分で消えた定数項）。
よってヤコビ行列Jは、
J(Θ) = [[- L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3), - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3), - L3sin(θ1+θ2+θ3)], [ L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3), L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3), L3cos(θ1+θ2+θ3)]]
になります。2行はx, y座標（ベクトル）に対応、3列はJoint1、2、3に対応。
Jはその都度更新されるΘによって変化するので、Jを求める関数J(Θ)として表したほうがいいでしょう。
逆運動学 / 疑似逆行列：
逆運動学では目標座標に対してあとどのくらいJointの角度を回転させればいいのかを計算するため、その角度の変化量を求めます。Jの逆行列J-1を用意して運動学の式の右辺と左辺を入れ替えて、
ΔΘ = J-1(Θ)・ΔP
という関係式にすれば、目標座標に近づくための角度の補正量が求まります。
しかしながら、Jが正方行列ではないためJの逆行列は存在しません。2リンクであれば正方行列になりますが、Jointの数が増えるほど横長の行列になってしまいます。
ここで疑似逆行列を使うテクニックが必要となるようです。
その疑似逆行列J+（Moore-Penrose Pseudo-Inverse Matrix）は、
J+ = J.T・(J・J.T)-1
J.TはJの転置行列。行列式の掛け算なのでドット積。
少し面倒ですがnumpyなら、
J+ = np.linalg.pinv(J)
ですぐに求まります（ちなみに正方行列の逆行列はnp.linalg.inv()を使う）。
よって、
ΔΘ = J+(Θ)・ΔP
になります。
ΔPは現時点でのEnd-Effectorの座標と目標座標との差分であるため、目標座標をP、現時点でのEnd-Effectorの座標を現時点での角度Θiと運動学の関数をfで表せばf(Θi)、
ΔΘ = J+(Θi)・(P - f(Θi))
になりΔΘが求まるため、次にどのくらいの角度で動かせばいいかわかります。
実際は、この式に刻み幅をより細かくするためのスケーラー：λ=0.1を掛け合わせて以下のようになります。
ΔΘ = J+(Θi)・(P - f(Θi))λ
つまりプログラム上では、現在の角度ΘiにΔΘ=[Δθ1, Δθ2, Δθ3]を1ループごとに加算していき、
Θi+1 = Θi + J+(Θi)・(P - f(Θi))λ
ということになります。
疑似ヤコビ逆行列J+は、次のΘi+1が代入されることでそのつど更新されていきます。そして徐々にEnd-Effectorの位置が目標座標に近づいていきます。
手順としては：
- 運動学でEnd-Effectorの座標[x, y]を求める式を用意する
- 運動学の式を偏微分してヤコビ行列Jを求める
- ヤコビ行列Jの疑似逆行列J+を求める
- 目標座標とEnd-Effector座標の差分とJ+を用いて必要な回転量を求める
- スケーラーとしてλ=0.1程度を掛け合わせる
- 以後この操作を繰り返し徐々に目標に近づく
図形的に仕組みを見てみる：
計算式だけではイメージが湧かないので、図形的にヤコビ行列による操作を見ていきます。
ヤコビ行列を求める際にEnd-Effectorの座標(x, y)を各角度で偏微分しましたが（以下）、
dx/dθ1 = - L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3) dx/dθ2 = 0 - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3) dx/dθ3 = 0 - 0 - L3sin(θ1+θ2+θ3) dy/dθ1 = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) dy/dθ2 = 0 + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) dy/dθ3 = 0 + 0 + L3cos(θ1+θ2+θ3)
これらの変化量は以下の図のような感じになっています。
Chart1：
dx/dθ1はJoint1を回転軸とした角度θ1の変化率で、Joint1だけを回転させる時、現状の角度にΔθ1加えるとEnd-EffectorがTargetに最も近くなります。
Chart2：
次にdx/dθ2に関しては、Joint2だけを回転させる時、現状の角度にΔθ2加えるとEnd-EffectorがTargetに最も近くなります。
Chart3：
同様にdx/dθ3に関しては、Joint3だけを回転させる時、現状の角度にΔθ3加えるとEnd-EffectorがTargetに最も近くなります。
Chart4：
これ以降はChart1に戻ってJoint1の回転操作から順に繰り返していきます。
このような手順で各Jointの角度の回転の割合が決まって、徐々にEnd-EffectorがTargetに近づいていきます。
これは以前に試したCCD法に似た手順です。CCD法では図形的に理解しましたが、ヤコビ行列で変化量を求めてそれぞれのJointを個別に回転させることは、同じようなことをやっているのではないでしょうか（証明していないので不明）。
コード：
- 最初にFKの計算式、そしてヤコビ行列を求めたあと疑似逆行列を求めてIKを計算しています。
- numpyにはmatrixクラスもありますが、将来的に廃止となるようなので行列式にはnp.array()を使用しています。
- np.array()の場合、行列同士の掛け算はドット積np.dot(A, B)で計算しますが、A@Bのように@でも計算可能です。
- 最後にインタラクティブにマウス座標を追従するプログラムがあるため、Jupyter Notebookのbackendは%matplotlib notebookを使用しています。
まだ不完全な部分も多いため、引き続き以下の点についても試してみようと思っています。
- 同次変換行列を用いる方法
- 特異点における問題点や回避策
- 回転角範囲の制約と設定
- End-Effectorを固定したまま他のジョイントを動かす方法
- クロス積を用いたヤコビアンの計算方法
- 3Dへの拡張
AliExpress.com Product - Metal mechanical arm Multi-degree of freedom manipulator Industrial robot model Six-axis robot 202
0 件のコメント:
コメントを投稿