563 views

この記事は最終更新から 385日 が経過しています。

【1】やりたいこと

自作したラズパイ作品が 地面に垂直？ を判定したい。

これには加速度センサーを使う。
今回は MPU-6050 なる I2C接続可能なデバイスを使うことにした。

実際には、このデバイス MPU-6050 を載せたモジュールを購入した。

Amazonで 3個 990円 の物を購入した。
https://amzn.to/3O0o9ou

【2】加速度センサーで傾きがわかるの？

加速度センサーとは、
X軸、Y軸、Z軸の 3軸それぞれの軸と並行方向にどれだけの加速度で移動したか？
を測定するためのセンサーだ。

ここで疑問が湧くかも…
3軸上の加速度を測定することで、なぜ傾きを検出できるのか？

その理由は…
物体を移動しなくても、常に Z軸方向に重力加速度 9.8[m/s²] がかかっているから。

Case 1 : 物体が地面に対して垂直の場合（傾き角＝0°）

後日追記

Case 2 : 物体が地面に対して垂直ではない場合（傾き角≠0°）

後日追記

【2】MPU-6050の情報収集

データシート (tdk.com)

レジスタマップ (tdk.com)

【3】初期化

ラズベリーパイに加速度センサー MPU-6050 を接続して使う。

電源投入後、まずは、データシートの記述に従って、デバイスを初期化する。
制御対象は以下の5つのレジスタ。

0x6B	電源管理レジスタ
0x1C	加速度設定レジスタ

初期設定(1) 電源管理レジスタ

後日追記

初期設定(2) 加速度設定レジスタ

後日追記

【4】傾き角度の測定

後日追記

【5】プログラム作成＆実行

(1) センサーの実装面を上向きに配置した場合

センサーモジュール基板をこの向き（↓）に配置した場合、以下のプログラムで垂直方向の検知が可能である。

ChatGPT大先生にご助力をいただき、プログラムの各行に適切なコメントを付けていただいた。本当に素晴らしい！

import smbus  # I2C通信のためのライブラリをインポート
import time   # 遅延処理のためのライブラリをインポート
import math   # 数学関数（角度計算用）のためのライブラリをインポート

# MPU-6050のI2Cアドレスとレジスタの定義
MPU6050_ADDR = 0x68      # MPU-6050のI2Cアドレス
PWR_MGMT_1   = 0x6B      # 電源管理レジスタ
GYRO_CONFIG  = 0x1B      # ジャイロ設定レジスタ
ACCEL_CONFIG = 0x1C      # 加速度設定レジスタ
ACCEL_XOUT_H = 0x3B      # 加速度X軸データ（高位）レジスタ

# I2Cバス1を使用してMPU-6050に接続
bus = smbus.SMBus(1)

# 電源管理レジスタに0を書き込み、スリープモードを解除
bus.write_byte_data(MPU6050_ADDR, PWR_MGMT_1, 0)
#-------------------------------------------------------------------------------
# MPU-6050は起動時にデフォルトでスリープモードに入ります。
# このモードではセンサーが低消費電力で待機しており、データ取得はできません。
# そのため、センサーからデータを読み取る前に、スリープモードを解除する必要があります。
#-------------------------------------------------------------------------------

# 加速度レンジを最狭(=最高精度)の±2[g]に設定（ACCEL_CONFIGレジスタに0x00を書き込み）
# 1g = 9.8 m/s² : 重力加速度(gravity acceleration の g)
bus.write_byte_data(MPU6050_ADDR, ACCEL_CONFIG, 0x00)
#-------------------------------------------------------------------------------
# ACCEL_CONFIGレジスタに設定する値によって、以下の4つの加速度範囲が選択できます：
#  +--------+----------+---------------+
#  | 設定値 | 測定範囲 | 分解能（感度）|
#  +--------+----------+---------------+
#  | 0x00   |  ±2g    | 16384 LSB/g   | 量子化誤差小
#  | 0x08   |  ±4g    |  8192 LSB/g   |
#  | 0x10   |  ±8g    |  4096 LSB/g   |
#  | 0x18   | ±16g    |  2048 LSB/g   | 量子化誤差大
#  +--------+----------+---------------+
# ここで0x00を設定することで、加速度センサーは±2gの範囲を測定範囲とします。
# ±2gの範囲は、MPU-6050が最も高感度で動作する設定で、地球の重力加速度（1g ≈ 9.8 m/s²）
# を安定して計測するのに適しています。
#
# ■分解能（感度）
# ±2g設定では、1gに相当するデジタル出力は16384 LSBです。この分解能により、
# 加速度データは非常に小さな変化も捉えることができます。たとえば、±2gの設定だと、
# 1 LSB（最小単位）は約0.00006g（9.8 m/s²を16384で割った値）となります。
#
# MPU-6050のセンサーで使われる「g」は、重力加速度を表しており、「重さ」ではなく、
# 加速度の単位として使われています。
# ここでの「g」は、地球の重力加速度（1g ≈ 9.8 m/s²）を基準にしています。
#
# ■センサーが測定しているのは何か？
# MPU-6050の加速度センサーは、物体がどの方向にどれくらいの加速度を受けているかを計測しています。
# ここで「加速度」とは、物体の速度がどれだけ速く変わっているか（加速や減速の度合い）を表しています。
#
# ■センサーのメカニズム
# MPU-6050の加速度センサーは、**MEMS（Micro-Electro-Mechanical Systems）**技術を使って加速度を測定しています。
# 具体的には、センサー内部の小さな構造物が物体の加速度に応じてわずかに変形し、その変位を電気信号に変換することで加速度を検出しています。
# 〇内部構造
# センサーの内部には、非常に小さな「質量」（マイクロサイズの重りのようなもの）と、質量を支えるばね構造が含まれています。
# この質量は、センサーが動くと一緒に動きますが、慣性によってわずかにずれる性質を持ちます。
# さらに、質量の周囲には静電容量センサーが配置されており、質量の位置の変化を検出します。
# 〇加速度による質量の変位：
# センサーが動いたり、加速度を受けたりすると、慣性の働きで内部の質量がわずかに変位します。
# たとえば、センサーを上向きに静止させると、地球の重力により質量がZ軸方向に押され、その影響で少し変位します。
# 〇静電容量の変化：
# 質量が動くと、その周囲にある静電容量センサーの静電容量が変化します。
# 静電容量は質量の位置（距離）に依存するため、この変化を利用して質量の変位量を測定できます。
# 〇変位量から加速度を計算：
# 質量の変位量に基づいて、どれだけの加速度がかかっているかを計算します。
# 質量が受ける変位量は加速度の大きさに比例するため、適切な校正や換算を行うことで、正確な加速度値が得られます。
#-------------------------------------------------------------------------------

