1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

Day 13

STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ） Advent Calendar 2025

@RamTuckey

姿勢推定アルゴリズム実装：STM32単体で検証する「Madgwickフィルタ」の性能

1
Posted at

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

1. はじめに：制御入力の信頼性を確保する

これまでの工程で、以下の2つの要素が完成しました。

  • Day 11 (脳): PyTorchで学習したAIモデルのマイコン実装。
  • Day 12 (筋肉): モータを駆動するFOCベクトル制御の実装。

しかし、AIモデルへの入力となる「現在の姿勢（Roll角）」の取得には課題が残っています。MEMSセンサから得られる生のデータは、ノイズやバイアスを含んでおり、そのままフィードバック制御に使用すればシステムは発散（転倒）します。

本日のゴールは、ジャイロセンサと加速度センサのデータを融合する 「Madgwickフィルタ」 をC言語で実装することです。
今回はハードウェア要因（配線やはんだ付け不良）を排除するため、STM32内部で「理想的な動き」と「ノイズ」を数式で生成し、アルゴリズムの挙動を単体検証する HILS (Hardware-in-the-Loop Simulation) 的なアプローチをとります。

2. センサ誤差の数学的モデルとフュージョンの必要性

倒立振子の制御において、なぜ単一のセンサでは不十分なのか。まずはジャイロセンサと加速度センサの特性を数式で整理します。

2.1 ジャイロセンサ：積分ドリフトの罪

ジャイロセンサは角速度 $\omega(t)$ [rad/s] を計測します。角度 $\theta(t)$ を得るには、時間積分が必要です。

連続時間系:
$$\theta(t) = \theta(0) + \int_{0}^{t} \omega(\tau) d\tau$$

離散時間系（サンプリング周期 $\Delta t$）:
マイコン内では以下の漸化式で計算されます。
$$\theta_k = \theta_{k-1} + \omega_k \cdot \Delta t$$

誤差モデル:
実際のセンサ出力 $\omega_{meas}$ には、真の角速度 $\omega_{true}$ に加えて、温度や経年変化に起因する定常バイアス $b$ とホワイトノイズ $\eta$ が含まれます。
$$\omega_{meas}(t) = \omega_{true}(t) + b + \eta(t)$$

これを積分すると、以下のようになります。
$$\theta_{est}(t) = \int_{0}^{t} (\omega_{true}(\tau) + b) d\tau = \theta_{true}(t) + \underbrace{b \cdot t}_{\text{Drift}}$$

ノイズ $\eta$ は積分で相殺されやすいですが、バイアス $b$ は時間 $t$ と共に線形に増大します。これが 「積分ドリフト」 であり、数秒で実用不能な誤差を生みます。

2.2 加速度センサ：外乱ノイズの罪

加速度センサは、センサ座標系における重力加速度ベクトル $g$ と、運動加速度 $a_{lin}$ の合力を計測します。
静止状態であれば、重力方向から傾きを逆算できます。例えばロール角 $\phi$ は以下で求まります。

$$\phi = \text{atan2}(a_y, a_z)$$

誤差モデル:
しかし、倒立振子は激しく運動するため、並進加速度やモータ振動 $a_{noise}$ が加わります。
$$a_{meas} = R(\theta) \cdot g + a_{lin} + a_{noise}$$

ここから算出した角度は、微分（D制御）に使用できないほど高周波ノイズにまみれており、瞬時値としての信頼性はありません。

2.3 センサフュージョンの概念検証 (Python)

これら2つのセンサを混ぜ合わせる（フュージョンする）効果を視覚的に理解するため、Pythonでシミュレーションを行いました。

シミュレーションコード:

import numpy as np
import matplotlib.pyplot as plt

# シミュレーション設定
dt = 0.01
steps = 1000
t = np.linspace(0, steps*dt, steps)

# 真の角度（正弦波で揺らす）
true_angle = np.sin(t) * 45  # ±45度

