[發明專利]一種基于魯棒混合濾波的車輛側傾角與俯仰角估計方法

有效

• 申請號: 201810060029.7
• 申請日: 2018-01-22
• 公開（公告）號: CN108413923B
• 公開（公告）日: 2020-07-24
• 發明（設計）人: 馮宇;聶煊赫;高佳斌;俞曉天;陳焯明
• 申請（專利權）人: 浙江工業大學
• 主分類號: G01C1/00
• 分類號: G01C1/00;G01C9/00
• 代理公司: 杭州浙科專利事務所(普通合伙) 33213
• 代理人: 周紅芳

1.一種基于魯棒混合濾波的車輛側傾角與俯仰角估計方法,其特征在于包括如下步驟：

1)對四輪車行駛情況建立連續動力學模型,具體包括以下步驟：

1.1)不考慮地球自轉速度的情況下，假定側傾角速度和俯仰角速度為零，建立四輪車行駛過程中的連續動力學方程如式(1)所示：

其中，v_x,v_y分別表示縱向速度和橫向速度，上標·表示微分，a_x,a_y分別表示縱向加速度和橫向加速度，w_z表示橫擺角速度，g表示重力加速度，α,β分別表示側傾角與俯仰角；

1.2)由式(1)得到側傾角α和俯仰角β的表達式如式(2)所示：

實際車輛行駛過程中，相對于車輛的縱向速度v_x和縱向加速度車輛的橫向速度v_y和橫向加速度均很小能忽略不計，因此式(2)簡化后如式(3)所示：

其中α,β分別表示側傾角與俯仰角，縱向加速度a_x，橫向加速度α_y，縱向速度v_x以及橫擺角速度w_z由四輪車上安裝的傳感器測得，而縱向速度v_x的微分通過測得的v_x對時間求導來得到；

2)考慮復雜環境下的混合擾動，建立系統狀態方程，包括觀測方程和輸出方程，并構建系統濾波器,所述考慮復雜環境下的混合擾動，建立系統狀態方程和觀測方程包括以下步驟：

2.1)離散化后的狀態方程如式(4)所示：

x(k+1)＝Ax(k)+B₀w₀(k)+B₁w(k) (4)

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，估計對象x表示側傾角α和俯仰角β，即x＝[α β]^T，上標T表示矩陣的轉置，A表示估計對象x的狀態轉移矩陣，w₀表示均值為零方差為1的高斯白噪聲，B₀表示高斯白噪聲w₀的輸入矩陣，w表示非隨機有界擾動信號，B₁表示非隨機有界擾動信號w的輸入矩陣；

2.2)離散化后的觀測方程如式(5)所示：

y(k)＝C₂x(k)+Dw₀(k) (5)

其中，k表示離散化時刻，y表示觀測向量，x表示估計對象,C₂表示估計對象x的觀測矩陣，w₀表示均值為零方差為1的高斯白噪聲，D表示高斯白噪聲w₀的觀測矩陣；

離散化后的輸出方程如式(6)所示：

其中，k表示離散化時刻，z,z₀表示系統輸出，x表示估計對象，C₁,C₀表示估計對象x的輸出矩陣；

2.3)基于上述系統設計一個濾波器，如式(7)所示：

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，A表示估計對象x的狀態轉移矩陣，表示估計對象x的估計值，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，y表示觀測向量，分別表示系統輸出z,z₀的估計值，C₁,C₀表示估計對象x的輸出矩陣；

該濾波器的作用是使得估計值接近系統輸出z,z₀，從而實現對估計對象x,即側傾角α和俯仰角β的實時高精度估計；

3)給出系統誤差模型，進一步給出基于最差非隨機擾動下的系統誤差模型，設計并通過迭代算法求解濾波器增益，具體包括以下步驟：

3.1)通過式(4)(5)(6)(7)得到誤差系統模型如式(8)所示：

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，e_x表示估計對象x與對應估計值的差值,

e表示系統輸出z與對應估計值的差值，e₀表示系統輸出z₀與對應估計值的差值，A表示估計對象x的狀態轉移矩陣，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，w₀表示均值為零方差為1的高斯白噪聲，B₀為高斯白噪聲w₀的輸入矩陣，D表示高斯白噪聲w₀的觀測矩陣，w表示非隨機有界擾動信號，B₁為非隨機有界擾動信號w的輸入矩陣，C₁,C₀表示估計對象x的輸出矩陣；

3.2)基于誤差系統，定義最差的非隨機有界擾動信號w如式(9)所示：

w(k)＝We_x(k) (9)

將式(9)代入式(8)，進一步得到最差非隨機擾動下的系統誤差模型如式(10)所示：

定義中間矩陣A_L和A_W分別如式(11)、式(12)所示：

