3次元ベクトルの回転「ロール・ピッチ・ヨー」

9軸センサ制御シリーズの目次はこちら

はじめに

現在、9軸センサー(加速度・角速度・方位それぞれ3軸)を用いた制御を勉強中です。

これらのセンサ情報を用いると、現在の「姿勢」を知ることができ、例えば今流行りのドローンの制御などに応用できます。

「姿勢」とは、とどのつまり「基準位置(例えば地面と水平で、北を向いた状態とか)」から、どれくらい「回転」した状態であるか、と捉えることができます。

このように、姿勢を考えるにあたって「3次元ベクトルの回転」の知識が必須となるため、何回かに分けてまとめてみようと思います。

今回は、回転を表す方法のひとつである「ロール・ピッチ・ヨー」について、回転行列の数式を含めて解説しようと思います。

まずは、イメージを捉えるため、こんな動画を作ってみました。

なお、今後解説する予定の知識は「3D-CGプログラマーのためのクォータニオン入門」にて勉強しました。線形代数の復習から解説されている本で、説明もとても分かりやすい素晴らしい本でした。

ベクトルと行列の表記について

本ブログでは、ベクトルを「丸カッコ \(()\) 」、行列を「角カッコ \([]\) 」で表記しています。

というのも、ベクトルは中身の要素が重要なのであって、縦横の並びは本質的には意味がありません。一方、行列は縦と横のどちらに並べるかによって、意味や演算結果が変わります

具体的に、ベクトルの表記には縦長の「列ベクトル \( \left( \begin{array}{c} a \\ b \end{array} \right) \) 」と、横長の「行ベクトル \( (a,b) \) 」がありますが、意味は同じです。

しかし、行列の \( \left[ \begin{array}{c} a \\ b \end{array} \right] \) と \( [a\ b] \) では、意味や演算方法が異なります。

ここでは深く触れませんが、そもそも数学的にベクトルとは「線形和・内積・回転」ができるもの全般であり、それを満たすもののひとつが「行列」や「複素数」である、という関係にあります(この辺の詳細は、クォータニオンの解説の際に再度触れることになると思います)。

ただ、表記が似ていることもあり、高校の教科書だと区別なく記載されていたりします。

しかし、ベクトルと行列の区別なしに深い議論をしようとすると、混乱を招くことがあります。

このため、本ブログでは表記を使い分けつつ、必要に応じてベクトルを行列とみなして演算する、といった方法で解説しています。

ベクトルの回転方法

主に、3次元のベクトルを回転させる方法は、大雑把に言って次の3つがあります。
----
1. 回転行列
2. 外積の性質を利用
3. クォータニオン
----

それぞれに利点・欠点がありますが、今回は1番の「回転行列」を用いた方法について説明します。

まずは2次元で考え、その後3次元へ拡張してみます。

最後に、姿勢を表すのに使用されることの多い「ロール・ピッチ・ヨー」についても解説します。

2次元での回転

まずは、高校数学の復習も兼ねて、回転行列を使った2次元の回転を考えてみます。

