1 引言

導(dǎo)航是智能車領(lǐng)域中基本問題，其任務(wù)是利用傳感器獲得的量測(cè)信息，對(duì)智能車當(dāng)前位置進(jìn)行精確的估計(jì)。智能車正常行駛中，動(dòng)態(tài)模型通常都是在笛卡爾坐標(biāo)系下建立的，而全球定位系統(tǒng)(Global Positioning System，GPS)量測(cè)值都是以極坐標(biāo)形式表示，因此，智能車位置估計(jì)也就變?yōu)橐粋€(gè)非線性估計(jì)問題[1-3]。

當(dāng)模型中的非線性因素不可忽略時(shí)，通常采用非線性濾波進(jìn)行狀態(tài)估計(jì)。應(yīng)用較廣泛的方法是擴(kuò)展卡爾曼濾波器(Extended Kalman filer，EKF)[4-5]，這種方法存在線性化誤差大、雅可比矩陣難于計(jì)算等缺陷，濾波不穩(wěn)定，甚至發(fā)散[6-7]。S.J.Juliear和J.K.Uhmann在1997提出基于無跡(Unscented，UT)變換的無跡卡爾曼濾波(UnscentedKalman Filter，UKF)，其均值和協(xié)方差的估計(jì)比EKF方法要精確[7]。

UKF雖然克服了EKF存在的一系列問題，但系統(tǒng)噪聲相關(guān)信息的不確定性以及狀態(tài)模型擾動(dòng)等都會(huì)影響UKF的濾波精度。鑒于此，本文提出了一種帶衰減因子的自適應(yīng)無跡卡爾曼濾波算法(IMM—AFUKF)，該算法基于交互式多模型結(jié)構(gòu)(Interacting multiple mode，IMM) [3]，克服了模型不確定性因素的影響，并引入衰減因子及自適應(yīng)漸消因子[3]來修正預(yù)測(cè)誤差協(xié)方差，調(diào)整濾波增益，減弱舊數(shù)據(jù)對(duì)濾波的影響，從而消除濾波的發(fā)散現(xiàn)象。最終得到具有更高精度、更強(qiáng)抗擾動(dòng)能力的濾波器，以滿足無人駕駛智能車控制系統(tǒng)的需求。

2 智能車導(dǎo)航系統(tǒng)控制原理

2.1無人駕駛智能車控制系統(tǒng)

本文以北京工業(yè)大學(xué)無人駕駛智能車BJUT—IV為對(duì)象，研究其位置估計(jì)問題。BJUT—IV控制系統(tǒng)包含三大模塊：環(huán)境感知模塊，決策模塊，底層控制模塊，如圖1所示，其中環(huán)境感知模塊主要包括車外環(huán)境感知模塊、導(dǎo)航定位模塊、車體信息模塊。其中導(dǎo)航定位模塊提供車輛在全局地圖中的位置和速度，用于全局路徑規(guī)劃、路徑跟蹤，是否能提供快速、準(zhǔn)確、可靠的導(dǎo)航信息直接決定上位機(jī)的決策的正確性，并最終影響底層控制的效果。

2.2導(dǎo)航系統(tǒng)車體位置模型

BJUT—IV采用GPS作為導(dǎo)航系統(tǒng)，GPS輸出坐標(biāo)為ECEF坐標(biāo)系統(tǒng)下的經(jīng)度、緯度、高度，并不適合于平面運(yùn)動(dòng)的車輛導(dǎo)航。本文采用通用橫軸墨卡托坐標(biāo)系(Universal Transverse Mercator，UTM)將經(jīng)緯度轉(zhuǎn)化到平面坐標(biāo)系，如圖2所示UTM坐標(biāo)系統(tǒng)原理。在實(shí)際應(yīng)用中，UTM坐標(biāo)的轉(zhuǎn)換公式較為復(fù)雜，且在系統(tǒng)中有實(shí)時(shí)性的要求下很難達(dá)到，因此本文采用經(jīng)過簡(jiǎn)化處理的一組UTM坐標(biāo)轉(zhuǎn)換公式[8-9]，篇幅限制不再贅述。將坐標(biāo)原點(diǎn)o定于北緯39.8719，東經(jīng)116.4789，車體位置相對(duì)坐標(biāo)原點(diǎn)關(guān)系如圖3所示。

