一回の回転を直交する軸での二回の回転に分ける

March 21, 2023

一回の回転を直交する軸での二回の回転に分割する方法です．座標系の設定に使うために導出しました． C++による実装も公開しています．

モチベーション

ロボットを歩行させるため，足を置く位置を地面に固定された座標系map上で計画するつもりです．そのためにロボットの座標系base_linkの「前方」がmapではどの向きなのかの情報が必要です．ロボットには IMU が搭載されており，鉛直上向きを z 軸とする適当な初期位置odomを基準にbase_linkの姿勢が推定されます．重力加速度の向きからbase_linkの傾きは容易にわかりますが，鉛直軸まわりの方向は定まりずらい状況です．そこで，mapとodomの間の，z 軸のまわりの回転については，ロボットを起動した後に「ロボットの前方がmapの x 軸の方向と合うようにodomの向きを変更しなさい」という指令をしたいと考えています．これを実現するために，odomからbase_linkに至る回転を

z 軸まわりの回転
z 軸と直交な軸のまわりの回転

の 2 回の回転に分解すると良いと考えました．

表記に関する約束

三次元での回転はクォータニオンで表現し，太字で$$\boldsymbol{q}$$のように書くことにします．回転の合成については，$$\boldsymbol{q}$$による回転の後に続けて$$\boldsymbol{p}$$によって回転させると$$\boldsymbol{r}$$によって回転させたのと同じ結果を得られることを

$$ \boldsymbol{r}=\boldsymbol{p}\boldsymbol{q} $$

と書くことにします．ベクトルは矢印を使った$$\vec{v}$$で表記し，クォータニオンと混同しないようにします．クォータニオン$$\boldsymbol{q}$$によってベクトル$$\vec{v}$$を回転させる操作は，クォータニオンからベクトルにする操作を$$\mathrm{vect}$$，ベクトルからクォータニオンにする操作を$$\mathrm{quat}$$として丁寧に書くと

$$ \mathrm{vect}\left(\boldsymbol{q}\cdot \mathrm{quat}\left(\vec{v}\right) \cdot\bar{\boldsymbol{q}}\right) $$

のようになりますが，単に

$$ \boldsymbol{q}\vec{v} $$

と書くことにします．

解決する問題

回転を表すクォータニオン$$\boldsymbol{r}$$と一回目の回転の軸の向きを表す単位ベクトル$$\vec{e}_q$$を入力とし， 2 つの回転を表すクォータニオン$$\boldsymbol{p}, \boldsymbol{q}$$を求めます． 2 回の回転$$\boldsymbol{p}, \boldsymbol{q}$$は $$\boldsymbol{q}$$による回転の後に続けて$$\boldsymbol{p}$$によって回転させると$$\boldsymbol{r}$$によって回転させたのと同じ結果を得られることと， $$\boldsymbol{p}, \boldsymbol{q}$$の回転軸は直交する（または少なくとも一方は無回転である）ことの 2 つの条件を満たします．

解法

回転軸は回転によって変化しないので，$$\vec{e}_q=\boldsymbol{q}\vec{e}_q$$です．任意のベクトル$$\vec{v}$$で$$\boldsymbol{r}\vec{v}=\boldsymbol{p}\boldsymbol{q}\vec{v}$$が成り立ちます．ここに$$\vec{e}_q$$を代入すると

$$ \boldsymbol{r}\vec{e}_q = \boldsymbol{p}\boldsymbol{q}\vec{e}_q = \boldsymbol{p}\vec{e}_q $$

となります． 2 回目の回転$$\boldsymbol{p}$$は$$\vec{e}_q$$を$$\boldsymbol{r}\vec{e}_q$$に動かすことが確認できました．

回転軸に垂直なベクトルは，回転後も回転軸に垂直です． 2 つの回転$$\boldsymbol{p}, \boldsymbol{q}$$の軸は直交（$$\vec{e}_p\perp\vec{e}_q$$）していることから， $$\vec{e}_p\perp\boldsymbol{p}\vec{e}_q$$です． $$\boldsymbol{r}\vec{e}_q = \boldsymbol{p}\vec{e}_q$$ だったので

$$ \vec{e}_p\perp\boldsymbol{r}\vec{e}_q $$

が導けます．

