はじめに
姿勢vから姿勢v′への変換はどう表現するのかは、ロボットや、ドローンの開発、運用に避けられない要素である。本文に出ているソースコードは、Python言語を用いる。
オイラー角、回転行列の表現

物体をX軸、Y軸(Y′軸)、Z軸(Z”軸)まわりの順にそれぞれオイラー角のロール角ϕ、ピッチ角θ、ヨー角ψだけ回転させたときに、物体の姿勢の変換は、オイラー角と、もしくはオイラー角の三角関数を用いる以下の回転行列で表す。
Rxyz=[CθCψ−CθSψSθSϕSθCψ+CϕSψ−SϕSθSψ+CϕCψ−SϕCθ−CϕSθCψ+SϕSψ−CϕSθSψ+SϕCψCϕCθ] ただし、C=cos,S=sin とする。
しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点(Gimbal Lock)があり、例えばY′軸回りを90°回転すると、X′軸とZ軸が同軸となってしまい、以降は姿勢制御(表現)ができなくなる。

このジンバルロックを解消するにはクォータニオンの使命となった。
回転ベクトルの表現
ROSでは回転ベクトルのクォータニオン(四元数)q=ix+jy+kz+wは、
q=(x,y,z,w) |
と表す。原点を通す回転軸を表す単位ベクトルa=(ax,ay,az)で、この回転軸まわり、角度θだけを回転する場合は、クォータニオンは
(x,y,z,w)=(ax*sin(θ/ 2 ), ay*sin(θ/ 2 ), az*sin(θ/ 2 ),cos(θ/ 2 )) |
と表す。
以下の例では、XY平面(z=0)にて辺長1mの正方形に沿って例えば仮に自律移動ロボットに走行させて、動作を確認しよう。勿論Gazeboでも確認できる。
ROSでは、ロボットの位置&姿勢の表現について、ロボットの中心または、ロボットにある他のポイントの位置はx,y,z(m)、姿勢ポーズはクォータニオンで表す。
from geometry_msgs.msg import Pose, PoseWithCo var ianceStamped, 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 ]; |
関連記事
オイラー角~ジンバルロック~クォータニオン
9軸IMU 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
研究開発用 台車型ロボット キット
0