6軸IMUセンサ MPU-6050をロボットに組み込もう

はじめに

6軸IMUのMPU-6050はTDK InvenSense社製I2Cインターフェースの3軸ジャイロセンサ+3軸加速度センサIC、amazonで格安販売されている。内蔵DMP(Digital Motion Processor)機能を使うことで、補正済みデータとしての4元数Quaternionまたはオイラー角、ロールRoll・ピッチPitch・ヨウYaw角の出力が選べる。本文は6軸MPU-6050 DMPから、4元数Quaternionを読み込んで可視化するまでの手順を以下のとおり示して、ROSドライバをGithubへ公開する。また、MPU-6050を本文のArduinoに接続ではなくMain BoardのI2Cポートへ繋ぐなどの方法がある。また、MPUシリーズはすでに新規設計非推奨になっているため、ICMシリーズは後継機種となっている。安価のため、MPU-6050サンプルの入手ルートはAliexpressにした。

mpu6050-mpu9250
mpu6050(6軸)-mpu9250(9軸)

I2Cインターフェースは、vcc、gnd、scl、sdaの4pinインターフェース

環境

・ubuntu 16.04 Tinker board(or Raspiberry Pi, PC)
・ROS kinetic
・DFRobot Romeo mini v1.1(or arduino uno互換)
・MPU-6050 GY-521

準備①

・ros-kinetic-rosserial-arduino、ros-kinetic-rosserial、rviz_imu_pluginを入れる

$sudo apt-get update
$sudo apt-get install ros-kinetic-rosserial-arduino
$sudo apt-get install ros-kinetic-serial
$cd ~catkin_ws/src/
$git clone -b kinetic https://github.com/ccny-ros-pkg/imu_tools
$cd ..
$catkin_make --pkg imu_tools

・mpu6050_imu_rosを入れる

$cd ~/catkin_ws/src/
$git clone https://github.com/soarbear/mpu6050_imu_ros.git
$cd ~/catkin_ws/
$catkin_make

準備②

・mpu6050_imu_driver/firmware/MPU6050_DMP6/MPU6050_DMP6.inoをArduino IDEでArduinoに書き込む。

imu/dataの可視化

・実に使われるポートtty????を確認する。
・rvizが自動起動して、画面にあるセンサの動きを観察する。

$sudo ls -l /dev/ttyACM*
$sudo chmod 777 /dev/ttyACM0
$roslaunch mpu6050_imu_driver mpu6050_imu.launch

・以下スクリーンショットをクリックすると、youtubeへ遷移する。

mpu6050_imu_ros
mpu6050_imu_ros

センサ融合について

MPU-6050内蔵DMPおよび、センサ融合またはデータ同化Fusionに定番アルゴリズムであるKalman Filterの他、Complementary Filter、Madgwick Filterがある。振動やシステム誤差によって測定値に大きな影響あり、フィルタリングが必須とは言える。

校正

ジャイロのドリフト、加速度センサのバイアスの校正が必要、i2cdevlibのArduino/MPU6050/examples/IMU_Zeroをarduinoに入れてオフセットを読み取り、MPU6050_DMP6.inoに盛り込む。またドリフトに対して、時間平均などキャンセリング手法の取り組みも必要だろう。

感想

「ないよりマシ」の観点から、マイナスにならないが、精度があまり追求しないロボットなどに使用可能と考えられる。ジャイロセンサの温度特性あり、またドリフトは時間とともに蓄積するので、一方加速度センサのバイアスが測定毎にあり、ただし蓄積しないので、最初から校正Calibrationの方法を講じることを考えれば、DMP機能まで用意されて可用性がある。Yaw方位角がジャイロから積分計算して合成していないので要注意で、他のセンサ例えばコンパスまたSlam Lidarなどとの組み合わせが可能である。

ソースコード

mpu6050_imu_rosソースコード(Github)

参考文献

Jeff Rowberg氏:I2C driver
ROS Repository:ROS imu_tools

関連記事

