Unityでロボットアームの逆運動学

前回記事でロボットアームの順運動学をUnityで実装しましたので、今回は次のステップとして逆運動学を実装してみました。逆運動学は三角関数で解析的に解く方法と、ヤコビアンを利用して数値的に解く方法がありますが、今回は数値的に解いています。本記事ではアルゴリズムの概要を解説し、C#スクリプトのソースコードも載せますが、基礎から説明するととても長くなってしまうので、ある程度書籍などで学んだ知識を前提とします。私は梶田秀司編著「ヒューマノイドロボット」を参考にしています。

前回記事をご覧いただいた方へ
前回の順運動学は、Matrix4x4で同次変換行列を作り、回転と並行移動をまとめて計算しましたが、今回は同次変換行列を用いていません。前回のソースコードを利用できないことについてご了承願います。

ロボットアームの構造

ロボットアームの構造については前回記事「Unityでロボットアームの順運動学」に同じとします。逆運動学の計算で利用するパラメータを示します。

・pw_j ：ワールド座標系での位置
・p_j：ローカル座標系での位置（親リンク相対）
・Rw_j：ワールド座標系での姿勢
・R_j：ローカル座標系での姿勢（親リンク相対）
・angle_j：関節の回転角度
・a_j：関節軸ベクトル（親リンク相対）

逆運動学（数値解法）のアルゴリズム

逆運動学ではアーム先端の位置と姿勢を指定できますが、今回は位置のみにして簡略化しました（初心者なので^^;）。逆運動学を数値計算で解くアルゴリズムは次のようになります。

①逆運動学で求める、各関節の角度を並べたベクトルをangleとし、初期値を設定する。
②アーム先端の目標位置pw_{4_target}を指定する。
③順運動学で、angleから各関節の位置と姿勢を計算する。
④目標位置からの誤差Δpw₄を計算する。(Δpw₄ が十分に小さければ終了。)
　Δpw₄ = pw_{4_target} – pw₄

⑤Δpw₄を小さくする角度修正量Δangleを計算する。
⑥Δangleを関節角度に反映する。
　angle = angle + Δangle

⑦③から繰り返す。

 角度修正量Δangleをどのように求めるかが問題となりますが、ここでヤコビアンを使います。各関節を微小に変化（Δangle）させたときのアーム先端の位置の微小変化量（Δpw₄）をヤコビアンJを用いてΔpw₄ = J Δangleと表現できれば、Jの逆行列を使ってΔangleを求めることができます。

　Δangle = J^-1 Δpw₄

ヤコビアンの計算方法

ある関節が微小に回転したとき、アーム先端の位置がどのように変化するかを考えます。例えば、第2関節の微小回転角度をΔangle₂とすると、アーム先端の位置の微小変化量Δpw₄は次のように計算できます（×は外積）。

　Δpw₄ = Rw₂a₂×(pw₄ - pw₂)Δangle₂

全ての関節についてこれを計算し、足し合わせれば、Δpw₄を求めることができます。それを行列で表すと、

 

先に示した式Δpw₄ = J Δangleと比較すると、

ということになります。（Jは3x3行列となる。）

C#スクリプト

上記のアルゴリズムを実装したソースコードを載せます。一つ面倒なこととして、Unityには4x4行列しか用意されていないため、行列演算にはMath.Net Numericsというライブラリを使っています。このライブラリは.NETFrameworkのバージョン4.0以降ですが、Unityは3.5なので使うにはひと手間必要です。こちらの記事が参考になりました。

Unityで数値計算ライブラリMath.NET Numericsを使う方法 - arXiv探訪

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra.Single;    //Unityの行列は4x4しかないため、行列演算にはこのライブラリを用いる

public class RobotControllerScript : MonoBehaviour {

    public Transform axis1_tf, axis2_tf, axis3_tf;      //各軸のTransform(逆運動学の結果を出力してロボットを動かす)
    public Transform hand_transform;                    //アーム先端のTransform(逆運動学の結果確認用)
    public Vector3 pw4_target;                          //アーム先端の目標位置（Inspectorビューから入力）

    //---リンクのパラメータ---//
    private Vector3 pw1, pw2, pw3, pw4;    //ワールド座標系での位置
    private Quaternion Rw1, Rw2, Rw3;      //ワールド座標系での姿勢
    private Vector3 p1, p2, p3, p4;        //ローカル座標系での位置（親リンク相対）
    private Quaternion R1, R2, R3;         //ローカル座標系での姿勢（親リンク相対）
    private float angle1, angle2, angle3;  //関節角度
    private Vector3 a1, a2, a3;            //関節軸ベクトル（親リンク相対）

    private float lambda = 0.1f;    //逆運動学の収束を調整する係数（0～1で設定）

    //UI
    public UnityEngine.UI.Text transform_label;

