對比直立車模控制中的互補濾波、Karlman濾波和參考濾波方案

前幾天，有參加智能車競賽的同學在公衆號上詢問一個關於直立車模控制中角度融合算法的問題。感覺這是所有初次準備智能車模競賽的同學都會碰到的問題。

卓大大，請問您可以針對卡爾曼濾波、互補濾波、清華濾波的原理與異同做一個推送嗎？最近在查閱相關的資料學習濾波算法，感覺好難理解呀/::<

^{▲ 平衡雙輪自行車}

 

01慣性傳感器

---

在車模直立控制中被廣泛使用的慣性傳感器通常是由加速度計和陀螺儀組成。價格比較便宜的慣性測量傳感器，輸出的信號往往包含有各種噪聲。陀螺儀輸出車模傾角$\theta$的變化率（角速度）$\dot \theta$信號。它通常包括緩慢變化的偏移量$\omega _b$和高斯白噪聲$n_\omega$。$\dot \theta = \omega _T - \omega _b - n_\omega$
其中偏移量$\omega _b$的導數是隨機的，通常描述爲一個白噪聲信號：$\dot \omega _b = n_\alpha$。

在重力場中，加速度傳感器正交的兩個輸出，通過求反正切可以傳感器相對於重力加速度的傾角。但傳感器的運動所產生的加速度也會疊加在輸出信號中，進而產生干擾信號。$\theta _M = \theta _T + n_\theta$

^{▲ 採集數據及其顯示}

通過濾波器算法將陀螺儀和加速度計所得到的角度信息進行融合，最大限度抑制噪聲對測量角度的影響。

前面提問中所提到的三種濾波算法，分別利用傳感器噪聲不同特點來抑制噪聲的影響。看似這些方法原理相差很大，藉助於以下條件，能夠便於我們進行分析和比較：

• 這三種算法都屬於線性濾波算法；
• 在穩態下，可以藉助於輸入輸出之間的傳遞函數來比較算法的特性；
• 針對車模直立控制，所涉及到的角度只有一個；

 

02三種濾波算法

---

1.參考濾波方案

在2012年直立車模組別首次引入智能車競賽的，競賽祕書處給出了一個 參考設計方案 。它是在對陀螺儀信號進行積分的基礎上，使用加速度計得到的角度信號對輸出進行負反饋調節，從而抑制積分中存在的發散問題。下圖是參考設計方案中相應的原理框圖和程序實現。

^{▲ 參考濾波方案軟件實現}

加速度傳感器的Z周輸出電壓$V_a \left( t \right)$（VOLTAGE_GRAVITY），經過偏移量$B_a$（GRAVITY_OFFSET），比率$R_a$（GRAVITY_ANGLE_RATIO）調整之後，形成算法的輸入$A\left( t \right)$（g_fGravityAngle）:$A\left( t \right) = R_a \cdot \left[ {V_a \left( t \right) - B_a } \right]$

陀螺儀的輸出電壓$V_g \left( t \right)$（VOLTAGE_GYRO）經過偏移量$B_g$（GYROSCOP_OFFSET），比率$R_g$（GYROSCOPE_ANGLE_RATIO）調整之後，形成算法輸入$G\left( t \right)$（g_fGyroscopeAngleSpeed）:$G\left( t \right) = R_g \cdot \left[ {V_g \left( t \right) - B_g } \right]$

算法的輸出爲角度：$\theta \left( t \right)$（g_fCarAngle）。

^{▲ 參考濾波方案的核心算法}

根據參考程序代碼，算法可以使用迭代方程描述爲：
$\begin{aligned} \theta \left[ n \right] = T_s \cdot \left\{ {G\left[ n \right] + {1 \over {T_g }}\left\{ {A\left[ n \right] - \theta \left[ {n - 1} \right]} \right\}} \right\} + \theta \left[ {n - 1} \right]\\= T_s \cdot G\left[ n \right] + {{T_s } \over {T_g }}A\left[ n \right] + \left( {1 - {{T_s } \over {T_g }}} \right)\theta \left[ {n - 1} \right]\\\end{aligned}$

