GNSS拒止環境下的手機車載定位研究

丁龍陽，曠儉，陳起金，牛小驥

（武漢大學 衛星導航定位技術研究中心，湖北 武漢 430079）

近年來，隨著駕車出行方式的普及以及自動駕駛技術的發展，對車載導航定位的需求日益突出。全球衛星導航系統(global navigation satellite sys‐tem,GNSS)在開闊環境下能夠全天候地提供高精度定位服務，被廣泛應用于車載導航。但是GNSS 在城市峽谷、地下停車場等場景中，會因多路徑效應及信號遮擋的影響，導致定位精度下降甚至失效。慣性導航系統(inertial navigation system,INS)具有自主導航的能力，可以為車輛定位提供包括三維位置、速度與姿態在內的完整導航信息，但是其定位誤差隨時間累積。尤其是低成本微機電系統(micro elec‐tro-mechanical system,MEMS)INS，零偏和噪聲大［1］，短時間內位置誤差就會迅速積累至無法單獨使用。

GNSS/INS 組合可以充分發揮各自的優勢，從而提供連續可靠的定位結果，在車載導航中得到了廣泛的應用［2-3］。但是當GNSS 信號出現中斷時，這種組合方案就會退化成INS 方案，定位誤差依然會隨時間迅速累積。因此，需要尋求可用的輔助信息或輔助傳感器來抑制慣導誤差累積。

在很多自動駕駛或高精度車載定位系統中，搭載多種輔助傳感器，如里程計［4］、激光雷達［5］、相機［6］、超聲波傳感器［7］等，通過多傳感器數據融合獲得較高精度的位置信息。但是，上述專用設備往往價格昂貴、安裝復雜，目前難以在大眾應用中普及。對一般用戶而言，實際駕車過程中往往是靠智能手機來實現車輛定位。智能手機不僅體積小巧、價格低廉，還內置有豐富的傳感器，如GNSS 接收機、MEMSIMU、磁力計、相機等［8］，可為車輛導航定位提供所需要的全部觀測值。但是智能手機內置的傳感器精度都比較低，如何挖掘低精度傳感器的性能，在沒有GNSS輔助的場景下（例如地下車庫）保證位置推算精度成為了研究熱點和難點。

利用車輛的運動約束信息，構建虛擬觀測，是一種有效的、無額外成本的慣導修正方法。車輛在正常行駛（不發生側滑和垂向跳躍）的過程中一般滿足非完整性運動約束(non-holonomic constraints,NHC)，即車輛只會有前向的速度，橫向和垂向速度為零［9］。文獻［9-11］利用這一約束顯著提高了車輛定位精度。由于NHC無法對前向速度進行約束，文獻［12］通過藍牙接口引入車載診斷系統中的輪速信息，與NHC的橫向、垂向零速共同構成完整性約束，從而進一步提高GNSS拒止環境下的定位性能。但是這種方法的缺點在于不同車輛生產商沒有提供統一的接口，因此難以普及。文獻［13］采用有監督學習方式訓練前向速度預測模型，測速誤差可以控制在0.5 m/s 以內，但是該方法的缺點在于不同場景下手機的晃動程度差異較大，訓練好的模型不具有普適性。此外，車輛可能會偶爾停車，此時可以使用零速修正以及航向角速率修正［14］來有效控制速度誤差和航向誤差累積。

除了利用車輛自身的運動信息，磁力計常被用于提供航向信息［15-16］。但是車輛外殼主要由鐵磁性物質組成，會造成磁干擾使磁航向不可用，因此磁力計在車載導航中的應用受到了較大限制。文獻［17］提出的方法可以較好隔絕周圍環境的干擾，但是所用傳感器十分昂貴。文獻［13］將車輛外殼的影響等效為磁零偏進行標定，使用比絕對磁航向更加可靠的磁航向角變化量，構建航向角觀測值輔助慣導推算。

本文在沒有GNSS 輔助的條件下，研究基于智能手機的車載定位方案在車輛行駛時（不考慮車輛靜止）的推算能力。該車載定位方案主要是利用非完整性約束以及手機內置磁力計輔助手機慣導進行推算。本文通過在地下車庫這種典型場景中實測了多組數據，研究了如何最大程度發揮NHC 的作用，維持慣導的推算精度，具體討論了非完整性約束中的安裝角和桿臂、磁航向修正在基于手機的車載定位方案中的作用，并嘗試為后續的研究指出方向。

