1 引言
導航是智能車領域中基本問題，其任務是利用傳感器獲得的量測信息，對智能車當前位置進行精確的估計。智能車正常行駛中，動態(tài)模型通常都是在笛卡爾坐標系下建立的，而全球定位系統(Global Positioning System，GPS)量測值都是以極坐標形式表示，因此，智能車位置估計也就變?yōu)橐粋€非線性估計問題[1-3]。
當模型中的非線性因素不可忽略時，通常采用非線性濾波進行狀態(tài)估計。應用較廣泛的方法是擴展卡爾曼濾波器(Extended Kalman filer，EKF)[4-5]，這種方法存在線性化誤差大、雅可比矩陣難于計算等缺陷，濾波不穩(wěn)定，甚至發(fā)散[6-7]。S.J.Juliear和J.K.Uhmann在1997提出基于無跡(Unscented，UT)變換的無跡卡爾曼濾波(UnscentedKalman Filter，UKF)，其均值和協方差的估計比EKF方法要精確[7]。
UKF雖然克服了EKF存在的一系列問題，但系統噪聲相關信息的不確定性以及狀態(tài)模型擾動等都會影響UKF的濾波精度。鑒于此，本文提出了一種帶衰減因子的自適應無跡卡爾曼濾波算法(IMM—AFUKF)，該算法基于交互式多模型結構(Interacting multiple mode，IMM) [3]，克服了模型不確定性因素的影響，并引入衰減因子及自適應漸消因子[3]來修正預測誤差協方差，調整濾波增益，減弱舊數據對濾波的影響，從而消除濾波的發(fā)散現象。最終得到具有更高精度、更強抗擾動能力的濾波器，以滿足無人駕駛智能車控制系統的需求。
2 智能車導航系統控制原理
2.1無人駕駛智能車控制系統
本文以北京工業(yè)大學無人駕駛智能車BJUT—IV為對象，研究其位置估計問題。BJUT—IV控制系統包含三大模塊：環(huán)境感知模塊，決策模塊，底層控制模塊，如圖1所示，其中環(huán)境感知模塊主要包括車外環(huán)境感知模塊、導航定位模塊、車體信息模塊。其中導航定位模塊提供車輛在全局地圖中的位置和速度，用于全局路徑規(guī)劃、路徑跟蹤，是否能提供快速、準確、可靠的導航信息直接決定上位機的決策的正確性，并最終影響底層控制的效果。
2.2導航系統車體位置模型
BJUT—IV采用GPS作為導航系統，GPS輸出坐標為ECEF坐標系統下的經度、緯度、高度，并不適合于平面運動的車輛導航。本文采用通用橫軸墨卡托坐標系(Universal Transverse Mercator，UTM)將經緯度轉化到平面坐標系，如圖2所示UTM坐標系統原理。在實際應用中，UTM坐標的轉換公式較為復雜，且在系統中有實時性的要求下很難達到，因此本文采用經過簡化處理的一組UTM坐標轉換公式[8-9]，篇幅限制不再贅述。將坐標原點o定于北緯39.8719，東經116.4789，車體位置相對坐標原點關系如圖3所示。
在直角坐標系下，車輛所處位置的方位角、斜距之間關系為
BJUT—IV的主要運動方式為勻加速行駛、勻速行駛、勻速水平轉彎，本文在不影響實際模型精度的情況下將勻速水平轉彎運動狀態(tài)近似為勻速行駛狀態(tài)。BJuT—IV行駛軌跡水平高差均值為41.6/m，無明顯高差，因此車輛縱向運動軌跡問題本文不加考慮。
為驗證提出的帶衰減因子的自適應濾波算法IMM-AFUKF的性能，文章第四部分將其與基于IMM模型的UKF濾波算法、基于勻加速運動模型(CA)的EKF濾波算法進行了仿真及實驗的對比和分析。
2.3導航系統運動控制模型
根據智能車的運動狀態(tài)與典型模型匹配程度及其復雜性，本文選擇兩個典型的二維智能車運動控制模型建立IMM模型組，分別為勻速運動控制模型(Constant Velocity，CV)、勻加速運動控制模型(Constant Acceleration，CA)[10-11]。
智能車運動學模型可表示為
勻加速模型(Constant Acceleration，CA)取狀態(tài)變量X
為狀態(tài)轉移陣，其它參數設置同上。
2.4濾波器量測模型
量測方程可由下式給出
2.5智能車導航系統技術難點
導航定位模塊中GPS提供的速度、位置、航向等導航定位信息本身帶有噪聲，且由于環(huán)境因素會產生較大誤差，需通過濾波器濾波后發(fā)，發(fā)送至決策計算機進行決策并完成車體控制。
因此，研究具有更高性能的濾波器意在為車輛控制系統提供更快速、準確而穩(wěn)定的地理位置及速度信息。進一步講，濾波器的性能直接決定了導航定位系統的性能，并最終影響車體控制。所以，對于濾波器的優(yōu)化在導航定位系統乃至車輛控制系統中具有十分重要的意義。
現階段導航系統中，針對濾波器的研究仍然存在以下幾個難點問題：①實際路況中，存在大量非線性因素：智能車導航系統中濾波器普遍采用的擴展卡爾曼算法(EKF) [8]，該算法雖能實現非線性濾波，但是濾波精度已無法滿足智能車導航的需求;②實際車況下，車輛運動狀態(tài)多變：目前采用無跡卡爾曼濾波(UKF)或UKF的優(yōu)化算法[7]，在精度上較傳統的EKF有較大提高，但對于所提出的濾波算法的驗證局限于理想化的仿真模型，依舊無法與真正智能車行駛狀態(tài)相匹配;③由于真實路況下噪聲干擾，會造成濾波效果降低甚至發(fā)散：鑒于車輛行駛狀態(tài)的多變性，有學者提出采用交互式多模型(IMM)來逼近車輛真實狀態(tài)[15]，取得較好濾波效果，但算法不具有自適應性，且歷史數據對濾波影響較大。因此，如何找到一種高精度的，可靠的，具有自適應性且能與無人駕駛智能車導航系統相匹配的濾波算法，仍然是現階段工作的技術難點。
3 多模型無跡卡爾曼濾波算法
3.1算法原理
模型的匹配性不佳和噪聲統計特性的不準確經常導致UKF濾波性能降低[12-13]，為了解決這一問題，本文引用自適應UKF濾波(Adaptive UKF，AUKF)，該方法通過自適應漸消因子來平衡狀態(tài)方程、預測信息與觀測信息的權重，能實時地對模型誤差和噪聲統計特性進行估計并修正，較好地抑制模型誤差對導航解的影響，從而提高UKF的估計精度[14]。同時，加入衰減因子(Fading factor)[15-16]，加大當前觀測量的作用，減小歷史觀測值的影響，達到抑制濾波發(fā)散目的。AFUKF有效克服了噪聲及與模型不匹配的不足問題，但對模型描述準確性仍要求較高，引入交互式多模型算法(IMM)可較好解決此問題。IMM包含多個濾波器、模型概率估計、交互作用器、狀態(tài)估計混合器，基本原理為：通過建立不同的模型來描述系統可能的運動模態(tài)，模型間的切換由基于馬爾可夫鏈的轉換概率控制，對每個模型采用獨立的AFUKF，以模型轉移概率加權后的交互值作為濾波迭代輸入。通過輸出殘差不斷更新模型概率，對多個濾波器進行融合，使其盡可能地接近那個最能反映實際系統行為方式的模型濾波輸出。AFUKF是UKF的優(yōu)化，作為IMM結構中的并行濾波器，以下稱為IMM-AFUKF算法，結構示意圖4所示。
3.2算法流程
IMM—AFUKF算法具體步驟如下所示：
步驟1：計算混合概率系統非線性隨機的狀態(tài)空間模型為
步驟2：交互/混合
步驟3：模型濾波
4 仿真分析及實驗
4.1仿真及實驗分析
實驗室的智能車輛實驗平臺安裝應用載波相位技術的差分GPS(DGPS)，設備組為FlexPak-G2系列GPS接收處理機和GPS-702-GG接收天線及PDL4535數傳電臺。設備實物如圖5所示，GPS-702-GG接收天線關鍵參數如表1所示，FlexPak-G2系列GPS接收處理機關鍵參數如表2所示。
從仿真路徑圖中可看出，IMM-AFUKF與真實路徑最接近，誤差最小，CA-EKF，IMM-UKF均在彎道處理方面較為遜色。
同時，實驗的采用均方根誤差(Root mean square error,RMSE)定量分析并衡量算法優(yōu)越性[1-3]，其定義為
其中，n=500。RMSE越小，說明所采用的算法與真實值的誤差越小;運行時間越短，說明算法的計算量越小，在實際應用中數據輸出速度越快。圖7～12為采用三種濾波方法位置、速度、加速度均方根誤差曲線比較，表3為三種濾波算法均值和均方根誤差統計數據。
從仿真結果及性能統計值比較可以看出，在仿真的初始時刻，濾波器都有較大的初始偏差，這是由于初始估值造成的。隨著濾波的進行，IMM-AFUKF算法能很快地收斂到真值位置附近，并保持較高的濾波精度。IMM-AFUKF算法較IMM-UKF、CA-EKF算法提高了解算精度及收斂速度，狀態(tài)估計也更接近于真實狀態(tài)，且增強了數值穩(wěn)定性，其中IMM-AFMUKF算法的位置和速度的均方根誤差明顯小于標準IMM—UKF算法。究其原因，主要在于IMM-AFUKF算法引入了衰減因子，加大了對新測量值的利用權重，減少了對陳舊測量值和初始估值的依賴，同時增加自適應漸消因子，能夠較好的抑制初始值偏差和模型異常擾動誤差對導航解的影響，使算法具有一定的自適應能力。因此，IMM-AFUKF算法有效的控制了初始值偏差、模型異常擾動誤差、以及未知噪聲對濾波解的影響。
4.2實車實驗
本文以北京工業(yè)大學無人駕駛智能車BJuT-IV為實驗對象如圖13所示，研究其行駛軌跡的跟蹤問題。
BJUT-IV由高爾夫球電動車經過對電控系統的改裝而成。BJUT-IV采用GPS作為導航系統，并用UTM將經緯度轉化到平面坐標系，重新定義坐標原點O于北緯39.8719，東經116.4789即圖14中N8點為原點。BJUT-IV控制系統以Microsoft VS2008為軟件開發(fā)平臺，編寫了導航與定位系統軟件，利用此軟件平臺進行實驗驗證，用以檢驗IMM-AFUKF算法在智能車導航系統中的可靠性。實驗分別采用KF(綠色軌跡)、IMM-UKF(黃色軌跡)、CA-EKF(藍色軌跡)、IMM-AFUKF(紅色軌跡)對實際道路采樣點進行濾波估計(橙色軌跡代表真實道路)。取智能車的運動參數(位置、速度、加速度)作為系統的狀態(tài)變量，采用東北天坐標系。
實驗車輛沿道路車道線線行駛，根據實際情況做變速及勻速運動。實車實驗結果表明，IMM-AFUKF算法較其它算法更準確、穩(wěn)定;且在車輛轉彎時性能更加突出，如圖14中N10點和N9點彎道放大圖所示，紅色軌跡(IMM-AFUKF)幾乎與橙色軌跡(實際道路)重合，黃色軌跡(IMM-UKF)與綠色軌跡(KF)均與實際行駛軌跡有較大偏差且有較大滯后性。所提出算法更貼切的反應車輛實際行駛軌跡，該實驗結論與仿真結果一致，再次驗證所提出算法優(yōu)越性。
5結束語
在北京工業(yè)無人駕駛智能車BJUT-IV導航系統中，UKF處理非線性問題較EKF性能優(yōu)越，然而，UKF算法對模型匹配度要求較高、受系統噪聲影響較大，且對初始值較敏感，為此，本文在UKF中引入衰減因子及自適應漸消因子，并結合IMM濾波結構，提出了一種針對BJUT-IV導航系統的濾波算法IMM-AFUKF。理論分析、仿真、實車實驗結果均表明，IMM-AFUKF算法的濾波精度、穩(wěn)定性、收斂速度都明顯優(yōu)于標準IMM-UKF及EKF算法，具有較高實用價值。
該算法改善了導航系統精度，為上位機的控制決策提供良好的導航信息數據，將無人駕駛智能車控制系統整體性能進行了提升。為進一步改進IMM-AFUKF對初始值過于敏感等情況，需要在魯棒性方面做進一步的探討;并需克服模型轉換時濾波精度降低的問題，以上兩點將作為后續(xù)工作重點。
參考文獻：
[1]楊柳.改進卡爾曼濾波的目標跟蹤研究[J]·計算機仿真，[J] 2010—9：351—355.
[2]付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導航系統中的應用[M].北京：科學出版社，2003.
[3]秦永兒，張洪城，汪叔華.卡爾曼濾波與組合導航原理[M].西安：西北土業(yè)大學出版社，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].計算機仿真，2010，03：348—352.
[8] 王凡.智能車輛中基于GPS和陀螺儀的導航與定位[D].北京工業(yè)大學。2013.
[9] 王凡.智能車輛中基于GPS和陀螺儀的導航與定位[D].北京工業(yè)大學。2013.
[10] 潘泉，劉剛.聯合交互式多模型概率數據關聯算法[J].航空學報，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] 范韜，茅旭初.基于非線性濾波算法的高動態(tài)GPS定位模型[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算法實現非線性狀態(tài)估計的研究與仿真[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)賢，周春暉.漸消卡爾曼濾波器的最佳自適應算法及其應用[J].自動化學報，1990—3：210—216.