輸出角度$\theta \left[ n \right]$由兩部分線性疊加而成，使用z變換對上面差分方程進行變化，可得：$\Theta \left( z \right) = H_G \left( z \right) \cdot G\left( z \right) + H_A \cdot A\left( z \right)$
其中：
$H_G \left( z \right) = {{T_s } \over {1 - \left( {1 - {{T_s } \over {T_g }}} \right)z^{ - 1} }},\,\,\,\,H_A \left( z \right) = {{T_s } \over {T_g }} \cdot {1 \over {1 - \left( {1 - {{T_s } \over {T_g }}} \right)z^{ - 1} }}$

通過上面的化簡公式可以看出，實際上角度輸出信號是陀螺儀和加速度計信號經過一級低通濾波後疊加在一起，所以陀螺儀和加速度計輸出信號中如果存在直流分量的漂移，都會引起輸出角度信號的零點漂移。

 

2. 互補濾波器

互補濾波器的基本結構如下圖所示。假設兩個測量信號$B\left( s \right),A\left( s \right)$都包含了實際信號$\theta \left( s \right)$以及噪聲。在信號$A\left( s \right)$中的噪聲主要分佈在高頻，而在信號$B\left( s \right)$中的噪聲主要分佈在低頻。

$H\left( s \right)$代表着低通濾波器，那麼對應的$1 - H\left( s \right)$就是互補的高通濾波器。它們分別提取$A\left( s \right),B\left( s \right)$中的低頻和高頻成分，然後疊加在一起。

^{▲ 左：基本互補濾波器的結構。右：基本結構的變形，只對噪聲進行低通濾波}

在上圖右邊給出了互補濾波器基本結構的恆等變形。將$B\left( s \right)$與$A\left( s \right)$相減之後，就剩下了高頻噪聲$n_2$與低頻噪聲$n_2$相減。再經過低通濾波器$H\left( s \right)$，就剩下$n_1$去抵消$B\left( s \right)$中的低頻噪聲了。

只所以討論這個變形，就是因爲前面參考方案就是變形的互補濾波器。將陀螺儀信號$G\left( t \right)$的積分看做帶有低頻噪聲（積分漂移信號）的觀察信號$B\left( t \right)$。它與加速度計信號$A\left( t \right)$（帶有高頻運動噪聲）相減，後面有比例、積分組成的負反饋組成一個一階低通濾波器$H\left( s \right) = {1 \over {T_A \cdot s + 1}}$，濾波之後與$B\left( t \right)$疊加，形成最後的輸出信號。

^{▲ 參考濾波方案本質上是變形後的互補濾波器}

通過上面討論可以看出，如果在互補濾波器中的低通濾波器的階次爲一階的話，那麼就和前面的參考方案是一致了。在實際應用中，可以增加濾波器的階次，來提高角度計算的精度。比如在陀螺儀輸入信號中，再增加一級高通濾波，可以進一步抑制陀螺儀信號中的直流分量的漂移。

 

3. 卡爾曼濾波

使用卡爾曼濾波的方法來融合來自陀螺儀和加速度計的信號，不在頻率範圍內考慮它們各自噪聲的特點，而是將這兩個信號從功能上分爲兩類。陀螺儀信號$\omega _M$是過程驅動變量，它激動系統狀態（車模傾角，角速度）發生變化；加速度計得到的角度$\theta _M$則是觀察變量，是來修正系統狀態。。

選擇車模傾角$\theta _k$和陀螺儀偏變化量$\omega _b$作爲系統狀態變量，得到離散系統狀態方程爲：

令：$A = \begin{bmatrix} \begin{matrix} 1 & { - t_s }\\0 & 1\\\end{matrix} \end{bmatrix},B = \begin{bmatrix} \begin{matrix} {t_s }\\0\\\end{matrix} \end{bmatrix},H = \begin{bmatrix} \begin{matrix} 1 & 0\\\end{matrix} \end{bmatrix},Q = t_s^2 \begin{bmatrix} \begin{matrix} {\sigma _\omega ^2 } & {\sigma _{\omega \alpha } }\\{\sigma _{\alpha \omega } } & {\sigma _\alpha ^2 }\\\end{matrix} \end{bmatrix},R = \sigma _\theta ^2$

則對應卡爾曼濾波方程爲：

$\begin{aligned} \hat x_{k+1}^* = Ax_k + Bu \cdot \omega _{M,k}\\P_{k + 1}^* = AP_k A^T + Q\\K_k = P_k^* H^T \left( {HP_k^* H^T + R} \right)^{ - 1}\\\hat x_k = \hat x_k^* + K\left( {\theta _{M,k} - H\hat x_k^* } \right)\\P_k = \left( {I - K_k H} \right)P_k^*\\\end{aligned}$

