加速度センサで角度の計算|ArduinoとMMA8452Q

加速度センサで角度の計算|ArduinoとMMA8452Q
加速度センサで角度の計算|ArduinoとMMA8452Q

重力加速度から姿勢角度を計算する

重力加速度から姿勢角度を計算する
重力加速度から姿勢角度を計算する

これから行うことは次のとおり。

  • 重力加速度から姿勢角度を計算する
  • Arduinoで加速度センサMMA8452Qを使ってみる
  • Arduinoと加速度センサで姿勢角度を計算する

加速度センサを使って姿勢角度を計算する方法を解説します。ここで説明するのは、重力加速度から姿勢角度を計算する方法になります。また話を簡単にするため「重力方向のz軸」と、「前後方向のx軸」の2軸に絞って考えます。

加速度センサから角度を計算する方法

センサにおけるx軸方向の加速度を\(a_x\)、z軸方向の加速度を\(a_z\)とします。

ここでは以前にRaspberry Piで作った 倒立振子 のようなモデルで姿勢角度を考えてみましょう。

加速度センサと倒立振子モデル
加速度センサと倒立振子モデル

倒立振子がθだけ傾いた時を考えてみます。ただし、この状態で静止しているものとします。

倒立振子がθだけ傾いている
倒立振子がθだけ傾いている

物体は静止しているので、重力\(a\)のみの加速度がかかってます。 よって、センサで読み取った加速度\(a_x\)と\(a_z\)のベクトルの合成値が、そのまま重力加速度\(a\)になるはずです。

センサのx軸とz軸の加速度
センサのx軸とz軸の加速度

これを分かりやすく書き直すと、次の図のようになります。

加速度センサのベクトル合成
加速度センサのベクトル合成

つまり、θは加速度\(a_x\)と\(a_z\)を使って次の式で表すことができます。

$$tanθ=\frac{a_x}{a_z}$$

姿勢角度θを計算するために、逆三角関数のアークタンジェントを使います。姿勢角度θを、次式であらわすことができました。

$$θ=tan^{-1}\frac{a_x}{a_z}$$

Arduino言語(C++)でアークタンジェントを使う場合は、atan2関数使います。

cpp
atan2(ax, az)

また、Raspberry PiなどのPythonでアークタンジェントを使うには、numpyarctan2関数を使います。

py
np.arctan2(ax, az) 

Arduinoで加速度センサMMA8452Qの使い方

それでは実際にArduinoで加速度センサをつかってみましょう。

つかうもの

ここで使うものを紹介します。

Arduino

Arduino Uno Rev3を使用しました。

▼ もちろん、ほかのArduinoをお使いになってもらっても構いません。

もしまだArduinoをお持ちでないようでしたら、 おすすめArduinoどれを選べばいい?Arduinoで電子工作をはじめる方へ をご覧ください。

加速度センサ

ここでは、MMA8452Qを搭載した加速度センサモジュールを使用しました。MMA8452Qは、I2C通信対応で、12ビットもしくは8ビットの分解能をもつ加速度センサです。

その他の電子部品

ブレッドボードやジャンプワイヤをお持ちでない方は揃えてください。

開発環境

項目バージョン
Arduino IDE1.8.13
パソコンmacOS Big Sur 11.0.1

ArduinoとMMA8452Qの配線

こちらが、ArduinoとMMA8452Qの配線図になります。

ArduinoとMMA8452Qの配線図
ArduinoとMMA8452Qの配線図

ArduinoとMMA8452Qの配線で注意があります。それは、330Ωの抵抗を通してSCLとSDAをつなぐことです。ArduinoのGPIOが5Vであるのに対し、MMA8452Qが3.3V基準なので破損を防ぐため、抵抗でレベルシフトさせる必要があります。お使いのArduinoのデジタルピンが3.3Vの場合は、レベルシフトの必要はありません。

Arduinoで加速度センサMMA8452Qの使い方
Arduinoで加速度センサMMA8452Qの使い方

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

加速度センサからデータを読み取るには、SparkFunが公開しているMMA8452QのArduinoライブラリを使うと簡単です。 下記のGitHubページからzipファイルをダウンロードしてください。

GitHub-sparkfun/SparkFun_MMA8452Q_Arduino_Library

zipファイルを解凍し、フォルダを~/Document/Arduino/libraries/へ移動します。そして、Arduino IDEを再起動します。

ライブラリのインストールができていれば、FileExamplesSparkFun MMA8452Q Accelerometer以下にMMA8452Qのサンプルプログラムが表示されるはずです。

