伍星華，戰(zhàn)興群，劉 銘

（上海交通大學(xué)航空航天學(xué)院,上海201812）

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）中的陀螺和加速度計(jì)的誤差會(huì)隨時(shí)間累積，因此SINS的精度是隨時(shí)間下降的。GPS具有全球性、全天候的特點(diǎn)，高精度和低成本是它的顯著優(yōu)點(diǎn)[1]。但是，GPS也存在一些不足之處，主要表現(xiàn)為：GPS接收機(jī)的工作受載體機(jī)動(dòng)的影響較大，GPS接收機(jī)的數(shù)據(jù)更新頻率低（一般為1Hz），還容易受到干擾和人為控制，在遮擋的情況下（如室內(nèi)、水下、地下及城市峽谷環(huán)境）不能使用[2]。因此，GPS和SINS具有優(yōu)勢互補(bǔ)的特點(diǎn)，將兩者組合可以構(gòu)建一種比較理想的導(dǎo)航系統(tǒng)，是目前組合導(dǎo)航系統(tǒng)的主要形式之一。根據(jù)組合結(jié)構(gòu)、信息交換及組合程度的不同，GPS/SINS的組合方式可以分為松組合、緊組合、超緊組合等模式。由于模型不復(fù)雜，導(dǎo)航精度較高，緊組合的方式被廣泛使用。

傳統(tǒng)的Kalman濾波算法常被用于GPS/SINS緊組合導(dǎo)航系統(tǒng)以實(shí)現(xiàn)兩者之間的信息融合，然而即使是擴(kuò)展Kalman濾波也未能充分考慮到緊組合導(dǎo)航系統(tǒng)的非線性特性。因此，引入了滾動(dòng)時(shí)域估計(jì)以便更充分地利用系統(tǒng)的多時(shí)歷信息。文獻(xiàn)[3]提出 “有限記憶”估計(jì)以替代Kalman濾波，被認(rèn)為是首次應(yīng)用滾動(dòng)時(shí)域估計(jì)。文獻(xiàn)[4]將此算法引入對(duì)約束線性系統(tǒng)的優(yōu)化估計(jì)問題。

一直以來，滾動(dòng)時(shí)域估計(jì)（Moving Horizon Estimation，MHE）多被應(yīng)用于控制領(lǐng)域的狀態(tài)估計(jì)，這是一個(gè)耗時(shí)的過程。因此，對(duì)于響應(yīng)速度有要求的控制系統(tǒng)，往往在未得到最優(yōu)估計(jì)的情況下便被強(qiáng)制反饋到了輸出反饋回路中。這樣，就需要快速的MHE算法，在不犧牲穩(wěn)定性的前提下削減計(jì)算的復(fù)雜度[5]。有關(guān) MHE的文獻(xiàn)[6]～文獻(xiàn)[8]，均提出了使用快速技術(shù)進(jìn)行在線優(yōu)化。文獻(xiàn)[9]記錄了基于梯度與Newton法的初步結(jié)果。文獻(xiàn)[10]證明了基于Newton法、MHE算法的系統(tǒng)方程在一般非線性假設(shè)中的局部穩(wěn)定性。

1 GPS/SINS緊組合導(dǎo)航系統(tǒng)

在GPS/SINS緊組合中，狀態(tài)方程由SINS誤差狀態(tài)和GPS誤差狀態(tài)組成。將由SINS信息、衛(wèi)星星歷推算出的接收機(jī)相對(duì)于衛(wèi)星視線方向的偽距、偽距率與據(jù)GPS碼環(huán)和載波環(huán)得到的偽距、偽距率相減，作為Kalman濾波器的量測信息，然后利用Kalman濾波器估計(jì)SINS的誤差量及接收機(jī)鐘差等，最后根據(jù)SINS的誤差量對(duì)SINS進(jìn)行輸出校正或反饋校正。

1.1 狀態(tài)方程

SINS以東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，其主要誤差有位置誤差、速度誤差、失準(zhǔn)角、加速度計(jì)常值偏置以及陀螺常值漂移等，SINS的系統(tǒng)狀態(tài)方程為