在直角坐標(biāo)系下，車輛所處位置的方位角、斜距之間關(guān)系為

BJUT—IV的主要運(yùn)動(dòng)方式為勻加速行駛、勻速行駛、勻速水平轉(zhuǎn)彎，本文在不影響實(shí)際模型精度的情況下將勻速水平轉(zhuǎn)彎運(yùn)動(dòng)狀態(tài)近似為勻速行駛狀態(tài)。BJuT—IV行駛軌跡水平高差均值為41.6/m，無明顯高差，因此車輛縱向運(yùn)動(dòng)軌跡問題本文不加考慮。

為驗(yàn)證提出的帶衰減因子的自適應(yīng)濾波算法IMM-AFUKF的性能，文章第四部分將其與基于IMM模型的UKF濾波算法、基于勻加速運(yùn)動(dòng)模型(CA)的EKF濾波算法進(jìn)行了仿真及實(shí)驗(yàn)的對(duì)比和分析。

2.3導(dǎo)航系統(tǒng)運(yùn)動(dòng)控制模型

根據(jù)智能車的運(yùn)動(dòng)狀態(tài)與典型模型匹配程度及其復(fù)雜性，本文選擇兩個(gè)典型的二維智能車運(yùn)動(dòng)控制模型建立IMM模型組，分別為勻速運(yùn)動(dòng)控制模型(Constant Velocity，CV)、勻加速運(yùn)動(dòng)控制模型(Constant Acceleration，CA)[10-11]。

智能車運(yùn)動(dòng)學(xué)模型可表示為

勻加速模型(Constant Acceleration，CA)取狀態(tài)變量X

為狀態(tài)轉(zhuǎn)移陣，其它參數(shù)設(shè)置同上。

2.4濾波器量測(cè)模型

量測(cè)方程可由下式給出

2.5智能車導(dǎo)航系統(tǒng)技術(shù)難點(diǎn)

導(dǎo)航定位模塊中GPS提供的速度、位置、航向等導(dǎo)航定位信息本身帶有噪聲，且由于環(huán)境因素會(huì)產(chǎn)生較大誤差，需通過濾波器濾波后發(fā)，發(fā)送至決策計(jì)算機(jī)進(jìn)行決策并完成車體控制。

因此，研究具有更高性能的濾波器意在為車輛控制系統(tǒng)提供更快速、準(zhǔn)確而穩(wěn)定的地理位置及速度信息。進(jìn)一步講，濾波器的性能直接決定了導(dǎo)航定位系統(tǒng)的性能，并最終影響車體控制。所以，對(duì)于濾波器的優(yōu)化在導(dǎo)航定位系統(tǒng)乃至車輛控制系統(tǒng)中具有十分重要的意義。

現(xiàn)階段導(dǎo)航系統(tǒng)中，針對(duì)濾波器的研究仍然存在以下幾個(gè)難點(diǎn)問題：①實(shí)際路況中，存在大量非線性因素：智能車導(dǎo)航系統(tǒng)中濾波器普遍采用的擴(kuò)展卡爾曼算法(EKF) [8]，該算法雖能實(shí)現(xiàn)非線性濾波，但是濾波精度已無法滿足智能車導(dǎo)航的需求;②實(shí)際車況下，車輛運(yùn)動(dòng)狀態(tài)多變：目前采用無跡卡爾曼濾波(UKF)或UKF的優(yōu)化算法[7]，在精度上較傳統(tǒng)的EKF有較大提高，但對(duì)于所提出的濾波算法的驗(yàn)證局限于理想化的仿真模型，依舊無法與真正智能車行駛狀態(tài)相匹配;③由于真實(shí)路況下噪聲干擾，會(huì)造成濾波效果降低甚至發(fā)散：鑒于車輛行駛狀態(tài)的多變性，有學(xué)者提出采用交互式多模型(IMM)來逼近車輛真實(shí)狀態(tài)[15]，取得較好濾波效果，但算法不具有自適應(yīng)性，且歷史數(shù)據(jù)對(duì)濾波影響較大。因此，如何找到一種高精度的，可靠的，具有自適應(yīng)性且能與無人駕駛智能車導(dǎo)航系統(tǒng)相匹配的濾波算法，仍然是現(xiàn)階段工作的技術(shù)難點(diǎn)。

