質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

新規登録して質問してみよう
ただいま回答率
87.20%
Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

解決済

MPU6050を用いて角度(yaw角)を算出したい

tannu
tannu

総合スコア13

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

3回答

0リアクション

0クリップ

2131閲覧

投稿2019/03/25 12:46

編集2019/03/25 17:36

六軸センサMPU6050(GY521)を用いて角度算出を行いたいです

上記センサを用いて角度検出を行い,Arduino UNOを用いてシリアルモニタ上に表示しようとしています.最終的には検出した値を3dモデルと同期させることが目的です.
角度はモニタの回転角(相対角度?)ではなく,地面に対しての角度(絶対角度?)を取得を行いたいです.

また,madgwickフィルタを用いた補正についても行いたいので教えて下さると助かります.

それらを満たすソースコードを教えて頂けるとありがたいです.

x軸周りとy軸周りの角度は取得できていますが,z軸周りが取得できません

プログラムを1から自作するのは能力不足のため行えませんでしたので,GitHubよりソースコードを拝借いたしました.
コードの内容は殆ど把握できていないので引っ張ってきたものをそのまま使用しています.
下記のソースコードを試したところ,x軸(pitch)とy軸(roll)まわりの角度は地面に対する角度で取得できているのですが,z軸まわりだけはシリアルモニタを開いた時点からの回転角でしか表示できません.(シリアルモニタを表示した時点が0度)

以下にソースコードを載せますが,引用しただけですので,これを修正していただく,
もしくは1から作り変えてくださると大変助かります.

該当のソースコード

Arduino言語

#include <Wire.h> #include <MPU6050.h> MPU6050 mpu; // Pitch, Roll and Yaw values int pitch = 0; int roll = 0; float yaw = 0; void setup() { Serial.begin(115200); Serial.println("Initialize MPU6050"); while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) { Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); delay(500); } // Calibrate gyroscope. The calibration must be at rest. // If you don't want calibrate, comment this line. mpu.calibrateGyro(); // Set threshold sensivty. Default 3. // If you don't want use threshold, comment this line or set 0. mpu.setThreshold(1); // Check settings checkSettings(); } void loop() { // Read normalized values Vector normAccel = mpu.readNormalizeAccel(); Vector normGyro = mpu.readNormalizeGyro(); // Calculate Pitch & Roll pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI; roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI; //Ignore the gyro if our angular velocity does not meet our threshold if (normGyro.ZAxis > 1 || normGyro.ZAxis < -1) { normGyro.ZAxis /= 100; yaw += normGyro.ZAxis; } //Keep our angle between 0-359 degrees if (yaw < 0) yaw += 360; else if (yaw > 359) yaw -= 360; // Output Serial.print("Pitch = "); Serial.print(pitch); Serial.print("\tRoll = "); Serial.print(roll); Serial.print("\tYaw = "); Serial.print(yaw); Serial.println(); delay(10); } void checkSettings() { Serial.println(); Serial.print(" * Sleep Mode: "); Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled"); Serial.print(" * Clock Source: "); switch(mpu.getClockSource()) { case MPU6050_CLOCK_KEEP_RESET: Serial.println("Stops the clock and keeps the timing generator in reset"); break; case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break; case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break; case MPU6050_CLOCK_PLL_ZGYRO: Serial.println("PLL with Z axis gyroscope reference"); break; case MPU6050_CLOCK_PLL_YGYRO: Serial.println("PLL with Y axis gyroscope reference"); break; case MPU6050_CLOCK_PLL_XGYRO: Serial.println("PLL with X axis gyroscope reference"); break; case MPU6050_CLOCK_INTERNAL_8MHZ: Serial.println("Internal 8MHz oscillator"); break; } Serial.print(" * Gyroscope: "); switch(mpu.getScale()) { case MPU6050_SCALE_2000DPS: Serial.println("2000 dps"); break; case MPU6050_SCALE_1000DPS: Serial.println("1000 dps"); break; case MPU6050_SCALE_500DPS: Serial.println("500 dps"); break; case MPU6050_SCALE_250DPS: Serial.println("250 dps"); break; } Serial.print(" * Gyroscope offsets: "); Serial.print(mpu.getGyroOffsetX()); Serial.print(" / "); Serial.print(mpu.getGyroOffsetY());![イメージ説明](d3ea6c978f9b7305cff2783c90dcc219.png) Serial.print(" / "); Serial.println(mpu.getGyroOffsetZ()); Serial.println(); }