式（1）中，F(xiàn)I（t）為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，GI（t）為系統(tǒng)的噪聲矩陣，WI（t）為系統(tǒng)的過程噪聲矢量，XI（t）為系統(tǒng)的誤差狀態(tài)矢量。XI（t）的具體表達(dá)式為

式（2）中，δx、δy、δz為地心地固坐標(biāo)系3 方向上的位置誤差，為地心地固坐標(biāo)系3方向上的速度Δ誤差，ΔφE、ΔφN、φU為東北天3方向上的失準(zhǔn)角，x、y、z為載體坐標(biāo)系3方向上的加速度計(jì)常值偏置，εx、εy、εz為載體坐標(biāo)系3方向上的陀螺常值漂移。

GPS的狀態(tài)參數(shù)取2個(gè)時(shí)間相關(guān)的誤差，δtu為接收機(jī)的時(shí)鐘偏差，δfu為接收機(jī)的時(shí)鐘頻漂，則GPS的狀態(tài)方程為

式（3）中，β為誤差相關(guān)時(shí)間，ωtu、ωfu為驅(qū)動(dòng)噪聲。

將式（3）改寫為矩陣形式，即有

合并式（1）和式（4），可得緊組合導(dǎo)航系統(tǒng)的狀態(tài)方程

1.2 量測方程

緊組合系統(tǒng)采用偽距、偽距率組合方式，其量測方程為

式（6）中，H（t）為量測矩陣，V（t）為接收機(jī)在地理坐標(biāo)系下位置與速度的誤差，分別為偽距、偽距率量測矩陣。

在地心地固坐標(biāo)系中，設(shè)載體的真實(shí)位置為（x，y，z），SINS 測量得到的載體位置為 （xI，yI，zI），據(jù)衛(wèi)星星歷給出的衛(wèi)星位置為（xG，yG，zG）。當(dāng)選取4顆可見衛(wèi)星時(shí)，偽距和偽距率量測方程為

因此，

其中，li、mi、ni為載體到衛(wèi)星的向量的方向余弦，bclk為與時(shí)鐘誤差等效的距離誤差，dclk為與時(shí)鐘頻率等效的距離率誤差。

2 滾動(dòng)時(shí)域估計(jì)

眾所周知，GPS/SINS緊組合導(dǎo)航系統(tǒng)實(shí)質(zhì)上是非線性系統(tǒng)。一般研究時(shí)常將其看作線性系統(tǒng)，然后使用Kalman濾波器求解其狀態(tài)方程和量測方程，或是使用擴(kuò)展Kalman濾波，而擴(kuò)展Kalman濾波的中心思想便是將非線性系統(tǒng)線性化后再做Kalman處理。相比較而言，后者對(duì)導(dǎo)航系統(tǒng)狀態(tài)的估計(jì)更加精確，但其在處理過程中仍然忽略了較多的非線性特性。

GPS/SINS緊組合導(dǎo)航系統(tǒng)的離散化非線性系統(tǒng)方程為

類似于滾動(dòng)時(shí)域控制，滾動(dòng)時(shí)域估計(jì)可被描述為基于當(dāng)前和過去的測量輸出在線重復(fù)求解一個(gè)約束優(yōu)化的問題，即利用滾動(dòng)時(shí)域估計(jì)綜合信息批上多個(gè)時(shí)刻的信息估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)向量。其中心思想是，在每個(gè)當(dāng)前時(shí)刻，信息批上總有最老的測量量被最新的測量量替換掉，然后通過最優(yōu)化信息批的最小平方代價(jià)函數(shù)得到狀態(tài)估計(jì)。

式（14）的滾動(dòng)時(shí)域估計(jì)依賴于計(jì)算得到的當(dāng)前時(shí)刻t狀態(tài)矢量xt的估計(jì)，而對(duì)的估計(jì)則基于yt-N，…，yt，ut-N，…，ut等。其中，N為考慮的信息批的長度，被稱作滾動(dòng)時(shí)域窗口的寬度。此外，規(guī)定滾動(dòng)窗口的起始時(shí)狀態(tài)估計(jì)xt-N的預(yù)測值，有