9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続
9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
9軸IMU ICM-20948をロボットに組み込もう
9軸IMU MPU-9250をロボットに組み込もう

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

RGB-D 3Dカメラ市販品

RGB-D 3Dカメラ(デプスカメラ)市販品(一部)の主なスペックを以下のとおりまとめてみよう。

主なスペック

銘柄
型番
深度、Depth-FPS、Depth-解像度、Depth-FOV、Pose FPS)、検出法市場
相場
Hight
Light
STEREO LABS ZED100fps、720p 2560x720他上位解像度あり、0.3~25m、90° (H)x60° (V)x100° (D)、Max100Hz、Stereo Depth Sensing10万円前後深度
Realsense L5150.25~9m、30fps、1024x768、70°±3 x 43°±2°、TOF Lidar方式、IMU付5万円前後精度
Realsense D4550.4~20m、1280×720@30fps、848 × 480@90fps、D455:86° × 57° (±3)、Active IR stereo4万円前後深度
Percipio確認中1.5万円~価格

防水
Orbbec確認中1.5万円~価格
OCCIPITAL0.3~10m、1280x 960@30 fps、FOV:横59° x 縦 46°、IRカメラ&レーザーパターン投影、IMU付7万円前後
MYNT EYE 3D 10300.3~10m(18mはRGB)、752x480@60FPS、D: 146° H:122 V°:76°、IRカメラ&レーザーパターン投影、IMU3万円前後FOV
Matterport Pro2 3DRGB-D Camera x 3、360°回転、8092x4552 pixels @ 70% zoom level (36 MP)本体50万円前後FOV

※ 銘柄並べ替え順=イニシャルアルファベット順

以上

1+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

屋内3D地図で可視化、空間情報をスマート管理

インドアマップが活躍の時代に

オフィスビル、デパート、工場など建物の内部は構造が複雑で、通常の2D地図では各場所の違いを十分に表現できない課題がある。地理情報システムとビッグデータ技術の進歩により、3D地図が室内外の空間情報の可視化技術で設備管理のスマート化やIoTのソリューションを提供することで、この課題を解決する日が近づいている。商業施設、産業IoTの他、交通監視、観光、旅行、セキュリティ、消防、会議、展示、娯楽、公共サービス等の分野で活躍する時代が訪れる。

屋内3D地図が活躍の時代に
屋内3D地図が活躍の時代に

プロダクト・サービス

お客様に対して地図可視化プラットフォームを提供して、クラウド上で各シーンに対応した情報システムを構築する。弊社とFengMap社と連携して、屋内外の3Dマップの作成サービスを提供する。また、開発者向けには専用のエンジンを提供してより簡単に各OS環境に対応したマップアプリの開発ができるようになる。

プロダクト・サービス
プロダクト・サービス

商業施設へ展開の例

商業施設では、CADデータによって室内のデータモデルを構築して各店舗の経営内容を組み込むことで空間データモデルを形成する。それにより、ショッピング案内、店舗管理、経営状況などの情報を共有し、可視化できる。スマート現場クラウド監視プラットフォームを提供する。

スマート現場クラウド監視プラットフォーム
スマート現場クラウド監視プラットフォーム

産業IoTへ展開の例

産業用として、Fengmapは可視化技術をIoTと融合し、設備の位置確認機能構内の設備、車両、人員の所在地と状態を把握して作業のモニタリング、消費エネルギー量の管理、データ統計などを行うことができる。スマート工場可視化管理システムを提案する。

スマート工場可視化管理システム
スマート工場可視化管理システム

フェングマップ社について

Beijing FengMap Technology Co.LTDは、2013年に設立された、北京に拠点を置く技術会社です。同社は、屋内および屋外の空間情報の可視化研究と開発に焦点を当て、地図データの作成、地図の編集、 ストレージ、マップ統合ソフトウェアアプリケーション開発。 空間情報の可視化技術に基づいて、資産管理、人事管理、施設および環境の監視、リモート制御、データ分析を含むさまざまな管理ソフトウェアシステムを多くの顧客に提供しました。創業以来、同社は商業用不動産、工業用IoT、工業団地から、家庭および幅広い公共サービスまで、多くの顧客を獲得した。500社、8000案件を開発した実績をもつという。
英文サイト→ https://www.fengmap.com/en/

