IMUセンサー入門：基礎から応用まで
はじめに
IMU（Inertial Measurement Unit：慣性計測装置）は、物体の動きや姿勢を計測するためのセンサーデバイスです。ロボット工学、ドローン、スマートフォン、VR/ARデバイスなど、様々な分野で活用されています。
IMUとは
IMUは以下の主要なセンサーを組み合わせたものです：
- 加速度センサー：直線運動の加速度を検出
- ジャイロスコープ：角速度（回転の速さ）を検出
- 磁気センサー（オプション）：方位を検出
主な用途
ロボティクス
- 自己位置推定
- 姿勢制御
- 動作安定化
モバイルデバイス
- 画面の向き検出
- ステップカウント
- ナビゲーション補助
ドローン
- 飛行安定化
- 自動制御
- ホバリング制御
センサーの基本仕様
一般的なIMUセンサーの仕様：
- 加速度センサー：±2g～±16g
- ジャイロスコープ：±250～±2000度/秒
- サンプリングレート：100Hz～1000Hz
データの取得方法
一般的なIMUセンサーは以下のインターフェースでデータを取得できます：
- I2C
- SPI
- UART
# MPU6050を使用した基本的なデータ取得例 from mpu6050 import mpu6050 from time import sleep # センサーの初期化 sensor = mpu6050(address=0x68) # データ読み取り accel_data = sensor.get_accel_data() gyro_data = sensor.get_gyro_data() print(f"加速度: {accel_data}") print(f"角速度: {gyro_data}")
キャリブレーション
IMUセンサーの正確な測定には、適切なキャリブレーションが不可欠です。
1. キャリブレーションの理論
1.1 加速度センサーのキャリブレーション
加速度センサーの出力は以下のモデルで表現できます：
acc_measured = S * acc_true + B + N
ここで：
- acc_measured: 測定値
- S: スケール係数行列（3x3）
- acc_true: 真の加速度
- B: バイアス（オフセット）ベクトル
- N: ノイズ
1.2 ジャイロセンサーのキャリブレーション
ジャイロセンサーの出力は以下のモデルで表現できます：
gyro_measured = S * gyro_true + B + B_t(T) + N
ここで：
- gyro_measured: 測定値
- S: スケール係数行列
- gyro_true: 真の角速度
- B: 静的バイアス
- B_t(T): 温度依存バイアス
- N: ノイズ
2. キャリブレーションの実装
import numpy as np from typing import Tuple, List from dataclasses import dataclass @dataclass class CalibrationConfig: """キャリブレーション設定""" num_static_samples: int = 1000 # 静止状態でのサンプル数 num_rotation_samples: int = 2000 # 回転時のサンプル数 temperature_samples: int = 500 # 温度キャリブレーション用サンプル数 @dataclass class CalibrationResult: """キャリブレーション結果""" acc_scale: np.ndarray # 加速度スケール係数行列 acc_bias: np.ndarray # 加速度バイアスベクトル gyro_scale: np.ndarray # ジャイロスケール係数行列 gyro_bias: np.ndarray # ジャイロバイアスベクトル temp_coef: np.ndarray # 温度係数 class IMUCalibrator: def __init__(self, config: CalibrationConfig = CalibrationConfig()): self.config = config self.reference_gravity = 9.81 # m/s^2 def calibrate_accelerometer( self, static_samples: List[np.ndarray] ) -> Tuple[np.ndarray, np.ndarray]: """加速度センサーのキャリブレーション Args: static_samples: 6つの異なる静止姿勢での測定値 （上下左右前後） Returns: scale_matrix: スケール係数行列 bias_vector: バイアスベクトル """ # 測定値行列の作成 measurements = np.array(static_samples) # 理想値行列の作成（重力加速度の基準値） reference = np.array([ [0, 0, self.reference_gravity], # 上向き [0, 0, -self.reference_gravity], # 下向き [0, self.reference_gravity, 0], # 左向き [0, -self.reference_gravity, 0], # 右向き [self.reference_gravity, 0, 0], # 前向き [-self.reference_gravity, 0, 0] # 後向き ]) # 最小二乗法でスケール係数とバイアスを推定 A = np.vstack([measurements, np.ones((6, 3))]) b = reference x, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None) # 結果の分離 scale_matrix = x[:-1] bias_vector = x[-1] return scale_matrix, bias_vector def calibrate_gyroscope( self, static_samples: List[np.ndarray], rotation_samples: List[Tuple[np.ndarray, float]] ) -> Tuple[np.ndarray, np.ndarray]: """ジャイロセンサーのキャリブレーション Args: static_samples: 静止状態での測定値 rotation_samples: 既知の角速度での回転時の測定値と 真の角速度のペア Returns: scale_matrix: スケール係数行列 bias_vector: バイアスベクトル """ # 静止状態でのバイアス推定 static_data = np.array(static_samples) bias_vector = np.mean(static_data, axis=0) # 回転データからスケール係数を推定 measured_rates = np.array([sample[0] for sample in rotation_samples]) true_rates = np.array([sample[1] for sample in rotation_samples]) # バイアス補正済みの測定値 corrected_rates = measured_rates - bias_vector # スケール係数の推定（対角行列を仮定） scale_matrix = np.diag( np.mean(true_rates / corrected_rates, axis=0) ) return scale_matrix, bias_vector def calibrate_temperature( self, temp_samples: List[Tuple[np.ndarray, float]] ) -> np.ndarray: """温度依存性のキャリブレーション Args: temp_samples: ジャイロ測定値と温度のペア Returns: temp_coef: 温度係数（3次元ベクトル） """ # データの準備 gyro_data = np.array([sample[0] for sample in temp_samples]) temp_data = np.array([sample[1] for sample in temp_samples]) # 温度と出力の関係を線形回帰 A = np.vstack([temp_data, np.ones(len(temp_data))]).T temp_coef = np.linalg.lstsq(A, gyro_data, rcond=None)[0] return temp_coef def apply_calibration( self, raw_data: np.ndarray, cal_result: CalibrationResult, temperature: float = None ) -> np.ndarray: """キャリブレーション結果を適用 Args: raw_data: 生データ（nx6行列、加速度とジャイロ） cal_result: キャリブレーション結果 temperature: 温度データ（オプション） Returns: calibrated_data: キャリブレーション済みデータ """ # データの分離 acc_data = raw_data[:, :3] gyro_data = raw_data[:, 3:] # 加速度データの補正 acc_calibrated = np.dot( acc_data - cal_result.acc_bias, cal_result.acc_scale ) # ジャイロデータの補正 gyro_calibrated = np.dot( gyro_data - cal_result.gyro_bias, cal_result.gyro_scale ) # 温度補正（温度データがある場合） if temperature is not None: temp_correction = cal_result.temp_coef * temperature gyro_calibrated -= temp_correction # 結果の結合 return np.hstack([acc_calibrated, gyro_calibrated]) # 使用例 def calibration_example(): # キャリブレータの初期化 calibrator = IMUCalibrator() # サンプルデータの生成（実際のデータに置き換えてください） static_acc_samples = [ np.array([0.1, 0.2, 9.7]), # 上向き np.array([0.2, 0.1, -9.9]), # 下向き np.array([0.1, 9.8, 0.2]), # 左向き np.array([0.2, -9.7, 0.1]), # 右向き np.array([9.8, 0.1, 0.2]), # 前向き np.array([-9.9, 0.2, 0.1]) # 後向き ] # キャリブレーションの実行 acc_scale, acc_bias = calibrator.calibrate_accelerometer( static_acc_samples ) print("加速度センサーキャリブレーション結果:") print(f"スケール係数:\n{acc_scale}") print(f"バイアス: {acc_bias}")
3. キャリブレーションの手順
静的キャリブレーション
- センサーを完全に静止させた状態で測定
- バイアスとノイズレベルの推定
- 複数の姿勢で測定してスケール係数を推定
動的キャリブレーション
- 既知の角速度で回転させて測定
- ジャイロセンサーのスケール係数を推定
- クロスアキシス感度の評価
90度回転による簡易キャリブレーション
class SimpleGyroCalibrator: def __init__(self): self.sampling_rate = 100 # Hz self.target_angle = np.pi/2 # 90度 def calibrate_with_90deg_rotation( self, gyro_data: np.ndarray, axis: int ) -> float: """ジャイロデータの90度回転によるスケール係数の推定 Args: gyro_data: ジャイロデータ（Nx3行列） axis: 回転軸（0=X, 1=Y, 2=Z） Returns: scale_factor: スケール係数 """ # 角速度の積分 dt = 1.0 / self.sampling_rate integrated_angle = np.sum(gyro_data[:, axis]) * dt # スケール係数の計算 scale_factor = self.target_angle / integrated_angle return scale_factor def perform_calibration(self): """キャリブレーション手順の例""" print("ジャイロセンサーの90度回転キャリブレーション") print("手順:") print("1. センサーを水平な面に設置") print("2. X軸回りに90度回転し、データを記録") print("3. Y軸回りに90度回転し、データを記録") print("4. Z軸回りに90度回転し、データを記録") # 実装例 scale_factors = [] for axis in range(3): input(f"{['X', 'Y', 'Z'][axis]}軸の90度回転を完了したらEnterを押してください") # ここで実際のデータ取得を行う # 例としてダミーデータを使用 dummy_data = np.array([[0.1, 0.0, 0.0]] * 100) scale_factor = self.calibrate_with_90deg_rotation(dummy_data, axis) scale_factors.append(scale_factor) return np.diag(scale_factors)
この手法の特徴： - 特別な装置が不要 - 手動で正確に90度回転 - 各軸のスケール係数を個別に推定 - 回転速度は任意（積分値で評価）
注意点： - 回転はできるだけ正確に90度に - 回転中は他の軸の動きを最小限に - 各軸のキャリブレーションを複数回実施して平均を取る
- 温度キャリブレーション
- 異なる温度環境で測定
- 温度依存性の係数を推定
- 温度補正モデルの作成
4. キャリブレーション時の注意点
環境条件
- 振動のない安定した場所で実施
- 温度が安定した環境で実施
- 磁気の影響を受けない場所で実施
測定手順
- 十分な数のサンプルを取得
- 各姿勢で安定するまで待機
- 温度変化は緩やかに実施
データ処理
- 外れ値の除去
- ノイズフィルタリング
- 結果の妥当性確認
定期的な再キャリブレーション
- センサー特性の経時変化に注意
- 環境変化への対応
- 定期的な精度確認
5. 地球の自転の影響と補正
高精度なIMUセンサーでは、地球の自転角速度（約 0.004°/s または 7.27×10⁻⁵ rad/s）を検知できます。
class EarthRotationCompensator: def __init__(self, latitude: float): """ Args: latitude: 緯度（度単位） """ self.earth_rotation_rate = 7.2921150e-5 # rad/s self.latitude_rad = np.radians(latitude) def get_earth_rotation_rates(self) -> np.ndarray: """各軸における地球の自転角速度を計算 Returns: np.ndarray: [x, y, z]軸方向の地球自転角速度 [rad/s] """ # 北方向（X軸）の成分 omega_x = self.earth_rotation_rate * np.cos(self.latitude_rad) # 東方向（Y軸）の成分 omega_y = 0.0 # 上方向（Z軸）の成分 omega_z = self.earth_rotation_rate * np.sin(self.latitude_rad) return np.array([omega_x, omega_y, omega_z]) def compensate_gyro_data( self, gyro_data: np.ndarray, orientation: np.ndarray = None ) -> np.ndarray: """ジャイロデータから地球の自転の影響を除去 Args: gyro_data: ジャイロデータ（Nx3行列） orientation: 現在の姿勢（オイラー角）。Noneの場合、 センサーが水平に設置されていると仮定。 Returns: np.ndarray: 補正済みのジャイロデータ """ earth_rotation = self.get_earth_rotation_rates() if orientation is not None: # 姿勢から回転行列を計算 roll, pitch, yaw = orientation R = self.euler_to_rotation_matrix(roll, pitch, yaw) # 地球の自転をセンサー座標系に変換 earth_rotation = R @ earth_rotation # 地球の自転の影響を除去 compensated_data = gyro_data - earth_rotation return compensated_data @staticmethod def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: """オイラー角から回転行列を計算""" # 各軸の回転行列 Rx = np.array([ [1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) Ry = np.array([ [np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)] ]) Rz = np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) # ZYX順の回転行列の合成 R = Rz @ Ry @ Rx return R # 使用例 def earth_rotation_example(): # 東京の緯度（35.6762° N）での補正例 compensator = EarthRotationCompensator(latitude=35.6762) # 地球の自転角速度の計算 earth_rates = compensator.get_earth_rotation_rates() print("地球の自転角速度（rad/s）:") print(f"X軸（北方向）: {earth_rates[0]:.9f}") print(f"Y軸（東方向）: {earth_rates[1]:.9f}") print(f"Z軸（上方向）: {earth_rates[2]:.9f}") # ジャイロデータの補正例 # ダミーデータを使用 gyro_data = np.array([ [0.0001, 0.0, 0.0001], # rad/s [0.0001, 0.0, 0.0001], [0.0001, 0.0, 0.0001] ]) # 水平設置の場合 compensated_data = compensator.compensate_gyro_data(gyro_data) # 任意の姿勢の場合 orientation = np.array([np.pi/6, np.pi/4, np.pi/3]) # オイラー角 compensated_data_with_orientation = compensator.compensate_gyro_data( gyro_data, orientation ) print("\n補正前のジャイロデータ:") print(gyro_data[0]) print("\n補正後のジャイロデータ（水平設置）:") print(compensated_data[0])
地球の自転の影響を考慮する上での重要なポイント：
緯度依存性
- 自転の影響は緯度によって変化
- 赤道付近では水平成分が最大
- 極地付近では垂直成分が最大
センサーの姿勢
- センサーの姿勢によって地球自転の影響が変化
- 正確な補正には現在の姿勢を考慮する必要
センサーの精度
- 地球の自転角速度は非常に小さい
- 高精度なセンサーでないと検知が困難
- ノイズやドリフトの影響を考慮する必要
応用例
- 慣性航法システム
- 高精度な姿勢推定
- 地球物理学的な測定
姿勢推定の基礎
座標系
- 慣性座標系（固定座標系）: 地球に固定された座標系
- ボディ座標系（移動座標系）: センサーに固定された座標系
姿勢表現方法
ZYXオイラー角
- ヨー角（ψ）: Z軸周りの回転
- ピッチ角（θ）: Y軸周りの回転
- ロール角（φ）: X軸周りの回転
クォータニオン
- 4つのパラメータ（q0, q1, q2, q3）で3次元の回転を表現
- ジンバルロックを回避可能
- 計算効率が良い
姿勢推定の実装
1. 加速度センサーによる姿勢推定
加速度センサーは重力加速度を検出できるため、静止時の姿勢推定に使用できます。
import numpy as np from math import atan2, sqrt def estimate_attitude_from_accel(ax, ay, az): """ 加速度データからオイラー角を推定 Parameters: ax, ay, az: 各軸の加速度 [m/s^2] Returns: roll, pitch: ロール角、ピッチ角 [rad] """ # ロール角の計算 roll = atan2(ay, sqrt(ax*ax + az*az)) # ピッチ角の計算 pitch = atan2(-ax, sqrt(ay*ay + az*az)) return roll, pitch # ヨー角は加速度センサーのみでは求められない
2. ジャイロセンサーによる姿勢推定
2.1 オイラー角による実装
import numpy as np from math import sin, cos class EulerAttitudeEstimator: def __init__(self): self.roll = 0.0 # ロール角 [rad] self.pitch = 0.0 # ピッチ角 [rad] self.yaw = 0.0 # ヨー角 [rad] def update(self, wx, wy, wz, dt): """ 角速度データからオイラー角を更新 Parameters: wx, wy, wz: 各軸の角速度 [rad/s] dt: 時間間隔 [s] """ # オイラー角の変化率 roll_dot = wx + sin(self.roll) * tan(self.pitch) * wy + \ cos(self.roll) * tan(self.pitch) * wz pitch_dot = cos(self.roll) * wy - sin(self.roll) * wz yaw_dot = sin(self.roll) / cos(self.pitch) * wy + \ cos(self.roll) / cos(self.pitch) * wz # オイラー角の更新（積分） self.roll += roll_dot * dt self.pitch += pitch_dot * dt self.yaw += yaw_dot * dt
2.2 クォータニオンによる実装
クォータニオンは以下の形式で表現します：
q = (q0, q1, q2, q3)
ここで： - q0: スカラー部（実部） - (q1, q2, q3): ベクトル部（虚部）で回転軸ベクトルを表す
import numpy as np class QuaternionAttitudeEstimator: def __init__(self): # クォータニオン初期化 [q0, q1, q2, q3] self.q = np.array([1.0, 0.0, 0.0, 0.0]) def update(self, gyro: np.ndarray, dt: float): """ 角速度データからクォータニオンを更新 Args: gyro: 角速度ベクトル [rad/s] (x, y, z) dt: 時間間隔 [s] """ # 角速度ベクトルの大きさ w_norm = np.linalg.norm(gyro) if w_norm > 1e-10: # ゼロ除算を防ぐ # 回転軸の単位ベクトル rotation_axis = gyro / w_norm # 回転角 rotation_angle = w_norm * dt # 回転のクォータニオン dq = np.array([ np.cos(rotation_angle/2), rotation_axis[0] * np.sin(rotation_angle/2), rotation_axis[1] * np.sin(rotation_angle/2), rotation_axis[2] * np.sin(rotation_angle/2) ]) # クォータニオンの更新（クォータニオン積） self.q = self._quaternion_multiply(self.q, dq) # 正規化 self.q = self.q / np.linalg.norm(self.q) @staticmethod def _quaternion_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: """クォータニオンの積を計算 Args: q1: 1番目のクォータニオン [q0, q1, q2, q3] q2: 2番目のクォータニオン [q0, q1, q2, q3] Returns: np.ndarray: クォータニオンの積 [q0, q1, q2, q3] """ """クォータニオンの積を計算""" q10, q11, q12, q13 = q1 q20, q21, q22, q23 = q2 return np.array([ q10*q20 - q11*q21 - q12*q22 - q13*q23, q10*q21 + q11*q20 + q12*q23 - q13*q22, q10*q22 - q11*q23 + q12*q20 + q13*q21, q10*q23 + q11*q22 - q12*q21 + q13*q20 ]) def to_euler(self): """クォータニオンからオイラー角への変換""" q0, q1, q2, q3 = self.q # ロール（X軸周り） roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) # ピッチ（Y軸周り） pitch = asin(2*(q0*q2 - q3*q1)) # ヨー（Z軸周り） yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) return roll, pitch, yaw
3. 実装例
3.1 仮想IMUを使用した実装
import numpy as np import time from dataclasses import dataclass from typing import Tuple @dataclass class IMUData: acc_x: float acc_y: float acc_z: float gyro_x: float gyro_y: float gyro_z: float class VirtualIMU: def __init__(self, noise_std: float = 0.01, dt: float = 0.01): """仮想IMUセンサーの初期化 Args: noise_std: センサーノイズの標準偏差 dt: シミュレーションの時間間隔 """ self.orientation = np.zeros(3) # [roll, pitch, yaw] self.noise_std = noise_std self.dt = dt self.gravity = np.array([0.0, 0.0, 9.81]) def get_data(self) -> IMUData: """センサーデータの取得 Returns: IMUData: 加速度と角速度のデータ """ # 角速度（ゆっくりとした回転を仮定） t = time.time() gyro = np.array([ 0.01 * np.sin(t), 0.01 * np.cos(t), 0.01 * np.sin(2 * t) ]) + np.random.normal(0, self.noise_std, size=3) # 姿勢を更新 self.orientation += gyro * self.dt # 加速度（重力+ノイズ） acc = self.gravity + np.random.normal(0, self.noise_std, size=3) return IMUData( acc_x=acc[0], acc_y=acc[1], acc_z=acc[2], gyro_x=gyro[0], gyro_y=gyro[1], gyro_z=gyro[2] ) # メイン処理 def main_virtual(duration: float = 10.0, dt: float = 0.1): """仮想IMUによる姿勢推定のデモ Args: duration: シミュレーション時間 [秒] dt: サンプリング間隔 [秒] """ # センサーと推定器の初期化 imu = VirtualIMU(noise_std=0.01, dt=dt) euler_estimator = EulerAttitudeEstimator() quat_estimator = QuaternionAttitudeEstimator() # シミュレーションの開始時刻 start_time = time.time() try: while time.time() - start_time < duration: # センサーデータの取得 imu_data = imu.get_data() # ジャイロデータによる姿勢推定 euler_estimator.update( imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z, dt ) quat_estimator.update( imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z, dt ) # 加速度データによる姿勢推定 roll, pitch = estimate_attitude_from_accel( imu_data.acc_x, imu_data.acc_y, imu_data.acc_z ) # 結果の表示 print("\n=== 仮想IMUによる姿勢推定 ===") print(f"Elapsed Time: {time.time() - start_time:.1f}s") print("オイラー角による推定:") print(f"Roll: {np.degrees(euler_estimator.roll):.1f}°") print(f"Pitch: {np.degrees(euler_estimator.pitch):.1f}°") print(f"Yaw: {np.degrees(euler_estimator.yaw):.1f}°") time.sleep(dt) except KeyboardInterrupt: print("\n処理を終了します")
3.2 CSVファイルからのデータ読み込みと処理
import pandas as pd from pathlib import Path def process_csv_data(csv_path: Path): """CSVファイルからIMUデータを読み込んで処理""" # CSVファイルの読み込み df = pd.read_csv(csv_path) # 推定器の初期化 euler_estimator = EulerAttitudeEstimator() quat_estimator = QuaternionAttitudeEstimator() # 結果を格納するリスト results = [] # データを順次処理 for i in range(1, len(df)): # 時間間隔の計算 dt = df.iloc[i]['timestamp'] - df.iloc[i-1]['timestamp'] # 現在のデータ current_data = df.iloc[i] # ジャイロデータによる姿勢推定 euler_estimator.update( current_data['gyro_x'], current_data['gyro_y'], current_data['gyro_z'], dt ) quat_estimator.update( current_data['gyro_x'], current_data['gyro_y'], current_data['gyro_z'], dt ) # 加速度データによる姿勢推定 roll, pitch = estimate_attitude_from_accel( current_data['acc_x'], current_data['acc_y'], current_data['acc_z'] ) # 結果を保存 euler_angles = { 'timestamp': current_data['timestamp'], 'euler_roll': np.degrees(euler_estimator.roll), 'euler_pitch': np.degrees(euler_estimator.pitch), 'euler_yaw': np.degrees(euler_estimator.yaw), 'accel_roll': np.degrees(roll), 'accel_pitch': np.degrees(pitch) } results.append(euler_angles) # 結果をDataFrameに変換 results_df = pd.DataFrame(results) return results_df def main_csv(): # CSVファイルのパス csv_path = Path('sample_imu_data.csv') # データの処理 results = process_csv_data(csv_path) # 結果の表示 print("\n=== CSVデータによる姿勢推定結果 ===") print(results) # 結果の保存（オプション） results.to_csv('attitude_estimation_results.csv', index=False) if __name__ == '__main__': print("1: 仮想IMUによる実時間処理") print("2: CSVファイルからのデータ処理") choice = input("処理モードを選択してください (1/2): ") if choice == '1': main_virtual() elif choice == '2': main_csv() else: print("無効な選択です")
センサー性能の分析
アラン分散（Allan Variance）
アラン分散はセンサーの安定性やノイズ特性を評価するための手法です。異なる時間スケールでのセンサーの安定性を評価できます。
def calculate_allan_variance(data: np.ndarray, fs: float, max_clusters: int = 100) -> tuple: """アラン分散を計算 Args: data: センサーデータ fs: サンプリング周波数[Hz] max_clusters: 最大クラスタ数 Returns: tau: 時間スケール avar: アラン分散 """ n = len(data) # クラスタサイズの設定 clusters = np.logspace(0, np.log10(n/2), max_clusters).astype(int) clusters = np.unique(clusters) tau = clusters / fs avar = np.zeros(len(clusters)) for i, k in enumerate(clusters): # k個のデータ点で平均を取る n_groups = int(n / k) if n_groups < 2: break # データをk個ずつのグループに分割 groups = np.array_split(data[:n_groups*k], n_groups) group_means = np.array([np.mean(group) for group in groups]) # アラン分散の計算 avar[i] = np.sum(np.diff(group_means)**2) / (2 * (n_groups-1)) return tau[:i], np.sqrt(avar[:i]) # アラン偏差を返す def plot_allan_deviation(tau: np.ndarray, adev: np.ndarray, title: str = ''): """アラン偏差をプロット Args: tau: 時間スケール adev: アラン偏差 title: タイトル """ plt.figure(figsize=(10, 6)) plt.loglog(tau, adev) plt.grid(True) plt.xlabel('平均時間 τ [s]') if title.lower().find('gyro') != -1: plt.ylabel('角速度アラン偏差 σ(τ) [deg/s]') elif title.lower().find('acc') != -1: plt.ylabel('加速度アラン偏差 σ(τ) [m/s²]') else: plt.ylabel('アラン偏差 σ(τ)') if title: plt.title(title) plt.show()
アラン分散の読み方
アラン偏差の単位は入力データの単位と同じになりますが、目的に応じて適切な単位に変換して表示します：
- 角速度センサー：
- 短時間の安定性評価： deg/s または rad/s
- 長時間のドリフト評価： deg/h または rad/h
- 加速度センサー：
- 動的特性評価： m/s²
- 静的特性評価： μg または mg
単位変換例：
# deg/s から deg/h への変換 adev_deg_h = adev_deg_s * 3600 # 3600秒 = 1時間 # m/s² から μg への変換 adev_ug = adev_ms2 * 1e6 / 9.81 # 1g = 9.81 m/s²
アラン分散とアラン偏差の関係
重要な注意点として、アラン分散（Allan Variance）とアラン偏差（Allan Deviation）の違いを理解する必要があります：
- アラン偏差はアラン分散の平方根です
- 多くの文献ではグラフ化にアラン偏差を使用しています
- そのため、グラフの傾きの解釈が異なることに注意が必要です
ノイズ特性の詳細
| ノイズ種別 | アラン分散の傾き (log-log) |
アラン偏差の傾き (log-log) |
主に現れるτの範囲（目安） | 特徴・意味 |
|---|---|---|---|---|
| 量子化ノイズ (Quantization Noise) |
-2 | -1 | 最も短いτ（~0.01～0.1秒） | デジタル変換時の丸め誤差。非常に高速に観測される |
| 角度ランダムウォーク (Angle Random Walk) |
-1 | -0.5 | 短いτ（~0.1～1秒） | 角速度の短時間ノイズ。IMUの精度を支配する成分 |
| バイアス不安定性 (Bias Instability) |
0 | 0 | 中間 τ（~1～10秒） | ゆっくり変動するオフセット。グラフの谷に現れる |
| レートランダムウォーク (Rate Random Walk) |
+1 | +0.5 | 長いτ（~10～100秒以上） | 長時間でのドリフト傾向。IMUがずれる原因 |
| 角加速度ランダムウォーク (Angular Accel. RW) |
+3 | +1.5 | 極めて長いτ（数百秒以上） | 加速度の変化がノイズに与える影響。通常はあまり現れない |
これらの特性を理解することで、センサーの適切な使用方法や必要なフィルタ処理を判断できます。また、センサーの品質評価や比較にも役立ちます。
実装における注意点
センサーの特性と課題
- ジャイロセンサーの単純積分ではドリフトが避けられない
- 加速度センサーは動的な状況での正確な姿勢推定が困難
- 各センサーの短所を補うフィルタアルゴリズムが必要
フィルタアルゴリズムの選択
- 拡張カルマンフィルタ：確率的アプローチで高精度な推定が可能
- Madgwickフィルタ：効率的な勾配降下法で安定した推定が可能
- 相補フィルタ：ハイパス・ローパスフィルタの組み合わせでドリフトとノイズを除去
座標系の考慮
- オイラー角では特定の姿勢でジンバルロックが発生
- クォータニオンを使用することで特異点を回避可能
高度な姿勢推定フィルタ
1. 拡張カルマンフィルタ（Extended Kalman Filter: EKF）
拡張カルマンフィルタは非線形システムに対応したカルマンフィルタの拡張版です。
class IMU_EKF: def __init__(self): # 状態ベクトル: [q0, q1, q2, q3, bx, by, bz] self.x = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 共分散行列 self.P = np.eye(7) * 0.1 # プロセスノイズ self.Q = np.eye(7) * 0.001 # 観測ノイズ self.R = np.eye(6) * 0.1 # 重力加速度 self.gravity = np.array([0, 0, 9.81]) def predict(self, gyro: np.ndarray, dt: float): """予測ステップ""" # ジャイロバイアスを考慮した角速度 wx = gyro[0] - self.x[4] wy = gyro[1] - self.x[5] wz = gyro[2] - self.x[6] # 状態遷移行列の計算 F = self._compute_state_transition(wx, wy, wz, dt) # 状態予測 self.x[:4] = self._quaternion_update(self.x[:4], wx, wy, wz, dt) self.P = F @ self.P @ F.T + self.Q def update(self, acc: np.ndarray, mag: np.ndarray = None): """更新ステップ""" # 観測行列とイノベーションの計算 H = self._compute_observation_matrix() z = self._compute_measurement(acc, mag) y = z - self._compute_expected_measurement() # カルマンゲインの計算 S = H @ self.P @ H.T + self.R K = self.P @ H.T @ np.linalg.inv(S) # 状態と共分散の更新 self.x = self.x + K @ y self.P = (np.eye(7) - K @ H) @ self.P # クォータニオンの正規化 q_norm = np.linalg.norm(self.x[:4]) self.x[:4] /= q_norm def _compute_state_transition(self, wx: float, wy: float, wz: float, dt: float) -> np.ndarray: """状態遷移行列の計算""" q0, q1, q2, q3 = self.x[:4] F = np.eye(7) # クォータニオンの状態遷移 F[0:4, 0:4] = np.array([ [1, -wx*dt/2, -wy*dt/2, -wz*dt/2], [wx*dt/2, 1, wz*dt/2, -wy*dt/2], [wy*dt/2, -wz*dt/2, 1, wx*dt/2], [wz*dt/2, wy*dt/2, -wx*dt/2, 1] ]) # バイアスの状態遷移 F[0:4, 4:7] = np.array([ [q1*dt/2, q2*dt/2, q3*dt/2], [-q0*dt/2, q3*dt/2, -q2*dt/2], [-q3*dt/2, -q0*dt/2, q1*dt/2], [q2*dt/2, -q1*dt/2, -q0*dt/2] ]) return F def _quaternion_update(self, q: np.ndarray, wx: float, wy: float, wz: float, dt: float) -> np.ndarray: """クォータニオンの更新""" omega = np.array([ [0, -wx, -wy, -wz], [wx, 0, wz, -wy], [wy, -wz, 0, wx], [wz, wy, -wx, 0] ]) return q + 0.5 * omega @ q * dt def _compute_observation_matrix(self) -> np.ndarray: """観測行列の計算""" q0, q1, q2, q3 = self.x[:4] H = np.zeros((6, 7)) # 加速度に関するヤコビアン H[0:3, 0:4] = np.array([ [2*(q0*q2 - q1*q3), 2*(q1*q2 + q0*q3), 1-2*(q1**2 + q3**2), 2*(q2*q3 - q0*q1)], [2*(q1*q3 + q0*q2), 1-2*(q0**2 + q3**2), 2*(q2*q3 - q0*q1), 2*(q1*q2 - q0*q3)], [1-2*(q1**2 + q2**2), 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2), 2*(q2*q3 + q0*q1)] ]) * self.gravity[2] return H def _compute_measurement(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray: """観測値の計算""" z = np.zeros(6) z[:3] = acc if mag is not None: z[3:] = mag return z def _compute_expected_measurement(self) -> np.ndarray: """期待観測値の計算""" q0, q1, q2, q3 = self.x[:4] h = np.zeros(6) # 加速度の期待値 R = self._quaternion_to_rotation_matrix(q0, q1, q2, q3) h[:3] = R @ self.gravity return h @staticmethod def _quaternion_to_rotation_matrix(q0: float, q1: float, q2: float, q3: float) -> np.ndarray: """クォータニオンから回転行列への変換""" return np.array([ [1-2*(q2**2 + q3**2), 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], [2*(q1*q2 + q0*q3), 1-2*(q1**2 + q3**2), 2*(q2*q3 - q0*q1)], [2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), 1-2*(q1**2 + q2**2)] ])
特徴： - 確率的なアプローチでノイズを処理 - ジャイロバイアスの推定が可能 - 計算コストが比較的高い
2. Madgwickフィルタ
Madgwickフィルタは効率的な勾配降下法に基づくフィルタです。
class MadgwickFilter: def __init__(self, beta=0.1): self.beta = beta # 勾配降下法のゲイン self.q = np.array([1.0, 0.0, 0.0, 0.0]) # クォータニオン (q0, q1, q2, q3) def update(self, gyro: np.ndarray, acc: np.ndarray, dt: float): """フィルタ更新""" # 加速度の正規化 if np.linalg.norm(acc) == 0: return if np.linalg.norm(acc) > 0: acc = acc / np.linalg.norm(acc) # 現在のクォータニオンの分解 q0, q1, q2, q3 = self.q # 事前計算 _2q0 = 2.0 * q0 _2q1 = 2.0 * q1 _2q2 = 2.0 * q2 _2q3 = 2.0 * q3 # 勾配降下法の目的関数の勾配 grad = np.array([ _2qy * (2.0 * (qx * qz - qw * qy) - acc[0]) + _2qz * (2.0 * (qw * qx + qy * qz) - acc[1]) + _2qx * (2.0 * (0.5 - qx * qx - qy * qy) - acc[2]), _2qz * (2.0 * (qx * qz - qw * qy) - acc[0]) + _2qy * (2.0 * (qw * qx + qy * qz) - acc[1]) + _2qw * (2.0 * (0.5 - qx * qx - qy * qy) - acc[2]), _2qx * (2.0 * (qx * qz - qw * qy) - acc[0]) + _2qw * (2.0 * (qw * qx + qy * qz) - acc[1]) + _2qz * (2.0 * (0.5 - qx * qx - qy * qy) - acc[2]) ]) if np.linalg.norm(grad) > 0: grad = grad / np.linalg.norm(grad) # クォータニオンの変化率 qDot = 0.5 * np.array([ -q1 * gyro[0] - q2 * gyro[1] - q3 * gyro[2], q0 * gyro[0] + q2 * gyro[2] - q3 * gyro[1], q0 * gyro[1] - q1 * gyro[2] + q3 * gyro[0], q0 * gyro[2] + q1 * gyro[1] - q2 * gyro[0] ]) - self.beta * grad # 積分 self.q = self.q + qDot * dt # 正規化 if np.linalg.norm(self.q) > 0: self.q = self.q / np.linalg.norm(self.q) def _normalize(self, v: np.ndarray) -> np.ndarray: """ベクトルの正規化""" norm = np.linalg.norm(v) return v / norm if norm != 0 else v def _quaternion_multiply(self, q: np.ndarray, r: np.ndarray) -> np.ndarray: """クォータニオンの乗算""" return np.array([ r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3], r[0]*q[1] + r[1]*q[0] + r[2]*q[3] - r[3]*q[2], r[0]*q[2] - r[1]*q[3] + r[2]*q[0] + r[3]*q[1], r[0]*q[3] + r[1]*q[2] - r[2]*q[1] + r[3]*q[0] ]) def get_euler_angles(self) -> np.ndarray: """オイラー角の取得""" q0, q1, q2, q3 = self.q # roll (フィルター回転) roll = np.arctan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) # pitch (仰角) pitch = np.arcsin(2*(q0*q2 - q3*q1)) # yaw (方位角) yaw = np.arctan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) return np.array([roll, pitch, yaw])
3. 相補フィルタ（Complementary Filter）
相補フィルタはジャイロ出力にハイパスフィルタ、加速度計出力にローパスフィルタをかけて合成するフィルタです。これにより、ジャイロのドリフトと加速度計のノイズを効果的に除去できます。
class ComplementaryFilter: def __init__(self, fc=0.1): # fc: カットオフ周波数[Hz] self.fc = fc self.prev_angle = np.zeros(3) # [roll, pitch, yaw] self.prev_gyro = np.zeros(3) self.prev_acc_angle = np.zeros(2) # [roll, pitch] self.prev_filtered_acc = np.zeros(2) self.first_update = True def update(self, gyro: np.ndarray, acc: np.ndarray, dt: float): """フィルタ更新 Args: gyro: 角速度 [rad/s] acc: 加速度 [m/s^2] dt: サンプリング時間 [s] """ # 加速度からの角度計算 roll_acc = np.arctan2(acc[1], acc[2]) pitch_acc = np.arctan2(-acc[0], np.sqrt(acc[1]**2 + acc[2]**2)) acc_angles = np.array([roll_acc, pitch_acc]) if self.first_update: self.prev_angle[:2] = acc_angles self.prev_acc_angle = acc_angles self.prev_filtered_acc = acc_angles self.first_update = False return self.prev_angle # ローパスフィルタ（加速度計） tau = 1 / (2 * np.pi * self.fc) alpha = tau / (tau + dt) filtered_acc = alpha * self.prev_filtered_acc + (1 - alpha) * acc_angles # ハイパスフィルタ（ジャイロ） gyro_hp = np.zeros(3) for i in range(2): # rollとpitchのみ gyro_hp[i] = (tau / (tau + dt)) * ( self.prev_gyro[i] + gyro[i] - self.prev_gyro[i] ) # 角度の計算 angles = np.zeros(3) angles[:2] = filtered_acc + gyro_hp[:2] angles[2] = self.prev_angle[2] + gyro[2] * dt # yawはジャイロのみ # 状態の更新 self.prev_angle = angles self.prev_gyro = gyro self.prev_acc_angle = acc_angles self.prev_filtered_acc = filtered_acc return angles
特徴： - 実装が非常に簡単 - 計算コストが低い - 高度なノイズ処理は難しい
各フィルタの比較
精度
- EKF: 最も高精度（適切なモデル化が必要）
- Madgwick: 高精度で安定性が高い
- 相補: 中程度の精度
計算コスト
- EKF: 高い（行列演算が多い）
- Madgwick: 中程度
- 相補: 低い（単純な線形結合）
パラメータ調整
- EKF: 複数のパラメータ調整が必要
- Madgwick: 単一パラメータ（beta）
- 相補: 単一パラメータ（alpha）
利点・注意点
- EKF
- バイアス推定が可能
- モデル化が重要
- 初期化が重要
- Madgwick
- 確率モデル不要
- 確実な収束
- 磁気の影響を考慮可能
- 相補
- 実装が簡単
- 直感的な調整
- 加速度ノイズに弱い
- EKF
まとめと今後の展望
IMUは現代のセンサーテクノロジーの中で最も重要なデバイスの一つです。正しい理解と適切な使用方法を習得することで、様々な革新的なアプリケーションを開発することができます。特に姿勢推定においては、オイラー角とクォータニオンの両方の実装方法を理解し、用途に応じて適切な手法を選択することが重要です。
マルチセンサーアレイの登場
- 複数のセンサーを組み合わせることで高精度化を実現
- 安価なセンサーでも高精度な測定が可能に
高感度化の進展
- 地球の自転を検知できるレベルの高感度センサーが一般にも入手可能に
- 小型化と低価格化が進み、広く普及
広がる応用範囲
- ウェアラブルデバイスやヘルスケア機器への展開
- スマートホームやIoT機器での活用
- 自動運転やロボット工学での高度な応用
これらの進展により、IMUはより軌密な動きの検出や高精度な姿勢推定が可能となり、新しいアプリケーションの創出につながっていくことが期待されます。
参考文献
"An efficient orientation filter for inertial and inertial/magnetic sensor arrays" - Sebastian O.H. Madgwick (2010)
- 論文と実装コードが公開されています
"Strapdown Inertial Navigation Technology, 3rd Edition" - D. H. Titterton and J. L. Weston, IET (2014)
- IMUの基礎理論から実装までを網羅した標準的な教科書です
"Fusion: A MARG Orientation Filter for Inertial, Magnetic, and GPS Sensors" - x-io Technologies
- Madgwickフィルターの最新実装です
"ROS IMU Tools" - ROS Wiki
- ROSでの実装例とドキュメントです
