王付有 高成發(fā) 甘露 張瑞成 王劍超

0 引言

車輛導(dǎo)航是智慧交通、智能車快速發(fā)展中不可或缺的一項(xiàng)，關(guān)系著快速尋路、規(guī)避擁堵、提高交通運(yùn)轉(zhuǎn)率等切實(shí)需求[1].隨著硬件傳感器的普及，車輛定位的方法也從單GNSS定位，擴(kuò)展到GNSS接收機(jī)、慣性導(dǎo)航系統(tǒng)[2]、激光雷達(dá)[3]、里程計(jì)[4]等設(shè)備的融合定位.而對于用戶而言，手機(jī)作為出行必不可少的設(shè)備，在大多數(shù)情況下承載著導(dǎo)航定位的任務(wù).

現(xiàn)如今實(shí)時(shí)動態(tài)RTK定位已經(jīng)在部分機(jī)型實(shí)現(xiàn)，且能夠在開闊的環(huán)境下發(fā)揮穩(wěn)定作用，而在復(fù)雜環(huán)境中，定位精度及穩(wěn)定性均難以保證[5].智能手機(jī)內(nèi)部集成的慣性傳感器為航位推算(Dead Reckoning,DR)提供了可能，而實(shí)際應(yīng)用中，低成本的慣導(dǎo)設(shè)備數(shù)據(jù)精度較低，短時(shí)間的積分運(yùn)算即帶來較大的速度、姿態(tài)、位置誤差.此時(shí)RTK、航位推算的松組合結(jié)果會跟隨RTK結(jié)果變化，難以帶來精度上的有效提升.

影響航位推算過程精度的因素主要是航向角.航位推算過程中，通過角速度、加速度積分更新的航向角誤差積累較快，有必要進(jìn)行航向角的實(shí)時(shí)修正.航向角的修正一般圍繞著車輛運(yùn)動狀態(tài)和場景信息進(jìn)行，有零速修正[6]、道路線約束[7]、直線檢測[8]、地圖匹配[9]等方法.常規(guī)的地圖匹配是將定位點(diǎn)糾正到道路當(dāng)中，對地圖精度要求較高.目前可獲取的開源地圖OSM(OpenStreetMap)數(shù)據(jù)在點(diǎn)位精度上較低，但幾何精度較好[10]，具備修正航向角精度水平，因此本文采用了基于OSM地圖匹配的航向角修正方法.首先，根據(jù)RTK/DR融合定位結(jié)果、航向角、點(diǎn)投影依次篩選，得到候選匹配道路線，再結(jié)合車輛運(yùn)動狀態(tài)、垂線距離、路線長度選擇出最符合的行駛道路，最后使用該道路的方向來糾正航向角.在OSM地圖數(shù)據(jù)的輔助下，航位推算的航向角與車輛的行駛方向更加一致.

智能手機(jī)RTK為抗差Kalman濾波浮點(diǎn)解，解算后與航位推算結(jié)果進(jìn)行松組合，最后使用標(biāo)準(zhǔn)Kalman濾波對速度和位置進(jìn)行估計(jì).此外，本文增加了松組合的開關(guān)，結(jié)合RTK抗差Kalman解算待估參數(shù)的協(xié)方差陣Pk、車輛運(yùn)動方向、RTK定位航向角變化量綜合判斷是否進(jìn)行量測更新，以此減小RTK的嚴(yán)重漂移對松組合結(jié)果的影響.

1 關(guān)鍵算法

針對手機(jī)GNSS定位易受環(huán)境影響、手機(jī)內(nèi)部傳感器精度低等問題，本文提出一種RTK/DR組合定位方法，在RTK/DR松組合的基礎(chǔ)上，設(shè)計(jì)了地圖匹配算法，充分發(fā)揮OSM數(shù)據(jù)在航向角修正和松組合開關(guān)中的作用，進(jìn)一步提升智能手機(jī)在復(fù)雜環(huán)境的定位精度.

算法流程主要有：

1)手機(jī)RTK定位；

2)航位推算；

3)航向角修正；

4)RTK/DR松組合.

1.1 手機(jī)RTK定位

