Arduino〈倒單擺 Inverted Pendulum〉(3) 互補濾波器

陀螺儀能夠輕易的量測到角速度，但是要量到角度卻要陀螺儀與加速規的配合，而能不能準確的量到角度將是控制倒單擺維持平衡的關鍵。

理論上角度可由兩種方式求得:

1. 利用三軸加速規量測重力加速的大小與方向作為參考值，當加速規隨車體擺動時，重力加速度在加速規三個方向的分量會隨擺動的角度變化，計算其中兩個方向的分量比值再以反正切值(arctangent)推算目前倒單擺與鉛直線的夾角。

2. 利用陀螺儀量測車體擺動的角速度，並對其進行積分得到角度的訊號。


而實際上，感測器各有其限制，當角度快速變化(高頻)時，陀螺儀取其角速度，這時候積分得到的角度比較準確，但這種快速變化會對加速規造成顯著的外力干擾，反而無法透過計算重力加速度的分量來計算角度；另一方面，在角度幾乎靜止(低頻)時，加速規能夠很好的計算角度，但陀螺儀的offset與漂移(drift)的問題，會隨著積分不斷的累積誤差，反而失去量測角度的功能。

互補濾波器(Complementary Filter)便是利用這樣的特性，取高頻的陀螺儀訊號積分值，同時也感測低頻的加速規訊號，將兩者分別計算的角度以固定比例相加，如下列公式:

Angle_comp = a(Angle_gyro) +(1 - a)(Angle_acc)

 0 < a < 1   (通常a值接近1)

如此可達到感測互補的作用，既抑制加速規的干擾，又可消除陀螺儀的積分誤差，更重要的是這個方法在程式碼中容易實現，並具有調整的彈性，算是很好的方法了，下面是一段互補濾波器的虛擬程式碼(Pseudocode)範例，實際寫法得依個人做調整。

loop :
    accAngle <-- Acc.getAngle();
      
    omega <-- Gyro.getOmega();

    gyroAngle <-- lastAngle + omega * dt;
    
    compAngle <--  0.98 * gyroAngle + 0.02 * accAngle ;

    lastAngle <-- compAngle;
loop <-- go to


利用Processing做示波器記錄量測結果中便可以清楚看到差異，橫軸為時間，縱軸則是計算的角度，紅線是只用加速規計算角度的結果，在角度變化或是振動時都會受到嚴重干擾導致量測誤差，綠線則僅用陀螺儀計算，角度變化時反應很快，但在角度靜止時卻有明顯的偏移誤差。在使用互補濾波器後，藍線幾乎可以貼合紅線，同時又抑制了干擾，使得感測效果明顯提升。

Processing示波器記錄量測結果(1)

Processing示波器記錄量測結果(2)

除了互補濾波，還有其他的方法可以應用，其中最被廣泛討論的大概就是移動平均濾波(Moving Average Filter)與卡爾曼濾波器(Kalman Filter)了吧。 移動平均很好理解，把數個感測值加總後取其平均，以此平均值作為每次計算的依據，用這個方法能夠很好的去除訊號突波與白雜訊，但也會帶來感測訊號延遲的影響，等同降低了感測頻寬。若用於平均的訊號取樣數量越多，延遲便會更加嚴重，使得控制反應不及，因為濾波器在量到角度的正確變化之前，單擺就已經倒了。所以在倒單擺系統中，僅使用移動平均濾波恐怕是不夠的。相較之下，卡爾曼濾波就顯得高深且強大了，然而其背後的數學架構也是挺繁雜，過度簡化的設計恐怕無法帶來什麼優勢，效果甚至不如一個低通濾波器。我相信卡爾曼濾波器肯定能夠駕馭如此簡單的倒單擺系統，但是有沒有必要為此殺雞用牛刀，最好還是依據控制的目的作取捨。