注目キーワード
  1. Photoshop
  2. Python
  3. Raspberry Pi
  4. Arduino

ラズパイとBMX055で加速度・地磁気からオイラー角を算出・検証してみた

ここでは「Raspberry Pi:ラズパイ」と「BMX055」を使って加速度からオイラー角を算出する方法を紹介します。

 

この記事を読むことで

加速度からRoll、Pitch角を取得する方法

地磁気からYaw角を取得する方法

BMX055で取得した角度の精度

を知ることができます。

ラズパイとBMX055を使って、加速度からRoll、Pitch角を取得したい方や、地磁気からYaw角を取得したい方は是非ご覧ください。

 

またこの記事は下記の続きとなっていますので、BMX055の基礎的な使い方やセンサ値の取得プログラム(def関数部分)は割愛させて頂きます。

BMX055でセンサ値を取るプログラムを知りたい方などは、先にこちらをご覧ください。

ラズパイでBMX055の使い方と地磁気などのセンサ値を取得してみた

 

では早速、BMX055で取得した加速度・地磁気からRoll、Pitch、Yaw角を取得していきたいと思います。

 

加速度からRoll、Pitch角を取得する

加速度からRoll、Pitch角を取得する理論式

まずは加速度の値からオイラー角のRoll、Pitch角の値を取得する理論式を調べました。

X軸、Y軸、Z軸の加速度をそれぞれAx、Ay、Azとし、この各軸に対しての回転角をΦ、θ、ψ(roll、pitch、yaw)とすると、roll、pitch角は以下で表すことができます。

bmx055-euler-02

引用:特集③:人間工学のための計測手法.pdf

 

この式を使ってオイラー角を取得できそうです。

また、加速度からオイラー角を求める方法を調べていたら、このような記事も見つけたので共有しておきます。

2.2 Accelerometer
When the IMU is at rest, the accelerometer measures gravitational acceleration only. Therefore, by resolving forces using basic trigonometry, we can calculate an estimate of both pitch and roll using the raw data as follows:

bmx055-euler-03

引用:Philip Salmony:IMU Attitude Estimation

 

何が違うかというと、Φ(roll)角の分母がAxとAzのノルムになっています。

X軸の加速度を加えることで、より精度の高いΦ(roll)角が算出できるみたいですね。

 

この2つのオイラー角算出方法を試した結果

前者:roll、pitch角のどちらかが80°を超えると、両方とも80°以上となる

後者:roll、pitch角が双方に影響を与えない

という現象が起こったので、後者の理論式を元にプログラムを作成することにしました。

理由は不明です(-ω-)/

ですが、80°以内であれば角度誤差はほぼありませんでした。

どちらの理論式を使うかは本人次第でしょう。

 

加速度からRoll、Pitch角を取得するプログラム

では早速上記の理論式を元にPythonプログラムを作成し、ラズパイでBMX055を動かしてみます。

Roll、Pitch角を求めるとき

静止時のRoll角、Pitch角0.0°を基準として回転角を計算する

と誤差なく角度を算出できるので、初期誤差算出用の関数も作成しました。

 

関数と言っても簡単なもので、プログラム実行時にBMX055が静止している状態を地面と水平と考えます。

プログラムが実行されたら一度だけセンサ値を読みとり、その値を取得したセンサ値から減算することで静止時の角度を0としています。

例えば

初期のRoll、Pitch角の値 : 1.23、0.56

のとき

初期値誤差 = 初期のRoll、Pitch角の値

として、この初期値誤差をこれ以降取得するRoll、Pitch角の値から減算すれば(すなわち↓)

誤差補正後のRoll、Pitch角の値 = 算出したRoll、Pitch角 - 初期値誤差

初期値誤差を無くすことができるということです。

言葉よりもプログラムを見た方が分かりやすいかもしれません。(>_<)

 

また前回のBMX055の使い方同様、CSV出力でRoll、Pitch角の値を書き出せるようになっています。

 

bmx055_euler_test.py

# -*- coding: utf-8 -*-
from smbus import SMBus
import time
import math
import datetime
import csv

# ・・・
#  センサデータ取得関数は省略
# ・・・

