ホーム / MPU6050 / ラズパイ /

MPU6050で現在の姿勢を推定する~3次元の回転についての勉強編~

2022/02/21

MPU6050 ラズパイ

水平状態でMPU6050から加速度を取得できるようになりました。しかし設置場所によっては水平に置くことが出来ずに、出力結果にバイアスがかかってしまう場合があります。

調べてみると座標を回転させることで補正が出来そうだという事が分かったので、現在どのような姿勢(グローバル座標から何度傾いているか)を求めて補正を掛けていきたいと思います。

初めて座標の回転について勉強したため間違った情報があればご指摘お願いいたします。

今回はこちらの記事を元に他の記事も参考にしながら勉強していきます。

(３次元における回転座標変換行列)

2次元の回転について考える

いきなり3次元の回転について考えるのは難しいため、まずは二次元の回転について考えます。

まず初めに図１に示したように座標Pの点$\beta$回転させて、点Qに移動する場合を考えます。

図1：回転前後の座標

回転前の点Pは原点からの長さr、角度$\alpha$なので極座標形式であらわすと次のようになります。

\begin{equation}x_p = r\cos \alpha \\ y_p =r\sin \alpha \end{equation}

次に点Pを$\beta$回転させた点Qを考えます。点Qの角度は$\alpha+\beta$なので、(1)式同様に極座標形式であらわすと以下のようになります。

\begin{equation}x_q = r\cos (\alpha+\beta) \\ y_q =r\sin (\alpha+\beta) \end{equation}

(2)式を加法定理を使って展開すると以下のようになります。

\begin{equation}x_q = r(\cos \alpha \cos\beta - \sin\alpha\sin\beta) \\y_q =r(\sin\alpha \cos\beta + \cos\alpha\sin\beta) \end{equation}

この(3)式に(1)式を代入して整理すると以下のようになります。

\begin{equation}x_q = x_p\cos\beta - y_p\sin\beta \\y_q =x_p \sin\beta + y_p\cos\beta \end{equation}

最後にこれを行列形式であらわすと以下のようになり、回転行列を求めることが出来ます。

\begin{equation}\begin{bmatrix}x_q \\ y_q \end{bmatrix} = \begin{bmatrix}\cos\beta & -\sin\beta \\ \sin\beta & \cos \beta \end{bmatrix} \begin{bmatrix}x_p \\ y_p \end{bmatrix} \end{equation}

座標の回転を考える

先ほどまでは点の回転を考えてきましたが、今度は"座標"の回転を考えていきます。

図1のように$x - y$座標を$\beta$回転させて、$x^{\prime} - y^{\prime}$に変換する場合を考えます。

図2：今回考える座標の回転

この座標系に点Pを置いて、回転前の座標から見た点Pが回転後の座標から見ると次のように点Pが近づいてきて点Qに移動するように見えます。

図3：回転後の座標から見た点P

これは(5)式の$\beta$を$-\beta$に置換したのと同等なので次のように表せます。

(偶関数、奇関数の性質を使ってます。)

\begin{equation}\begin{bmatrix}x_q \\ y_q \end{bmatrix} = \begin{bmatrix}\cos\beta & \sin\beta \\ -\sin\beta & \cos \beta \end{bmatrix} \begin{bmatrix}x_p \\ y_p \end{bmatrix} \end{equation}

(5)式と(6)式を使っているサイトがありますが、点を移動させてるか座標を移動させてるかの違いだと思ってます。(この辺はきちんと理解できていないため、簡単な問題を考えて検算してみます。)

簡単な検算で(5)式、(6)式どちらがいいか確認

(5)式は点の回転、(6)式は座標の回転という事はわかったのですが、今回私が使用する際にどちらがいいかピンと来てないので、簡単な検算で確認してみます。

図4のようなグローバル座標系ではz軸方向に1G掛かっているが、ローカル座標系(加速度センサーの読み取り値)ではグローバル座標の値から30度傾いている値を出力する場合を考えます。

図4：グローバル座標(黒色)とセンサーが傾いた場合のローカル座標(青色)

今回は図4の座標軸で考えるので、(5)、(6)式の$x_q$,$y_q$は重力加速度とし、それぞれ$0$,$1$、$x_p$,$y_p$はセンサー読み取り値のx,zの加速度とし、それぞれ$\frac{1}{2}$,$\frac{\sqrt 3}{2}$とします。

以上を考慮すると(5)式は次のようになります。

\begin{equation}\begin{bmatrix}0 \\ 1 \end{bmatrix} = \begin{bmatrix}\cos 30 & -\sin 30 \\ \sin 30 & \cos 30 \end{bmatrix} \begin{bmatrix} \frac{1}{2}\\ \frac{\sqrt{3}}{2} \end{bmatrix} \end{equation}

