(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

隨著電子安全技術(shù)的發(fā)展，主動(dòng)安全系統(tǒng)如電子穩(wěn)定控制系統(tǒng)(Electronic stability control system, ESC)和防抱死制動(dòng)系統(tǒng)(Anti-lock brake system, ABS)在危及安全的情況下發(fā)揮著越來越重要的作用。例如，2015年ESC在美國挽救了1 949 人的生命，但仍有2 272 人死于各類車輛事故[1]。為了保證此類安全系統(tǒng)的最佳性能，獲取的車輛姿態(tài)信息必須非常準(zhǔn)確，航向角輕微的偏差將會(huì)導(dǎo)致車輛偏離原來的行駛路線；質(zhì)心側(cè)偏角是判斷車輛是否穩(wěn)定的關(guān)鍵依據(jù)；橫擺角速度是車輛動(dòng)力學(xué)控制系統(tǒng)進(jìn)行控制決策的主要依據(jù)?，F(xiàn)有的車輛姿態(tài)測量數(shù)據(jù)來源主要有傳感器、陀螺儀和全球?qū)Ш叫l(wèi)星定位系統(tǒng)[2-5]。然而，在測量過程中，車輛姿態(tài)的數(shù)據(jù)通常會(huì)受噪聲的污染以及環(huán)境的影響，難以精確或者經(jīng)濟(jì)地測量車輛姿態(tài)，這就產(chǎn)生了對可靠的在線估計(jì)算法的需求。

1 問題描述

智能車輛轉(zhuǎn)向系統(tǒng)如圖1所示。圖1中XOY為慣性坐標(biāo)系；a和b分別為車輛質(zhì)心到前軸和后軸的距離；Fyf和Fyr分別為車輛前輪和后輪輪胎側(cè)向力；δf為前輪轉(zhuǎn)向角；v為車輛速度，vx為車輛縱向速度，vy為車輛橫向速度；β為質(zhì)心側(cè)偏角；r為橫擺角速度。

圖1 車輛轉(zhuǎn)向系統(tǒng)模型Fig.1 The model of vehicle steering system

在車輛動(dòng)力學(xué)建模中，為簡化模型，通常忽略空氣動(dòng)力學(xué)和懸架俯仰運(yùn)動(dòng)的影響，假設(shè)縱向速度在短時(shí)間內(nèi)保持不變，忽略縱向動(dòng)力學(xué)，則二自由度動(dòng)力學(xué)模型的數(shù)學(xué)表達(dá)式為

(1)

式中：m為車輛質(zhì)量；Iz為車輛繞z軸的轉(zhuǎn)動(dòng)慣量；Cf和Cr分別為車輛前輪和后輪側(cè)偏剛度。在車輛側(cè)向輪胎力未達(dá)到飽和狀態(tài)時(shí)，前、后輪側(cè)向力與輪胎側(cè)偏角基本呈線形關(guān)系，其表達(dá)式分別為

(2)

在運(yùn)動(dòng)學(xué)建模中，假設(shè)車輛為剛體。車輛縱向位置x0、側(cè)向位置y0和橫擺角φ可分別表示為

(3)

基于小角度θ假設(shè)(θ表示每個(gè)角度，包括前輪偏轉(zhuǎn)角和前后輪胎側(cè)偏轉(zhuǎn)角)，其滿足近似條件cosθ≈1，sinθ≈θ。則質(zhì)心側(cè)偏角β可近似為

車輛縱向位置x0和側(cè)向位置y0可簡化為

(4)

聯(lián)合式(1，4)，可得車輛轉(zhuǎn)向系統(tǒng)模型為

(5)

其中系數(shù)矩陣A和B分別為

為獲取車輛姿態(tài)信息需對車輛狀態(tài)進(jìn)行采樣測量。以Ts為采樣周期，將式(5)離散化得

x(T+1)=Acx(T)+Bcδf(T)+w(T)

(6)

y(T)=Cx(T)+v(T)

(7)

式中：y∈Rp為T時(shí)刻收集到的車輛數(shù)據(jù)；C為已知觀測矩陣，(C,Ac)可觀；v∈V?Rp為T時(shí)刻接收到的車輛數(shù)據(jù)中所帶有的不確定噪聲，滿足vmin≤v≤vmax。

