まなびサイエンス

3 軸加速度・角速度センサ モジュールを使う

カバーイメージ

 I2C 接続の 3 軸加速度・角速度センサ モジュールを入手できたので、RaspberryPi に接続して実験してみました。

結線

 センサの核心部分は MPU6050 というチップで、GY-521 という刻印のある製品がメジャーなようです。X-Y-Z 3 軸方向の加速度と、3 軸まわりの回転速度、それと温度を計測できます。
 RaspberryPi とは I2C で接続でき、I2C で必要な 3 本とは別に電源が必要で、計 4 本で接続します。基盤上に 3.3V のレギュレータが載っており、5V 電源で動作しますが、3.3V でも動くみたいです*1

  • センサ: VCC ⇔ RasPi: 3V3 または 5V
  • センサ: SCL ⇔ RasPi: GPIO3 (SCL)
  • センサ: SDA ⇔ RasPi: GPIO2 (SDA)
  • センサ: GND ⇔ RasPi: GND
SODIAL(R) GY-521 MPU-60503軸ジャイロ+3軸加速度センサ
SODIAL(R)
売上ランキング 17,992
Personal Computers
¥ 290

初期設定とデータの試し読み

 まずは、シェルからポチポチとコマンドを叩いて実験してみます。念のため、MPU6050 のデータシートを見ながら進めますが、このチップのデータシートがまた非常に読みにくいbearing.gif

### バスに接続されたスレーブを一覧して、センサのアドレスを調べます
$ i2cdetect -y 1 ⇒ センサのアドレスは 0x68
...

### レジスタの値を読み書きしてみます
$ i2cget -y 1 0x68 0x1b
0x00 ← Gyroscope Configuration を読む。ジャイロ センサの感度は ±250deg/s
$ i2cget -y 1 0x68 0x1c
0x00 ← Accelerometer Configuration を読む。加速度センサの感度は ±2G
$ i2cget -y 1 0x68 0x6b
0x40 ← Power Management 1 を読む。デバイスは sleep 状態
$ i2cset -y 1 0x68 0x6b 0x00 ← sleep を解除して計測開始

### 計測値を読出します
$ i2cget -y 1 0x68 0x3b w ← X 軸方向の加速度
0x78fc ← 上位バイト(MSB)と下位バイト(LSB)が入れ替わっているので注意

 センサをあっちこっちに傾けながら i2cget コマンドを叩くと、取得でき値が変化していることが判ります。

データの読出し

 とりあえず、センサを正しく結線できて、何等かのデータを読み出せているようなので、連続してデータを取得できるよう python スクリプトを書いてみます。

#!/usr/bin/python
# -*- coding: utf-8 -*-
import smbus            # use I2C
import math
from time import sleep  # time module

### define #############################################################
DEV_ADDR = 0x68         # device address
PWR_MGMT_1 = 0x6b       # Power Management 1
ACCEL_XOUT = 0x3b       # Axel X-axis
ACCEL_YOUT = 0x3d       # Axel Y-axis
ACCEL_ZOUT = 0x3f       # Axel Z-axis
TEMP_OUT = 0x41         # Temperature
GYRO_XOUT = 0x43        # Gyro X-axis
GYRO_YOUT = 0x45        # Gyro Y-axis
GYRO_ZOUT = 0x47        # Gyro Z-axis

# 1byte read
def read_byte( addr ):
    return bus.read_byte_data( DEV_ADDR, addr )

# 2byte read
def read_word( addr ):
    high = read_byte( addr   )
    low  = read_byte( addr+1 )
    return (high << 8) + low

# Sensor data read
def read_word_sensor( addr ):
    val = read_word( addr )
    if( val < 0x8000 ):
        return val # positive value
    else:
        return val - 65536 # negative value

# Get Temperature
def get_temp():
    temp = read_word_sensor( TEMP_OUT )
    # offset = -521 @ 35℃
    return ( temp + 521 ) / 340.0 + 35.0

# Get Gyro data (raw value)
def get_gyro_data_lsb():
    x = read_word_sensor( GYRO_XOUT )
    y = read_word_sensor( GYRO_YOUT )
    z = read_word_sensor( GYRO_ZOUT )
    return [ x, y, z ]