    void Start () {
        //固定値のパラメータを設定
        p1 = new Vector3(0f, 1f, 0f);   //第1軸のローカル座標（親リンク相対）
        p2 = new Vector3(0f, 1f, 0f);   //第2軸のローカル座標（親リンク相対）
        p3 = new Vector3(0f, 3f, 0f);   //第3軸のローカル座標（親リンク相対）
        p4 = new Vector3(0f, 3f, 0f);   //アーム先端のローカル座標（親リンク相対）
        a1 = Vector3.up;                //第1軸の関節軸ベクトル（親リンク相対）
        a2 = Vector3.right;             //第2軸の関節軸ベクトル（親リンク相対）
        a3 = Vector3.right;             //第3軸の関節軸ベクトル（親リンク相対）

        //関節角度の初期値を設定
        //※0度だとヤコビアンの逆行列が存在しないため少し角度を付けておく
        angle1 = 10f;                   //第1軸の回転角度
        angle2 = 10f;                   //第2軸の回転角度
        angle3 = 10f;                   //第3軸の回転角度
    }

    void FixedUpdate () {

        for (int i = 0; i < 50; i++)
        {
            //// 順運動学 ////
            pw1 = p1;
            Rw1 = Quaternion.AngleAxis(angle1, a1);

            pw2 = Rw1 * p2 + pw1;
            R2 = Quaternion.AngleAxis(angle2, a2);
            Rw2 = Rw1 * R2;

            pw3 = Rw2 * p3 + pw2;
            R3 = Quaternion.AngleAxis(angle3, a3);
            Rw3 = Rw2 * R3;

            pw4 = Rw3 * p4 + pw3;


            //// ヤコビアンを作成 ////
            Vector3 j1 = Vector3.Cross(Rw1 * a1, pw4 - pw1);
            Vector3 j2 = Vector3.Cross(Rw2 * a2, pw4 - pw2);
            Vector3 j3 = Vector3.Cross(Rw3 * a3, pw4 - pw3);

            var J = DenseMatrix.OfArray(new float[,]     // ※Math.Netを使用
            {
                { j1.x, j2.x, j3.x },
                { j1.y, j2.y, j3.y },
                { j1.z, j2.z, j3.z }
            });

            //// 逆運動学 ////

            //アーム先端の目標位置からの誤差を計算
            Vector3 err = pw4_target - pw4;
            float err_norm = err.sqrMagnitude;

            //err_norm < 1E-5なら計算終了
            if (err_norm < 1E-5) break;

            // ※Jの逆行列と掛け算するためにerrをMath.Netの行列に入れなおす
            var err2 = DenseMatrix.OfArray(new float[,]
            {
                { err.x },
                { err.y },
                { err.z }
            });

            var d_angle = lambda * J.Inverse() * err2;

            //逆運動学で得た角度修正量を各軸に反映する(d_angleはラジアンなので度に変換)
            angle1 += d_angle[0, 0] * 180f / 3.14159f;
            angle2 += d_angle[1, 0] * 180f / 3.14159f;
            angle3 += d_angle[2, 0] * 180f / 3.14159f;
        }

        //逆運動学で計算した関節角度をロボットに反映
        Quaternion axis1_rot = Quaternion.AngleAxis(angle1, Vector3.up);
        Quaternion axis2_rot = Quaternion.AngleAxis(angle2, Vector3.right);
        Quaternion axis3_rot = Quaternion.AngleAxis(angle3, Vector3.right);

        axis1_tf.localRotation = axis1_rot;
        axis2_tf.localRotation = axis2_rot;
        axis3_tf.localRotation = axis3_rot;

        //UIにアーム先端の位置を表示
        transform_label.text = "Transform X:" + hand_transform.position.x.ToString("F2") +
                                " Y:" + hand_transform.position.y.ToString("F2") +
                                " Z:" + hand_transform.position.z.ToString("F2");


    }
}

実行結果

アーム先端の目標位置（pw4_target）をx = 3、y = 3、z = 3 として逆運動学を実行した結果を示します。初期状態は各関節の角度を10度としています。0度だとヤコビアンの逆行列が存在しないためです。画面にはアーム先端のTransform.positionを表示しており、指定した通りになっています。

また、関節角度がどのように収束しているのか気になったため、グラフ化してみました。ヤコビアンのおかげでうまく収束していることがわかります（第2関節（angle2）が一旦大きくマイナスに振れているのが気になりますが）。

追記（2020.01.24）

当ブログをtwitterで参照してくださり、6軸の位置・姿勢制御に拡張までされている方を発見したので紹介させていただきます。twitter活動はしていないのでこのような形で恐縮ですが、参照してくださりありがとうございました。とても励みになりました(^^)

こちらのソースを試させてもらって順・逆運動学テスト。
同じ教科書のヒューマノイドロボット（オーム社 梶田秀司 著）使ってる！
これはタイムリー！　4月の記事で、やはりこの教科書は絶版でもまだメジャーなんだなぁ。

Unityでロボットアームの逆運動学-mizu-mi bloghttps://t.co/G9tC5a7D6C pic.twitter.com/6AfqUo1xNU

— でべ (@devemin) 2019年12月10日

っしゃー！
逆運動学 (IK) のgithub MATLAB コードを unity に移植して実行できた！
６軸アームをIKで位置・姿勢の制御。嬉しい～
赤いオブジェ（位置・姿勢）をターゲットにして、アーム先端を動かしています。

参考：ヒューマノイドロボット（オーム社 梶田秀司 著）githubhttps://t.co/SmHO1mXrwt pic.twitter.com/r1K8zrGH4F

— でべ (@devemin) 2019年12月16日