城市復雜環境下的車輛組合定位算法研究

朱 元,吳博宇,陸 科,吳名芝

(1.同濟大學 汽車學院，上海 201800； 2.南昌智能新能源汽車研究院， 南昌 330052)

0 引言

近年來隨著智能汽車技術的快速發展，車輛對于環境感知、定位、決策和規劃控制等系統的要求愈發增高。其中，定位是一臺自動車的必備基礎，它需要告訴車輛相對于外界環境的精確位置，如果定位偏差過大，會引發車輛駕駛安全問題，嚴重的甚至會引發交通安全事故[1]。目前常用的車輛定位技術有：全球衛星導航系統(global navigation satellite system,GNSS)、慣性導航系統(inertial navigation system,INS)、視覺定位和激光雷達定位等。其中GNSS定位存在抗干擾能力弱、輸出頻率低的問題，而慣性導航自主性高且采樣頻率快，但長期精度差，將兩者組合可以實現優勢互補[2]。目前GNSS/INS組合定位作為一種主流的定位方案，在車輛導航定位領域得到了廣泛應用。

城市環境是車輛最主要的工作環境，但城市環境的復雜性也給車載定位系統帶來了巨大挑戰。城市中隨處可見的高樓、立交橋和樹木等會導致衛星信號受到遮擋、多路徑效應和非視距信號，從而導致GNSS系統的定位精度下降。由于目前多使用卡爾曼濾波作為GNSS/INS組合定位的主要方法，當GNSS信號出現異常時，若不能及時準確地對濾波參數進行修正，異常的GNSS測量值將導致組合定位系統的性能下降甚至發散[3]。

國內外學者為解決上述問題進行了大量研究，目前的研究方向之一為動態調節濾波器參數，主要是通過調節系統方差和量測方差，降低觀測量異常對組合濾波器的影響[4]。一些學者使用改進的Sage-Husa自適應濾波算法，在進行狀態估計的同時通過量測輸出在線實時地估計系統的噪聲參數，提高了系統的定位精度[5-7]。但Sage-Husa自適應濾波器對于突變工況的跟蹤能力不足，在面對城市復雜工況時較難及時獲得準確的噪聲估計。還有學者使用數據驅動的方式進行濾波器參數的實時估計，如高斯過程[8]和支持向量機[4]等。但這類方法需要提前對模型進行訓練，計算量較大。由于GNSS傳感器除了可以得到計算后的定位信息外，還可以同時獲得可觀測衛星數量、位置精度因子(position dilution of precision,PDOP)、載噪比、衛星仰角等輔助信息，有學者提出采用上述信息進行衛星信號優劣的分類，如王加芳[9]對GNSS輔助信息的統計量構建檢測方程進行閾值判斷，將GNSS狀態分為良好、異常和失鎖三類；近年來，一些學者將上述信息作為輸入，采用模糊推理系統(fuzzy inference system,FIS)進行傳感器信號分類，進而實現對量測噪聲的估計[10-11]。相比于采用閾值進行信號分類時界限分明的方式，模糊推理系統可以更為合理地確定測量信號所屬類別并進行量測噪聲的估計。但是由于模糊推理規則和隸屬度函數需要人為設置，所以直接使用模糊推理系統得到的量測噪聲估計會不可避免地引入人為誤差。

為了提高城市環境下車輛組合定位系統的性能，本文在上述研究的基礎上設計了一種基于模糊自適應卡爾曼濾波的車輛組合定位算法，濾波方法選用誤差狀態卡爾曼濾波，將GNSS的輔助信息作為模糊推理系統的輸入，系統的輸出作為量測噪聲協方差的先驗調整系數，通過改進的Sage-Husa自適應濾波算法對量測噪聲進行自適應估計，同時輔以車輛運動約束。通過計算機仿真與實車實驗對算法性能進行了驗證，結果表明：提出的算法在保持了對突變工況的跟蹤能力的同時，在GNSS異常區域可以獲得較高的定位精度。

1 GNSS/INS組合定位系統

1.1 GNSS/INS組合定位模型

對于GNSS/INS組合定位系統，本文使用誤差狀態卡爾曼濾波[12](error state Kalman filter，ESKF)作為濾波的方法，將系統的真實狀態分為名義狀態和誤差狀態兩類：

xt=x⊕δx

(1)

其中,名義狀態向量x共16維，分別為東北天向的位置、速度、方位四元數以及加速度計和陀螺儀的三軸偏差；誤差狀態向量δx共15維，分別為東北天向位置誤差、速度誤差、旋轉向量誤差以及加速度計和陀螺儀的三軸測量偏差的誤差，分別表示如下：

