IMUセンサーフュージョンをPythonで実装する｜加速度計とジャイロから傾きを求める(相補フィルタ)

センサー超入門で「IMUはロボットの三半規管」と紹介しました。ですが実際のIMU（加速度計+ジャイロ）は、そのままでは使い物にならないほどクセの強いセンサーです。加速度計は振動でブレるし、ジャイロは積分すればするほど値がズレていきます。この記事では、この2つの弱点を補い合う**相補フィルタ（Complementary Filter）**をPythonでゼロから実装し、「なぜ混ぜると安定するのか」を数値で確かめます。

加速度計とジャイロ、それぞれの弱点

加速度計は「重力がどちら向きに感じられるか」から傾き角度を逆算できます。静止していれば正確ですが、ロボットが動いて振動が加わると、重力以外の加速度まで拾ってしまい、値が暴れます。一方ジャイロは角速度（回転の速さ）しか測れないため、傾き角度を得るには時間積分が必要です。ジャイロには必ずわずかな**バイアス（オフセット誤差）**があり、積分するとこのバイアスがそのまま蓄積し続け、時間が経つほど実際の角度からズレていきます¹。

準備

pip install numpy

① シミュレーション用データを作る

本物のIMUの代わりに、「真の傾き角」と、そこにノイズ・バイアスを乗せた加速度計・ジャイロの模擬データを作ります。ロボットは最初静止しており、1秒後にゆっくり15度まで傾いて、そのまま止まる設定です。

import numpy as np
np.random.seed(0)

dt = 0.01          # 100Hzでサンプリング
N = 1000            # 10秒分
t = np.arange(N) * dt

# 真の傾き角(ロール角):静止 -> 1秒かけて15度まで傾く -> 静止
true_angle = np.clip((t - 1.0) * 20, 0, 15)      # deg
true_gyro = np.gradient(true_angle, dt)           # 真の角速度(真の傾きの微分)

# 加速度計:真の傾き + 振動ノイズ
accel_angle_raw = true_angle + np.random.normal(0, 3.0, N)

# ジャイロ:真の角速度 + バイアス(温度等でズレる) + ノイズ
gyro_bias = 1.5  # deg/s
gyro_raw = true_gyro + gyro_bias + np.random.normal(0, 0.3, N)

② 加速度計だけで傾きを求める

静止時なら、加速度計が感じる重力ベクトルの向きから atan2 で傾き角度が求まります。今回はシミュレーションなので直接ノイズ入りの角度として持たせていますが、実機では angle = atan2(ay, az) のような式で計算します。

print(np.round(accel_angle_raw[:8], 2))

[ 5.29  1.2   2.94  6.72  5.6  -2.93  2.85 -0.45]

真の値は最初0度のはずですが、-2.93〜6.72度まで暴れています。加速度計だけでは、1サンプルごとの値をそのまま信じられません。

③ ジャイロだけを積分して傾きを求める

ジャイロは角速度なので、時間で積分（足し合わせ）すれば角度になります。

gyro_only_angle = np.cumsum(gyro_raw) * dt

print("t=2.0s:", round(gyro_only_angle[200], 2), "度  (真値:", round(true_angle[200], 2), ")")
print("t=9.99s:", round(gyro_only_angle[999], 2), "度  (真値:", round(true_angle[999], 2), ")")

t=2.0s: 18.07 度  (真値: 15.0 )
t=9.99s: 30.04 度  (真値: 15.0 )

t=2.0秒の時点ですでに真値より3度ズレ、t=9.99秒では2倍(30度)まで膨らんでいます。ジャイロのバイアス(1.5deg/s)が毎ステップ積分され続け、時間が経つほど誤差が線形に増えていくのがドリフトの正体です。

④ 相補フィルタで合成する

相補フィルタのアイデアはシンプルです。「短時間の変化はジャイロを信じて積分し、長い時間で見た基準（絶対角度）は加速度計で定期的に補正する」——これを1つの式にまとめます。

alpha = 0.98  # ジャイロ(積分)をどれだけ信じるか
comp_angle = np.zeros(N)
comp_angle[0] = accel_angle_raw[0]

for i in range(1, N):
    gyro_estimate = comp_angle[i - 1] + gyro_raw[i] * dt   # ジャイロ側の予測
    comp_angle[i] = alpha * gyro_estimate + (1 - alpha) * accel_angle_raw[i]  # 加速度計で補正

for idx in [0, 100, 200, 500, 999]:
    print(f"t={t[idx]:.2f}s 真値={true_angle[idx]:.2f} "
          f"加速度計={accel_angle_raw[idx]:.2f} ジャイロ積分={gyro_only_angle[idx]:.2f} "
          f"相補フィルタ={comp_angle[idx]:.2f}")

t=0.00s 真値=0.00 加速度計=5.29 ジャイロ積分=0.02 相補フィルタ=5.29
t=1.00s 真値=0.00 加速度計=5.65 ジャイロ積分=1.61 相補フィルタ=1.70
t=2.00s 真値=15.00 加速度計=13.89 ジャイロ積分=18.07 相補フィルタ=15.82
t=5.00s 真値=15.00 加速度計=16.15 ジャイロ積分=22.56 相補フィルタ=15.73
t=9.99s 真値=15.00 加速度計=13.93 ジャイロ積分=30.04 相補フィルタ=16.12