以下のような、長さ \(r\) の \(\vec{p}\) に対して、 \(\phi\) 回転させた \( \vec{p'} \) を求めてみます。

ここで、\(\vec{p}\) と \(\vec{p'}\) の関係を調べるため、\(\vec{p'}\)の \( x, y \) 成分をそれぞれ加法定理で分解してみます。

$$
p'_x = r \cos(\theta+\phi) = r \cos\theta \cos\phi - r \sin \theta \sin\phi = p_x \cos\phi - p_y \sin\phi \\
p'_y = r \sin(\theta+\phi) = r \sin\theta \cos\phi + r \cos \theta \sin\phi = p_y \cos\phi + p_x \sin\phi
$$

これらをまとめて行列として表現すると、以下のようになります。

$$
\left[ \begin{array}{c}
p'_x \\
p'_y
\end{array} \right]
=
\left[ \begin{array}{cc}
\cos\phi & -\sin\phi \\
\sin\phi & \cos\phi \\
\end{array} \right]
\left[ \begin{array}{c}
p_x \\
p_y
\end{array} \right]
$$

つまり、あるベクトル \( \vec{p} \) を行列 \( \left[ \begin{array}{c} p_x \\ p_y \end{array} \right] \) とみなした時、それを \( \phi \) だけ回転させるためには、

$$
\left[ \begin{array}{cc}
\cos\phi & -\sin\phi \\
\sin\phi & \cos\phi \\
\end{array} \right]
$$

を掛ければよいことがわかりました。このような行列を「回転行列」といい、 \( R(\phi) \) などと表記します(Rは回転を意味するRotationより)。

注意点として、あるベクトルに回転行列を適用する場合は、上記の通り必ず「左から掛ける」事です。

回転行列の合成

次に、回転を複数回適用したとき、どうなるかを考えてみます。

例えば、90度回転して、そのあと-30度回転させてみる。

すると「あれ、これって60度の回転を一回だけするのと同じじゃない?」と気づくかと思います。

その通りで、この例は「90度と-30度の複数回の回転を、60度回転という一つの回転に合成した」ことを意味します。

ただ、3次元になるとこう単純にはいかないので、今のうちに回転の合成を数式で表しておきます。

まず、 \( \phi \) 回転、\( \psi \) 回転させる回転行列をそれぞれ \(R(\phi), R(\psi)\) とします。

すると、 \( \vec{p} \) を \( \phi \) 回転し、さらに \( \psi \) 回転した \(\vec{p'}\) は、次のように表すことができます。

$$
\vec{p'} = R(\psi) R(\phi) \vec{p}
$$

\(\vec{p}\)に近い行列から、回転が適用されることに注意してください(記号順が文章と逆になる)。

行列の性質から、先に\( R(\psi) R(\phi) \) を計算することが可能で、その結果はそれぞれの回転を合成した回転行列になります。

先ほどの合成の例で出てきた「60度回転すればいいじゃん」の60度を、90度と-30度から計算した事と同じイメージですね。

合成された回転行列を求めておくと、行列計算の回数が減ったり、場合によっては計算精度が上がったりするので、色々と有用です。

3次元への拡張

さて、2次元の性質を踏まえた上で、3次元へ拡張してみます。

\(z軸回転\)

まず、\(z軸\)を中心とした回転を考えてみます。

以上より、\(z\)軸まわりに \(\theta\) 回転させる回転行列 \(R_z(\theta)\) は以下のようになります。

$$
R_z(\theta) =
\left[ \begin{array}{ccc}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{array} \right]
$$

試しに \(\vec{p}=(p_x, p_y, p_z)\) を行列 \( \left[ \begin{array}{c} p_x \\ p_y \\ p_z \end{array} \right] \) とみなし、z軸まわりに回転させた \(\vec{p'}\) を求めてみると

$$
\vec{p'} =
R_z(\theta)\vec{p} =
\left[ \begin{array}{ccc}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{array} \right]
\left[ \begin{array}{c}
p_x \\
p_y \\
p_z
\end{array} \right]
= \left[ \begin{array}{c}
p_x \cos\theta - p_y \sin\theta \\
p_x \sin\theta + p_y \cos\theta \\
p_z \\
\end{array} \right]
$$

ということで、確かに \(p'_x,p'_y\) は二次元と同じ結果、 \(p'_z\) は \( p_z \) そのままの値、という結果になりますね。

\(y軸回転\)

先ほどの考え方と同様、\(y\)軸まわりの回転では、 \(y\) の値が変わりません。

なので、\(zx\)平面の回転だけ考えると、回転行列 \(R_y(\theta)\) は以下の通り。

$$
R_y(\theta) = \left[
\begin{array}{ccc}
\cos\theta & 0 & \sin\theta \\
0 & 1 & 0 \\
-\sin\theta & 0 & \cos\theta
\end{array} \right]
$$

\(x軸回転\)

同様に、\(x\)軸まわりの回転行列は以下の通り。

$$
R_x(\theta) = \left[
\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos\theta & -\sin\theta \\
0 & \sin\theta & \cos\theta
\end{array} \right]
$$

ロール・ピッチ・ヨー

2次元の時と同様、複数の3次元の回転行列を適用することが可能であり、これによって複雑な回転を表現することができます。

ただし注意が必要で、2次元の時と違って「回転させる順番」によって、結果が異なります。

具体的に、2次元の場合は「90度回転して、-30回転」と「-30回転して、90度回転」は、共に同じ60度回転を意味しましたが、3次元では回転軸が複数あるため、必ずしもそうなりません。

このため、目的によって掛ける順番に色々な作法があります。

よく使われる作法の一つが「ロール・ピッチ・ヨー」であり、\(x軸 ⇒ y軸 ⇒ z軸\)の順で、回転行列を適用する方法です。

これを先ほど登場した、各軸まわりの回転行列で表現してみましょう。

ロール・ピッチ・ヨーの回転角をそれぞれ \( r, p, y \) とした場合、 \( \vec{p} \) を回転行列により回転した結果\( \vec{p'} \) は、次のように表現できます。

$$
\vec{p'} = R_z(y)R_y(p)R_x(r) \vec{p}
$$

また、2次元の時と同様、回転行列の合成も可能です。合成した結果は以下のようになります。

$$
R_z(y)R_y(p)R_x(r) =
\left[ \begin{array}{ccc}
C_y C_p & C_y S_p S_r - S_y C_r & C_y S_p C_r + S_y S_r \\
S_y C_p & S_y S_p S_r + C_y C_r & S_y S_p C_r - C_y S_r \\
-S_p & C_p S_r & C_p C_r
\end{array} \right]
$$
(ただし、\(C_\theta = \cos\theta, S_\theta = \sin\theta\) を表す)


以上の方法により、回転行列を用いることで、3次元の任意の回転を表現することが可能になります。

回転行列とロール・ピッチ・ヨーの位置づけ

今回は、X,Y,Z軸周りの回転行列に限って記載しました。

しかし、回転行列は実はもっと凄い奴で、X,Y,Z軸以外の任意の軸周りの回転までも表現可能な、とても汎用性の高い手法です。
(今回は深く触れませんが、頭の片隅に置いといてください)

ただその汎用性がゆえに、回転行列をぱっと見て、それがどんな回転なのか想像しにくい、という欠点があります。

そこで、より直感的な方法として、回転をX,Y,Z軸周りに限定し、その組み合わせて任意の回転を表現する方法の一つが「ロール・ピッチ・ヨー」です。

ただ、導入動画の最後に出てきた通り、弱点として「ジンバルロック」という現象があります。

また、今回は触れませんでしたが、実はやっかいな座標系の話も絡んできたりします。このため、本やネットでロール・ピッチ・ヨーで情報を探すと「あれ、自分が知ってる数式となんか違うな…。」という事態が出てきます。

このため、座標系の話は今後、解説しようと思っています(固定角とオイラー角の考え方の違い等)。

おわりに

今回は初めて、数式を絡めた記事を書いてみました。

本当はクォータニオンについて勉強していたのですが、自分の備忘録と数式記事の練習を兼ねて、まずは回転行列についてまとめてみました。

初めに述べたように、3次元ベクトルの回転にはいくつか方法があり、それぞれに長所短所があります。

なので、もう少し解説を進めたのち、それらを比較してみたいと思っています。

また、ただ数式を説明して終わりではなく、それを生かした作品も今後紹介できればと思っています。

ということで、また次回をお楽しみに。