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

0 引言

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

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

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

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

1 關(guān)鍵算法

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

算法流程主要有：

1)手機RTK定位；

2)航位推算；

3)航向角修正；

4)RTK/DR松組合.

1.1 手機RTK定位

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

(1)

(2)

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

手機采用的抗差方案[13]如下：

(3)

(4)

(5)

1.2 航位推算

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

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

(6)

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

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

(7)

(8)

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

位置更新基本公式為

(9)

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

1.3 航向角修正

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

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

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

1.4 RTK/DR松組合

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

(10)

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

(11)

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

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

(12)

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

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

2 實驗及結(jié)果分析

2.1 實驗方案

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

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

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

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

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

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

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

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

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

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

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

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

圖5 手機定位誤差Fig.5 Mobile phone positioning errors

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

3 結(jié)束語

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

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

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