基於卡爾曼濾波算法融合的汽車質心側偏角觀測器

車輛穩定性控制系統能夠提高汽車在極限情況下的操縱穩定性，而質心側偏角尤其重要。目前高度集成化的傳感器已經可以輕鬆測得車輛行駛過程中的橫擺角速度和側向加速度，但是質心側偏角則無法直接測量，因此對於質心側偏角而言必須通過估算來得到。

對於質心側偏角的估算國內外學者也有進行不同程度的探索，但是大多采用的是積分算法、卡爾曼濾波算法等等，但是在工程實際應用中卻很少。

質心側偏角通常定義爲側向車速與縱向車速的比值：

$\beta =\arctan (\frac{{{v}_{y}}}{{{v}_{x}}})\approx \frac{{{v}_{y}}}{{{v}_{x}}}\tag{1}$ 其中， ${{v}_{x}}$ 表示車輛縱向速度， ${{v}_{y}}$ 爲車輛側向速度。

本博文結合卡爾曼濾波算法設計質心側偏角觀測器。

卡爾曼濾波算法

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

卡爾曼濾波是以最小均方差誤差爲最佳估計準則，假設線性離散系統狀態方程和觀測方程如下所示：
$x(k+1)=A(k)x(k)+w(k)\tag{2}$ $y(k)=H(k)x(k)+v(k)\tag{3}$ 其中，系統的隨機噪聲協方差矩陣爲 $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{4}$
2.狀態估算計算：
$\hat{x}(k)=\hat{x}{{(k)}^{-}}+K(k)[y(k)-H(k)\hat{x}{{(k)}^{-}}] \tag{5}$
3濾波增益矩陣：
$K(k)=P{{(k)}^{-}}{{H}^{T}}(k){{[H(k)P{{(k)}^{-}}{{H}^{T}}(k)+R]}^{-1}}\tag{6}$
4.一步預測誤差方差陣：
$P{{(k)}^{-}}=A(k)P(k){{A}^{T}}(k)+Q\tag{7}$
5.估算誤差方差陣：
$P(k)=[1-K(k)H(k)]P{{(k)}^{-}}\tag{8}$
上述即隨機線性離散系統卡爾曼濾波的基本方程，只要給定初值，就可以根據觀測值 ${{y}_{k}}$ 遞推計算得到狀態值的估算。

基於卡爾曼濾波算法的質心側偏角觀測器

線性二自由度的汽車模型如圖1所示，運動微分方程爲：
$m{{a}_{y}}={{F}_{yf}}\cos \delta +{{F}_{yr}}\tag{9}$ ${{I}_{z}}{{\dot{w}}_{r}}=a{{F}_{yf}}\cos \delta -b{{F}_{yf}}\tag{10}$

圖1 線性二自由度模型根據二自由度汽車運動微分方程如下：

$({{C}_{f}}+{{C}_{r}})\beta +\frac{1}{{{v}_{x}}}({{l}_{f}}{{C}_{f}}-{{l}_{r}}{{C}_{r}})-{{C}_{f}}\delta =m({{\dot{v}}_{y}}+{{v}_{x}}{{w}_{r}})\tag{11}$

$({{l}_{f}}{{C}_{f}}-{{l}_{r}}{{C}_{r}})\beta +\frac{1}{{{v}_{x}}}(l_{f}^{2}{{C}_{f}}+l_{r}^{2}{{C}_{r}}){{w}_{r}}-{{l}_{f}}{{C}_{f}}\delta ={{I}_{z}}{{\dot{w}}_{r}}\tag{12}$
將二自由度的汽車運動微分方程進行離散處理得到13和14：
$\beta (k+1)=(1+\Delta t\frac{{{C}_{f}}+{{C}_{r}}}{m{{v}_{x}}(k)})\beta (k)+\Delta t(\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{m{{v}_{x}}^{2}(k)}-1){{w}_{r}}(k)+\Delta t\frac{{{C}_{f}}}{m{{v}_{x}}(k)}{{\delta }_{f}}(k)\tag{13}$

${{w}_{r}}(k+1)=(1-\Delta t\frac{{{C}_{f}}l_{f}^{2}+{{C}_{r}}l_{r}^{2}}{{{I}_{Z}}{{v}_{x}}(k)}){{w}_{r}}(k)+\Delta t\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{{{I}_{Z}}}\beta (k)+\Delta t\frac{{{l}_{f}}{{C}_{f}}}{{{I}_{Z}}}{{\delta }_{f}}(k)\tag{14}$
爲了估算質心側偏角需要建立卡爾曼濾波的方程，因此將上述建立的離散二自由度車輛的運動微分方程轉換成如下式15和式16：
$X(k)=AX(k-1)+B\delta (k)+W(k)\tag{15}$ $\hat{Y}(k)=CX(k)+D\delta (k)+V(k)\tag{16}$
式中， $X(k)=\left[ \begin{matrix} \beta (k) \\ {{w}_{r}}(k) \\ \end{matrix} \right]$ ， $A=\left[ \begin{matrix} & 1+\Delta t\frac{{{C}_{f}}+{{C}_{r}}}{m{{v}_{x}}(k)}\mathop{{}}^{{}}\Delta t(\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{mv_{x}^{2}(k)}-1) \\ & \Delta t\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{{{I}_{Z}}}\mathop{{}}^{{}}1-\Delta t\frac{{{C}_{f}}l_{f}^{2}+{{C}_{r}}l_{r}^{2}}{{{I}_{Z}}{{v}_{x}}(k)} \\ \end{matrix} \right]$ ，

$B=\left[ \begin{matrix} & \Delta t\frac{{{C}_{f}}}{m{{v}_{x}}(k)} \\ & \Delta t\frac{{{l}_{f}}{{C}_{f}}}{{{I}_{Z}}} \\ \end{matrix} \right]$ ， $W(k)=\left[ \begin{matrix} & {{w}_{1}}(k) \\ & {{w}_{2}}(k) \\ \end{matrix} \right]$ ， $\hat{Y}(k)=\left[ \begin{matrix} & {{{\hat{a}}}_{y}}(k) \\ & {{{\hat{w}}}_{r}}(k) \\ \end{matrix} \right]$ ，

$C=\left[ \begin{matrix} & -\frac{{{C}_{f}}+{{C}_{r}}}{m}\overset{{}}{\mathop {}}\,\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{m} \\ & \overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,0\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,1 \\ \end{matrix} \right]$ ， $D=\left[ \begin{matrix} & \frac{{{C}_{f}}}{m} \\ & \overset{{}}{\mathop {}}\,0 \\ \end{matrix} \right]$ ， $V(k)=\left[ \begin{matrix} & {{v}_{1}}(k) \\ & {{v}_{2}}(k) \\ \end{matrix} \right]$ 。