息子と一緒に Makers：So-netブログ

	ブログをはじめるログイン

2020年05月｜ 2020年06月｜2020年07月ブログトップ
- | 次の10件

【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する（１）　[ロボット]

しばらく Python で遊んでいましたが、倒立振子も忘れたわけではありません。今回は、BMI160用のフィルターを検討したいと思います。

今回、試してみるフィルターは次の３つです。

・ Complementary Filter
・ Kalman Filter
・ Madgwick Filter

それぞれに対して簡単に紹介します。

■ Complementary Filter
Complementary Filter は、ジャイロデータの積分値と加速度データで割り出したロールとピッチをある比（0.93:0.02）で足したものを出力とするものです。非常に単純で、応答速度が早いですが、ノイズも大きいのが欠点です。英語ですがこちらの記事が参考になります。

Reading a IMU Without Kalman: The Complementary Filter

■ Kalman Filter
Kalman Filter は、もう説明が必要のないくらい有名なフィルターです。Arduino用のライブラリがありますので、こちらをダウンロードして使いました。

Kalman Filter Library

カルマンフィルターの詳細を知りたい方はこちらの記事が参考になります。

カルマンフィルタの考え方

■ Madgwick Filter
Madgwick Filter は、少しマイナーですがカルマンフィルター同様に自己位置推定によく使われるフィルターです。カルマンフィルターより応答が早いのが特長ですが、収束が遅いと言われています。Madgwick Filter も Arduino ライブラリがあるのでこちらを使用しました。

Madgwick

Madgwick フィルターの詳細を知りたい方はこちらの記事が参考になると思います。

Madgwick Filterを読んでみた

■ Madgwick Filter を改造する
使用した Madgwick Filter はデータのサンプリング周波数が固定値でしか与えられません。時間保証ができるのであればよいのですが、SPRESENSEはOS上で走っているので若干のジッターは避けられません。ですので、サンプリング間隔を計算の度に与えられる関数を追加しました。

// MadgwickAHRS.h に追加
void updateIMU2(float gx, float gy, float gz, float ax, float ay, float az, double dt);

// MadgwickAHRS.cpp に追加
void Madgwick::updateIMU2(
           float gx, float gy, float gz, float ax, float ay, float az, double dt) {
	invSampleFreq = 1.0f / (1.0f / dt);
    	updateIMU(gx, gy, gz, ax, ay, az);
}

■　SPRESENSE用スケッチを書く
それぞれのフィルターがどのように振る舞うか調べるために次のスケッチを書いてみました。

#include <Wire.h>
#include <math.h>
#include <BMI160Gen.h>
#include <Kalman.h>
#include <MadgwickAHRS.h>

#define PRINT_ROLL
// #define PRINT_PITCH

#define BAUDRATE  115200
#define SENSE_RATE   100
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

Kalman kalmanRoll;
Kalman kalmanPitch;
Madgwick madgwick;

float convertRawGyro(int gRaw) {
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) {
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0; 
void print_roll_pitch() 
{     
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);

  float rad_a_roll = atan2(accY, accZ);
  float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
  float accl_roll = rad_to_deg(rad_a_roll);
  float accl_pitch = rad_to_deg(rad_a_pitch);

  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  unsigned long cur_mills = micros();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  double dt = duration / 1000000.0; // us->s  
  if (dt > 0.1) return;

  // Gyro data
  gyro_roll  += omega_roll  * dt; // (ms->s) omega x time = degree
  gyro_pitch += omega_pitch * dt;
  gyro_yaw   += omega_yaw   * dt;
  
  // Complementary filter data
  comp_roll  = 0.93 * (comp_roll  + omega_roll  * dt) + 0.07 * accl_roll; 
  comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; 

  // Kalman filter data
  float kalm_roll  = kalmanRoll.getAngle(accl_roll, omega_roll, dt);
  float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); 

  // Madgwick filter data
  madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt);
  float madw_roll  = madgwick.getRoll();
  float madw_pitch = madgwick.getPitch();
  float madw_yaw   = madgwick.getYaw();

#ifdef PRINT_ROLL
  Serial.print(cur_mills);  // sampling time
  Serial.print(",");
  Serial.print(accl_roll);  // roll data calculated by the accelerometer
  Serial.print(",");
  Serial.print(gyro_roll);  // roll data calculated by the gyroscope
  Serial.print(",");
  Serial.print(comp_roll);  // roll data calculated by the complementary filter
  Serial.print(",");
  Serial.print(kalm_roll);  // roll data calculated by the kalman filter
  Serial.print(",");
  Serial.print(madw_roll);  // roll data calculated by the madgwick filter
#endif

#ifdef PRINT_PITCH
  Serial.print(cur_mills);    // sampling time
  Serial.print(",");
  Serial.print(accl_pitch);  // pitch data calculated by the acclerometer
  Serial.print(",");
  Serial.print(gyro_pitch);  // pitch data calculated by the gyroscope 
  Serial.print(",");
  Serial.print(comp_pitch);  // pitch data calculated by the complemtary filter
  Serial.print(",");
  Serial.print(kalm_pitch);  // pitch data calculated by the kalman fitler
  Serial.print(",");
  Serial.print(madw_pitch);  // pitch data calculated by the madgwick filter