3 多模型無跡卡爾曼濾波算法

3.1算法原理

模型的匹配性不佳和噪聲統(tǒng)計(jì)特性的不準(zhǔn)確經(jīng)常導(dǎo)致UKF濾波性能降低[12-13]，為了解決這一問題，本文引用自適應(yīng)UKF濾波(Adaptive UKF，AUKF)，該方法通過自適應(yīng)漸消因子來平衡狀態(tài)方程、預(yù)測(cè)信息與觀測(cè)信息的權(quán)重，能實(shí)時(shí)地對(duì)模型誤差和噪聲統(tǒng)計(jì)特性進(jìn)行估計(jì)并修正，較好地抑制模型誤差對(duì)導(dǎo)航解的影響，從而提高UKF的估計(jì)精度[14]。同時(shí)，加入衰減因子(Fading factor)[15-16]，加大當(dāng)前觀測(cè)量的作用，減小歷史觀測(cè)值的影響，達(dá)到抑制濾波發(fā)散目的。AFUKF有效克服了噪聲及與模型不匹配的不足問題，但對(duì)模型描述準(zhǔn)確性仍要求較高，引入交互式多模型算法(IMM)可較好解決此問題。IMM包含多個(gè)濾波器、模型概率估計(jì)、交互作用器、狀態(tài)估計(jì)混合器，基本原理為：通過建立不同的模型來描述系統(tǒng)可能的運(yùn)動(dòng)模態(tài)，模型間的切換由基于馬爾可夫鏈的轉(zhuǎn)換概率控制，對(duì)每個(gè)模型采用獨(dú)立的AFUKF，以模型轉(zhuǎn)移概率加權(quán)后的交互值作為濾波迭代輸入。通過輸出殘差不斷更新模型概率，對(duì)多個(gè)濾波器進(jìn)行融合，使其盡可能地接近那個(gè)最能反映實(shí)際系統(tǒng)行為方式的模型濾波輸出。AFUKF是UKF的優(yōu)化，作為IMM結(jié)構(gòu)中的并行濾波器，以下稱為IMM-AFUKF算法，結(jié)構(gòu)示意圖4所示。

3.2算法流程

IMM—AFUKF算法具體步驟如下所示：

步驟1：計(jì)算混合概率系統(tǒng)非線性隨機(jī)的狀態(tài)空間模型為

 

步驟2：交互/混合

步驟3：模型濾波

4 仿真分析及實(shí)驗(yàn)

4.1仿真及實(shí)驗(yàn)分析

實(shí)驗(yàn)室的智能車輛實(shí)驗(yàn)平臺(tái)安裝應(yīng)用載波相位技術(shù)的差分GPS(DGPS)，設(shè)備組為FlexPak-G2系列GPS接收處理機(jī)和GPS-702-GG接收天線及PDL4535數(shù)傳電臺(tái)。設(shè)備實(shí)物如圖5所示，GPS-702-GG接收天線關(guān)鍵參數(shù)如表1所示，F(xiàn)lexPak-G2系列GPS接收處理機(jī)關(guān)鍵參數(shù)如表2所示。

從仿真路徑圖中可看出，IMM-AFUKF與真實(shí)路徑最接近，誤差最小，CA-EKF，IMM-UKF均在彎道處理方面較為遜色。

同時(shí)，實(shí)驗(yàn)的采用均方根誤差(Root mean square error,RMSE)定量分析并衡量算法優(yōu)越性[1-3]，其定義為

其中，n=500。RMSE越小，說明所采用的算法與真實(shí)值的誤差越小;運(yùn)行時(shí)間越短，說明算法的計(jì)算量越小，在實(shí)際應(yīng)用中數(shù)據(jù)輸出速度越快。圖7～12為采用三種濾波方法位置、速度、加速度均方根誤差曲線比較，表3為三種濾波算法均值和均方根誤差統(tǒng)計(jì)數(shù)據(jù)。

