Unityでロボットのフィードバックシミュレーションをする

ROSはPythonで動かすんだけど、毎回実際のロボットを動かすなんてやってられないし、GUIはないに等しい。Gazeboは使い方が分からんし、Matlabやsimulinkはそれはそれでとっつきにくい。
あれ、複雑な微分方程式を解くとかじゃなければ、Unityの方が楽に見栄え良く出来るんじゃない？？
そんな思い付きで始めたプログラミング。後で工程を追えるように、メモを残しておきます。

今回は、Unity上の仮想ロボット(円柱オブジェクト)に、2つの機能を実装します。

①クリックで掴んで任意の位置に持っていけるようにする
②目標姿勢(位置&回転)へのフィードバックコントローラ実装

今回作成するシミュレーターは、以下の実験環境を仮想化したものになります。

今回は5台のロボットを動かすと仮定。更に限定的にはなりますが、私の使っているロボットの仕様に合わせて、ロボットの姿勢制御ついて次のような環境を仮定します。

・ロボットの位置は位置ベクトルposition(x,y,z)及び、自身の回転姿勢クオータニオンorientation(x,y,z,w)で与えられるものとします。
・ロボットへの入力は、目標姿勢を達成するような速度ベクトルlinear(x,y,z)及び角速度ベクトルangular(x,y,z)になります。
・当面はxz平面内を動く3輪ロボットを動かすこととします。
・ロボットの姿勢状態は位置x,zとy軸回転θのみによって決まるため、ロボットは入力情報のうち、linear(x,z)、及びangular(y)しか動作に反映しません。

以上が前提条件となります。それでは初めにフィールドにロボットを5台おいてみましょう。

工程は次の通りです。

①Blenderで円柱を作成、編集モード>UV展開でUVマップを作製
②Blender UVエディタでUV>UV配置をエクスポート、出てきたpng画像にペイントでデザイン
③Blenderのマテリアル>ベースカラー>画像テクスチャから先ほど作成したテクスチャを選択。3Dビューで適用されているのを確認して、objファイルをエクスポート
④UnityのAssetsフォルダにobjとテクスチャをコピー。シーンに読み込むとPrefabとして扱われるので、(必要に応じて)Prefab>unpackでモデルだけを取り出す

これで向きテクスチャ付きのロボットを表すオブジェクトが作成できました。続いて、各ロボットをドラッグで任意の位置(x,z)に持っていけるようにスクリプトを組みます。そのため、初めに次の2つの準備をしておきます。

① ドラッグで動かすのはロボットのみです。ロボットと、それ以外のフィールドオブジェクトの識別のため、ロボットたちにまとめて"Drone"というタグ付けをします
②クリックされたかどうかの検知にRaycastを利用します。この時rayのヒット判定にcoliderが必要なため、全てのロボットに(Capsule) Colliderを追加します

さらにnullobjectを作成し、それに次のスクリプトをアタッチします。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class clickandcatch : MonoBehaviour
{
   GameObject clicked = null;
   // Start is called before the first frame update
   void Start()
   {
   }
   // Update is called once per frame
   void Update()
   {
       if (Input.GetMouseButtonDown(0))
       {
           Vector3 pos = Input.mousePosition;
           pos.z = 4f;
           Ray ray = Camera.main.ScreenPointToRay(pos);
           RaycastHit hit = new RaycastHit();
           if (Physics.Raycast(ray, out hit)&& hit.collider.gameObject.CompareTag("Drone"))
           {
               clicked = hit.collider.gameObject;
           }
       }
       if (clicked && Input.GetMouseButton(0))
       {
           Vector3 pos2 = Input.mousePosition;
           pos2.z = 4f;
           clicked.transform.position = Camera.main.ScreenToWorldPoint(pos2);
       }
       if (Input.GetMouseButtonUp(0))
       {
           clicked = null;
       }
   }
}

アイデアとしては次のような手順を踏んでいます。

