Arduinoを用いた移動位置推定ノードの開発

指導教員　坂本直志教授
11EC035　久保田夏穂

1. はじめに

水面上では電波反射などによる影響で電波が不安定になり情報伝達障害が発生する可能性がある。本研究では情報伝達障害の調査をする為の無線機器が水面上でどのように動いているか、以下の図のように水面上での位置情報取得のためのノード開発を行う。

図1　ノード開発の簡略図

本研究では水面上での位置推定技術を行うための基礎的な技術として、生活環境内での加速度センサを利用した位置推定を目的としている。

屋外における自己位置推定手法ではGPSにより人工衛星からの信号を受け取り、通信を行うことで数m~100mの測定精度で受信者の現在位置推定が可能となる。しかし屋内や地下施設等の電波が届かない場所ではGPSを使用できない問題点がある。その為GPSを利用せず、どのような場所においても高精度な位置推定を可能にする手法が求められている。

屋内位置推定手法では無線LAN（Wi-Fi）を用いてWi-Fiアクセスポイントの位置から測位するWi-Fi測位方法などがあるが ^４）通常数メートルの誤差が生じ、さらに吹き抜けがあるビル内など条件によっては測位に不向きな場所がある。その他にもRFIDや可視光など多くの手法が検討されているがそれぞれに課題があり ² ^）、標準的となる技術は確立されていない。

このように移動体の位置推定には様々な方法がある。広大な野外であればGPSがよく用いられるが、数センチ単位の測定では別の手法が必要となる。

本研究では上記のように最終目標を海面上の浮遊体の位置推定とする。その為、様々な測定方式のうちから加速度センサを選んだ。そしてArduinoと組み合わせたセンサノードを作成し、計測した。また加速度値から位置を推定するソフトウェアを作成した。その結果、僅かな計測の誤差から等速直線運動と静止状態の区別が難しくなるなど課題が明らかになった。

章の構成は2章では現在の位置推定方法や本研究と関連する研究を述べている。3章では本研究で使用した実験機器についての説明、4章では実験でのハードウェアやソフトウェアのシステムについて述べている。5章では実験内容、結果、考察を、最後に本研究のまとめを6章で述べている。

2. 位置推定方法について

2.1屋外位置推定方法

現在、屋外位置推定方法をして最も利用されているのはGPS（Global Positioning System）である。GPSから得られる位置情報を利用した様々なアプリケーションが開発されてきた。GPSシステムの基本原理としては、米国国防総省によって運行される「ナビスター衛星」と呼ばれる人工衛星が発信する電波を利用して、地球上の様々な物体の現在位置を測位している。衛星は24機が地球を周回しており、各衛星が自身の現在位置と現在時刻の情報を電波に乗せて発信している。その為、GPSの電波が届かない屋内や地下での利用ができない。

カーナビやスマートフォンなどに組み込まれている「GPS受信装置」は、ナビスター衛星のうち4機の衛星が発信する電波を受信することで、電波に乗っている情報から端末の現在位置、すなわち経度、緯度、海抜高度などを算出する仕組みになっている。

GPSの他に利用されている屋外位置推定方法には無線LANを用いた方法がある。携帯電話などの端末付近のいくつかの基地局から電波を受信し各基地局から受信する電波の強度が端末と基地局の距離によって変わることを利用し距離を算出している。しかし基地局がカバーする範囲は数百メートルから数キロメートルのため屋内で利用するレベルの高精度な位置推定が行なえない。このように屋外位置推定方法ではそれぞれに課題があり屋内での利用が難しい。

2.2屋内位置推定方法

屋外位置推定方法に比べ、屋内位置推定方法では様々な技術が検討されているが現在一般的に利用されている技術は存在しない。現在、屋内位置推定方法では無線LANの電波強度、RFID、超音波、可視光、加速度などのセンサを利用したものなどが検討されている。

電波強度を利用したものでは事前に機器のデータベースを構築する必要性がある。RFIDや可視光を利用した方法では携帯電話やスマートフォンでは外部インターフェースを接続する必要がある。

