基于UPF的車輛緊耦合定位算法*

蔣 榮，陳 柏

(1.東南大學 儀器科學與工程學院，江蘇 南京210096；2.南京航空航天大學 機電學院，江蘇 南京210016)

0 引 言

隨著智能交通系統(intelligent transportation system，ITS)的高速發展，高精度和高可靠性定位系統在智能車輛相關應用中受到了廣泛地關注，例如駕駛輔助系統、防碰撞預警系統和交通控制等[1-3]。目前，作為主流定位系統，組合導航系統融合了多種優勢互補的傳感器信息，能有效提高定位精度，被廣泛應用于車輛導航定位領域。其中，比較常用的多傳感器融合方式是全球衛星導航系統(GNSS)和慣性導航系統(INS)的融合。

全球衛星導航系統作為車輛定位的主要信息來源，近年來發展迅速。除了GPS外，中國的北斗(BeiDou)、俄羅斯的格洛納斯(GLONASS)以及歐洲的伽利略(GALILEO)等衛星導航系統都逐漸成熟。相對于單一模式的GNSS，多模GNSS在衛星的幾何分布和衛星數量上，均有著較大的優勢，即使在復雜的城市環境下，仍有較多的衛星可用于定位解算。因此，多模GNSS被越來越多地應用到車輛組合導航定位中[4-5]。但是衛星導航系統也有局限性，例如在峽谷、隧道等遮擋環境下無法提供精確的導航定位。相比而言，慣性導航系統(INS)由加速度計和陀螺儀組成，它能夠完全依賴設備自身完成導航定位，工作時不受外界的影響。但其定位誤差會隨著時間不斷累積，從而導致定位精度和可靠性下降。然而，高精度的全維慣性單元因為成本較高并不適用于車輛導航領域。因此如何實現穩定、精確且低成本的車輛導航定位已成為研究的熱點[6-7]。

此外，對于INS/GNSS非線性組合導航系統而言，基于序貫蒙特卡洛方法和遞推貝葉斯估計的粒子濾波算法(Particle FilterPF)，由于其相比卡爾曼濾波、擴展卡爾曼濾波等具有較強的處理非線性、非高斯系統的能力，已經被廣泛地用來實現組合導航中的多傳感器信息融合。但是，傳統的粒子濾波也存在缺點，例如：粒子貧瘠和退化現象等，尤其是在INS/GNSS組合導航應用中，傳統的粒子濾波選擇先驗概率密度作為重要性密度，而沒有考慮到當前的觀測值。這樣僅僅依賴先驗概率密度分布的粒子群會與真實的后驗概率密度分布產生很大的偏差，影響定位精度[8-10]。

針對上述問題，本文提出一種基于無跡粒子濾波(UPF)的車輛緊耦合定位算法。

1 非線性系統模型

1.1 狀態模型

本文基于降維慣性導航系統(RISS)來設計車輛非線性運動模型。通常組合導航中使用的全維慣性單元包含3個陀螺儀和3個加速度計，這是造成車輛組合導航定位成本高的主要原因。而本文使用的降維慣性導航系統只使用一個垂直方向陀螺儀和兩個水平方向的加速度計以及輪速傳感器。雖然是降維慣導，但通過陀螺儀估計車輛的航向角，兩個加速度計估計車輛的俯仰角和側傾角，同時再結合輪速傳感器信息便仍能夠實現三維定位[11]。在本文中，橫向加速度、縱向加速度和輪速等信息都可通過車身CAN總線從汽車自帶的傳感器中獲取，節約了成本。

系統狀態方程如下：

Xk=f(Xx-1,Uk-1,Wk-1,Tk-1)

(1)

式中：X—系統狀態向量；U—系統輸入向量；f(·)—非線性狀態轉移方程；W—系統噪聲矩陣；T—輸入噪聲矩陣。

在INS/GNSS組合導航中，系統噪聲和輸入噪聲通常設置為零均值的高斯噪聲，其協方差陣分別是Q和Γ。

系統輸入量由加速度計，垂直方向陀螺儀和輪速傳感器的信息組成，系統輸入向量如下：

U=[vwawfxfyωz]T

(2)

式中：vw—通過輪速測得的車輛速度；aw—從車輛輪速求出的車輛加速度；fx—橫向加速度測量值；fy—縱向加速度測量值；ωz—垂直方向陀螺儀輸出的橫擺角速度。

