読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

ロボットの位置推定におけるクォータニオン

Robot

http://blogs.msdn.com/blogfiles/ito/WindowsLiveWriter/morebones03_D941/yaw-pitch-roll%20copy_8.png

はじめに

ロボットの位置・姿勢推定や、

観測対象の座標変換などで、

よく出てくるのが、

クオータニオン(四元数)です。


一般的に、

ある物体の姿勢を表すときに基本となるのは、

冒頭の絵にあるような、

オイラー角である、いわゆるRoll, Pitch, Yawだと思います。


しかし、

IMUによる姿勢推定ライブラリや、

カルマンフィルタのサンプルプログラムを見ていると、

クオータニオンを使用してフィルタリングを行なっているものも多く、

それらを見るたびに、

便利そうだけど、

よくわからないなと思っていました。



今回、

一般的にロボット位置・姿勢推定に必要な分だけの

クオータニオンの知識をまとめてみます。


クォータニオン(四元数)とは


まず初めにクオータニオンとは、

高校や大学で勉強した

複素数を拡張したものです。



複素数は一種類の虚数iと実部で構成されていましたが、

a+bi


クオータニオンは三種類の虚数i,j,kで構成されます。

ai+bj+ck

実はこれら3つの虚数がそれぞれ3つの次元を表しているため、

それを現実世界の3次元空間に当てはめることができるのです。



例えば i+2j+4kという四元数は、

よく使用するx,y,z空間の[1,2,4]という三次元空間の一点を表すことができます。


複素数平面の回転と四元数


高校で複素数を勉強した人は

覚えているかもしれませんが、

複素数では、その値を二次元の平面(複素平面)と考えると、

虚数をiかけるということは、

上記の絵のように、

その点を90度回転させることになります。



なので,

三次元空間のそれぞれの軸が虚数で表されている四元数の場合、

それぞれの虚数i,k,jをかけると、

ある点を簡単に回転できるのです。

これが四元数が点の回転などで使用される理由の一つなのです。


四元数における回転


実際に、

四元数で指定されたある点Pを回転させたいときには、

下記のように、四元数の回転行列みたいなやつと



それの共役な四元数(虚数の符号を反転させたもの)


で挟むこむように掛けると点を回転させることができます。

QPQ'

ここで、θは回転する角度[rad],

α、β、γは回転する軸を表した四元数の虚数の定数(単位ベクトル)で、

α^2+β^2+γ^2=1を満たす必要があります。



例えば、i,j,k軸の虚数空間の中でi軸周りに回したい時は、

1i+0j+0kと考えると、

α=1,β=0,γ=0となり、上記の式を満たすことがわかります。

なので、上記の式に、α=1,β=0,γ=0を代入し、

回転角θと元の点の四元数を代入すると、

回転したあとの点の四元数が計算されるのです。


クォータニオンの良い所

1: 直感的である

単純に、ある軸を起点にある角度だけ回転させたい場合は、

先ほどの説明のように、

簡単に計算することができます。

オイラー角の場合、

その軸の回転がオイラー角でどれぐらい回転させるべきなのかを

計算しないといけないため、大変ですね。



また、オイラー角の回転は各軸の回転の順番によって、

結果が異なります。

クォータニオンはそのようなことを考えなくて良いことはいい所です。


2: 不連続点がない

これは、

ロボットの姿勢推定では重要な特徴です。



例えばオイラー角のyaw角の場合、

北を0度とした場合、

南は+180 or -180度となります。



すると、右回りでロボットが旋回していたとすると、

真南を向く瞬間に180度から急に-179度に変化します。



物理的には、たった一度しか方位が変化していないのに、

数学的には359度も変化することになります。


これにより、

例えばカルマンフィルタで姿勢を推定している時など、

Process Modelの結果は180度で、

観測値の結果は-179度である時、

実際、観測値は悪くないのに、

計算上は観測値と推定値の値が乖離していると判断され、

計算がおかしくなることがあるようです。


(カルマンフィルタによる位置計測に関しては、こちらを参照

移動ロボットにおける拡張カルマンフィルタを使用した自己位置推定のサンプルプログラム - MY ENIGMA)

 

3. 計算が早い

オイラー角の場合、三次元空間の回転は、

基本的に回転行列を三回掛けて、

計算する必要があります。

しかし、

クォータニオンは三次元空間の回転を、

上記の計算方法で一発で計算できます。


たった一つの点の変換ならあまり差はありませんが、

大量の点を変換したい時などは、

結構、差が出るようです。


特に、ゲームなどの3Dプログラミングなどでは、

大量の座標変換をしなくてはならないため、

昔からクォータニオンが使用されているようです。

クォータニオンの悪い所

1. クォータニオンの値だけを見ても、どのような角度なのかがわかりにくい。

前述のように、

クォータニオンを使うと

ある軸に対して回転する時などは楽なのですが、

結果のクォータニオンの値をみても、

それが実際にどのような角度であるのかが

人間にはわかりにくいという問題はあります。

やはり人間にはオイラー角の方が認識しやすいみたいですね。

 

ROSにおけるオイラー角⇔クォータニオンの計算

ROSを使っている場合は、

座標変換ライブラリであるtfを使うことで、

簡単にオイラー角⇔クォータニオンの変換をすることができます。

myenigma.hatenablog.com


 

C++の場合

オイラー角→クォータニオンは下記のように変換できます。