(7)式の右辺を計算すると[0,1]となり、左辺と一致するため、センサー値をグローバル座標系に変換する場合は(5)式をベースに考えればよさそうだという事が分かりました。

逆にグローバル座標系の加速度を回転させたい場合は(6)式を使えばよさそうです。

簡単な検算になりますが、以下のようになります。先ほどと同様左辺を計算すれば右辺と一致するため問題なさそうです。

\begin{equation}\begin{bmatrix}\cos 30 & \sin 30 \\ -\sin 30 & \cos 30 \end{bmatrix} \begin{bmatrix}0 \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{1}{2}\\ \frac{\sqrt{3}}{2} \end{bmatrix} \end{equation}

後ほど勉強して分かったのですが、回転行列は直行行列で逆行列＝転置行列なので、知ってる人からすると当たり前の計算をやっただけっぽいですね。。。

3次元の回転の座標について

右手系と左手系

3次元の回転に考える前に3次元の座標系について勉強します。

3次元の座標系には右手系と左手系の2種類があります。

図5：右手系と左手系

右手系、左手系ともに親指がx軸、人差し指がy軸、中指がz軸となってます。

右手系と左手系については下記記事がためになりました。

なぜ座標には右手系と左手系があるのか

この記事によると右手系は学術系などに用いられ、左手系は計算処理やゲームなどに使われているみたいです。今回は学術系ベースの右手系で考えていきます。

ロール/ピッチ/ヨー角

次に回転角の表現について勉強します。

3次元の回転なので、x軸周りの回転、y軸周りの回転、z軸周りの回転の3種類があります。

x軸周りの回転はロールといい$\varphi$で表します。y軸周りの回転はピッチといい$\theta$で表します。z軸周りの回転はヨーといい$\psi$で表します。

各回転については図5を参照して下さい。(ちなみにですが、ロボット工学だと定義が異なるみたいです。)

図6：ロール/ピッチ/ヨー(wikipediaより)

3次元の回転について考える

z軸周りの回転を考える

前置きが少し長くなりましたが、いよいよ3次元の回転について考えていきます。最初はz軸周りの回転を考えます。

最初にx軸周りではなく、z軸周りの回転を考える理由は横軸がxで、縦軸がyで図2と同じ座標系として考えることが出来るからです。

図7：z軸周りの回転

この回転はz軸が回転しないため、(5)式を元にz軸成分を加えて次のように表せます。

(参考にした記事：３次元における回転座標変換行列では(6)式をベースにしていますが、私が今回したい回転は(5)式なので、表現が異なってます。転置を取れば参考文献と同じ形式になるので、注意をお願いします。)

\begin{equation}\begin{bmatrix}x^{\prime}\\ y^{\prime} \\z^{\prime}\end{bmatrix} = \begin{bmatrix}\cos\psi & -\sin\psi & 0\\ \sin\psi & \cos \psi & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}x \\ y \\ z\end{bmatrix} \end{equation}

これがz軸周りの回転行列となります。

y軸/z軸周りの回転を考える

続いてy軸周りの回転を考えます。y軸周りの回転は図8のような座標系となります。

図8：y軸周りの回転

こちらを見て貰えば分かりますが、図7のxをzに、yをxに、zをyに変換したものなので(9)式のxをzに、yをxに、zをyに置き換えると以下のようになります。

\begin{equation}\begin{bmatrix}z^{\prime} \\ x^{\prime} \\y^{\prime}\end{bmatrix} = \begin{bmatrix}\cos\theta & -\sin\theta & 0\\ \sin\theta & \cos \theta & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}z \\ x \\ y\end{bmatrix} \end{equation}

(10)式でも各成分の値は求まるのですが、順番が普通とは異なるため並べ替えていきます。

参考にした記事では一度展開したうえで並び替えていましたが、別の方法で並べ替えていきます。図8の左に行列積の例を示します。

図9：行列積の例

その中でxの項が掛かる部分は図8の右側となります。また、xを1行目にもっていくという事はxの項が掛かる部分が1列目に来ます。それを考慮してxと行にある$cos\theta$を１列目の1行目に、yと同じ行にある0を1列目の2行目に、zと同じ行にあるに$sin\theta$を1列目の3行目に並び替えると以下のようになります。(説明下手ですが伝わりますかね。。。)

図10：xの項だけ入れ替えた結果

これと同様に2行目、3行目を入れ替えていくと次のようになります。

\begin{equation}\begin{bmatrix}x^{\prime} \\ y^{\prime} \\z^{\prime}\end{bmatrix} = \begin{bmatrix}\cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos \theta\end{bmatrix}\begin{bmatrix}x \\ y \\ z\end{bmatrix} \end{equation}

最後にx軸周りの回転を考えます。基本的な考えはy軸周りの回転と同じなため導出は省略しますが、下記のようになります。

\begin{equation}\begin{bmatrix}x^{\prime} \\ y^{\prime} \\z^{\prime}\end{bmatrix} = \begin{bmatrix}1 & 0 & 0 \\ 0 & \cos\varphi & -\sin\varphi \\ 0 & \sin\varphi& \cos \varphi\end{bmatrix}\begin{bmatrix}x \\ y \\ z\end{bmatrix} \end{equation}

加速度センサの値から回転角を導出する

各軸の回転を求めることが出来たので、加速度センサの値からセンサーが何度傾いているか求めます。この傾きを元にセンサーの値を補正していきます。

参考文献(右手系とか左手系とか回転とかの話)によると今回の回転行列は左側からかけていけばいいことが分かります。(左手系かつ行ベクトルの場合は右からかけるみたいです。)

回転については順番によって結果が異なり12パターンの表現が出来る(最初にX軸回転、次にY軸回転、最後にZ軸回転など)のですが、今回はセンサー値をX→Y→Zの順で回転させていきます。

今回は角度を求めるためグローバル座標に回転を作用させていきます。理由としてはx成分、y成分が0なので計算が楽だからです。(センサー値をX→Y→Z軸の順で回転させるので、グローバル座標系の回転にするとZ→Y→Xの順で作用させているように見えるので注意です。)

今回(5)式ベースで求めてしまったのが、ここで仇となりました。。。ただし、1-2にも少しだけ記載しましたが、グローバル座標に回転を作用させる場合は回転行列の転置を取ったものを掛け合わせれば大丈夫です。

以上を踏まえると式としては以下のようになります。((9)、(11),(12)式の転置を取ったものになってるのに注意)

\begin{equation}\begin{bmatrix}1 & 0 & 0 \\ 0 & \cos\varphi & \sin\varphi \\ 0 & -\sin\varphi& \cos \varphi\end{bmatrix}\begin{bmatrix}\cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos \theta\end{bmatrix} \begin{bmatrix}\cos\psi & \sin\psi & 0\\ -\sin\psi & \cos \psi & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix} = \begin{bmatrix}a_x\\ a_y \\a_z\end{bmatrix}\end{equation}

これを計算すると以下のようになります。(計算ミスあったら教えてください。。。)

\begin{equation}\begin{bmatrix}\cos \theta \cos \psi & \cos \theta \sin \psi & - \sin\theta \\ \sin \varphi \sin \theta \cos \psi - \cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi + \cos \varphi \cos \psi & \sin\varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi + \sin \varphi \sin \psi& \cos \varphi \sin \theta \cos \psi - \sin \varphi \cos \psi & \cos \varphi \cos \theta \end{bmatrix} \begin{bmatrix}0 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix}a_x \\ a_y \\ a_z \end{bmatrix} \end{equation}

さらに計算すると以下のようになります。

\begin{equation}\begin{bmatrix} -\sin \theta \\ \sin \varphi \cos \theta \\ \cos \varphi \cos \theta\end{bmatrix} = \begin{bmatrix}a_x \\ a_y \\ a_z \end{bmatrix}\end{equation}

(15)式を見ると分かりますが、ヨー($\psi$)が無いためヨーは求めることが出来ないことが分かります。(重力加速度がある方向に回転させても値が変化しないことからもわかると思いますが。)

また、(15)式から$\frac{a_y}{a_z}$を計算すると

\begin{equation}\frac{a_y}{a_z} = \frac{\sin \varphi}{\cos \varphi} = \tan \varphi \nonumber \end{equation}

となるため下記のように$\varphi$を求めることが出来ます。

\begin{equation}\varphi = \arctan \left(\frac{a_y}{a_z}\right ) \end{equation}

次に$\theta$を求めていきます。最初$\arcsin \theta (-ax)$で求められるかと思いましたが、$arcsin$の定義域が-1から1までだったことを忘れていたため、汎用性がある$arctan$で求められるようにします。(参考：逆三角関数(arcsin,arccos,arctan)の定義と諸性質まとめ)

\begin{equation}\frac{a_x}{\sqrt{a_y^2 + a_z^2}} = \frac{-\sin \theta}{(\sin^2 \varphi + \cos^2 \varphi ) \cos \theta} = -\tan \theta \nonumber\end{equation}

となるため下記のように$\theta$を求めることが出来ます。

\begin{equation}\theta = \arctan \left( \frac{-a_x}{\sqrt{a^2_y+a^2_z}} \right ) \end{equation}