基于EKF的GNSS/SINS組合導(dǎo)航系統(tǒng)應(yīng)用

,王海霞,陳云

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

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

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

1 捷聯(lián)慣性導(dǎo)航

1.1 捷聯(lián)慣性導(dǎo)航系統(tǒng)基本原理

SINS屬于遞推導(dǎo)航系統(tǒng)，以初始對準(zhǔn)得到的初始姿態(tài)、載體的初始位置、速度為基礎(chǔ)，利用陀螺儀、加速度計得到角速率及比力值，再通過捷聯(lián)慣性導(dǎo)航算法推算出下一時刻載體的姿態(tài)、位置、速度信息。以此類推，可以計算出載體每一時刻的導(dǎo)航信息。SINS的數(shù)據(jù)更新頻率高，且在短時間內(nèi)擁有較高的導(dǎo)航精度。

1.2 捷聯(lián)慣性導(dǎo)航系統(tǒng)基本方程

采用當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系(n系), 當(dāng)?shù)氐乩碜鴺?biāo)系也稱為東北天(east-north-up, ENU)坐標(biāo)系[7]。SINS導(dǎo)航方程[8-9]主要包括姿態(tài)微分方程、速度微分方程和位置微分方程：

1) SINS的姿態(tài)微分方程為：

(1)

(2)

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

(3)

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

(4)

2 GNSS/SINS組合導(dǎo)航

2.1 GNSS/SINS組合導(dǎo)航原理

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

圖1 GNSS/SINS組合導(dǎo)航原理圖Fig.1 Schematic of the integrated navigation of GNSS/SINS

2.2 GNSS/SINS組合導(dǎo)航模型

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

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

(5)

系統(tǒng)的狀態(tài)方程和量測方程[13]分別為：

(6)

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

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

(7)

式中：Xk-1、Xk分別為k-1、k時刻系統(tǒng)的狀態(tài)向量；Φk,k-1為k-1時刻至k時刻的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；Γk-1為k-1時刻的系統(tǒng)噪聲驅(qū)動矩陣；Wk-1為k-1時刻的系統(tǒng)噪聲向量；Zk為k時刻的量測向量；Hk為k時刻的量測矩陣；Vk為k時刻的量測噪聲向量。

3 EKF算法

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

GNSS/SINS組合導(dǎo)航系統(tǒng)采用閉環(huán)反饋校正系統(tǒng)，使?fàn)顟B(tài)誤差能夠保持在較小值，提高組合導(dǎo)航定位的精度。擴展卡爾曼濾波過程包括時間更新和量測更新兩部分，其基本步驟[9]：

1) 時間更新：

(8)

(9)

2) 量測更新：

(10)

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

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

(11)

式中，Pk為求得的k時刻系統(tǒng)狀態(tài)協(xié)方差矩陣。

(12)

表1 SPAN-LCI慣導(dǎo)性能指標(biāo)Tab.1 Performance index of the SPAN-LCI

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

4 車載實驗及分析

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

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

4.1 SINS單獨導(dǎo)航實驗

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

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

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

圖 5 SINS姿態(tài)誤差Fig. 5 The attitude error of SINS

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

4.2 GNSS/SINS組合導(dǎo)航實驗

利用GNSS與SINS優(yōu)勢互補的特性，對SPAN-LCI戰(zhàn)術(shù)級IMU慣導(dǎo)和GNSS采集的車載數(shù)據(jù)進行基于EKF算法的組合導(dǎo)航解算，得到GNSS/SINS組合導(dǎo)航系統(tǒng)的定位結(jié)果，通過與IE解算的結(jié)果比較后得到的位置誤差、速度誤差和姿態(tài)誤差分別如圖6～8所示。

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

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

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

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

圖 8 GNSS/SINS組合導(dǎo)航姿態(tài)誤差Fig. 8 The attitude error of GNSS/SINS integrated navigation

5 結(jié)語

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