基于EKF的GNSS/SINS組合導航系統應用

,王海霞,陳云

(1.山東科技大學 山東省機器人與智能技術重點實驗室，山東 青島 266590；2.山東科技大學 海洋工程研究院，山東 青島 266590；3.山東科技大學 電氣信息系，山東 濟南 250031)

為獲得更高精度的定位結果，將多種導航定位技術組合，已成為導航定位的熱門發展方向。全球衛星導航系統(global navigation satellite system, GNSS)是全球所有的衛星導航系統的總稱，包括GPS(美國)、GLONASS(俄羅斯)、GALILEO(歐洲)和 BDS(中國)等衛星導航系統。隨著各導航系統的不斷發展建設，多頻多系統GNSS應用已成為衛星導航定位系統的主要發展趨勢之一。與單一系統相比，GNSS系統可以為衛星導航系統提供更豐富的可用信號，更好的星座條件，獲得更高精度的載體三維位置信息[1]。然而，當衛星信號被遮擋或受到干擾時，如在室內、高樓密集的城市、茂密的樹林、海上等環境，GNSS系統無法得到穩定、持續的定位信息。慣性導航系統(inertial navigation system, INS)是一種自主式導航系統，依據慣性工作原理，應用陀螺儀、加速度計等慣性元件測得載體的角速度、加速度，然后通過積分得到其位置、速度和姿態等定位信息。INS具有自主性、隱蔽性及連續提供位置、速度、姿態信息等優點，但其導航誤差會隨時間增加而積累，嚴重影響定位的精度，不適合長時間連續工作[2]。INS主要包括平臺慣性導航系統(platform inertial navigation system, PINS)和捷聯慣性導航系統(strapdown inertial navigation system, SINS)，二者本質相同，但PINS采用物理平臺模擬導航坐標系，而SINS采用的是數學平臺。SINS由于具有結構簡單、體積小、重量輕、成本低和可靠性高等優點，逐漸成為目前最常用的慣性導航系統[3]。

通過以上分析可發現，若將兩者結合起來，GNSS與SINS就能達到優勢互補的效果[4-6]。當衛星信號接收良好時，利用GNSS的定位結果對SINS定位結果進行校正，消除SINS的累積誤差；當衛星信號受到遮擋時，SINS發揮短時間內定位精度高的優點單獨進行定位，可以持續得到高精度、高可靠性的導航信息。文獻[11]對基于擴展卡爾曼濾波(extended Kalman filter, EKF)的GNSS/SINS深組合導航系統進行仿真，其位置誤差較大，地向誤差達到10 m左右。文獻[12]對GPS/SINS緊密組合導航系統進行了仿真研究，表明緊密組合導航比松散組合導航收斂性更好、精度更高，仿真實驗的位置誤差在0.5 m左右。文獻[13]通過仿真實驗證明基于EKF的GPS/SINS松組合導航算法能對載體的速度和姿態實現有效跟蹤，但沒有給出是否能對載體的位置進行有效跟蹤，且載體的導航誤差8 min后才收斂，位置誤差最低值在5 m·s-1左右。本研究針對GNSS、SINS單獨應用時存在不足的問題，將兩者組合，并通過車載實驗采集導航數據，利用EKF算法對GNSS/SINS組合導航系統進行解算，得到的位置誤差、速度誤差及姿態誤差都快速收斂，且位置誤差精度保持在厘米級，速度誤差在0.1 m·s-1以內。

1 捷聯慣性導航

1.1 捷聯慣性導航系統基本原理

SINS屬于遞推導航系統，以初始對準得到的初始姿態、載體的初始位置、速度為基礎，利用陀螺儀、加速度計得到角速率及比力值，再通過捷聯慣性導航算法推算出下一時刻載體的姿態、位置、速度信息。以此類推，可以計算出載體每一時刻的導航信息。SINS的數據更新頻率高，且在短時間內擁有較高的導航精度。

1.2 捷聯慣性導航系統基本方程

采用當地地理坐標系作為導航坐標系(n系), 當地地理坐標系也稱為東北天(east-north-up, ENU)坐標系[7]。SINS導航方程[8-9]主要包括姿態微分方程、速度微分方程和位置微分方程：

1) SINS的姿態微分方程為：

(1)

(2)

2) SINS的速度微分方程為：

(3)

3) SINS的位置微分方程為：

(4)

2 GNSS/SINS組合導航

2.1 GNSS/SINS組合導航原理