由于外界環(huán)境中不確定擾動(dòng)的影響以及現(xiàn)有測量工具的局限性，車輛姿態(tài)信息通常難以精確地測量，或者無法經(jīng)濟(jì)地測量，如測量車輛質(zhì)心側(cè)偏角和外部擾動(dòng)的傳感器十分昂貴，且信號的可靠性問題也尚未完全解決，使得這些信息目前在實(shí)際車輛系統(tǒng)中很難通過傳感器測量直接獲得。GNSS定位系統(tǒng)雖然可輸出航向角度信息，但其隨機(jī)噪聲大，且速度越低噪聲越大。此外，衛(wèi)星信號的傳遞經(jīng)常因受到地形和地物的影響而發(fā)生信號錯(cuò)誤，甚至出現(xiàn)信號丟失等問題，因此在實(shí)際使用時(shí)經(jīng)常會(huì)出現(xiàn)GPS無法提供信息或者所提供的信息偏差較大的問題。陀螺儀用于測量橫擺角速度，但存在幾何尺寸大、強(qiáng)度差、容易掉電和噪聲大等問題?，F(xiàn)在使用較多的微機(jī)械陀螺儀的幾何尺寸雖然有所減小，噪聲問題也有所改善，但還是存在著諸如標(biāo)定誤差和溫度漂移等問題，其價(jià)格隨性能的提高而增加，并且陀螺儀安裝比較麻煩，要盡量安裝在車輛質(zhì)心位置，限制了它的廣泛使用。

綜合考慮硬件測量的精度、經(jīng)濟(jì)性和可操作性，設(shè)計(jì)觀測器以減少對硬件的依賴非常必要。筆者基于噪聲的不確定性，設(shè)計(jì)了魯棒滾動(dòng)時(shí)域估計(jì)，有效利用測量數(shù)據(jù)來估計(jì)車輛的姿態(tài)信息。

2 轉(zhuǎn)向姿態(tài)魯棒滾動(dòng)時(shí)域估計(jì)

(8)

與經(jīng)典滾動(dòng)時(shí)域估計(jì)的目標(biāo)函數(shù)不同，考慮數(shù)據(jù)測量以及傳輸過程中不確定噪聲的影響，在目標(biāo)函數(shù)中添加噪聲抑制項(xiàng)來獲得魯棒性。參考H∞濾波的博弈近似理論[25,27]，定義擾動(dòng)抑制函數(shù)為

(9)

考慮數(shù)據(jù)傳輸時(shí)受不確定性噪聲的影響，基于不確定離散線性模型式(6)的魯棒全信息滾動(dòng)時(shí)域估計(jì)問題(Robust full information estimation, RFIE)可表示為

(10)

s.t.xk+1=Acxk+Bcδfk+wk∈X
vk=yk-Cxk∈V
wk∈W,k∈I0∶T-1

(11)

其中目標(biāo)函數(shù)為

(12)

隨著時(shí)間的推移，全信息滾動(dòng)時(shí)域估計(jì)需處理更多的數(shù)據(jù)，為使估計(jì)問題可行并在計(jì)算上易于處理，引入固定時(shí)域N。當(dāng)T>N時(shí)，優(yōu)化問題RFIE的計(jì)算時(shí)域可分為{0≤k≤T-N-1}和{T-N≤k≤T}。則目標(biāo)函數(shù)式(12)可改寫為

對任意可行初始狀態(tài)x0和干擾序列{wk}，定義T時(shí)刻系統(tǒng)狀態(tài)可達(dá)集為

(13)

取s∈Sa，到達(dá)代價(jià)為

(14)

其中問題式(14)滿足約束式(11)。

為適用于車輛快速轉(zhuǎn)向，引入擾動(dòng)塊結(jié)構(gòu)，在估計(jì)過程中，每個(gè)時(shí)刻均對估計(jì)窗口內(nèi)的擾動(dòng)進(jìn)行分塊，強(qiáng)制令同一擾動(dòng)塊結(jié)構(gòu)內(nèi)的擾動(dòng)值相等，從而減少所求的優(yōu)化變量，降低計(jì)算量。

圖的塊結(jié)構(gòu)示意圖Fig.2 Segment structure of

給定估計(jì)時(shí)域N>n，在T時(shí)刻求解以下帶有擾動(dòng)塊的魯棒滾動(dòng)時(shí)域估計(jì)問題(Blocking moving horizon estimation, BRMHE)，即

(15)

(16)

(17)

到達(dá)代價(jià)為

(18)

(19)

(20)

算法步驟：

1) (初始化)：當(dāng)T=0時(shí)，設(shè)定矩陣P0和車輛初始狀態(tài)x0，其中P0為初始條件x0的協(xié)方差矩陣。

2) 當(dāng)1≤T≤N時(shí)，求解優(yōu)化問題RFIE。

3) 當(dāng)T>N時(shí)，求解基于擾動(dòng)塊的魯棒滾動(dòng)時(shí)域估計(jì)問題BRMHE。

3 可行性與穩(wěn)定性分析

