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翔・電子部品ストア

カルマンフィルタの導出

はじめに

wiki: 「カルマンフィルタ (Kalman Filter、KFと略す) は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルタの一種である」。観測値に観測雑音、状態予測値にシステム雑音があって場合により時間とともに誤差がドンドン蓄積してそのまま使えないので、観測と予測のガウス性を利用した線形センサデータフュージョンがカルマンフィルタの原点である。

状態空間モデル

時系列解析の中で、予測値と観測値の間何らかの因果関係を見つけて、何らかの方法でそれらのデータを絡んでモデル化して状態を推定していく。ここで汎用的な状態方程式観測方程式は以下の式にする。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1} ) + q_{t-1}\\&& y_t = H_t(x_t) + r_t\end{eqnarray*}$$
ただし、システム雑音、観測雑音\((q_t, r_t)\)は期待値が\(0\)、分散が\((Q_t, R_t)\)の独立正規分布(ガウス分布)にする。
$$\begin{eqnarray*}&& p(q_t) ~ N(0,Qt)\\&& p(r_t) ~ N(0,Rt)\\&& E[q_t r_t]=0 \end{eqnarray*}$$

基本原理

カルマンフィルタの原理は以下のイメージに示すように、最小解析誤差共分散推定(最小二乗)から予測値、観測値をそれぞれの割合\(I-K_t H_t , K_t\)で線形合成した値を状態推定値にする。

カルマンフィルタの原理
カルマンフィルタの原理

式の導出

時系列データに適用して、以下予測Predict→更新Correctまたは、時間更新Time Update→測定更新 Measurement Updateの繰り返しによって、状態の数学期待最小解析誤差共分散を求める手順になる。

Step0=パラメータ初期化(t=0)

状態推定(数学期待)、解析誤差共分散の初期化
$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

Step1=状態値、共分散行列予測(t>0)

$$\begin{eqnarray*}&& \bar{x_t} = F_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + Q_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q^T_{t-1}]\)

Step2=カルマンゲイン、状態値、共分散行列更新

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(R_{t}=E[r_{t}r^T_{t}]\)

KFフローチャート

Kalman_Filter_Flowchart_2
Kalman_Filter_Flowchart_2

拡張カルマンフィルタ

拡張カルマンフィルタ(Extended Kalman Filter、EKFと略す)は、非線形フィルタリングである。前述した状態方程式、観測方程式より、以下の状態空間モデルの\(f(⋅)\)または\(h(⋅)\)が非線形関数であり、拡張カルマンフィルタが適用される。テイラー展開より、2次微分以降の項目を省略して、非線形である\(f(⋅), h(⋅)\)の1次微分を線形化とし、前述したカルマンフィルタのアルゴリズムが適用可能となる。しかし、\(f(⋅), h(⋅)\)の微分では\(f(⋅), h(⋅)\)の一部しか表現できず、この線形化処理(1次微分)が誤差を大きく招く可能性がある。
$$\begin{eqnarray*}&& F_{t-1} =\frac{\partial f_{t-1}(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\&& H_{t} =\frac{\partial h_{t}(x)}{\partial x}|_{x=\hat{x}_{t}}\end{eqnarray*}$$

状態空間モデル

$$\begin{eqnarray*}&& x_t = f_{t-1}(x_{t-1}) + q_{t-1}\\&& y_t = h_t(x_t) + r_t\end{eqnarray*}$$

Step0=パラメータ初期化(t=0)

$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

Step1=状態値、共分散行列予測(t>0)

$$\begin{eqnarray*}&& \bar{x_t} = f_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + Q_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q^T_{t-1}]\)

Step2=カルマンゲイン、状態値、共分散行列更新

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – h_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(R_t=E[r_t r^T_t]\)

EKFフローチャート

Extended_Kalman_Filter_Flowchart_2
Extended_Kalman_Filter_Flowchart_2

拡張カルマンフィルタを6軸IMUへの適用

6軸IMU~拡張カルマンフィルタ

カルマンフィルタの再考

カルマンフィルタでは、状態推定値を予測結果\(x_t\)(実装例ではジャイロセンサーデータ)と観測データ\(y_t\)(実装例では加速度センサデータ)の線形結合で合成し,その誤差分散を最小にする推定法だと分かる。これはシステム誤差、観測誤差の数学期待が0の正規分布との前提条件から由来した推定法である。しかし、カルマンフィルタをかけることで、状態推定値は予測結果と観測データの間にあるのは、真値からかなり乖離してしまう場合にあるのか。これはシステム誤差と観測誤差が無相関かつ直交という前提から、勿論真値が推定値と観測値の間にある結論を結ぶ考えである。また、ジャイロセンサーデータと、加速度センサデータとも観測値\(y_t\)にする方法がある。それぞれのパーフォーマンスの検証は、比較的精確な実験環境(比較用の高精度ジャイロセンサ、加速度センサ、エンコーダ、モータ)がないと、実は容易ではない。というよりも、シミュレーションをかけてカルマンフィルタのアルゴリズムを検証するのが、確実に可能である。

参考文献

1-wikipedia: カルマンフィルタオイラー角
2-Greg Welch氏、Gary Bishop氏: An Introduction to the Kalman Filter
3-田島洋氏: マルチボディダイナミクスの基礎―3次元運動方程式の立て方

関連記事

6軸IMU MPU-6050をロボットに組み込もう
YDLIDAR G4=16m 薄型 ROS対応SLAM LIDAR
研究開発用 台車型ロボット キット
オイラー角~ジンバルロック~クォータニオン
SLAM~拡張カルマンフィルタ
SLAM~Unscentedカルマンフィルタ

3+

ロボット・ドローン部品お探しなら
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翔・電子部品ストア

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翔・電子部品ストア