9軸IMU ICM-20948をロボットに組み込もう

はじめに

TDK Invensense製9軸IMUのICM20948は、TDK MPU-9250の後継機種で、MPU-9250のVDDは2.4V~3.6V、VDDIOは1.71V~VDDに対して、ICM-20948のVDDは1.71V~3.6V、VDDIOは1.71V~1.95Vに低めに設定して、省電力となった。また、デジタルモーションプロセッサDMPによるデータフュージョン(FPGAによるFusion)の特長が継承して、MPU-9250 DMPの増強版となったと、詳細まで公開されていないが以下参考文献を読むと分かる。

環境

・Ubuntu 18.04
・ROS Melodic
・MCU: SAMD21G18A
・IMU: ICM-20948

DMP3の出力確認

以下のように、出力周波数50Hz、加速度Ax Ay Az、角速度Gx Gy Gz、磁場Mx My Mz、4元数Qw Qx Qy Qzの順に出力させる。

imu-icm20948-output
imu-icm20948-output

4時間+にわたる連続動作して出力を確かめる。完全静止状態でもないので、ドリフトは納得いく範囲内にとどまっている。rvizで確かめてもドリフトが多少あって肉眼では見えないほど。

imu-icm20948-output-4hours
imu-icm20948-output-4hours

出力確認動画は以下イメージをクリックすると、youtubeへジャンプする。

icm20948_imu_ros
icm20948_imu_ros

感想

MPUシリーズと比べて、ユーザの事前校正いらず、長時間(実験は4時間程度まで)においても、ドリフトとくにヨウ角(Yaw、方向角)のドリフトは目立たないほどとなった。また、1.71Vの低電圧でも動作可能なのでスマートデバイスや、ロボットの長時間電池駆動が可能になる。なお、出力周波数はMax 200Hzと確認できた。Icm-20948 DMP3(IMU内蔵FPGA)から出力した、Accel/Gyro/Mag計9軸データ出力にQuaternionの4元数データがそのまま利用可能で、遅延もソフト・カルマンフィルタなどより少なく他社IMUより優れるため、ロボットの精度向上に利用可能。1.8V VDDIO対応、DMP3の出力に手間かかった末、コンパス単体は評価できなくて残念だったが、総じて優秀としか思わないこのICM-20948をロボット装置に組み込もうと決めた。

参考文献

Migrating from MPU-9250 to ICM-20948-InvenSense
DS-000189-ICM-20948-v1.3.pdf

関連記事

9軸IMU MPU-9250をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

2+

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

9軸IMU MPU-9250をロボットに組み込もう

本文の読む時間は約5分で、再現時間は約2時間程度に設定する。

はじめに

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

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

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

環境

・ubuntu 18.04 Tinker board(or Raspiberry Pi, PC)
・ROS melodic
・DFRobot Romeo mini v1.1(or arduino uno互換)
・MPU-9250/6500

準備①

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

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

・mpu9250_imu_rosを入れる

$cd ~/catkin_ws/src/
$git clone https://github.com/soarbear/mpu9250_imu_ros.git
$cd ~/catkin_ws/
$catkin_make_isolated

準備②

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

imu/dataの可視化

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

$sudo ls -l /dev/ttyACM*
$sudo chmod 777 /dev/ttyACM0
$roslaunch mpu9250_imu_driver mpu9250_imu.launch

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

mpu6050_imu_ros
mpu9250_imu_ros

センサ融合について

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

ソースコード

mpu9250_imu_rosソースコードがGithubへ公開済み。

参考文献

1-Jeff Rowberg氏: I2C driver
2-ROS Repository: ROS imu_tools

関連記事

9軸IMU ICM-20948をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

2+

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

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

本文の読む時間は約5分で、再現時間は約2時間程度に設定する。

はじめに

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シリーズは後継機種となっている。

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 ICM-20948をロボットに組み込もう
9軸IMU MPU-9250をロボットに組み込もう

2+

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

struct2depth~単眼カメラ2D camera Visual SLAM

はじめに

Google がTensorflowのResearch Modelとしてstruct2depth、vid2depthを公開したので、struct2depthを利用して単眼カメラMonocular Cameraで撮った写真から深度Depthを推定してみよう。struct2depth、vid2depthは、KITTIまたは、CITYSCAPEの学習データを通してVisual Odometry、Depthの推定を習得するモデルである。また他の学習データを入れ替えてもあり得ると考えられる。SFM:Structure From Motionに基づく技術で、Depth深度まで推定できれば、3D Recontruction3次元復元まで使われる。

実測

雑居ビール内、ドラッグストア前および、ホールで写真を撮って完了とした。

推定

画像サイズを416×128に縮小して、推定の時間を短縮する。

環境

