9軸カルマンフュージョンセンサ「JY901」

JY901っていう9軸センサフュージョンセンサの話

Tue, 29 Mar 2016

INPUTで使った方位センサについて紹介します．

今年のロボカップジュニアでも話題になった(?)ジャイロセンサ．近年，方位センサの暴走が多くなり，外乱に影響されないジャイロセンサで自己角度を推定するチームも増えてきています．北九州の方ではブロック会場の地場が悪く，ジャイロがあたりまえだとか．

 

ただジャイロセンサって，難しいんですよ．ドリフトというものがありどんどん角度が変わっていったり，動かしてると角度がずれていったり・・・．一般的にはドリフトを除去したうえでフィルタリングすると思うんですが，フィルタリングも奥が深くて・・・．

しかもYaw軸（ロボカップで使う軸）は加速度センサが使えないので資料があまりないです．ドローンの普及によりここらへんのフィルタリングが一般的に広まっていきいますが，どうしてもYaw軸は弱いです．ドローンが落ちないためには関係ない軸ですしね．

 

詳しい方は名前を聞いたことがあると思うんですが「カルマンフィルタ」っていうフィルタリングが一般的には最強です．

高専3年間の数学知識では理解不能でした．やりたいことだけは理解しました．（つまりコードを自分で書くのは出来ない）

一応，MPU6050（加速度＋ジャイロ）というセンサがあり，それのカルマンフィルタライブラリってのもArduinoにあります．結構出てきます．

しかし，こういうライブラリ，ドローン用だと思うんですよ．今書いたとおり，加速度って結局Yaw軸に使えないんで．

 

そんな時にnncbotひぐちが見つけたのが「JY901」です．

 

JY901 MPU6050 MPU9250 module angle output 9 axis Accelerometer Gyroscope Ser…

 

画像を見て，9軸でカルマンフィルタかけて角度を出力している！？

これ，なんか強そうｗｗｗｗって感じで1個購入しました．

 

で，結論から言って使えました．

Arduinoのライブラリが用意されているんですが，バグがあります．ここに１日ほど苦戦しました．

ただ，英語でわかりやすくデータシートが書かれていたのでそれをみて書いたらできました．この会社，中国なのにやるな！（偏見）

 

ということで自分で書いたYaw軸読み取りプログラムとキャリブレーションプログラムをあげときます．ご活用ください．

あ，Arduino用です．まあmbedでも容易に置き換え可能でしょう．

/***Z軸の角度(yaw)を読み取る方法***
[はじめに]
JY901は"-180から180"の間で値を返してきます。ご注意ください。
また、角度は小数点２桁まで出できます。
本プログラムはデータシートpp10 "5.1.4 Angle Output" を主に参考にしました。

[手順]
1.JY901のIICアドレスは"0x50"なので、Wire.beginTransmission(0x50)を行う。
2.yawを読み取る信号は"0x3f"を送ることで認識されるので、Wire.write(0x3f)を行う。
3.データの送信作業は終わりなので、 Wire.endTransmission(false)を行う。
4.2byteのデータが送られてくるので、Wire.requestFrom(0x50, 2)を行う。
5.2byteのデータが送られてくるまで待機したいので、while (Wire.available() < 2)を行う。
6.2byteのデータを読むので、Wire.read()を２回行う。
7.計算を行い、2byteのデータを角度にする。
  計算式 : (2byte目<<8 | 1byte目) / 32768 * 180

[あとづけ]
JY901発売元　ElecMaster http://www.aliexpress.com/store/1836321

本プログラム作成　あんばら(twitter : @ISM__M)
***********************************/


#include <Wire.h>

int yawl, yawh;
float angle;

void setup()
{
  Wire.begin();
  Serial.begin(9600);
  Serial.println("JY901 Yaw Output");
}

void loop()
{
  //1
  Wire.beginTransmission(0x50); //J901との通信開始

  //2
  Wire.write(0x3f); //z軸の角度を読み取るアドレスを送信

  //3
  Wire.endTransmission(false); //送信終了

  //4
  Wire.requestFrom(0x50, 2); //データ要求

  //5
  while (Wire.available() < 2) //データ待機
  {
    Wire.beginTransmission(0x50); //J901との通信開始
    Wire.write(0x3f); //z軸の角度を読み取るアドレスを送信
    Wire.endTransmission(false); //送信終了
    Wire.requestFrom(0x50, 2); //データ要求
  }

  //6
  yawl = Wire.read(); //1byte目受信
  yawh = Wire.read(); //2byte目受信

  //7
  angle = (float)((yawh << 8) | yawl)  / 32768 * 180;


  Serial.println(angle);

  delay(10);
}
//JY901のライブラリを使わないでキャリブレーションするプログラム

#include <Wire.h>

void setup()
{
  Wire.begin();
  Serial.begin(9600);
  
  //Z軸調整モード
  Wire.beginTransmission(0x50); //J901との通信開始
  Wire.write(0x01); //キャリブレーションモード選択
  Wire.write(0x03); //下位ビット
  Wire.write(0); //上位ビット
  Wire.endTransmission(); //通信終了
  
  delay(1000); //待機時間）（任意）
  
  //磁気センサキャリブレーションモード
  Wire.beginTransmission(0x50); //J901との通信開始
  Wire.write(0x01); //キャリブレーションモード選択
  Wire.write(0x02); //下位ビット
  Wire.write(0); //上位ビット
  Wire.endTransmission(); //通信終了
  
  delay(5000); //キャリブレーション時間（任意）
  
  //キャリブレーションモード終了
  Wire.beginTransmission(0x50); //J901との通信開始
  Wire.write(0x01); //キャリブレーション
  Wire.write(0); //下位ビット
  Wire.write(0); //上位ビット
  Wire.endTransmission(); //通信終了
}

void loop()
{
  //無し
}
© anbalab 2019