自作CNCマシン・レーザーカッターについて: 3月 2021

2021年3月31日水曜日

Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）

前回は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への拡張

2021年3月25日木曜日

Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD

ロボットアーム（マニピュレータ）における逆運動学のアルゴリズム。

今回は以下の方法について試してみます。

Forward + Shift
FABRIK（Foward and Backward Reaching Inverse Kinematics）
CCD（Cyclic　Coordinate　Descent）

環境：

Python3.6
Jupyter Notebook

運動学の場合、ロボットアームの各関節の角度を入力するとアーム先端（End-Effector）の座標値を求めることができます。

逆運動学の場合、End-Effector（アーム先端部）の座標値を入力すると各関節の角度が求められます。

上記の方法以外にヤコビ行列を用いた方法がありますが、それは次回へ。

Backward Shift：

この方法は、解析的に各関節の角度を求めるというより、図形的に回転と移動を繰り返すことで徐々に目標の座標に近似させていきます。Baseに近いほうからLink1、Link2、Link3という順番にしておき、Backwardの名前の通り、Link3から回転移動処理を始め、次にLink2、そしてLink1と操作を続けていきます。

Chart1：

アーム先端（End-Effector）の根元の関節Joint3から目標座標Target3に線を引いて角度θ3を得る。

Joint3を軸にLink3をθ3回転させる。

Chart2：

回転させたのち、End-EffectorとTarget3が一致するようにLink3をd3だけ平行移動する。

Chart3：

Link2とLink3の間に隙間ができるが、これでLink3の操作は一旦終了。

次はLink2の回転移動操作。目標座標はLink3下端（Target2）。

Chart4：

Joint2からTarget2へ向けて線を引き角度θ2を得る。

Joint2を軸にLink2をθ2回転させる。

Chart5：

回転させたのち、Link2上端とTarget2が一致するようにLink2をd2だけ平行移動する。

Chart6：

Link1とLink2の間に隙間ができるが、これでLink2の操作は一旦終了。

次のLink1についても同様の手順で動かす。目標座標はLink2の下端（Target1）。

Chart7：

Joint1からTarget1へ向けて線を引き角度θ1を得る。

Joint1を軸にLink1をθ1回転させる。

Chart8：

回転させたのち、Link1上端とTarget1が一致するようにLink1をd1だけ平行移動する。

Chart9：

Link1下端とBaseの間には隙間d0ができるので、Link1下端がBaseと一致するようにd0だけ全体的に平行移動する。

Chart10：

全体的に平行移動すると、End-Effectorと目標座標Target3との間に隙間ができる。

Joint3からTarget3へ線を引き、これ以降はChart1に戻って同じ処理を繰り返す。

最終的にはEnd-Effectorが目標座標Target3に限りなく近づく。

プログラム上では、この繰り返し処理に回数制限を設けるか、End-Effectorと目標座標との誤差が0.00001以下になったらループ処理を抜け出すかなどの設定にします。

コード：

リンク（Arm）のclassを用意して、長さ、両端座標、角度などを定義しておきます。
ならびに回転移動処理に必要なbackwardとshiftというメソッドも定義しておきます。
角度はnp.arctan2()で二点の座標から求めています。
変数Nを変えれば、Link数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。

FABRIK：

この方法は、「Forward and Backward Reaching Kinematics」の略で、ForwardとBackwardの回転移動操作を繰り返して徐々に目標座標に近づいていきます。

前述したBackward ShiftのShiftさせた部分をForward（前方からの）の回転移動操作に置き換えたようなものです。

まず、End-Effectorのほうから目標座標に対して回転移動操作（Backward）し、Link1まで操作したら、今度はLink1、Link2、Link3という順番で回転移動操作（Forward）していきます。向きが変わるだけで基本的な手順は前述のChart1〜Chart8までの操作と同じです。

コード：

Linkのclassを用意して、長さ、両端座標、角度などを定義しておきます。
回転移動処理に必要なbackwardとforwardのメソッドも定義しておきます。
角度はnp.arctan2()で二点の座標から求めています。
変数Nを変えれば、Link数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。

CCD:

この方法は、２つのベクトル（回転軸から目標座標までのベクトル、回転軸からEnd-Effectorまでのベクトル）の角度の差分だけ回転させますが、Linkを回転させるというよりも、回転軸以降にあるJointの座標を回転移動させると言ったほうがいいでしょう。回転軸はJoint3、Joint2、Joint1という順番で移行し、それを繰り返します。

Chart1：

まずEnd-Effectorに近いJoint3を回転軸とする。

Joint3からTargetまでのベクトル、Joint3からEnd-Effectorまでのベクトル間の角度θ3を求める。

回転軸となるJoint3以降にあるJoint（この場合End-Effectorのみ）をθ3回転させる。

Chart2：

次の回転軸はJoint2。

Joint2からTargetまでのベクトルとJoint2からEnd-Effectorまでのベクトル間の角度θ2を求める。

Joint2以降のJoint（この場合、Joint3とEnd-Effector）をJoint2を中心にθ2回転させる。

Chart3：

次の回転軸はJoint1。

Joint1からTargetまでのベクトルとJoint1からEnd-Effectorまでのベクトルの角度θ1を求める。

Joint1以降のJoint（この場合、Joint2、Joint3、End-Effector）をJoint1を中心にθ1回転させる。

Chart4：

再度、回転軸はJoint3に戻り、Chart1同様の手順で回転移動させる。

あとはこの繰り返しで徐々にEnd-EffectorはTargetに近づいていく。

コード：

各Linkごとに動かすというよりも、回転軸となるJointを中心にそれ以降にあるJointの座標を回転移動させています。
CCD()ファンクション内の二重のforループは、最初のforループが回転軸用、次のforループが回転移動させるJoint座標用になります。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。

３つの比較：

どれも比較的簡単なアルゴリズムなので軽快に動きますが、Forward & Backward系とCCDでは少し動きに違いが出てきます。

以下は、マウス（赤x印）の動きに追従するインタラクティブなプログラム。３つの方法を重ねて同時に動かしています。

BackwardとFABRIKは同じアルゴリズムを使っているので似たような動きになります。

FABRIKは全体的に張りのある動きをしています。Backwardのほうはやや柔らかめ。

CCDはやや反応が遅く、先端に近いほうの動きとBaseに近いほうの動きに差が出て蛇行することがあります。

リンク数を多くすると、その動き方の違いも顕著になってきます。

リンク数20の場合。

マウスの動かし方にもよりますが、CCDは２つに比べるとやや不自然な形になっています。

結果、BackwardとFABRIKが反応も早く、動きも自然という感じです。それぞれの回転移動方法やコードも比較的簡単。

3つ同時のコード：

Backward Shift、　FABRIK、CCDの３つ方法をマウスに追従させることでインタラクティブに試すことができます。
変数Nを変えればリンク数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。

これらのアルゴリズムは、CGのキャラなどを動かすときに向いているようです。角度制限がないため実際のロボットアームに使うアルゴリズムには向いていないようです。

2021年3月31日水曜日

Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）

運動学：

変化量 / ヤコビ行列：

逆運動学 / 疑似逆行列：

図形的に仕組みを見てみる：

コード：

2021年3月25日木曜日

Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD

Backward Shift：

コード：

FABRIK：

コード：

CCD:

コード：

３つの比較：

3つ同時のコード：

人気の投稿