x=[pvqabwb]T

(2)

δx=[δpδvδθδabδwb]T

(3)

對于名義狀態，其遞推方程為：

(4)

(5)

(6)

對于誤差狀態，當旋轉向量誤差δθ定義在世界坐標系下時，其遞推方程如式(7)所示。其中vi、θi、ai、wi為符合高斯分布的零均值白噪聲，其方差分別寫作Vi、Θi、Ai、Ωi，則系統噪聲矩陣表示為式(8)。

(7)

(8)

系統的名義狀態轉移函數f可由式(4)確定，誤差狀態轉移矩陣Φδ,k/k-1和系統噪聲分配矩陣Γ可由式(7)確定。綜上，系統預測方程表示如下：

(9)

在GNSS/INS組合定位中，將GNSS的位置信息作為觀測量：

Z=[xyz]T

(10)

則量測矩陣：

(11)

系統的量測更新方程為：

(12)

(13)

(14)

系統狀態更新完成后需要將名義狀態和誤差狀態合并，并將誤差狀態參數重置：

(15)

(16)

其中，當旋轉向量誤差δθ定義在世界坐標系下時，方位四元數的合并式及方差重置矩陣Gk分別為：

(17)

(18)

1.2 車輛運動約束在GNSS/INS定位中的應用

考慮到車輛在正常行駛狀態下，其相對載體坐標系的側向和縱向速度均應為0，故對于城市車輛的GNSS/INS組合定位系統，可以利用車輛運動約束對模型進行修正[13]，據此構建觀測量：

Z′=[vyvz]T=[0 0]T

(19)

則量測矩陣表示為：

(20)

其中：

(21)

之后利用式(12)—(16)進行系統的量測更新、狀態合并以及參數重置，實現運動約束對組合定位系統的修正。

2 基于模糊自適應卡爾曼濾波的城市車輛組合定位方法

對于GNSS/INS組合定位系統，標準的ESKF算法在整個濾波過程中將式(12)中的量測噪聲協方差Vk設置為固定值，在理想工況下可以取得較好的定位結果，但是在城市復雜環境中GNSS的真實量測噪聲會發生顯著變化，若不能及時準確地對Vk進行修正，異常的GNSS測量值將導致系統的定位精度下降。

針對這一問題，提出了一種基于模糊自適應卡爾曼濾波的城市車輛組合定位方法，在前述添加了車輛運動約束的GNSS/INS組合定位系統的基礎上加入量測噪聲模糊自適應估計模塊，將模糊推理系統與改進的Sage-Husa算法相結合，在保證對系統突變工況的跟蹤能力的同時提升系統在GNSS異常區域的定位精度?；谀：赃m應卡爾曼濾波的城市車輛組合定位方法的完整結構如圖1所示。

圖1 基于模糊自適應卡爾曼濾波的城市車輛組合定位方法結構框圖

圖1中虛線框部分為量測噪聲模糊自適應估計模塊，使用模糊推理系統監測GNSS提供的輔助信息(衛星數量N和PDOP)并輸出量測噪聲調整系數tk作為先驗信息，之后利用基于改進Sage-Husa算法的量測噪聲自適應估計算法對量測噪聲進行估計，下面對該模塊進行具體介紹。

2.1 基于模糊推理系統的量測噪聲估計

模糊推理系統又稱為模糊系統，它以模糊邏輯理論為主要計算工具，由模糊化、模糊規則庫、模糊推理方法以及去模糊化幾部分組成，其輸入輸出都是精確值，可用于GNSS量測噪聲的估計。現有研究表明，可觀測衛星數量N和衛星的PDOP值可以用于場景的區分[4]，將其作為FIS的輸入，量測噪聲協方差調整系數tk作為輸出實現GNSS量測噪聲的調整，FIS的結構如圖2所示。

圖2 FIS結構框圖

根據經驗，將N、PDOP和tk分別劃分為3個模糊集，并使用合適的隸屬度函數對其進行描述，如圖3—5所示(若衛星數量小于4顆，認為此時GNSS的定位結果不可用，在GNSS/INS組合定位中需要切斷GNSS的輸入，故設計隸屬度函數時將這種情況忽略)。

圖3 衛星數量N隸屬度函數曲線

圖4 PDOP隸屬度函數曲線

