基于智能手機的車輛多源融合導航方法

紀新春，魏東巖，袁 洪，周德云

（1.西北工業大學電子信息學院，西安 710114；2.中國科學院空天信息創新研究院，北京 100094）

車輛導航應用要求導航系統能夠實時提供精確、穩定和連續的車輛位置、速度及航向信息?？紤]到成本問題，大眾車輛導航通常選取微機電系統（Micro Electro Mechanical System，MEMS）-捷聯慣性導航系統（Strap-down Inertial Navigation System，SINS）與全球導航衛星系統（Global Navigation Satellite System，GNSS）進行組合定位。然而，在城市環境中，車輛易受樓宇、隧道、高架橋等遮擋，GNSS 定位常出現精度降低甚至完全中斷的問題。此時，低成本微機電慣性測量單元（MEMS-based Inertial Measurement Unit，MIMU）的漂移誤差會使MIMU-SINS/GNSS 組合導航系統的性能在短時間內迅速惡化[1,2]。

為了解決該問題，車輛多源融合導航技術近年來得到了廣泛研究。除GNSS、MIMU 外，常用的車輛導航源還包括里程計、車輛運動約束、磁力計、視覺傳感器、數字地圖等[3-8]?；诙嘣慈诤系亩ㄎ凰惴軌虺浞掷酶鲗Ш皆吹奶攸c，實現量測信息的優勢互補，構成多冗余、高可靠的車輛導航系統，進而提高城市復雜環境中車輛導航的性能。

此外，隨著應用的普及，智能手機導航已逐漸取代專用導航終端而成為大眾車輛導航的主流方式。目前絕大多數的智能手機均內置有GNSS 模塊、MIMU、磁力計等導航傳感器以及高性能處理器，具有強大的感知能力、運算能力與通信交互能力[9]?；诖?，本文提出了一種基于智能手機的車輛多源融合導航方法，在MIMU-SINS/GNSS 組合的基礎上：通過藍牙接口引入車載診斷系統（On-Board Diagnostic，OBD）中的輪速信息，與橫向、垂向零速構成完整性運動約束，提高姿態估計精度及短時GNSS 失鎖后的定位性能；對于深度城市峽谷、地下車庫等應用場景，采用基于里程信息輔助的地磁匹配定位算法，提供絕對位置校準，解決長時間GNSS 不可用情況下MIMU-SINS的誤差發散問題。選取典型城市環境進行跑車測試，結果表明所提出方法能夠顯著提高智能手機的導航性能，滿足城市環境車輛導航應用需求。

1 車輛多源融合導航方案

針對城市環境車輛導航應用，在常規MIMU-SINS與GNSS 組合導航的基礎上，增加OBD 輪速、車輛運動約束以及地磁匹配等觀測信息，構成車輛多源融合導航系統，算法流程如圖1所示。

圖1 車輛多源融合導航流程框圖Fig.1 The diagram of vehicle multi-source fusion navigation

圖1中，多源信息融合過程采用序貫濾波方法，以保證計算實時性，并滿足各導航源即時接入、退出的要求。其中，基于加速度計、陀螺儀測量值實時判斷零速狀態，當檢測到車輛處于零速狀態時進行陀螺零偏計算、補償，同時采用零速、零角速約束，否則采用橫向、垂向零速約束；在GNSS 長期不可用的情況，利用淺層地殼及地表建筑物磁化后形成的地磁異常場特征進行匹配定位，進而為MIMU-SINS 提供絕對位置校準；OBD輪速除了提供前向速度與橫向、垂向零速構成完整性運動約束外，積分后獲得的行駛里程為地磁匹配過程中的磁特征測量值空間刻度化處理提供尺度基準，提高匹配解算的效率和精度。

2 車輛智能手機MIMU-SINS

2.1 車輛SINS 模型

導航坐標系（n系）選擇東北天坐標系，載體坐標系（b系）選為右前上坐標系。對于SINS 解算，車輛的姿態、速度和位置變化率可以通過式(1)計算得出：

式中，表示b系相對于n系的姿態矩陣，vn為n系下東北天速度矢量，p為n系下經緯高位置矢量，為b系相對于慣性系（i系）的角速率，由陀螺儀測得，表示n系相對于i系的旋轉角速度，由地球自轉和車輛地速引起的n系旋轉形成，fb為加速度計測量的比力矢量，gn為重力加速度矢量，Mpv為車輛速度至位置的轉換矩陣：

式中，L表示緯度，h為高度，RM、RN分別為當地子午圈和卯酉圈的曲率半徑。

當初始參數已知，通過加速度計和陀螺儀可以獲取fb和，根據式（1）計算姿態、速度和位置的變化率后，通過積分即可獲得車輛當前的姿態、速度和位置信息。