①マウスから画面奥に向かう光(ray)はScreenPointToRay()で作成できます。このrayの衝突情報はRaycastHit hitに格納されますが、(ⅰ)rayとオブジェクトで衝突が起きている、(ⅱ)衝突オブジェクトがDroneタグを持っているの両方を満たすときのみ、その衝突オブジェクト(=動かしたいオブジェクト)の位置をマウスが示す位置に移動させます。
②マウス座標Input.mousePositionは(画面x,画面y,0)の3次元ベクトルですが、カメラがあるのは今回対象のスクリーンから4離れた位置としています。これを考慮しないと座標変換が狂うため、z=4としています。クリックされたオブジェクトの位置変更に関しても、マウス座標からワールド座標に変換しての移動が必要なため、z=4とした上でScreenToWorldPoint()を使用しています。
③このままだとマウスがオブジェクトを離れた瞬間に操作が切れます。従ってマウスポインタを素早く動かすと、その時点で「このポインタはロボットをクリックしていないので無視」となり、ロボットを思うようにドラッグできません。その為マウスが押された瞬間にGetMouseButtonDownでhitしたオブジェクトを固定し、以降GetMouseButtonUpが来るまでは、ポインタrayの衝突如何に関わらず、対象オブジェクトをマウスポインタの位置に動かします。

続いて目標姿勢へのフィードバックコントローラーを実装します。ここでは分散制御の観点から、ロボットが各々でなるべく同一のプログラムを持つようにします。つまり、全ロボットの位置情報から管理コンピューターが各ロボットに目標位置を算出し、個別に指令を渡すのではなく、基本的には個々のロボットが「自機」と「自機以外」の位置情報から自分が取るべき位置を各々計算するようにするということです。

このような発想を念頭に、次の3つのプログラムを作ります。

プログラムAは様々な目的に応じて柔軟に設計する必要があります。今後様々なシミュレーションをする上で、一番ミソになるのはこのプログラムです。
プログラムBはいわゆるフィードバックコントローラです。PID制御器や各種フィルタを設計し、ここに入れることになります。
プログラムCはいわゆるプラントです。実際のロボットを動かす時にはここが駆動部になります。本シミュレーターでは最も単純に、dx(t)/dt=v(t)を満たすようにロボットは動くと仮定します。

通常、ロボットの挙動は物理的解析をすることで状態方程式や伝達関数という数学の世界で表現することができます。本来はそのような数学的考察から適切なフィードバックコントローラ(B)を設計するのですが、当面は様々なAを作ることを目的としていますので、B,Cは以下のような理論から最も単純なものを設計していきます。
※今後、回転行列Rを用いたより汎用性が高く、全ての回転姿勢を達成できるB,Cを作ろうとは思っていますが、まずは「形」にすることを優先して、今回は前述の通りy軸回転のみを考えます。

以上のアイデアから、指定したオブジェクト(GameObject targetobj)の姿勢を目標姿勢とするプログラム(コントローラー+プラント)を作りました。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class test : MonoBehaviour
{
   public GameObject targetobj;
   // Start is called before the first frame update
   
   void Start()
   {
   }
   // Update is called once per frame
   void Update()
   {
       //determine v&omega
       Vector3 lin = targetobj.transform.position - transform.position;
       Quaternion tgtq = targetobj.transform.rotation;
       float tgtang = qtn2yangle(tgtq);
       Quaternion thisq = transform.rotation;
       float thisang = qtn2yangle(thisq);
       float angdiftmp = tgtang - thisang;
       float omega = angdiftmp - 2 * Mathf.PI;
       if (Mathf.Abs(omega) > Mathf.Abs(angdiftmp)) omega = angdiftmp;
       if (Mathf.Abs(omega) > Mathf.Abs(angdiftmp+ 2 * Mathf.PI)) omega = angdiftmp+ 2 * Mathf.PI;
       //movement
       float dt = Time.deltaTime;
       transform.position += lin * dt;
       float domega = omega*dt;
       Quaternion qdt = new Quaternion(0, Mathf.Sin(domega / 2), 0, Mathf.Cos(domega / 2));
       transform.rotation *= qdt;
   }
   float qtn2yangle(Quaternion q)
   {
       float ang = Mathf.Atan2(1 - 2 * q.w * q.w, 2 * q.y * q.w);
       return ang;
   }
}

ロボットを5台用意し, ロボット1はロボット2に、ロボット2はロボット3に...と追従させるよう設定し、ロボット5を手で動かした結果はこちら。

きちんと位置/回転姿勢両方で同期が行われているのが確認できます。

というわけで今回のシミュレータ制作はここまで。
今後はより高度なマルチエージェントシステムや、被覆制御などをプログラムAを作ってシミュレーションしていきたいと思います。