10秒後、ジャイロ単独は真値から15度もズレましたが、相補フィルタは16.12度と真値の15度に近い値を保っています。RMSE（誤差の二乗平均平方根）で比べると差は歴然です。

rmse_accel = np.sqrt(np.mean((accel_angle_raw - true_angle) ** 2))
rmse_gyro = np.sqrt(np.mean((gyro_only_angle - true_angle) ** 2))
rmse_comp = np.sqrt(np.mean((comp_angle - true_angle) ** 2))
print("RMSE 加速度計のみ:", round(rmse_accel, 3))
print("RMSE ジャイロ積分のみ:", round(rmse_gyro, 3))
print("RMSE 相補フィルタ:", round(rmse_comp, 3))

RMSE 加速度計のみ: 2.964
RMSE ジャイロ積分のみ: 8.711
RMSE 相補フィルタ: 1.182

相補フィルタのRMSE(1.182)は、加速度計のみ(2.964)よりもジャイロ積分のみ(8.711)よりも小さくなりました。それぞれ単独では使い物にならなかった2つのセンサーが、混ぜることでどちらより良い結果を生んでいます。

まとめ

• 加速度計は静止時は正確だが、振動でノイズが乗り瞬間的に大きくズレる
• ジャイロは短時間は正確だが、バイアスを積分し続けるとドリフトで誤差が時間とともに線形に増える
• 相補フィルタは「短時間はジャイロ、長時間の基準は加速度計」で重み付き合成し、単独より小さい誤差を実現する
• alpha（重み）はドリフトへの強さと反応の速さのトレードオフ。実機では0.95〜0.99あたりが目安

もう少し詳しく(背景と理論)

相補フィルタは実装が数行で済み計算負荷も小さいため、マイコンでも動く手軽なセンサーフュージョンとして広く使われますが、傾き(ロール・ピッチ)の2軸を別々に扱う簡易版であり、方位(ヨー)まで含めた3次元の姿勢を扱おうとするとジンバルロックなどの問題が出やすいという限界があります。これを解決するのが、姿勢をクォータニオン(4次元の数)で表し、ジャイロによる姿勢予測を加速度計・地磁気センサーの観測で補正するMahonyフィルタ³やMadgwickフィルタ⁴です。どちらも相補フィルタと同じ「予測はジャイロ、補正は他センサー」という発想を、比例積分(PI)補償やこう配降下法という形で洗練させたもので、ドローンの飛行制御ボード(フライトコントローラ)やモーションキャプチャ用IMUの姿勢推定に実際に使われています。より汎用的に複数センサー・複数状態を扱いたい場合は、本サイトのSLAM記事で扱ったパーティクルフィルタと同じ「予測→観測で補正」の枠組みを、正規分布の仮定のもとで最適化した**(拡張)カルマンフィルタ**が使われ、ROS 2エコシステムではrobot_localizationパッケージがIMU・車輪オドメトリ・GPSなどを拡張カルマンフィルタで統合する標準的な手段として使われています⁵。どの手法を選ぶかは、必要な精度・計算資源・扱うセンサーの数で決まり、「まず相補フィルタで動かしてみて、精度が足りなければカルマンフィルタ系に進む」という順番が実務でも一般的です。

次の一歩 🌸

センサー全体の位置づけはセンサー超入門、IMUが命綱になる歩行制御は脚ロボットの歩行、姿勢推定を活かした自己位置推定はSLAMをPythonで実装する、推定した傾きをもとに実際にモーターを動かす制御はPID制御をPythonで実装するへどうぞ。

Footnotes

1. MEMSジャイロのバイアスは温度変化や個体差で数十〜数百 deg/h 単位でずれることがあり、積分するとオフセット誤差がそのまま角度誤差として蓄積する。この現象は一般に「ドリフト」と呼ばれる。 ↩

2. 相補フィルタの重みは、対象とするノイズ・ドリフトの大きさやサンプリング周波数によって最適値が変わるため、実機では加速度計のノイズ量とジャイロのバイアス安定度を見ながら経験的に調整するのが一般的。 ↩

3. Mahony, R., Hamel, T., & Pflimlin, J.-M. (2008). "Nonlinear Complementary Filters on the Special Orthogonal Group." IEEE Transactions on Automatic Control, 53(5), 1203–1218. 比例積分(PI)補償によりジャイロの予測を加速度計・地磁気センサーの観測で補正する非線形相補フィルタを提案。 ↩

4. Madgwick, S. O. H., Harrison, A. J. L., & Vaidyanathan, R. (2011). "Estimation of IMU and MARG orientation using a gradient descent algorithm." IEEE International Conference on Rehabilitation Robotics (ICORR). こう配降下法を使い、クォータニオンで表した姿勢を加速度計・地磁気センサーの誤差最小化方向へ補正する手法を提案。 ↩

5. ROS 2のrobot_localizationパッケージは拡張カルマンフィルタ／無香料カルマンフィルタを用いて、IMU・車輪オドメトリ・GPSなど複数のセンサー入力を融合し、ロボットの姿勢・位置を推定する標準的な手段として広く使われている。 ↩