在這個過程中，如果系統噪聲協方差矩陣$Q$以及觀測噪聲方差$R$始終爲常量。那麼由上面方程2、3、5迭代之後，誤差協方差矩陣$P$和卡爾曼增益矩陣$K$就會逐步收斂到一個常量。

在下面參數下，經過迭代1000次之後，卡爾曼增益矩陣參數$K = \left( {k_1 ,k_2 } \right)^T$的變化曲線。可以看去它逐漸曲線與常數$\left( {0.005,0.001} \right)^T$。
$t_s = 0.01,\,\,\sigma _\omega ^2 = 1.505 \times 10^{ - 3} ,\sigma _\alpha ^2 = 1.721 \times 10^{ - 4} ,\sigma _{\omega ,\alpha }^{} = 0,R = 250$

^{▲ K參數迭代數值}

當$K$爲常量的時候，對方程1、4再聯立起來，就形成關於狀態變量$\hat x_k = \left( {\theta _k ,\omega _{b,k} } \right)^T$的差分方程。考慮到$k_1 ,k_2$，$t_s$都比較小，將化簡過程中涉及到這些小量乘積（二階小量）都省略掉，那麼最終可以將輸出角度$\theta$與輸入$\omega _M ,\theta _m$之間的差分方程化簡爲：$\Theta \left( z \right) = {{t_s } \over {1 - \left( {1 - k_1 } \right)z^{ - 1} }}G\left( z \right) + {k \over {1 - \left( {1 - k_1 } \right)z^{ - 1} }}A\left( z \right)$

其中$\Theta \left( z \right),G\left( z \right),A\left( z \right)$分別是$\theta \left[ n \right],\omega _M \left[ n \right],\theta _M \left[ n \right]$的z變換。對照上面簡化的輸入輸出之間的關係與前面參考方案中對應的方程，可以看到卡爾曼濾波器本質上和互補濾波器是相同的。如果不省略那些參數小的項數，濾波算法就變成二階的互補濾波器的形式了。

觀察噪聲$R$來自於加速度計傳感器，系統內部噪聲$Q$來自於陀螺儀。當$R$遠遠大於$Q$的時候，所計算出的卡爾曼濾波增益$K$的數值比較小，這樣才能夠進行上述的算法簡化。

 

03算法關係

---

可以看出。當所使用的慣性傳感器，陀螺儀的噪聲遠遠低於加速度傳感器的噪聲時，低通濾波器的階次取一階濾波器的情況下，三種濾波器的算法效果在平穩狀態下是相同的。

從原理上，參考方案與互補濾波器比較接近，它們是利用不同傳感器噪聲在頻率上的差別抑制噪聲的。卡爾曼濾波器則根據傳感器對傾角影響的機制不同，通過構造系統狀態變量將信息進行融合。

在車模直立控制應用中，互補濾波器和參考方案所需要的計算量小，參數只有濾波時間常數$T_A$，調試相對比較簡單。但該算法對於陀螺儀工作點的漂移無法抑制，使用時，可以對陀螺儀信號再通過一個高通濾波器，消除陀螺儀信號工作點變化對輸出角度的影響。

卡爾曼濾波器計算量相對較大，適應於高速控制器。由於卡爾曼濾波器內部變量包括有對陀螺儀工作點的估計，所以陀螺儀的工作點的緩慢變化不會影響計算角度的變化。

^{▲ RECORD1數據}

在使用濾波器算法的時候，都會涉及到算法的初始狀態的設置問題。對於互補濾波器相對比較簡單，將內部積分累加變量初始化爲0便可以適應大部分情況。對於卡爾曼濾波器，它的參數矩陣$P$則需要選擇恰當的初始值，以使得算法儘快達到收斂的目的。比如可以將算法收斂後的$P$矩陣的值作爲程序下一次運行的初始值，這樣就可以更快使得算法進入穩定狀態。

此外，需要根據傳感器輸出信號的實際情況，合理的確定卡爾曼濾波器中的$Q,R$參數，這樣才能夠在算法的收斂以及濾波效果方面取得良好的平衡。

關於這三種算法在理論上的分析就到這裏，它們在實際控制中的效果以後在給出。