def initial_error(acc):    # オイラー角の初期値誤差を算出
    roll = math.degrees(math.atan2(acc[1] , math.sqrt(acc[0]**2+acc[2]**2)))
    pitch = -math.degrees(math.atan2(acc[0] , math.sqrt(acc[1]**2+acc[2]**2)))

    return -roll, -pitch

if __name__ == "__main__":
    bmx_setup()
    time.sleep(0.1)
    acc = acc_value()    #センサの初期値を取得
    roll_error, pitch_error = initial_error(acc)    # 初期値を誤差へ

    now_time = datetime.datetime.now()
    filename = 'test_' + now_time.strftime('%Y%m%d_%H%M%S') + '.csv'
    # ファイル,1行目(カラム)の作成
    with open(filename, 'a') as f:
        writer = csv.writer(f)
        writer.writerow(['Roll', 'Pitch'])

    while True:
        acc = acc_value()
        gyro= gyro_value()
        mag = mag_value()

        roll = math.atan2(acc[1] , math.sqrt(acc[0]**2+acc[2]**2))
        pitch = -math.atan2(acc[0] , math.sqrt(acc[1]**2+acc[2]**2))

        roll = math.degrees(roll) + roll_error
        pitch = math.degrees(pitch) + pitch_error

        print("roll:{}".format(round(roll,2)))
        print("pitch:{}".format(round(pitch,2)))
        print("\n")
        time.sleep(0.1)

        with open(filename, 'a', newline="") as f:
            writer = csv.writer(f)
            writer.writerow([roll, pitch])

 

プログラムをコピペしたらラズパイ側でプログラムを実行し、Roll、Pitch角が取得できているかを確認します。

Roll角の動作確認

まずはRoll角の動作を確認します。

Pitch角回りの方向を固定し、Roll角回りに動かして動作確認を行いました。

bmx055-euler-04

 

Pitch角は-1.4~-1.2と変化せず、Roll角が-48~-53と変化していることが確認できます。

 

Pitch角の動作確認

続いて、Pitch角の動作を確認します。

Roll角回りの方向を固定し、Pitch角回りに動かして動作確認を行いました。

bmx055-euler-05

 

Roll角は-1.1~-0.9と変化せず、Pitch角が38~45と変化していることが確認できます。

 

BMX055を使ってRoll、Pitch角の精度を検証する

上記ではTeraTerm上で確認を行いましたが、これをCSVファイルへ出力&グラフ化して精度を検証します。

 

Roll角の精度検証

まずはRoll角を

0° → -90° → 90° → 0° → ランダムでRoll角±方向

と動かしたときの精度を検証しました。

bmx055-euler-06

-90°→90°では多少の誤差はあるものの、しっかりと角度を計算できていることが分かります。

一方、ランダムでRoll角±方向動かしたとき

値が飛び飛びになりやすい

ので、センサフィードバックなどで姿勢制御しようと思うと大変かもしれません。

 

Pitch角の精度検証

続いてPitch角を

0° → -90° → 90° → 0° → ランダム

と動かしたときの精度を検証しました。

bmx055-euler-07

こちらもRoll角同様、動作に合った角度を計算できていることが確認できました。

ランダムで動かした場合も、許容範囲内だと思います。

 

今回取得したRoll角、Pitch角の精度をさらに向上させたい場合は、相補フィルタ、カルマンフィルタ、Madgwickフィルタ、四元数を使うと良いかと思います。

 

地磁気からオイラー角を取得する

地磁気からYaw角を取得する理論式

加速度からRoll角、Pitch角の算出ができたので、地磁気からYaw角の算出も行います。

Yaw角を取得する理論式は、加速度から算出したRoll角、Pitch角(radians)とX軸、Y軸、Z軸の地磁気をそれぞれMx、My、Mzとして

bmx055-yaw-angle0

と表すことができます。

 

式の詳しい導出は「参考にしたサイト」よりご覧ください。

 

地磁気からYaw角を取得するプログラム

では早速上記の理論式を元にPythonプログラムを作成し、ラズパイでBMX055を動かしてみます。

加速度からRoll角、Pitch角を取得したプログラムの中から、

CSV出力部分

Whileループ

の部分を書き換えています。

 

bmx055_euler_test.py

