目次
- 目次
- はじめに
- クォータニオン(四元数)とは
- 複素数平面の回転と四元数
- 四元数における回転
- クォータニオンの良い所
- クォータニオンの悪い所
- ROSにおけるオイラー角⇔クォータニオンの計算
- ROSが使えない環境でのオイラー角⇔クォータニオンの計算
- オイラー角→クォータニオンの変換数式
- クォータニオン→オイラー角の変換数式
- オイラー角 → 回転行列の変換公式
- 単位四元数 → 回転行列の変換公式
- 回転行列の小ネタ
- より深くクォータニオンを学びたい人は、
- 参考資料
- MyEnigma Supporters
はじめに
ロボットの位置・姿勢推定や、
観測対象の座標変換などで、
よく出てくるのが、
クオータニオン(四元数)です。
一般的に、
ある物体の姿勢を表すときに基本となるのは、
冒頭の絵にあるような、
オイラー角である、いわゆるRoll, Pitch, Yawだと思います。
しかし、
IMUによる姿勢推定ライブラリや、
カルマンフィルタのサンプルプログラムを見ていると、
クオータニオンを使用してフィルタリングを行なっているものも多く、
それらを見るたびに、
便利そうだけど、
よくわからないなと思っていました。
今回、
一般的にロボット位置・姿勢推定に必要な分だけの
クオータニオンの知識をまとめてみます。
クォータニオン(四元数)とは
まず初めにクオータニオンとは、
高校や大学で勉強した
複素数を拡張したものです。
複素数は一種類の虚数iと実部で構成されていましたが、
a+bi
クオータニオンは三種類の虚数i,j,ｋで構成されます。
ai+bj+ck+d
実はこれら３つの虚数がそれぞれ３つの次元を表しているため、
それを現実世界の３次元空間に当てはめることができるのです。
例えば i+2j+4kという四元数は、
よく使用するx,y,z空間の[1,2,4]という三次元空間の一点を表すことができます。
dは実部であり、結果として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プログラミングなどでは、
大量の座標変換をしなくてはならないため、
昔からクォータニオンが使用されているようです。
4. 回転の補間が簡単に計算できる
ある２つの回転を表すクォータニオンq1とq2の間の回転を補間したい場合、
q1, q2のなす角度をθとした場合、0 - 1の媒介変数tを使って、
任意の回転補間のクォータニオンq(t)は下記で計算されます。
このように簡単に回転を補間できるため、
アニメーションなどでこのクォータニオンはよく使われます。
クォータニオンの悪い所
1. クォータニオンの値だけを見ても、どのような角度なのかがわかりにくい。
前述のように、
クォータニオンを使うと
ある軸に対して回転する時などは楽なのですが、
結果のクォータニオンの値をみても、
それが実際にどのような角度であるのかが
人間にはわかりにくいという問題はあります。
やはり人間にはオイラー角の方が認識しやすいみたいですね。
ROSにおけるオイラー角⇔クォータニオンの計算
ROSを使っている場合は、
座標変換ライブラリであるtfを使うことで、
簡単にオイラー角⇔クォータニオンの変換をすることができます。
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); }
ROSが使えない環境でのオイラー角⇔クォータニオンの計算
ROSを使っている場合は、
tfのライブラリを使えば、
簡単にクォータニオン⇔オイラー角の変換は可能ですが、
tfが使えない場合の環境での
オイラー角⇔クォータニオンの計算方法を説明したいと思います。
C++: Eigenを使う
C++コードの場合、
行列演算ライブラリであるEigenを使うことで、
クォータニオンの変換を実施することができます。
myenigma.hatenablog.com
myenigma.hatenablog.com
オイラー角→クォータニオンへの変換では、
下記のコードのように、Roll,Pitch,Yaw角から、
３次元の回転行列を作成し、
その行列からクォータニオンを計算できます。
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
しかし、一つ注意点として、
オイラー角に変換する場合、
回転の順番によって角度表現が変わるため注意しましょう。
Python
ROSが無い環境で、Pythonでクォータニオンを扱いたい場合は
scipy.spatial.transform.Rotationを使うと便利です。
詳細は、下記を参照ください。
オイラー角→クォータニオンの変換数式
クォータニオン→オイラー角の変換数式
オイラー角 → 回転行列の変換公式
Roll, Pitch, Yawのそれのそれぞれの回転行列が下記なので、
これらを、3つ掛け算して一つの行列にすると下記になります。
単位四元数 → 回転行列の変換公式
単位四元数 q = [q0, q1, q2, q3]からその四元数の回転を回転行列として表すと下記のようになります。
回転行列の小ネタ
回転行列は直交行列なので、転置でも行けるという話です。
この前後輩が、ある二次元の点を回転行列で回転させて、それを同じ回転行列を使って元に戻す計算をする時に、逆行列を使っていて、転置でいいんじゃない？って言ったら、”ああ、これがいわゆる教養ですね”って言ってて、なんかとてもいい話だなと思いました。 pic.twitter.com/ZKU5nT70sY
— Atsushi Sakai (@Atsushi_twi) 2023年2月12日
ちなみに同次変換行列は転置じゃだめでした。(当たり前な気がするけど、自信がなくてあとで計算した) pic.twitter.com/BzpSM1Mp7d
— Atsushi Sakai (@Atsushi_twi) 2023年2月12日
より深くクォータニオンを学びたい人は、
より深くクォータニオンを学びたい人は、
下記の書籍を参照してもらえると、
より体系的にクォータニオンを学ぶことができると思います。