・ Google Colab, 18.04.3 LTS Bionic Beaver, GPU Tesla k80
・ Tensorflow 1.15.2
・ Research model struct2depth/KITTI

手順

学習せずKITTIモデルをそのまま利用したので、推定手順は以下のとおり。
・tensorflow_versionを1.xに合わせる。

・ランタイムを再起動。

%tensorflow_version 1.x
import tensorflow
print(tensorflow.__version__)

・以下確認できるまで、またランタイムを再起動する。

TensorFlow 1.x selected.
1.15.2

・インファレンス

!python inference.py --logtostderr --file_extension png --depth --egomotion true --input_dir image --output_dir output --model_ckpt model/KITTI/model-199160

結果

単眼カメラで撮ったRGB写真、レンダリングした深度推定イメージを結果として出力される。点群データの3Dイメージは別途プログラムを作成してレンダリングRenderingとする。

うまくいく例

完璧ではないが、扉、旗まで殆ど良く推定できている。

struct2depth_depth_ok_case
struct2depth_depth_ok_case

mayaviで点群Point Cloudデータの3D表現

Mayaviは、matplotlibよりパワーアップして、強力なエンジンVTKを利用した3Dツールである。

point_cloud_3d_plot
point_cloud_3d_plot

上図のように3Dで写真を細かく表現できた。点群データ(npyファイル)による3D表現のpythonソースは、Githubへ公開済み。

うまくいかない例

左下に推定が失敗と見られる。他の場所はなんとなく推定てきている。

struct2depth_depth_ng_case
struct2depth_depth_ng_case

原因を探る

・ KITTIモデルは屋外モデルでそのままでは屋内に向かない場合ある。測定環境にふさわしい学習データセット(モデル)が必要である。
・ 照明の強弱、特徴量に大きく関わること。
・ ついてはまだ実験が不十分だが、商用可能なVisual SLAMに道が長く感じさせられる。

参考文献

Depth Prediction Without the Sensors: Leveraging Structure for Unsupervised Learning from Monocular Videos, Auther: Vincent Casser etc
github google tensorflow model struct2depth

以上

1+

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

機械学習の13、SVD特異値分解

はじめに

本文には、大文字表現=行列/マトリクス、\(\boldsymbol{bold}\)小文字表現=ベクトル\(R^d\)、普通小文字表現=スカラー\(R\) と記す。

SVD(Singular Value Decomposition)は機械学習の分野で広く使用されているアルゴリズムで、次元削減アルゴリズムの特徴分解だけでなく、推薦システム(Recommender System)や自然言語処理(Nature Language Process)にも使用される。

原理

SVDは前述した特徴分解同じく行列を分解するが、SVDは分解する行列が正方行列にする必要ない。行列Aの形状が\(m×n\)であると仮定すると、行列AのSVDを次のように定義する。
$$ A = UΣV^T $$
ただし、\(U\)は\(m×m\)の行列\((u_1, u_2,…, u_m)\)、\(Σ\)は\(m×n\)の行列、主対角線上の要素を除く全ての要素が実数ゼロ\(0\)であり、主対角線上の各要素は特異値(Singular Value)という、\(V\)は\(n×n\)行列\((v_1, v_2,…, v_n)\)。

\(A\)の転置\(A^T\)と\(A\)を行列で乗算すると、\(n×n\)の正方行列\(A^T A\)が得られる。\(A^T A\)は正方行列であるため、特徴の分解を実行でき、得られた固有値と固有ベクトルは次の式を満たす。
$$ (A^T A)\boldsymbol{v_i}=λ_i\boldsymbol{v_i} $$
これで行列\(A^T A\)の\(n\)個の固有値と対応する\(n\)個の固有ベクトル\(v_i\)を取得できる。

\(A\)と\(A\)の転置\(A^T\)を行列で乗算すると、\(m×m\)の正方行列\(AA^T\)が得られる。\(AA^T\)は正方行列であるため、特徴の分解を実行でき、得られた固有値と固有ベクトルは次の式を満たす。
$$ (AA^T) \boldsymbol{u_i}=λ_i\boldsymbol{u_i} $$
これで行列\(AA^T\)の\(m\)個の固有値と対応する\(m\)個の固有ベクトル\(u_i\)を取得できる。

\(Σ\)は対角線上の特異値を除いて全て\(0\)で、各特異値\(σ_i\)を見つけるだけで\(Σ\)が求められる。
$$ \sigma_i = \sqrt{λ_i} $$

各特異値\(σ_i\)のうち、比較的大きいほう(主成分)とそれに対応する特異ベクトル\(u_i, v_i\)を\(k\)個\((k << n)\)残すとAの次元を削減する。
$$ A_{m×n}=U_{m×m}Σ_{m×n}V^T_{n×n} ≈ U_{m×k}Σ_{k×k}V^T_{k×n} $$