# Get Gyro data (deg/s)
def get_gyro_data_deg():
    x,y,z = get_gyro_data_lsb()
    # Sensitivity = 131 LSB/(deg/s), @cf datasheet
    x = x / 131.0
    y = y / 131.0
    z = z / 131.0
    return [ x, y, z ]

# Get Axel data (raw value)
def get_accel_data_lsb():
    x = read_word_sensor( ACCEL_XOUT )
    y = read_word_sensor( ACCEL_YOUT )
    z = read_word_sensor( ACCEL_ZOUT )
    return [ x, y, z ]
# Get Axel data (G)
def get_accel_data_g():
    x,y,z = get_accel_data_lsb()
    # Sensitivity = 16384 LSB/G, @cf datasheet
    x = x / 16384.0
    y = y / 16384.0
    z = z / 16384.0
    return [x, y, z]

### Main function ######################################################
bus = smbus.SMBus( 1 )
bus.write_byte_data( DEV_ADDR, PWR_MGMT_1, 0 )
while 1:
    temp = get_temp()
    print 't= %.2f' % temp, '\t',

    gyro_x,gyro_y,gyro_z = get_gyro_data_deg()
    print 'Gx= %.3f' % gyro_x, '\t',
    print 'Gy= %.3f' % gyro_y, '\t',
    print 'Gz= %.3f' % gyro_z, '\t',

    accel_x,accel_y,accel_z = get_accel_data_g()
    print 'Ax= %.3f' % accel_x, '\t',
    print 'Ay= %.3f' % accel_y, '\t',
    print 'Az= %.3f' % accel_z, '\t',
    print # 改行
    sleep( 1 )

メモ

  • 40 行目 いきなり「+36.53」と書かれているソースコードは、仕様書にある「31 ℃において -521 のオフセット」というところから式変形すると出てくるっぽい。

 センサ モジュールをだいたい水平なテーブル上に静置した状態で、上記のプログラムを利用して、加速度センサとジャイロ センサの計測値 60 点分をグラフにしてみました(Fig.1 , Fig.2 )。


Fig.1


Fig.2

 加速度センサの計測値(A*)を見ると、おおよそ安定しており、計測値をそのまま姿勢制御に使うことができそうです。Y 軸(赤)が -0.2 程度と少し大き目の値を示しているのは、テーブルが微妙に傾いていることに加えて、ブレッドボードに刺したセンサ自体が傾いていたからです。sin-1(-0.2) = 11.5°と計算できますが、まぁ、だいたいそれくらは傾いていた感じ。また、Z 軸(緑)は、地面に鉛直方向なので、地球重力の 1G を概ね計測できているようです。
 一方、センサを静置していたにも関わらず、ジャイロ センサの計測値(G*)には大きな変動がみられます。また、オフセットも大きく、計測値を使うには骨が折れそうです。実際、高価なジャイロ センサにおいても、このような計測値の変動があります。得られた計測値について、カルマン フィルタなどを利用してやれば、それなりの値を求められると思います*2

 次に、センサの Z 軸を水平に保ち、センサを Z 軸周りに回転させ、X 軸および Y 軸の計測値を X-Y プロットしてみました(Fig.3 )。微妙に軸がずれている気もしますが*3、概ね半径 1G の円を描けており、加速度センサとしては、無調整でもそのまま使えそうな感じです。


Fig.3

メモ

  • AD0 端子は基盤上で GND にプルダウンされているので未接続でも問題ありません。これを H レベルに接続すると、デバイス アドレスを 0x69 に変更できます。2 台目のセンサを同時使用したい場合には使えるかも。
  • 温度センサは 340 LSB/℃ の分解能があるものの、チップ自体の発熱や熱雑音などの影響もあるはずで、どこまで信頼できるかは要調査。

アイディア

  • 教科書的に、ロボットなどの移動体に搭載して、姿勢の決定に使う。
  • 手袋やスティックに埋め込んでジェスチャを与えて機器を操作する。

参考リンク

  1. *1 レギュレータで電圧が更に下がりますが、チップの動作電圧下限までは下がらないのかも。
  2. *2 20 年近く昔。学生時代にカルマン フィルタの勉強をしようと思って、最初の数ページで挫折した思い出が…
  3. *3 もし、壁の垂直がきっちり出ていない場合でも、半径が小さくなるだけで、楕円の軸に偏角が出ることはないハズなんだけどなぁ?

カテゴリー

過去ログ