本研究では現在iPhoneやAndroidなどスマートフォンにも搭載されており、また基地局などにも関わらず測定することが可能な加速度センサ，地磁気センサ，ジャイロセンサを用いて位置推定を行った。センサを使った位置測定での課題としては時間が経過すればするほどに誤差が蓄積すること、地磁気センサを利用する際は残留磁場の影響を受けやすいなどが挙げられる。

2.3関連研究

2.3.1 毛利健太郎氏、難波和彦氏、高尾明秀氏らの「加速度センサによる移動体の自己位置検出に関する研究」 ^１）

農作業の機械化に伴いラジコンヘリの自動化の需要が高まっている。その為、飛行の自動化への第一段階として自己位置の検出が必要となる。この研究では加速度センサによる移動体の自己位置の検出を目的として基礎研究を行っている。

重力センサユニットとプロセッサユニットからなる姿勢計測装置から三軸の加速度を出力し、方位データにより絶対座標に変換して積分することで距離を求めている。5通りの実験を行い加速度センサの基本特性と実際の移動体の位置検出を試みている。

実験結果としては短時間の実験なら，比較的簡単な補正方法で，ノイズを消去し，良好な自己位置検出を行うことが出来ている。したがって，加速度センサでも単純な飛行パターンであればラジコンヘリの自己位置検出が可能であると考えられた。

以上の研究は採用方式が本研究と類似しているので、彼らの研究の補正方法などは本研究にも応用できると考えた。課題点としては長時間での実験での場合の正確な位置推定である。

2.3.2村尾真氏の「3軸加速度センサを用いた車いすの移動経路推定」 ^３）

この研究では加速度センサを用いて車いすでの移動距離推定とエレベータでの移動距離推定を行っている。本研究に関わる項目としてはエレベータでの上下移動に関する移動距離推定である。数値積分を用いないエレベータの移動距離推定手法を行っている。

上階への移動時の加速度から上昇開始時と停止動作開始時の直後に加速度が大きく変化していることを利用し、この研究の提案方法ではこれら加速度が大きく変化する時刻を利用している。

エレベータで計測される加速度の特徴を利用した提案手法と数値積分を利用した従来手法との比較を行い。結果として提案手法の有効性が示されている。本研究では加速度の特徴を利用した手法は用いていないが、研究目的として水面での計測を行えるようにする為、水の深さなどによって加速度の特徴などを利用できるのではないかと考えた。

3. 実験機器

3.1Arduinoについて

ArduinoとはAtmel社がリリースしているマイクロコントローラと最小限の周辺回路を備えたワンボードマイコンである。入出力ボードとProcessing言語で実装された開発環境を備えている。開発は2005年にイタリアで始まり、学生向けのプロトタイピングシステムを製造することを目的にスタートした。

大きな特徴としてはオープンソースハードウェアであり、ハードウェア設計情報から開発に使うソフトウェアも公開されている。Arduinoは様々なセンサからの信号を受け取ることが可能であり、センサ情報を利用した製品が数多く開発されている。

本研究ではAVRマイクロコントローラのATmega8が搭載されたArduino　Unoを使用している。

図2　Arduino UNO

3.2加速度センサ

本研究で使用した加速度センサはSparkfun社製でInvenSenseのMPU-9150を搭載した9軸センサモジュールである。

MPU-9150は3軸ジャイロセンサと3軸加速度センサ、複雑な9軸MotionFusion アルゴリズムの処理を行うDigital Motion Processor、そして3軸デジタルコンパスのAK8975を一体化した2つのチップを組み合わせたパッケージシステムとなっている。またその他の機能としてオンチップデジタル温度センサが備わっている。

MPU-9150にはジャイロセンサと加速度センサの出力をデジタル化する為の16ビットアナログ - ディジタルコンバータ（ADC）、磁力計をデジタル化する為の13ビットADCが内蔵されており、各センサの為のローパスフィルタを持っている。出力されたデータはDMP（Digital Motion Processor）を使用することで6軸分のセンサデータから計算した姿勢出力や四元数を得ることも出来る。本研究ではこのDMPという機能を使用して加速度計測を行った。

また、デバイスでの全ての通信は400kHzのI2Cを使用して行われている。

本センサはI2Cで通信を行い、各センサから加速度、各速度、地磁気の値を送信している。またセンサ感知の軸方向については図4、図5のように本研究では加速度センサの値を利用した。