考慮到當車輛行駛到GNSS信號異常區域時，可觀測衛星數量會明顯減少，與此同時PDOP值顯著增大，GNSS的定位誤差增大[4]，據此設計模糊推理規則，如表1所示。

表1 模糊推理規則

分別選用Mamdani法和重心法作為FIS中的模糊推理方法和去模糊化方法，得到的輸入輸出關系如圖6所示。利用FIS對GNSS提供的輔助信息進行實時監測，輸出的量測噪聲協方差調整系數tk可用于對量測噪聲進行調整，將式(12)進行改寫，如式(22)所示。

圖6 FIS的輸入輸出特性曲面

(22)

式中：V0為良好工況下量測噪聲協方差的參考值。使用FIS對量測噪聲進行調整可以使系統獲得對突變工況的跟蹤能力，但直接使用其進行量測噪聲的估計會不可避免地引入人為誤差?，F將FIS的輸出結果作為先驗信息，利用改進的Sage-Husa算法對量測噪聲進行估計。

2.2 基于Sage-Husa算法的量測噪聲自適應估計

除可以利用傳感器輔助信息進行組合定位系統的噪聲估計外，系統模型預測值與傳感器觀測值之差(一般稱為新息)也可以用于表征系統的噪聲水平，其中一種使用較廣泛的算法為Sage-Husa自適應濾波算法，該算法可以在進行系統狀態估計的同時利用時變噪聲統計估計器對量測噪聲參數和系統噪聲參數進行實時在線估計和修正。但現有的研究發現，對所有的噪聲參數進行準確估計很難實現，實際研究中最常用也比較有效的是一種對Sage-Husa算法改進的量測噪聲方差陣自適應算法[14]。

在卡爾曼濾波中，新息表示為：

(23)

(24)

(25)

對于第i個分量，記：

(26)

(27)

改進的Sage-Husa算法在保證較高的濾波精度的同時減少了計算量，但在面對突變工況時無法及時準確地進行量測噪聲估計，使用FIS的輸出作為先驗信息進行量測噪聲自適應估計可以有效解決這一問題。

將tk映射到[0,1]區間上：

(28)

構造量測噪聲跳變參數：

Δαk=|αk-αk-1|

(29)

當檢測到量測噪聲跳變參數較大時，可以認為車輛所處環境發生較大變化，當前的量測噪聲存在較大誤差，需要及時進行調整，首先利用式(22)進行量測噪聲的快速修正，之后使用改進的Sage-Husa算法對量測噪聲進行估計。綜合考慮算法突變工況的跟蹤能力、算法的穩定性以及計算量，引入自適應跟蹤參數m、跳變檢測閾值ηΔα和自適應窗口閾值n0，分別用于根據突變程度的大小調整對突變工況的跟蹤強度、環境改變的檢測以及自適應窗口的長度限定；同時，若檢測到連續多幀α小于閾值ηα，則使用初始V0作為量測噪聲協方差，以用于環境良好工況下的組合定位系統。

綜上所述，基于改進Sage-Husa算法的量測噪聲自適應估計算法流程如圖7所示。

圖7 基于Sage-Husa算法的量測噪聲自適應估計算法流程框圖

3 實驗與結果分析

3.1 仿真實驗與結果分析

為驗證算法的有效性，基于Matlab平臺進行仿真測試，假設車輛從原點出發，向東北方向以5 m/s的速度勻速運動，仿真時長240 s。參考真實傳感器手冊參數，將仿真環境的傳感器參數設置為：慣性測量單元相關參數如表2所示，采樣頻率100 Hz；GNSS采樣頻率設置為1 Hz，0～100 s及150～240 s的GNSS水平位置誤差0.5 m，可觀測衛星數量N為14，PDOP為1.5，模擬GNSS信號良好環境；100～150 s模擬車輛進入GNSS信號異常區域，GNSS水平位置誤差設置為4 m，N和PDOP分別為8和2.5，仿真軌跡及GNSS定位結果如圖8所示。在其他參數完全一致的條件下，分別使用標準ESKF算法、改進的Sage-Husa自適應濾波算法(記作SHAESKF)、僅使用FIS進行量測噪聲估計的模糊卡爾曼濾波算法(記作FESKF)以及本文中提出的基于量測噪聲模糊自適應估計的模糊自適應卡爾曼濾波算法(記作FAESKF)對上述仿真數據進行處理，不同算法在GNSS異常區域的定位結果如圖9、10所示。

表2 慣性測量單元參數

圖8 仿真軌跡及GNSS定位結果曲線

