ここでは「Raspberry Pi：ラズパイ」と「BMX055」を使って加速度からオイラー角を算出する方法を紹介します。
この記事を読むことで
加速度からRoll、Pitch角を取得する方法
地磁気からYaw角を取得する方法
BMX055で取得した角度の精度
を知ることができます。
ラズパイとBMX055を使って、加速度からRoll、Pitch角を取得したい方や、地磁気からYaw角を取得したい方は是非ご覧ください。
またこの記事は下記の続きとなっていますので、BMX055の基礎的な使い方やセンサ値の取得プログラム（def関数部分）は割愛させて頂きます。
BMX055でセンサ値を取るプログラムを知りたい方などは、先にこちらをご覧ください。
では早速、BMX055で取得した加速度・地磁気からRoll、Pitch、Yaw角を取得していきたいと思います。
加速度からRoll、Pitch角を取得する
加速度からRoll、Pitch角を取得する理論式
まずは加速度の値からオイラー角のRoll、Pitch角の値を取得する理論式を調べました。
X軸、Y軸、Z軸の加速度をそれぞれAx、Ay、Azとし、この各軸に対しての回転角をΦ、θ、ψ（roll、pitch、yaw）とすると、roll、pitch角は以下で表すことができます。
この式を使ってオイラー角を取得できそうです。
また、加速度からオイラー角を求める方法を調べていたら、このような記事も見つけたので共有しておきます。
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:
引用：Philip Salmony：IMU Attitude Estimation
何が違うかというと、Φ（roll）角の分母がAxとAzのノルムになっています。
X軸の加速度を加えることで、より精度の高いΦ（roll）角が算出できるみたいですね。
この2つのオイラー角算出方法を試した結果
前者：roll、pitch角のどちらかが80°を超えると、両方とも80°以上となる
後者：roll、pitch角が双方に影響を与えない
という現象が起こったので、後者の理論式を元にプログラムを作成することにしました。
理由は不明です(-ω-)/
ですが、80°以内であれば角度誤差はほぼありませんでした。
どちらの理論式を使うかは本人次第でしょう。
加速度からRoll、Pitch角を取得するプログラム
では早速上記の理論式を元にPythonプログラムを作成し、ラズパイでBMX055を動かしてみます。
Roll、Pitch角を求めるとき
と誤差なく角度を算出できるので、初期誤差算出用の関数も作成しました。
関数と言っても簡単なもので、プログラム実行時にBMX055が静止している状態を地面と水平と考えます。
プログラムが実行されたら一度だけセンサ値を読みとり、その値を取得したセンサ値から減算することで静止時の角度を0としています。
例えば
のとき
として、この初期値誤差をこれ以降取得する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角回りに動かして動作確認を行いました。
Pitch角は－1.4～－1.2と変化せず、Roll角が－48～－53と変化していることが確認できます。
Pitch角の動作確認
続いて、Pitch角の動作を確認します。
Roll角回りの方向を固定し、Pitch角回りに動かして動作確認を行いました。
Roll角は－1.1～－0.9と変化せず、Pitch角が38～45と変化していることが確認できます。
BMX055を使ってRoll、Pitch角の精度を検証する
上記ではTeraTerm上で確認を行いましたが、これをCSVファイルへ出力＆グラフ化して精度を検証します。
Roll角の精度検証
まずはRoll角を
と動かしたときの精度を検証しました。
－90°→90°では多少の誤差はあるものの、しっかりと角度を計算できていることが分かります。
一方、ランダムでRoll角±方向動かしたとき
ので、センサフィードバックなどで姿勢制御しようと思うと大変かもしれません。
Pitch角の精度検証
続いてPitch角を
と動かしたときの精度を検証しました。
こちらもRoll角同様、動作に合った角度を計算できていることが確認できました。
ランダムで動かした場合も、許容範囲内だと思います。
今回取得したRoll角、Pitch角の精度をさらに向上させたい場合は、相補フィルタ、カルマンフィルタ、Madgwickフィルタ、四元数を使うと良いかと思います。
地磁気からオイラー角を取得する
地磁気からYaw角を取得する理論式
加速度からRoll角、Pitch角の算出ができたので、地磁気からYaw角の算出も行います。
Yaw角を取得する理論式は、加速度から算出したRoll角、Pitch角（radians)とX軸、Y軸、Z軸の地磁気をそれぞれMx、My、Mzとして
と表すことができます。
式の詳しい導出は「参考にしたサイト」よりご覧ください。
地磁気から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角回りのみ動かして動作確認を行いました。
Roll角、Pitch角は値がほとんど変化せず、Yaw角が－72°～－80°と変化していることが確認できました。
この計算したYaw角ですが
誤差が大きい時がある
基準点0.0°が分かりにくい
といったこともあるので、使用するときは先ほども述べたフィルタによる誤差補正を行う必要がありそうです。
BMX055を使ってYaw角の精度を検証する
では最後にYaw角の精度を検証してみましょう。
Yaw角の精度検証
Yaw角を
させたときの精度を検証しました。
Yaw角の誤差は多少あるものの、概ねYaw角の値を取得できていることが確認できます。
上図のグラフでデータが急に飛ぶのは（時間ｔ=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でも公開しているので、是非下記もご覧ください！
参考にした他のサイト