pythonでの角速度からの角度算出方法を教えてください。

Question

環境

raspberry pi 3
python 3
MEMS:MPU6050

###やりたいこと
ＭＰＵ６０５０から角度算出を行いたいです。

加速度センサだけだとノイズが多く、角速度だけだとドリフトを起こしてしまうそうなので、両方を使った角度算出を行いたいです。

以下のサイトを参照し、計算を行いました。
角速度と加速度を用いた角度算出方法
MPU6050の角速度、加速度算出

下のリンクでは加速度からの角度算出を行っていますが、私は角速度からの角度算出も考慮して角度算出を行いたいです。

###わからないこと
角速度を使っての角度算出方法がわかりません。
もっと詳しく言うと、角速度にどの時間をかければよいかわかりません。

角度＝角速度×角速度を計測した時間　

というのは分かりますが、その「角速度を計測した時間」がどう求めればよいのかわかりません。

import sys
from PyQt5.QtWidgets import *
from PyQt5.QtGui import *
from PyQt5.QtCore import *
import subprocess
import time
import numpy as np
import cv2
import RPi.GPIO as GPIO

import smbus
import math
from time import sleep



class Tab1Widget(QWidget):

    def __init__(self):
        super().__init__() ##スーパークラスの"QWidget"を呼び出す
        self.title = "ジャイロテスト"
        self.left = 50
        self.top = 50
        self.width = 1300
        self.height = 1200
        self.initUI()
        self.counter = 0

    def initUI(self):
        self.a=0

        btn1 = QPushButton("スタート", self)
        btn2 = QPushButton("ストップ", self)

        btn1.clicked.connect(self.start )
        btn2.clicked.connect(self.stop )

        self.textbox3 = QLineEdit(self)
        self.textbox4 = QLineEdit(self)
        self.textbox5 = QLineEdit(self)


        label2 = QLabel("ジャイロセンサ")
        label3 = QLabel("ピッチ角")
        label4 = QLabel("ロール角")
        label5 = QLabel("ヨー角")


        layoutA = QGridLayout()
        layoutA.addWidget(btn1,0,0)
        layoutA.addWidget(btn2,0,1)
        layoutA.addWidget(label3,1,0)
        layoutA.addWidget(self.textbox3,1,1)
        layoutA.addWidget(label4,2,0)
        layoutA.addWidget(self.textbox4,2,1)
        layoutA.addWidget(label5,3,0)
        layoutA.addWidget(self.textbox5,3,1)


        layoutB = QVBoxLayout()
        layoutB.addWidget(label2)
        layoutB.addLayout(layoutA)


        self.setLayout(layoutB)

        self.show()


    def start(self):
        
        self.a = 1

        #servo
        #GPIO.setmode(GPIO.BCM)
        #GPIO.setup(Servo_pin, GPIO.OUT)
        #Servo = GPIO.PWM(Servo_pin, 50)     #GPIO.PWM(ポート番号, 周波数[Hz])
        #Servo.start(0)                      #Servo.start(デューティ比[0-100%])

        self.startGyro()

        timer = QTimer(self)
        timer.timeout.connect(self.startGyro)
        timer.start(100)


    def startGyro(self):
        if self.a == 0:
            return
        
        DEV_ADDR = 0x68
        
        ACCEL_XOUT = 0x3b
        ACCEL_YOUT = 0x3d
        ACCEL_ZOUT = 0x3f
        TEMP_OUT = 0x41
        GYRO_XOUT = 0x43
        GYRO_YOUT = 0x45
        GYRO_ZOUT = 0x47
        
        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
        
        # Sensor data read
        def read_word_sensor(adr):
            val = read_word(adr)
            if (val >= 0x8000):         # minus
                return -((65535 - val) + 1)
            else:                       # plus
                return val
        
       
        
        
        def getGyro():
            t1=time.time()
            x = read_word_sensor(GYRO_XOUT)/ 131.0
            y = read_word_sensor(GYRO_YOUT)/ 131.0
            z = read_word_sensor(GYRO_ZOUT)/ 131.0
            t2=time.time()
            t=t2-t1
            return [x, y, z, t]
        
        
        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]
        
        
        
        
        
        ax, ay, az = getAccel()
        gx, gy, gz, t = getGyro()
        
        #print ('{0:4.3f},   {0:4.3f},    {0:4.3f},     {0:4.3f},      {0:4.3f},      {0:4.3f},' .format(gx, gy, gz, ax, ay, az))
        accel_Roll = math.atan2(ay,az) * 57.324 #57.324 = 360/2π であり、rad から ° に変換
        accel_Pitch = math.atan2(-ax , math.sqrt( ay* ay+ az*az ) ) * 57.324
        
        gyro_Roll = gx*t
        gyro_Pitch = gy*t
        
        Roll = 0.96*gyro_Roll+0.04*accel_Roll
        Pitch = 0.96*gyro_Pitch+0.04*accel_Pitch
        
        
        


        pitch = str(round(Pitch))
        self.textbox3.setText(pitch + "°")

        roll = str(round(Roll))
        self.textbox4.setText(roll + "°")

    def stop(self):
        self.a = 0
        Servo = GPIO.PWM(Servo_pin, 50)
        Servo.stop()
        GPIO.cleanup(Servo_pin)
        print("CLEAN")