A_L＝A+LC₂ (11)

A_W＝A+B₁W (12)

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，γ為預設的常數，e_x表示估計對象x與對應估計值的差值,e表示系統輸出z與對應估計值的差值，e₀表示系統輸出z₀與對應估計值的差值，A表示估計對象x的狀態轉移矩陣，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，w₀表示均值為零方差為1的高斯白噪聲，B₀表示高斯白噪聲w₀的輸入矩陣，D表示高斯白噪聲w₀的觀測矩陣，w表示非隨機有界擾動信號，B₁表示非隨機有界擾動信號w的輸入矩陣，C₁,C₀表示估計對象x的輸出矩陣，W，A_L和A_W均為中間矩陣；

3.3)當k＝0時,對中間矩陣P₁和P₂以及中間矩陣W和濾波器增益L賦初值，如式(13)所示：

3.4)基于誤差系統，由H_∞濾波算法得到最差的非隨機有界擾動信號w如式(14)所示：

因此如式(15)所示：

其中，k表示當前離散化時刻,w表示非隨機有界擾動信號，γ為預設的常數，I表示單位矩陣，上標-1表示矩陣的逆，上標T表示矩陣的轉置，B₁表示非隨機有界擾動信號w的輸入矩陣，C₁,C₀表示估計對象x的輸出矩陣，W,A_L和P₁均為中間矩陣，e_x表示估計對象x與對應估計值的差值；

3.5)基于最差非隨機擾動下的系統誤差模型，通過H₂濾波算法得到濾波器增益L的表達式如式(16)所示：

其中，上標-1表示矩陣的逆，上標T表示矩陣的轉置，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，B₀表示高斯白噪聲w₀的輸入矩陣，A_W和P₂均為中間矩陣，D表示高斯白噪聲w₀的觀測矩陣；

3.6)當k＝1時，由式(11)(12)(15)(16)分別得到式(17)、式(18)、式(19)及式(20)所示：

A_W(1)＝A+B₁W(0) (17)

A_L(1)＝A+L(0)C₂ (18)

其中，A表示估計對象x的狀態轉移矩陣，B₁表示非隨機有界擾動信號w的輸入矩陣，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，γ為預設的常數，I表示單位矩陣，上標-1表示矩陣的逆，上標T表示矩陣的轉置，B₀表示高斯白噪聲w₀的輸入矩陣，D表示高斯白噪聲w₀的觀測矩陣，w表示非隨機有界擾動信號，C₁,C₀表示估計對象x的輸出矩陣，W,A_L,A_W,P₁和P₂均為中間矩陣；

3.7)中間矩陣P₁滿足如下Riccati黎卡提方程如式(21)所示：

因此得到中間矩陣P₁(1)如式(22)所示：

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，B₁表示非隨機有界擾動信號w的輸入矩陣，L表示需要設計的濾波器增益，C₂表示估計對象x的觀測矩陣，γ為預設的常數，I表示單位矩陣，上標-1表示矩陣的逆，上標T表示矩陣的轉置，B₀表示高斯白噪聲w₀的輸入矩陣，D表示高斯白噪聲w₀的觀測矩陣，w表示非隨機有界擾動信，C₁表示估計對象x的輸出矩陣，A_L和P₁均為中間矩陣；

3.8)中間矩陣P₂滿足如下Riccati(黎卡提)方程如式(23)所示：

因此得到中間矩陣P₂(1)如式(24)所示：

其中，k表示當前離散化時刻，k+1表示下一離散化時刻，C₂表示估計對象x的觀測矩陣，上標-1表示矩陣的逆，上標T表示矩陣的轉置，B₀表示高斯白噪聲w₀的輸入矩陣，D表示高斯白噪聲w₀的觀測矩陣，A_W和P₂均為中間矩陣；

3.9)重復步驟3.6)、步驟3.7)及步驟3.8)：

若k＝T時刻時，矩陣P₁(T)和矩陣P₁(T-1)差值的二范數小于給定誤差，分別得到如式(25)、式(26)所示：

P₁＝P₁(T)＝P₁(T-1) (25)

A_L＝A_L(T)＝A_L(T-1) (26)

同樣，若k＝T時刻時，矩陣P₂(T)和矩陣P₂(T-1)差值的二范數小于給定誤差，得到分別得到如式(27)、式(28)所示：

P₂＝P₂(T)＝P₂(T-1) (27)

A_W＝A_W(T)＝A_W(T-1) (28)

其中，A_L,A_W,P₁和P₂均為中間矩陣；

3.10)將中間矩陣P₂和中間矩陣A_W代入式(16)得到濾波器增益矩陣L，從而由濾波器(7)，實現對估計對象x的實時高精度估計，估計對象x為側傾角α和俯仰角β。