MPU6050を用いた姿勢推定＆Rvizで可視化

はじめに

最近、研究室に転がっていたRaspberry Piを使って遊んでいます。

今回は、ワンコインで買える6軸センサであるMPU6050を使って姿勢の推定を行い、その結果をRvizで可視化できるようにしました。

MPU6050

実行環境

• Raspberry Pi 3 Model B
• MPU6050
• ROS : Kinetic
• Python : 2.7
• Host PC : Ubuntu16.04

Raspberry Pi側の設定

Raspberry PiにROSをインストールする方法は、こちらの記事を参考にさせていただきました。

mpu6050との接続

Raspberry Piの各ピンをMPU6050の以下の部分に繋げてください。

- 1番ピン → VCC
- 3番ピン → SDA
- 5番ピン → SCL
- 6番ピン → GND

きちんと接続できれば、LEDが点灯します。

ROSのプログラム

MPU6050から得られたx, y, z方向の角速度の値から、オイラー角を計算し、

その値をTFにブロードキャストします。

mpu6050.py

#! /usr/bin/env python
# coding:utf-8
import smbus
import math
import time
import rospy, tf
from sensor_msgs.msg import *
from geometry_msgs.msg import *

#アドレスの設定
DEV_ADDR = 0x68
ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c

bus = smbus.SMBus(1)
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)

def read_word(adr):
    high = bus.read_byte_data(DEV_ADDR, adr)
    low = bus.read_byte_data(DEV_ADDR, adr+1)
    val = (high << 8) + low
    return val

def read_word_sensor(adr):
    val = read_word(adr)
    if (val >= 0x8000):#minus
        return -((65535 - val) + 1)
    else:#plus
        return val

def getAccel():
    x = read_word_sensor(ACCEL_XOUT)/16384.0
    y = read_word_sensor(ACCEL_YOUT)/16384.0
    z = read_word_sensor(ACCEL_ZOUT)/16384.0
    return [x, y, z]

def calcEuler(x,y,z):
    theta = math.atan( x / math.sqrt(y*y + z*z) )
    psi = math.atan( y / math.sqrt(x*x + z*z) )
    phi = math.atan( math.sqrt( x*x + y*y ) / z)
    return [theta, psi, phi]

if __name__ == "__main__":
    rospy.init_node("mpu6050")
    br = tf.TransformBroadcaster()

    while True:
        try:
            ax, ay, az = getAccel()

            roll, pitch, yaw = calcEuler(ax,ay,az)

            br.sendTransform((0,0,0), tf.transformations.quaternion_from_euler(roll,pitch,yaw), rospy.Time.now(), "mpu6050", "map")

            #print "Roll:[%s]"%str(roll)
            #print "Pitch:[%s]"%str(pitch)
            #print "Yaw:[%s]"%str(yaw)

        except KeyboardInterrupt:
            print "Ctrl + c"
            break

        except Exception as e:
            print str(e)
            #break

適当なパッケージを作って、上記のファイルをコピペするか、
Githubから直接パッケージをcloneしてください。

ROSの分散処理の設定

Raspberry PiだとRvizで実行結果を見ることができません。

そこで、別のPCをマスターとしてroscoreを実行し、RvizもそのPCで起動します。
ROSの分散処理については、こちらの記事が参考になりました。

Raspberry Pi と Host PC 共通の設定

LANケーブルで接続し、pingやifconfigコマンドを使って、接続の確認とIPアドレスの確認をしてください。

Host PC側の設定

端末を立ち上げて、

export ROS_IP=XXX.YYY.ZZZ.WWW  #"XXX.YYY.ZZZ.WWW"はHost PCのIPアドレス
roscore

を実行してください。
端末を切ると、設定がリセットされるのでご注意を。

毎回設定するのが面倒な場合は.bashrcに書いておいてください。

Raspberry Pi側の設定

端末を立ち上げて、

export ROS_MASTER_URI=http://XXX.YYY.ZZZ.WWW:11311  #"XXX.YYY.ZZZ.WWW"はHost PCのIPアドレス
export ROS_IP=aaa.bbb.ccc.ddd  #"aaa.bbb.ccc.ddd"はRaspberry PiのIPアドレス

を実行してください。
端末を切ると、設定がリセットされるのでご注意を。

毎回設定するのが面倒な場合は.bashrcに書いておいてください。

実行方法

• Host PC側

  rviz

別の端末で、

  rosrun tf static_transform_publisher 0 0 0 0 0 0 map frame 100

• Raspberry Pi側

  rosrun {package_name} mpu6050.py

Rvizが初期状態だと、何も表示されないと思います。
Addを押して、By display typeの中からTFを選んでください。

実行結果

Twitter

うまく姿勢の推定ができてるっぽいですが、誤差や振動が大きいです。
カルマンフィルタ等を使って補正する必要があります。