MPU-6050を使った慣性航法 (3)
次に、加速度センサのバイアスを計算します。
加速度の補正式
今回の前提として、加速度を次の式で補正することにします。
\(\begin{array}{l}{{a'}_x} = {A_x}({a_x} - {B_x})\\{{a'}_y} = {A_y}({a_y} - {B_y})\\{{a'}_z} = {A_z}({a_z} - {B_z})\end{array}\)
バイアスの計算式
この式の\({A_x},{A_y},{A_z}\)と\({B_x},{B_y},{B_z}\)を求める方法として、以下の手順を考えました。
X軸を真上に向けた状態で加速度センサの値を測定し、これを\((X{P_x},X{P_y},X{P_z})\)とします。
X軸を真下に向けた状態で加速度センサの値を測定し、これを\((X{N_x},X{N_y},X{N_z})\)とします。
Y軸を真上に向けた状態で加速度センサの値を測定し、これを\((Y{P_x},Y{P_y},Y{P_z})\)とします。
Y軸を真下に向けた状態で加速度センサの値を測定し、これを\((Y{N_x},Y{N_y},Y{N_z})\)とします。
Z軸を真上に向けた状態で加速度センサの値を測定し、これを\((Z{P_x},Z{P_y},Z{P_z})\)とします。
Z軸を真下に向けた状態で加速度センサの値を測定し、これを\((Z{N_x},Z{N_y},Z{N_z})\)とします。
この値を元に、ゼロ点のバイアス\({B_x},{B_y},{B_z}\)を、次のように計算します。
\(\begin{array}{l}{B_x} = \frac{{(X{P_x} + X{N_x})}}{2}\\{B_y} = \frac{{(Y{P_y} + Y{N_y})}}{2}\\{B_z} = \frac{{(Z{P_z} + Z{N_z})}}{2}\end{array}\)
計算した\({B_x},{B_y},{B_z}\)を使って、測定した６種類の加速度センサの値を補正し、各加速度の大きさを求めます。
\(\begin{array}{l}\left| {XP'} \right| = \sqrt {{{(X{P_x} - {B_x})}^2} + {{(X{P_y} - {B_y})}^2} + {{(X{P_z} - {B_z})}^2}} \\\left| {XN'} \right| = \sqrt {{{(X{N_x} - {B_x})}^2} + {{(X{N_y} - {B_y})}^2} + {{(X{N_z} - {B_z})}^2}} \\\left| {YP'} \right| = \sqrt {{{(Y{P_x} - {B_x})}^2} + {{(Y{P_y} - {B_y})}^2} + {{(Y{P_z} - {B_z})}^2}} \\\left| {YN'} \right| = \sqrt {{{(Y{N_x} - {B_x})}^2} + {{(Y{N_y} - {B_y})}^2} + {{(Y{N_z} - {B_z})}^2}} \\\left| {ZP'} \right| = \sqrt {{{(Z{P_x} - {B_x})}^2} + {{(Z{P_y} - {B_y})}^2} + {{(Z{P_z} - {B_z})}^2}} \\\left| {ZN'} \right| = \sqrt {{{(Z{N_x} - {B_x})}^2} + {{(Z{N_y} - {B_y})}^2} + {{(Z{N_z} - {B_z})}^2}} \\\end{array}\)
正確な重力加速度の値をgとしたとき、仮の\({A_x},{A_y},{A_z}\)である\({{A'}_x},{{A'}_y},{{A'}_z}\)を、仮に次のように計算します。
\(\begin{array}{l}{{A'}_x} = \frac{1}{2}\left( {\frac{g}{{\left| {XP'} \right|}} + \frac{g}{{\left| {XN'} \right|}}} \right)\\{{A'}_y} = \frac{1}{2}\left( {\frac{g}{{\left| {YP'} \right|}} + \frac{g}{{\left| {YN'} \right|}}} \right)\\{{A'}_z} = \frac{1}{2}\left( {\frac{g}{{\left| {ZP'} \right|}} + \frac{g}{{\left| {ZN'} \right|}}} \right)\end{array}\)
\({{A'}_x},{{A'}_y},{{A'}_z}\)を使って、測定した６種類の加速度センサの値を補正し、各加速度の大きさを求めます。
\(\begin{array}{l}\left| {XP''} \right| = \sqrt {{{({{A'}_x}X{{P'}_x})}^2} + {{({{A'}_y}X{{P'}_y})}^2} + {{({{A'}_y}X{{P'}_y})}^2}} \\\left| {XN''} \right| = \sqrt {{{({{A'}_x}X{{N'}_x})}^2} + {{({{A'}_y}X{{N'}_y})}^2} + {{({{A'}_y}X{{N'}_y})}^2}} \\\left| {YP''} \right| = \sqrt {{{({{A'}_x}Y{{P'}_x})}^2} + {{({{A'}_y}Y{{P'}_y})}^2} + {{({{A'}_y}Y{{P'}_y})}^2}} \\\left| {YN''} \right| = \sqrt {{{({{A'}_x}Y{{N'}_x})}^2} + {{({{A'}_y}Y{{N'}_y})}^2} + {{({{A'}_y}Y{{N'}_y})}^2}} \\\left| {ZP''} \right| = \sqrt {{{({{A'}_x}Z{{P'}_x})}^2} + {{({{A'}_y}Z{{P'}_y})}^2} + {{({{A'}_y}Z{{P'}_y})}^2}} \\\left| {ZN''} \right| = \sqrt {{{({{A'}_x}Z{{N'}_x})}^2} + {{({{A'}_y}Z{{N'}_y})}^2} + {{({{A'}_y}Z{{N'}_y})}^2}} \end{array}\)
これらの値を元に、\({A_x},{A_y},{A_z}\)を計算します。
\(\begin{array}{l}{A_x} = {{A'}_x}\frac{1}{2}\left( {\frac{g}{{\left| {XP''} \right|}} + \frac{g}{{\left| {XN''} \right|}}} \right)\\{A_y} = {{A'}_y}\frac{1}{2}\left( {\frac{g}{{\left| {YP''} \right|}} + \frac{g}{{\left| {YN''} \right|}}} \right)\\{A_z} = {{A'}_z}\frac{1}{2}\left( {\frac{g}{{\left| {ZP''} \right|}} + \frac{g}{{\left| {ZN''} \right|}}} \right)\end{array}\)
加速度の測定手順
加速度センサの調整で難しいのは、機体の姿勢や正しい重力加速度の値がよくわからない点にあります。
角速度については、静止状態での角速度を0と乱暴に決めて、ゼロ点のバイアスを補正するだけでそれなりに正しいそうな値が計算できました。
しかし、加速度については、機体の姿勢や重力の大きさなどを何らかの方法で知る必要があります。
残念ながらそのような測定器具が手元にないので、簡易的な方法を考えました。
まず、重力加速度は単純に1gとします。
静止したテーブル上を動かす程度なら、これで問題ないでしょう。
軸の向きを合わせるには、本棚の側板と棚板を利用しました。
これは、「本棚の各面は、ほぼ垂直か水平であろう」という想定を利用した方法です。
具体的には、以下のような手順を実行しています。
1) MPU6050を乗せたブレッドボードを用意します。この状態でMPU-6050のX軸とY軸は、ブレッドボードの上面と平行であり、Z軸は垂直な関係にあります。厳密にはズレがあるでしょうが、そこは諦めます。
2) 本棚の側板にブレッドボードの底面を当てて、ブレッドボードの側面を本棚の側面の端に合わせ、X軸が真上になるように固定して、センサが静止した状態で加速度を測定します。
3) 同様にして、X軸が真下、Y軸が真上と真下の場合の加速度を測定します。
4) ブレッドボードを棚板の上面に置いて、ブレッドボードの側面を本棚の棚板の端に合わせ、Ｚ軸が真上になるように固定して、センサが静止した状態で加速度を測定します。
5) ブレッドボードを棚板の底面に逆さまに押し当てて、Ｚ軸が真下になるように固定し、加速度を測定します。
測定結果
それぞれの加速度について数秒ぶんのデータ収集し、Excelでグラフにしたのが、以下の図です。
実際には角速度も同時に計測していて、この後、利用します。
X軸が真上と真下を向いたときの加速度 
Y軸が真上と真下を向いたときの加速度 
加速度の補正式
今回の前提として、加速度を次の式で補正することにします。
\(\begin{array}{l}{{a'}_x} = {A_x}({a_x} - {B_x})\\{{a'}_y} = {A_y}({a_y} - {B_y})\\{{a'}_z} = {A_z}({a_z} - {B_z})\end{array}\)
バイアスの計算式
この式の\({A_x},{A_y},{A_z}\)と\({B_x},{B_y},{B_z}\)を求める方法として、以下の手順を考えました。
X軸を真上に向けた状態で加速度センサの値を測定し、これを\((X{P_x},X{P_y},X{P_z})\)とします。
X軸を真下に向けた状態で加速度センサの値を測定し、これを\((X{N_x},X{N_y},X{N_z})\)とします。
Y軸を真上に向けた状態で加速度センサの値を測定し、これを\((Y{P_x},Y{P_y},Y{P_z})\)とします。
Y軸を真下に向けた状態で加速度センサの値を測定し、これを\((Y{N_x},Y{N_y},Y{N_z})\)とします。
Z軸を真上に向けた状態で加速度センサの値を測定し、これを\((Z{P_x},Z{P_y},Z{P_z})\)とします。
Z軸を真下に向けた状態で加速度センサの値を測定し、これを\((Z{N_x},Z{N_y},Z{N_z})\)とします。
この値を元に、ゼロ点のバイアス\({B_x},{B_y},{B_z}\)を、次のように計算します。
\(\begin{array}{l}{B_x} = \frac{{(X{P_x} + X{N_x})}}{2}\\{B_y} = \frac{{(Y{P_y} + Y{N_y})}}{2}\\{B_z} = \frac{{(Z{P_z} + Z{N_z})}}{2}\end{array}\)
計算した\({B_x},{B_y},{B_z}\)を使って、測定した６種類の加速度センサの値を補正し、各加速度の大きさを求めます。
\(\begin{array}{l}\left| {XP'} \right| = \sqrt {{{(X{P_x} - {B_x})}^2} + {{(X{P_y} - {B_y})}^2} + {{(X{P_z} - {B_z})}^2}} \\\left| {XN'} \right| = \sqrt {{{(X{N_x} - {B_x})}^2} + {{(X{N_y} - {B_y})}^2} + {{(X{N_z} - {B_z})}^2}} \\\left| {YP'} \right| = \sqrt {{{(Y{P_x} - {B_x})}^2} + {{(Y{P_y} - {B_y})}^2} + {{(Y{P_z} - {B_z})}^2}} \\\left| {YN'} \right| = \sqrt {{{(Y{N_x} - {B_x})}^2} + {{(Y{N_y} - {B_y})}^2} + {{(Y{N_z} - {B_z})}^2}} \\\left| {ZP'} \right| = \sqrt {{{(Z{P_x} - {B_x})}^2} + {{(Z{P_y} - {B_y})}^2} + {{(Z{P_z} - {B_z})}^2}} \\\left| {ZN'} \right| = \sqrt {{{(Z{N_x} - {B_x})}^2} + {{(Z{N_y} - {B_y})}^2} + {{(Z{N_z} - {B_z})}^2}} \\\end{array}\)
正確な重力加速度の値をgとしたとき、仮の\({A_x},{A_y},{A_z}\)である\({{A'}_x},{{A'}_y},{{A'}_z}\)を、仮に次のように計算します。
\(\begin{array}{l}{{A'}_x} = \frac{1}{2}\left( {\frac{g}{{\left| {XP'} \right|}} + \frac{g}{{\left| {XN'} \right|}}} \right)\\{{A'}_y} = \frac{1}{2}\left( {\frac{g}{{\left| {YP'} \right|}} + \frac{g}{{\left| {YN'} \right|}}} \right)\\{{A'}_z} = \frac{1}{2}\left( {\frac{g}{{\left| {ZP'} \right|}} + \frac{g}{{\left| {ZN'} \right|}}} \right)\end{array}\)
\({{A'}_x},{{A'}_y},{{A'}_z}\)を使って、測定した６種類の加速度センサの値を補正し、各加速度の大きさを求めます。
\(\begin{array}{l}\left| {XP''} \right| = \sqrt {{{({{A'}_x}X{{P'}_x})}^2} + {{({{A'}_y}X{{P'}_y})}^2} + {{({{A'}_y}X{{P'}_y})}^2}} \\\left| {XN''} \right| = \sqrt {{{({{A'}_x}X{{N'}_x})}^2} + {{({{A'}_y}X{{N'}_y})}^2} + {{({{A'}_y}X{{N'}_y})}^2}} \\\left| {YP''} \right| = \sqrt {{{({{A'}_x}Y{{P'}_x})}^2} + {{({{A'}_y}Y{{P'}_y})}^2} + {{({{A'}_y}Y{{P'}_y})}^2}} \\\left| {YN''} \right| = \sqrt {{{({{A'}_x}Y{{N'}_x})}^2} + {{({{A'}_y}Y{{N'}_y})}^2} + {{({{A'}_y}Y{{N'}_y})}^2}} \\\left| {ZP''} \right| = \sqrt {{{({{A'}_x}Z{{P'}_x})}^2} + {{({{A'}_y}Z{{P'}_y})}^2} + {{({{A'}_y}Z{{P'}_y})}^2}} \\\left| {ZN''} \right| = \sqrt {{{({{A'}_x}Z{{N'}_x})}^2} + {{({{A'}_y}Z{{N'}_y})}^2} + {{({{A'}_y}Z{{N'}_y})}^2}} \end{array}\)
これらの値を元に、\({A_x},{A_y},{A_z}\)を計算します。
\(\begin{array}{l}{A_x} = {{A'}_x}\frac{1}{2}\left( {\frac{g}{{\left| {XP''} \right|}} + \frac{g}{{\left| {XN''} \right|}}} \right)\\{A_y} = {{A'}_y}\frac{1}{2}\left( {\frac{g}{{\left| {YP''} \right|}} + \frac{g}{{\left| {YN''} \right|}}} \right)\\{A_z} = {{A'}_z}\frac{1}{2}\left( {\frac{g}{{\left| {ZP''} \right|}} + \frac{g}{{\left| {ZN''} \right|}}} \right)\end{array}\)
加速度の測定手順
加速度センサの調整で難しいのは、機体の姿勢や正しい重力加速度の値がよくわからない点にあります。
角速度については、静止状態での角速度を0と乱暴に決めて、ゼロ点のバイアスを補正するだけでそれなりに正しいそうな値が計算できました。
しかし、加速度については、機体の姿勢や重力の大きさなどを何らかの方法で知る必要があります。
残念ながらそのような測定器具が手元にないので、簡易的な方法を考えました。
まず、重力加速度は単純に1gとします。
静止したテーブル上を動かす程度なら、これで問題ないでしょう。
軸の向きを合わせるには、本棚の側板と棚板を利用しました。
これは、「本棚の各面は、ほぼ垂直か水平であろう」という想定を利用した方法です。
具体的には、以下のような手順を実行しています。
1) MPU6050を乗せたブレッドボードを用意します。この状態でMPU-6050のX軸とY軸は、ブレッドボードの上面と平行であり、Z軸は垂直な関係にあります。厳密にはズレがあるでしょうが、そこは諦めます。
2) 本棚の側板にブレッドボードの底面を当てて、ブレッドボードの側面を本棚の側面の端に合わせ、X軸が真上になるように固定して、センサが静止した状態で加速度を測定します。
3) 同様にして、X軸が真下、Y軸が真上と真下の場合の加速度を測定します。
4) ブレッドボードを棚板の上面に置いて、ブレッドボードの側面を本棚の棚板の端に合わせ、Ｚ軸が真上になるように固定して、センサが静止した状態で加速度を測定します。
5) ブレッドボードを棚板の底面に逆さまに押し当てて、Ｚ軸が真下になるように固定し、加速度を測定します。
測定結果
それぞれの加速度について数秒ぶんのデータ収集し、Excelでグラフにしたのが、以下の図です。
実際には角速度も同時に計測していて、この後、利用します。
コメント