基於卡爾曼濾波器的車速觀測器

寫在前

汽車的縱向車速的識別是汽車控制系統的一個極其重要的數據，車速識別的精度將直接影響到汽車穩定性控制系統性能的好壞。

目前已經可以買到準確測量車輛的縱向車速的裝備，但是這種設備往往體積過大或者是成本過高，並且不適合長期安裝在車上，長期使用，有耐用性較差，容易受干擾等等缺點。因此在實際車輛中均採用估算的方式間接得到車速。目前基於車輛的傳感器進行縱向車速估算的方法主要有兩種：

• 通過測量從動輪的輪速信號，同時採集車輛在行駛過程中的加速度信號來進行測量和換算的方法：
  通過輪速乘以車輪的滾動半徑用於間接估算車速或者利用加速度信號與速度信號的積分關係來估算車速，這一類的算法雖然比較簡單，但是長時間的積分會由於各種路面等環境噪聲的影響導致結果嚴重偏離真實值，因此該類方法不適合長時間進行車速估算。
• 基於模型間接估計的方法：
  利用輪速信號等等的方法是指基於模型的間接估計就是利用原始信號首先經過數據處理，然後通過車輛模型，最後估計模型的狀態信息作爲最終的結果，其估計精度的好差也受到很多因素的制約。

本博文介紹一種基於平均輪速信號卡爾曼濾波算法的車速觀測器。

卡爾曼濾波算法

卡爾曼濾波是一種最優估算方法，其算法具有遞推性。對於車輛控制系統而言，由於在車輛的行駛過程中，會受到各種非確定性因素的干擾，因此車用的傳感器所測得的信號都夾帶有隨機的噪聲，所以爲了保證信號的準確性，需要使用卡爾曼濾波對信號進行處理。估算問題是指通過對這一系列帶有觀測噪聲和干擾信號的實際觀測數據的處理，從中得到所需要的各種參量的估算值的過程。而在車輛動力學系統控制系統中，通過狀態估算的方法得到車輛的各種狀態參數的求解問題，稱爲車輛動力學估算問題。

卡爾曼濾波是以最小均方差誤差爲最佳估計準則，假設線性離散系統狀態方程和觀測方程如下所示：
$x(k+1)=A(k)x(k)+w(k)\tag{1}$ $y(k)=H(k)x(k)+v(k)\tag{2}$其中，系統的隨機噪聲協方差矩陣爲$Q$，觀測噪聲協方差矩陣爲$R$，$x(k+1)$是系統n維狀態向量，$A(k)$爲系統狀態轉移矩陣，$y(k)$是系統的m維觀測向量，$w(k)$是系統的隨機噪聲，$v(k)$是系統的m維觀測噪聲，$H(k)$爲觀測矩陣。假設$w(k),v(k)$爲相互獨立正態分佈的白噪聲。

卡爾曼濾波器主要包括預測和校正兩個過程。總結卡爾曼濾波算法的基本步驟如下：
1.狀態一步預測：
$\hat{x}{{(k)}^{-}}=A(k)\hat{x}(k-1) \tag{3}$
2.狀態估算計算：
$\hat{x}(k)=\hat{x}{{(k)}^{-}}+K(k)[y(k)-H(k)\hat{x}{{(k)}^{-}}] \tag{4}$
3濾波增益矩陣：
$K(k)=P{{(k)}^{-}}{{H}^{T}}(k){{[H(k)P{{(k)}^{-}}{{H}^{T}}(k)+R]}^{-1}}\tag{5}$
4.一步預測誤差方差陣：
$P{{(k)}^{-}}=A(k)P(k){{A}^{T}}(k)+Q\tag{6}$
5.估算誤差方差陣：
$P(k)=[1-K(k)H(k)]P{{(k)}^{-}}\tag{7}$
上述即隨機線性離散系統卡爾曼濾波的基本方程，只要給定初值，就可以根據觀測值${{y}_{k}}$遞推計算得到狀態值的估算。

在汽車行駛過程中，可以將車身速度認爲是均勻變化的信號，利用輪速的平均值作爲測量值來估算車輛的縱向車速。若是四輪驅動的車輛，則可以採集四輪的輪速信號的平均值。由於車輛在信號的採集過程中，受到各種路面環境噪聲的影響，使得采集到的信號往往會帶有毛刺和誤差，這種毛刺會隨着誤差的積累影響估算的精度，因此，採用了卡爾曼濾波法對信號進行濾波處理。

基於卡爾曼濾波的車速觀測器

依據線性二自由度車輛模型建立卡爾曼濾波器。車身速度估計的系統狀態方程與量測方程如下：
$X(k)=AX(k-1)+Ba(k)+W(k)\tag{8}$ $\hat{Y}(k)=CX(k)+V(k)\tag{9}$
其中，$X(k)=\left[ \begin{matrix}v(k) \\ {{a}_{e}}(k) \\ \end{matrix} \right]$，

$A=\left[ \begin{matrix} 1&\mathop{{}}^{{}}\Delta t \\ 0& \mathop{{}}^{{}}1 \\ \end{matrix} \right]$，

$B=\left[ \begin{matrix} \Delta t \\ 0 \\ \end{matrix} \right]$，

$W(k)=\left[ \begin{matrix} {{w}_{1}}(k) \\ {{w}_{2}}(k) \\ \end{matrix} \right]$，

$\hat{Y}(k)=\left[ {{v}_{wheel}} \right]$，

$C=\left[ 1\mathop{{}}^{{}}0 \right]$，

$V(k)=\left[ {{v}_{1}}(k) \right]$。
$\Delta t$爲系統的採樣時間，$v$爲系統估算的車速，${{a}_{e}}$爲加速度信號的偏差，${{v}_{wheel}}$爲系統測量到的輪速信號。假設${{w}_{1}},{{w}_{2}},{{v}_{1}}$均爲零均值相互獨立的高斯分佈的隨機信號，$Q,R$分別爲噪音$W(k),V(k)$的協方差矩陣。