タグ: ロボット

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

はじめに

TDK Invensense製9軸IMUのICM-20948は、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(ICM-20948内蔵FPGA)によるデータフュージョン(FPGAによるFusion)の特長が継承して、さらにRAM容量が拡張して、6軸フュージョンのみならず9軸フュージョンまで増強して、較正機能もあると、以下参考文献を読むと詳細まで分かる。

環境

・Ubuntu 18.04
・ROS Melodic
・MCU: Cortex M0+
・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で確かめてもドリフトが肉眼では見えないほど。ドリフトにおいては、MPU-9250から大いに改善されたと見られる。

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

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

icm20948_imu_ros
icm20948_imu_ros

最後に

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

商品化モジュール

ICM-20948とCoretex M0+を組み込んだ回路を設計して、ROSに対応したロボット専用センサモジュールとして商品化して、2021年1月～、リリースと予定している。この商品は皆さんの学術研究にお役に立てるようと願う。主な仕様は以下のとおり。
・構成　CortexM0+ & TDK Invensense ICM-20948(9軸)実装
・接続　USB Type-Cコネクタ実装
・出力　6軸／9軸融合4元数はFPGA on chipから低遅延で出力、別途ソフトでフュージョン必要なし、出力レート225Hz、同時に加速度(アクセル)3軸データ225Hz、角速度(ジャイロ)3軸データ225Hz、地磁気(コンパス)3軸データ75Hzまで出力可能
・ROSパッケージ、Githubへ公開、ROS Kinetic以降対応、ROS TopicへSubscribeすることでデータが受け取り可能
・rviz実演、実演ビデオあり

【2021年3月いま現在】1回目制作分（評価版）、大学など研究機関へ無料配布中（アンケート調査あり）、WEBでの募集を含めて順次終了。

【2021年4～5月予定】2回目制作分（商用版）、販売の予定。

詳細情報

9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応

取扱店舗

9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 ROS対応 | ROBOT翔

参考文献

Migrating from MPU-9250 to ICM-20948-InvenSense

関連記事

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

2+

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

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

はじめに

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となって、MPU-9250に比べて、事前校正不要で、ドリフトの低減、省電力などにおいてパフォーマンスが改善された。本文は、6軸MPU-6050に続いて、9軸MPU-9250 DMPから4元数Quaternionを読み込んで可視化するまでの手順を以下のとおりに示して、ROSドライバをGithubへ公開する。安価のため、MPU-9250サンプルの入手ルートはAliexpressにした。

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-rosserial
$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)

後継機種

ICM-20948はMPU-9250の後継機種、その製品化情報は 9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続、9軸IMU 6軸／9軸フュージョン ICM-20948 Cortex-M0+内蔵 ROS対応

参考文献

1-Jeff Rowberg氏: I2C driver
2-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をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

3+

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

研究開発用 台車型ロボット キット

低コスト台車ロボットプラットフォーム

外形寸法は400x395x212.5mm、可搬重量は約40kg、最高速度は約0.5m/s、アルミ合金により軽量で高剛性なロボットキットとなります。SLAM、ROS、AI等の機能が実装する台車型ロボット、自律移動ロボット、自律走行ロボット、無人搬送ロボット、AGVの研究開発用プラットフォームとしてご利用いただけます。

主な規格

外形寸法　L400mm×W395mm×H212.5mm
台車重量　約10kg
積載重量　40kg（台車重量を除く）
駆動方式　駆動輪2個（二輪駆動）、前部キャスター1個
最高速度　約1.0m/s
駆動輪直径　125mm
駆動輪素材　アルミホイル、ゴム
ブラシ付きモータ　定格電圧12V、定格電流3A、定格回転数77RPM、定格トルク2.79Nm、減速比51、ストール電流7A
光学式エンコーダ　 500PPR(モータ・シャフト)／13500PPR(ギヤボックス・シャフト)
モータドライバ　エンコーダAB相入力2系統、モータ制御入力2系統、最大电流30A

販売元

研究開発用 台車ロボット キット
カスタマイズ、特注もお承りいたします。

関連記事

9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応
点検向け自律移動ロボットRED（薄型・小型）
SLAM～拡張カルマンフィルタ
SLAM～Unscentedカルマンフィルタ
粒子フィルタ
オイラー角～ジンバルロック～クォータニオン

2+

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

2輪差動駆動型ロボット2WDの運動学モデル

運動学分析

2輪差動駆動型ロボット運動学モデル
2輪差動駆動型ロボット運動学モデル

以上のイラストに示された、車軸中心Cの瞬間速度はv、回転角速度ω、回転半径rは、以下の数式で表される。
$$\begin{bmatrix}v\\ω\\r\end{bmatrix}=\begin{bmatrix}\frac{v_1 + v_2}{2}\\\frac{v_1 – v_2}{d}\\\frac{v}{ω}\end{bmatrix}=\begin{bmatrix}\frac{v_1 + v_2}{2}\\\frac{v_1 – v_2}{d}\\\frac{d}{2} \frac{v_1 + v_2}{v_1 – v_2}\end{bmatrix}$$
但し、d: 車輪間距離、v1: 左輪速度、v2: 右輪速度、ω: 角速度、r: 回転半径