從而得到最小平方代價(jià)函數(shù)

在當(dāng)前時(shí)刻t，在以下條件下的最小平方代價(jià)函數(shù)可被最優(yōu)化

在初始時(shí)刻，應(yīng)當(dāng)首先初始化狀態(tài)估計(jì)值，設(shè)定初始時(shí)刻的先驗(yàn)估計(jì)值，然后根據(jù)圖1所示的流程繼續(xù)優(yōu)化、傳播和預(yù)測。

根據(jù)對(duì)滾動(dòng)時(shí)域估計(jì)算法原理的介紹，理論上該算法可比擴(kuò)展Kalman濾波獲得更高的導(dǎo)航精度（此結(jié)論已通過實(shí)驗(yàn)仿真結(jié)果得到證明）。但是，在應(yīng)用該算法前，還應(yīng)考慮以下兩個(gè)問題：

1）滾動(dòng)時(shí)域估計(jì)需要考慮到多個(gè)歷元時(shí)刻的狀態(tài)信息，計(jì)算量相對(duì)更大，因此計(jì)算速度會(huì)有差距。為保證計(jì)算效率，本文控制了滾動(dòng)時(shí)域窗口的數(shù)量，在仿真時(shí)分別取值N=3、5、7等，以比較不同窗口大小下的導(dǎo)航精度和計(jì)算效率。

圖1 滾動(dòng)時(shí)域估計(jì)的示意圖Fig.1 Diagram of MHE process

2）應(yīng)當(dāng)認(rèn)識(shí)到，滾動(dòng)時(shí)域估計(jì)不能被應(yīng)用于線性化緊組合導(dǎo)航系統(tǒng)中。這是由于在線性化系統(tǒng)中，滾動(dòng)時(shí)域估計(jì)的工作原理與擴(kuò)展Kalman濾波一致。

3 Newton迭代法

為使?jié)L動(dòng)時(shí)域估計(jì)值快速收斂，應(yīng)采用梯度下降算法。梯度下降算法一般可分為梯度下降法、共軛梯度法和Newton法。

梯度下降算法形式簡單，在一般情況下均可保證收斂，但是收斂速度較慢。

作為二階迭代方法，Newton法需要對(duì)Hessen矩陣及其逆矩陣開展計(jì)算和存儲(chǔ)，其收斂速度快，但收斂的穩(wěn)定性不能保證。

共軛梯度法是介于梯度下降法與Newton法之間的一個(gè)方法，是一個(gè)一階方法。它克服了梯度下降法收斂慢的缺點(diǎn)，又避免了存儲(chǔ)和計(jì)算Newton法所需要的二階導(dǎo)數(shù)信息。但是，其形式比前兩者都要復(fù)雜，對(duì)解算效率會(huì)產(chǎn)生較大的損害。

結(jié)合上面的分析，如果能夠保證Newton迭代方法穩(wěn)定收斂，那么Newton法為最佳的方法。

在使用Newton法之前，首先應(yīng)證明迭代算法估計(jì)誤差的穩(wěn)定性。

在x t-N+1附近進(jìn)行T a y l o r展開，可得

已知α0≤1，于是可以證明Newton迭代法指數(shù)穩(wěn)定。因此，本文采用Newton法作為迭代方法。

應(yīng)用Newton迭代公式，有

綜上，本文使用基于Newton法的多步迭代滾動(dòng)時(shí)域估計(jì)，其步驟如下：

1）設(shè)定起始迭代參數(shù)為

2）當(dāng)m從1迭代到M時(shí)，依次有

4）最終根據(jù)式（26），可解算出結(jié)果

4 驗(yàn)證實(shí)驗(yàn)與仿真分析