日本総代理店

フェングマップ3Dマッピング作成サービスおよびSDK販売。
株式会社翔雲 令和2年3月1日から新住所↓
〒260-0026 千葉市中央区千葉みなと2-2-1502
代表取締役 柳建雄 電話 050-3598-8286
会社サイト https://soarcloud.com
技術情報サイト https://memo.soarcloud.com
販売サイト https://store.soarcloud.com

令和2年4月~5月特別キャンペーンお知らせ

上記時間限定、利益なし特別価格で3D地図作成サービスをご利用いただけます。どうぞお気軽にお問合せされるよう宜しくお願い申し上げます。

1+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

深層学習の2、LSTM Seq2Seqモデル実装

概要

株価、為替、天気、動画など時系列データの予測でよく使われるディープラーニングの代表的手法RNN(再帰型ニューラルネットワーク)の拡張バージョンに、LSTM(Long short-term memory)と呼ばれるモデルがある。今回は、LSTM Seq2Seq、Many to Manyモデルを実装して、円の第4象限の一部(訓練未実施)に対して、時系列データの予測を行ってみる。

環境

keras/LSTM, Google Colab CPU

モデル

lstm_seq2seq_model
lstm_seq2seq_model

以下モデルの概要を説明する。
・Sequentialは、あるレイヤの全ノードと、次レイヤの全ノードをつなぐRNNのモデル。
・RNNの第1 Layerとして、LSTMを追加する。第1引数は出力の次元数、activationは活性化関数で、input_shapeは入力データのフォーマット(in_time_steps x in_features)となる。このlayerがEncoderのように見える。
・RepeatVectorにより、出力を入力として繰り返す。ここでの繰り返し回数は予測範囲(out_vectors)となる。
・再度のLSTM、ただし、ここではシーケンス出力まさにmany outputつまり、return_sequencesをTrueに指定する。このlayerがDecoderのように見える。
・TimeDistributedを指定し、かつ、Dense(out_features)で、出力の次元数を指定する。
・最後にcompileメソッドで、学習時の最適化手法や、損失関数を指定する。ここでは最適化手法としてAdamを、損失関数としてMSE(Mean Squared Errorつまり平均2乗誤差)に指定する。

実装

# -*- coding: utf-8 -*-
# Brief: A Seq2Seq Model Implementation by keras Recurrent Neural Network LSTM.
# Author: Tateo_YANAGI @soarcloud.com
#
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

from keras.models import Sequential
from keras.layers.recurrent import LSTM
from keras.layers.core import Dense, Activation
from keras.layers.wrappers import TimeDistributed
from keras.layers import RepeatVector

PI = 3.1415926535
EPOCHS = 100
TRAIN_SPLIT = 0.98

#
# Class Prediction for lstm model.
#
class Prediction:
  def __init__(self):
    # Array dimension of input&output:(batch_size, time_steps or vectors, features).
    self.batch_size = 48
    self.in_time_steps = 6
    self.in_features = 2
    self.out_vectors = 3
    self.out_features = 2
    self.hidden_neurons = 100 # Neurons of hidden layer.
    self.epochs = EPOCHS # Iteration times.
    self.train_split = TRAIN_SPLIT # Partition of learning dataset.
    self.activation_hidden = 'tanh' # Activation function for hidden units.
    self.activation_output = 'linear' # Activation function for output units.
    self.optimizer= 'adam' # Optimization function.
    self.loss = 'mse' # Loss(cost) function.