図3　MPU−9105

図4　加速度センサ，ジャイロセンサの感知の軸方向

図5　地磁気センサの感知の軸方向

4. 本研究のシステム

本研究ではArduinoUNOと加速度センサを接続しセンサから得られたデータを表計算ソフトで補正し求めたデータ値を積分することで速度や変位を求めている。以下ではハードウェア，ソフトウェア使用した計算方法について詳細を述べる。

4.1ハードウェア

ハードウェアは加速度センサとArduinoとパソコンの3つから構成され、回路を図6、実体配線図を図7に示す。Arduino UNOに搭載されているATMEGA8とMPU-9150の回路図となっている。実体配線図では黄色の線の部分が裏側、青色の線の部分が裏側に配線されている。

I2Cを使用する為にArduinoUNOをマスタ側、MPU-9150をスレーブ側としてMPU-9150のSCL，SDAピンとATMEGA8のSCL(アナログ5番ピン)，SDA(アナログ4番ピン)同士を接続している。

また割り込みを使用できるようにMPU-9150の割り込みデジタル出力ピンINTとATMEGA8の外部割り込み用のINT0と接続している。

MPU-9150のAD0はI2Cアドレス設定のピンなので本研究ではAD0=Lとする為、GNDに接続した。

図6　接続回路

図7　実体配線図

4.2ソフトウェア

研究でのプログラムはArduino IDEを使用して作成した。ArduinoとMPU-9150のサンプルプログラムを元にして秒単位での出力時間と加速度センサX軸，Y軸，Z軸の各センサ値が出力されるようになっている。

4.2.1 プログラム説明

使用したプログラムではWireライブラリと呼ばれるI2CやTWIデバイスで通信する際に必要なライブラリとMPU6050を使用するためのライブラリを実装している。そして#defineで使用する変数を定義した。

setup関数内ではWireライブラリを初期化、I2Cバスにマスタとして接続、シリアル通信のデータ転送レートの指定などを行いI2Cの接続が確認できるようになっている。またMPU-6050との接続確認やMPU-9150の機能であるDMPプログラミングのデモを行った後、DMPを初期化の確認をし、有効にしている。そして最後に外部割り込みを検出している。

#ifdef文内ではOUTPUT_READABLE_REALACCELで重力加速度を除去していないセンサ値、OUTPUT_READABLE_WORLDACCELでは重力加速度を除去したセンサ値を出力している。また同時に時間に関する関数であるmillis関数を用いてプログラムの実行を開始した時から現在までの時間をミリ秒単位で取得し秒単位に直して出力している。そしてFIFOバッファを使用しそれぞれの取得データを古い順に配列に格納し0.01秒ごとに取得データを出力している。

4.3計算方法について

Arduinoへの出力はUSB経由で画面出力し、表計算ソフトに貼り付けた。表計算ソフトで取得したデータを元に位置推定を行う。その為に数値積分を行う。表計算ソフトを用いて行う数値積分は数多く存在するが、本研究では台形法を使った方法を採用した。

まず表計算ソフトで取得した生データを物理量へ補正した。本研究でのプログラムから出力されるMPU-9150の生データは加速度，ジャイロセンサは符号付き16ビット、地磁気センサは13ビットで格納されている。

また初期設定ではセンサ情報が加速度は±2(G)，ジャイロは±250(degree/sec)，地磁気は±1200(μT)で表されている。データシートよりMPU-9150の加速度センサの感度は16384(LSB/g)であるため出力された生データを16384で除算し物理量（G表記）に直し、mks表記に補正した。

次に得られたデータで数値積分を行う。本研究で採用した台形法とは積分を台形の面積の和に置き換える方法である。

図8　台形の面積と積分

はじめに S ₁の台形の面積を求める。式は次のようになる。

S ₁と同様に S ₂の場合には y ₁と y ₂を用いて面積を求める。

S ₃や S ₄も同様に求めていく。

そしてこれらの総面積が区間 x ₀から x ₄の定積分値の近似値 Sになる。

S _iの iが1から Nまでの和を求めると、次の式になる。

これを2回行うことで加速度から位置へ変換できる。また台形法では分割数 hを大きくすればするほど誤差が大きくなる。その他のエクセルで行える数値積分では中点法やシンプソン公式を用いた方法などが挙げられる。

