ROS・Unity・ロボット・ドローン姿勢制御に関わるクォータニオン

オイラー角による回転行列の表現

coordinate
coordinate

物体を座標系とともにx軸、y´軸、z´軸まわりの順にそれぞれ角度ϕ、θ、ψだけ回転させたときに、物体の位置の変換を表す回転行列は、

euler_angle
euler_angle

と表する。 3つの角度ϕ、θ、ψ をオイラー角と呼ぶ。

しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点(Gimbal lock)があり、例えば西方向のY軸を90度傾けると、X軸とZ軸が同軸となってしまい、姿勢制御が困難となる。

ジンバルロック
ジンバルロック

このジンバルロックを解消するのがクォータニオンの出番だ。

クォータニオンによる回転の表現

ROSではクォータニオンのq=(x,y,z,w)を虚数形式で

q=ix+jy+kzq=ix+jy+kz+W

と表する。原点を通す回転軸を表す単位ベクトルa=(ax,ay,az)で、この回転軸から物体軸の回転角度がθの場合は、クォータニオンは

(x,y,z,w)=(ax*sin(θ/2), ay*sin(θ/2), az*sin(θ/2),cos(θ/2))

となる。

以下の例では、平坦地面にて辺長1mの正方形に沿って自律移動ロボット(クローラ)に走行させて、動作を確認する。

※ROSでは、ロボットの位置&姿勢の表現について、ロボットの中心(センタ・ポイント)または、ロボットにある他のポイントの位置はx,y,z(m)、姿勢はそのポイントのクォータニオンにて表する。

from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
...
locations['square_vertex_1'] = pose = Pose(Point(1.0,0.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
locations['square_vertex_2'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_3'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_4'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
...

クォータニオンからオイラー角へ変換する

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;    
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
double yaw, pitch, roll;    
mat.getEulerYPR(yaw, pitch, roll);

オイラー角からクォータニオンへ変換する

tf::Quaternion q;
q.setRPY(Out_X, Out_Y, Out_Z);
sensor_msgs::Imu imu_data;
imu_data.orientation.x=q[0];
imu_data.orientation.y=q[1];
imu_data.orientation.z=q[2];
imu_data.orientation.w=q[3];

以上

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

Unity3DでTCPパケットを扱う

coming soon

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

Unity3DでUDPパケットを扱う

ソースコードは、githubへ公開ずみ。

関連記事は→ こちら

以上。

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

加速度センサに連動してCUBEを回転させるApp(Unity3D使用)

1、PCを加速度センサにシリアルにて接続して、ControlObjectByGsensorUnity.cs をunity3d のcube objectにattachして、再生します。
ControlObjectByGsensorUnity.cs

using System;
using System.IO.Ports;
using System.Threading;
using UnityEngine;

public class ControlObjectByGsensorUnity: MonoBehaviour {
	private const string SERIAL_PORT = "COM5";
	private const int SERIAL_BAUD_RATE = 9600;
	private const int SERIAL_TIMEOUT = 100;

	private Thread _readThread;
	private static SerialPort _serialPort;
	private static bool _continue;

	//private static Quaternion _handQuaternion = new Quaternion();
    private static float x,y,z;

	void Start() {
		_readThread = new Thread(Read);
		_serialPort = new SerialPort(SERIAL_PORT, SERIAL_BAUD_RATE);
		_serialPort.ReadTimeout = SERIAL_TIMEOUT;
		_serialPort.Open();
		_continue = true;
		_readThread.Start();
	}

	void Update() {
		transform.rotation = Quaternion.AngleAxis(y, Vector3.forward) * Quaternion.AngleAxis(x, Vector3.right);
	}

	void OnApplicationQuit() {
		_continue = false;
		_readThread.Join();
		_serialPort.Close();
	}

	private static void Read() {
		string[] values;
		while (_continue) {
		 if (_serialPort.IsOpen) {
		  try {
		   values = _serialPort.ReadLine().Split('\t');
		   if (values[0] != "\t") {
			x = float.Parse(values[0]);
			y = float.Parse(values[1]);
		   }
		  } catch (TimeoutException) {
		  }
		 }
		 Thread.Sleep(1);
		}
	}
}

2、加速度センサを傾けてみて、Cubeが動けば成功です。動かない場合は、Arduino本体のリセットボタンを押してみます。

3、3軸加速度センサは、重力加速度によるx、y軸の分量により傾斜を計算するもので、水平面に沿う回転は検知できません。水平面の回転を検知したい場合、他のセンサを組み合わせるかまたはAll in oneのセンサ例えば9軸センサを使う方法があると思います。のち9軸センサの使う実験に関するブログを公開します。

2015-6-24追記:
4、シリアルケーブル(USB)を外して、センサからPC/Unityへ wifi UDP或いはTCPにて接続して、cubeの回転動作が確認できています。
5、更に、センサからAndroid/Unity、iPad/UnityへUDP或いはTCPにて接続して、cubeの回転動作が確認できています。TCP接続と比べて、UDP接続は若干、遅延が少なくなるように見受けています(遅延時間の計測は相当難しいので、省いています)。
6、ビデオ(ジャイロセンサ~WiFi~UNity3D App)、ジャイロセンサを使っていますので、unityアプリCUBEの傾斜は多少ずれているように見えます。

【追記】Wifi UDP接続に関するソースコードは、Githubに公開済み。

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください