紀(jì)新春，魏東巖，袁 洪，周德云

（1.西北工業(yè)大學(xué)電子信息學(xué)院，西安 710114；2.中國科學(xué)院空天信息創(chuàng)新研究院，北京 100094）

車輛導(dǎo)航應(yīng)用要求導(dǎo)航系統(tǒng)能夠?qū)崟r(shí)提供精確、穩(wěn)定和連續(xù)的車輛位置、速度及航向信息?？紤]到成本問題，大眾車輛導(dǎo)航通常選取微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）-捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strap-down Inertial Navigation System，SINS）與全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）進(jìn)行組合定位。然而，在城市環(huán)境中，車輛易受樓宇、隧道、高架橋等遮擋，GNSS 定位常出現(xiàn)精度降低甚至完全中斷的問題。此時(shí)，低成本微機(jī)電慣性測量單元（MEMS-based Inertial Measurement Unit，MIMU）的漂移誤差會使MIMU-SINS/GNSS 組合導(dǎo)航系統(tǒng)的性能在短時(shí)間內(nèi)迅速惡化[1,2]。

為了解決該問題，車輛多源融合導(dǎo)航技術(shù)近年來得到了廣泛研究。除GNSS、MIMU 外，常用的車輛導(dǎo)航源還包括里程計(jì)、車輛運(yùn)動約束、磁力計(jì)、視覺傳感器、數(shù)字地圖等[3-8]?；诙嘣慈诤系亩ㄎ凰惴軌虺浞掷酶鲗?dǎo)航源的特點(diǎn)，實(shí)現(xiàn)量測信息的優(yōu)勢互補(bǔ)，構(gòu)成多冗余、高可靠的車輛導(dǎo)航系統(tǒng)，進(jìn)而提高城市復(fù)雜環(huán)境中車輛導(dǎo)航的性能。

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

1 車輛多源融合導(dǎo)航方案

針對城市環(huán)境車輛導(dǎo)航應(yīng)用，在常規(guī)MIMU-SINS與GNSS 組合導(dǎo)航的基礎(chǔ)上，增加OBD 輪速、車輛運(yùn)動約束以及地磁匹配等觀測信息，構(gòu)成車輛多源融合導(dǎo)航系統(tǒng)，算法流程如圖1所示。

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

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

2 車輛智能手機(jī)MIMU-SINS

2.1 車輛SINS 模型

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

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

式中，L表示緯度，h為高度，RM、RN分別為當(dāng)?shù)刈游缛兔先Φ那拾霃健?/p>

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

2.2 智能手機(jī)MIMU 安裝矩陣投影

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

在車輛初始靜止時(shí)，依據(jù)加速度計(jì)的輸出fm計(jì)算m系與當(dāng)?shù)厮矫鎶A角，然后將fm、統(tǒng)一投影至當(dāng)?shù)厮矫妗２捎眉铀俣?、角速率方差檢測法進(jìn)行車輛零速狀態(tài)檢測：

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

當(dāng)檢測出車輛零速狀態(tài)時(shí)，得到m系與當(dāng)?shù)厮矫孀鴺?biāo)系（t系）夾角為：

式中，Δθ為俯仰方向夾角，Δγ為橫滾方向夾角，基于Δθ、Δγ構(gòu)建旋轉(zhuǎn)矩陣即可將MIMU 測量值轉(zhuǎn)換至當(dāng)?shù)厮矫妗?/p>

通常情況，車輛初始靜止時(shí)b系與當(dāng)?shù)厮矫娌煌耆睾希宜酵队昂蟮腗IMU 航向與b系航向不重合。當(dāng)MIMU-SINS/GNSS 組合模式濾波穩(wěn)定后，t系下載體速度可表示為：

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

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

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

3 車輛運(yùn)動約束與地磁匹配

3.1 運(yùn)動約束

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

式中，vOBD為OBD 輪速信息。

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

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

3.2 地磁匹配

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

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