智能手機(jī)通過網(wǎng)絡(luò)連接校內(nèi)基站，實(shí)時(shí)接收并解碼基站觀測數(shù)據(jù)，與手機(jī)原始觀測值依次星間差分、站間差分，組成雙差模型，并使用抗差Kalman濾波對位置、速度、加速度、雙差模糊度進(jìn)行估計(jì)[11-12].手機(jī)實(shí)時(shí)動態(tài)RTK難以固定模糊度，模型中使用浮點(diǎn)解進(jìn)行解算.雙差觀測方程如下：

(1)

(2)

式中：1代表流動站，2代表基準(zhǔn)站，s為其他衛(wèi)星，r為參考衛(wèi)星.在短基線下，雙差模型大幅削弱了電離層延遲、對流層延遲的影響，消除了衛(wèi)星鐘差及測站接收機(jī)鐘差.

手機(jī)采用的抗差方案[13]如下：

(3)

(4)

(5)

1.2 航位推算

航位推算是一種相對定位技術(shù)，基本原理為已知起算點(diǎn)姿態(tài)、速度、坐標(biāo)，根據(jù)傳感器采集到的信息去計(jì)算下一時(shí)刻載體的姿態(tài)、速度和位置，循環(huán)往復(fù)來完成一段時(shí)間的導(dǎo)航任務(wù)[14-15].

航位推算包括起算點(diǎn)初始化、速度更新、航向角更新、位置更新四個(gè)過程.起算點(diǎn)初始化可利用的信息有限，只能通過RTK定位進(jìn)行賦值，具體公式為

(6)

速度更新方式與式(6)一致，其準(zhǔn)確度與航向角的精度密切相關(guān).

航向角更新為姿態(tài)更新的一部分，采用智能手機(jī)100 Hz采集的慣導(dǎo)傳感器數(shù)據(jù)進(jìn)行解算.慣導(dǎo)姿態(tài)更新算法參考嚴(yán)恭敏教授開源PSINS工具箱代碼，獲取到手機(jī)采集的加速度、角速度、重力加速度后，進(jìn)行四元數(shù)更新.將四元數(shù)轉(zhuǎn)換為姿態(tài)矩陣，最后計(jì)算出歐拉角，如下所示:

(7)

(8)

式中：q0至q3為手機(jī)四元數(shù)；φ，θ，γ分別為航向角、俯仰角和側(cè)滾角.

位置更新基本公式為

(9)

式中：vn，ve，vu分別為北向、東向、天向的速度；dt為RTK定位時(shí)間間隔；Rm為子午圈主曲率半徑；Rn為卯酉圈主曲率半徑.

1.3 航向角修正

航向角修正策略就是通過地圖匹配獲取當(dāng)前行駛路線，使用路線的方向來糾正.地圖采用開源的OSM數(shù)據(jù)，其質(zhì)量經(jīng)學(xué)者分析，位置精度不高，但幾何精度較好，具備修正航向角的能力.

地圖匹配通常有點(diǎn)到點(diǎn)、點(diǎn)到線、線到線的傳統(tǒng)方法[16]，也有基于隱馬爾可夫[17]的概率方法，考慮到定位的實(shí)效性本文采用點(diǎn)到線的匹配方法.基本思路為：根據(jù)上個(gè)歷元的融合定位結(jié)果，篩選出周圍50 m以內(nèi)的道路線段；根據(jù)此前航向角，篩選掉相差超過20°的線段，再去除點(diǎn)投影之外的線段，即可得到候選的路段；最后根據(jù)點(diǎn)到線最短垂線距離找到符合的線段，使用該線段的方向修正航向角.由于OSM地圖位置精度有偏移，因此存在匹配到相鄰車道的情況，而本文只使用道路的方向信息，所以平行的道路線段方向不影響對航向角的修正.

OSM地圖的道路在轉(zhuǎn)彎時(shí)，突變的角度與車輛平滑的角度變化相矛盾，因此航向角修正更適合在直線路段進(jìn)行.本文實(shí)時(shí)記錄慣導(dǎo)姿態(tài)更新后的航向角，當(dāng)航向角連續(xù)2 s變化量超過3°，即可認(rèn)為車輛處于轉(zhuǎn)彎狀態(tài)，從而停止航向角的修正，修正的具體效果將在實(shí)驗(yàn)部分給出.

1.4 RTK/DR松組合

RTK、DR松組合采用基本Kalman濾波，對手機(jī)的位置和速度進(jìn)行估計(jì)，其狀態(tài)方程和量測方程為

(10)