# 1. ジャイロセンサ（ドリフトあり）
# 角速度（真の値の微分） + バイアスノイズ
gyro_bias = 2.0 # 2deg/s のオフセット
gyro_data = np.diff(true_angle, prepend=0)/dt + gyro_bias
# 単純積分
gyro_angle = np.cumsum(gyro_data * dt)

# 2. 加速度センサ（ノイズあり）
# 真の値 + ホワイトノイズ
acc_noise = np.random.normal(0, 5.0, steps) # ±5deg程度の激しいノイズ
acc_angle = true_angle + acc_noise

# 3. 簡易フュージョン（相補フィルタ）
# alpha = 0.95 (ジャイロ寄り)
fusion_angle = np.zeros(steps)
current_angle = 0
alpha = 0.95

for i in range(1, steps):
    # (前回角度 + ジャイロ変化分) * alpha + (加速度角度) * (1-alpha)
    pred_angle = fusion_angle[i-1] + gyro_data[i]*dt
    fusion_angle[i] = alpha * pred_angle + (1-alpha) * acc_angle[i]

# グラフ描画
plt.figure(figsize=(10, 6))
plt.plot(t, true_angle, 'k--', linewidth=2, label="True Angle")
plt.plot(t, gyro_angle, 'r', label="Gyro Integration (Drift)")
plt.plot(t, acc_angle, 'g', alpha=0.3, label="Accel (Noisy)")
plt.plot(t, fusion_angle, 'b', linewidth=2, label="Sensor Fusion")

plt.title("Why Sensor Fusion is Necessary?")
plt.xlabel("Time [s]")
plt.ylabel("Angle [deg]")
plt.legend()
plt.grid(True)
plt.show()

実行結果:
image.png

  • 赤線 (Gyro): 滑らかですが、時間とともに真値から離れていきます。
  • 緑線 (Accel): 真値の周りにいますが、ノイズが酷すぎます。
  • 青線 (Fusion): 両者の欠点を補い合い、真値（黒破線）に追従しています。

これをSTM32上で、より高度かつ軽量に実現するのが Madgwickフィルタ です。

3. Madgwickフィルタの理論詳解

Madgwickフィルタは、直感的なオイラー角（Roll, Pitch, Yaw）ではなく、四元数 (Quaternion) を用いて内部計算を行います。これは、オイラー角特有の「ジンバルロック」を回避し、かつ計算を効率化するためです。

四元数 $q$ は、スカラー部 $q_0$ とベクトル部 $q_1, q_2, q_3$ で表される4次元の数です。

$$
q = [q_0, q_1, q_2, q_3]
$$

このアルゴリズムの本質は、「ジャイロセンサによる『推測』」 と 「加速度センサによる『補正』」 という2つのプロセスを、「姿勢の変化率（微分値）」 の段階で融合させる点にあります。

3.1 Step 1: ジャイロセンサによる「推測」（積分）

まず、ジャイロセンサの役割です。
現在の姿勢 $q_{t-1}$ に対して、ジャイロで計測した角速度 $\omega$ で回転し続けた場合、次の瞬間に姿勢がどう変化するかを計算します。

角速度ベクトルを四元数形式 $\omega = [0, \omega_x, \omega_y, \omega_z]$ と定義すると、四元数の運動学的微分方程式は以下のように記述されます。

$$
\dot{q}_{\omega} = \frac{1}{2} q_{t-1} \otimes \omega
$$

  • $\otimes$: 四元数の乗算
  • $\dot{q}_{\omega}$: ジャイロセンサのみから導かれる「姿勢の変化速度」

これを単純に積分（$q_t = q_{t-1} + \dot{q}_{\omega}\Delta t$）すれば新しい姿勢が得られます。しかし、第2章で述べた通り、ここにはセンサのバイアス誤差が含まれるため、このままでは計算上の姿勢はゆっくりと、しかし確実に真の姿勢からズレていきます（ドリフト）。

3.2 Step 2: 加速度センサによる「補正」（勾配降下法）

ここで、加速度センサの出番です。「ズレ」を正すための絶対的な基準として「重力」を利用します。

考え方:
もし、計算上の推定姿勢 $q_{t-1}$ が正しいならば、その姿勢で感じるはずの重力方向（予測値）は、実際の加速度センサの計測値（観測値）と一致するはずです。

  1. 世界座標系の重力: $g_E = [0, 0, 0, 1]$ （Z軸下向き）
  2. センサ座標系の観測値: $a_S = [0, a_x, a_y, a_z]$ （加速度センサの値・正規化済み）
  3. 姿勢 $q$ による回転予測: 推定姿勢 $q$ で $g_E$ を回転させた予測ベクトル $d_S$ を計算。

ここで、「計算上の重力 $d_S$」と「実際の重力 $a_S$」の誤差関数 $f(q)$ を定義します。

$$
f(q) = d_S - a_S = \left( q^{-1} \otimes g_E \otimes q \right) - a_S
$$

この誤差 $f(q)$ が $0$ になれば、姿勢は正しいことになります。
そこで、「誤差 $f(q)$ が最も小さくなる方向（勾配）」 を数学的に計算し、そちらへ姿勢を少しだけ引っ張ります。これが勾配降下法（Gradient Descent）です。

$$
\text{補正ベクトル} = \nabla f = J^T f(q)
$$

  • $J$: 誤差関数のヤコビ行列（Jacobian）
  • $\nabla f$: 「加速度センサから見て、姿勢をどちらに修正すべきか」を示すベクトル

3.3 Step 3: センサフュージョン（統合）

最後に、Step 1（ジャイロの勢い）と Step 2（加速度による補正）を統合します。

Madgwickフィルタの最大の発明は、「角度」ではなく「角度の変化率（微分値）」の段階でフュージョンを行う点です。

$$
\dot{q}_{est} = \dot{q}_{\omega} - \beta \frac{\nabla f}{||\nabla f||}
$$

  • $\dot{q}_{\omega}$ (赤信号): 「ジャイロ的には、こっちに回転しているはずだ！」
  • $\nabla f$ (青信号): 「いや、重力的に考えると、こっちに修正すべきだ！」
  • $\beta$ (Beta): フィルタゲイン。「加速度の言い分をどれくらい聞くか」を決める重み係数。

数式の意味:
ジャイロが計算した回転速度 $\dot{q}_{\omega}$ から、重力とのズレを解消する成分（誤差勾配）を $\beta$ 倍して引き算しています。
これにより、「ジャイロの素早い動き（高周波成分）」を生かしつつ、「重力方向へのドリフト（低周波成分）」だけを継続的にキャンセルします。

最終的な姿勢更新は、この統合された変化率 $\dot{q}_{est}$ を積分することで行われます。

$$
q_t = q_{t-1} + \dot{q}_{est} \cdot \Delta t
$$

3.4 つまり「Madgwickフィルタ」とは何か？

以上の数式展開をまとめると、Madgwickフィルタの正体は次のように定義できます。

「ジャイロセンサによる『積分の推測』を、加速度センサによる『重力の観測』を使って、勾配降下法でリアルタイムに補正し続けるアルゴリズム」

カルマンフィルタが「確率統計（共分散行列）」を用いて最適解を推定するのに対し、Madgwickフィルタは「幾何学的な誤差の最小化（勾配降下）」によって最適解を探索します。

この手法の最大の利点は、逆行列のような重い線形代数演算が一切不要である点です。四則演算と平方根だけで実装できるため、STM32のような組み込みマイコンでも超高速に動作し、かつカルマンフィルタに匹敵する精度を叩き出すことができるのです。

4. 実装戦略：STM32でのHILS検証

実センサをI2C接続してデバッグを行う場合、不具合が発生した際に「通信エラー」「センサ故障」「アルゴリズムのバグ」の切り分けが困難です。
そこで、STM32内部で仮想信号を生成し、フィルタ処理を実行するコードを実装します。

