加速度、ジャイロ、磁力で自己位置推定がしたい

Question

現在手元にLSM9DS1のセンサがあり、そちらは９軸センサとなっており、加速度、ジャイロ、磁力が計測できます。そちらを使って、GPSを使わずに移動した軌跡をマップに表示したいと考えています。
そこで色々調べていくうちにわからなくなってしまい、お聞きしたいと思いました。

ソース→http://www.lodging.co.jp/multi-003.pdf

取れるデータは

x y z
加速度 -0.04. 0.05. 1.00
ジャイロスコープ 0.79. 0.49. 0.79
磁力計 3.88 20.43 -73.03

このようなデータで0.1秒ほどの感覚以上の速さで取得が可能です。

また、上記のデータでは精度が悪く、机に放置しているのに値が変動しています。ということでArduinoのライブラリである、MadgwickFilterを使用してみました。そこで出てきたのが、ロール、ピッチ、ヨーです。

しかし実際使ってみるといまいちよくわからない値しか取れず、困っています。センサを移動させたり、回転させたりしても値は変わらなかったです。

コードは以下

#include <Arduino_LSM9DS1.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
float ROLL, PITCH, YAW;

void setup() {
  Serial.begin(9600);
  while (!Serial);
  Serial.println("\nStarted\n");

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println("Hz  Acceleration in G's X\tY\tZ");

  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println("Hz  Gyroscope in degrees/second X\tY\tZ");

  Serial.print("Magnetic field sample rate = ");
  Serial.print(IMU.magneticFieldSampleRate());
  Serial.println("uT  Magnetic Field in uT X\tY\tZ");
  MadgwickFilter.begin(100); //100Hz
}

void loop() {
  //float xa, ya, za, xg, yg, zg,xm, ym, zm;
  float gx, gy, gz, ax, ay, az, mx, my, mz;
  IMU.readAcceleration(ax, ay, az);
  IMU.readGyroscope(gx, gy, gz);
  IMU.readMagneticField(mz, my, mz);
  MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  Serial.println("");
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW   = MadgwickFilter.getYaw();
  Serial.print(ROLL); Serial.print(",");
  Serial.print(PITCH); Serial.print(",");
  Serial.print(YAW); Serial.print("\n");
  delay(200);
  /*

  
  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(xa, ya, za);
    
    Serial.println("加速度　　　　　　　"+String(xa)+'\t'+String(ya)+'\t'+String(za));
  }
  if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(xg, yg, zg);
    Serial.println("ジャイロスコープ　　"+String(xg)+'\t'+String(yg)+'\t'+String(zg));
    }
  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(xm, ym, zm);
    Serial.println("磁力計　　　　　　　"+String(xm)+'\t'+String(ym)+'\t'+String(zm));
  }
  delay(2000);  
  */
}

上記のコードで取れる値は下の通りです。

20:30:44.091 -> 7.73,-2.47,181.69
20:30:44.305 -> 
20:30:44.305 -> 7.83,-2.49,181.74
20:30:44.518 -> 
20:30:44.518 -> 7.93,-2.51,181.80
20:30:44.697 -> 
20:30:44.697 -> 8.03,-2.53,181.85
20:30:44.911 -> 
20:30:44.911 -> 8.14,-2.55,181.90
20:30:45.126 -> 
20:30:45.126 -> 8.24,-2.57,181.95
20:30:45.309 -> 
20:30:45.309 -> 8.34,-2.59,182.00
20:30:45.523 -> 
20:30:45.523 -> 8.44,-2.61,182.05
20:30:45.739 -> 
20:30:45.739 -> 8.54,-2.63,182.11
20:30:45.919 -> 
20:30:45.919 -> 8.64,-2.65,182.16
20:30:46.130 -> 
20:30:46.130 -> 8.74,-2.68,182.21
20:30:46.348 -> 
20:30:46.348 -> 8.85,-2.70,182.26
20:30:46.523 -> 
20:30:46.523 -> 8.95,-2.72,182.31
20:30:46.738 -> 
20:30:46.738 -> 9.05,-2.74,182.37

聞きたいことは以下になります。

1, 上記のデータだけで自己位置推定は本当に可能でしょうか？
2, もしできるとしたらピッチ、ロール、ヨーは必要でしょうか？(9軸のセンサだけで実装可能？)
3, 実装の手順、方法を教えていただけると助かります。
4, 加速度から二回積分でほんとに移動距離(変位)は求められるのでしょうか。また、そのやり方について教えていただけると助かります。
5, マップ面に移動した軌跡をプロットする場合、2Dのプロットでいいのでz軸の移動は無視できる？と考えています。いかがしょうか。
6, 上記のロール、ピッチ、ヨーのデータを取得している際、センサは全く動かしていません。なのにも関わらず、値が変動してしまっています。逆に、動かした際にほぼ値が変わりません。

質問の量が多くなってしまい、申し訳ございません。
数学や、物理の知識が乏しく、根本的なことが理解できていないと考えています。
ご教授いただけると大変助かります。

Comments

[y_waiwai]

> 1, 上記のデータだけで自己位置推定は本当に可能でしょうか？
ってより、それが可能だとするソースを提示しよう

[AGLAAGLA]

修正しました。

Answer 1

• AHRSアルゴリズムに値を入れるときに単位系の調整が入っていないように見えます。
• AHRSによる姿勢トラッキング＋加速度（適切なフィルタを通したもの）の２回積分である程度の３Dトラッキングは可能だとは思います。
• が、どちらも誤差がどうしても乗るのでそれに対し2回積分をすると誤差が指数関数的に乗ってきます。
• 参照している資料はその乗ってくる誤差をGNSSと連携しお互いの欠点を補完しようというものです。
• そこをGNSSなしで行うのであれば誤差を許容するかほかの方法で誤差を減らす工夫は必要です。

Comments

[AGLAAGLA]

ご回答ありがとうございます。
加速度は16384.0で、ジャイロは131.0で割ると近似できるというのはわかるのですが、磁力の数値が分かりません。なんと調べればいいのでしょうか？

また、この手の研究や既存でやっている人が見つかると思っていたのですが、位置情報を算出するといった例があまりないのですがやはり難易度は高いのでしょうか。現在詰まっている状態です。
お時間が許す限り、今からゴールまでの順序を簡単に教えていただけないでしょうか？

よろしくお願いいたします。

[nobonobo]

実際に実現している人のコード（MATLAB）やデモンストレーション動画はありました。
https://x-io.co.uk/oscillatory-motion-tracking-with-x-imu/
まずはAHRSが動くところまでを追及すると良いのではないでしょうか。
IMUの出力単位とAHRSが要求する単位を調査して合わせこみをお願いします。
（最悪磁気はなくてもAHRSはある程度動きます）