狀態(tài)方程表達(dá)的是上一時(shí)刻的狀態(tài)到當(dāng)前狀態(tài)的一個(gè)轉(zhuǎn)變，狀態(tài)轉(zhuǎn)移矩陣Φk/k-1描述了狀態(tài)的轉(zhuǎn)換過程，其構(gòu)成為

(11)

式中：dt為慣導(dǎo)數(shù)據(jù)采樣時(shí)間間隔，Bk為緯度.量測方程中Zk為RTK定位得到的三維坐標(biāo)和速度.

狀態(tài)更新和量測更新按照式(12)進(jìn)行：

(12)

式中：Rk為觀測信息的方差矩陣，對應(yīng)位置和速度的噪聲，本文使用RTK抗差Kalman濾波后的協(xié)方差陣Pk來對Rk賦值；Hk為松組合的系數(shù)矩陣.

在道路兩邊均是高樓的復(fù)雜環(huán)境下，RTK定位結(jié)果會出現(xiàn)跳動或者漂移，給松組合引入較大的誤差，也給后續(xù)的航位推算帶來錯(cuò)誤的起算點(diǎn).針對這種情況，本文增加了松組合開關(guān)，結(jié)合RTK抗差Kalman濾波后的協(xié)方差陣Pk、車輛運(yùn)動方向、RTK定位航向角變化量綜合判斷是否進(jìn)行松組合中的量測更新.將Pk的位置方差值從WGS84坐標(biāo)轉(zhuǎn)為東北天坐標(biāo)系，并計(jì)算平面的誤差ep，作為判斷定位質(zhì)量的一項(xiàng)指標(biāo).當(dāng)ep大于2、車輛運(yùn)動方向呈現(xiàn)直線、RTK定位的航向角變化量大于10°，本文即認(rèn)為RTK存在漂移或者跳動，此時(shí)松組合僅進(jìn)行狀態(tài)更新，而不進(jìn)行量測更新.

2 實(shí)驗(yàn)及結(jié)果分析

2.1 實(shí)驗(yàn)方案

本次實(shí)驗(yàn)場地為東南大學(xué)九龍湖校區(qū)，車輛按照圖1的路線行駛，起點(diǎn)和終點(diǎn)均為開闊的白色點(diǎn)，共行駛3圈.設(shè)備安裝如圖2所示：木板的兩端各固定一個(gè)測地型接收機(jī)，用于計(jì)算手機(jī)的真實(shí)參考坐標(biāo)；采用的手機(jī)為華為P30與P40，通過支架固定在木板上.實(shí)驗(yàn)過程中，手機(jī)預(yù)先安裝團(tuán)隊(duì)開發(fā)的定位軟件，實(shí)時(shí)接收校內(nèi)基站觀測數(shù)據(jù)，并進(jìn)行RTK定位，同時(shí)存儲手機(jī)的加速度、角速度、磁強(qiáng)度、重力加速度、GNSS原始觀測值信息.RTK定位頻率1 Hz，慣性傳感器數(shù)據(jù)采樣頻率設(shè)置為100 Hz，事后進(jìn)行RTK、航位推算的組合解算.

圖1 車輛行駛路線Fig.1 Vehicle route

圖2 設(shè)備安裝位置Fig.2 Equipment placement

2.2 航向角修正結(jié)果分析

圖3為航向角修正前后的航位推算軌跡，對應(yīng)著表1中的P40-2，橫坐標(biāo)加上118.8°為實(shí)際的經(jīng)度，黑色為測地型接收機(jī)解算的基準(zhǔn)軌跡，紅色為僅進(jìn)行初始對準(zhǔn)的航位推算軌跡，藍(lán)色為經(jīng)航向角修正的航位推算軌跡.圖3顯示，純航位推算的航向角誤差逐漸積累，偏離黑色的基準(zhǔn)軌跡，而航向角修正后，航位推算的軌跡與基準(zhǔn)軌跡更加平行一致.結(jié)合表1和圖3，可以說明地圖數(shù)據(jù)的引入能夠有效控制航向角誤差累積.

表1 航向角誤差統(tǒng)計(jì)

圖3 航位推算軌跡對比(橫坐標(biāo)加118.800°為實(shí)際經(jīng)度)Fig.3 Comparison of dead reckoning trajectories

2.3 定位誤差統(tǒng)計(jì)

