Rustとquaternion-coreで始める四元数（クォータニオン）入門

はじめに

この記事は、最近流行りのRust言語と、筆者が開発した四元数演算用ライブラリであるquaternion-coreを使って四元数の計算をしてみよう！という記事です。

「わかりにくい」「とっつきにくい」と言われることの多い四元数ですが、いきなり数式を見て悩む前に、まずは何かしら具体的な計算してみることで親しみが持てるのではないかと思います。あと、折角なら私が開発したライブラリを使ってくれたら嬉しいな、というのもこの記事を執筆した理由の一つです。

本記事ではRustのインストール方法や言語仕様などには触れませんが、Rustについて詳しくなくても要点はわかるように書いたつもりです。最低限cargoコマンドが使用できる状態になっていれば大丈夫です。

quaternion-core について

quaternion-coreは筆者が大学生の頃に作り始めた四元数演算用ライブラリです。四元数の基本的な演算から回転補間まで、三次元回転を扱う際に必要な機能を幅広く提供しています（ドキュメンテーションページはこちら）。

オイラー角、方向余弦行列、axis/angle、回転ベクトルといった各種回転表現との相互変換も可能で、特に24種類のオイラー角との相互変換ができるのは他の四元数演算ライブラリと比べてもかなり珍しいと思います。

また、quaternion-coreはマイコン上での使用を想定しており、no_std環境での実行に対応しています。これは単にno_std環境でも同じように計算できるという話ではなく、fma feature や norm-sqrt feature を指定することでコンパイル時に計算方法を切り替え、計算量を削減することができます。これは計算リソースの限られた環境において大きなメリットとなります。

元々は自分が使うために作り始めたライブラリですが、最近は予想以上に多くの方に使って頂けているようで、Rustの公式パッケージレジストリであるcrates.ioでは32,000ダウンロードに達しています（2024年10月17日時点）。

因みに、演算子オーバーロードを活用してより数式に近い書き方ができるquaternion-wrapperというライブラリも作りました。話がややこしくなるので今回は使用しませんが、複雑な数式を手早く実装したいときは便利だと思います。

下準備

Rustで計算していくにあたり、まずは新しいプロジェクトファイルを作りましょう。適当なディレクトリで以下のコマンドを実行して、sampleという名前のクレートを作ります（クレート名は何でもいいです）。

$ cargo new --bin sample

上記のコマンドを実行すると、以下のような構造のフォルダが作られます。

.
└── sample
　　　　├── Cargo.toml
　　　　└── src
　　　　　　　└── main.rs

次に、quaternion-coreを使えるようにするためCargo.tomlファイル内の[dependencies]セクションに以下のように追記します。

[dependencies]
quaternion-core = "0.5"

あとはmain.rsの先頭に

use quaternion_core as quat;  // プログラム内では名前にハイフンを使えないので、アンダーバーに置き換えます。

と記述しておくことで、quat::add(a, b)のようにしてquaternion-coreの機能を使用することができます。

ベクトルを回す

では早速具体的な計算を行っていきましょう。

あまり複雑にすると確認しにくくなるので、ここでは三次元ベクトル$( \boldsymbol{a} = [1 , 0 , 1] $)を$( \mathrm{Z} $)軸回りに$( \pi / 2 $)ラジアンだけ回して、回転後のベクトル$( \boldsymbol{b} $)を求めることにします。

※本記事では、見易さのためにベクトルを $( \boldsymbol{a} = [a_1, a_2, a_3] $) のように表記します。

この回転を図で表すと以下のようになります。この例では回転後のベクトルが$( \boldsymbol{b} = [0, 1, 1] $)となることは半ば自明ですが、任意の回転軸・回転角に対して考えられるのが四元数の強みです。

ベクトル回転の図解1

計算を行うために、まずは回転させるベクトル$( \boldsymbol{a} $)の変数を用意します。 quaternion-coreでは要素数3の配列をVector3型として扱っており、変数$( \boldsymbol{a} $)を定義するには以下のように書きます。

let a: quat::Vector3<f64> = [1.0, 0.0, 1.0];  // 各要素がf64(倍精度)のVector3型を定義

因みに、Rustには型推論があるので型名を省略して

let a = [1.0, 0.0, 1.0];