慣性數(shù)據(jù)與GPS數(shù)據(jù)采集設(shè)備主要包括：MEMS-IMU、高性能商用型GPS接收機(jī)、實(shí)驗(yàn)車、筆記本電腦等。MEMS是一款基于微機(jī)電的捷聯(lián)慣性導(dǎo)航系統(tǒng)，其主要特點(diǎn)為體積小、質(zhì)量小、集成化、諧振頻率高、響應(yīng)時(shí)間短，由陀螺和加速度計(jì)構(gòu)成。其中，陀螺的敏感度為0.989（″）/脈沖，速度增量為27.5μg×s/脈沖 。圖2為實(shí)驗(yàn)車主要測量裝置示意圖，由于實(shí)時(shí)動(dòng)態(tài)（Real-time Kinematic，RTK）技術(shù)的定位精度可達(dá)厘米級(jí)，故可將RTK數(shù)據(jù)作為參考。

圖2 實(shí)驗(yàn)車示意圖Fig.2 Diagram of experiment vehicle

緊組合導(dǎo)航系統(tǒng)可分為硬件部分和軟件部分。硬件部分由導(dǎo)航計(jì)算機(jī)、GPS接收機(jī)、捷聯(lián)慣導(dǎo)系統(tǒng)組成；軟件部分即導(dǎo)航程序，主要包括捷聯(lián)慣導(dǎo)解算程序、中頻數(shù)據(jù)處理程序、緊組合導(dǎo)航濾波器等，可完成GPS定位信息和SINS數(shù)據(jù)間的數(shù)據(jù)融合。

實(shí)驗(yàn)車在不平坦的路面上行駛，在起始時(shí)刻（t=0）時(shí)車輛開始啟動(dòng)。 汽車行駛于北緯 30°54′、東經(jīng)121°40′，實(shí)驗(yàn)區(qū)域?yàn)橐黄_闊地帶，車輛運(yùn)動(dòng)軌跡如圖3所示。

圖3 實(shí)驗(yàn)車運(yùn)動(dòng)軌跡圖Fig.3 Motion trail diagram of experiment vehicle

最終，通過緊組合導(dǎo)航系統(tǒng)解算求出的位置濾波誤差和速度濾波誤差的對(duì)比圖如圖4、圖5所示，位置誤差均值比對(duì)和速度誤差均值對(duì)比如表1、表2所示。

圖4 濾波算法NMHE3、NMHE5和NMHE7位置誤差比較Fig.4 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in position error

圖5 濾波算法NMHE3、NMHE5和NMHE7速度誤差比較Fig.5 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in velocity error

表1 濾波算法NMHE3、NMHE5和NMHE7位置誤差均值對(duì)比Table 1 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in mean value of position error

表2 濾波算法NMHE3、NMHE5和NMHE7速度誤差均值對(duì)比Table 2 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in mean value of velocity error

首先，為比較不同時(shí)域窗口下的MHE算法對(duì)載體運(yùn)動(dòng)狀態(tài)估計(jì)的精度，分別取窗口數(shù)N=3、5、7，并分別記相應(yīng)的算法為NMHE3、NMHE5、NMHE7。

圖4顯示NMHE算法均快速收斂，位置誤差曲線光滑，波動(dòng)較小；圖5顯示速度誤差均在±1m/s范圍內(nèi)振蕩。由表1和表2可知，NMHE5的位置誤差均值比NMHE3的位置誤差均值提升均超過25%，而NMHE7相較于NMHE5僅提升了不到10%；且NMHE5的水平面上速度誤差均值比NMHE3的水平面上速度誤差均值提高了約23%，而NMHE7相較于NMHE5提高了約14%。

所有計(jì)算都是在PC的Matlab上完成的，PC配置了2.7GHz的Intel Core i7 CPU及16GB的RAM。計(jì)算得到了計(jì)算時(shí)間、導(dǎo)航誤差與滾動(dòng)時(shí)域窗口N之間的折線圖，如圖6所示。

因此，NMHE5相比NMHE3獲得了較大提升，而NMHE7相比NMHE5改善不是很大。隨著時(shí)域窗口N值的增加，計(jì)算效率線性增大。故綜合考慮，設(shè)置滾動(dòng)時(shí)域窗口值N=5，下文中NMHE如無特殊說明即取N=5。

圖6 導(dǎo)航誤差和平均計(jì)算時(shí)間折線圖Fig.6 Line chart of navigation error and average computing time