圖9 不同算法在GNSS異常區域的定位結果曲線

圖10 不同算法在GNSS異常區域的定位結果曲線(局部放大)

在GNSS信號良好區域行駛一段時間后，使用不同算法均能得到較好的定位結果，但進入GNSS信號異常區域后，不同算法的定位效果出現較大差異。對不同算法在GNSS信號異常區域的定位效果進行統計，結果如表3、圖11所示。

表3 不同算法的定位誤差

圖11 不同算法的定位誤差曲線(異常區域)

同時，對幾種算法在GNSS異常區域的量測噪聲方差估計值進行統計，如表4所示。

表4 量測噪聲方差估計平均值

由圖10(a)可知，在車輛進入GNSS信號異常區域時，使用標準ESKF算法和SHAESKF算法的組合定位方案均受到異常值的影響出現軌跡漂移；由圖10(d)可知，基于SHAESKF算法的定位方案在車輛重新進入GNSS信號良好區域時仍需較長時間才能恢復準確的定位結果；而基于FESKF算法和FAESKF算法的組合定位方案能夠對突變工況進行及時跟蹤。同時，綜合分析上述仿真結果可知，在GNSS信號異常區域，本文中提出的基于量測噪聲模糊自適應估計的模糊自適應卡爾曼濾波算法可以修正模糊推理系統存在的誤差，對量測噪聲進行較為準確地估計，在保證了對突變工況跟蹤能力的同時，可以獲得較高的定位精度。

3.2 實車實驗與結果分析

3.2.1實驗設計

為進一步驗證所提算法的有效性，使用搭載車規級六軸慣性測量單元IAM-20680和u-blox M8型GNSS模塊的實驗車輛在同濟大學嘉定校區內進行了實車實驗，車輛行駛路線如圖12所示，圖中的框線區域為仰望星空大樓，車輛在該大樓下通過時GNSS信號會出現異常，大樓的結構如圖13(a)所示。

圖12 車輛的行駛路線場景

同時，為了對算法的定位效果進行評判，使用超寬帶(ultra wide band，UWB)定位設備的定位結果作為車輛行駛軌跡的參考(實驗前對該區域的UWB定位精度進行了測試，結果表明定位誤差在10 cm以內，可以作為車輛行駛軌跡的參考值)，仰望星空區域的UWB基站布置方式如圖13(b)所示。同時，使用高精度RTK設備對測試區域附近開闊環境基準點的經緯度進行測量，并計算出UWB基站在大地坐標系下的位置，從而實現UWB局部坐標系與大地坐標系的統一。

圖13 實驗環境場景及基站布置示意圖

3.2.2實驗結果與結果分析

分別使用ESKF算法、加入運動約束的ESKF算法(記作ESKF+KC)以及本文中提出的基于模糊自適應卡爾曼濾波的城市車輛組合定位算法(記作FAESKF+KC)對采集的數據進行處理，結果如圖14所示。

圖14 不同算法的定位結果曲線

在GNSS良好區域，不同算法得到的定位結果差別不大，但進入GNSS異常區域后，不同算法的定位效果出現較大差異，GNSS異常區域的衛星定位結果及使用不同算法進行數據處理得到的定位結果分別如圖15、16所示。將不同算法得到的定位結果與車輛的參考行駛軌跡進行比較，得到不同算法在GNSS異常區域下的平均定位誤差，如表5所示。

圖15 GNSS異常區域的定位結果曲線

圖16 不同算法在GNSS異常區域的定位結果曲線

表5 不同算法的定位誤差

實驗結果表明，基于模糊自適應卡爾曼濾波的城市車輛組合定位方法通過量測噪聲的模糊自適應估計和車輛運動約束對模型誤差進行了有效修正，在GNSS異常區域仍能夠獲得較為準確的定位結果，平均定位誤差相比于標準的ESKF方法減少了0.479 m，平均定位精度提高了41.8%。

4 結論

針對城市環境下車輛組合定位系統的需求，提出了一種基于模糊自適應卡爾曼濾波的車輛組合定位算法，綜合利用GNSS提供的輔助信息以及濾波過程中的新息向量對量測噪聲進行及時調整，同時輔以車輛運動約束對模型誤差進行修正，提高了系統的定位精度。通過計算機仿真和實車實驗對所提算法進行驗證，結果表明，提出的算法能夠及時有效地對量測噪聲進行調整，在GNSS信號異常區域仍可以保持較高的定位精度，能有效解決城市環境中車輛的定位問題。