概要
前回は公式ライブラリをどうにか補正しようとしたが、できないことがわかった。そこで内部の処理を確認して、もう少し手前の段階で補正やパラメータ変更をしてみたいと思います。
現状の動作
#include <M5StickC.h>
void setup() {
M5.begin();
M5.IMU.Init();
Serial.printf("DEG_TO_RAD = %f\n", DEG_TO_RAD);
delay(1000);
}
void loop() {
float accX;
float accY;
float accZ;
float gyroX;
float gyroY;
float gyroZ;
float gyroGain = DEG_TO_RAD;
float pitch;
float roll;
float yaw;
M5.IMU.getAccelData(&accX, &accY, &accZ);
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
MahonyAHRSupdateIMU(gyroX * gyroGain, gyroY * gyroGain, gyroZ * gyroGain, accX, accY, accZ, &pitch, &roll, &yaw);
Serial.printf(" %f,%f,%f\n", pitch, roll, yaw);
delay(10);
}
ざっくりと同じような処理を書き出してみました。加速度とジャイロを取得して、MahonyAHRSupdateIMU関数を呼び出すことで、ピッチとロール、ヨーを計算しています。
ジャイロは数値を直接ではなく、係数をかけていました。DEG_TO_RADという定数が公式ライブラリでは使われていて0.017453でした。前回の実験から係数が大きすぎる気がしたので変更して動かしてみたいと思います。
係数0
係数を0にするとジャイロは常に0になります。つまり加速度の情報だけで姿勢測定を行うことになります。ヨー軸は加速度からは計算できないので左右に振っても動きませんが、加速度に反応するように動かすと若干だけ動きます。
0.017453では大きすぎ、0だと小さすぎるのがわかりました。
係数0.001
一桁少なくしてみます。動き的にはかなりいいです。しかしながら静止状態でも徐々に数値が上がっていくドリフトがあります。気持ちヨー軸の反応が少ない気がします。
係数0.0001
目に見えるドリフトはなくなりました。しかしながらヨー軸の回転がほとんど反映されていません。これでは少なすぎますね。
係数は0.001前後が最適そうです。ドリフトがあるので、ジャイロの値を補正してみたいと思います。
ジャイロのばらつき確認
#include <M5StickC.h>
void setup() {
M5.begin();
M5.IMU.Init();
}
void loop() {
float gyroX;
float gyroY;
float gyroZ;
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
Serial.printf(" %f,%f,%f\n", gyroX, gyroY, gyroZ);
delay(10);
}
静止状態のジャイロを見てみます。理想的には0のはずです。
ドリフトを起こしているってことは、もちろんオフセットがありますね。適当に動かすと400とかの数字になったので、1%ぐらいの誤差があるのかな？
これをかんたんに補正してみたいと思います。
ジャイロのオフセット補正
#include <M5StickC.h>
float gyroOffsetX;
float gyroOffsetY;
float gyroOffsetZ;
void setup() {
M5.begin();
M5.IMU.Init();
float gyroSumX = 0;
float gyroSumY = 0;
float gyroSumZ = 0;
int count = 100;
for (int i = 0; i < count; i++) {
float gyroX;
float gyroY;
float gyroZ;
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
gyroSumX += gyroX;
gyroSumY += gyroY;
gyroSumZ += gyroZ;
}
gyroOffsetX = gyroSumX / count;
gyroOffsetY = gyroSumY / count;
gyroOffsetZ = gyroSumZ / count;
}
void loop() {
float gyroX;
float gyroY;
float gyroZ;
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
gyroX -= gyroOffsetX;
gyroY -= gyroOffsetY;
gyroZ -= gyroOffsetZ;
Serial.printf(" %f,%f,%f\n", gyroX, gyroY, gyroZ);
delay(10);
}
単純に100回取得した平均で補正してみます。
ブレはありますが、原点に近づきました。この補正したジャイロを使って姿勢計測をしてみたいと思います。
ジャイロ補正後姿勢計測
#include <M5StickC.h>
float gyroOffsetX;
float gyroOffsetY;
float gyroOffsetZ;
void setup() {
M5.begin();
M5.IMU.Init();
float gyroSumX = 0;
float gyroSumY = 0;
float gyroSumZ = 0;
int count = 100;
for (int i = 0; i < count; i++) {
float gyroX;
float gyroY;
float gyroZ;
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
gyroSumX += gyroX;
gyroSumY += gyroY;
gyroSumZ += gyroZ;
}
gyroOffsetX = gyroSumX / count;
gyroOffsetY = gyroSumY / count;
gyroOffsetZ = gyroSumZ / count;
}
void loop() {
float accX;
float accY;
float accZ;
float gyroX;
float gyroY;
float gyroZ;
float gyroGain = 0.001;
float pitch;
float roll;
float yaw;
M5.IMU.getAccelData(&accX, &accY, &accZ);
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
gyroX -= gyroOffsetX;
gyroY -= gyroOffsetY;
gyroZ -= gyroOffsetZ;
MahonyAHRSupdateIMU(gyroX * gyroGain, gyroY * gyroGain, gyroZ * gyroGain, accX, accY, accZ, &pitch, &roll, &yaw);
Serial.printf(" %f,%f,%f\n", pitch, roll, yaw);
delay(10);
}
補正したジャイロを入れただけになります。
動かしてみたところ、結構安定しています！ ただ若干ヨー軸の反応が弱いですね。
停止状態です。ドリフトは若干している気がしますが、前よりは少なくなりました。
ここで姿勢計測のソースを確認する
最適な係数はいくつかを確認するために、ちょっとソースを見てみます。
#define sampleFreq 25.0f // sample frequency in Hz
えっ、25Hz。秒間25更新なので、40ミリ秒間隔ですね。ちょっと少ない気がします。暫定でdelay()を10から40にしたところ、動きが遅延してヌルヌルになりました。加速度が変わった場合に、一気に姿勢に反映させるのではなく、徐々に反映させているようです。
ここでベースになったライブラリを確認してみます。
Arduinoのライブラリマネージャに登録されているのを見ると512Hzですね。いやなぜそんな割り切れない数値にして、外部から変更できないところにあるんだ、、、
元々のコードがこれですね。たしかに512Hzです。
512Hzで動くのか確認
delay()をなくして2秒間の出力行数をカウントしたところ2600行でした。秒間1000Hz以上でそうですね。とはいえ、高精度タイマーを使わないとずれていきそうなのでdelay()ではだめそうですね。
タイマーで動かしてみる
#include <M5StickC.h>
hw_timer_t *timer;
QueueHandle_t xQueue;
TaskHandle_t taskHandle;
const int sampleFrequency = 100;
// タイマー割り込み
void IRAM_ATTR onTimer() {
int8_t data;
// キューを送信
xQueueSendFromISR(xQueue, &data, 0);
}
// 実際のタイマー処理用タスク
void task(void *pvParameters) {
uint8_t calibration = 0; // キャリブレーションの状態(0:初期化直後, 1:データ取得中, 2:完了)
uint16_t calibrationCount = 0; // データ取得数
float gyroXSum = 0; // ジャイロX軸の累計数
float gyroYSum = 0; // ジャイロY軸の累計数
float gyroZSum = 0; // ジャイロZ軸の累計数
float gyroXOffset = 0; // ジャイロX軸のオフセット
float gyroYOffset = 0; // ジャイロY軸のオフセット
float gyroZOffset = 0; // ジャイロZ軸のオフセット
float pitchSum = 0; // ピッチの累計数
float rollSum = 0; // ロールの累計数
float yawSum = 0; // ヨーの累計数
float pitchOffset = 0; // ピッチのオフセット
float rollOffset = 0; // ロールのオフセット
float yawOffset = 0; // ヨーのオフセット
float gyroGain = 0; // キャリブレーションが終わるまでは0
while (1) {
int8_t data;
float accX;
float accY;
float accZ;
float gyroX;
float gyroY;
float gyroZ;
float pitch;
float roll;
float yaw;
// タイマー割り込みがあるまで待機する
xQueueReceive(xQueue, &data, portMAX_DELAY);
// 加速度、ジャイロ取得
M5.IMU.getAccelData(&accX, &accY, &accZ);
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
// ジャイロ補正
gyroX -= gyroXOffset;
gyroY -= gyroYOffset;
gyroZ -= gyroZOffset;
// AHRS計算
MahonyAHRSupdateIMU(gyroX * gyroGain, gyroY * gyroGain, gyroZ * gyroGain, accX, accY, accZ, &pitch, &roll, &yaw);
// AHRS補正
pitch -= pitchOffset;
roll -= rollOffset;
yaw -= yawOffset;
// キャリブレーション
if (calibration == 0) {
// 最初の200個は読み捨てる
calibrationCount++;
if (200 <= calibrationCount) {
calibration = 1;
calibrationCount = 0;
}
} else if (calibration == 1) {
// 一定時間データを取得してオフセットを計算する
float gyro = abs(gyroX) + abs(gyroY) + abs(gyroZ);
if (20 < gyro) {
// 振動があった場合には再度キャリブレーション
calibrationCount = 0;
gyroXSum = 0;
gyroYSum = 0;
gyroZSum = 0;
pitchSum = 0;
rollSum = 0;
yawSum = 0;
Serial.printf("Calibration Init!!!!! %f\n", gyro);
} else {
// 累計を保存
gyroXSum += gyroX;
gyroYSum += gyroY;
gyroZSum += gyroZ;
pitchSum += pitch;
rollSum += roll;
yawSum += yaw;
calibrationCount++;
if (500 <= calibrationCount) {
// 一定数溜まったらオフセット計算
calibration = 2;
gyroXOffset = gyroXSum / calibrationCount;
gyroYOffset = gyroYSum / calibrationCount;
gyroZOffset = gyroZSum / calibrationCount;
pitchOffset = pitchSum / calibrationCount;
rollOffset = rollSum / calibrationCount;
yawOffset = yawSum / calibrationCount;
Serial.printf(" gyroXSum = %f\n", gyroXSum);
Serial.printf(" gyroYSum = %f\n", gyroYSum);
Serial.printf(" gyroZSum = %f\n", gyroZSum);
Serial.printf(" gyroXOffset = %f\n", gyroXOffset);
Serial.printf(" gyroYOffset = %f\n", gyroYOffset);
Serial.printf(" gyroZOffset = %f\n", gyroZOffset);
Serial.printf(" pitchSum = %f\n", pitchSum);
Serial.printf(" rollSum = %f\n", rollSum);
Serial.printf(" yawSum = %f\n", yawSum);
Serial.printf(" pitchOffset = %f\n", pitchOffset);
Serial.printf(" rollOffset = %f\n", rollOffset);
Serial.printf(" yawOffset = %f\n", yawOffset);
// 組み込みライブラリは25Hz動作なので実際のサンプリングレートとの比で調整する
gyroGain = DEG_TO_RAD / (sampleFrequency / 25);
}
}
} else {
// 出力
Serial.printf(" %f,%f,%f\n", pitch, roll, yaw);
Serial.flush();
}
}
}
void setup() {
M5.begin();
M5.IMU.Init();
// キュー作成
xQueue = xQueueCreate(1, sizeof(int8_t));
// Core1の優先度5でタスク起動
xTaskCreateUniversal(
task, // タスク関数
"task", // タスク名(あまり意味はない)
8192, // スタックサイズ
NULL, // 引数
5, // 優先度(大きい方が高い)
&taskHandle, // タスクハンドル
APP_CPU_NUM // 実行するCPU(PRO_CPU_NUM or APP_CPU_NUM)
);
// 4つあるタイマーの1つめを利用
// 1マイクロ秒ごとにカウント(どの周波数でも)
// true:カウントアップ
timer = timerBegin(0, getApbFrequency() / 1000000, true);
// タイマー割り込み設定
timerAttachInterrupt(timer, &onTimer, true);
// マイクロ秒単位でタイマーセット
timerAlarmWrite(timer, 1000 * 1000 / sampleFrequency, true);
// タイマー開始
timerAlarmEnable(timer);
}
void loop() {
// メイン処理は無し
delay(1);
}
いろいろ試してみたのですが、ジャイロを補正した値にしています。そして係数は内蔵ライブラリの25Hzとの比にしました。今回は100Hzで動かしているので、100/25で4になり、DEG_TO_RAD/4の値が係数にしてみました。
かなり安定しました！
まだちょっとだけドリフトがありますが、それなりに使えそうな値になりました！
まとめ
もう少し補正しないとだめな気がします。現在加速度のオフセットは処理していないのでそこで誤差が溜まっている可能性があります。。。
あとは方向をリセットする処理がないと実際には使いにくいかな？
コメント
DEG_TO_RADは度(度数法)からラジアン(弧度法)に変換するための定数(≒3.14/180)として定義されているのではないでしょうか。
ジャイロセンサの値が[deg/s]で取得され、MahonyAHRSupdateIMUの内部ロジックで想定しているジャイロの単位が[rad/s]なのでこの値を乗算しているものを思われます。
表現の問題かもしれませんが、ジャイロの値をどれだけ信用するか的なゲインとして使用するのであれば、別途新しい定数を用いた方が良いかもしれません。
Mahonyは事前予測の値としてラジアンに変換して投げているのですが、そのときにIMUの取得周波数も渡していました。
IMUの取得周波数が事前と実測値で同じであればラジアンに変換するだけでいいのですが、IMUの取得周波数が違うので調整しています
Gainという表現はちょっと違うのですが、なんかジャイロの値が過敏だぞってことで当初Gainという名前で調整をしていました。
次回以降ですこしソースを整理する予定です、、、
いつも勉強させていただいております。
M5Stickで下記もどきができないかなと、勉強を始めたところです(^^;
https://www.epson.jp/products/msensor/mt500g2/feature_1.htm
ちなみに、現物を持ってます。
センサーの生の値をBluetoothSerialで取り出すところまでは楽なんですが、
そこから先が大変そうです。
続報を心待ちにしております(^^;;
ありがとうございます
なかなか実際にIMUを使うときにはデータの使い方が難しいですね、、、