# .....省略.....
  
  with open(filename, 'a') as f:
        writer = csv.writer(f)
        writer.writerow(['roll', 'pitch', 'yaw'])

    while True:
        acc = acc_value()
        gyro= gyro_value()
        mag = mag_value()

        roll = math.atan2(acc[1] , math.sqrt(acc[0]**2+acc[2]**2))
        pitch = -math.atan2(acc[0] , math.sqrt(acc[1]**2+acc[2]**2))
        numerator = math.cos(roll)*mag[1] - math.sin(roll)*mag[2]
        denominator = math.cos(pitch)*mag[0] + math.sin(pitch)*math.sin(roll)*mag[1] + math.sin(pitch)*math.cos(roll)*mag[2]
        yaw = math.atan2(numerator,denominator)

        roll = math.degrees(roll) + roll_error
        pitch = math.degrees(pitch) + pitch_error
        yaw = math.degrees(yaw)

        print("roll:{}".format(round(roll,2)))
        print("pitch:{}".format(round(pitch,2)))
        print("yaw:{}".format(round(yaw,2)))
        print("\n")
        time.sleep(0.1)

        with open(filename, 'a', newline="") as f:
            writer = csv.writer(f)
            writer.writerow([roll, pitch, yaw])

 

該当箇所のプログラムをコピペしたらラズパイ側でプログラムを実行し、Yaw角が取得できているかを確認します。

 

Yaw角の動作確認

では早速、Pitch角の動作を確認します。

Roll角、Pitch角回りの方向を固定し、Yaw角回りのみ動かして動作確認を行いました。

bmx055-yaw-angle1

Roll角、Pitch角は値がほとんど変化せず、Yaw角が-72°~-80°と変化していることが確認できました。

 

この計算したYaw角ですが

誤差が大きい時がある

基準点0.0°が分かりにくい

といったこともあるので、使用するときは先ほども述べたフィルタによる誤差補正を行う必要がありそうです。

 

BMX055を使ってYaw角の精度を検証する

では最後にYaw角の精度を検証してみましょう。

 

Yaw角の精度検証

Yaw角を

時計回りに回転

させたときの精度を検証しました。

bmx055-yaw-angle2

Yaw角の誤差は多少あるものの、概ねYaw角の値を取得できていることが確認できます。

上図のグラフでデータが急に飛ぶのは(時間t=15付近)、Yaw角の取得できる範囲を超えたためだと考えられます。

なのでバグではなく、正常に動作していると分かりますね。

 

追記

また、Yaw角の値はRoll、Pitch角を変化させると、Yaw軸方向に回転させていなくても大きくなってしまいます。

原因は分かっており、Yaw角算出時の式にRoll、Pitch角の値を使っているからです。

これでは、せっかくYaw角のデータを取得したのに、意味がほとんどなくなってしまいます。

なので、Roll、Pitch角の値が5°以上を傾いたら、Yaw角の値を欠損値(Nan)として扱っても良いと思います。

Roll、Pitch、Yaw角の計算の最後に

import numpy as np
        
        if abs(roll) > 5 or abs(pitch) > 5:
            yaw = np.nan        # yaw = 0.0 でも可

        print("roll........")
        print("pitch........")
        print("yaw........")

と追記すれば、データを処理するときに困らないと思います。

 

以上、ラズパイとBMX055を使って加速度・地磁気からオイラー角を取得してみました。

基本的な姿勢角を取得できることが分かったので、これでロボットなどに搭載することができますね!

 

まだ多少の誤差などがあるので、フィルタで補正するのが今後の課題です。

BMX055を使って姿勢角を取得する際に役に立てれば幸いです。

お疲れ様でした。

 

さらに、姿勢角取得の精度向上のため相補フィルタでの補正も記事にしました。

GitHubでも公開しているので、是非下記もご覧ください!

BMX055から取得したセンサ値を相補フィルタで補正&Pythonで実装してみた

 

参考にした他のサイト

Watako-Lab:3軸加速度センサを用いた姿勢推定

 

Watako-Lab:3軸方位センサを用いた姿勢推定

 

[Kaze’s test]プログラミングメモ:加速度センサー、地磁気センサーから姿勢算出

bmx055-acc-mag-topthumbnail
学びに関する情報をチェック!