MPU6050のDMPで姿勢を取得する

ロボット

Top

前回行った MPU6886 での Yaw 角推定ではどうしてもドリフトが起こってしまい綺麗に値を取ることができず、調べていると MPU6050 の DMP を使うと簡単に姿勢を取得できるということがわかったので、その方法をまとめておきます。

DMP(Digital Motion Processor)とはモーションセンサ MPU シリーズに内蔵された自身の姿勢角を算出する機能です。

準備

  • M5Stack Basic(Arduino 系であれば可)
  • GY-521(MPU6050)
  • macOS(書き込みは PlatformIO)

接続

M5StackGY-521
3.3VVCC
GNDGND
SDASDA
SCLSCL

ライブラリのインストール

PlatformIO の場合は、platformio.iniに以下を追加する。

1lib_deps = electroniccats/MPU6050@^0.6.0

センサーのキャリブレーションプログラムの実行

ライブラリにあるIMU_Zeroというサンプルスケッチを実行し、出力された値を記録しておく。

プログラムの実行

サンプルスケッチのMPU6050_DMP6を M5Stack 用に書き換えた。 描画の際は画面がちらつくと見づらいので LovyanGFX を使って描画している。

1
2#define LGFX_AUTODETECT
3#define LGFX_USE_V1
4
5#include <M5Stack.h>
6#include "MPU6050_6Axis_MotionApps20.h"
7#include <LovyanGFX.hpp>
8#include <LGFX_AUTODETECT.hpp>
9MPU6050 mpu;
10
11static LGFX lcd;
12static LGFX_Sprite canvas(&lcd);
13
14// MPU control/status vars
15uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
16uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
17uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
18uint8_t fifoBuffer[64]; // FIFO storage buffer
19
20float GYdegree = 0.0F;
21int yaw = 0;
22
23// orientation/motion vars
24Quaternion q;           // [w, x, y, z]         quaternion container
25VectorFloat gravity;    // [x, y, z]            gravity vector
26float ypr[3];           // [roll, pitch, yaw]   roll/pitch/yaw container and gravity vector
27
28void setupMPU() {
29  Wire.begin();
30  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
31  mpu.initialize();
32  devStatus = mpu.dmpInitialize();
33
34  // supply your own gyro offsets here, scaled for min sensitivity
35  mpu.setXAccelOffset(-2634);
36  mpu.setYAccelOffset(-38);
37  mpu.setZAccelOffset(472);
38  mpu.setXGyroOffset(203);
39  mpu.setYGyroOffset(54);
40  mpu.setZGyroOffset(28);
41
42  // make sure it worked (returns 0 if so)
43  if (devStatus == 0) {
44    // Calibration Time: generate offsets and calibrate our MPU6050
45    mpu.CalibrateAccel(6);
46    mpu.CalibrateGyro(6);
47    mpu.setDMPEnabled(true);
48    packetSize = mpu.dmpGetFIFOPacketSize();
49  } else {
50    Serial.print("DMP Initialization failed.");
51  }
52}
53
54void getYawPitchRoll() {
55  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
56    mpu.dmpGetQuaternion(&q, fifoBuffer);
57    mpu.dmpGetGravity(&gravity, &q);
58    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
59    Serial.println(int(ypr[0] * 180 / M_PI));
60  }
61}
62
63int GetGyro()   //yaw角のみ角度の取得
64{
65    getYawPitchRoll();
66    yaw = int(ypr[0] * 180 / M_PI);
67    return (int)yaw;
68}
69
70void DrawGyro()
71{
72  int gyro = GetGyro();
73  M5.Lcd.clear();
74  while (true)    //LGFXを用いて円上に現在の角度の点を表示
75  {
76    M5.update();
77    canvas.fillScreen(BLACK);
78    gyro = GetGyro();
79    GYdegree = (gyro-90) / (180 / PI);
80    canvas.drawCircle(160, 120, 80, WHITE);
81    canvas.fillCircle(160 + 80 * cos(GYdegree), 120 + 80 * sin(GYdegree), 10, GREEN);
82    canvas.setCursor(160, 0);
83    canvas.printf("%4d", gyro);
84    canvas.pushSprite(0,0);
85  }
86}
87
88void setup() {
89  M5.begin();
90  M5.Power.begin();
91  Serial.begin(115200);
92  M5.Lcd.fillScreen(BLACK); M5.Lcd.setTextColor(WHITE , BLACK); M5.Lcd.setTextSize(2);  //M5StackのLCDの設定
93  lcd.init(); lcd.setRotation(1); canvas.setColorDepth(8); canvas.setTextWrap(false); canvas.setTextSize(2); canvas.createSprite(lcd.width(), lcd.height());   //LGFXの初期設定
94  setupMPU();
95}
96
97void loop() {
98  DrawGyro();
99}

34 行目~39 行目の mpu.setXAccelOffset(-2634);等の値は、IMU_Zero で出力された値を入力する。

動作

Top

しっかり動いてくれています。ドリフトに関しても１時間放置して+-1 度程度だったので許容範囲内だと思います。 センサーを激しく振り回しても、角度がずれることはありませんでした。