#
# Create dataset.
#
  def create_dataset(self, train_split=0.8, in_time_steps=10, out_vectors=4, in_features=2, out_features=2):
    a = np.array([[np.cos(0.0), np.sin(0.0)]])
    for i in range(1, int(2*PI*10)):
      a = np.append(a, np.array([[np.cos(i*0.1), np.sin(i*0.1)]]), axis=0)
    print(f'[Debug-0]\nlen(a):{len(a)}\na:\n{a}')

    # Create dataset for train and test
    # Initialize x_train and y_train.
    x_total = a[0:in_time_steps, 0:in_features]
    y_total = a[in_time_steps:in_time_steps+out_vectors, 0:in_features]

    # Calculate length for x_total and y_total.
    total_len = a.shape[0] - in_time_steps - out_vectors + 1
    print(f'[Debug-1]\ntotal_len:{total_len}')

    # Fill out x_total and y_total.
    for i in range(1, total_len):
      x_total = np.append(x_total, a[i:i+in_time_steps], axis=0)
      y_total = np.append(y_total, a[i+in_time_steps:i+in_time_steps+out_vectors], axis=0)
    print(f'[Debug-2]\nx_total.shape[0]):{x_total.shape[0]}\nx_total:\n{x_total}\ny_total.shape[0]:{y_total.shape[0]}\ny_total:\n{y_total}')

    # Reshape x_toal and y_total from 2D to 3D.
    x_total = np.reshape(x_total[0:x_total.shape[0]], (-1, in_time_steps, in_features))
    y_total = np.reshape(y_total[0:y_total.shape[0]], (-1, out_vectors, out_features))

    # Split dataset for train and test
    x_train = x_total[0:int(x_total.shape[0]*train_split)]
    y_train = y_total[0:int(x_total.shape[0]*train_split)]
    #x_test  = x_total[x_train.shape[0]:]
    #y_test  = y_total[y_train.shape[0]:]
    x_test  = x_total[-2:]
    y_test  = y_total[-2:]
    print(f'[Debug-3]\nx_train.shape[0]:{x_train.shape[0]}\nx_train:\n{x_train}\ny_train.shape[0]:{y_train.shape[0]}\ny_train:\n{y_train}')
    print(f'[Debug-4]\nx_test.shape[0]:{x_test.shape[0]}\nx_test:\n{x_test}\ny_test.shape[0]:{y_test.shape[0]}\ny_test:\n{y_test}')

    return x_train, y_train, x_test, y_test

#
# Create lstm model.
#
  def create_model(self) :
    model = Sequential()
    # Encoder
    model.add(LSTM(self.hidden_neurons, activation=self.activation_hidden, input_shape=(self.in_time_steps, self.in_features)))
    # Output used as Input.
    model.add(RepeatVector(self.out_vectors))
    # Decoder, output sequence
    model.add(LSTM(self.hidden_neurons, activation=self.activation_hidden, return_sequences=True))
    model.add(TimeDistributed(Dense(self.out_features, activation=self.activation_output)))
    #model.add(Activation(self.activation_output))   
    model.compile(optimizer=self.optimizer, loss=self.loss, metrics=['accuracy'])
    return model

  def train_model(self,model, x_train, y_train) :
    model.fit(x_train, y_train, epochs=self.epochs, verbose=2, batch_size=self.batch_size)
    return model

#
# main()
#
if __name__ == "__main__":
  predict = Prediction()
  train_in_data = None
  # Create dataset
  x_train, y_train, x_test, y_test = predict.create_dataset(predict.train_split, predict.in_time_steps,\
                                                            predict.out_vectors, predict.in_features, predict.out_features)
  # Create model
  model = predict.create_model()
  # Train model
  model = predict.train_model(model, x_train, y_train)
  print(model.summary())
  # Test
  predicted = model.predict(x_test, verbose=1)
  print(f'[info_1]predicted:\n{predicted}')
  # Plot result
  predicted = np.reshape(predicted, (-1, predict.out_features))
  x_test = np.reshape(x_test, (-1, predict.in_features))
  y_test = np.reshape(y_test, (-1, predict.out_features))
  x_train = np.reshape(x_train, (-1, predict.in_features))
  y_train = np.reshape(y_train, (-1, predict.out_features))
  plt.figure(figsize=(8, 8))
  plt.scatter(x_train[:,0], x_train[:,1])
  plt.scatter(x_test[:,0], x_test[:,1])
  plt.scatter(y_test[:,0], y_test[:,1])
  plt.scatter(predicted[:,0], predicted[:,1])
  plt.show()