運動状態分析

2輪差動駆動型ロボット運動状態
2輪差動駆動型ロボット運動状態

if v1 > v2 →　ロボットが右方向へ回転する
if v1 = v2 →　ロボットが直進または後退する
if v1 = -v2 →　ロボットが車軸中心Cまわりに回転する、回転半径0m

運動学モデル

ロボット座標系からワールド座標系へ回転変換する際、2次元回転行列を用いる。
$$\begin{bmatrix}x_{t+1}\\x_{t+1}\\θ_{t+1}\end{bmatrix}=\begin{bmatrix}x_{t}\\x_{t}\\θ_{t}\end{bmatrix}+ \begin{bmatrix}cosθ&-sinθ&0\\sinθ&cosθ&0\\0&0&1\end{bmatrix}\begin{bmatrix}Δx_r\\Δy_r\\Δθ_r\end{bmatrix}$$
$$\begin{bmatrix}Δx_r\\Δy_r\\Δθ_r\end{bmatrix}=\begin{bmatrix}cosΔθ_r&0\\sinΔθ_r&0\\0&1\end{bmatrix}\begin{bmatrix}v_rΔt\\ω_rΔt\end{bmatrix}$$
また、他の計算方法があり、いずも近似式で、走行距離とともに誤差が累積していく。
ROSでは、上記情報のもとで、odomトピックを他のノードへブロードキャストし続ける。

関連記事

9軸IMU 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応

0

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

ROS・Unity・ロボット・ドローン姿勢の回転表現

はじめに

姿勢\(v\)から姿勢\(v’\)への変換はどう表現するのかは、ロボットや、ドローンの開発、運用に避けられない要素である。本文に出ているソースコードは、\(Python\)言語を用いる。

オイラー角、回転行列の表現

固定座標系・機体座標系・XYZ軸回転順オイラー角
固定座標系・機体座標系・XYZ軸回転順オイラー角

物体を\(X\)軸、\(Y\)軸(\(Y’\)軸)、\(Z\)軸(\(Z”\)軸)まわりの順にそれぞれオイラー角のロール角\(ϕ\)、ピッチ角\(θ\)、ヨー角\(ψ\)だけ回転させたときに、物体の姿勢の変換は、オイラー角と、もしくはオイラー角の三角関数を用いる以下の回転行列で表す。
$$\small R_{xyz} = \begin{bmatrix} CθCψ & -CθSψ & Sθ \\ SϕSθCψ+CϕSψ & -SϕSθSψ+CϕCψ & -SϕCθ \\-CϕSθCψ+SϕSψ & -CϕSθSψ+SϕCψ & CϕCθ \end{bmatrix} $$ ただし、\(C=cos,S=sin\) とする。
しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点\((Gimbal \space Lock)\)があり、例えば\(Y’\)軸回りを\(90°\)回転すると、\(X’\)軸と\(Z\)軸が同軸となってしまい、以降は姿勢制御(表現)ができなくなる。
ジンバルロック
ジンバルロック

このジンバルロックを解消するにはクォータニオンの使命となった。

回転ベクトルの表現

\(ROS\)では回転ベクトルのクォータニオン(四元数)\(q=ix+jy+kz+w\)は、

q=(x,y,z,w)

と表す。原点を通す回転軸を表す単位ベクトル\(a=(ax,ay,az)\)で、この回転軸まわり、角度\(θ\)だけを回転する場合は、クォータニオンは

(x,y,z,w)=(ax*sin(θ/2), ay*sin(θ/2), az*sin(θ/2)，cos(θ/2))

と表す。

以下の例では、\(XY\)平面\((z=0)\)にて辺長\(1m\)の正方形に沿って例えば仮に自律移動ロボットに走行させて、動作を確認しよう。勿論\(Gazebo\)でも確認できる。

\(ROS\)では、ロボットの位置＆姿勢の表現について、ロボットの中心または、ロボットにある他のポイントの位置は\(x,y,z(m)\)、姿勢ポーズはクォータニオンで表す。

from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
...
locations['square_vertex_1'] = pose = Pose(Point(1.0,0.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
locations['square_vertex_2'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_3'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_4'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
...

回転ベクトル→オイラー角

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;    
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
double yaw, pitch, roll;    
mat.getEulerYPR(yaw, pitch, roll);

オイラー角→回転ベクトル

tf::Quaternion q;
q.setRPY(Out_X, Out_Y, Out_Z);
sensor_msgs::Imu imu_data;
imu_data.orientation.x=q[0];
imu_data.orientation.y=q[1];
imu_data.orientation.z=q[2];
imu_data.orientation.w=q[3];

関連記事

オイラー角～ジンバルロック～クォータニオン
9軸IMU 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応
研究開発用 台車型ロボット キット

0

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