系統狀態向量包括車輛的位置、速度、姿態，如下所示：

X=[φλhvEvNvUprA]T

(3)

式中：φ—緯度信息；λ—經度信息；h—高度信息；vE—車輛的東向速度；vN—車輛的北向速度；vU—車輛的垂向速度；p—車輛的俯仰角；r—車輛的側傾角；A—車輛的航向角。

根據上文所提的RISS定位模型，可得狀態方程具體如下：

(4)

式中：RM—地球子午線曲率半徑；RN—正常的地球曲率半徑；ωe—地球自轉角速度；g—重力加速度。

1.2 觀測模型

在緊耦合算法中，多模衛星的偽距信息被用來作為觀測向量。

觀測方程如下：

Zk=h(Xk)+εk

(5)

式中：Z—觀測向量；h(·)—觀測方程；ε—觀測噪聲矩陣。

同樣，觀測噪聲也被認為是零均值的高斯噪聲，其協方差陣為R。

在本文提出的緊耦合定位算法中，采用GPS/北斗雙模衛星導航系統的偽距測量值作為觀測量。觀測向量如下：

Z=[ρGPS,1...ρGPS,m1ρBDS,1...ρBDS,m2]T

(6)

式中：ρGPS,ρBDS—指校正過對流層誤差和電離層誤差的GPS和北斗衛星偽距；m1,m2—各時刻接收到的GPS和北斗衛星數目。

根據衛星偽距定位原理，可得具體的觀測方程如下：

(7)

式中：[xyz]—接收機(車輛)的位置；[xGPSyGPS

zGPS]—接收到的GPS衛星位置；[xBDSyBDSzBDS]—接收到的北斗衛星位置；δbGPS,δbBDS—GPS和北斗接收機鐘差；e—地球橢圓離心率。

北斗和GPS的衛星位置可以根據雙模接收機接受到的各自的星歷數據解算獲得。

2 信息融合算法

2.1 粒子濾波

系統狀態模型和觀測模型都是非線性的。粒子濾波(PF)因為能較好地處理非線性、非高斯系統，被廣泛地運用在INS/GNSS多傳感器信息融合中。粒子濾波算法主要有以下幾個步驟[12]：

2.2 改進UPF

2.2.1 無跡粒子濾波

UPF的算法由以下幾部分組成：

(1)初始化：

(8)

分散粒子：

(9)

(10)

時間更新：

(11)

(12)

測量更新：

(13)

(14)

(15)

(3)更新粒子權重并歸一化處理：

(16)

(4)狀態估計：

(17)

(5)重采樣：

(18)

2.2.2 模糊算法

通常，在INS/GNSS的組合導航算法中，每個衛星偽距的觀測噪聲都被認為是具有相同噪聲協方差陣的高斯噪聲。因此，公式(13)中的觀測噪聲協方差陣通常被設置為：

(19)

為了更好地適應車輛導航環境，提高定位精度，本文的融合算法中采用了一種模糊算法，依據衛星信號的高度角和載波信噪比(C/N0)Pi來對觀測噪聲協方差陣進行自適應調整[14-15]。

通過先驗性知識可以知道：衛星高度角越大的衛星，其信號受周圍建筑物和樹木的影響會越小，精度相對要高；同時衛星信號的載波信噪比也是評估衛星信號精度的特性之一，精度高的衛星信號，載波信噪比相對較小。依據上面的分析，本文所設計的模糊算法以衛星高度角和載波信噪比作為兩個輸入量。模糊輸出量為調整系數。

該模糊邏輯的輸入量、輸出量的隸屬度函數如圖1所示。

圖1 模糊輸入、輸出隸屬度函數

其中，高(High)、中(Mid)、低(Low)3個隸屬度函數描述輸入特征量高度角；大(Large)和小(Small)兩個隸屬度函數描述輸入特征量載波信噪比；精度好(Good)、精度一般(Normal)、精度差(Bad)3個隸屬度函數描述輸出變量。

該模糊算法的模糊規則如表1所示。

表1 模糊規則

高度高、信噪比大的衛星信號精度最好；高度低、信噪比小的衛星信號精度最差。因此，本文方法中的觀測噪聲協方差陣可以設置為：

Rk=diag[(μ1R0)2... (μm1+m2R0)2]

(20)

3 實 驗

實驗中橫向和縱向加速度以及輪速信息可直接從車身CAN總線上獲取。同時，使用了低成本的GPS/北斗雙模衛星接收機NovAtel-C260-AT和MEMS陀螺儀VG440CA-200。此外，NovAtel SPAN-CPT組合導航系統被用來提供參考值。