double roll,pitch,yaw;//[rad]
tf::Quaternion quaternion=tf::createQuaternionFromRPY(roll,pitch,yaw);

ちなみに、

上記のクォータニオンのデータはtf::Quaternionなので、

トピックとしてよく使うgeometry_msgs::Quaternionに変換するする場合は、

下記のようにquaternionTFToMsgという関数を使うと簡単です。

geometry_msgs::Quaternion quat_Msg;
quaternionTFToMsg(quat,quat_Msg);//この関数はROSのライブラリ

 

頻繁にオイラー角から、geometry_msgs::Quaternionを取得する人は

下記のような関数を定義しておくと便利かもしれません

/**
 * @brief RPYからクオータニオンを取得する関数
 *
 * @param roll [rad]
 * @param pitch [rad]
 * @param yaw [rad]
 * @param[out] q クオータニオン
 */
void GetQuaternionMsg(
    double roll,double pitch,double yaw,
    geometry_msgs::Quaternion &q
    ){
   tf::Quaternion quat=tf::createQuaternionFromRPY(roll,pitch,yaw);
   quaternionTFToMsg(quat,q);
}

 

続いて、クォータニオン→オイラー角は下記のように変換できます。

tf::Quaternion quat;//入力値
double r,p,y;//出力値
tf::Matrix3x3(quat).getRPY(r, p, y);//クォータニオン→オイラー角

入力がgeometry_msg::Quaternionの場合は

一度tf::Quaternionに変換することで

同じようにオイラー角に変換することができます。

geometry_msgs::Quaternion quat_msg;//入力値
double r,p,y;//出力値
tf::Quaternion quat(quat_msg.x,quat_msg.quat_msg,quat_msg.z,quat_msg.w);
tf::Matrix3x3(quat).getRPY(r, p, y);//クォータニオン→オイラー角

こちらも下記のように関数を作っておくと、

一発でgeometry_msgs::Quaternionからオイラー角を抜き出すことができます。

/**
 *  @brief ROSのトピックのクオータニオンの構造体から
 *         Roll,Pitch,Yaw角を取得する関数
 *  @param q トピックのクオータニオン
 *  @param[out] roll [rad]
 *  @param[out] pitch [rad]
 *  @param[out] yaw [rad]
 */
void GetRPY(const geometry_msgs::Quaternion &q,
    double &roll,double &pitch,double &yaw){
  tf::Quaternion quat(q.x,q.y,q.z,q.w);
  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}

  

Pythonの場合

ROSのPythonコードの場合は、

下記の記事の方法を使うことで、

オイラー角⇔クォータニオンに変換をすることができます。

yura2.hateblo.jp

 

ROSが使えない環境でのオイラー角⇔クォータニオンの計算

ROSを使っている場合は、

tfのライブラリを使えば、

簡単にクォータニオン⇔オイラー角の変換は可能ですが、

tfが使えない場合の環境での

オイラー角⇔クォータニオンの計算方法を説明したいと思います。

Eigenを使う

C++コードの場合、

行列演算ライブラリであるEigenを使うことで、

クォータニオンの変換を実施することができます。

myenigma.hatenablog.com
myenigma.hatenablog.com


  

オイラー角→クォータニオンへの変換では、

下記のコードのように、Roll,Pitch,Yaw角から、

3次元の回転行列を作成し、

その行列からクォータニオンを計算できます。

  double roll  = 45.0*M_PI/180.0;
  double pitch = 90.0*M_PI/180.0;
  double yaw   = 180.0*M_PI/180.0;

  Matrix3f m;
  m = AngleAxisf(roll, Vector3f::UnitX())
    * AngleAxisf(pitch, Vector3f::UnitY())
    * AngleAxisf(yaw, Vector3f::UnitZ());
  
  Quaternionf quat(m);

  cout << "x:"<<quat.x() << endl;
  cout << "y:"<<quat.y() << endl;
  cout << "z:"<<quat.z() << endl;
  cout << "w:"<<quat.w() << endl;

 

クォータニオンからオイラー角の変換は、

一度、三次元回転行列にし、

Matrix3fのメソッドであるeulerAnglesを使うことで、

オイラー角の計算が可能です。

  Matrix3f m2=quat.toRotationMatrix();
  Vector3f ea = m2.eulerAngles(0, 1, 2); 
  cout<<ea(0)*180.0/M_PI<<",";//roll
  cout<<ea(1)*180.0/M_PI<<",";//pitch
  cout<<ea(2)*180.0/M_PI<<endl;//yaw

しかし、一つ注意点として、

オイラー角に変換する場合、

回転の順番によって角度表現が変わるため注意しましょう。

 

オイラー角→クォータニオンの変換数式

f:id:meison_amsl:20160731212236p:plain

クォータニオン→オイラー角の変換数式

f:id:meison_amsl:20160731212200p:plain

より深くクォータニオンを学びたい人は、

より深くクォータニオンを学びたい人は、

下記の書籍を参照してもらえると、

より体系的にクォータニオンを学ぶことができると思います。

3Dグラフィックスのための数学入門 クォータニオン・スプライン曲線の基礎

3Dグラフィックスのための数学入門 クォータニオン・スプライン曲線の基礎

ベクトル・行列がビジュアルにわかる線形代数と幾何

ベクトル・行列がビジュアルにわかる線形代数と幾何