回転

回転。
回転行列。

roll / pitch / yaw

まず最初に、下記を定義します。

• x軸に対して時計回りの回転 : roll
• y軸に対して時計回りの回転 : pitch
• z軸に対して時計回りの回転 : yaw

本稿ではやりたいことがIMUセンサーを使って角度を取得することなので、完全な球の剛体を考えて、3次元の直交座標を考えて剛体の中心にとります。

分かりやすさのために球の最もZの値が大きいところの点を初期位置と考えます。

剛体のそれぞれの軸回りの回転を総合して「姿勢」と呼ぶことにします。
英語だとAttitudeだと思います。

それぞれの角度が軸の回りの「回転」の量であることを強く意識しましょう。

本稿と本稿に続く記事について下記とします。

• x軸に対して時計回りの回転 : roll : \(\theta\)
• y軸に対して時計回りの回転 : pitch : \(\phi\)
• z軸に対して時計回りの回転 : yaw : \(\psi\)

最初を姿勢を下記とします。

• roll=0deg
• pitch=0deg
• yaw=0deg

これを任意の軸回りに回転させます。これを3軸に分解します。回転後を現在の姿勢とします。
1つのアプリケーションの中ではz軸回転→y軸回転→x軸回転やx軸回転→y軸回転→z軸回転など順番を決めて必ず守る必要があります。

この動画の赤い線はこちら向きのx軸なのでrollについて反時計回りの回転となります。
微小な回転ごとに姿勢が決まると考えます。

• roll=-10deg
• pitch=0deg
• yaw=0deg

図を描いて任意の位置に点を1つ選ぶとそれぞれ3つの角度が決定します。回転の順番を変えてみましょう。

直交座標の利用

現在の姿勢に対して、決めた順番(もしくは決められた順番)でそれぞれの角度を変化させることで回転させます。
任意の軸を考えるより、直交座標を利用した方が簡単になる、と思います。

プログラムを作成するときの単位について

回転のときの単位はradianです。
atan2f()など、自分の知る限り全て単位はradianなので、これに合わせるしかないと思います。
自分の知る限りIMUセンサーの角速度の単位は[degree / sec]なので[radian / sec]に変換する必要があります。

得られた角度を使って次の動作を決める場合はradianよりもdegreeの方が分かりやすいと思いますので出力はdegreeに再変換することになると思います。

二次元上の回転

点\((3,1)\)を円周上に束縛して点\((1,3)\)に移動させることを考えます。適当に選んだ値です。出来るだけ具体化させるために値を入れています。
点\((x,y)\)を円周上に束縛して点\((x’,y’)\)に移動させることを考えます。

直交座標と極座標で表します。
加法定理の証明 4
加法定理の証明 5
古典力学 5 (座標 / 一般化座標)

\[
\begin{gather*}
x=r\cos(\alpha)=3 \\
y=r\sin(\alpha)=1
\end{gather*}
\]

\[
\begin{gather*}
x’=r\cos(\alpha+\beta)=1 \\
y’=r\sin(\alpha+\beta)=3
\end{gather*}
\]

\[
\begin{gather*}
x’=r\cos(\alpha+\beta)=r\cos\alpha\cos\beta-r\sin\alpha\sin\beta \\
y’=r\sin(\alpha+\beta)=r\sin\beta\cos\alpha+r\sin\alpha\cos\beta
\end{gather*}
\]

\[
\begin{gather*}
x’=r\cos(\alpha+\beta)=r\cos\alpha\cos\beta-r\sin\alpha\sin\beta=x\cos\beta-y\sin\beta \\
y’=r\sin(\alpha+\beta)=r\sin\beta\cos\alpha+r\sin\alpha\cos\beta=x\sin\beta+y\cos\beta
\end{gather*}
\]

これで二次元の回転行列が簡単に構成できます。
単位円で考えて\(r=1\)とすると、式が上記より簡単になります。議論の内容は同じです。

\[
\begin{bmatrix}
x’ \\
y’
\end{bmatrix}
=
\begin{bmatrix}
\qquad & \qquad \\
\qquad & \qquad
\end{bmatrix}
\begin{bmatrix}
x \\
y
\end{bmatrix}
\]

ここまで整理したら上記の空白を埋められない人はいないでしょう。
(マウスオーバーで文字が見えるようになります)

三次元上の回転(roll)

rollについて、yz平面と平行な面を考えます。
これはx軸に沿って自由に移動できるとします。
これを\((平行な面)_{yz}\)とします。

現在の位置を\((x,y,z)\)とします。
回転した後の位置を\((x’,y’,z’)\)とします。

\[
x’=x
\]

\[
\begin{bmatrix}
y’ \\
z’
\end{bmatrix}
=
\begin{bmatrix}
\cos\beta & -\sin\beta \\
\sin\beta & \cos\beta
\end{bmatrix}
\begin{bmatrix}
y \\
z
\end{bmatrix}
\]

これで三次元の回転行列が簡単に構成できます。

\[
\begin{bmatrix}
x’ \\
y’ \\
z’
\end{bmatrix}
=
\begin{bmatrix}
\qquad & \qquad & \qquad \\
\qquad & \qquad & \qquad \\
\qquad & \qquad & \qquad
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
\]

ここまで整理したら上記の空白を埋められない人はいないでしょう。
(マウスオーバーで文字が見えるようになります)

この行列を\(R_{roll}\)とします。回転行列です。

三次元上の回転(pitch & yaw)

省略します。

これらの行列を\(R_{pitch}\)と\(R_{yaw}\)とします。回転行列です。

回転行列

ここではroll-pitch-yawを使うとしているので、下記となります。

\[
\begin{bmatrix}
x’ \\
y’ \\
z’
\end{bmatrix}
=R_{yaw}R_{pitch}R_{roll}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
\]

\(R_{yaw}R_{pitch}R_{roll}\)も回転行列です。正しいかどうかは知りません。

広告