針對RFIE問題可行性，為符號簡便，式(12)記為

(21)

為證明算法可行性，需要以下引理來保證RFIE從x0估計(jì)x1的鞍點(diǎn)解的存在。

引理1[25]假設(shè)X，W，V為凸的緊集，ΦT在(x0,w0,v0)處連續(xù)。若對?v0∈V，ΦT在點(diǎn)(x0,w0)處為凸；對?x0∈X,w0∈W，ΦT在點(diǎn)v0處為凹，則函數(shù)ΦT存在鞍點(diǎn)。若函數(shù)ΦT為嚴(yán)格凹凸，則該鞍點(diǎn)是唯一的。

(22)

(23)

(24)

定義1[18]考慮線性系統(tǒng)，即

xk+1=Acxk+Bcδfk,yk=Cxk

(25)

(26)

為書寫清晰，令

(27)

基于引理3，可獲得如下結(jié)論：

定理1考慮車輛轉(zhuǎn)向模型式(6)，假設(shè)矩陣P0,Q,R正定，且假設(shè)1成立，則RFIE是漸近穩(wěn)定的。

(28)

為證明穩(wěn)定性，設(shè)ε>0，考慮T=n時(shí)，選擇足夠小的γ>0。若選擇δ>0，使得ρδ2<γ，則對所有T≥n有

(29)

圖3 S=2時(shí)的塊結(jié)構(gòu)示意圖Fig.3 Segment structure of when S=2

最大分塊長度塊結(jié)構(gòu)的擾動(dòng)示意圖如圖3所示。式(15～18)特例可表示為

(30)

(31)

為符號簡便，式(17)記為

(32)

通過定理2和定理3討論BRMHE的可行性和引入擾動(dòng)塊結(jié)構(gòu)后算法的估計(jì)性能。

(33)

(34)

(35)

則式(15～18)在每個(gè)采樣時(shí)間均存在唯一鞍點(diǎn)解，其中PT由式(20)給出。

存在唯一解，即式(33,34)。

(36)

假設(shè)2系統(tǒng)矩陣Ac非奇異且滿足

定理2若假設(shè)2成立，且S

滿足

推論1若將窗口前N-1個(gè)擾動(dòng)分為S塊，BRMHE問題可行性和估計(jì)性能同樣能得到保證。

證明由定理1可知，S=2時(shí)等價(jià)解存在，且S=2為S∈I3∶N的特殊情況，故當(dāng)S∈I3∶N時(shí)至少存在一組等價(jià)解。其中S=N時(shí)，BRMHE等價(jià)于不設(shè)有塊結(jié)構(gòu)的RMHE。

基于推論1，若S=2時(shí)BRMHE收斂，則當(dāng)S∈I3∶N時(shí)算法收斂性依然存在。故驗(yàn)證S=2時(shí)(最小分塊數(shù)量)算法的收斂性。

為書寫清晰，令

(37)

引理6考慮車輛轉(zhuǎn)向模型式(6)，則對任意s∈Rn，協(xié)方差矩陣PT滿足

(38)

該引理證明過程與文獻(xiàn)[28]引理4.4.4相似，此處省略。

定理3考慮網(wǎng)聯(lián)車隊(duì)模型式(6)，若假設(shè)1，2成立，且N≥n，BRMHE估計(jì)器是漸近穩(wěn)定的。

(39)

根據(jù)假設(shè)1，有

(40)

引理6可保證

(41)

聯(lián)合式(40,41)，則有

(42)

為證明穩(wěn)定性，設(shè)ε>0，考慮T=N時(shí)，選擇足夠小的γ>0。若選擇δ>0，使得ρδ2<γ，則對所有T≥N有

(43)

考慮到min-max問題具有較高的計(jì)算復(fù)雜度，因此引入坐標(biāo)輪換法，分離min-max問題近似求解。min-max問題近似求解過程為

(44)

其中極小化問題滿足約束式(11)。

(45)

其中優(yōu)化問題滿足約束式(11)。

若初始狀態(tài)的置信度低，則可轉(zhuǎn)至步驟2)進(jìn)行多輪循環(huán)，直至連續(xù)兩次循環(huán)求得的最優(yōu)解的差小于給定收斂精度。魯棒滾動(dòng)時(shí)域估計(jì)最小-最大問題的近似解類似于魯棒全信息滾動(dòng)時(shí)域估計(jì)。

4 仿真與結(jié)果分析

本節(jié)考慮在同向雙車道上勻速運(yùn)行的智能車輛，以評估所提算法的有效性。仿真中使用的自主車輛模型選取CarSim軟件中的C級掀背式轎車模型，車輛模型中參數(shù)以及控制器參數(shù)如表1所示。