本文將其與基于PF算法來進行信息融合的定位方法進行了對比，結果如圖2所示。

圖2 定位結果示意圖黑色虛線—SPAN-CPT系統輸出的參考值；黑色實線—改進UPF算法的定位結果；黑色點畫線—PF算法的定位結果

圖2展示的是局部切平面坐標系下的定位結果，橫坐標表示車輛東向位置x，縱坐標表示車輛北向位置y。通過對比可以看出，改進UPF和PF都能實現連續的車輛定位，但UPF的定位精度要優于PF。

兩種方法定位結果的水平位置誤差的統計數據如表2所示。

表2 水平位置誤差統計結果

從表2中可以得出，本文所用的改進UPF算法的水平位置誤差分別改善了29.46%和21.88%。

兩種方法定位誤差比較如圖3所示。

圖3 水平位置誤差對比圖黑色虛線—PF的定位誤差；黑色實線—改進UPF的定位誤差。

結合表2和圖3可知：相比PF算法，本文所使用的算法在定位精度和穩定性上都有明顯改善

4 結束語

本文融合雙模衛星定位系統和其他低成本車載傳感器信息，提出了一種基于UPF的車輛緊耦合定位算法。首先基于降維慣性導航系統(RISS)和雙模衛星導航系統建立系統模型；然后使用UPF算法實現多傳感器信息融合；此外還運用一種模糊算法對偽距觀測噪聲協方差值進行自適應調整，從而提高了定位精度。

最后實車實驗結果表明：相比基于PF的定位算法，本文方法定位誤差的最大值和均方根值分別能改善29.46%和21.88%。因此，本文方法能有效提高車輛定位精度和穩定性，同時還能降低定位成本。

參考文獻(References):

[1] VU A, RAMANANDAN A, CHEN A, et al. Real-time computer vision/DGPS-aided inertial navigation system for lane-level vehicle navigation[J].IEEETransactionsonIntelligentTransportationSystems,2012,13(2):899-913.

[2] JO K, CHU K, SUNWOO M. Interacting multiple model filter-based sensor fusion of GPS with in-vehicle sensors for real-time vehicle positioning[J].IEEETransactionsonIntelligentTransportationSystems,2012,13(1):329-343.

[3] SKOG I, HANDEL P. In-car positioning and navigation technologies—a survey[J].IEEETransactionsonIntelligentTransportationSystems,2009,10(1):4-21.

[4] 謝 鋼.全球導航衛星系統原理[M].北京:電子工業出版社,2013.

[5] LI X, GE M, DAI X, et al. Accuracy and reliability of multi-GNSS real-time precise positioning: GPS, GLONASS, BeiDou, and Galileo[J].JournalofGeodesy,2015,89(6):607-635.

[7] GEORGY J, NOURELDIN A, KORENBERG M J, et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation[J].IEEETransactionsonIntelligentTransportationSystems,2010,11(4):856-872.

[8] ZHANG D, FU K, GE S S, et al. Analysis of filtering methods for the SINS/CNS integrated navigation system of missile motion[C]. 2014 11th World Congress on Intelligent Control and Automation, Shenyang: IEEE,2014.

[9] YIN S, ZHU X. Intelligent particle filter and its application to fault detection of nonlinear system[J].IEEETransactionsonIndustrialElectronics,2015,62(6):3852-3861.

[10] CHEN X, SHEN C, ZHAO Y. Study on GPS/INS system using novel filtering methods for vessel attitude determination[J].MathematicalProblemsinEngineering,2013(3):289-325.

[11] NOURELDIN A, KARAMAT T B, GEORGY J. Fundamentals of inertial navigation, satellite-based positioning and their integration[M]. Berlin: Springer Science &Business Media,2012.

[12] GEORGY J, KARAMAT T, IQBAL U, et al. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter[J].GPSsolutions,2011,15(3):239-252.

[13] 朱志宇.粒子濾波算法及其應用[M].北京：科學出版社,2010.

[14] 王 宇.基于車內總線的車輛雙模衛星融合定位技術研究[D].南京：東南大學儀器科學與工程學院,2016.

[15] LI X, JIANG R, SONG X, et al. A tightly coupled positioning solution for land vehicles in urban canyons[J].JournalofSensors,2017(3)：1-11.