感覚的に理解する相補フィルタ

はじめに
相補フィルタ概略
(簡易)相補フィルタ実装
おわりに

はじめに

ロボットの姿勢推定ではカルマンフィルタや相補フィルタが用いられることがある．

以前は加速度のみで傾斜計を実装してみたが，今回は簡易な相補フィルタを実装して，姿勢推定の様子を観察していく．

ここでは簡単のため細かい数学的な解説や導出は省略して，感覚的に相補フィルタを理解していく．そのため一部正確性にかける表現があるかもしれない．

相補フィルタ概略

相補フィルタは，センサフュージョンの一種で，複数のセンサ値を組み合わせてもっともらしい値を求める手法である．ロボットなどの姿勢推定では，加速度，ジャイロ，地磁気などを用いる場合が多い．

加速度から傾きを求める

加速度センサでは加速度［m/s²］が測定できる．地球上で活動している限り我々は重力加速度の影響を受ける．加速度センサではこれを利用して姿勢推定を行う．

センサの値には誤差やノイズが含まれているので，当然傾きの計算結果にも誤差が含まれる．

また加速度センサから傾きを求める場合は鉛直方向の加速度を基準に計算しているため，ロールとピッチの傾きは計算できても，ヨーは求められない．ヨー方向も姿勢推定したい場合は地磁気など別の基準を取り入れる必要があるが，ここでは説明を省略する．

角速度から傾きを求める

ジャイロセンサでは角速度［角度／秒］が測定できる．例えば物体がある状態から２０［度／秒］で１秒間回転したとすると，当然２０度傾いたことになる．この移動した角度の和を計算していくことで姿勢推定を行う．

ただし，現実世界では物体は常に等角速度運動をしているとは限らないので，角速度の時間積分の和だけでは積分誤差が蓄積し，実際の物体の傾きと推定結果のズレ（ドリフ）が発生する．

数学的には測定間隔を限りなく小さくしていくことで積分誤差も小さくすることができるが，現実世界ではコンピュータの計算速度などによる制約があるため，測定間隔の最小化には限界がある．また加速度センサと同様にセンサの値にも誤差やノイズが含まれており，傾きの計算結果にも誤差が含まれてしまう．

相補フィルタ

相補フィルタは，複数のセンサ値を組み合わせてもっともらしい値を求める手法である．読んで字のごとく，各センサの弱点を互いに補いあって推定値を求める．

加速度から傾きを求める場合は重力加速度が基準となっている．重力加速度の方向は，地球が崩壊しない限り鉛直下向きのまま変化しないので，長期的に見れば誤差は収束する．一方で，時々刻々とロボットの姿勢が変化する場面においてはロボットの振動や落下などにより，重力加速度をうまく測定できない場合があり，短期的に見れば誤差は発散する．

角速度から傾きを求める場合は都度センサの測定値を時間積分することで姿勢を推定する．前述の通り測定間隔を限りなく小さくすることができれば，短期的に見れば誤差は収束する．一方で，センサ誤差や積分誤差を無くすことはできず，長期的に見れば誤差は発散する．

相補フィルタでは，これらのセンサの特性を鑑みて，各センサから導出した推定結果に周波数特性などに応じた加重平均を求め，もっともらしい姿勢を推定する．

係数は 0.98 と紹介されている場合が多いようだが，測定環境によって最適値を探す必要がある．

(簡易)相補フィルタ実装

実装環境

実装に使ったものは次の通り．

Adafruit のサンプルをコードを参考に，加速度から鉛直方向との傾きを求める関数をクラスに追加．

import adafruit_mpu6050
from math import atan2,degrees
from busio import I2C

class MPU6050(adafruit_mpu6050.MPU6050):
    def __init__(self, i2c_bus: I2C, address: int = adafruit_mpu6050._MPU6050_DEFAULT_ADDRESS) -> None:
        super().__init__(i2c_bus,address)

    def getAngle(self,x,y):
        angle = degrees(atan2(y,x))
        if angle < 0:
            angle += 360
        return angle

    @property
    def accel_inclination(self):
        x,y,z = self.acceleration
        return self.getAngle(x,z), self.getAngle(y,x)

加速度と角速度から求めた傾きと相補フィルタの結果を比較できるようプログラムを作成．計算結果を直接グラフィカルに表示したいけど，今回の測定環境ではめんどくさそうなので，ちょっとだけ sleep を入れてログ量をほんの気持ち程度に削減しておく．

なおここでは簡単のためセンサ値へのフィルタ適用は省略する．

import board
import MPU6050
import time
from math import degrees

i2c = board.I2C()
mpu = MPU6050.MPU6050(i2c)

comp_pitch = 0.0
comp_roll  = 0.0

gyro_pitch = 0.0
gyro_roll  = 0.0

accel_pitch = 0.0
accel_roll  = 0.0

k = 0.98
wait = 0.01
time_last  = time.time()
time_now   = time.time()
time_start = time.time()

print(f"time,elapsed_time,accel_pitch,accel_roll,gyro_pitch,gyro_roll,comp_pitch,comp_roll")

while True:
    time_now = time.time()
    dt = time_now - time_last

    gyro = mpu.gyro
    accel_roll,accel_pitch = mpu.accel_inclination

    comp_pitch = k * (comp_pitch + degrees(gyro[0] * dt))  + (1 - k) * accel_pitch
    comp_roll  = k * (comp_roll  + degrees(gyro[1] * dt))  + (1 - k) * accel_roll

    gyro_pitch += degrees(gyro[0] * dt)
    gyro_roll  += degrees(gyro[1] * dt)

    elap_time = time_now - time_start
    print(f"{time_now:17f},{elap_time:17f},{accel_pitch:7.2f},{accel_roll:7.2f},{gyro_pitch:7.2f},{gyro_roll:7.2f},{comp_pitch:7.2f},{comp_roll:7.2f}")
    time_last = time_now
    time.sleep(wait)