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側の設定

端末を立ち上げて、

.terminal

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

を実行してください。
端末を切ると、設定がリセットされるのでご注意を。
毎回設定するのが面倒な場合は.bashrcに書いておいてください。

Raspberry Pi側の設定

端末を立ち上げて、

.terminal

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

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