#endif
  Serial.println();
}

void setup() {
  Serial.begin(115200);
  BMI160.begin(BMI160GenClass::I2C_MODE);
  madgwick.begin(SENSE_RATE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  delay(100);

  /* Set kalman and gyro starting angle */
  int rawXAcc, rawYAcc, rawZAcc;
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);
  
  float roll  = rad_to_deg(atan(accY / accZ));
  float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ)));

  // Set starting angle
  kalmanRoll.setAngle(roll); 
  kalmanPitch.setAngle(pitch);
  gyro_roll  = comp_roll  = roll;
  gyro_pitch = gyro_pitch = pitch;
}

void loop() {
  print_roll_pitch();

}

■　出力データを確認する
ロール方向、ピッチ方向にそれぞれ筐体を振ってみてデータを収集しました。

スライド2.png

それぞれのデータはこちらです。

2020-06-07 (3).png

どのフィルタもジャイロのドリフトはキャンセルしてくれているのは分かりますが、グラフだけではどれが良いのかピンと来ませんね。もう少しうまく評価できる方法を考えてみたいと思います。🤨
(-ω-;)

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサーモジュールIIC SPIブロード

出版社/メーカー: SODIAL
メディア: その他

トコトンやさしい自動運転の本 (今日からモノ知りシリーズ)

作者: クライソン・トロンナムチャイ
出版社/メーカー: 日刊工業新聞社
発売日: 2018/03/29
メディア: 単行本

タグ：Spresense BMI160 倒立振子

2020-06-07 23:40 nice!(26) コメント(4)
共通テーマ：趣味・カルチャー

謎のインターステラー天体「オウムアムア」に新展開！？　[トンデモ学説]

宇宙ものの話題が続きますが、皆んな大好き謎のインターステラー天体「オウムアムア」に新説が現れました！

太陽系外から来た天体「オウムアムア」は、“水素氷山”だった？新たな仮説の真実味

なんと、「オウムアムア」が大量の水素が固まった水素氷山なのではという説です。銀河中に漂う分子雲の中にある水素が、数十万年という長い年月をかけて凝固した氷山？の可能性があるとのことです。水素氷山であれば、オウムアムアの奇妙な形状や動きが説明できるとのこと。

ここで、オウムアムアの軌道を振り返ってみましょう。

いやぁ、ここまで太陽に近い軌道を回ってるなら、凝固しているとはいえ、水素ならあっという間に蒸発してしまわないですかね。近日点で水星の軌道の内側ですよ～。ちょっと無理筋じゃないですか？

しかし、地球には存在しないような物質の可能性があるという視点は面白いですね。水素氷山はさすがにないと思いますが、地球では存在し得ない代表的な物質として考えられるのは・・・

そう「モノポール」です！

http://www.tikalon.com/blog/blog.php?article=2013/anapole

モノポールはN極やS極の単一の極をもつ物質です。単一の極をもつということは内部の電子のスピンの方向はそれぞれ独自に制御できるようになり、電気工学ならぬ電磁気工学ができます。スピンの方向をメモリに活用した磁気メモリとかは有名な応用分野ですよね。

ナノスピントロニクス研究センター

しかし、それだけじゃなく、電磁気力は光子が媒介することを考えると、モノポールに光があたると交換する先がないので何かしらスピンに影響を与えるように思います。（知らんけど）

となるとモノポール内の量子のスピン状態が変わると思われます。「量子スピン」といえば「量子もつれ」！「量子もつれ」といえば「量子通信」！

世界初の多次元量子テレポーテーションに成功

量子通信は時空を超えて、どんなに離れている相手にも瞬時に情報を伝えられる夢の技術です。なので、もしこれがモノポールの塊だとしたら、それはやはり探査機の可能性が大ですね。

情通機構、５０ｋｇ超小型衛星を利用した量子通信の実証に成功

