マルチ機能(MARG)センサー(ATD-M4S,GY-80)
前回の記事(2018/4/4)から大分時間が空いてしまいました
オオハマリからの放置…(;'∀')
オオハマリからの放置…(;'∀') 完ぺきではないのですが、それらしくマルチセンサー※1から得られたデータを、フィルター※2を通して姿勢を推定し、リアルタイムにPC画面上の3Dオブジェクト※3に反映できました！
1. 実物と3Dモデルが連動！
処理の流れは以下1.1～1.3の通りです。
回路接続図をFritzingを使って図1の様に描いてみました。
また図1の通り実際に繋げた状態を図2に示します。
このブレッドボードを動かす事で、MARGセンサーからArduinoUNOに動きのデータが送信されます。
図2：実際に作ったもの
ArduinoUNOには図3に示す「受信→推定→送信」プログラムを事前に書き込んでおきます
このプログラムで図4の通り数百時間ハマりました(;'∀')
このプログラムで図4の通り数百時間ハマりました(;'∀')
#include <GY80.h>
GY80 sensor = GY80(); //GY80インスタンスを作成する
void a_set_scale(uint8_t scale);
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;}
/*------------------------以下Madgwickフィルター--------------------------*/
//システム定数
float deltat = 0.001f;// 実測値、1秒間のサンプリング周期（秒単位）デフォルトは「0.001」
float lastt = 0;
float ct = 0;
float beta=2.5f; //--------------------------初期値、デフォルトは「2.5」
//初期化終了後のbeta値
void Beta() {beta =0.5f;} //--------------------------デフォルト0.041f----------------------0.2でノイズ減を除去する！
//グローバルシステム変数
float q0 = 1.0f,q1 = 0.0f,q2 = 0.0f,q3 = 0.0f;// 初期条件付き推定方位四元数要素
void filterUpdate(float gg_x, float gg_y, float gg_z, float aa_x, float aa_y, float aa_z, float mm_x, float mm_y, float mm_z){ //w:角速度計、a:加速度計、m:地磁気計
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
gg_x *= 0.0174533f;
gg_y *= 0.0174533f;
gg_z *= 0.0174533f;
qDot1 = 0.5f * (-q1 * gg_x - q2 * gg_y - q3 * gg_z);
qDot2 = 0.5f * (q0 * gg_x + q2 * gg_z - q3 * gg_y);
qDot3 = 0.5f * (q0 * gg_y - q1 * gg_z + q3 * gg_x);
qDot4 = 0.5f * (q0 * gg_z + q1 * gg_y - q2 * gg_x);
// 加速度計の測定が有効な場合にのみフィードバックを計算する（加速度計の正規化でNaNを回避する）
if(!*1 > 3.0f || val.g_x > 20000 || val.g_x < -20000){;}else{gx = val.g_x;}
if (abs*2 > 3.0f || val.g_y > 20000 || val.g_y < -20000){;}else{gy = val.g_y;}
if (abs*3 > 3.0f || val.g_z > 20000 || val.g_z < -20000){;}else{gz = val.g_z;}
if (val.a_x > 700 || val.a_x < -700){;}else{ax = val.a_x;} //700
if (val.a_y > 700 || val.a_y < -700){;}else{ay = val.a_y;}
if (val.a_z > 700 || val.a_z < -700){;}else{az = val.a_z;}
if (val.m_x > 400 || val.m_x < -400){;}else{mx = val.m_x;} //400
if (val.m_y > 400 || val.m_y < -400){;}else{my = val.m_y;}
if (val.m_z > 400 || val.m_z < -400){;}else{mz = val.m_z;}
//ジャイロスコープ静止状態ゼロ点補正---------------------------------------------------------------------------------------------------------
gxOld2=gxOld1,gyOld2=gyOld1,gzOld2=gzOld1,gxOld1=val.g_x,gyOld1=val.g_y,gzOld1=val.g_z;
xG = convertRawGyro(gx - CoefficientGX); //静止状態のgx平均値でゼロ点補正
yG = convertRawGyro(gy - CoefficientGY); //静止状態のgy平均値でゼロ点補正
zG = convertRawGyro(gz - CoefficientGZ); //静止状態のgz平均値でゼロ点補正
//Madgwickフィルタによる角速度・加速度・地磁気から四元数推定----------------------------------------------------------------------------------
//四元数から姿勢(ピッチ・ヨウ・ロール)計算----------------------------------------------------------------------------------------------------
pitch = asinf(-2.0f * (q1*q3 - q0*q2))* 57.29578f;
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)* 57.29578f;
roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)* 57.29578f;
//姿勢推定結果の出力----------------------------------------------------------------------------------------------------------------------------
Serial.print("Orientation: ");
Serial.print(-yaw+ 180.0f);Serial.print(" "); // def:X * -1 + 180.0f
Serial.print(-pitch+ 180.0f);Serial.print(" ");// def:X * -1 + 180.0f
Serial.println(-roll);
}
//補正係数計算
void Coefficient() {
//ジャイロスコープゼロ点補正係数
int jx=0,jy=0,jz=0;
for(int i=0;i<100;i++){ //------------------------------50～100回平均をする
GY80_raw val = sensor.read_raw();
//外れ値は除去して平均値を計算する
if (val.g_x > 50 || val.g_x < -50){;}else{CoefficientGX += val.g_x;jx+=1;}
if (val.g_y > 50 || val.g_y < -50){;}else{CoefficientGY += val.g_y;jy+=1;}
if (val.g_z > 50 || val.g_z < -50){;}else{CoefficientGZ += val.g_z;jz+=1;}}
CoefficientGX = -CoefficientGX/jx ;//x軸角速度補正値計算
CoefficientGY = -CoefficientGY/jy ;//y軸角速度補正値計算
CoefficientGZ = -CoefficientGZ/jz ;//z軸角速度補正値計算
}
void setup(){
Serial.begin(115200); // 毎秒9600,115200ビットでシリアル通信を初期化する
sensor.begin();//センサーを初期化する
for(int i=0;i<100;i++){Measurement();}
Coefficient();
for(int i=0;i<100;i++){Measurement();}
Coefficient();
for(int i=0;i<300;i++){Measurement();}
Coefficient();
Beta();}
void loop(){Measurement();}
図3プログラムの内容
図4：日の目を見る事のなかったArduinoIDEプログラムの数々…合掌
図3のプログラムで姿勢を推定したデータがPCに出力されてくるので、それを図5のProcessingプログラムで3Dオブジェクトに反映させます。
実験した結果が動画1の通りですこのプログラムで図6の通り数十時間ハマりました(;'∀')
このプログラムで図6の通り数十時間ハマりました(;'∀')
import processing.serial.*;
Serial myPort;
float yaw = 0.0;
float pitch = 0.0;
float roll = 0.0;
void setup(){
println(Serial.list());
size(1200,1000, P3D);
myPort = new Serial(this,Serial.list()[1], 115200);
textSize(16);// set text size
textMode(SHAPE); // set textmode to shape
}
void draw(){
serialEvent(); // read and parse incoming serial message
background(255);// set background to white
lights();
translate(width/2, height/2,500);// set position to centre
pushMatrix();// begin object
float c1 = cos(radians(roll-0));//-10flont up
float s1 = sin(radians(roll-0));
float c2 = cos(radians(pitch+0));//+25Right up
float s2 = sin(radians(pitch+0));
float c3 = cos(radians(yaw+0));//+10cloc Rlight
float s3 = sin(radians(yaw+0));
applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
-s2, c1*c2, c2*s1, 0,
c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
0, 0, 0, 1);
drawArduino();
popMatrix();// end of object
print(roll);
print("\t");
print(pitch);
print("\t");
print(yaw);
println();
}
void serialEvent(){
int newLine = 13; // new line character in ASCII
String message;
do{
message = myPort.readStringUntil(newLine); // read from port until new line
if (message != null) {
String[] list = split(trim(message), "");
if (list.length>= 4 && list[0].equals("Orientation:")) {
yaw = float(list[1]); //convert to float yaw
pitch = float(list[2]); //convert to float pitch
roll = float(list[3]); //convert to float roll
}
}
} while(message != null);
}
void drawArduino(){
stroke(0,90, 90); // 輪郭の色を暗い色に設定する
fill(0,130, 130); // 塗りつぶしの色をより明るい青色に設定する
box(200,10, 300); // Arduinoボードの基本形状を描く
stroke(0);// アウトラインカラーを黒に設定する
fill(80);// 塗りつぶし色を濃いグレーに設定する
translate(90,-10, -55); // Arduinoボックスの端から端まで設定する
box(10,20, 170); // ピンヘッダーをボックスとして描く
translate(-180,0, 15); // 位置をArduinoボックスの他の端に設定する
box(10,20, 210); // 他のピンヘッダーをボックスとして描く
}
図5：プログラムの内容
図6：日の目を見る事のなかったProcessingプログラムの数々…合掌
2. まとめ
電子工作ブログを始めてから最大のハマりとなりましたが、何とかMARGセンサーを使えそうな感じになりました！
これで自作ドローンが作れるかも！
これで自作ドローンが作れるかも！
励みになりますのでよければクリック下さい(^o^)/
*1:aa_x == 0.0f) && (aa_y == 0.0f) && (aa_z == 0.0f))) {
// 加速度計の測定値を正規化する
recipNorm = invSqrt(aa_x * aa_x + aa_y * aa_y + aa_z * aa_z);
aa_x *= recipNorm;
aa_y *= recipNorm;
aa_z *= recipNorm;
// 磁力計の測定を正規化する
recipNorm = invSqrt(mm_x * mm_x + mm_y * mm_y + mm_z * mm_z);
mm_x *= recipNorm;
mm_y *= recipNorm;
mm_z *= recipNorm;
// 繰り返し演算を避けるための補助変数
_2q0mx = 2.0f * q0 * mm_x;
_2q0my = 2.0f * q0 * mm_y;
_2q0mz = 2.0f * q0 * mm_z;
_2q1mx = 2.0f * q1 * mm_x;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// 地球の磁場の基準方向
hx = mm_x * q0q0 - _2q0my * q3 + _2q0mz * q2 + mm_x * q1q1 + _2q1 * mm_y * q2 + _2q1 * mm_z * q3 - mm_x * q2q2 - mm_x * q3q3;
hy = _2q0mx * q3 + mm_y * q0q0 - _2q0mz * q1 + _2q1mx * q2 - mm_y * q1q1 + mm_y * q2q2 + _2q2 * mm_z * q3 - mm_y * q3q3;
_2bz = -_2q0mx * q2 + _2q0my * q1 + mm_z * q0q0 + _2q1mx * q3 - mm_z * q1q1 + _2q2 * mm_y * q3 - mm_z * q2q2 + mm_z * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// 勾配適切なアルゴリズム修正ステップ
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - aa_x) + _2q1 * (2.0f * q0q1 + _2q2q3 - aa_y) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mm_x) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - mm_y) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mm_z);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - aa_x) + _2q0 * (2.0f * q0q1 + _2q2q3 - aa_y) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - aa_z) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mm_x) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - mm_y) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mm_z);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - aa_x) + _2q3 * (2.0f * q0q1 + _2q2q3 - aa_y) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - aa_z) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mm_x) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - mm_y) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mm_z);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - aa_x) + _2q2 * (2.0f * q0q1 + _2q2q3 - aa_y) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mm_x) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - mm_y) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mm_z);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // ステップの大きさを正規化する
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// フィードバックステップを適用する
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;}
q0 += qDot1 * deltat;
q1 += qDot2 * deltat;
q2 += qDot3 * deltat;
q3 += qDot4 * deltat;
// 四元数を正規化する
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;}
/*------------------------以上Madgwickフィルター--------------------------*/
// 角速度生データを角度に変換-------------------------------------------------------------------------------------------------------------
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 2300.0) / 32768.0; //--------------------------------------------------------def:2500.0?
return g;}
//MARGセンサーからデータ取得
float gxOld2=0,gyOld2=0,gzOld2=0,gxOld1=1,gyOld1=1,gzOld1=1,xG,yG,zG,xA,yA,zA,gx,gy,gz,ax,ay,az,mx,my,mz,CoefficientGX,CoefficientGY,CoefficientGZ;
float axOld2=0,ayOld2=0,azOld2=0,axOld1=1,ayOld1=1,azOld1=1,mxOld2=0,myOld2=0,mzOld2=0,mxOld1=1,myOld1=1,mzOld1=1;
double pitch=0,yaw=0,roll=0,pi1=0,ya1=0,ro1=0,pi2=0,ya2=0,ro2=0;
void Measurement(){
//実行時間間隔のバラツキ補正------------------------------------------------------------------------------------------------------------------
GY80_raw val = sensor.read_raw();
ct = millis();
deltat = (ct - lastt) * 0.001;
lastt = ct;
//MARGセンサー測定外れ値の除去(ジャイロスコープのみ前回測定値からの傾き変化も考慮して補正)---------------------------------------------------
