それでも地球は回っている!

はじめに

SONYの「Spresense マルチIMUアドオンボード」の商品ページには、技術者なら思わず目を疑うような、インパクトのある一文が記載されています。

「16個の民生MEMS IMUをリアルタイム合成することで、低バイアス変動および地球自転検出可能な低ノイズ密度を可能にする」

しかし、これほど挑戦的な仕様を掲げているにもかかわらず、実際にこのボードで地球自転（自転角速度）を検出したという具体的な事例や検証記事は、ほとんど見当たりません。

そこで本記事では、「Spresense マルチIMUアドオンボードで、本当に地球自転を検出できるのか？」 という点に焦点を当て、実機を用いたデータ取得と解析による検証を行いました。

補足
普段使う一日というのは地球が太陽に対してぐるっと一周する時間である太陽日のことをさします。
しかし、自転する間に地球は公転しているので、宇宙から見て地球は本当の一回転よりも
多く回らないと太陽の方向に戻ってきません。
この本当の一回転を恒星日といい、太陽日よりも4分ほど短くなります。
そしてジャイロセンサではこの恒星日についての回転しか検出できません。

検証の目的

本検証の最終目標は、マルチIMUから得られる地球の角速度データを用いて 「恒星日（地球が1回転する時間）」を推定すること です。

一般的にMEMS IMUを用いた地球自転検出は原理的にもノイズの観点からも厳しく、結果として少なくとも数十分程度の誤差が生じると予想されますが、それを踏まえたうえで「どこまで迫れるのか」を明らかにすることが真の目的です。

検証のアプローチ

最小二乗法による推定

本検証では、短時間であればバイアスが大きく変化しないという性質を利用します。

地球の角速度は

$\omega \fallingdotseq 7.29\times10^{-5} rad/s$

であり、理想的には、ジャイロセンサーの三軸のノルム

$\sqrt{\omega_x^{2} + \omega_y^{2} + \omega_z^{2}}$

はこの地球の角速度と一致します。

そして、様々な方向にセンサーを向けてデータを取っていくと、空間内にある点 $(\omega_x, \omega_y, \omega_z)$ は原点を中心とした、半径 $\omega$ の球上に位置することになります。

しかし、実際にはそうはなりません。IMUのデータにはバイアスが乗っています。

これにより球は原点からずれ、バイアスのベクトルを$\vec{B}$とすると、球の中心は$(B_x, B_y, B_z)$となります。

このバイアスは前述した通り、長い時間でみると変化していきますが (これをドリフトと呼びます) 、短時間であればあまり変化しません。

つまり短時間でセンサーを様々な向きに向け、取ったデータについて、どのような形の球の上に乗っているのかを推定し、半径を求めることで角速度が分かり、ひいては恒星日が求められます。

球の推定については以下のサイトを参考にしました。

https://risalc.info/src/Least-square-sphere.html

詳しい導出過程はこのサイトを見てほしいのですが、簡単に説明すると、

まず、点群について重心$(\overline{x}, \overline{y}, \overline{z})$を計算して、そこを座標原点とします。

そして、点群$(x_i, y_i ,z_i)$と球の中心$(c_x, c_y, c_z)$を重心座標系では$(X_i, Y_i , Z_i)$と$(C_x, C_y, C_z)$と表すとし、球の半径$r$について$R=r^2$と置くと、次の総和Sを最小化すれば良いことになります。

$S = \sum_{i=1}^{N} \lbrace (X_i - C_x)^2 + (Y_i - C_y)^2 + (Z_i - C_z)^2 - R\rbrace$

あとは$R, C_x, C_y, C_z$それぞれについて微分し、連立方程式の形に持ち込むことで、逆行列を用いて、球の中心と半径が推定できました。

実験結果

測定方法

ChromebookにSpresenseを貼り付け、Arduino IDEのシリアルモニタで操作しました。Chromebookは回転台の上に乗せ、回しながら測定していきます。

測定結果にはバイアスの他に熱雑音によるホワイトノイズが含まれています。

このノイズは平均化することで緩和できるので計測時間はなるべく長くしたいですが、長すぎるとバイアスのドリフトの影響が出てくるので、最適な測定時間を選ばなければなりません。

実験の結果回転台を用いた8方向の計測では一回1分で測定すると良いことが分かりました。

また、できるだけ球面上に均等に点が分布するよう、向きの異なる8方向で計測を行いました。

実際の結果

下に8方向のそれぞれの測定結果を示します。

これを3次元でプロットすると下のようになり、球 (円) になっていることが確認できます。

この結果から球の半径は $\omega = 7.657707\times10^{-5} rad/s$ となり恒星日は
$\dfrac{\omega}{2\pi\times3600}=$約22.8時間と求めることができました!

実際は約23.9時間なので、相対誤差約 -5 %です！

プログラムの解説

プログラムは