※ソースコード公開→
https://github.com/soarbear/lstm_seq2seq_model_prediction

結果

lstm_seq2seq_model_prediction
lstm_seq2seq_model_prediction

今回、epochsとneuronsを増やすことで、上図のとおり精度において納得いく結果を得た。これでコスト関数、活性化関数の選定はじめ、ハイパーパラメータの調整が精度にいかに重要なことを分かる。熱伝播、振動、柔軟ロボット関節の駆動力など元々フーリエ変換、ラプラス変換、Z変換しか解けない偏微分方程式が、LSTMモデルを適用できたらどうかと今度試してみる。

参考文献

1、LSTMネットワークを理解する(英文原稿)、Christopher Olah氏
2、Keras公式資料

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

粒子フィルタ

はじめに

ガウス分布に従う時系列確率変数の非線形関数は必ずしもガウス分布にならない、また非ガウス分布の確率変数が存在するから、優雅なるカルマンフィルタファミリーには限界あり、無理な場合がある。非線形、非ガウス時系列空間モデルに対して、1990年代から未知の状態密度関数p(x)に対して、既知の密度関数q(x)、p(x)⊂q(x) から逐次モンテカルロ法(逐次重要性サンプリングSequential Importance Samplingに、リサンプリングResampling)による粒子フィルタ(パーティクル・フィルタ、PF)が提案されて以来、研究・応用が活発になっている。粒子フィルタがROSのSLAM Gmappingアルゴリズムに組まれている。

粒子フィルタ

状態空間モデル

・システムモデル(状態方程式)
$$x_t = f_t(x_{t-1},v_t)$$

・観測モデル(観測方程式)
$$y_t = h_t(x_t,w_t)$$

ただし、\( v_t、w_t\)はフィルタリングをかける事前に既知で、非ガウス分布可能、\( y_t\)は2項分布またポアソン分布に従うcount dataのモデル化なども可能になる。また、粒子フィルタには、\( v_t、w_t\)はガウス分布でも構わない。
以下に粒子\( x_t^{[i]} \)~\( f_t (x_t^{[i]} | x_{t−1}^{[i]}, v_t) \)として、粒子フィルタリングの手順を示す。

粒子フィルタリングの手順

1、初期化\( (t=0) \)
初期分布\( f_t (x_0^{[i]}, v_0) \)に従って、n個の粒子\( \{x^{[i]}_0 | i = 1,2, ⋯ ,n\} \)を無作為に発生させる。

2、一期先予測\( (t=t+1) \)
粒子\( x_t^{[i]} \)を\( f_t (x_t^{[i]} | x_{t−1}^{[i]}, v_t) \)に従って状態推移させ、それぞれの類似値が\(x_t^{[i]}\)である粒子の集合\( \{\hat{x}^{[i]}_t | i = 1,2, ⋯ ,n\} \)を発生させる。

3、フィルタリング
・重み計算
粒子\( \hat{x}_t^{[i]} \) の重み\( w_t^{[i]} = p(y_k | \hat{x}_k^{[i]}) \)を計算する。
ただし、\( p(y_k | \hat{x}_k^{[i]}) \)は、尤度であり、観測値または類似値ではないことに注意する。

・重みの正規化
$$\hat{w}_t^{[i]} = \frac{w_t^{[i]}}{ \sum_{i=1}^n w_t^{[i]} }$$

・リサンプリング
粒子\( \hat{x}^{[i]}_0 \)を\(\hat{w}_t^{[i]}\)に従った確率でリサンプリングし、粒子集合\( \{x^{[i]}_t | i = 1,2, ⋯ ,n\} \)を発生させる。

・2に戻る

参考文献

粒子フィルタ、著者:樋口知之 先生

1+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア