FIGROBOKOUBOU

バイアスを補正した加速度から、機体の位置を計算するプログラムを作ります。

ローパスフィルタをかける
すでに見たように、MPU-6050から取得した生の加速度や角速度のデータにはノイズらしきものが乗っているので、ローパスフィルタをかけることにします。

ここでは簡単な方法として、ローパスフィルタには１０点の移動平均を使いました。要するに、加速度データや角速度データとして、過去10個ぶんのデータの平均値を使う、ということです。10個であることに、特に意味はありませんが、結果としてうまくいっているようです。

位置の計算式
「機体座標系の加速度」と「機体座標系から基準座標系への回転を表わすクォータニオン」から、以下の手順で現在位置を求めることにします。

加速度データ\(({a_x},{a_y},{a_z})\)をバイアスで補正し、加速度のクォータニオン\({p_1}\)を求めます。
\(\begin{array}{l}{p_{1w}} = 0.0\\{p_{1x}} = {A_x}({a_x} - {B_x})\\{p_{1y}} = {A_y}({a_y} - {B_y})\\{p_{1z}} = {A_z}({a_z} - {B_z})\end{array}\)

加速度のクォータニオン\({p_1}\)を、回転を表わすクォータニオン\(q\)で回転します。
\(\bar q\)は、\(q\)の共役クォータニオンです。共役クォータニオンは虚数部の符号を反転させることで求まります。
\({p_2} = q{p_1}\bar q\)
これで、機体座標系の加速度\({p_1}\)が、基準座標系の加速度\({p_2}\)に変換されます。

座標系を変換した加速度から、重力の影響を引き算します。
\({p_{2z}} = {p_{2z}} - gravity\)

直前の速度\(({v'_x},{v'_y},{v'_z})\)、現在の加速度\(({p_{2x}},{p_{2y}},{p_{2z}})\)、直前の加速度\(({a'_x},{a'_y},{a'_z})\)、前回データを取得してから経過した時間\(\Delta t\)（秒）を使って、台形積分で現在の速度\(({v_x},{v_y},{v_z})\)を求めます。
\(\begin{array}{l}{v_x} = {{v'}_x} + \frac{1}{2}({{a'}_x} + {a_x})\Delta t\\{v_y} = {{v'}_y} + \frac{1}{2}({{a'}_y} + {a_y})\Delta t\\{v_z} = {{v'}_z} + \frac{1}{2}({{a'}_z} + {a_z})\Delta t\end{array}\)

直前の位置\(({p'_x},{p'_y},{p'_z})\)、現在の速度\(({v_x},{v_y},{v_z})\)、直前の速度\(({v'_x},{v'_y},{v'_z})\)、前回データを取得してから経過した時間\(\Delta t\)（秒）を使って、台形積分で現在の位置\(({p_x},{p_y},{p_z})\)を求めます。
\(\begin{array}{l}{p_x} = {{p'}_x} + \frac{1}{2}({{v'}_x} + {v_x})\Delta t\\{p_y} = {{p'}_y} + \frac{1}{2}({{v'}_y} + {v_y})\Delta t\\{p_z} = {{p'}_z} + \frac{1}{2}({{v'}_z} + {v_z})\Delta t\end{array}\)

１回の処理の流れ
Arduino Nano互換機上では、MPU-6050から１回データを取得するごとに、次のような処理をします。

1) 加速度と角速度の過去10回ぶんのデータの平均値を計算する。
2) 加速度と回転を表わすクォータニオンを使って、現在の位置を計算する。
3) 角速度を使って、回転を表わすクォータニオンを更新する。

この他に、起動時に重力の影響の加速度からロール角、ピッチ角、ヨー角を求め、クォータニオンを初期化する処理や、ログを出力する処理などが必要です。

プログラムの実行結果
これまで考えてきた処理をArduino Nano互換機に実装して出力したログを紹介します。
今回のプログラムでは、加速度だけでなく、角速度についても、バイアスには事前に計算した固定値を使っています。
また、ログは、0.25秒間隔で出力しています。

次の図は、MPU-6050とArduino Nano互換機を乗せたブレッドボードをほぼ真上に持ち上げて、だいたい同じ位置に戻したときのログの座標値のグラフです。

最終的にほぼ同じ場所に戻り、静止状態になっているので、理想的には、x座標、y座標、z座標ともに、0に近い一定値に落ち着いてほしいところです。
実際にはグラフの通り、特にx座標のズレが目立っています。実はこれはバイアスを計算したときにある程度予想されていたことで、x座標の補正はy座標、z座標よりも弱いことが計算されていました。

今回の方法やプログラムでは、これ以上の精度で位置を求めるのは困難な気がします。
もっときちんと作り込めば精度よく計算できるのかもしれませんが、目的を考えると、慣性航法だけに頼らず、他の方法を使って、数秒とか数分単位で位置を修正するほうが現実的です。

以下の図は、同じブレッドボードを、ヨー軸（Z軸）、ピッチ軸（Y軸）、ロール軸（X軸）の順に回転させて、元の状態に戻したときの、機体の姿勢のグラフです。
プログラムが出力したクォータニオンの値から、Excel上でヨー角、ピッチ角、ロール角を計算してグラフにしています。

位置情報と比べると、機体の姿勢はかなりいい感じで計算できています。
私の目的では、位置よりも姿勢の情報のほうが重要なので、これは歓迎できる傾向です。