基于里程計的克服衛(wèi)星失鎖時的gnss/ins車載組合定位定向算法
【專利摘要】本發(fā)明公開了基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向算法，將GNSS數(shù)據(jù)與慣性導航系統(tǒng)解算出位置和速度，二者求差后的值當做卡爾曼濾波器的量測觀測值，反饋校正各狀態(tài)量；使用里程計來校正IMU中各狀態(tài)量，利用INS/ODO組合的方法，以IMU的輸出量和里程計在濾波時刻期間的輸出值求和作差來校正里程計的刻度因子，里程計坐標系和導航坐標系之間的安裝誤差角；GNSS衛(wèi)星失鎖時，里程計校正IMU中各誤差項，尤其是對于提升IMU獨立導航的位置精度有很大的幫助。這非常適合應用于在城市峽谷中，GNSS衛(wèi)星信號常會受到遮擋導致的信號失鎖現(xiàn)象，使得整個系統(tǒng)仍能保持高精度的定位結果。
【專利說明】
基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向 算法
技術領域
[0001] 本發(fā)明設及高精度的車載組合定位定向系統(tǒng)，具體是基于里程計的克服衛(wèi)星失鎖 時的GNSS^NS車載組合定位定向算法。
【背景技術】
[0002] 和GNSS/INS實時組合導航相比，測量領域的GNSS/INS組合定位定姿具有位置姿態(tài) 精度要求高、可事后處理等特點。GNSS(Global化vigation Satellite System,全球衛(wèi)星 導航系統(tǒng))工作于差分模式，利用載波相位觀測量求解整周模糊度并獲取厘米級位置信息， IMUQnedial measurement unit,慣性測量單元）提供姿態(tài)信息，在GNSS失鎖期間提供載 體的連續(xù)位置信息。然而在城市峽谷等導航衛(wèi)星信號接收條件較差的地區(qū)，GNSS存在大量 短時甚至長時間的失鎖，此時GNSS/INS組合系統(tǒng)的精度也快速下降，不能滿足MMS無控制測 量要求，因此有必要引入其它傳感器輔助定位。里程計(Odometer, 0D0)是測量兩廂正交脈 沖個數(shù)的傳感器，乘W相應的刻度因子即可得到載體行駛距離的一種傳感器，具有自主性 高、測量范圍寬的特點，但是單獨的里程計不具備導航定位功能，必須要和慣性導航系統(tǒng)同 時使用才能進行導航定位，其誤差隨行駛距離而線性累積，相對于INS( Inertial 化vigation System,即慣性導航系統(tǒng)）誤差隨時間呈二次增長關系，在地面車載等低速運 動情況下，里程計的誤差要小的多，因此GNSS/0DCVINS的組合有利于提高在城市峽谷中的 定位定姿精度。
[0003] 另一方面，里程計刻度因子受溫度、胎壓等影響，其初值并不精確，而P0S與載體之 間也存在一定的安裝誤差角，因此在MMS測量之前需對P0S系統(tǒng)進行誤差標定。本文分析了 里程計刻度因子誤差和P0S安裝誤差對測量結果的影響，利用里程計與INS里程增量之間約 束的關系，在GNSS連續(xù)觀測和固定模糊度條件下，對誤差進行校正；反之在GNSS失鎖條件 下，利用校正過的里程計對INS導航誤差進行估計。跑車實驗驗證了誤差校正算法的有效 性;同時，補償了安裝誤差角和刻度因子后，P0S的定位精度有顯著提高。

【發(fā)明內容】

[0004] 本發(fā)明的目的在于提供基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位 定向算法，為了解決車載導航定位中，因衛(wèi)星失鎖導致組合系統(tǒng)轉變成慣性導航系統(tǒng)單獨 導航時各項導航參數(shù)誤差隨時間積分而發(fā)散運一技術難題。
[0005] 為實現(xiàn)上述目的，本發(fā)明提供如下技術方案：
[0006] 基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向算法，包括W下步 驟：
[0007] 步驟1，GNSS數(shù)據(jù)載波相位差分解算，得到厘米級的定位精度，即提供整個GNSS/ INS車載組合系統(tǒng)的位置基準，慣性導航系統(tǒng)利用GNSS提供的位置觀測量和已知的地球自 轉參數(shù)進行粗對準，完成后進行慣性導航系統(tǒng)的單獨導航推算；
[0008] 步驟2，GNSS數(shù)據(jù)解算出位置和速度，慣性導航系統(tǒng)同時也算出位置和速度，二者 求差后的值當做卡爾曼濾波器的量測觀測值，反饋校正各狀態(tài)量；當GNSS/INS車載組合系 統(tǒng)構成的主濾波器已經收斂時，INS/0D0濾波器開啟工作，即利用IMU來校正0D0中的誤差， WIMU的輸出量和里程計在濾波時刻期間的輸出值求和作差來校正里程計的刻度因子、里 程計坐標系和導航坐標系之間的安裝誤差角；
[0009] 步驟3,若車載或者基站處的接收機觀測數(shù)據(jù)無法進行差分解算，并且持續(xù)了不少 于5秒的時間，此時能計算出相應時間內里程計輸出的脈沖個數(shù)，結合已經被IMU校正過后 的里程計刻度因子誤差、安裝誤差角，轉換成距離觀測量，對GNSS/INS車載組合系統(tǒng)的狀態(tài) 量進行校正。
[0010] 作為本發(fā)明進一步的方案:里程計安裝在從動輪上，避免車輛打滑。
[0011] 作為本發(fā)明進一步的方案:里程計誤差的校正使用的是慣性導航系統(tǒng)的位置和相 應的協(xié)方差矩陣。
[0012] 作為本發(fā)明進一步的方案:卡爾曼濾波器采用GNSS/INS與INS/0D0兩級卡爾曼濾 波器級聯(lián)結構;其中INS/0D0卡爾曼濾波器的時間更新和量測更新周期需要從里程計的分 辨率、車輪直徑W及車速等多方面考慮，并且時間更新和量測更新周期可W不一致。
[0013] 作為本發(fā)明進一步的方案：里程計校正慣性導航系統(tǒng)狀態(tài)量，僅當GNSS失鎖一定 時間后才進行，一定時間是指不少于5秒。
[0014] 本發(fā)明的原理
[0015] 在GNSS/INS的組合定位測姿中，濾波的狀態(tài)方程是非線性的，在經過線性化后各 狀態(tài)量也就變成各狀態(tài)的誤差量，因而得采用擴展卡爾曼濾波的形式進行組合，其狀態(tài)、量 測方程如下式：
[0016]
|| I
[0017] Z =冊(t)+V (2)
[0018] 式中：X(t)是狀態(tài)向量，其協(xié)方差矩陣為P陣，W(t)是系統(tǒng)噪聲，其相應的協(xié)方差矩 陣為Q，G(t)是噪聲驅動矩陣，F(xiàn)(t)是狀態(tài)轉移矩陣;Z是量測值，V的協(xié)方差矩陣是R陣。公式 和公式給出了 GNSS/INS濾波器的狀態(tài)更新方程和量測更新方程。即是對本發(fā)明的步驟2中 GNSS/INS濾波器的具體說明。
[0019]導航參考坐標系選擇地球固連坐標系ECEF，記為e系;載體坐標系XYZ軸分別為右、 前、上Ξ個方向，記為V系；IMU慣性傳感器坐標系記為b系。里程計由于安裝和測量原理的關 系，只能測量載體Y方向的運動;里程計在載體坐標系下的測量值可寫為： 瓣]
Is)
[0021]其中η為里程計脈沖個數(shù)。里程計在載體系內的里程增量為：
[00。]
《排
[0023]其中k為里程計刻度因子。通過旋轉矩陣，可將里程計在載體系內的里程增量換算 至地固系：
[0024]
[0025] 其中爲:為b系至e系旋轉矩陣，嫣'為V系至b系旋轉矩陣。在短時間內，認為蜀為常
值矩陣，令 馬/.:;:和竣<;分別為濾波周期前后的旋轉矩陣。若IMU > 不存在安裝誤差，則變?yōu)閱挝魂嚕话愕那闆r下該矩陣為由航向安裝誤差角ay、俯仰安 裝誤差角ap和橫滾安裝誤差角ar按順序旋轉構成的非單位陣。記安裝誤差角為a=[ap ar ay]T，在GNSS連續(xù)觀測條件下忽略旋轉矩陣夠的誤差，同時考慮式中的刻度因子誤差，則實 際計算的里程計在e系中的里程增量可寫為：
[0026]
[0027]其中化為里程計刻度因子誤差，a X表示安裝誤差角向量的反對稱矩陣。忽略式 (4)中的二階小量，有
[002引
[0029] 其中公式右邊第一項為理想里程增量，第二項和第Ξ項則為由里程計刻度因子誤 差和P0S安裝誤差引起的里程增量誤差。顯然，由里程計刻度因子誤差和安裝誤差角造成的 位置誤差與單位時間內里程增量成正比關系，若未補償則將分別引起烹ib、泉皆與 X《的里程誤差。W1 %的刻度因子誤差和1°航向安裝誤差角為例，若載體行駛1km，貝。 里程計將分別有10m和17.45m的誤差，并最終進入GNSS/0DCVINS組合定位結果。
[0030] 另一方面，由GNSS^NS組合導航結果，有在地固系下的Ξ維里程增量：
[0031]
[0032] 在GNSS連續(xù)觀測和固定整周模糊度的情況下，忽略式的位置誤差。顯然里程計理 想里程增量等于GNSS^NS理想里程增量，將式代入式，有：
[0033]
[0034] 根據(jù)式，WGNSS^NS組合結果作為觀測，即可對里程計刻度因子誤差和P0S安裝誤 差角進行估計。此上發(fā)明原理解釋了
【發(fā)明內容】
的步驟2中INS/0D0濾波器，尤其是最后給出 的公式是INS/0D0濾波器的觀測方程。
[0035] 在GNSS失鎖條件下，可W使用校正過刻度因子的里程計和安裝誤差角輔助IMU，此 良陽DCVIMU組合導航。將式重寫為
[0036]
[0037] 其中ε為旋轉矩陣攝失準角，.超.和k均使用校正值。式中右邊第一項為理想里程增 量，第二項為由失準角引起的里程誤差。將式第一項用IMU里程增量代替，并寫為誤差的形 式：
[00；3 引
[0039] 其中與癡為^11在e系中的Ξ維位置誤差。式即為0D0/IMU組合導航卡爾曼濾波器 量測方程，方程左邊為觀測量，右邊寫為矩陣的形式，有：
[0043] 0DCVIMU組合卡爾曼濾波器的狀態(tài)方程與公式一致。
[0044] 運闡述了本
【發(fā)明內容】
的步驟3中提到的0D0/IMU濾波器內容。
[0045] 與現(xiàn)有技術相比，本發(fā)明的有益效果是：
[0046] 本發(fā)明基于里程計輔助的克服衛(wèi)星失鎖時的GNSS^NS車載組合定位定向算法，針 對里程計刻度因子和P0S安裝誤差角的校正，在不改變原GNSS^NS濾波器的基礎上，設計了 GNSS/INS與INS/0D0兩級卡爾曼濾波器級聯(lián)結構，將INS導航誤差與里程計刻度因子誤差、 安裝誤差角依次與兩個濾波器的系統(tǒng)狀態(tài)對應，其包括GNSS結果正常時對于里程計刻度因 子誤差校正，里程計坐標系和導航坐標之間安裝誤差角校正;GNSS衛(wèi)星數(shù)據(jù)丟失時，里程計 校正IMU中各誤差項，尤其是對于提升IM閑蟲立導航的位置精度有很大的幫助。運非常適合 應用于在城市峽谷中，GNSS衛(wèi)星信號常會受到遮擋導致的信號失鎖現(xiàn)象，使得整個系統(tǒng)仍 能保持高精度的定位結果。
【附圖說明】
[0047] 圖1是GNSS^NS/ODO誤差校正濾波器結構示意圖；
[004引圖2是0D0/INS組合導航濾波器結構示意圖；
[0049] 圖3是實驗載體軌跡圖；
[0050] 圖4是實驗安裝誤差角示意圖；
[0051 ]圖5是實驗里程計刻度因子示意圖；
[0化2]圖6是實驗0D0/INS組合平面誤差圖；
[0化3]圖7是實驗0D0/INS組合高程誤差圖。
【具體實施方式】
[0054]下面將結合本發(fā)明實施例，對本發(fā)明實施例中的技術方案進行清楚、完整地描述， 顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而不是全部的實施例?；诒景l(fā)明中的 實施例，本領域普通技術人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都 屬于本發(fā)明保護的范圍。
[0化5] 實施例1
[0056] 請參閱圖1-圖2,本發(fā)明實施例中，基于里程計的克服衛(wèi)星失鎖時的GNSS^NS車載 組合定位定向算法，其包括GNSS/INS濾波器收斂時對于里程計刻度因子誤差校正，里程計 坐標系和導航坐標之間安裝誤差角校正;GNSS衛(wèi)星數(shù)據(jù)丟失時，里程計校正IMU中各誤差 項。
[0057] 具體操作步驟如下:GNSS/INS與INS/0D0兩級卡爾曼濾波器級聯(lián)結構
[0058] 利用式對刻度因子和安裝誤差角校正的前提是IMU被充分校正，此時安裝誤差角 和刻度因子與INS導航誤差無關，因此本文中濾波器采用兩級卡爾曼濾波級聯(lián)設計，如圖1 所示，第一級為GNSS/INS卡爾曼濾波器，第二級為INS/0D0卡爾曼濾波器。GNSS/INS卡爾曼 濾波器利用GNSS偽距和載波相位觀測量，經雙頻相關法(DUFC0M)解算整周模糊度，由此獲 得厘米級位置信息，與IMU進行松散組合，校正INS導航及傳感器誤差。在GNSS^NS卡爾曼濾 波器充分收斂后，啟動INS/0D0卡爾曼濾波器，WGNSS^NS輸出的高精度位置和姿態(tài)信息作 為INS/0D0的輸入，其中姿態(tài)角用作將里程計增量轉換至e系內，而前后濾波間隔內的INS里 程增量作為里程計的外部觀測，估計IMU安裝誤差角和里程計刻度因子。
[0059] 由于里程計在b系中的里程增量與橫滾安裝誤差角ar無關，因此濾波器狀態(tài)量選 取里程計刻度因子誤差化、航向安裝誤差角ay和俯仰安裝誤差角ap，均模擬為隨機游走過 程，因此相應的有濾波器狀態(tài)方程：
[0060]
[0061]其中Ksk、-^、和我，分別為Ξ個狀態(tài)量的隨機游走功率譜密度系數(shù)。式給出了濾波 器觀測方程的一般形式，其中方程左邊為觀測量：
[0066]將式右邊寫為矩陣的形式，并忽略橫滾安裝誤差角ar，則INS/0D0濾波器觀測方程 為：
[0067]
[0068] 兩級濾波器結構將INS誤差與里程計等相關誤差分別計算，有利于在原GNSS/INS 組合算法的基礎上進行改進，工程實現(xiàn)相對簡單;此外，相對于把INS導航誤差和里程計刻 度因子、安裝角誤差歸為一個濾波器狀態(tài)的濾波器結構，級聯(lián)濾波器更適合于在里程計更 新頻率較GNSS頻率高的情況，而通常情況也是如此;第Ξ，級聯(lián)結構避免了過高的矩陣階 數(shù)，系統(tǒng)更加穩(wěn)定。對于卡爾曼濾波器的更新頻率，由于事后處理不存在計算量上的考量， 因此更新頻率可相對實時導航較高。GNSS/INS濾波器的時間更新W0.1 s間隔為宜，量測更 新可根據(jù)GNSS數(shù)據(jù)率選擇1-10化。而INS/0D0濾波器的更新時間間隔不宜過長，否則不滿足 短時間內旋轉矩陣據(jù)為常值矩陣的條件，若時間過短，則更新周期內脈沖數(shù)目過低，引起較 大的截斷誤差，因此INS/0D0濾波器的時間更新和量測更新周期需要從里程計的分辨率、車 輪直徑W及車速等多方面考慮，在車速過低的情況下，可W停止校正P0S安裝誤差角和里程 計刻度因子。
[0069] 為考察本發(fā)明對于GNSS失鎖時系統(tǒng)輸出量精度提升的效果，W具體數(shù)據(jù)說明。W 2014年6月20日在武漢某地的車載P0S系統(tǒng)和里程計組合采集的數(shù)據(jù)進行分析，實驗全程中 GNSS觀測條件良好（目的是把GNSS定位正常的結果作為基準，供其他方法的比較），速度在 18m/s，車輛首先靜止約10分鐘，完成IMU熱啟動后，按照預先規(guī)劃好的線路行駛。
[0070] 首先計算出GNSS衛(wèi)星未失鎖時，GNSS/INS/0D0(里程計)組合的結果，作為比對的 基準，選擇路線中一段曲線路段，刪除掉數(shù)據(jù)長度為2分鐘的GNSS定位結果，現(xiàn)若不使用0D0 數(shù)據(jù)輔助導航，即該時間段只能由IMU進行獨立導航，若按照本發(fā)明提出的基于里程計的克 服衛(wèi)星失鎖時的GNSS^NS車載組合定位定向算法，和完整GNSS^NS組合的結果做對比。圖3 實驗載體軌跡圖，圖4和圖5分別為實驗II P0S安裝誤差角和里程計刻度因子校正過程，其 中俯仰安裝誤差角約為0.9°，航向安裝誤差角相對較大，約為1.8°，里程計刻度因子校正結 果為0.00209。使用校正獲得的安裝誤差角和里程計刻度因子，考察在GNSS失鎖條件按下 0DCVINS組合導航。人為在實驗中刪除一段2分鐘GNSS數(shù)據(jù)，此時W里程計里程增量作為INS 的外部觀測，對INS誤差進行校正。處理按5種方式進行處理，INS表示INS獨立導航;0DCVINS 1#表示使用里程計校正INS，里程計刻度因子使用初始值，安裝誤差角未校正;0D0/INS 2# 表示使用里程計校正INS，里程計刻度因子使用校正值，但安裝誤差角未校正;0D0/INS 3# 表示使用里程計校正INS，里程計刻度因子和安裝誤差角均使用校正值;0D0/INS 4#表示在 0D0/INS 3#的基礎上，使用事后平滑卡爾曼濾波器做平滑濾波處理。圖6和圖7分別為實驗 GNSS中斷條件下的平面和高程誤差，由圖6和圖7可知，在未補償任何初始誤差的條件下， 0D0/INS 1#組合導航的平面誤差在開始階段要大于INS獨立導航結果，但INS誤差累計速度 明顯高于里程計，前者明顯為二次關系，后者基本為線性關系，因此在GNSS中斷的后半段 0D0/INS定位結果要小于INS獨立導航結果;在補償了里程計刻度因子條件下，0D0/INS 2# 定位誤差能夠一開始就小于純INS，說明了里程計刻度因子校正結果的有效性;若里程計刻 度因子和安裝誤差角均使用校正值，則0D0/INS 3#的定位誤差略小于0D0/INS 2#，運是由 于俯仰安裝誤差角和偏航安裝誤差角均比較小，對最終定位結果的影響有限；使用了卡爾 曼平滑濾波器后，ODO/INS 4#平面誤差又相較ODO/INS 3#有大幅減小，比INS獨立導航結果 提高了一個數(shù)量級，運是得益于每個歷元均使用了前后歷元數(shù)據(jù)作為觀測，而實時或者單 向濾波只能使用時間上較前的數(shù)據(jù)。對于GNSS中斷的高程誤差，使用校正后的結果略有提 高，但是并不顯著，而平滑后則有顯著提高。
[0071] 對于本領域技術人員而言，顯然本發(fā)明不限于上述示范性實施例的細節(jié)，而且在 不背離本發(fā)明的精神或基本特征的情況下，能夠W其他的具體形式實現(xiàn)本發(fā)明。因此，無論 從哪一點來看，均應將實施例看作是示范性的，而且是非限制性的，本發(fā)明的范圍由所附權 利要求而不是上述說明限定，因此旨在將落在權利要求的等同要件的含義和范圍內的所有 變化囊括在本發(fā)明內。
[0072] 此外，應當理解，雖然本說明書按照實施方式加 W描述，但并非每個實施方式僅包 含一個獨立的技術方案，說明書的運種敘述方式僅僅是為清楚起見，本領域技術人員應當 將說明書作為一個整體，各實施例中的技術方案也可W經適當組合，形成本領域技術人員 可W理解的其他實施方式。
【主權項】
1. 基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向算法，其特征在于，包 括以下步驟： 步驟1，GNSS數(shù)據(jù)載波相位差分解算，得到厘米級的定位精度，即提供整個GNSS/INS車 載組合系統(tǒng)的位置基準，慣性導航系統(tǒng)利用GNSS提供的位置觀測量和已知的地球自轉參數(shù) 進行粗對準，完成后進行慣性導航系統(tǒng)的單獨導航推算； 步驟2，GNSS數(shù)據(jù)解算出位置和速度，慣性導航系統(tǒng)同時也算出位置和速度，二者求差 后的值當做卡爾曼濾波器的量測觀測值，反饋校正各狀態(tài)量；當GNSS/INS車載組合系統(tǒng)構 成的主濾波器已經收斂時，INS/ODO濾波器開啟工作，即利用頂U來校正0D0中的誤差，以頂U 的輸出量和里程計在濾波時刻期間的輸出值求和作差來校正里程計的刻度因子、里程計坐 標系和導航坐標系之間的安裝誤差角； 步驟3,若車載或者基站處的接收機觀測數(shù)據(jù)無法進行差分解算，并且持續(xù)了不少于5 秒的時間，此時能計算出相應時間內里程計輸出的脈沖個數(shù)，結合已經被MU校正過后的里 程計刻度因子誤差、安裝誤差角，轉換成距離觀測量，對GNSS/INS車載組合系統(tǒng)的狀態(tài)量進 行校正。2. 根據(jù)權利要求1所述的基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向 算法，其特征在于，里程計安裝在從動輪上。3. 根據(jù)權利要求1所述的基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向 算法，其特征在于，里程計誤差的校正使用的是慣性導航系統(tǒng)的位置和相應的協(xié)方差矩陣。4. 根據(jù)權利要求1所述的基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向 算法，其特征在于，卡爾曼濾波器采用GNSS/INS與INS/ODO兩級卡爾曼濾波器級聯(lián)結構。5. 根據(jù)權利要求1所述的基于里程計的克服衛(wèi)星失鎖時的GNSS/INS車載組合定位定向 算法，其特征在于，里程計校正慣性導航系統(tǒng)狀態(tài)量，僅當GNSS失鎖一定時間后才進行，一 定時間是指不少于5秒。
【文檔編號】G01S19/48GK105824039SQ201610152739
【公開日】2016年8月3日
【申請日】2016年3月17日
【發(fā)明人】孫紅星, 丁學文, 王暉
【申請人】孫紅星