變換前磁測量值與時(shí)間、行駛里程與時(shí)間的對應(yīng)關(guān)系如下：

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

對里程序列（D,T）進(jìn)行等里程間隔的采樣，并根據(jù)采樣點(diǎn)所對應(yīng)的測量時(shí)刻讀?。∕,T）中相應(yīng)的磁測量值，完成磁測量值的空間刻度化處理：

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

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

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

式中，與分別為滑動窗口內(nèi)實(shí)測磁場數(shù)據(jù)與基準(zhǔn)庫磁場數(shù)據(jù)的均值，在搜索區(qū)間kL∈（w,KL）內(nèi)，相關(guān)系數(shù)峰值所對應(yīng)的基準(zhǔn)庫位置即為匹配位置。

4 多源融合模型

4.1 狀態(tài)方程

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

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

4.2 量測方程

4.2.1 GNSS 信息

當(dāng)GNSS 信息有效時(shí)，采用GNSS 定位、測速結(jié)果校正MIMU-SINS 解算誤差，GNSS 位置觀測量為：

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

GNSS 速度觀測量為：

4.2.2 地磁匹配結(jié)果

相比開闊空間，在城市峽谷、地下車庫等GNSS信號遮蔽環(huán)境中，地磁特征更加豐富。當(dāng)車輛行駛至地磁基準(zhǔn)數(shù)據(jù)庫可用區(qū)域時(shí)，采用地磁匹配位置結(jié)果校正MIMU-SINS 解算誤差，觀測方程為：

式中，LMag、λMag、hMag為緯度、經(jīng)度、高度匹配結(jié)果，觀測矩陣為匹配位置觀測噪聲。

4.2.3 車輛運(yùn)動約束

當(dāng)車輛處于運(yùn)動狀態(tài)時(shí)，車輛載體的橫向、垂向虛擬零速與OBD 輪速信息構(gòu)成完成性運(yùn)動約束，觀測矢量為：

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

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

當(dāng)處于運(yùn)動狀態(tài)時(shí)，車輛運(yùn)動約束觀測方程為：

當(dāng)車輛處于靜止?fàn)顟B(tài)時(shí)，為了提高航向角的修正精度，同時(shí)進(jìn)行ZUPT 和ZIHR 修正，觀測矢量對于MIMU-SINS 解算，航向角變化率表示為：

式中，θ為俯仰角，γ為橫滾角，車輛靜止時(shí)中僅包含地球自轉(zhuǎn)項(xiàng)。

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

靜止?fàn)顟B(tài)下車輛航向角φk=φk-1，則可計(jì)算相鄰時(shí)刻航向角差值為：

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

靜止?fàn)顟B(tài)下，車輛運(yùn)動約束觀測方程為：

式中，

為觀測矩陣，V5為靜止?fàn)顟B(tài)下車輛東向、北向、天向速度和航向角觀測噪聲。

4.3 多源信息融合

車輛行駛環(huán)境與狀態(tài)復(fù)雜多變，使GNSS、地磁匹配及運(yùn)動約束等導(dǎo)航源的可用性并不穩(wěn)定，需要信息融合算法滿足即插即用要求。本文采用序貫濾波[12]方法進(jìn)行多源融合解算，算法結(jié)構(gòu)如圖4所示。

圖4 序貫濾波信息融合算法結(jié)構(gòu)圖Fig.4 The diagram of sequential-filtering fusion algorithm

在序貫濾波信息融合過程中，需要對各導(dǎo)航源信息的有效性進(jìn)行判斷，包括GNSS 定位、測速有效性、地磁匹配有效性及零速狀態(tài)，并依據(jù)有效性檢測結(jié)果在同一周期內(nèi)順序?yàn)V波處理。狀態(tài)更新過程如下：

式中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，W為系統(tǒng)噪聲矩陣，K為增益矩陣，i=2…N為導(dǎo)航源序號。對于第k個(gè)濾波

