加速度センサで角度の計算｜ArduinoとMMA8452Q
重力加速度から姿勢角度を計算する
これから行うことは次のとおり。
- 重力加速度から姿勢角度を計算する
- Arduinoで加速度センサMMA8452Qを使ってみる
- Arduinoと加速度センサで姿勢角度を計算する
加速度センサを使って姿勢角度を計算する方法を解説します。ここで説明するのは、重力加速度から姿勢角度を計算する方法になります。また話を簡単にするため「重力方向のz軸」と、「前後方向のx軸」の2軸に絞って考えます。
加速度センサから角度を計算する方法
センサにおけるx軸方向の加速度を\(a_x\)、z軸方向の加速度を\(a_z\)とします。
ここでは以前にRaspberry Piで作った 倒立振子 のようなモデルで姿勢角度を考えてみましょう。
倒立振子がθだけ傾いた時を考えてみます。ただし、この状態で静止しているものとします。
物体は静止しているので、重力\(a\)のみの加速度がかかってます。 よって、センサで読み取った加速度\(a_x\)と\(a_z\)のベクトルの合成値が、そのまま重力加速度\(a\)になるはずです。
これを分かりやすく書き直すと、次の図のようになります。
つまり、θは加速度\(a_x\)と\(a_z\)を使って次の式で表すことができます。
$$tanθ=\frac{a_x}{a_z}$$姿勢角度θを計算するために、逆三角関数のアークタンジェントを使います。姿勢角度θを、次式であらわすことができました。
$$θ=tan^{-1}\frac{a_x}{a_z}$$Arduino言語（C++）でアークタンジェントを使う場合は、atan2関数使います。
atan2(ax, az)また、Raspberry PiなどのPythonでアークタンジェントを使うには、numpyのarctan2関数を使います。
np.arctan2(ax, az) Arduinoで加速度センサMMA8452Qの使い方
それでは実際にArduinoで加速度センサをつかってみましょう。
つかうもの
ここで使うものを紹介します。
Arduino
Arduino Uno Rev3を使用しました。
▼ もちろん、ほかのArduinoをお使いになってもらっても構いません。
もしまだArduinoをお持ちでないようでしたら、 おすすめArduinoどれを選べばいい？Arduinoで電子工作をはじめる方へ をご覧ください。
加速度センサ
ここでは、MMA8452Qを搭載した加速度センサモジュールを使用しました。MMA8452Qは、I2C通信対応で、12ビットもしくは8ビットの分解能をもつ加速度センサです。
その他の電子部品
ブレッドボードやジャンプワイヤをお持ちでない方は揃えてください。
開発環境
| 項目 | バージョン |
|---|---|
| Arduino IDE | 1.8.13 |
| パソコン | macOS Big Sur 11.0.1 |
ArduinoとMMA8452Qの配線
こちらが、ArduinoとMMA8452Qの配線図になります。
ArduinoとMMA8452Qの配線で注意があります。それは、330Ωの抵抗を通してSCLとSDAをつなぐことです。ArduinoのGPIOが5Vであるのに対し、MMA8452Qが3.3V基準なので破損を防ぐため、抵抗でレベルシフトさせる必要があります。お使いのArduinoのデジタルピンが3.3Vの場合は、レベルシフトの必要はありません。
MMA8452Qライブラリのインストール
加速度センサからデータを読み取るには、SparkFunが公開しているMMA8452QのArduinoライブラリを使うと簡単です。 下記のGitHubページからzipファイルをダウンロードしてください。
GitHub-sparkfun/SparkFun_MMA8452Q_Arduino_Library
zipファイルを解凍し、フォルダを~/Document/Arduino/libraries/へ移動します。そして、Arduino IDEを再起動します。
ライブラリのインストールができていれば、File → Examples → SparkFun MMA8452Q Accelerometer以下にMMA8452Qのサンプルプログラムが表示されるはずです。
もしくはライブラリマネージャから、「SparkFun MMA8452Q」でライブラリ検索してインストールしてください。
ソースコード
こんな感じのプログラムを作っていきます。
サンプルプログラムをそのまま使わせて頂きました。加速度センサで取得した値をシリアルモニタや、シリアルプロッターで表示させることができます。
#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と加速度センサで姿勢角度を計算させたプログラムがこちらです。冒頭で説明したように、アークタンジェントを使って姿勢角度を制御してます。
#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のソースコード
Processingのプログラム内容は次のとおりです。
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)のようにして起点をずらしました。また、角度はラジアンで受信してます。
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のプログラムです。
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回路のローパスフィルタを、デジタル処理に置き換えたものが先ほどの式になります。
▼ デジタルフィルタを学びたい方は、こちらの書籍が詳しくておすすめです。
＼加速度センサを使った記事／
