🐱
トラ技2019/7月号でカルマンフィルタを学ぶ(3)-IMUを使った傾斜角測定

2024/12/15に公開
 目的IMU(加速度センサー・ジャイロセンサー）で傾斜角を測定する原理を考えます。

 参考書タイトル通り、トランジスタ技術 2019/7月号

 加速度センサーを使った傾斜角測定詳細な解説は参考書にありますが、加速度センサーを使うとセンサーの傾斜角が算出できます。傾斜角の算出には重力加速度を活用します。ここで注意すべきは「加速度センサーは加速度を感じ取る」こと。つまり、センサーが加速度運動をしている状態では重力加速度に加えその加速度運動も感じ取ってしまうこと。重力加速度だけであればセンサーが感じ取る加速度は「下向きのみ」ですが、加速度運動をしている場合にはセンサーが感じ取る加速度は「下向きのみ」では無くなってしまい、正しい傾斜角を求められなくなります。

 ジャイロセンサーを使った傾斜角測定ジャイロセンサーは回転運動の「角速度」を測定できるセンサーです。ジャイロセンサーはセンサー自体が加速度運動しても角速度の影響を受けません。つまり、加速度センサーの問題点をジャイロセンサーでは発生しないということです。
角速度を使って傾斜角を求めるということは、角速度を時間で積分する必要がある。つまり、
最初の角度が分かっていることが必要
微小な測定周期の間は角速度はほぼ一定値とみなす

となります。
また、実際のジャイロセンサーの測定値には「オフセット」という大体一定の誤差が含まれてしまいます。すると、測定時間が長くなれば長くなるほど（測定回数が増えれば増えるほど）「オフセット」誤差が増えてしまうこととなります。よって「センサ値を積分して使う場合オフセット誤差の影響を無視できない」ことになります。

 傾斜角測定方法まとめ参考書記載の文言でいうと「加速度センサーによる方式は突発的な加速変化に弱く、長い時間で平均すれば精度の良い値が得られると考えられます。逆に、ジャイロセンサーは長時間使っていると誤差が蓄積して使い物にならなくなりますが、短時間の角度測定に利用するならばそれなりに良い精度が期待できます」。加速度センサーとジャイロセンサーは互いの弱点を補い合うデバイスであると言えます。この２つのセンサーの測定値を利用して真の傾斜角を推定する処理を「カルマンフィルタ」を使うことで実現できます。

 センサーの精度を可視化するセンサーの測定値のばらつきが「正規分布」に従うものとして「カルマンフィルタ」を使っていくようですが、そもそもセンサーの測定値のばらつきを可視化します。
とはいっても、(2)で無限ループさせていた部分を2000回出力に書き換えて、その結果をpythonで表示させるだけです。
imu_2000.c
#include <stdio.h>
#include <pico/stdlib.h>
#include <math.h>

#include "lib/mpu9250.h"

int sample_num = 100;
float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;

void get_data(float *theta, float *theta_dot)
{
  int16_t accel[3], gyro[3], temp;
  mpu9250_read_raw(accel, gyro, &temp);
  *theta = atan((float)accel[2] / (float)accel[0]) * 360.0 / 2.0 / M_PI;
  *theta_dot = (float)gyro[1] / 131.0;    // degree per sec
}

void acc_gyro_init()
{
  float theta, theta_dot;
  float theta_array[sample_num];
  float theta_dot_array[sample_num];

  // get data
  for (int i = 0; i < sample_num; i++)
  {
    get_data(&theta, &theta_dot);
    theta_array[i] = theta;
    theta_dot_array[i] = theta_dot;
    sleep_ms(10);
  }

  // calculate mean
  theta_mean = 0;
  theta_dot_mean = 0;
  for (int i = 0; i < sample_num; i++)
  {
    theta_mean += theta_array[i];
    theta_dot_mean += theta_dot_array[i];
  }
  theta_mean /= sample_num;
  theta_dot_mean /= sample_num;

  // calcurate variance
  float temp1, temp2;
  theta_variance = 0;
  theta_dot_variance = 0;
  for (int i = 0; i < sample_num; i++)
  {
    temp1 = theta_array[i] - theta_mean;
    theta_variance += temp1 * temp1;

    temp2 = theta_dot_array[i] - theta_dot_mean;
    theta_dot_variance += temp2 * temp2;
  }
  theta_variance /= sample_num;
  theta_dot_variance /= sample_num;
}

int main() {
  stdio_init_all();

  mpu9250_init();
  mpu9250_reset();
  mpu9250_settings();

  acc_gyro_init();

  printf("Theta,Theta_dot\n");

  for (int i = 0; i < 2000; i++) {
    float theta, theta_dot;
    get_data(&theta, &theta_dot);
    printf("%f,%f\n", theta, theta_dot);

    sleep_us(2500);
  }
  return 0;
}
この結果をファイル(imu.csv)に出力して、pythonでヒストグラム化しました。
histgram.py 
import matplotlib.pyplot as plt

file_name = "imu.csv"

stream = open(file_name, "r")

raw_data = stream.readlines()

stream.close()

fig = plt.figure()
ax1 = fig.add_subplot(1, 2, 1)
ax2 = fig.add_subplot(1, 2, 2)

theta = []
theta_dot = []
for i in raw_data:
  if i != "\n":
    tokens = i.split(',')
    theta.append(float(tokens[0]))
    theta_dot.append(float(tokens[1]))

ax1.hist(theta, bins=20)
ax1.set_title("Theta")
ax2.hist(theta_dot, bins=20)
ax2.set_title("ThetaDot")

plt.show()
基板を机においた状態でプログラムを実行、結果はこんなヒストグラムとなりました。

角速度(ThetaDot)は微妙ですが、まぁ正規分布に見えるな、という感じです。

 まとめひとまず、IMU(加速度センサーとジャイロセンサー）で傾斜角を取れそうです。また、センサーデータは正規分布に従っているというのもわかりました。
目的

参考書

加速度センサーを使った傾斜角測定

ジャイロセンサーを使った傾斜角測定

傾斜角測定方法まとめ

センサーの精度を可視化する

まとめ

Discussion
