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

（上海交通大學航空航天學院,上海201812）

0 引言

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

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

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

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

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

1.1 狀態(tài)方程

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

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

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

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

式（3）中，β為誤差相關時間，ωtu、ωfu為驅動噪聲。

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

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

1.2 量測方程

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

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

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

因此，

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

2 滾動時域估計

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

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

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

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

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

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

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

根據(jù)對滾動時域估計算法原理的介紹，理論上該算法可比擴展Kalman濾波獲得更高的導航精度（此結論已通過實驗仿真結果得到證明）。但是，在應用該算法前，還應考慮以下兩個問題：

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

圖1 滾動時域估計的示意圖Fig.1 Diagram of MHE process

2）應當認識到，滾動時域估計不能被應用于線性化緊組合導航系統(tǒng)中。這是由于在線性化系統(tǒng)中，滾動時域估計的工作原理與擴展Kalman濾波一致。

3 Newton迭代法

為使?jié)L動時域估計值快速收斂，應采用梯度下降算法。梯度下降算法一般可分為梯度下降法、共軛梯度法和Newton法。

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

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

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

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

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

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

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

應用Newton迭代公式，有

綜上，本文使用基于Newton法的多步迭代滾動時域估計，其步驟如下：

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

2）當m從1迭代到M時，依次有

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

4 驗證實驗與仿真分析

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

圖2 實驗車示意圖Fig.2 Diagram of experiment vehicle

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

實驗車在不平坦的路面上行駛，在起始時刻（t=0）時車輛開始啟動。 汽車行駛于北緯 30°54′、東經121°40′，實驗區(qū)域為一片開闊地帶，車輛運動軌跡如圖3所示。

圖3 實驗車運動軌跡圖Fig.3 Motion trail diagram of experiment vehicle

最終，通過緊組合導航系統(tǒng)解算求出的位置濾波誤差和速度濾波誤差的對比圖如圖4、圖5所示，位置誤差均值比對和速度誤差均值對比如表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位置誤差均值對比Table 1 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in mean value of position error

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

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

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

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

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

圖6 導航誤差和平均計算時間折線圖Fig.6 Line chart of navigation error and average computing time

圖7顯示濾波結果不再隨著時間積累而發(fā)散。采用擴展 Kalman濾波 (Extended Kalman Filter，EKF）時，東北天三方向的位置誤差是收斂的，但是天向與北向位置誤差的波動較大。而本文提出的Newton迭代MHE算法得到的位置誤差振蕩約為15s，可見它的位置誤差不僅可快速收斂，而且曲線光滑，波動較小。由圖8可知，采用EKF算法時，得到的東向速度誤差在±0.4m/s范圍內振蕩，北向、天向速度誤差在±0.2m/s范圍內振蕩，波動較大，而改進的自適應EKF算法所得到的速度誤差在更小的范圍內振蕩，穩(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分別對比了兩種濾波算法在東北天3個方向的位置誤差均值和速度誤差均值。從數(shù)據(jù)中可以看出，與EKF算法相比，NMHE算法計算出的位置誤差和速度誤差較小，確保了GPS/SINS緊組合的導航精度。

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

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

最后，使用滾動時域估計算法仿真出實驗車的運動軌跡，該軌跡接近于精確的RTK軌跡。截取其中一段典型的轉彎路段軌跡進行對比，對比結果如圖9所示。

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

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

5 結論

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

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

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