1 手機端車載組合定位算法

1.1 算法框架

本文所討論的算法是在GNSS/INS 組合導航的基礎上，使用非完整性約束（NHC）以及磁航向變化來輔助手機MEMSIMU 進行車輛位置推算。在GNSS 信號中斷的情況下，該算法就退化為NHC 和磁航向輔助的MEMS慣導的航位推算模式。

圖1 給出了基于智能手機的車載組合定位算法的原理流程。首先通過慣導機械編排獲得IMU 的位置、速度和姿態。然后在開闊環境下，通過融合GNSS 位置信息可以有效抑制慣導誤差累積并使MEMSIMU 的零偏等增廣誤差至收斂狀態；在GNSS 信號中斷的場景下（圖中虛線框內部分），將INS 推算結果、非完整性約束信息以及磁航向修正信息通過擴展卡爾曼濾波(extended kalman filter,EKF)進行數據融合得到修正后的手機位速姿。

圖1 手機端車輛定位算法原理流程Fig.1 Vehicle positioning algorithm flowchart based on smartphones

1.2 系統誤差狀態模型

為解決系統的非線性問題，帶有輔助信息的慣性導航常采用誤差狀態卡爾曼濾波進行組合導航解算［18］。本文主要采用了15 維和20 維2 種誤差狀態向量。其中15 維誤差狀態量包括導航狀態向量以及IMU誤差向量：

為充分發揮非完整性約束的作用，需要補償IMU 安裝角和NHC 桿臂。因為不補償IMU 安裝角會使得NHC 約束直接作用在IMU 的3個軸向上，而不是車輛的前向、橫向和垂向［19］；不補償NHC 桿臂會使得NHC 約束直接作用在IMU 中心而不是NHC最符合的點上［20］，造成其側向速度與垂向速度并不為零，這都會影響NHC 的性能從而降低定位精度。其中，IMU 安裝角定義為IMU 坐標系（b系）與車輛坐標系（v系）之間因軸系不對正引起的角度差異；NHC 桿臂定義為IMU 測量中心到NHC 約束點（本文具體選在車輛后輪中心）的向量，如圖2所示。

圖2 IMU安裝角和NHC桿臂定義Fig.2 Misalignment angle of IMU and the lever arm of NHC

在實際車輛行駛過程中，手機位置通常不固定而且存在晃動，使得安裝角和桿臂會發生變化。因此預先標定好安裝角和桿臂，然后直接設定的方式勢必會影響NHC性能的發揮，而如果采用在線估計的方式則可以實現自動標定安裝角和桿臂，從而緩解上述問題。因此本文將俯仰、航向安裝角誤差及NHC 桿臂誤差（三維）增廣到狀態向量中進行在線估計，形成20維誤差狀態向量。完整的20維誤差狀態向量為：

式中：(δγ)T為俯仰安裝角和航向安裝角誤差（橫滾安裝角對于NHC 不需要也不可估［19］）；()T為NHC桿臂誤差，即IMU 測量中心到車輛后輪中心的桿臂誤差。

通過誤差擾動分析，可以推導出位置、速度與姿態的誤差微分方程，詳細的推導過程以及完整的變量描述可參見文獻［21］。

陀螺和加速計零偏建模為一階高斯-馬爾可夫過程：

式中：Tbg、Tba分別為陀螺零偏、加速度計零偏的相關時間；wbg、wba分別為陀螺零偏和加速度計零偏的驅動白噪聲。

1.3 約束觀測模型

1）非完整性約束。

在車輛載體坐標系下，當NHC 成立時，速度觀測向量可以表示為：

式中：vforward為車輛沿前進方向的速度；ev為觀測噪聲，可以根據運動狀態初步給定，并通過殘差調整。本文所討論的算法只針對側向速度和垂向速度進行約束，因此下面的觀測方程只取側向和垂向分量。同時，由IMU 推算的速度投影到載體坐標系下可以表示為：

