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を買いました.
勉強資料
今回,ついでに姿勢の勉強もしました.
以下はスタンフォードの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は以下のライブラリを用いてコードを書きました. スケッチ例も参考にしています.
Arduino-PC間の通信にはrosserialを用いました.
謎遮断するし、あんま評判良くないんですけどね!
以下書いた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
ノードは以下のようになります.
比較
imu_filter_madgwickはinputに6軸(角速度と加速度)か9軸(6軸+地磁気)を選べます.
折角なので比較してみました.
6軸のみの場合,誤差が累積していっています.安いIMUだとこんなもんなんですかね?
もう少し調査したいです.
追記:加速度からでは絶対的な方位角を推定できないので,こんなもんですかね.
角速度のキャリブレーションを精密にやればもうちょいよくなるのかな?
9軸では,ブルブルはしてますがドリフトはないです. 積分して相対的に計測していくのって大変だなぁと感じました.
参考にしたもの
- IMUの姿勢計算に便利なmadgwickフィルタ - メカトロニクスにうってつけの日
- Madgwick Filterを読んでみた - Qiita
- ロボットのための慣性計測装置(IMU)入門 - MyEnigma