2 回目の回転$$\boldsymbol{p}$$は$$\vec{e}_q$$を$$\boldsymbol{r}\vec{e}_q$$に動かすことと，回転軸$$\vec{e}_p$$は 2 つのベクトル$$\vec{e}_q$$と$$\boldsymbol{r}\vec{e}_q$$の両方に垂直であることから，ほとんどの場合，$$\boldsymbol{p}$$の回転軸$$\vec{e}_p$$は

$$ \vec{e}_p=\frac{\vec{e}_q\times\boldsymbol{r}\vec{e}_q}{\left|\vec{e}_q\times\boldsymbol{r}\vec{e}_q\right|} $$

で，回転角$$\theta_p$$は

$$ \theta_p=\cos^{-1}{\left(\vec{e}_q\cdot\boldsymbol{r}\vec{e}_q\right)} $$

です．以上から$$\boldsymbol{p}$$が求まるので，$$\boldsymbol{q}$$は

$$ \boldsymbol{q}=\bar{\boldsymbol{p}}\boldsymbol{r} $$

で得られます．

例外は$$\left|\vec{e}_q\times\boldsymbol{r}\vec{e}_q\right|=0$$の場合です． $$\left|\vec{e}_q\right|=\left|\boldsymbol{r}\vec{e}_q\right|=1$$だから，この問題が起こるのは， $$\vec{e}_q=\boldsymbol{r}\vec{e}_q$$の場合と$$\vec{e}_q=-\boldsymbol{r}\vec{e}_q$$の場合に限られます．

$$\vec{e}_q=\boldsymbol{r}\vec{e}_q$$の場合，$$\boldsymbol{r}$$によって$$\vec{e}_q$$が変化しないことから， $$\vec{e}_q$$が回転軸だと分かります．そこで，$$\boldsymbol{q}=\boldsymbol{r}$$として， $$\boldsymbol{p}$$は無回転を表すクォータニオン$$w=1, x=y=z=0$$にすれば求める解になります．

$$\vec{e}_q=-\boldsymbol{r}\vec{e}_q$$の場合，$$\boldsymbol{r}$$によって$$\vec{e}_q$$と垂直な軸周りに 180 度回転していることがわかります．そこで，$$\boldsymbol{p}=\boldsymbol{r}$$とし， $$\boldsymbol{q}$$は無回転を表すクォータニオン$$w=1, x=y=z=0$$とすれば解が得られます．

以上の方法を C++で実装しました． Eigen というライブラリを使用しました． Actat/rotation-decompositionでテストに使ったコードとともに公開しています．

#include <eigen3/Eigen/Geometry>

std::tuple<Eigen::Quaterniond, Eigen::Quaterniond> decompose_rotation(
    Eigen::Quaterniond r,
    Eigen::Vector3d e_q) {
  e_q.normalize();
  r.normalize();
  Eigen::Vector3d re_q = r * e_q;
  if (re_q.isApprox(e_q)) {
    return std::forward_as_tuple(Eigen::Quaterniond(1, 0, 0, 0), r);
  } else if (re_q.isApprox(-1 * e_q)) {
    return std::forward_as_tuple(r, Eigen::Quaterniond(1, 0, 0, 0));
  } else {
    Eigen::Vector3d e_p = (e_q.cross(re_q)).normalized();
    double theta_p      = std::acos(e_q.dot(re_q));
    auto p              = Eigen::Quaterniond(
        std::cos(theta_p / 2), std::sin(theta_p / 2) * e_p.x(),
        std::sin(theta_p / 2) * e_p.y(), std::sin(theta_p / 2) * e_p.z());
    Eigen::Quaterniond q = p.conjugate() * r;
    return std::forward_as_tuple(p, q);
  }
}

テスト条件

クォータニオンとベクトルの組を入力し，条件を満たすクォータニオンの組が返されることを確認しました．

入力

無回転を表すクォータニオンを入力する場合
クォータニオンの回転軸と平行な回転軸を入力する場合
クォータニオンの回転軸と垂直な回転軸を入力する場合
ランダムなクォータニオンと回転軸の組を入力する場合

出力が満たすべき条件

クォータニオンそれぞれが回転を表すクォータニオン（大きさが 1）であること
2 回の回転が入力を分解したものであること（$$\boldsymbol{r}=\boldsymbol{p}\boldsymbol{q}$$であること）
2 つのクォータニオンの回転軸が直交すること（$$\vec{e}_p\cdot\vec{e}_q=0$$であること）

感想

同じ計算をしている人が見つからなかったので，モチベーションに誤りがあるかもしれません．こわいなぁ．