4.1 プロジェクト構成

  1. MadgwickAHRS.c, MadgwickAHRS.h をプロジェクトに追加。
    github:MadgwickAHRS これを使用しています
  2. main.c に <math.h>, <stdlib.h> をインクルード。
  3. MadgwickAHRS.h に float dt を引数とする関数宣言を追加。
  4. MadgwickAHRS.c の積分部分を修正。

4.2 MadgwickAHRSの改造

オリジナルのMadgwickコードはサンプリング周波数 sampleFreq が固定定数として定義されていることが多く、実機のように処理負荷でループ周期が変動する場合、積分誤差の原因となります。
そこで、経過時間 dt を引数として受け取り、動的に積分計算を行う ように改造します。
MadgwickAHRS.h (修正箇所)

// 引数の末尾に float dt を追加
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt);

MadgwickAHRS.c (修正箇所)

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) {
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // Use IMU algorithm if magnetometer measurement invalid
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt); // こちらも dt 付きで呼ぶ
        return;
    }
// ... (中略: 正規化処理) ...
}
    


// 定義部分も同様に変更
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
    // ... (中略: 勾配降下法の計算) ...

    // 積分計算: 固定周波数(1.0f/sampleFreq)ではなく、計測したdtを直接掛ける
    q0 += qDot1 * dt;
    q1 += qDot2 * dt;
    q2 += qDot3 * dt;
    q3 += qDot4 * dt;

    // ... (中略: 正規化処理) ...
}

4.3 検証コードの実装 (main.c)

main ループ内で高精度タイマーによる dt 計測を行い、ダミーデータを生成してフィルタ処理を実行します。

/* USER CODE BEGIN Includes */
#include "MadgwickAHRS.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
/* USER CODE END Includes */

/* USER CODE BEGIN PV */
extern volatile float beta;
#define BETA_GAIN 0.1f 

float sim_time = 0.0f;
float gyro_integ_rad = 0.0f; // ジャイロ単体積分の比較用
/* USER CODE END PV */

/* USER CODE BEGIN 2 */
// タイマー開始 (TIM2: 1MHz設定を想定)
HAL_TIM_Base_Start(&htim2);

// フィルタ初期化
beta = BETA_GAIN;
uint32_t last_time_us = __HAL_TIM_GET_COUNTER(&htim2);

// CSVヘッダ出力
printf("Time_s, True_Roll, Noisy_Accel_Roll, Madgwick_Roll, Gyro_Only_Roll, dt_ms\r\n");
/* USER CODE END 2 */

/* USER CODE BEGIN 3 */
while (1) {
    // 1. 時間計測 (dt)
    // 高精度タイマーを使って、前回のループからの正確な経過時間を測る
    uint32_t current_time_us = __HAL_TIM_GET_COUNTER(&htim2);
    uint32_t elapsed_us = current_time_us - last_time_us;
    last_time_us = current_time_us;

    float dt = (float)elapsed_us / 1000000.0f;
    if (dt <= 0.0f) dt = 0.001f; // 安全策

    sim_time += dt;

    // 2. ダミーデータ生成 (物理シミュレーション)
    // 真の動き: ±45度 (0.785rad), 0.5Hz の正弦波
    float freq = 0.5f;
    float amplitude = 0.785f; 
    float true_angle_rad = amplitude * sinf(2.0f * 3.14159f * freq * sim_time);

    // 真の角速度: d/dt(angle)
    float true_rate_rad = amplitude * (2.0f * 3.14159f * freq) * cosf(2.0f * 3.14159f * freq * sim_time);

    // ジャイロ入力: 真値 + 定常バイアス誤差 (+0.1 rad/s)
    float gx = true_rate_rad + 0.1f; 
    float gy = 0.0f;
    float gz = 0.0f;

    // 加速度入力: 真値 + 振動ノイズ
    float ax = 0.0f;
    float ay = sinf(true_angle_rad);
    float az = cosf(true_angle_rad);
    float noise = ((float)rand() / RAND_MAX - 0.5f) * 0.4f;
    ay += noise;
    az += noise;

    // 3. フィルタ実行
    // 計測した dt を渡すことで、処理落ちしても積分精度が保たれる
    MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);

    // 4. 比較用データの計算
    gyro_integ_rad += gx * dt; // ジャイロ単体の単純積分

    // 5. 結果出力 (CSV形式)
    // 四元数 -> オイラー角変換
    extern volatile float q0, q1, q2, q3; 
    float roll_rad = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
    
    // 表示用に [deg] に変換
    float roll_deg = roll_rad * 57.29578f;
    float true_deg = true_angle_rad * 57.29578f;
    float acc_deg = atan2f(ay, az) * 57.29578f;
    float gyro_only_deg = gyro_integ_rad * 57.29578f;

    printf("%.3f, %.2f, %.2f, %.2f, %.2f, %.2f\r\n", 
           sim_time, 
           true_deg,       // 黒: 真の値
           acc_deg,        // オレンジ: ノイズ (Accel)
           roll_deg,       // 緑: Madgwick
           gyro_only_deg,  // 水色: ドリフト (Gyro Only)
           dt * 1000.0f);  // ループ周期確認用

    HAL_Delay(10);
}
/* USER CODE END 3 */