ということで、「オウムアムア」は地球外文明が送り込んだ探査機であると断定させていただきます。😤
( ｰ`дｰ´)キリッ

モノポール―宇宙誕生の鍵をにぎる幻の素粒子 (ブルーバックス (B‐625))

作者: 広瀬立成
出版社/メーカー: 講談社
発売日: 2020/06/05
メディア: 新書

量子暗号と量子テレポーテーション―新たな情報通信プロトコル

出版社/メーカー: 共立出版
発売日: 2020/06/05
メディア: 単行本

ファースト・コンタクト―地球外知性体と出会う日 (文春新書)

作者: 金子隆一
出版社/メーカー: 文藝春秋
発売日: 2020/06/05
メディア: 新書

タグ：地球外文明オウムアムア宇宙

2020-06-04 23:59 nice!(24) コメント(0)
共通テーマ：趣味・カルチャー

【祝】スペースX 有人宇宙船打ち上げ成功！日本の宇宙開発の未来は？？？　[トンデモ学説]

スペースXが有人宇宙船の打上成功につづきISSへのドッキングも成功させました。民間初の快挙です！NASAからの受注なので商用というわけではないですが、民間組織がゼロから開発ですから凄いですよね。

2020-06-03 (3).png

https://www.spacex.com/vehicles/dragon/

ブースターロケットも自分で着陸して再利用しちゃうんですから、技術面でもコスト面でもスペースシャトルを遥かに凌駕しています。

日本もH3の導入が間近ですし、イプシロンも運用されはじめ、衛星や物資の輸送コストがどんどん安くなり負けていません！ということで、日本の宇宙開発の今後の展望について少し調べて見ました。

JAXAにおける宇宙輸送に関わる取り組み

■　日本の基幹ロケットはH3とイプシロンに
"H3" は2020年後半に試験機１号、2021年中盤に試験機2号が打ち上げられるようです。機体価格は従来の半分以下の50億円。現在のイプシロンと同等の機体価格です。

2020-06-02 (1).png

しかも個体ロケットブースターはイプシロンと共用できるように設計し、イプシロンの機体価格は30億円まで低価格化。これは新しいビジネスが生まれそうですね。通信など宇宙空間を利用したサービス産業が増えそうです。それに伴い衛星開発のビジネスも活性化しそう。

2020-06-02 (2).png

■　再使用型輸送システムの研究を推進
スペースXは第一弾ロケットの再利用が注目されましたが、日本も再使用型宇宙輸送システムを研究しています。当初、スペースシャトル型の有翼往還機「HOPE-X」を検討していましたが2000年前半に凍結。

2020-06-02 (3).png

その後、再使用・観測ロケットの研究を実施していましたが、2015年頃に技術実証が終了しています。

2020-06-02 (4).png

ただ、この研究成果は現在の”再使用ロケット”「CALLISTO」に継承されています。「CALLISTO」は平たく言うならスペースXが実現した再使用ロケットの実験機です。こちらはなんとフランス（CNES）、ドイツ（DLR）との三カ国共同プロジェクト。

2020-06-02 (5).png

「CALLISTO」は2022年にギアナ宇宙センターで高度30km 飛行試験を行うそうです。その頃、スペースXは火星への物資輸送準備を進めている頃でしょうか。民間企業のスペースX１社が成し遂げたことを三カ国の国家研究機関が後追いをするというシュールな構図。

この分野では日本勢の迷走っぷりが伺えます。ここはアイディアの後追いとかじゃなく思い切った発想の転換がほしいところだなぁ。

■　LNG推進系で日本はトップランナー
JAXAはLNG推進系の開発も進めています。狙いは燃料の圧倒的な低コスト化と安全性向上です。なんと日本はこの分野で世界トップレベル！

2020-06-02 (6).png

ただ、一番の欠点は燃料が重いこと。水素燃料に比べ６倍重い。なので、軌道間輸送、軌道ー月面輸送や、ロケットの二段目エンジンでの活用が考えられているようです。

■　民間との共創型プログラム「J-SPARC」
JAXAが民間の宇宙輸送機事業を技術支援するプログラムです。次のベンチャー企業との共同開発が進んでいるようです。

（株）スペースウォーカー
再使用型有翼ロケット実験機の開発

PDエアロスペース（株）
ジェット燃料・ロケット燃料の切り替えを行うエンジンを有した有翼ロケット実験機の開発

インターステラテクノロジズ（株）
言わずと知れたホリエモンロケットです。早く実用化されるといいですね。

スペースワン（株）
固体燃料３段式ロケット。21年に事業化を目指すそうです。（スゴイ！）

2020-06-03 (1).png

日本の民間企業もがんばっていますね。スペースXよりも遥かにコストが安い宇宙輸送機が実現できるのも時間の問題かもしれません。がんばれー日本！😄

■　ロックーンは再発明されないの？
再利用ロケットもいいですけど、個人的には超小型衛星打上用の気球型空中発射ロケット（ロックーン）がまた注目されるんじゃないかなと思っています。

戦後すぐに高価なロケットに変わるものとして考案されたものです。ヴァン・アレン帯の発見に貢献するなど当時でもそこそこ使える技術でした。

時は流れて現代。気球はさらに丈夫で軽くなり、当時よりも遥か高高度まで到達できます。また機器は精密・高性能になり小型軽量のロケット・衛星でも十二分な能力を発揮できる時代になりました。

ロックーンなら、JAXAのLNG推進系も活躍できるでしょうし、繊維・縫製の技術は日本はトップクラス。日本なら高性能のロックーンが出来上がる気がします。大規模な発射場とかいらないし、数百万円程度で小型衛星が打ち上がるようになったら世界は変わるでしょうねぇ。😁
（＾＾）／～

（追記）
少し調べてみたら、ロックーンは欧州のESAが力を入れているようですね。エンジンにLPGガスを使うという発想まで同じ！おぉ、単なる思いつきの割にはいい線いってるじゃん。

機体価格は数百万とはならず、２億円程度なのでまだまだ高いですが、ロケットに比べれば桁が違います。運用がうまくいけばもっと安くなるでしょう。JAXAもチャレンジして欲しいなぁ。

https://news.yahoo.co.jp/byline/akiyamaayano/20181118-00104617/