もしくはライブラリマネージャから、「SparkFun MMA8452Q」でライブラリ検索してインストールしてください。

Platform IOでMMA8452Qライブラリ検索
Platform IOでMMA8452Qライブラリ検索

ソースコード

こんな感じのプログラムを作っていきます。

加速度をシリアルプロッターで表示
加速度をシリアルプロッターで表示

サンプルプログラムをそのまま使わせて頂きました。加速度センサで取得した値をシリアルモニタや、シリアルプロッターで表示させることができます。

cpp
#include <Wire.h>
#include "SparkFun_MMA8452Q.h"

MMA8452Q accel;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (accel.begin() == false) {
    Serial.println("Not Connected. Please check connections and read the hookup guide.");
    while (1);
  }
}

void loop() {
  if (accel.available()) {
    Serial.print(accel.getCalculatedX(), 3);
    Serial.print("\t");
    Serial.print(accel.getCalculatedY(), 3);
    Serial.print("\t");
    Serial.print(accel.getCalculatedZ(), 3);
    Serial.println();
  }
}

ソースコードの解説

accel.getCalculatedXなどで取得される値は、重力加速度1gを単位としてます。また、Spark Funの MMA8452Qチュートリアル によれば、データの転送速度は800Hzで、±2gまで測ることができるそうです。

Arduinoと加速度センサで姿勢角度を計算

加速度センサの使い方が分かったところで、今度は実際に、Arduinoを使って加速度センサから角度を計算させてみます。

ソースコード

Arduinoと加速度センサで姿勢角度を計算させたプログラムがこちらです。冒頭で説明したように、アークタンジェントを使って姿勢角度を制御してます。

cpp
#include <Arduino.h>
#include <Wire.h>
#include "SparkFun_MMA8452Q.h"

MMA8452Q accel;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (accel.begin() == false) {
    Serial.println("Not Connected. Please check connections and read the hookup guide.");
    while (1);
  }
}

void loop() {
  if (accel.available()) {
    float ax = accel.getCalculatedX();
    float az = accel.getCalculatedZ();
    float angle = atan2(ax, az) * 180.0 / PI;

    Serial.print(angle, 3);
    Serial.println();
  }
}

Processingで視覚化

これから行うことは次のとおり。

  • Processingでシリアル通信、Arduinoからデータの受信
  • 加速度センサの姿勢に合わせ、Processingで図形を動かす
  • ローパスフィルタでセンサノイズの除去、動きの違いを視覚化

ArduinoのデータをProcessingで受け取る

まずは、Processingでシリアル通信からデータ受信を行います。 こちらの映像のように、ArduinoのSerial.printで出力したデータを、Processingで受け取ってテキストで表示させました。

ProcessingでArduinoのデータを表示
ProcessingでArduinoのデータを表示

Processingのソースコード

Processingのプログラム内容は次のとおりです。

pde
import processing.serial.*;

Serial serial;
String v = "";
int lf = 10;

void setup() 
{
  println(Serial.list()[3]);
  
  size(200, 150);
  frameRate(30);
  
  serial = new Serial(this, Serial.list()[3], 9600);
  serial.bufferUntil(lf);
}

void draw()
{
  background(255, 255, 255);
  
  if(v != null) {
    fill(0, 0, 255);
    textSize(28);
    textAlign(CENTER);
    text(v, 100, 75);
  }
}

void serialEvent(Serial p) {
  if (serial.available() >= 2) 
  {
     v = p.readString();    
  }
}

ソースコードの解説

シリアルポート名取得のために、Serial.list()Arduinoのポートを確認し、指定します。また、シリアル通信の速度をArduino側と同じにします。

printlnで送られた改行コード(Line Feed)が受信されると、serialEventのイベントが呼び出されますので、そこで描画を更新します。

Arduinoのソースコードは前半で紹介したものを流用しました。

加速度センサの姿勢をProcessingでシンクロさせる

Processingで長方形のオブジェクトを作り、加速度センサの姿勢に合わせてオブジェクトをシンクロさせてみました。

Processingのソースコード

Processingのプログラムがこちらになります。四角形の中心を軸に回転させるため、rect(-w/2, -h/2, w, h)のようにして起点をずらしました。また、角度はラジアンで受信してます。

pde
import processing.serial.*;

Serial serial;
int lf = 10;
float ra = 0;
int w = 100;
int h = 40;

void setup() 
{
  println(Serial.list()[3]);
  
  size(300, 300);
  frameRate(30);
  
  serial = new Serial(this, Serial.list()[3], 9600);
  serial.bufferUntil(lf);
}