表1 車輛及控制器參數(shù)Table 1 Vehicle and controller parameters

設(shè)自主車輛縱向行駛速度vx=25 m/s，初始位置x0=0 m，y0=-2 m，在行駛過程中設(shè)置靜態(tài)障礙，如圖4所示。采用模型預(yù)測控制(Model predictive control, MPC)規(guī)劃避障路徑[29]，假設(shè)該障礙物相鄰車道有足夠的橫向空間，自主車輛經(jīng)控制后，可從障礙物側(cè)面安全避開。為準(zhǔn)確獲取車輛行駛過程中的狀態(tài)，通過測量狀態(tài)分量x1(車輛縱向位置)來估計(jì)狀態(tài)。仿真滿足假設(shè)1～4，設(shè)不確定擾動(dòng)w∈[-0.1,0.1]，不確定噪聲v∈[-1,1]，α,P0,Q,R滿足式(33～35)，分別為α=0.9，P0=diag[0.01,0.01,0.01,0.01]，Q=diag[9×10-4,9×10-4,9×10-4,9×10-4]，R=0.1。采樣時(shí)間Ts=0.5 s，滾動(dòng)窗口N=6，初始狀態(tài)x0=[-2,0,0,0]。

圖4 MPC規(guī)劃路徑Fig.4 MPC planning path

在仿真中分別對比了3 種不同塊結(jié)構(gòu)BRMHE算法：1) 標(biāo)準(zhǔn)RMHE算法(擾動(dòng)不分塊)；2) 將擾動(dòng)分別分為長度為d1=3，d2=2，d3=1的塊結(jié)構(gòu)，記為RMHE-OP；3) 將擾動(dòng)以最大分塊形式劃分，即窗口內(nèi)最后擾動(dòng)單獨(dú)分為1 塊，其余擾動(dòng)分為1 塊，記為RMHE-OF。仿真結(jié)果如圖5，6所示，圖5，6分別為各算法估計(jì)狀態(tài)軌跡圖和估計(jì)誤差圖：實(shí)線表示實(shí)際狀態(tài)軌跡；虛線、點(diǎn)劃線和雙劃線分別表示RMHE，RMHE-OP和RMHE-OF的仿真結(jié)果。各算法的估計(jì)均方誤差(MSE)和各算法所需的平均計(jì)算時(shí)間(Tav)如表2所示。仿真結(jié)果顯示：3 種魯棒分塊算法的估計(jì)狀態(tài)軌跡均收斂于實(shí)際軌跡，且均方誤差相近，此外在塊結(jié)構(gòu)約束下，算法RMHE，RMHE-OP和RMHE-OF的優(yōu)化變量分別為7，4，3 個(gè)。分析圖6和表2可知：各算法的計(jì)算速度滿足RMHE

圖5 狀態(tài)軌跡Fig.5 State trajectory

圖6 各算法估計(jì)誤差Fig.6 Estimation error of each algorithm

表2 各算法估計(jì)均方誤差(MSE)和平均計(jì)算時(shí)間(Tav)

5 結(jié) 論

通過結(jié)合滾動(dòng)時(shí)域估計(jì)算法與博弈論方法，設(shè)計(jì)可處理不確定噪聲的魯棒滾動(dòng)時(shí)域估計(jì)器。采用坐標(biāo)輪換法近似求解min-max優(yōu)化問題。為降低算法復(fù)雜度，在魯棒滾動(dòng)時(shí)域估計(jì)算法中，通過設(shè)計(jì)擾動(dòng)塊結(jié)構(gòu)以及構(gòu)造等價(jià)解有效降低算法計(jì)算量，從而達(dá)到快速估計(jì)的目的。仿真結(jié)果表明：設(shè)計(jì)的魯棒滾動(dòng)時(shí)域估計(jì)算法能有效處理智能車輛轉(zhuǎn)向系統(tǒng)中帶有不確定噪聲的姿態(tài)估計(jì)問題，具有較好的抗擾性，且魯棒快速滾動(dòng)時(shí)域估計(jì)算法中擾動(dòng)分塊數(shù)量越少，算法估計(jì)時(shí)間越短，對于滿足可行性條件的不同分塊形式的魯棒滾動(dòng)時(shí)域估計(jì)算法，均可在保證估計(jì)性能的基礎(chǔ)上降低計(jì)算時(shí)間。筆者主要討論車輛轉(zhuǎn)向姿態(tài)模型中基于不確定擾動(dòng)與測量噪聲的估計(jì)，后續(xù)還可進(jìn)一步考慮模型的結(jié)構(gòu)和參數(shù)的不確定性問題。