2）磁航向修正。

本文采用文獻［13］提出的方法來發揮磁航向的修正作用。雖然鐵磁性的車輛外殼會造成磁干擾，但是這種磁干擾的主要部分可等效為磁力計零偏。通過預先準確標定磁力計零偏并扣除后，基于磁力計觀測值得到的磁航向角可以大致反映真實的航向角變化，因此可以利用磁航向角的變化量構建航向角觀測值。磁航向的計算方法以及航向角觀測值的構建方式如下。

首先根據手機的水平姿態角（即橫滾角和俯仰角）將磁力計測量值投影到水平面，然后利用投影后的磁力計水平分量計算磁航向角：

式中：φ、θ分別為手機的橫滾角和俯仰角；mx、my和mz為磁力計三軸原始觀測值；Mx、My分別為磁力計觀測值投影后的水平方向的2 個分量，ψmag為磁航向角。

得到磁航向后，可以使用磁航向角變化量構造航向角觀測值，具體觀測方程為：

式中：ψins、ψmag分別為慣導機械編排得到的航向角以及磁航向角；k為當前計算歷元編號；k0為準靜態磁場對應的歷元編號；n?為航向角觀測值的量測噪聲。準靜態磁場的判定方法可參見文獻［22］。

2 定位方案與手機車載實驗

根據是否補償安裝角和桿臂、是否在線估計手機IMU 安裝角和NHC 桿臂以及是否使用磁航向輔助3個條件，本文共探究了4種在GNSS中斷條件下的手機車載定位方案。4 種方案均使用了非完整性約束來抑制慣導誤差發散，具體見表1。

表1 無GNSS輔助下基于手機的車載定位方案Table 1 Description of vehicle positioning schemes based on smartphones in GNSS-Denied environments

為評估算法性能，在地下車庫場景中進行了車載實驗，包含“多轉彎型”復雜軌跡和“直線型”軌跡2 種典型軌跡。測試手機型號為HUAWEIMate 40 Pro，通過支架固定在車輛前端中控頂部，如圖3 所示。手機記錄了MEMSIMU、磁力計、GNSS 單點定位結果等原始數據。手機內置MEMSIMU 型號為ICM-20690。參考系統為邁普時空導航科技有限公司的戰術級GNSS/INS 定位定姿系統POS 320 參考位置通過事后動態差分(post processed kinematic,PPK)、INS 與里程計松組合并進行反向平滑處理獲得。位置精度可達厘米級到分米級。手機內置MEMS 級別IMU 與POS 320 中戰術級別IMU 的主要技術參數見表2。

表2 IMU的性能參數Table 2 Performance parameters of the IMUS

圖3 手機的安裝位置與安裝方式Fig.3 Installation position and method of smartphones

本文進行了2 項實驗，分別在典型的“多轉彎型”地下車庫（實驗1，見圖4（a））和“狹長型”地下車庫（實驗2，見圖4（b））開展測試。每項實驗均采集了10 組獨立的數據用于精度統計。由于用戶關心的是在地下車庫中車輛行駛的距離與定位精度的關系，本文以在地下車庫中行駛指定距離為前提開展實驗。具體地，實驗1在地下車庫中行駛500 m，其中包括10 個轉彎，期間GNSS 信號完全中斷，中斷時長大約為140 s；實驗2 在地下車庫中行駛800 m，其中包含一段大約為500 m 的長直線，期間GNSS 信號完全中斷，中斷總時長大約為280 s 。

圖4 實驗軌跡Fig.4 Trajectory of tests

由于手機內置MEMSIMU 性能低(含有較大噪聲以及陀螺穩定性差)，若其主要誤差不充分收斂，進入地下車庫后，定位誤差會迅速發散至不可用。因此，本文在進入地下停車場之前（GNSS 中斷之前），MEMSIMU 的主要誤差均已通過融合GNSS 定位結果充分收斂。每項實驗使用10組數據的終點平面位置誤差的RMS值作為統計結果，并計算終點平面位置相對測量誤差（位置誤差與軌跡長度的比值）。實驗數據的處理方式均為按照實時模式的離線處理。

3 多轉彎和長直線場景定位性能分析