実装

以下行列dataSetに対して、SVDアルゴリズムで\((U, \Sigma, V^T)\)を求めて、5次元→3次元つまり2次元を削減してが新しい\(\Sigma\)で\((U* \Sigma*V^T)\)が元の行列dataSetに戻せるかを確かめる。

from numpy import *
def loadExData():
    return[[0, 0, 0, 2, 2],
           [0, 0, 0, 3, 3],
           [0, 0, 0, 1, 1],
           [1, 1, 1, 0, 0],
           [2, 2, 2, 0, 0],
           [5, 5, 5, 0, 0],
           [1, 1, 1, 0, 0]]
dataSet = loadExData()
U, Sigma, VT = linalg.svd(dataSet)
print(f'dataSet:\n{dataSet}')
print(f'U:\n{U}\nSigma:\n{Sigma}\nVT:\n{VT}')
// 小さいSigmaを0にする(削除)
Sig3 = mat([[Sigma[0], 0, 0], [0, Sigma[1], 0], [0, 0, Sigma[2]]])
print(f'U[:,:3] * Sig3 * VT[:3,:]:\n{U[:,:3] * Sig3 * VT[:3,:]}')

ソースコード
https://github.com/soarbear/Machine_Learning/tree/master/svd

結果

\((U* \Sigma*V^T)\)は元のdataSetとほぼ同じ行列だと分かる。

dataSet:
[[0, 0, 0, 2, 2], [0, 0, 0, 3, 3], [0, 0, 0, 1, 1], [1, 1, 1, 0, 0], [2, 2, 2, 0, 0], [5, 5, 5, 0, 0], [1, 1, 1, 0, 0]]
U:
[[-2.22044605e-16  5.34522484e-01  8.41641151e-01 -1.37443101e-02
  -7.57428665e-02 -1.11022302e-16  1.38777878e-17]
 [ 0.00000000e+00  8.01783726e-01 -4.92426901e-01 -2.47257115e-01
   2.31349353e-01  3.15719673e-16 -2.77555756e-17]
 [ 0.00000000e+00  2.67261242e-01 -2.06001597e-01  7.69259966e-01
  -5.42562325e-01 -7.55450741e-16  1.09551769e-16]
 [-1.79605302e-01  2.77555756e-17 -3.00520660e-02 -2.15935735e-01
  -2.94749442e-01  9.05439185e-01 -1.16246358e-01]
 [-3.59210604e-01  5.55111512e-17 -6.01041319e-02 -4.31871470e-01
  -5.89498885e-01 -4.19124526e-01 -3.97074256e-01]
 [-8.98026510e-01  0.00000000e+00  3.60624791e-02  2.59122882e-01
   3.53699331e-01  5.40010673e-16 -6.71525577e-17]
 [-1.79605302e-01  2.77555756e-17 -3.00520660e-02 -2.15935735e-01
  -2.94749442e-01 -6.71901321e-02  9.10394870e-01]]
Sigma:
[9.64365076e+00 5.29150262e+00 8.36478329e-16 6.91811207e-17
 1.11917251e-33]
VT:
[[-5.77350269e-01 -5.77350269e-01 -5.77350269e-01  0.00000000e+00
   0.00000000e+00]
 [-2.46566547e-16  1.23283273e-16  1.23283273e-16  7.07106781e-01
   7.07106781e-01]
 [-7.01908483e-01 -1.02844064e-02  7.12192890e-01 -2.22044605e-16
  -1.66533454e-16]
 [-4.17122461e-01  8.16431808e-01 -3.99309347e-01  0.00000000e+00
  -1.11022302e-16]
 [-0.00000000e+00 -1.96261557e-16  1.96261557e-16  7.07106781e-01
  -7.07106781e-01]]
U[:,:3] * Sig3 * VT[:3,:]:
[[ 4.47427211e-17  1.57774942e-15  2.08638397e-15  2.00000000e+00
   2.00000000e+00]
 [-7.56974048e-16  5.27282824e-16  2.29691224e-16  3.00000000e+00
   3.00000000e+00]
 [-2.27747782e-16  1.76121044e-16  5.16267387e-17  1.00000000e+00
   1.00000000e+00]
 [ 1.00000000e+00  1.00000000e+00  1.00000000e+00  1.03851855e-16
   1.03851855e-16]
 [ 2.00000000e+00  2.00000000e+00  2.00000000e+00  2.07703709e-16
   2.07703709e-16]
 [ 5.00000000e+00  5.00000000e+00  5.00000000e+00 -6.69808260e-33
  -5.02356195e-33]
 [ 1.00000000e+00  1.00000000e+00  1.00000000e+00  1.03851855e-16
   1.03851855e-16]]

参考文献

[1] PeterHarrington. Machine Learning in Action.

2+

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