5 試驗(yàn)驗(yàn)證

5.1 試驗(yàn)場景

試驗(yàn)區(qū)域?yàn)橹锌圃罕本┬录夹g(shù)基地周邊區(qū)域，路線長度約8.4 km，包括高樓遮擋、地下車庫、高速路等典型城市環(huán)境。試驗(yàn)設(shè)備包括：別克昂科威車輛提供OBD 輪速信息（經(jīng)標(biāo)定后積分里程精度約1%）；某品牌智能手機(jī)作為車輛多源融合導(dǎo)航軟件運(yùn)行平臺，采集各傳感器數(shù)據(jù)并進(jìn)行信息融合處理；NovAtel-100C 組合導(dǎo)航系統(tǒng)用于離線建立地磁基準(zhǔn)庫時(shí)提供高精度空間坐標(biāo)，實(shí)時(shí)融合定位過程則作為基準(zhǔn)比對源。試驗(yàn)場景及設(shè)備如圖5所示。

圖5 試驗(yàn)場景及設(shè)備圖Fig.5 Experiment route and equipment

5.2 結(jié)果分析

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

圖6 安裝偏差角估計(jì)結(jié)果Fig.6 Estimation results of the mounting angle

圖7所示為測試過程GNSS 定位結(jié)果。分析可見：在普通道路及高速等開闊環(huán)境，GNSS 具有良好的定位性能，水平、高程定位誤差均優(yōu)于5.0 m；然而，當(dāng)車輛駛?cè)胫锌圃盒录夹g(shù)園區(qū)后，由于受到建筑物遮擋，GNSS 定位結(jié)果出現(xiàn)明顯波動，水平、高程定位誤差最大分別達(dá)到49.3 m 和18.7 m，且在兩處地下車庫中分別存在約215 m、286 m 長的不定位，嚴(yán)重限制了車輛導(dǎo)航的可用性。

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

圖8所示為典型的OBD 輪速積分里程誤差與磁場特征空間刻度化情況。圖8中可見，采用基于里程信息輔助的磁場特征空間刻度化處理能夠?qū)崿F(xiàn)在線測量數(shù)據(jù)與基準(zhǔn)庫數(shù)據(jù)的空間尺度統(tǒng)一，從而避免了類似DTW 中大量的尺度伸縮運(yùn)算，可顯著提高匹配解算的實(shí)時(shí)性。

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

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

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

圖10對比分析了智能手機(jī)不同導(dǎo)航源組合模式下的定位性能。圖10中可見，GNSS/MIMU/OBD 輪速組合模式能夠有效抑制GNSS 顯著跳點(diǎn)，且可在短時(shí)GNSS 信號遮蔽場景中提供連續(xù)、穩(wěn)定的定位功能。然而，由于MIMU、OBD 輪速均存在誤差累積的問題，導(dǎo)致車輛駛?cè)胫锌圃盒录夹g(shù)園區(qū)后仍存在明顯的定位性能下降?？梢灶A(yù)見，對于長時(shí)GNSS 信號遮蔽環(huán)境，GNSS/MIMU/OBD 輪速組合模式定位結(jié)果將逐漸發(fā)散。GNSS/MIMU/OBD 輪速/地磁匹配組合模式則進(jìn)一步引入地磁匹配絕對定位信息，能夠有效解決導(dǎo)航結(jié)果發(fā)散的問題，且不受應(yīng)用場景限制。測試過程中該模式水平定位誤差優(yōu)于2.4 m，高程定位誤差優(yōu)于0.7 m，顯著提高了車輛導(dǎo)航性能。

圖10 多源融合導(dǎo)航定位結(jié)果Fig.10 Multi-source fusion positioning results

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

圖11 多源融合導(dǎo)航測速結(jié)果Fig.11 Multi-source fusion velocity results

圖12 多源融合導(dǎo)航航向結(jié)果Fig.12 Multi-source fusion heading results

6 結(jié) 論

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