方向修正后，航位推算在位置上仍與基準(zhǔn)有偏差，需要與RTK組合進(jìn)一步提升精度.圖4給出了RTK、航位推算組合后的定位軌跡對比，對應(yīng)著P40手機(jī)的第2圈，橫坐標(biāo)加上118.8°為實(shí)際的經(jīng)度.黑色代表測地型接收機(jī)的基準(zhǔn)坐標(biāo)，紅色代表手機(jī)RTK實(shí)時(shí)定位結(jié)果，藍(lán)色代表RTK、航位推算組合定位結(jié)果.可以看到，在軌跡圖上方，紅色的RTK軌跡由于兩側(cè)高樓影響，定位出現(xiàn)了漂移，此時(shí)算法檢測到RTK的異常，僅進(jìn)行松組合的狀態(tài)更新，而不進(jìn)行量測更新，從而得到與基準(zhǔn)更符合的定位軌跡.

圖4 松組合定位軌跡(橫坐標(biāo)加118.800°為實(shí)際經(jīng)度)Fig.4 Loose combination positioning trajectory

本文按照手機(jī)進(jìn)行分組，統(tǒng)計(jì)了P30、P40手機(jī)3圈的定位結(jié)果，如圖5所示.可以看到，部分時(shí)間段，RTK存在著超過5 m定位誤差，而同時(shí)間段的RTK、航位推算組合定位性能明顯提升，有效抑制了大幅跳動的誤差點(diǎn).

表2統(tǒng)計(jì)了2種定位方式下的均方根誤差，每臺手機(jī)3組結(jié)果.數(shù)據(jù)顯示:有5組實(shí)驗(yàn)RTK、航位推算組合定位平面精度優(yōu)于RTK；P30的第2組實(shí)驗(yàn)中N方向精度有所改善，E方向精度不佳，平面精度2種定位方式相當(dāng)；6組實(shí)驗(yàn)中，組合定位精度最好達(dá)到1.512 7 m，最差2.052 7 m；相比于僅使用RTK定位，組合定位精度提升幅度最大的為P40-2實(shí)驗(yàn)，達(dá)到19.4%.結(jié)合圖4、圖5及表2，可以得到結(jié)論：航位推算的加入能夠在復(fù)雜環(huán)境下平滑軌跡、降低側(cè)向漂移.

圖5 手機(jī)定位誤差Fig.5 Mobile phone positioning errors

表2 定位結(jié)果均方根誤差統(tǒng)計(jì)

3 結(jié)束語

在智能手機(jī)車載導(dǎo)航場景下，針對手機(jī)RTK在復(fù)雜環(huán)境下易發(fā)生漂移或跳動、手機(jī)慣性導(dǎo)航航向角易發(fā)散等問題，本文提出一種智能手機(jī)RTK/DR組合定位方法.利用OSM地圖數(shù)據(jù)，經(jīng)地圖匹配修正慣導(dǎo)姿態(tài)更新的航向角，結(jié)合RTK抗差Kalman解算中的協(xié)方差陣、車輛運(yùn)動方向、RTK定位航向角變化量設(shè)置了松組合的開關(guān)，最后采用標(biāo)準(zhǔn)kalman濾波估計(jì)松組合結(jié)果.

本文采用了P30、P40兩臺智能手機(jī)，對比了航向角修正前后的航位推算、組合定位與手機(jī)RTK定位的精度.從實(shí)驗(yàn)結(jié)果來看，OSM數(shù)據(jù)能夠有效削弱航向角的發(fā)散，大幅提升方向精度，使純航位推算能夠獲得與測地型接收機(jī)平行一致的軌跡.組合定位結(jié)果在大部分實(shí)驗(yàn)中獲得了優(yōu)于RTK的定位結(jié)果.軌跡圖表明：算法能夠有效識別RTK誤差較大的路段，使得航位推算能夠按照更準(zhǔn)確的方向進(jìn)行狀態(tài)更新.6組實(shí)驗(yàn)結(jié)果中，組合定位相比于僅使用RTK定位，精度最高提升達(dá)19.4%.

本文的航位推算過程中，雖然在方向上徹底擺脫了對RTK的依賴，但在速度上使用的是RTK的速度投影，對RTK大幅跳變的漏判會導(dǎo)致航位推算的結(jié)果偏離實(shí)際軌跡.在速度上擺脫RTK，以及更準(zhǔn)確的RTK跳變判斷將挖掘出航位推算更大的潛力.