・SpresenseのIMUの公式ドキュメント
・公式のIMUサンプルコード
・トランジスタ技術 2025年7月号 「第5章　速報！Spresense用マルチIMUの使い方＆実力」
・最小二乗法の解説サイト

を参考にしました。

フローチャート

ソースコード

rotation.ino
#include <Arduino.h>

#include <arch/board/board.h>
#include <nuttx/sensors/cxd5602pwbimu.h>

#include <sys/ioctl.h>
#include <fcntl.h>
#include <time.h>
#include <math.h>

#include <sys/stat.h>
#include <errno.h>
#include <unistd.h>

#define Sampling_Rate 1920
#define FIFO_Threshold 1
#define Num_Reads 8
#define Acquisition_Time 65
#define IMU_Clock_Frequency 19200000.0

cxd5602pwbimu_data_t imudata;
double g_x[Num_Reads];
double g_y[Num_Reads];
double g_z[Num_Reads];

int inverse_matrix_3(double A[3][3], double B[3][3]) {
	double detA = 0;
	detA += A[0][0]*A[1][1]*A[2][2] + A[0][1]*A[1][2]*A[2][0] + A[0][2]*A[1][0]*A[2][1];
	detA -= A[0][0]*A[1][2]*A[2][1] + A[0][1]*A[1][0]*A[2][2] + A[0][2]*A[1][1]*A[2][0];
	
	if(detA == 0) return 1;
	
	B[0][0] = (A[1][1]*A[2][2] - A[2][1]*A[1][2]) / detA;
	B[0][1] = (A[2][0]*A[1][2] - A[1][0]*A[2][2]) / detA;
	B[0][2] = (A[1][0]*A[2][1] - A[2][0]*A[1][1]) / detA;
	
	B[1][0] = (A[2][1]*A[0][2] - A[0][1]*A[2][2]) / detA;
	B[1][1] = (A[0][0]*A[2][2] - A[2][0]*A[0][2]) / detA;
	B[1][2] = (A[2][0]*A[0][1] - A[0][0]*A[2][1]) / detA;

	B[2][0] = (A[0][1]*A[1][2] - A[1][1]*A[0][2]) / detA;
	B[2][1] = (A[1][0]*A[0][2] - A[0][0]*A[1][2]) / detA;
	B[2][2] = (A[0][0]*A[1][1] - A[1][0]*A[0][1]) / detA;

  return 0;
}

double average(int N, double *A) {
	double sum = 0;
	for (int i = 0; i < N; i++) {
		sum += A[i];
	}
	return sum / N;
}

double T(int l, int m, int n, double x, double y, double z) {
	return pow(x, l) * pow(y, m) * pow(z, n);
}

double radius_estimation(int N, double *x, double *y, double *z) {
	double ave_x = average(N, x);
	double ave_y = average(N, y);
	double ave_z = average(N, z);

	double T200, T110, T101 = 0;
	double T020, T011 = 0;
	double T002 = 0;

	double T300, T120, T102 = 0;
	double T210, T030, T012 = 0;
	double T201, T021, T003 = 0;

	double X, Y, Z;
	for (int i = 0; i < N; i++) {
		X = x[i] - ave_x;
		Y = y[i] - ave_y;
		Z = z[i] - ave_z;
		
		T200 += T(2, 0, 0, X, Y, Z);
		T110 += T(1, 1, 0, X, Y, Z);
		T101 += T(1, 0, 1, X, Y, Z);
		T020 += T(0, 2, 0, X, Y, Z);
		T011 += T(0, 1, 1, X, Y, Z);
		T002 += T(0, 0, 2, X, Y, Z);

		T300 += T(3, 0, 0, X, Y, Z);
		T120 += T(1, 2, 0, X, Y, Z);
		T102 += T(1, 0, 2, X, Y, Z);
		T210 += T(2, 1, 0, X, Y, Z);
		T030 += T(0, 3, 0, X, Y, Z);
		T012 += T(0, 1, 2, X, Y, Z);
		T201 += T(2, 0, 1, X, Y, Z);
		T021 += T(0, 2, 1, X, Y, Z);
		T003 += T(0, 0, 3, X, Y, Z);
	}
  
	double T[3][3] = {{T200, T110, T101}, {T110, T020, T011}, {T101, T011, T002}};
	double t[3] = {(T300 + T120 + T102) * 0.5, (T210 + T030 + T012) * 0.5, (T201 + T021 + T003) * 0.5};
	
	double T_inv[3][3];

	if (inverse_matrix_3(T, T_inv)) {
    return -1;
  };

	double C_x, C_y, C_z;

	C_x = T_inv[0][0] * t[0] + T_inv[0][1] * t[1] + T_inv[0][2] * t[2];
	C_y = T_inv[1][0] * t[0] + T_inv[1][1] * t[1] + T_inv[1][2] * t[2];
	C_z = T_inv[2][0] * t[0] + T_inv[2][1] * t[1] + T_inv[2][2] * t[2];

	double r;

	r = C_x*C_x + C_y*C_y + C_z*C_z + (T200 + T020 + T002) / N;

	return sqrt(r);
}