5. 実験

5.1実験内容

5.1.1 実験準備

実験準備として移動を制御する為の装置に実験機器を装着する。移動装置はステッピングモーターコントローラとステージで構成されている。

本研究で使用したステッピングモーターコントローラは駿河精機社のDS102　NRである。ステージは同社の薄型スチールハニカム光学定盤J09N-0405とXYZ軸自動ステージKWG06030-CAを使用した。

図9　移動装置

5.1.2 測定

初めにArduinoIDEで加速度を取得する為のプログラムを実行する。加速度を取得し始める。MPU-9150の仕様上、測定開始後にセンサ値が0になるまでに20秒ほどかかる為、静止させておく。静止後、ステッピングモーターコントローラで移動制御のプログラムを実行する。

5.1.3 移動制御プログラムの内容

ステッピングモーターコントローラの制御プログラム内容は以下の流れとなっている。制御プログラムの内容は付録に添付する。

移動距離は全ての速度において0.015[m]とする。

この流れを5回繰り返している。速度aは0.01[m/s]、0.02[m/s]、0.05[m/s]でそれぞれ実験を行った。等加速度運動については以下の表にまとめた。

速度[m/s]	経過時間 `t ₀`[s]	経過時間後の速度a[m/s]	加速度[m/s2]	速度一定時間 `t ₁`[s]
0.01[m/s]	0.5	0.01	0.02	1
0.02[m/s]	0.5	0.02	0.04	0.25
0.05[m/s]	0.1	0.05	0.5	0.2

※初速度は0[m/s]とする

表1　加速度について

5.1.4 測定終了後

移動装置静止後、加速度センサから得られた測定値を表計算ソフトへ移行する。移行されたデータを元にExcelで数値積分を行う。

5.2実験結果

5.2.1 速度0.01[m/s]

実験において取得した加速度データが図10である。

図10　速度0.01[m/s]実験結果

5.2.2 速度0.02[m/s]

実験において取得した加速度データが図11である。

図11　速度0.02[m/s]実験結果

5.2.3 速度0.05[m/s]

実験において取得した加速度データが図12である。

図12　速度0.05[m/s]実験結果

なお、一回積分、二回積分結果は付録へ添付した。

5.3考察

実験を行ってみて、速度が高いほどより正確な加速度データを取得することが出来ることがあった。その為、今後の実験ではレンジの設定を見直すことや、より精度の高い加速度センサを使用することが求められるだろう。

またどの速度のグラフにおいても移動後に加速度が0になるまでに時間がかかっていることが分かる。

これらの原因としては加速度センサの特性として速度が急激に変化した後に加速度がすぐに0の状態にならないことが考えられる。

その他にも速度が遅い場合実験に時間がかかってしまう為、誤差が蓄積し、良い結果を得ることができなかった。

よって実験結果より加速度センサを使用する場合、大きな衝撃が加わる環境など加速度値が大きい場合に位置推定への利用に有用的であることが分かる。

また今回の実験において測定開始地点から1.5センチ先への移動を台形法や加速度センサの誤差を含まない場合の理想的な加速度グラフ、一回積分での理想的な速度グラフ、二回積分での変位グラフを示す。

図13　理想的な加速度のグラフ

図14　理想的な速度グラフ

図15　理想的な変位グラフ

そして今回速度0.05[m/s]での結果と比較してみたいと思う。加速度値、速度、変位の各グラフの測定開始地点から1.5センチ先への移動の同じ部分を抽出してみた。

図16　0.05[m/s]実験値(加速度)

図17　0.05[m/s]実験値(速度)

図18　0.05[m/s]実験値(距離)

結果を比較してみると加速度と速度においては理想的なグラフと近似的なものが求められている。最終的な目標としてはこれらの理想的なグラフのような位置推定結果を出せるようにしたい。

6. まとめ

今回本研究を行ってみて加速度センサでの位置推定は時間が経過すればするほど誤差は蓄積してしまう為、非常に苦労した。長時間での位置測定の場合は定期的にリセットをする方法や除去する方法が必要だろう。

結果から加速度センサでの位置推定は他の位置推定に補助的に使用するのが有用的であると考えられる。

