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

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

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

と表する。 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];