研究的組合導航[9]形式為松組合，松組合中SINS的加速度計、陀螺儀分別輸出載體當前的比力、角速度信息，通過捷聯慣性導航解算系統可求得位置PSINS、速度VSINS、姿態矩陣；而通過GNSS得到位置PGNSS、速度VGNSS。SINS與GNSS的位置、速度的差值為擴展卡爾曼濾波器的輸入，通過組合導航算法得到慣導系統狀態誤差估計值[10-11]，利用該誤差估計值對捷聯慣導系統輸出值進行校正，最后得到高精度、穩定連續的載體位置、速度及姿態信息。其原理圖如圖1所示。

圖1 GNSS/SINS組合導航原理圖Fig.1 Schematic of the integrated navigation of GNSS/SINS

2.2 GNSS/SINS組合導航模型

GNSS/SINS組合導航模型將慣導誤差方程、GNSS定位測速的誤差方程分別作為系統狀態方程和量測方程的基礎，該系統的狀態變量是由姿態誤差、速度誤差、位置誤差、陀螺儀測量誤差及加速度計測量誤差組成的15維向量組成[12]，即

X=[φE,φN,φU,δνE,δνN,δνU,δL,δλ,δh,εx,εy,εz,▽x,▽y,▽z]T。

(5)

系統的狀態方程和量測方程[13]分別為：

(6)

式中：F(t)為系統狀態矩陣；G(t)為系統噪聲矩陣；Wd(t)為系統噪聲向量；Z(t)為量測向量；H(t)為量測矩陣；V(t)為量測噪聲向量。Wd(t)與V(t)均服從零均值白噪聲，相互獨立且互不相關。

式(6)中的系統狀態方程和量測方程是線性連續的，為了能夠在計算機中實現對數據的編程處理，需將其進行離散化處理[14]，即

(7)

式中：Xk-1、Xk分別為k-1、k時刻系統的狀態向量；Φk,k-1為k-1時刻至k時刻的系統狀態轉移矩陣；Γk-1為k-1時刻的系統噪聲驅動矩陣；Wk-1為k-1時刻的系統噪聲向量；Zk為k時刻的量測向量；Hk為k時刻的量測矩陣；Vk為k時刻的量測噪聲向量。

3 EKF算法

卡爾曼濾波(Kalman filter, KF)是狀態估計算法的基礎，同時也是導航系統解算中最常用的濾波方法。KF在處理線性系統時，一般能夠得到較好的結果。而GNSS/SINS組合導航系統的本質是非線性系統，使用KF則不能得到理想的濾波結果。1969年，Bucy等[15]提出了擴展卡爾曼濾波算法，并將其成功的在非線性領域中進行應用，其原理是將非線性系統進行泰勒級數展開，只取至一次項，從而得到其線性化的系統模型，解決了非線性系統不能得到理想的濾波結果的問題。

GNSS/SINS組合導航系統采用閉環反饋校正系統，使狀態誤差能夠保持在較小值，提高組合導航定位的精度。擴展卡爾曼濾波過程包括時間更新和量測更新兩部分，其基本步驟[9]：

1) 時間更新：

(8)

(9)

2) 量測更新：

(10)

式中，Rk為k時刻的量測噪聲協方差矩陣，Kk為k時刻KF增益矩陣。

Pk=(I-KkHk)Pk,k-1，

(11)

式中，Pk為求得的k時刻系統狀態協方差矩陣。

(12)

表1 SPAN-LCI慣導性能指標Tab.1 Performance index of the SPAN-LCI

由于SINS數據更新率比GNSS高，當只有SINS數據時，該系統僅進行時間更新過程，當GNSS的數據時間與SINS的一致時，組合導航系統既進行時間更新過程也進行量測更新過程。GNSS與SINS每次組合后的定位結果都會對該時刻SINS的結果進行校正，以消除其累積誤差，使GNSS/SINS組合導航系統持續取得高精度的載體導航信息。

4 車載實驗及分析

本實驗在山東科技大學附近采集車載數據，實驗設備為NovAtel的SPAN-LCI戰術級IMU慣導、雙系統GNSS(GPS、GLONASS)接收機。其中慣導數據的采樣頻率為200 Hz，GNSS數據的采樣頻率為5 Hz，慣導比GNSS的數據更新率更高。IMU慣導性能指標如表1所示。在Google Earth上顯示的車輛軌跡如圖2所示。實驗分別對SINS單獨導航和GNSS/SINS組合導航進行了定位解算，并將高精度組合導航后處理軟件IE 8.6的解算結果作為參照值。其中，在GNSS信號不丟失的情況下，由IE軟件處理的位置誤差精度水平方向能達到0.01 m，垂直方向能達到0.015 m；其速度誤差精度在水平和垂直方向都能達到0.01 m·s-1；其姿態誤差中的橫滾角、俯仰角誤差精度都達到了0.005°，航向角誤差精度達到了0.008°[16]。