if __name__ == "__main__":
    app = QApplication(sys.argv)
    ex = Tab1Widget()
    sys.exit(app.exec_())

上のようなGUIを作成してみたところ、角度が-1~1の範囲でしか出力されません。

どうかご教授ください！！

※ヨー角と書いていますがヨー角は算出する予定はありません。

Comments

[jbpb0]

> 角速度を使っての角度算出方法

このあたり見てください
https://www.microstone.co.jp/2021/05/23/%E5%8A%A0%E9%80%9F%E5%BA%A6%E3%83%BB%E8%A7%92%E9%80%9F%E5%BA%A6%E3%83%87%E3%83%BC%E3%82%BF%E3%81%8B%E3%82%89%E9%80%9F%E5%BA%A6%E3%83%BB%E8%A7%92%E5%BA%A6%E3%82%92%E6%8E%A8%E5%AE%9A%E3%81%99%E3%82%8B/
https://garchiving.com/angular-from-angular-acceleration/

[noooooooob]

jbpb0様、返信を頂きありがとうございます。
また、返信が遅れてしまい大変申し訳ございません。

そちらのURLを拝見し学習させていただきます。

Answer 1

角度＝角速度×角速度を計測した時間

これ、たぶん勘違いしています。
「角速度を計測するのにかかった時間」ではなく、dtつまり「角速度を計測する間隔」です。

「getGyro()を前回呼び出した時刻から今回呼び出した時刻までの差[秒]」を角速度[度/秒]に掛ければ角度[度]になります。
100ms間隔であれば、0.1を掛ければ、その角速度で100msに回転した角度が求まります。

で、開始してからの角度を出したいなら、これらを合計します。たとえば、
15秒間に回転した角度を求めたい場合、100ms間隔で150回呼び出したのなら、その150回の角度を合計すればよいです。

よね?

Comments

[ozwk]

> 15秒間に回転した角度を求めたい場合、100ms間隔で150回呼び出したのなら、その150回の角度を合計すればよいです。
>
> よね?

3Dの回転だとそう単純にことが運ばなくて、
例えば、
(センサーの座標系で) y軸に90度-> z軸に90度-> y軸に-90度-> x軸に-90度
とすると元の姿勢に戻りますが、各軸の単純な合計は -90,0,90 です。

[matobaa]

たしかにー。だからクォータニオンなんですね。

[matobaa]

あれ、MPU6050ってDMP搭載してて回転行列,四元数,オイラー角を返してくれるんですね。なんかもっとうまいやり方がありそうだ

[fana]

そうでなくとも，そんな処理では累積誤差で使い物にならないように思う．

[matobaa]

私もそう思ってます。でもそれは質問の本質ではないかな。

[noooooooob]

matobaa様、ozwk様、fana様、回答及び返信していただきありがとうございます。
私自身もう少しＭＰＵ６０５０について学習してみます。
また、質問の中に記載させていただいたコードは「0.1秒おき」のQTimer()を使ったコードです。この場合は dt=0.1　ということで間違いないのでしょうか？

[noooooooob]

申し訳ございません。
現状、なかなか上記の内容を試すことが出来ず、ベストアンサーを付けれない状態です。
ですが、近いうちに試し、質問またはベストアンサーをさせていただきます。
よろしくお願いします。