ssk tech blog

自律移動ロボットの話を書きます

Arduinoを用いてIMUで姿勢推定 on ROS

初めに

こんにちは.ササキ(@saitosasaki)です.
ROSにおいてIMUから姿勢推定できるようにしました. IMUの値を読み取るためにArduinoを用いました.
以下、姿勢を取得するまでの道のりです.

IMU選定 -InvenSense社のIMU-

安いimuといったらInvenSense社のイメージでした.1000円くらいで買えます.
以下,InvenSense社のIMUのまとめです.

品番 説明
mpu6000,6500 もう生産してない.6軸(角速度+加速度)
mpu9150 9軸(6軸+地磁気
mpu9250 9150の改良品で小型化し、磁気センサの分解能も高くなった.
icm20602 9250より性能が良いらしい.6軸.マイクロマウス界での最近の流行り.

今回は9軸取れ,amazonで売っているMPU9250を買いました.

f:id:ssk0109:20181215151714j:plain
MPU9250

勉強資料

今回,ついでに姿勢の勉強もしました.
以下はスタンフォードのIMUの資料です.
オイラー角やクォータニオンのことが非常にわかりやすくまとまっています.

https://stanford.edu/class/ee267/lectures/lecture9.pdf
https://stanford.edu/class/ee267/lectures/lecture10.pdf

すること

Arduino周りは下を参考にしてやっていきます.

ArduinoでMPU9250(加速度センサ、磁気センサ)を使う方法 : 試行錯誤な日々

ちなみに上に合わせてArduino買ってROSのコード入れたら,RAMが足りなくて一回詰みました. ArduinoでROSを使うときはnodehandleの宣言だけで1400bもってかれるので,
2kbしかないnano等ではコンパイルが通っても正常に動きません.Arduinoを選ぶ時は要注意です.

ArduinoからIMUデータをpublishする

Arduinoは以下のライブラリを用いてコードを書きました. スケッチ例も参考にしています.

github.com

Arduino-PC間の通信にはrosserialを用いました.
謎遮断するし、あんま評判良くないんですけどね!

github.com

以下書いたArduinoソースコードです.

クリックすると展開されます

ReadIMU.ico
#include <MPU9250_asukiaaa.h>

#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 26
#define SCL_PIN 25
#endif

MPU9250 mySensor;

#include <ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>

ros::NodeHandle nh;

sensor_msgs::Imu imu;
sensor_msgs::MagneticField mag;
ros::Publisher pubimu("imu/data_raw", &imu);
ros::Publisher pubmag("imu/mag", &mag);

void setup() {
  while(!Serial);
  Serial.begin(115200);
  Serial.println("started");

#ifdef _ESP32_HAL_I2C_H_ // For ESP32
  Wire.begin(SDA_PIN, SCL_PIN); // SDA, SCL
#else
  Wire.begin();
#endif

  mySensor.setWire(&Wire);
  mySensor.beginAccel();
  mySensor.beginGyro();
  mySensor.beginMag();

  // You can set your own offset for mag values
  // mySensor.magXOffset = -50;
  // mySensor.magYOffset = -55;
  // mySensor.magZOffset = -10;

  sensorId = mySensor.readId();
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(pubimu);
  nh.advertise(pubmag);
}
float gXOffset= 2.50, gYOffset=-1.40, gZOffset= 1.47;

void loop() {
  Serial.println("sensorId: " + String(sensorId));
  float aX, aY, aZ, aSqrt, gX, gY, gZ, mDirection, mX, mY, mZ;
  mySensor.accelUpdate();
  aX = mySensor.accelX() ;
  aY = mySensor.accelY() ;
  aZ = mySensor.accelZ() ;
  //aSqrt = mySensor.accelSqrt();
  Serial.println("accelX: " + String(aX));
  Serial.println("accelY: " + String(aY));
  Serial.println("accelZ: " + String(aZ));
  //Serial.println("accelSqrt: " + String(aSqrt));

  mySensor.gyroUpdate();
  gX = mySensor.gyroX()-gXOffset;
  gY = mySensor.gyroY()-gYOffset;
  gZ = mySensor.gyroZ()-gZOffset;
  Serial.println("gyroX: " + String(gX));
  Serial.println("gyroY: " + String(gY));
  Serial.println("gyroZ: " + String(gZ));
 
  mySensor.magUpdate();
  mX = mySensor.magX();
  mY = mySensor.magY();
  mZ = mySensor.magZ();
  mDirection = mySensor.magHorizDirection();
  Serial.println("magX: " + String(mX));
  Serial.println("maxY: " + String(mY));
  Serial.println("magZ: " + String(mZ));
  Serial.println("horizontal direction: " + String(mDirection));
  
  Serial.println("at " + String(millis()) + "ms");
  Serial.println(""); // Add an empty line
  
  imu.header.frame_id = "imu_link";
  imu.header.stamp = nh.now();
  imu.angular_velocity.x = gX;
  imu.angular_velocity.y = gY;
  imu.angular_velocity.z = gZ; // [rad/sec]
  imu.linear_acceleration.x = aX;      
  imu.linear_acceleration.y = aY;  
  imu.linear_acceleration.z = aZ; 
  pubimu.publish(&imu);
  
  mag.header.frame_id = "imu_link";
  mag.header.stamp = nh.now();
  mag.magnetic_field.x = mX;
  mag.magnetic_field.y = mY;
  mag.magnetic_field.z = mZ; // [μT]
  pubmag.publish(&mag);

  nh.spinOnce();
  delay(1);
}

後はPC側でrosserialを起動すればデータをPCに送れます

rosrun rosserial_python serial_node.py /dev/ttyACM0 _baud:=115200

バイスのpathは/dev/ttyUSB0になる場合もあります.
ちなみに,ここで例えばSRAMいっぱいに書き込んでrosserialを起動すると

software version mismatch such as hydro rosserial_python with groovy Arduino

とエラーが出るんですがバージョンは全く関係ないです. 他にも別の原因なのにこのエラーは良く出ます.

姿勢推定

姿勢推定にはimu_toolsのimu_filter_madgwickを使いました.
github.com Madgwick Filter(マッジウィック・フィルターと読むそう)は有名なKalman Filterと比べて,モデルが不必要で,高速(数百から数千Hzで回せるっぽいです!)なのに,同程度以上の精度のフィルターだそうです.
次コマンドで実行します.

rosrun imu_filter_madgwick imu_filter_node

ノードは以下のようになります. f:id:ssk0109:20181213000434p:plain

比較

imu_filter_madgwickはinputに6軸(角速度と加速度)か9軸(6軸+地磁気)を選べます.
折角なので比較してみました.
6軸のみの場合,誤差が累積していっています.安いIMUだとこんなもんなんですかね?
もう少し調査したいです.

追記:加速度からでは絶対的な方位角を推定できないので,こんなもんですかね.
角速度のキャリブレーションを精密にやればもうちょいよくなるのかな? f:id:ssk0109:20181215180801g:plain

9軸では,ブルブルはしてますがドリフトはないです. 積分して相対的に計測していくのって大変だなぁと感じました.

参考にしたもの

コード置き場

github.com