と書くことも出来ます。

ベクトルが用意できたので、次は$( \mathrm{Z} $)軸回りに$( \pi / 2 $)ラジアンの回転を表す四元数を作ります。回転を表す四元数は、回転軸を $( \boldsymbol{n} (\| \boldsymbol{n} \| = 1) $) 、軸回りの回転角を$( \theta $)としたときに

$[ \begin{equation*} \tilde{q} = \cos \frac{\theta}{2} + \boldsymbol{n} \sin \frac{\theta}{2} \end{equation*} $]

と書くことができます。今回は$( \boldsymbol{n} = [0, 0, 1] $)かつ$( \theta = \pi / 2 $)なので

$[ \begin{align*} \tilde{q} &= \cos \frac{\theta}{2} + \boldsymbol{n} \sin \frac{\theta}{2} \\ &= \cos \frac{\theta}{4} + [0, 0, 1] \sin \frac{\theta}{4} \\ &\approx (0.707, [0, 0, 0.707]) \end{align*} $]

となります。quaternion-coreでは、from_axis_angle関数を使うことで上記の四元数を作ることが出来ます。

let q: quat::Quaternion<f64> = quat::from_axis_angle([0.0, 0.0, 1.0], PI / 2.0);

以上で、回されるベクトル$( \boldsymbol{a} $)と、所望の回転を表す四元数$( \tilde{q} $)を用意できました。 $( \tilde{q} $)が表す回転を$( \boldsymbol{a} $)に作用させ、回転後のベクトル$( \boldsymbol{b} $)を得るには、以下の式を用います。

$[ \begin{equation*} \boldsymbol{b} = \tilde{q} \boldsymbol{a} \tilde{q}^* \end{equation*} $]

ここで、$( \tilde{q}^* $)は$( \tilde{q} $)の共役四元数を表します。

四元数の積はmul関数で、共役四元数はconj関数で計算できますので、上式を計算するには以下のようにします。

let tmp: quat::Quaternion<f64> = quat::mul(q, quat::mul((0.0, a), quat::conj(q)));  // aは実部がゼロの四元数として扱う
let b: quat::Vector3<f64> = b.1;  // タプルからベクトル部だけ取り出してbを定義．

四元数同士の積は四元数になるので、変数tmpは四元数型になっています。証明は省きますが、この計算では実部が必ずゼロとなるのでtmpからベクトル部を取り出して最終的な計算結果bを得ています（Quaternion型はタプルで実数型とVector3型をくっつけたものになっています）。

このように愚直に式を実装しても良いですが、ベクトルを回す式はよく使われるものなので、quaternion-coreではpoint_rotation関数を使ってより簡潔に書くことが出来ます。

let b = quat::point_rotation(q, a);

見た目もシンプルですし、内部実装的にもゼロの掛け算を無くしている分だけ計算量が少ないのでpoint_rotation関数を使ったほうが良いです。

ここまでの内容をまとめると、main.rsの中身は以下のようになります。

use quaternion_core as quat;
const PI: f64 = std::f64::consts::PI;  // 円周率

fn main() {
    let a = [1.0, 0.0, 1.0];  // ベクトルaを定義
    let q = quat::from_axis_angle([0.0, 0.0, 1.0], PI / 2.0);  // 回転を表す四元数を作る
    let b = quat::point_rotation(q, a);  // ベクトルを回す

    println!("q: {:?}", q);
    println!("a: {:?}", a);
    println!("b: {:?}", b);
}

これを実行すると、次の計算結果が得られます。

$ cargo run
q: (0.7071067811865476, [0.0, 0.0, 0.7071067811865475])
a: [1.0, 0.0, 1.0]
b: [2.220446049250313e-16, 1.0, 1.0]

数値誤差の関係で若干のズレはあるものの、ほぼ$( \boldsymbol{b} = [0, 1, 1] $)となっています。最初に示した図をもう一度確認してみてください。確かに正しい結果が得られていることがわかると思います。

余談ですが、直交座標系の模型（下図参照）を手に持ちながら考えるとかなり理解の手助けになります。

あると便利な直交座標系模型

回転の合成

回転中心が同じである場合、複数の回転は1つの回転にまとめることが出来ます。