2.2 智能手機MIMU 安裝矩陣投影

SINS 解算過程要求MIMU 測量坐標系（m系）與b系重合，而實際由于智能手機擺放的隨意性m系與b系之間通常存在差異，即安裝偏差角。對于低成本傳感器，由于MIMU 誤差特性不穩定且GNSS 觀測信息質量較差，信息融合過程將安裝偏差角作為獨立的狀態量很難獲得穩定的估計結果。因而，本文采取MIMU 安裝矩陣投影的方法來消除手機擺放姿態的影響，處理過程包括初始零速檢測、水平投影與安裝偏差角補償。

在車輛初始靜止時，依據加速度計的輸出fm計算m系與當地水平面夾角，然后將fm、統一投影至當地水平面。采用加速度、角速率方差檢測法進行車輛零速狀態檢測：

式中，Nw為滑動檢測窗口長度，、分別為窗口內的加速度、角速率均值，Tf、Tω為檢測門限，若k時刻方差小于閾值，則k時刻為零速時刻，否則為非零速時刻。

當檢測出車輛零速狀態時，得到m系與當地水平面坐標系（t系）夾角為：

式中，Δθ為俯仰方向夾角，Δγ為橫滾方向夾角，基于Δθ、Δγ構建旋轉矩陣即可將MIMU 測量值轉換至當地水平面。

通常情況，車輛初始靜止時b系與當地水平面不完全重合，且水平投影后的MIMU 航向與b系航向不重合。當MIMU-SINS/GNSS 組合模式濾波穩定后，t系下載體速度可表示為：

式中，αφ、αθ分別為水平投影后航向、俯仰安裝偏差角，當車輛行駛速度不為零時，可由、計算得出。

綜上，得到智能手機MIMU 的安裝矩陣為：

式中，Cist即為初始安裝矩陣，C1、C2、C3及C4分別為Δγ、Δθ、αθ及αφ擴展形成的姿態轉換矩陣。在完成安裝矩陣Cist投影后，MIMU 測量坐標系與車輛載體坐標系保持一致。

3 車輛運動約束與地磁匹配

3.1 運動約束

車輛在道路上正常行駛時，通常不發生側滑和跳躍，可假設車輛在b系下x軸和z軸方向的速度為零[4,5]。根據實際情況，可將x軸和z軸方向的速度考慮為零均值高斯白噪聲，同時結合OBD 輪速提供的前向（y軸）速度構成車輛完整性運動約束，即：

式中，vOBD為OBD 輪速信息。

對于零速狀態，采用零速校正（zero-velocity update，ZUPT）方法抑制導航誤差發散。由于零速狀態的車輛真實速度為零，MIMU-SINS 的速度輸出即為速度誤差，將該速度誤差作為量測值可對MIMU-SINS 的各項誤差進行辨識和補償。此外，零速狀態航向角誤差具有不可觀測性，僅利用ZUPT 無法對航向角誤差進行精準的估計。因而，引入零積分航向角速率（zero integrated heading rate，ZIHR）修正方法[6]，將相鄰時刻航向角差值作為量測值，減小陀螺漂移引起的航向角誤差，提高航向保持精度。

式中，φk為k時刻車輛載體航向角。

3.2 地磁匹配

現代城市環境中，地表鋼筋混凝土建筑、供電線路等磁性物質進一步豐富了地磁特征信息，且具有高空間分辨率，是有效的匹配導航信息源[10]。地磁匹配導航分為離線建庫與在線匹配兩個階段，由于車輛行駛速度不盡相同，會造成兩階段的磁測量數據無法空間對齊，導致無法利用磁場空間分布特征進行匹配定位?，F有文獻通常采用動態時間歸整（Dynamic Time Warping，DTW）算法解決該問題，但該算法計算復雜，不能滿足智能手機車輛導航的實時性要求。本文則利用里程信息來實現在線測量數據與離線基準庫數據的空間對齊，圖2所示為磁測量值等里程間隔空間轉換過程。

圖2 磁測量值時間序列變換至等里程間隔空間序列Fig.2 Transformation of the magnetic measurements from time sequence to spatial sequence

變換前磁測量值與時間、行駛里程與時間的對應關系如下：

式中，（M,T）為磁測量值時間序列，（mt,n,tn）為tn時刻的磁測量值，（D,T）為里程測量值時間序列，（dt,n,tn）為tn時刻里程測量值，可由OBD 輪速積分計算得出。

對里程序列（D,T）進行等里程間隔的采樣，并根據采樣點所對應的測量時刻讀?。∕,T）中相應的磁測量值，完成磁測量值的空間刻度化處理：

式中：d為里程采樣間隔；kL為里程采樣格點，（M,D）為磁測量值等里程間隔空間序列。