從仿真結(jié)果及性能統(tǒng)計(jì)值比較可以看出，在仿真的初始時(shí)刻，濾波器都有較大的初始偏差，這是由于初始估值造成的。隨著濾波的進(jìn)行，IMM-AFUKF算法能很快地收斂到真值位置附近，并保持較高的濾波精度。IMM-AFUKF算法較IMM-UKF、CA-EKF算法提高了解算精度及收斂速度，狀態(tài)估計(jì)也更接近于真實(shí)狀態(tài)，且增強(qiáng)了數(shù)值穩(wěn)定性，其中IMM-AFMUKF算法的位置和速度的均方根誤差明顯小于標(biāo)準(zhǔn)IMM—UKF算法。究其原因，主要在于IMM-AFUKF算法引入了衰減因子，加大了對(duì)新測(cè)量值的利用權(quán)重，減少了對(duì)陳舊測(cè)量值和初始估值的依賴，同時(shí)增加自適應(yīng)漸消因子，能夠較好的抑制初始值偏差和模型異常擾動(dòng)誤差對(duì)導(dǎo)航解的影響，使算法具有一定的自適應(yīng)能力。因此，IMM-AFUKF算法有效的控制了初始值偏差、模型異常擾動(dòng)誤差、以及未知噪聲對(duì)濾波解的影響。

4.2實(shí)車實(shí)驗(yàn)

本文以北京工業(yè)大學(xué)無人駕駛智能車BJuT-IV為實(shí)驗(yàn)對(duì)象如圖13所示，研究其行駛軌跡的跟蹤問題。

BJUT-IV由高爾夫球電動(dòng)車經(jīng)過對(duì)電控系統(tǒng)的改裝而成。BJUT-IV采用GPS作為導(dǎo)航系統(tǒng)，并用UTM將經(jīng)緯度轉(zhuǎn)化到平面坐標(biāo)系，重新定義坐標(biāo)原點(diǎn)O于北緯39.8719，東經(jīng)116.4789即圖14中N8點(diǎn)為原點(diǎn)。BJUT-IV控制系統(tǒng)以Microsoft VS2008為軟件開發(fā)平臺(tái)，編寫了導(dǎo)航與定位系統(tǒng)軟件，利用此軟件平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，用以檢驗(yàn)IMM-AFUKF算法在智能車導(dǎo)航系統(tǒng)中的可靠性。實(shí)驗(yàn)分別采用KF(綠色軌跡)、IMM-UKF(黃色軌跡)、CA-EKF(藍(lán)色軌跡)、IMM-AFUKF(紅色軌跡)對(duì)實(shí)際道路采樣點(diǎn)進(jìn)行濾波估計(jì)(橙色軌跡代表真實(shí)道路)。取智能車的運(yùn)動(dòng)參數(shù)(位置、速度、加速度)作為系統(tǒng)的狀態(tài)變量，采用東北天坐標(biāo)系。

實(shí)驗(yàn)車輛沿道路車道線線行駛，根據(jù)實(shí)際情況做變速及勻速運(yùn)動(dòng)。實(shí)車實(shí)驗(yàn)結(jié)果表明，IMM-AFUKF算法較其它算法更準(zhǔn)確、穩(wěn)定;且在車輛轉(zhuǎn)彎時(shí)性能更加突出，如圖14中N10點(diǎn)和N9點(diǎn)彎道放大圖所示，紅色軌跡(IMM-AFUKF)幾乎與橙色軌跡(實(shí)際道路)重合，黃色軌跡(IMM-UKF)與綠色軌跡(KF)均與實(shí)際行駛軌跡有較大偏差且有較大滯后性。所提出算法更貼切的反應(yīng)車輛實(shí)際行駛軌跡，該實(shí)驗(yàn)結(jié)論與仿真結(jié)果一致，再次驗(yàn)證所提出算法優(yōu)越性。

5結(jié)束語