3.1 實驗1（多次轉彎）

表3 展示了實驗1 采集的10 組數據在GNSS 信號中斷500 m（時長140 s）后終點平面位置誤差的所有結果，表格的最后2 行分別給出了終點平面位置誤差以及終點平面位置相對測量誤差（位置誤差與軌跡長度的比值）的RMS統計值。

表3 實驗1中斷GNSS信號500 m終點平面位置誤差結果Table 3 Error of terminal positionin the case of a 500 m GNSS signal outage (test 1)

從表2 最后的RMS 值可以看到，在直接使用NHC 時（方案1），終點定位誤差統計值為16.9 m；當進行安裝角和桿臂補償后（方案2），終點定位誤差RMS 統計值迅速減小到8.7 m，精度大約提升了48%。圖5 與圖6 分別為實驗1 中的一組數據在補償安裝角和桿臂前后，載體坐標系下的速度投影圖。從圖5、6中可以看到，在補償安裝角和桿臂后，與水平位置誤差相關的橫向速度投影有明顯的減小，從而能夠更嚴格地符合NHC 約束，提升定位精度。

圖5 實驗1進行IMU安裝角和NHC桿臂補償前的速度投影Fig.5 Velocity in v-frame before compensating the misalign‐ment angle of IMU and the lever arm of NHC（test1）

圖6 實驗1進行安裝角和桿臂補償后的速度投影Fig.6 Velocity in v-frame after compensating the misalign‐ment angle of IMU and the lever arm of NHC（test1）

將IMU 安裝角和NHC 桿臂進行在線估計后（方案3），終點位置誤差統計值會進一步減小到8.3 m，相比方案2 略有改善，終點平面位置相對測量誤差僅有1.66%。因為在實際車輛行駛過程中，即使手機被固定在支架上，也會因為路況顛簸以及車輛速度的變化導致手機晃動，進而會造成安裝角發生變化。在線估計安裝角可以更好地應對這一變化，對IMU 安裝角進行動態調整，進而更好地發揮出NHC 的性能，提升定位精度。此外，實驗1 結果顯示加入磁航向修正前后位置精度差異不大。這一方面因為相比長直線軌跡，實驗1 多轉彎軌跡中相同航向誤差引起的位置誤差更小；另一方面因為實驗1 頻繁的轉彎動作，使得NHC 能夠有效修正速度誤差，并進一步修正航向誤差，使航向誤差較小，因此磁航向修正難以起到顯著作用。

圖7 是實驗1 中一組典型數據4 種定位方案的軌跡推算結果對比。從圖7 中不難看出，相比于方案1，方案2、3、4 的推算軌跡明顯更加貼近參考真值，這主要是因為經過IMU 安裝角和NHC 桿臂補償后能夠更加準確充分地使用NHC。

圖7 實驗1中斷GNSS后推算軌跡對比Fig.7 Comparision of trajectories when GNSS is inter‐rupted in test 1

3.2 實驗2（長直線）

表4 給出了實驗2 采集的10 組數據在中斷GNSS 信號800 m（大約280 s）后終點位置的平面誤差的所有結果，表格的最后2 行分別給出了終點平面位置誤差以及終點平面位置相對測量誤差的RMS統計值。

表4 實驗2中斷GNSS信號800 m終點平面位置誤差結果Table 4 Error of terminal position in the case of a 800 m GNSS signal outage (test 2)

統計結果顯示，在長直線場景中，補償以及在線估計MU 安裝角和NHC 桿臂對定位精度無明顯提升，方案1～3 的定位結果差異較小。這主要是因為在直線行駛時由IMU 安裝角和NHC 桿臂產生的定位誤差占比較小。此外，磁航向修正在實驗2 中起到了一定的修正作用。相比于方案3，加入磁航向修正的方案4 終點平面位置誤差RMS 值由96.2 m減小到87.9 m，精度提升了8.7%，終點平面位置相對測量誤差減小到10.9%。

