高性能IMU ICM-42688を実装し、物理法則を可視化する

3軸リアクションホイール倒立振子を作るアドカレ の18日目です。

1. はじめに：アクチュエータからセンサへ

前回（Day 17）までの工程で、FOC（ベクトル制御）と電流フィードバックの実装が完了し、指定したトルクを正確かつ静粛に出力するアクチュエータシステムが完成しました。

しかし、倒立振子を成立させるためには、強力なモーターだけでは不十分です。「現在の車体がどれだけ傾いているか」を正確に検知するセンサが必要です。今回は、ロボットの姿勢推定における核心部品である 6軸IMU（Inertial Measurement Unit：慣性計測装置） の実装を行います。

選定したデバイスは、TDK InvenSense製の ICM-42688-P です。ドローンやハイエンドなロボット制御で実績のある高性能MEMSセンサであり、3軸加速度センサと3軸ジャイロセンサを内蔵しています。これをSTM32とSPI通信で接続し、物理データを高速に取得するシステムを構築します。

2. 実装：SPI通信とバースト読み出し

倒立振子の制御周期は1kHz（1ms）以下を目指しているため、センサとの通信速度は重要です。I2Cよりも高速なデータ転送が可能な SPI（Serial Peripheral Interface） を採用しました。

2.1 Burst Readによる高速化と同時性の担保

単純な実装では、加速度X、Y、Z、ジャイロX、Y、Zの各レジスタに対して、アドレス送信とデータ受信を6回繰り返すことになります。しかし、これでは通信オーバーヘッドが大きくなるだけでなく、データの「同時性」が損なわれるリスクがあります。

そこで、Burst Read（連続読み出し） を実装しました。先頭アドレスを指定した後、クロックを送り続けることで、加速度6バイト＋ジャイロ6バイト（計12バイト）を一括で読み出します。これにより通信時間を大幅に短縮し、同一時刻のサンプリングデータを取得することが可能になります。

2.2 データ結合とエンディアン処理

ICM-42688-Pから送られてくる16bitデータは Big Endian（上位バイトが先）です。一方、STM32（ARM Cortex-M4）は Little Endian で動作します。そのため、受信したバッファデータを結合する際はビットシフト演算を用いて適切に整形する必要があります。

以下に実装したSPI読み出し関数の主要部分を示します。

// SPI読み込み関数
uint8_t SPI_Read(uint8_t reg) {
    uint8_t tx_data[2];
    uint8_t rx_data[2];
    tx_data[0] = reg | 0x80;
    tx_data[1] = 0x00;


    HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_RESET);
    for (volatile int d=0; d<200; d++) { __NOP(); }   // 追????��?��??��?��???��?��??��?��?

    // 2. 送受信
    HAL_SPI_TransmitReceive(&hspi3, tx_data, rx_data, 2, 100);

       HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_SET);

    return rx_data[1];
}

// バースト読み込み（複数バイトを一気に読む）
// reg: 先頭アドレス, *data: データ格納先, size: 読むバイト数
void SPI_Read_Burst(uint8_t reg, uint8_t *data, uint16_t size) {
    // 送信バッファ: [レジスタアドレス] + [ダミーデータ * size]
    // 受信バッファ: [ゴミ] + [欲しいデータ * size]
    // 大きめのバッファを確保（ここでは最大20バイト程度を想定）
    uint8_t tx_buf[20] = {0};
    uint8_t rx_buf[20] = {0};

    if (size > 19) return; // バッファオーバーフロー防止

    tx_buf[0] = reg | 0x80; // Readフラグ

    HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_RESET);
    HAL_SPI_TransmitReceive(&hspi3, tx_buf, rx_buf, size + 1, 100);
    HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_SET);

    // 受信バッファの1バイト目以降がデータ
    memcpy(data, &rx_buf[1], size);
}