在北京工業(yè)無人駕駛智能車BJUT-IV導(dǎo)航系統(tǒng)中，UKF處理非線性問題較EKF性能優(yōu)越，然而，UKF算法對(duì)模型匹配度要求較高、受系統(tǒng)噪聲影響較大，且對(duì)初始值較敏感，為此，本文在UKF中引入衰減因子及自適應(yīng)漸消因子，并結(jié)合IMM濾波結(jié)構(gòu)，提出了一種針對(duì)BJUT-IV導(dǎo)航系統(tǒng)的濾波算法IMM-AFUKF。理論分析、仿真、實(shí)車實(shí)驗(yàn)結(jié)果均表明，IMM-AFUKF算法的濾波精度、穩(wěn)定性、收斂速度都明顯優(yōu)于標(biāo)準(zhǔn)IMM-UKF及EKF算法，具有較高實(shí)用價(jià)值。

該算法改善了導(dǎo)航系統(tǒng)精度，為上位機(jī)的控制決策提供良好的導(dǎo)航信息數(shù)據(jù)，將無人駕駛智能車控制系統(tǒng)整體性能進(jìn)行了提升。為進(jìn)一步改進(jìn)IMM-AFUKF對(duì)初始值過于敏感等情況，需要在魯棒性方面做進(jìn)一步的探討;并需克服模型轉(zhuǎn)換時(shí)濾波精度降低的問題，以上兩點(diǎn)將作為后續(xù)工作重點(diǎn)。

參考文獻(xiàn)：

[1]楊柳.改進(jìn)卡爾曼濾波的目標(biāo)跟蹤研究[J]·計(jì)算機(jī)仿真，[J] 2010—9：351—355.

[2]付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

[3]秦永兒，張洪城，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北土業(yè)大學(xué)出版社，1998.

[4]Yang Yuanxi，Gao Weiguang.A new learning statimic for adaptive filter based on predicted residuals[J].Progress in Natural SCience，2006—8：833—837.

[5]Cui Xianqiang and Yang Yuanxi.Adaptively robust filtering with classified adaptive factors[J].Progress in Natural Science，2006—8：846—851.

[6]Gao Sheshenga，Gao Yia，Zhong Yongminb，Wei Wenhuia.Random Weighting Estimation Method for Dynamic Navigation Positioning[J].Chinese Journal of Aeronautics，2011—3：318—323.

[7] 黃銚，張?zhí)祢U，高清山，李越雷.一種提高無跡卡爾曼濾波精確度的方法[J].計(jì)算機(jī)仿真，2010，03：348—352.

[8] 王凡.智能車輛中基于GPS和陀螺儀的導(dǎo)航與定位[D].北京工業(yè)大學(xué)。2013.

[9] 王凡.智能車輛中基于GPS和陀螺儀的導(dǎo)航與定位[D].北京工業(yè)大學(xué)。2013.

[10] 潘泉，劉剛.聯(lián)合交互式多模型概率數(shù)據(jù)關(guān)聯(lián)算法[J].航空學(xué)報(bào)，999，(3)：234—238.

[11]You Lilan,bu Dajie，Huang Jian，Liu Xue，Zhou Kechang.The Mathematic Models of the Kalman Filtering for Across—Fault Measurement[J].Earthquake Research in China，1994—3：92—103.

[12] 范韜，茅旭初.基于非線性濾波算法的高動(dòng)態(tài)GPS定位模型[J].計(jì)算機(jī)仿真，2011—10：66—69.

[13] Liu Ye，Yu Anxi，Zhu Jubo，Liang Diannong.Unscented Kalman filtering in the additive noise case[J].Science China(Teehnological Sciences)，2010-4：929—941.

[14] Hu Hmdong and Huang Xianlin.SINS/CNS/GPS integrated navisanon algorithm based on UKF[J].Journal of Systems Engineering andElectronics.2010一1：102—109.

[15] 陸可，肖建.IMM算法實(shí)現(xiàn)非線性狀態(tài)估計(jì)的研究與仿真[J].計(jì)算機(jī)仿真，2008—5：77—80，104.

[16]Yang Yuanxi，Gao Weiguang.An optimal adaptive Kalman filter[J].Journal of Geodesy，2006，80(4)：177 183.

[17]夏啟軍，孫優(yōu)賢，周春暉.漸消卡爾曼濾波器的最佳自適應(yīng)算法及其應(yīng)用[J].自動(dòng)化學(xué)報(bào)，1990—3：210—216.