int read_main(int fd, int i) {
  struct timespec start, now, delta;
  ssize_t ret;
  uint32_t prev;
  double diff;
  double total_time = 0;
  bool first = true;

  double g_x_sum = 0;
  double g_y_sum = 0;
  double g_z_sum = 0;

  printf("3\n");
  delay(1000);
  printf("2\n");
  delay(1000);
  printf("1\n");
  delay(1000);

  do {
    if (first) {
      ret = read(fd, &imudata, sizeof(cxd5602pwbimu_data_t));
      if (ret != sizeof(cxd5602pwbimu_data_t)) {
        printf("ERROR: read size mismatch! %d\n", ret);
        return 1;
      }

      prev = imudata.timestamp;
      first = false;

      clock_gettime(CLOCK_MONOTONIC, &start);
    }

    ret = read(fd, &imudata, sizeof(cxd5602pwbimu_data_t));
    if (ret != sizeof(cxd5602pwbimu_data_t)) {
        printf("ERROR: read size mismatch! %d\n", ret);
        return 1;
    }

    diff = (double)(imudata.timestamp - prev) / IMU_Clock_Frequency;
    total_time += diff;
    prev = imudata.timestamp;

    g_x_sum += (double)imudata.gx * diff;
    g_y_sum += (double)imudata.gy * diff;
    g_z_sum += (double)imudata.gz * diff;

    clock_gettime(CLOCK_MONOTONIC, &now);
    clock_timespec_subtract(&now, &start, &delta);

  } while (delta.tv_sec < Acquisition_Time) ;

  g_x[i] = g_x_sum / total_time;
  g_y[i] = g_y_sum / total_time;
  g_z[i] = g_z_sum / total_time;

  return 0;
}

void setup() {
  Serial.begin(115200);
  while (!Serial) ;
  pinMode(LED0, OUTPUT);
  delay(1000*3);

  int ret;
  ret = board_cxd5602pwbimu_initialize(5);
  if (ret < 0) {
      printf("ERROR: Failed to initialize CXD5602PWBIMU.\n");
      return;
  } else {
      printf("board_cxd5602pwbimu_initialize: OK\n");
  }
  
  int fd;
  fd = open("/dev/imu0", O_RDONLY);
  if (fd < 0) {
    printf("Open Error : %d\n", errno);
    return;
  }

  ret = ioctl(fd, SNIOC_SSAMPRATE, Sampling_Rate);
  if (ret) {
    printf("ERROR: Set a sampling rate failed. %d\n", errno);
    close(fd);
    return;
  }

  cxd5602pwbimu_range_t range;
  range.accel = 2; // G
  range.gyro  = 125; // dps
  ret = ioctl(fd, SNIOC_SDRANGE, &range);
  if (ret) {
    printf("ERROR: Set dynamic ranges failed. %d\n", errno);
    close(fd);
    return;
  }

  ret = ioctl(fd, SNIOC_SFIFOTHRESH, FIFO_Threshold);
  if (ret) {
    printf("ERROR: Set a FIFO threshold failed. %d\n", errno);
    close(fd);
    return;
  }

  ret = ioctl(fd, SNIOC_ENABLE, 1);
  if (ret) {
    printf("ERROR: Enable failed. %d\n", errno);
    close(fd);
    return;
  }

  printf("ready\n");
  int c;
  for (int i = 0; i < Num_Reads; i++) {
    c = 0;
    while (c != 'r') {
      while (!Serial.available());

      c = Serial.read();
      while (Serial.available()) {
        Serial.read();
      }
    }

    printf("start\n");
    if (read_main(fd, i)) {
      printf("An error occurred during measurement.\n");
      close(fd);
      return;
    };
    printf("finish\n");
  }

  close(fd);

  for(int i = 0; i < Num_Reads; i++) {
    printf("(%e, %e, %e)\n", g_x[i], g_y[i], g_z[i]);
  }

  double angular_velocity;
  angular_velocity = radius_estimation(Num_Reads, g_x, g_y, g_z);

  if (angular_velocity < 0) {
    printf("The calculation results are incorrect.\n");
    return;
  }

  printf("%e rad/s\n", angular_velocity);
  printf("A sidereal day is %lf hours long.\n", ((2*M_PI) / angular_velocity) / 3600.0);
}

void loop() {
  
}

まとめ

実際にSpresenseで地球の回転による角速度を計測することができました。これで、振り子を使わなくても地球が回っていることを証明できますね!

今後は、測定回数の増加や温度補償、長時間データを用いたドリフトモデルの導入などによって、さらなる精度向上が期待されます。

民生向けMEMS IMUで地球自転を直接捉えられることを示せた点は、本ボードの性能の高さを示す好例と言えるでしょう。