// ジャイロのキャリブレーション
void Calibrate_Gyro() {
    printf("Calibrating Gyro... Don't move!\r\n");

    float sum_x = 0, sum_y = 0, sum_z = 0;
    const int samples = 200;
    uint8_t raw_data[12];

    for (int i = 0; i < samples; i++) {
        // 加速度(6byte) + ジャイロ(6byte) = 計12バイト読む
        // Gyro X start is at offset 6
        SPI_Read_Burst(REG_ACCEL_DATA_X1, raw_data, 12);

        int16_t g_x_raw = (int16_t)((raw_data[6] << 8) | raw_data[7]);
        int16_t g_y_raw = (int16_t)((raw_data[8] << 8) | raw_data[9]);
        int16_t g_z_raw = (int16_t)((raw_data[10] << 8) | raw_data[11]);

        // dps変換 (±2000dps = 16.4 LSB/dps)
        sum_x += (float)g_x_raw / 16.4f;
        sum_y += (float)g_y_raw / 16.4f;
        sum_z += (float)g_z_raw / 16.4f;

        HAL_Delay(10);
    }

    gyro_offset_x = sum_x / samples;
    gyro_offset_y = sum_y / samples;
    gyro_offset_z = sum_z / samples;

    printf("Done! Offset X:%.2f Y:%.2f Z:%.2f\r\n", gyro_offset_x, gyro_offset_y, gyro_offset_z);
}
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_LPUART1_UART_Init();
  MX_SPI3_Init();
  /* USER CODE BEGIN 2 */
  printf("\r\n=== ICM-42688 6-Axis Test ===\r\n");
    HAL_Delay(100);

    // 1. ソフトリセット
    uint8_t reset_cmd[2] = {0x11, 0x01}; // Device Reset
    HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_RESET);
    HAL_SPI_Transmit(&hspi3, reset_cmd, 2, 100);
    HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_SET);
    HAL_Delay(100);

    // 2. ID確認
    uint8_t whoami = SPI_Read(REG_WHO_AM_I);
    printf("WHO_AM_I = 0x%02X\r\n", whoami);

    if (whoami == 0x47 || whoami == 0x4C) {
        printf(">> ID OK! Sensor Found.\r\n");

        // 3. センサ起動 (Gyro+Accel Low Noise Mode)
        // PWR_MGMT0 (0x4E) = 0x0F (Accel:LN, Gyro:LN)
        uint8_t wake_cmd[2] = {REG_PWR_MGMT0, 0x0F};
        HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_RESET);
        HAL_SPI_Transmit(&hspi3, wake_cmd, 2, 100);
        HAL_GPIO_WritePin(IMU_CS_PORT, IMU_CS_PIN, GPIO_PIN_SET);
        HAL_Delay(50);

        // 4. キャリブレーション実行
        Calibrate_Gyro();

    } else {
        printf(">> ID Mismatch or Communication Error.\r\n");
        Error_Handler(); // ここで停止
    }
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
    while (1)
      {
          // --- 1. 6軸データ一括取得 (Burst Read) ---
          uint8_t raw[12];
          SPI_Read_Burst(REG_ACCEL_DATA_X1, raw, 12);

          // --- 2. データの結合 (Big Endian) ---
          int16_t ax_raw = (int16_t)((raw[0] << 8) | raw[1]);
          int16_t ay_raw = (int16_t)((raw[2] << 8) | raw[3]);
          int16_t az_raw = (int16_t)((raw[4] << 8) | raw[5]);

          int16_t gx_raw = (int16_t)((raw[6] << 8) | raw[7]);
          int16_t gy_raw = (int16_t)((raw[8] << 8) | raw[9]);
          int16_t gz_raw = (int16_t)((raw[10] << 8) | raw[11]);

          // --- 3. 物理量への変換 ---
          // Accel: ±16G (default) -> 2048 LSB/g
          float ax_g = (float)ax_raw / 2048.0f;
          float ay_g = (float)ay_raw / 2048.0f;
          float az_g = (float)az_raw / 2048.0f;

          // Gyro: ±2000dps (default) -> 16.4 LSB/dps
          // オフセットを引いて補正する
          float gx_dps = ((float)gx_raw / 16.4f) - gyro_offset_x;
          float gy_dps = ((float)gy_raw / 16.4f) - gyro_offset_y;
          float gz_dps = ((float)gz_raw / 16.4f) - gyro_offset_z;

          // --- 4. 角度計算 (簡易版: 加速度のみ) ---
          // ロール (Roll): Y軸とZ軸
          float roll = atan2(ay_g, az_g) * 180.0f / 3.14159f;

          // ピッチ (Pitch): X軸と合成ベクトル
          // ※座標定義によって正負が変わります。必要に応じて符号反転してください
          float pitch = atan2(-ax_g, sqrt(ay_g * ay_g + az_g * az_g)) * 180.0f / 3.14159f;

          // --- 5. 表示 ---
          // 角度(Roll, Pitch) と ジャイロ(XYZ) を表示
          printf("Angle[deg] R:%6.1f P:%6.1f | Gyro[dps] X:%5.1f Y:%5.1f Z:%5.1f\r\n",
                 roll, pitch, gx_dps, gy_dps, gz_dps);

          HAL_Delay(50); // 更新頻度調整
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

3. 検証実験：正方形キューブと45度治具による精度確認

単に「数値が取れた」だけでは不十分です。その値が物理的に正しいかを厳密に検証するため、以下のセットアップで実験を行いました。
実験器具として正方形のキューブと45度に固定するための治具を3Dプリンタで制作しました　

1. 正方形キューブへの固定:
   センサ基板を精密な正方形のキューブに固定しました。これにより、90度ごとの回転（0° → 90° → 180°）を物理的に正確に作り出し、重力ベクトルの遷移を確認します。
2. 45度治具の使用:
   三角定規（45度）を用いて傾斜させ、0度と90度の中間地点におけるリニアリティ（直線性）を確認します。
3. 静止ドリフト確認:
   机の上に静置し、ジャイロセンサのゼロ点オフセットとノイズレベルを確認します。

4. データ分析：グラフが語る物理法則

取得した加速度・ジャイロの生データをシリアル通信でPCへ転送し、グラフ化した結果が以下です。

グラフは、キューブを「0度（水平）→ 45度 → 90度（垂直）→ 0度」と動かした際のログです。ここから、センサが物理法則を正確に捉えていることが読み取れます。

① 加速度（Angle Graph / 青線）の考察

• ステップ応答の正確性:
  グラフの青線（Roll角：加速度から算出）を見ると、キューブを転がしたタイミングで明確なステップ状の変化を示しています。平坦な部分はピタリと一定値を指しており、重力ベクトルをノイズなく安定して捉えていることがわかります。
• 中間角度の証明:
  グラフ中盤（X軸 550〜650付近）において、青線が 45度付近 で一定時間停止しています。0度や90度といった極端な値だけでなく、中間角度においても計算通りの出力が得られており、センサのリニアリティ（直線性）が確保されていることが証明されました。

② ジャイロ（Angular Velocity / 赤線）の考察

• 動きへの反応:
  赤線（角速度）は、角度が変化している（＝キューブを動かしている）瞬間だけ、鋭いスパイク波形として現れています。一方、角度が固定されている静止区間では、即座に 0 dps (degree per second) に戻っています。
• 物理的な相関:
  このグラフは、「角度の変化率（時間微分）＝ 角速度」すなわち \omega = \frac{d\theta}{dt} という物理法則を視覚的に証明しています。
• 角度が急激に変わる（傾きが急） \rightarrow 角速度が大きい（スパイクが高い）
• 角度が変わらない（傾きゼロ） \rightarrow 角速度はゼロ
  両者のデータ整合性は極めて高く、同一時刻にサンプリングできていることが確認できました。

5. 次なる課題：ノイズとドリフト

ICM-42688-Pの実装と基礎検証は成功しましたが、グラフを詳細に分析すると、それぞれのセンサ単体での限界も見えてきました。

1. 加速度センサのノイズ:
   グラフを拡大すると、加速度から算出した角度には、静止時であっても微細なギザギザ（高周波ノイズ）が含まれています。これは、モーター振動や路面の凹凸などの外乱加速度に対して脆弱であることを意味します。
2. ジャイロセンサのドリフト:
   短期的には極めて滑らかですが、積分して角度を求めようとすると、わずかなバイアス誤差が累積し、時間は経つにつれて「ドリフト（ズレ）」が発生します。

次回の展望:
これらの課題を解決するために、次回のDay 19では 「センサフュージョン」 を実装します。「長期的には正しい加速度」と「短期的には精密なジャイロ」を数学的に融合させる Madgwickフィルタ（またはカルマンフィルタ） を導入し、倒立振子の制御に耐えうる「強固で正確な角度データ」の生成に挑みます。