圖7顯示濾波結(jié)果不再隨著時(shí)間積累而發(fā)散。采用擴(kuò)展 Kalman濾波 (Extended Kalman Filter，EKF）時(shí)，東北天三方向的位置誤差是收斂的，但是天向與北向位置誤差的波動(dòng)較大。而本文提出的Newton迭代MHE算法得到的位置誤差振蕩約為15s，可見它的位置誤差不僅可快速收斂，而且曲線光滑，波動(dòng)較小。由圖8可知，采用EKF算法時(shí)，得到的東向速度誤差在±0.4m/s范圍內(nèi)振蕩，北向、天向速度誤差在±0.2m/s范圍內(nèi)振蕩，波動(dòng)較大，而改進(jìn)的自適應(yīng)EKF算法所得到的速度誤差在更小的范圍內(nèi)振蕩，穩(wěn)定性較好。

圖7 NMHE算法與EKF算法位置誤差比較Fig.7 Comparison between NMHE algorithm and EKF algorithm in position error

圖8 NMHE算法與EKF算法速度誤差比較Fig.8 Comparison between NMHE algorithm andEKF algorithm in velocity error

表3與表4分別對(duì)比了兩種濾波算法在東北天3個(gè)方向的位置誤差均值和速度誤差均值。從數(shù)據(jù)中可以看出，與EKF算法相比，NMHE算法計(jì)算出的位置誤差和速度誤差較小，確保了GPS/SINS緊組合的導(dǎo)航精度。

表3 NMHE算法與EKF算法的位置誤差均值對(duì)比Table 3 Comparison between NMHE algorithm and EKF algorithm in mean value of position error

表4 NMHE算法與EKF算法的速度誤差均值對(duì)比Table 4 Comparison between NMHE algorithm and EKF algorithm in mean value of velocity error

最后，使用滾動(dòng)時(shí)域估計(jì)算法仿真出實(shí)驗(yàn)車的運(yùn)動(dòng)軌跡，該軌跡接近于精確的RTK軌跡。截取其中一段典型的轉(zhuǎn)彎路段軌跡進(jìn)行對(duì)比，對(duì)比結(jié)果如圖9所示。

圖9 RTK軌跡與NMHE5仿真軌跡對(duì)比Fig.9 Comparison between RTK trajectory and NMHE5 simulation trajectory

利用RTK測量的高精度定位數(shù)據(jù)作為參考，模擬實(shí)驗(yàn)車的實(shí)際軌跡，與新算法NMHE5的仿真結(jié)果進(jìn)行對(duì)比，滾動(dòng)時(shí)域估計(jì)結(jié)果與真實(shí)軌跡接近，具備較高的定位精度。

5 結(jié)論

針對(duì)手機(jī)導(dǎo)航幾乎完全依賴GPS的問題，提出了基于Newton迭代MHE算法的GPS/SINS緊組合導(dǎo)航技術(shù)。該技術(shù)對(duì)手機(jī)內(nèi)置慣導(dǎo)系統(tǒng)的精度要求不高，又可有效提升導(dǎo)航精度，更可提升手機(jī)導(dǎo)航在復(fù)雜環(huán)境中的可用性。

相對(duì)于傳統(tǒng)EKF算法，基于Newton迭代MHE算法對(duì)位置誤差的估計(jì)精度提高了50%，對(duì)速度誤差的估計(jì)精度提高了1倍以上。因此，在手機(jī)、車載導(dǎo)航等領(lǐng)域，MHE算法有著廣泛的應(yīng)用前景。

MHE算法的效率和精度互相矛盾，不可兼得，其效率主要取決于滾動(dòng)時(shí)域窗口N的大小。在實(shí)際應(yīng)用過程中，應(yīng)針對(duì)不同場景設(shè)定相應(yīng)的參數(shù)N，如對(duì)高動(dòng)態(tài)載體設(shè)置較大的窗口，對(duì)低動(dòng)態(tài)載體設(shè)置較小的窗口，可以采用自適應(yīng)算法使得窗口大小隨運(yùn)動(dòng)狀態(tài)的變化而改變。