5. 結果と考察

STM32から出力されたCSVデータをExcelでグラフ化しました。
HILS検証の結果は以下の通りです。
実行結果:
image.png

グラフの解説

  • 水色 (Gyro Only):
    • 「積分ドリフト」の恐怖 が可視化されています。
    • わずか0.1rad/sのバイアス誤差でも、積分すると時間とともに右肩上がりにズレていき、最終的には画面外へ消えていきます。これでは倒立できません。
  • オレンジ色 (Noisy Accel):
    • 「振動ノイズ」 です。ギザギザしすぎていて、これを微分制御（D項）に使えばモータが暴走します。
  • 緑色 (Madgwick Filter):
    • 完璧です。
    • 加速度のノイズを平滑化しつつ、ジャイロのドリフトも強力に抑え込んでいます。
    • 紺色 (True Roll) の波形にしっかりと追従できています。
  • 検証の成果:
    C言語で実装したMadgwickフィルタが、マイコン上で正しく動作することを確認しました。
    特に、atan2(ay, az) で計算される加速度角度が激しく振動しているのに対し、est_angle (Madgwick出力) はノイズを遮断しつつ、遅れなく追従しています。

残された課題：定常偏差（オフセット）

よく見ると、緑色の線（Madgwick）が、真の値（紺色）より少しだけ上に浮いています。
これは、ジャイロのバイアス（+0.1）と、それを引き戻そうとする加速度補正が釣り合ってしまった結果（定常偏差）です。

対策:
実機では、電源投入直後の静止状態でジャイロの値を読み取り、「0点補正（キャリブレーション）」 を行うことで、このオフセットを完全に消し去ることができます。これは後の「実機調整編」で実装します。

6. まとめ

本日は、実センサを接続する前の段階として、STM32内部での数値シミュレーションによるアルゴリズム検証を行いました。

  • 検証完了: センサ実機がなくても、STM32単体でアルゴリズムが正常動作することを証明できた。
  • ライブラリの罠: オリジナルコードの sampleFreq 固定問題を、dt 引数化することで解決した。

これで、脳（AI）、神経（FOC）、三半規管（Madgwick）のすべてのソフトウェア・パーツが揃いました。

次回予告 [Day 14]

次回は、「SVPWM波形生成の実装と観測」 です。 まだモータも電流センサも繋ぎません。STM32単体でタイマーを叩き、ピンから計算通りの美しい 「鞍型波形（SVPWM）」 が出力されるかをオシロスコープで検証します。

これはいわば、モータドライバにとっての 「Lチカ」 であり、FOC実装における最初にして最大の山場です。理論通りの波形を現実世界に出力できるか？ 波形生成のコード実装に挑みます。

アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?