圖 2 Google Earth 中顯示的跑車軌跡圖Fig. 2 Vehicle trajectory in Google Earth

4.1 SINS單獨導航實驗

對由SPAN-LCI戰術級IMU慣導采集的車載數據單獨進行SINS導航解算，得到純慣導定位在ENU三個方向的位置、速度以及姿態結果，并通過與IE解算的高精度結果比較得到SINS的定位誤差，其位置誤差、速度誤差及姿態誤差分別如圖3～5所示。

圖 3 SINS位置誤差Fig. 3 The position error of SINS

圖 4 SINS速度誤差Fig. 4 The velocity error of SINS

圖 5 SINS姿態誤差Fig. 5 The attitude error of SINS

由圖3可以很明顯看出，SINS單獨導航時，其ENU三個方向的位置誤差只在最開始的短時間內較小，但是隨時間的增長，該誤差不斷累積，最后甚至達到幾千米。同樣，由圖4可得，SINS導航在ENU三個方向的速度誤差也隨著時間存在不同程度的累積，其中U方向的速度誤差累積最大，達到20 m·s-1。而由圖5可得，SINS單獨導航得到的三種姿態角誤差沒有隨時間的增加不斷累積。由此可知，SINS不適合單獨導航，需要其他的導航系統的輔助才能消除累積誤差。

4.2 GNSS/SINS組合導航實驗

利用GNSS與SINS優勢互補的特性，對SPAN-LCI戰術級IMU慣導和GNSS采集的車載數據進行基于EKF算法的組合導航解算，得到GNSS/SINS組合導航系統的定位結果，通過與IE解算的結果比較后得到的位置誤差、速度誤差和姿態誤差分別如圖6～8所示。

圖 6 GNSS/SINS組合導航位置誤差Fig. 6 The position error of GNSS/SINS integrated navigation

圖 7 GNSS/SINS組合導航速度誤差Fig. 7 The velocity error of GNSS/SINS integrated navigation

由圖6可得，GNSS/SINS組合導航的ENU三個方向的位置誤差都收斂了，其中位置誤差的精度很高，一直保持在厘米級。通過圖6與圖3的對比可看出，GNSS/SINS組合導航比SINS單獨導航在位置精度方面存在明顯優勢。由圖7得，GNSS/SINS組合導航在E、N兩個方向上的速度誤差都保持在0.05 m·s-1以內，U方向上的速度誤差也保持在0.1 m·s-1以內，與圖4相比，GNSS/SINS組合導航的速度誤差收斂了，并具有較高的精度。對圖8與圖5進行對比可以看出，GNSS/SINS組合導航的三種姿態角誤差比SINS單獨導航時精度稍有提高，其中橫滾角誤差提高了0.1°左右，俯仰角誤差與航向角誤差大概提高0.2°左右。盡管GNSS本身不能提供姿態信息，但是SINS與GNSS組合后能得到精度更高的姿態值。

通過對SINS單獨導航、GNSS/SINS組合導航的車載實驗解算分析可得，基于EKF的GNSS/SINS組合導航算法彌補了GNSS信號失鎖、數據更新率低、無法獲得姿態信息及SINS誤差累積等單系統導航定位中的不足，其能夠充分利用兩者的優勢，得到連續、高數據更新率和高精度的載體導航信息。

圖 8 GNSS/SINS組合導航姿態誤差Fig. 8 The attitude error of GNSS/SINS integrated navigation

5 結語

研究了基于EKF的GNSS/SINS組合導航算法，利用GNSS、SINS優勢互補的特性，通過EKF算法將兩者的定位信息進行融合。首先，研究了SINS、GNSS/SINS以及EKF算法方程，為組合導航算法的實現提供理論基礎；其次，實驗通過由NovAtel的SPAN-LCI戰術級IMU慣導、雙系統GNSS(GPS、GLONASS)接收機組成的車載設備進行數據采集，并分別對SINS導航、GNSS/SINS組合導航系統進行解算；最后，將通過自身組合導航算法解算的數據與高精度組合導航后處理軟件IE 8.6解算的數據對比，得到GNSS/SINS組合導航解算的位置誤差能快速收斂，且精度達到了厘米級，與SINS單獨導航系統相比，該組合導航系統具有明顯優勢。本研究為以后組合導航融合系統的發展提供了一定的實驗基礎。