# レジスタから2バイトのデータを読み取り、「符号付き16ビット整数値」として返す関数
def read_word(adr):
    high = bus.read_byte_data(MPU6050_ADDR, adr)         # 高位バイトを読み取る
    low  = bus.read_byte_data(MPU6050_ADDR, adr + 1)     # 低位バイトを読み取る
    val = (high << 8) + low                              # 高位バイトと低位バイトを結合して16ビット値にする
    if val >= 0x8000:                                    # 負の値の場合、2の補数で変換
        return -((65535 - val) + 1)
    else:
        return val

# X, Y, Z軸の加速度データを取得してg単位に変換する関数
def read_accel():
    x = read_word(ACCEL_XOUT_H)     / 16384.0            # X軸の加速度データを16384で割ってg単位に変換 (1[g]=16384[LSB/g])
    y = read_word(ACCEL_XOUT_H + 2) / 16384.0            # Y軸の加速度データを変換
    z = read_word(ACCEL_XOUT_H + 4) / 16384.0            # Z軸の加速度データを変換
    return {'x': x, 'y': y, 'z': z}                      # X, Y, Z軸データを辞書形式で返す

# 傾斜角度を計算する関数
# MPU-6050の加速度センサーから得られた各軸の加速度データを使って、
# センサーが地面に対してどの程度傾いているかを角度（度数）で計算
def calculate_tilt_angle( accel_data ):
    # X, Y, Z軸の加速度データを変数に代入
    x, y, z = accel_data['x'], accel_data['y'], accel_data['z']
    # ベクトルの長さを算出
    norm_xyz = math.sqrt(x**2 + y**2 + z**2)
    # Z軸方向の加速度からの角度を計算
    angle_rad = math.acos(z / norm_xyz)
    angle_deg = math.degrees(angle_rad)
    return angle_deg                                       # 計算した角度を返す

# センサーが垂直かどうかを角度の閾値(3度)で判定する関数
def is_vertical(angle, threshold=3):
    return abs(angle) < threshold                          # 角度がthreshold未満なら垂直とみなす

# メインループ：加速度データを取得し、傾斜角を計算して垂直判定を行う
while True:
    accel_data = read_accel()                              # 加速度データを取得
    tilt_angle = calculate_tilt_angle(accel_data)          # 傾斜角度を計算
    
    print("Tilt Angle:", tilt_angle, "degrees")            # 傾斜角度を出力
    if is_vertical(tilt_angle):                            # 垂直判定
        print("センサーは地面に対して垂直です")            # 垂直と判定された場合の出力
    else:
        print("センサーは垂直ではありません")              # 垂直でない場合の出力
    
    time.sleep(1)                                          # 1秒間の遅延

(2) センサーの実装面を下向きに配置した場合

センサーモジュール基板を上記(1)と反対向きに配置した場合、下記の 1行を挿入して Z軸の加速度を反転させればよい。

# X, Y, Z軸の加速度データを取得してg単位に変換する関数
def read_accel():
    x = read_word(ACCEL_XOUT_H)     / 16384.0            # X軸の加速度データを16384で割ってg単位に変換 (1[g]=16384[LSB/g])
    y = read_word(ACCEL_XOUT_H + 2) / 16384.0            # Y軸の加速度データを変換
    z = read_word(ACCEL_XOUT_H + 4) / 16384.0            # Z軸の加速度データを変換
    z = -z
    return {'x': x, 'y': y, 'z': z}                      # X, Y, Z軸データを辞書形式で返す

【6】所感

後日追記

---