[0057] 所以，可將Vd簡(jiǎn)化為
[0058] Vd= VxA θ + Δ Vy
[0059] 將上式除以Δ t，應(yīng)取當(dāng)Δ t趨于0時(shí)的極限，便得到了車輛質(zhì)心加速度在Y軸上 的分量，即為車輛行駛過程中的側(cè)向加速度
[0061] 因此，可得出
[0062] Vj, -·=αν -K -COr
[0063] 其中，橫擺角速度。
[0064] 將得到的!^帶入到處的表達(dá)式中，得到第一質(zhì)心側(cè)偏角的微分值矣
[0066] 對(duì)上式進(jìn)行最小二乘法估計(jì)，能夠較準(zhǔn)確得到第一質(zhì)心側(cè)偏角的微分值為。
[0067] 作為一種優(yōu)選的，本發(fā)明采用一種帶遺忘因子的最小二乘法對(duì)上式進(jìn)行估計(jì)，首 先將上式整理成為最小二乘法參數(shù)估計(jì)的標(biāo)準(zhǔn)形式：
[0069] 式中，卵)=A , < (0 =匕，Y⑴為測(cè)量值輸出，Y (t) = ay_ ω r · Vx，
[0070] 采用帶遺忘因子的最小二乘法對(duì)公式6中的Θ (t)進(jìn)行估計(jì)，算法如下：
CN 105151047 A 說明書 5/9 頁(yè)
[0074] 算法中I是單位矩陣；ε (t)是先驗(yàn)誤差；K (t)是卡爾曼增益；P (t)是協(xié)方差矩 陣；λ是遺忘因子，λ的值越小，歷史數(shù)據(jù)在新的參數(shù)估計(jì)周期中的權(quán)值就越低。
[0075] 通過使用帶遺忘因子的最小二乘法進(jìn)行參數(shù)估計(jì)，可得到第一質(zhì)心側(cè)偏角的微分 值處，再通過使用微處理器對(duì)處求積分，即可得到車輛實(shí)時(shí)的第一質(zhì)心側(cè)偏角的值，即：
[0077] Δ t是控制器的采樣周期，一般采用5ms或者5ms的倍數(shù)。
[0078] 由上式可知，在使用傳感器測(cè)量得到汽車行駛過程中的側(cè)向加速度ay、橫擺角速 度'和縱向速度Vx這三個(gè)參數(shù)后，就可以直接得到第一質(zhì)心側(cè)偏角的微分值處，再將爲(wèi) 進(jìn)行積分后就得出了第一質(zhì)心側(cè)偏角βρ
[0079] 步驟三S130 :汽車在行駛過程中，當(dāng)車輛側(cè)向加速度大于第二加速度&2時(shí)，采用 基于三自由度非線性汽車模型濾波算法進(jìn)行第二質(zhì)心側(cè)偏角估計(jì)。第二加速度的值可能因 不同的車型而變化。作為一種優(yōu)選的，a2= 0. 45g。
[0080] 三自由度車輛模型如圖3所示，車輛的運(yùn)動(dòng)方程為：
[0083] 公式中，&和K f分別為車輛的后輪側(cè)偏剛度和前輪側(cè)偏剛度；a和b則分別是前 軸和后軸到質(zhì)心的距離；m為車輛質(zhì)量；Iz為繞z軸的轉(zhuǎn)動(dòng)慣量、ω 1^為橫擺角速度、β 心側(cè)偏角、S前輪轉(zhuǎn)角（由方向盤轉(zhuǎn)角傳感器測(cè)量得到方向盤轉(zhuǎn)角，再將方向盤轉(zhuǎn)角值代 入傳動(dòng)比公式求得前輪轉(zhuǎn)角。）、VX車輛縱向車速、a JP a y分別為汽車縱向加速度和車輛側(cè) 向加速度，可由車輛加速度傳感器測(cè)得。
[0084] 采用非線性濾波算法能夠估計(jì)出第二質(zhì)心側(cè)偏角的值，通常可采用容積卡爾曼濾 波算法、容積卡爾曼濾波算法。
[0085] 作為一種優(yōu)選的，本發(fā)明采用容積卡爾曼濾波算法，對(duì)第二質(zhì)心側(cè)偏角進(jìn)行估計(jì)。
[0086] 整理為均方根嵌入式容積卡爾曼濾波器方程結(jié)構(gòu)的標(biāo)準(zhǔn)形式：
[0087] X (t) = f (x (t), u (t), w (t))
[0088] z (t) = h (x (t)，u (t)，v (t))
[0089] 式中狀態(tài)向量為x(t) = [cor，β，νχ]τ;量測(cè)向量為z(t) = [ay];控制向量為u(t) =[S，aJT;w(t)為過程噪聲；v(t)為量測(cè)噪聲；w(t)和v(t)滿足均值為0,誤差方差陣分 別為Q和R的正態(tài)分布。
[0090] 容積卡爾曼濾波算法分成三部分，分別是初始化、預(yù)報(bào)步和濾波步。根據(jù)三自由 度車輛模型中的狀態(tài)方程，利用k時(shí)刻的狀態(tài)量估計(jì)值和控制量Uk，計(jì)算狀態(tài)量預(yù)報(bào) 值％猶，即預(yù)報(bào)步；利用％k+Ι時(shí)刻的控制量uk+1通過量測(cè)方程計(jì)算量測(cè)量預(yù)報(bào)值 電獅.1:，并對(duì)％&^:進(jìn)行濾波并得到k+Ι時(shí)刻估計(jì)值即濾波步。
[0091] 初始化：在車輛行駛過程中，〇^和Vx可由加速度傳感器中的側(cè)向加速度測(cè)量模 塊和測(cè)速模塊測(cè)量得到初值，而第二質(zhì)心側(cè)偏角β Jlj設(shè)置為〇,則運(yùn)行周期k = 1時(shí)刻 車輛模型的狀態(tài)向量初值確定為.此時(shí)將車輛狀態(tài)估計(jì)的誤差方差初值設(shè)置為Plll = diag(10 10 10 10 10 10)
[0092] 預(yù)報(bào)：
[0093] (1)由k-Ι時(shí)刻估計(jì)結(jié)果得到k時(shí)刻車輛狀態(tài)量的估計(jì)值
和估計(jì)誤差方差陣 P k I k。由于使用的是容積卡爾曼濾波估計(jì)算法，所以需要生成車輛行駛狀態(tài)量的Cu b a t u r e 點(diǎn)，因此對(duì)Pklk進(jìn)行Cholesky分解，即可得到在k時(shí)刻估計(jì)誤差方差陣的平方根矩陣
[0094] (2)根據(jù)球面-徑向規(guī)則對(duì)車輛狀態(tài)量估計(jì)值生成一組等權(quán)值的Cubature點(diǎn)， 即在車輛狀態(tài)量估計(jì)值周圍以估計(jì)誤差方差陣的平方根矩陣形成一組均勻分布的點(diǎn)。根 據(jù)球面-徑向規(guī)則生成的Cubature點(diǎn)的個(gè)數(shù)是狀態(tài)維數(shù)的二倍，三自由度車輛模型的 狀態(tài)變量個(gè)數(shù)為3,則每個(gè)狀態(tài)量的Cubature點(diǎn)個(gè)數(shù)為6,即.
i = 1，2,…6,式中X1, k|k是在k時(shí)刻狀態(tài)量的第i個(gè)Cubature點(diǎn)；ξ i為矩陣
I的第i列，η為狀態(tài)量維數(shù)。
[0095] (3)利用離散化的車輛動(dòng)態(tài)方程對(duì)車輛狀態(tài)量的每一個(gè)Cubature點(diǎn)進(jìn)行變換，得 到車輛狀態(tài)量所有的Cubature點(diǎn)的預(yù)報(bào)值：
T;F為離散化形式的車輛模型的動(dòng)態(tài)方程。
[0096] (4)根據(jù)球面-徑向規(guī)則求狀態(tài)量的預(yù)報(bào)值。對(duì)所有的車輛狀態(tài)向量Cubature點(diǎn) 的與報(bào)紙進(jìn)行加權(quán)求和。每個(gè)Cubature點(diǎn)的權(quán)值均為l/m，m = 2n，則Cubature點(diǎn)權(quán)值為 1/6。車輛狀態(tài)向量預(yù)報(bào)值為
[0097] (5)得到車輛狀態(tài)向量的預(yù)報(bào)值后，還需要計(jì)算預(yù)報(bào)誤差協(xié)方差陣Pk+1|k:即
[0098] 式中：QkS k時(shí)刻車輛狀態(tài)向量預(yù)報(bào)誤差方差陣。
[00"] 濾波：
[0100] (1)得到車輛模型狀態(tài)向量的預(yù)報(bào)值和預(yù)報(bào)誤差方差陣后，就需要利用量測(cè)量對(duì) 狀態(tài)量預(yù)報(bào)值進(jìn)行濾波，進(jìn)而得到車輛狀態(tài)量的估計(jì)值。該過程需要利用量測(cè)方程對(duì)車輛 狀態(tài)向量的預(yù)報(bào)值進(jìn)行變換，進(jìn)而得到狀態(tài)估計(jì)量量測(cè)的預(yù)報(bào)值，并計(jì)算量測(cè)量的預(yù)報(bào)誤 差方差陣。因此，對(duì)狀態(tài)量預(yù)報(bào)誤差協(xié)方差陣Pk+1|k進(jìn)行Cholesky分解，得到狀態(tài)量預(yù)報(bào)誤 差方差陣的平方根矩陣Sk+1|k，即
[0101] (2)利用球面-徑向規(guī)則在車輛狀態(tài)量的預(yù)報(bào)值周圍按照狀態(tài)量預(yù)報(bào)誤差方差平 方根矩陣生成一組等權(quán)值的點(diǎn)，即Cubature點(diǎn)Xi,k+1|k:
[0102] (3)通過動(dòng)態(tài)狀態(tài)估計(jì)模型中的量測(cè)方程對(duì)每個(gè)狀態(tài)量預(yù)報(bào)值Cubature點(diǎn)進(jìn)行 變換，得到量測(cè)量 z (t) = [ay]預(yù)報(bào)值的 Cubature 點(diǎn) ZiMllkiZiMllk= H(Xi, k+1|k，uk)，式中 H為量測(cè)方程。
[0103] (4)對(duì)所有量測(cè)量預(yù)報(bào)值的Cubature點(diǎn)進(jìn)行加權(quán)求和，進(jìn)而得到量測(cè)量預(yù)報(bào) 值。根據(jù)球面-徑向規(guī)則，每個(gè)Cubature點(diǎn)的權(quán)值為1/6,則量測(cè)量的預(yù)報(bào)值島i+yk:
[0104] (5)計(jì)算量測(cè)量的預(yù)報(bào)誤差方差陣Pzz,k+1|k:
式中Rk+1為k+1 時(shí)刻量測(cè)誤差方差陣。
[0106] (6)計(jì)算車輛狀態(tài)量預(yù)報(bào)值和量測(cè)量預(yù)報(bào)值之間的互協(xié)方差矩陣Pxz, k+1|k:
[0107] (7)計(jì)算卡爾曼濾波增益
[0108] (8)利用k+Ι時(shí)刻量測(cè)量的量測(cè)值與量測(cè)量預(yù)報(bào)值之間的偏差，即新息，通過卡 爾曼濾波增益對(duì)車輛狀態(tài)量預(yù)報(bào)值進(jìn)行濾波，最終得到k+Ι時(shí)刻狀態(tài)量估計(jì)值
式中zk+