離線建庫與在線匹配過程采用相同的里程間隔d，即可實現二者空間尺度對齊?？紤]到單點匹配易產生誤匹配，在行駛里程上取距離為Dw=w?d長度的實測地磁數據與基準庫地磁數據進行滑動窗口匹配。其中，w表示窗口長度，匹配過程如圖3所示。

圖3 地磁匹配定位示意圖Fig.3 The diagram of geomagnetic matching positioning

式中，與分別為滑動窗口內實測磁場數據與基準庫磁場數據的均值，在搜索區間kL∈（w,KL）內，相關系數峰值所對應的基準庫位置即為匹配位置。

4 多源融合模型

4.1 狀態方程

多源融合濾波模型基于MIMU-SINS 誤差方程建立，選取n系中的位置誤差、速度誤差、姿態誤差、加速度計常值偏差和陀螺儀常值零偏構成狀態矢量：

式中，δφn,δ vn,δPn分別為姿態、速度及位置誤差，為加速度計常值零偏，為陀螺儀常值零偏。

4.2 量測方程

4.2.1 GNSS 信息

當GNSS 信息有效時，采用GNSS 定位、測速結果校正MIMU-SINS 解算誤差，GNSS 位置觀測量為：

式中，LI,λI,hI、δ LI,δλI,δhI分別為MIMU-SINS 的緯度、經度、高度及相應誤差，LG,λG,hG為GNSS 緯度、經度、高度，觀測矩陣，為GNSS 位置觀測噪聲。

GNSS 速度觀測量為：

4.2.2 地磁匹配結果

相比開闊空間，在城市峽谷、地下車庫等GNSS信號遮蔽環境中，地磁特征更加豐富。當車輛行駛至地磁基準數據庫可用區域時，采用地磁匹配位置結果校正MIMU-SINS 解算誤差，觀測方程為：

式中，LMag、λMag、hMag為緯度、經度、高度匹配結果，觀測矩陣為匹配位置觀測噪聲。

4.2.3 車輛運動約束

當車輛處于運動狀態時，車輛載體的橫向、垂向虛擬零速與OBD 輪速信息構成完成性運動約束，觀測矢量為：

完成MIMU 安裝矩陣投影后，認為已消除安裝偏差角影響，對式(17)求全微分可得：

式中，（vn×）為三維矢量vn的反對稱矩陣。

當處于運動狀態時，車輛運動約束觀測方程為：

當車輛處于靜止狀態時，為了提高航向角的修正精度，同時進行ZUPT 和ZIHR 修正，觀測矢量對于MIMU-SINS 解算，航向角變化率表示為：

式中，θ為俯仰角，γ為橫滾角，車輛靜止時中僅包含地球自轉項。

僅考慮航向角φ與陀螺零偏的影響，對式(20)求偏微分可得：

靜止狀態下車輛航向角φk=φk-1，則可計算相鄰時刻航向角差值為：

式中，TI為MEMS-SINS 解算周期。

靜止狀態下，車輛運動約束觀測方程為：

式中，

為觀測矩陣，V5為靜止狀態下車輛東向、北向、天向速度和航向角觀測噪聲。

4.3 多源信息融合

車輛行駛環境與狀態復雜多變，使GNSS、地磁匹配及運動約束等導航源的可用性并不穩定，需要信息融合算法滿足即插即用要求。本文采用序貫濾波[12]方法進行多源融合解算，算法結構如圖4所示。

圖4 序貫濾波信息融合算法結構圖Fig.4 The diagram of sequential-filtering fusion algorithm

在序貫濾波信息融合過程中，需要對各導航源信息的有效性進行判斷，包括GNSS 定位、測速有效性、地磁匹配有效性及零速狀態，并依據有效性檢測結果在同一周期內順序濾波處理。狀態更新過程如下：

式中，F為狀態轉移矩陣，W為系統噪聲矩陣，K為增益矩陣，i=2…N為導航源序號。對于第k個濾波

5 試驗驗證

5.1 試驗場景

試驗區域為中科院北京新技術基地周邊區域，路線長度約8.4 km，包括高樓遮擋、地下車庫、高速路等典型城市環境。試驗設備包括：別克昂科威車輛提供OBD 輪速信息（經標定后積分里程精度約1%）；某品牌智能手機作為車輛多源融合導航軟件運行平臺，采集各傳感器數據并進行信息融合處理；NovAtel-100C 組合導航系統用于離線建立地磁基準庫時提供高精度空間坐標，實時融合定位過程則作為基準比對源。試驗場景及設備如圖5所示。

圖5 試驗場景及設備圖Fig.5 Experiment route and equipment

5.2 結果分析