void draw()
{
  background(255, 255, 255);
  
  fill(77, 77, 77);
  translate(width/2, height/2);
  rotate(ra + PI);
  
  rect(-w/2, -h/2, w, h);
}

void serialEvent(Serial p) {
  if (serial.available() >= 2) 
  {
     ra = float(p.readString());    
  }
}

ローパスフィルタでセンサノイズの除去、動きの違いを視覚化

Arduinoからシリアル通信で受信した加速度センサの姿勢角度に、Processingでさまざまなローパスフィルタをかけてセンサノイズを除去し、動きの違いを図形で視覚化してみます。

ローパスフィルタの強さによるブレの違い
ローパスフィルタの強さによるブレの違い

動画の続きはYouTubeでご覧ください。

ローパスフィルタが強いほど手ブレが目立たなくなりますが、反応が鈍くなって速い動きに追従できなくなります。

Processingのソースコード

6つのオブジェクトを作り、加速度センサの姿勢角度に係数を変えたローパスフィルタかけたProcessingのプログラムです。

pde
import processing.serial.*;

Serial serial;
int lf = 10; // line feed
float ra = 0; // radian
int w = 100; // objects width
int h = 40; // objects height
int n = 6; // objects count
float[] y = {0, 0, 0, 0, 0, 0};
float[] a = {0, 0.2, 0.3, 0.5, 0.7, 0.9};
void setup() 
{
  println(Serial.list()[3]);
  
  size(900, 600);
  frameRate(30);
  
  serial = new Serial(this, Serial.list()[3], 9600);
  serial.bufferUntil(lf);
}

// y[n] = a*y[n-1] + (1-a)*x[n]
float rc_lpf(float y, float x, float a) {
  return a * y + (1-a) * x;
}

void drawObject(int i, float ra) {
  float dx = float(w) * float(i);
  float dy = 0;
  if(i >= 3) {
    dx = float(w) * float(i-3);
    dy = w * 1.5;
  }
  translate(width/(n+1) + dx, height/3 + dy);
  if(n == 0) {
  } else {
    translate(dx, 0);
  }
  textSize(16);
  text("a="+a[i], -w/2, h*2);
  float r = rc_lpf(y[i], ra, a[i]);
  y[i] = r;
  rotate(r + PI);  
  rect(-w/2, -h/2, w, h);
  resetMatrix();
}

void draw()
{
  background(255, 255, 255);
  fill(77, 77, 77);
  textSize(24);
  text("Low-Pass Filter", 50, 70);
  text("y[n] = a*y[n-1] + (1-a)*x[n]", 50, 120);
  
  for(int i = 0; i < n; i++) {
    drawObject(i, ra);
  }
  
}

void serialEvent(Serial p) {
  if (serial.available() >= 2) 
  {
     ra = float(p.readString());    
  }
}

ローパスフィルタの解説

ここでは、プログラム内のローパスフィルタについて解説します。

ローパスフィルタとは

ローパスフィルタとは、周波数の高い成分をカットするフィルタです。ArduinoやRaspberry Piなどのマイコンでは、センサノイズの除去に使ったりします。 たとえば、加速度センサで姿勢角度を測定する場合、微振動などのセンサノイズが混ざってしまいます。そこで、測定データにローパスフィルタをかけ、本来の動きよりも周波数の高いセンサノイズを除去します。

RCローパスフィルタのアルゴリズム

次の式は、デジタル信号におけるRCローパスフィルタです。非常に単純なアルゴリズムですが、手軽さゆえによく使われます。

$$y[n]=ay[n-1]+(1-a)x[n]$$

aは係数で、ローパスフィルタの強さと思ってください。yが出力で、xが入力です。また、nはある時点でのサンプルを意味します。 よって、この式で行なっていることは、1つ前の出力結果に係数をかけてフィードバックし、新たな入力と足し合わせたものを出力してます。

RCとは

RCローパスフィルタのRCとは、Resistor(抵抗)とCapacitor(コンデンサ)のことです。電子回路のアナログで使われるRC回路のローパスフィルタを、デジタル処理に置き換えたものが先ほどの式になります。

▼ デジタルフィルタを学びたい方は、こちらの書籍が詳しくておすすめです。

\加速度センサを使った記事/

関連記事

 
最後までご覧いただきありがとうございます!

▼ 記事に関するご質問やお仕事のご相談は以下よりお願いいたします。
お問い合わせフォーム

関連記事