つまり、以下のように$( \boldsymbol{a} $)から$( \boldsymbol{b} $)、 $( \boldsymbol{b} $)から$( \boldsymbol{c} $)という2つの回転を行った場合、 2つの回転を合成して$( \boldsymbol{a} $)から$( \boldsymbol{c} $)への回転を表す四元数を作ることができます。

ベクトル回転の図解2

回転をまとめて何になるんだと思うかもしれませんが、例えば多関節アームロボットの手先姿勢（基準面に対する回転量）は各関節の回転角度を合成した結果になっているため、そういったところで役に立ちます。

細かいことはさておき、早速計算してみましょう。四元数による回転の合成は非常に簡単で、四元数同士の積が回転の合成となっています。

つまり、1つめの回転を表す四元数を $( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{b} } $) 、2つめの回転を表す四元数を $( \tilde{q}_{ \boldsymbol{b} \to \boldsymbol{c} } $) としたとき、これらを合成した四元数 $( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } $) は

$[ \begin{equation*} \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } = \tilde{q}_{ \boldsymbol{b} \to \boldsymbol{c} } ~ \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{b} } \end{equation*} $]

より計算でき、プログラムでは以下のように書きます（四元数の積は非可換なのでこの順序が重要です）。

let q_a2b = quat::from_axis_angle([0.0, 0.0, 1.0], PI / 2.0);  // 1つめの回転を表す四元数
let q_b2c = quat::from_axis_angle([0.0, 1.0, 0.0], PI / 2.0);  // 2つめの回転を表す四元数

let q_a2c = quat::mul(q_b2c, q_a2b);  // 合成結果

さて、このようにして合成した四元数はどのような回転軸・回転角になっているのでしょうか。quaternion-coreではto_axis_angle関数を使うことで以下のようにして回転軸と回転角に変換できます。

let (axis, angle) = quat::to_axis_angle(q_a2c);

ここまでの内容をまとめると、main.rsの中身は以下のようになります（先程のプログラムに付け足しても構いません）。

use quaternion_core as quat;
const PI: f64 = std::f64::consts::PI;  // 円周率

fn main() {
    let q_a2b = quat::from_axis_angle([0.0, 0.0, 1.0], PI / 2.0);  // 1つめの回転を表す四元数
    let q_b2c = quat::from_axis_angle([0.0, 1.0, 0.0], PI / 2.0);  // 2つめの回転を表す四元数
    let q_a2c = quat::mul(q_b2c, q_a2b);  // 合成結果

    // 回転軸と回転角がどうなっているか見てみる
    let (axis, angle) = quat::to_axis_angle(q_a2c);

    // 一応ベクトルを回して確認
    let a = [1.0, 0.0, 1.0];
    let c = quat::point_rotation(q_a2c, a);

    println!("axis: {:?}, angle: {}[deg]", axis, angle * (180.0 / PI));
    println!("a: {:?}", a);
    println!("c: {:?}", c);
}

これを実行すると、次の計算結果が得られます。

$ cargo run
axis: [0.5773502691896257, 0.5773502691896258, 0.5773502691896258], angle: 119.99999999999999[deg]
a: [1.0, 0.0, 1.0]
c: [1.0, 0.9999999999999999, 1.1102230246251565e-16]

to_axis_angle関数はノルムが1の回転軸ベクトルを返すので0.577…という値をとっていますが、3軸全て同じ値なので回転軸は$( [1, 1, 1] $)と同じです。また、軸回りの回転角は120度となっています。

$( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } $) が表す回転を回転軸と回転角の形でイメージするのは少し難しいですが、確認のためにベクトル$( \boldsymbol{a} $)を回した結果は$( \boldsymbol{c} = [1, 1, 0] $)となっており、確かに正しい回転を表していることが確認できます。

オイラー角に変換してみる

せっかくなので、前項で求めた $( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } $) をオイラー角に変換してみましょう。回転軸と回転角に変換した場合よりは多少イメージしやすいかもしれません。

ところで、本記事の冒頭で「quaternion-coreは24通りのオイラー角との相互変換ができる」と述べたのを覚えているでしょうか。オイラー角は回転順序によって12通りの表現が存在していることはご存知の方も多いと思いますが、更に