為了提高車輛運動約束的性能，需要對智能手機隨意擺放所導致的安裝偏差角進行估計補償。如圖6（上）所示，組合導航的輸出為MIMU 測量坐標系下的姿態結果，濾波穩定后恒定的姿態誤差即為載體系與測量坐標系間的安裝偏差角，航向偏差角約為28.9 °，俯仰偏差角約為0.5 °（水平投影處理后）。開闊環境在GNSS/MIMU 組合模式下，采用式（6）的計算方法，對航向、俯仰安裝偏差角進行估計。分析圖6可知，俯仰安裝偏差角估計結果約為0 °，航向安裝偏差角則穩定在約29.3 °，與實際安裝偏差角值基本一致，投影處理后即可實現MIMU 測量坐標系與車輛載體坐標系的統一。

圖6 安裝偏差角估計結果Fig.6 Estimation results of the mounting angle

圖7所示為測試過程GNSS 定位結果。分析可見：在普通道路及高速等開闊環境，GNSS 具有良好的定位性能，水平、高程定位誤差均優于5.0 m；然而，當車輛駛入中科院新技術園區后，由于受到建筑物遮擋，GNSS 定位結果出現明顯波動，水平、高程定位誤差最大分別達到49.3 m 和18.7 m，且在兩處地下車庫中分別存在約215 m、286 m 長的不定位，嚴重限制了車輛導航的可用性。

圖7 GNSS 原始定位結果分析Fig.7 GNSS original positioning results

圖8所示為典型的OBD 輪速積分里程誤差與磁場特征空間刻度化情況。圖8中可見，采用基于里程信息輔助的磁場特征空間刻度化處理能夠實現在線測量數據與基準庫數據的空間尺度統一，從而避免了類似DTW 中大量的尺度伸縮運算，可顯著提高匹配解算的實時性。

圖9所示為測試過程地磁匹配定位結果。由于采用滑動窗口匹配，需要采集一定窗口長度的磁特征數據后啟動匹配解算，在初始階段無定位結果輸出。同時，為了降低成本，離線建庫過程僅采集了單條車道的磁特征信息。

如圖9中所示：對于高速等多車道路段，測試路線與建庫路線車道不一致會引入額外的匹配誤差，水平定位誤差約為3 m；對于中科院新技術園區及地下車庫等單車道路段，則不存在該問題，匹配精度更高，水平定位誤差約為1.5 m。整個測試過程平均匹配定位解算處理時延低于0.1 s，能夠滿足MIMU-SINS 誤差修正的實時性需求。

圖9 地磁匹配定位結果分析Fig.9 Geomagnetic matching positioning results

圖10對比分析了智能手機不同導航源組合模式下的定位性能。圖10中可見，GNSS/MIMU/OBD 輪速組合模式能夠有效抑制GNSS 顯著跳點，且可在短時GNSS 信號遮蔽場景中提供連續、穩定的定位功能。然而，由于MIMU、OBD 輪速均存在誤差累積的問題，導致車輛駛入中科院新技術園區后仍存在明顯的定位性能下降?？梢灶A見，對于長時GNSS 信號遮蔽環境，GNSS/MIMU/OBD 輪速組合模式定位結果將逐漸發散。GNSS/MIMU/OBD 輪速/地磁匹配組合模式則進一步引入地磁匹配絕對定位信息，能夠有效解決導航結果發散的問題，且不受應用場景限制。測試過程中該模式水平定位誤差優于2.4 m，高程定位誤差優于0.7 m，顯著提高了車輛導航性能。

圖10 多源融合導航定位結果Fig.10 Multi-source fusion positioning results

圖11、圖12分別為智能手機GNSS/MIMU/OBD輪速/地磁匹配組合模式測速誤差與航向角誤差，分析可見：初始對準完成后，三軸測速誤差優于0.5 m/s，航向角誤差可限制在±2 °內，很好地滿足了目前大眾車輛導航應用的需求。

圖11 多源融合導航測速結果Fig.11 Multi-source fusion velocity results

圖12 多源融合導航航向結果Fig.12 Multi-source fusion heading results

6 結 論

智能手機導航已經成為目前應用最廣泛的大眾車輛導航方案。本文提出了一種車輛多源融合導航方法，利用智能手機集成的GNSS、MIMU、磁力計等導航傳感器，以及通過藍牙端口獲得的OBD 輪速信息，實現復雜城市環境車輛連續、穩定的導航功能。測試結果表明，所提出的多源融合導航方法能夠很好地滿足大眾車輛導航應用需求。此外，現階段GNSS 仍為最有效的車輛絕對位置、速度信息源，在實際應用過程中，僅需對城市峽谷、復雜立交高架、隧道及大型地下車庫等“熱點”區域事先構建地磁基準庫，利用地磁匹配絕對定位信息，解決GNSS 長時間中斷所導致的導航結果發散問題。