圖8 給出了實驗2 的一組典型數據在中斷GNSS信號后，4種定位方案的推算軌跡對比圖。從圖8 中不難看出，沿著前進方向的位置誤差是平面位置誤差的主要來源。磁航向修正（方案4）雖然明顯降低了航向漂移進而減小了左右方向的位置誤差，但是它無法減小前向位置誤差，因此磁航向修正帶來的位置精度提升是有限的。另外，磁航向修正的直線軌跡存在彎彎曲曲的現象，這主要是因為磁干擾以及磁航向的噪聲較大所導致。

圖8 實驗2中斷GNSS后推算軌跡對比Fig.8 Comparision of trajectories when GNSS is inter‐rupted in test 2

此外，對比實驗1 和實驗2 的統計結果可以發現：

1)實驗2 的終點位置誤差漂移率要明顯大于實驗1。這一方面是因為慣導的精度是隨時間發散的，實驗1中斷時長大約為140 s，而實驗2中斷時長達到了280 s；另一方面從水平位置誤差曲線（見圖9與圖10）中可以看到，實驗1水平方向的誤差沒有沿一個方向持續累積，而實驗2 在直線段總體上呈發散趨勢。這是因為實驗1 在每次轉彎后，NHC 的速度修正作用都會有效減小各個方向的速度誤差；然而實驗2 在長直線中NHC 的速度修正只能作用于固定方向的速度誤差，對沿著軌跡方向的速度誤差無效。另外，實驗1在軌跡上還存在掉頭，而慣導位置誤差的一些影響因素（例如航向誤差與前向速度誤差）是與之相關的，進而導致水平方向的位置誤差發生了逆轉抵消的效應，而實驗2由于是直線，自然沒有“逆轉抵消誤差”的效果。因此，實驗2 的終點位置誤差漂移率要明顯大于實驗1。

圖9 實驗1水平位置誤差曲線Fig.9 Error curve of horizontal position in test 1

圖10 實驗2水平位置誤差曲線Fig.10 Error curve of horizontal position in test 2

2)相比實驗1，磁航向修正在實驗2 中帶來的精度改善更加明顯。這主要是因為實驗2的誤差發散時間較長以及沒有軌跡掉頭帶來的“逆轉誤差”的效果，從而產生了較大的航向漂移，基于磁力計的相對航向約束得以發揮作用。而實驗1本身航向誤差較小，含有較高噪聲的磁航向難以發揮顯著作用。

3)相比實驗2，IMU 安裝角和NHC 桿臂補償在實驗1中帶來的性能提升更加顯著。由IMU速度投影到NHC適用點處在載體坐標系下的公式為：

故而桿臂補償基本不會對直線段的NHC 產生影響。對于安裝角而言，由于在直線段中航向角不可觀，與水平位置相關的航向安裝角易被航向誤差吸收，從而難以表現其補償作用。因此，在實驗2長直線場景中，是否補償安裝角和桿臂對于定位性能影響不大。

4 結論

1）實驗1（多轉彎）結果表明，補償IMU 安裝角和NHC 桿臂可以充分發揮NHC 性能，有效提升定位精度（提升48%）；在線估計IMU安裝角和NHC桿臂可以緩解實際車載場景中因為手機晃動帶來安裝角不固定的問題，進一步提升定位精度，該方案也在實驗1 中取得了最好的精度，中斷GNSS 信號500 m終點位置誤差RMS 值為8.3 m，終點平面位置相對測量誤差為1.66%。

2）實驗2（長直線）結果表明，補償和在線估計安裝角和桿臂對定位精度影響不大；磁航向修正（方案4）可以抑制航向漂移進而提升定位精度，該方案在實驗2 中取得最好的效果，中斷GNSS 信號800 m后終點位置誤差RMS 值為84.4 m，終點平面位置相對測量誤差為10.56%，相比不使用磁航向修正的方案精度提升了8.7%。

本文所討論的基于手機的車輛位置推算方法，還有待進一步完善，尤其在沒有GNSS 信號的長直線場景下。例如，本文算法即使沒有剔除干擾，磁航向修正在長直線的場景下都表現出了明顯的積極作用，預期后續通過剔除磁干擾獲得更準確的磁航向信息可以進一步提升定位精度。此外，還可以設法獲取前向速度（例如車輪里程計或CAN總線車速信息）與NHC 構成三維速度約束，進一步提升GNSS拒止環境下的車載定位性能。