固定した座標系の各軸（$( X, Y, Z $)）を回転軸とする場合（Extrinsic rotation）
物体と共に回転する座標系の各軸（$( x, y, z $)）を回転軸とする場合（Intrinsic rotation）

という、回転軸をどの座標系でとるかという区別があるため、全部で24通りの表現が存在します。一例として、最初にy軸回りに回転させて、次にz軸回りに回転させる場合を図にしてみました。

ExtrinsicとIntrinsicの違い

回転順序で違いが出るのはもちろんですが、どの座標系の軸回りに回転させるかによっても大きく違ってくることがわかると思います。

なので、回転表現を扱うライブラリにto_euler_anglesやfrom_euler_anglesのような名前の関数が存在したら、まずその関数で変換されるオイラー角が24種類あるうちのどれなのかを良く確認しなければなりません。そもそも回転軸のとり方や回転順序に言及していないライブラリもあるので要注意です。

前置きが長くなってしまいましたが、ようやく $( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } $) をオイラー角に変換していきます。全て同じ回転を表せるので24種類あるうちのどれに変換しても良いのですが、ここでは航空系でよく使われている（気がする）IntrinsicのZ-Y-X系オイラー角に変換することにします。飛行機で言うとYaw-Pitch-Rollの順番で回転を表すものです。

quaternion-coreではto_euler_angles関数を使うことで24種類全てのオイラー角に変換できます。回転軸のとり方はRotationType enumで指定し、回転順序はRotationSequence enumで指定します。今回はRotationTypeとRotationSequenceの要素がそれぞれIntrinsicとZYXになるので、以下のように書きます。

let zyx_radian = quat::to_euler_angles(quat::RotationType::Intrinsic, quat::RotationSequence::ZYX, q_a2c);

to_euler_angles関数ではオイラー角をVector3型に入れて返します。また、この時点では角度の単位がラジアンになっているので、ラジアンから度に変換するためにscale関数を使って以下のようにします。

let zyx_degree = quat::scale(180.0 / PI, zyx_radian);

ここまでの内容をまとめると、main.rsの中身は以下のようになります。

use quaternion_core as quat;
const PI: f64 = std::f64::consts::PI;  // 円周率

fn main() {
    let q_a2b = quat::from_axis_angle([0.0, 0.0, 1.0], PI / 2.0);  // 1つめの回転を表す四元数
    let q_b2c = quat::from_axis_angle([0.0, 1.0, 0.0], PI / 2.0);  // 2つめの回転を表す四元数
    let q_a2c = quat::mul(q_b2c, q_a2b);  // 合成結果

    // オイラー角に変換
    let zyx_radian = quat::to_euler_angles(quat::RotationType::Intrinsic, quat::RotationSequence::ZYX, q_a2c);
    let zyx_degree = quat::scale(180.0 / PI, zyx_radian);  // [rad] ---> [deg]

    println!("ZYX Euler angles: {:?}", zyx_degree);
}

これを実行すると、次の計算結果が得られます。

$ cargo run
ZYX Euler angles: [90.0, 1.2722218725854067e-14, 89.99999999999999]

計算結果を見ると、$( z $)軸周りに90度、$( y $)軸周りに0度、$( x $)軸周りに90度の順に回転させれば、ベクトル$( \boldsymbol{a} $)から$( \boldsymbol{c} $)を得られることがわかります。この回転を図にすると以下のようになります。

オイラー角の表す回転

上図より、確かに $( \tilde{q}_{ \boldsymbol{a} \to \boldsymbol{c} } $) からオイラー角への変換が正しく行われていることが確認できました。

オイラー角が直感的に分かりやすいかと言われると「それはどうかな…？」と思ってしまいますが、回転軸と回転角に比べればまあ頑張ったらイメージできそうという希望が持てる感じはありますね。

おわりに

どうでしょう、記事中で行った3つの計算例を通して少しは親しみが持てたでしょうか。ここで示した計算例はどれも簡単なものでしたが、物体の回転を扱う上で基礎となる計算です。

本記事では四元数の数学的詳細にはほとんど触れなかったので、四元数について詳しく知りたい方は以前執筆した『四元数まとめ資料』をご覧ください。 quaternion-coreで提供している演算はほぼ全て載っていますし、回転の合成を行う際になぜその順序で掛けるのかといったところもこの資料の中で触れています。