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

3月 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$であること）

感想

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

ものづくり