補足情報

当方プログラミングに関して初心者なので最低限の知識しか持ち合わせていないと思います.
ソースコード等最低限の情報さえご教授いただければ後は自分で勉強させていただきます.
よろしくお願いします.

*追記
角度算出式

以下のような質問にはリアクションをつけましょう

  • 質問内容が明確
  • 自分も答えを知りたい
  • 質問者以外のユーザにも役立つ

リアクションが多い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

気になる質問をクリップする

クリップした質問は、後からいつでもマイページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

下記のような質問は推奨されていません。

  • 間違っている
  • 質問になっていない投稿
  • スパムや攻撃的な表現を用いた投稿

適切な質問に修正を依頼しましょう。

ikedas

2019/03/25 13:46

プログラム中で、pitch、roll、yawがどういう風に計算されているかを、普通の数式を使って説明できますか。できるのなら質問本文に書いてもらえますか (atan2やsqrtの意味が分からなければマニュアル等で調べてください)。
tannu

2019/03/25 15:59

ご指摘ありがとうございます.質問本文に少し説明を追加させて頂きました. 恥ずかしながら,プログラムの内容については理解が及んでいません. アークタンジェントによって座標の偏角を求めていたり,平方根の計算をしているのかなという程度の認識でおりました.申し訳ありません.
ikedas

2019/03/25 16:03

そこまで理解しているのなら、プログラムの中身を見て数式で説明してみましょうよ。Madgwick filterの解説ならもっと複雑な数式が出てきますよ。
tannu

2019/03/25 17:36

お返事ありがとうございます 理解が足りていなく少しあいまいな部分がありますが,自分なりに分かる部分を説明させていただきます. pitch角及びroll角は回転行列を使用して算出したのかと思います. 回転前の行列式(単位行列に重力成分を掛けたもの?)にそれぞれの軸周りの回転行列を掛け合わせたものと回転後の行列式(成分はセンサの加速度から取り出したもの)との恒等式について解いたものではないかと思います. 式については本文最後に追記します.(数式なのでここだと見にくいかと) yaw角に関しては,センサのgyroの値が1より大きいか-1未満の場合,gyroの値を100で割って,その値を加算し続けているんでしょうか?(あまり自信がありませんが,,) 間違っていましたらご指摘の方お願いできればと思います.
ozwk

2019/03/26 00:49 編集

結局質問はyaw角を水平面に対する絶対角度で取りたいということですか? だとしたら無理というかそんなものは無いですけど。
tannu

2019/03/26 11:15

yaw角という使い方が正しいか分かりませんが,水平面に対して鉛直方向の軸周りの角度を取りたいということです.
ikedas

2019/03/26 12:17

わたしも「そんなものは無い」とおもいます。回答にも書いたとおり、それぞれの軸がどれだけの角度ずれたかを、加速度センサだけで知ることはできません。 ただ、pitchやrollと同様に、加速度ベクトルのX-Y座標面への射影とX軸、Y軸との成す角や、加速度ベクトルがZ-X座標面やY-Z座標面と成す角を考えることはできるでしょう。そういったものに意味があるかどうかは、数式を作って考えてみては。
ozwk

2019/03/27 03:12 編集

> 水平面に対して鉛直方向の軸周りの角度を取りたい では水平面に対して鉛直軸周りの回転で 例えば0度どこに置くんですか? 加速度センサーでも角速度センサでも定められないでしょ?
ikedas

2019/03/26 14:54 編集

それは、ほかの値は2軸以上の軸の間の関係から導かれてるんだから、yawだって同じことができるでしょう (わたしは「真の値は求められない」という立場ですが)。ジャイロセンサの初期値に意味はないし、センサのZ軸まわりの回転量を採用するのがおかしいのは当然として。加筆

まだ回答がついていません

会員登録して回答してみよう

15分調べてもわからないことは
teratailで質問しよう!

ただいまの回答率
87.20%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問

同じタグがついた質問を見る

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。