また実験において数値積分を行う際に台形法を用いたが、中点法やシンプソン公式などその他の方法を利用し求めることで精度に違いがあるのではないかと考えた。

今後はより一層の精度上昇と計算方法の研究や地磁気センサ、ジャイロセンサなどから方位取得し1方向だけでなく3軸での位置推定を行えるよう研究していきたい。また加速度センサを複数組み合わせることでより良い結果が得られるのではないかと考えた。

7. 参考文献

毛利健太郎，難波和彦，高野明秀，“加速度センサによる移動体の自己位置検出に関する研究” http://mama.agr.okayama-u.ac.jp/kenkyu/96/hide/kasokudo.htm
吉澤菜津子，遠藤貫裕，永見健一，“屋内位置情報における推定技術の開発と新しいサービスの展開について”株式会社インテック，技術情報誌INTEC TECHNICAL JOURNAL，13号，pp.44-47（2013）
村尾真，“3軸加速度センサを用いた車いすの移動経路推定”電子情報通信学会技術研究報告. WIT, 福祉情報工学 112(426), pp.41-46（2013）
谷口功，“位置情報の基礎と技術”株式会社翔泳社，pp.12-21（2012）
Massimo　Banzi著　“Arduinoをはじめよう”　pp.10-25，株式会社オーム社（2009）
MPU-9150 Product Specification Revision4.0，https://strawberry-linux.com/pub/MPU-9150_DataSheet_V4%203.pdf
https://github.com/jrowberg/i2cdevlib

8.付録

8.1実験結果グラフ

8.1.1 速度0.01[m/s]の結果

図19　0.01[m/s]実験値(速度)

図20　0.01[m/s]実験値(変位)

8.1.2 速度0.02[m/s]の結果

図21　0.02[m/s]実験値(速度)

図22　0.02[m/s]実験値(変位)

8.1.3 速度0.05[m/s]の結果

図23　0.05[m/s]実験値(速度)

図24　0.05[m/s]実験値(変位)

8.2 ステッピングモーターコントローラの制御プログラム

図25　0.01[m/s]制御プログラム

図26　0.02[m/s]制御プログラム

図27　0.05[m/s]制御プログラム

8.3 MPU-9150の回路図

図28　MPU-9150回路図

8.4 ArduinoIDEプログラム内容

実験で使用したプログラムは以下である。


#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_REALACCEL
unsigned long time;
#define OUTPUT_READABLE_WORLDACCEL
#define LED_PIN 13
bool blinkState = false;
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 cal;
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
uint8_t teapotPacket[14] = {
  '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24;
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
  pinMode(LED_PIN, OUTPUT);
}
void loop() {
  if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
  }
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
  }
  else if (mpuIntStatus & 0x02) {
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_REALACCEL
    long long int ca,cb,cc;
    long cal;
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetAccel(&aa, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
    time = (millis())/1000;
    Serial.print(time);
    Serial.print("\t");
//Serial.print("areal\t");
    Serial.print(aaReal.x);
    Serial.print("\t");
    Serial.print(aaReal.y);
    Serial.print("\t");
    Serial.print(aaReal.z);
    Serial.print("\t");
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetAccel(&aa, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
    mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
    Serial.print("aworld\t");
    Serial.print(aaWorld.x);
    Serial.print("\t");
    Serial.print(aaWorld.y);
    Serial.print("\t");
    Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
    teapotPacket[2] = fifoBuffer[0];
    teapotPacket[3] = fifoBuffer[1];
    teapotPacket[4] = fifoBuffer[4];
    teapotPacket[5] = fifoBuffer[5];
    teapotPacket[6] = fifoBuffer[8];
    teapotPacket[7] = fifoBuffer[9];
    teapotPacket[8] = fifoBuffer[12];
    teapotPacket[9] = fifoBuffer[13];
    Serial.write(teapotPacket, 14);
    teapotPacket[11]++;
#endif
 // blink LED
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
  }
}

9. 謝辞

本研究を進めるにあたり、沢山のご指導を頂いた卒業論文指導教員の坂本直志准教授に感謝致します。また、日常の議論を通じて多くの知識や示唆を頂いたネットワークシステム研究室の皆様に感謝します。