李厚全，李偉剛，孫榮光，劉海光，丁　浩

（1.海軍潛艇學(xué)院，山東　青島　266042；2.海軍航空工程學(xué)院，山東　煙臺(tái)　264001）

SS-IUKF算法及其在成像聲納組合導(dǎo)航中的應(yīng)用*

李厚全1，2，李偉剛1，孫榮光1，劉海光1，丁浩1

（1.海軍潛艇學(xué)院，山東青島266042；2.海軍航空工程學(xué)院，山東煙臺(tái)264001）

摘要：利用成像聲納對(duì)海底進(jìn)行探測(cè)時(shí)需要高精度的導(dǎo)航信息。針對(duì)導(dǎo)航高維濾波問題，文中在球面單形sigma點(diǎn)變換的基礎(chǔ)上，將其與迭代UKF相結(jié)合，提出了球面單形迭代無(wú)跡卡爾曼濾波（SS-IUKF）算法，并應(yīng)用于捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）/多普勒流速剖面儀（ADCP）組合導(dǎo)航系統(tǒng)。仿真結(jié)果表明，SS-IUKF算法相比UKF、SR-UKF、EKF算法提高了濾波精度和計(jì)算速度，具有一定的工程應(yīng)用價(jià)值。

關(guān)鍵詞：無(wú)跡卡爾曼濾波，組合導(dǎo)航，非線性濾波，水下航行器

0　引言

對(duì)于非線性系統(tǒng)來說，通過改進(jìn)測(cè)量更新方法可以達(dá)到提高非線性近似精度的目的。易于工程實(shí)現(xiàn)的方法是使用迭代測(cè)量更新的方法，在EKF上已經(jīng)得到了很多成功應(yīng)用［1-4］。但EKF只能精確到一階近似，甚至有時(shí)雅克比矩陣無(wú)法求解等問題，雖然粒子濾波、無(wú)跡卡爾曼濾波（UKF）算法精度有很大提高，但計(jì)算量較大，尤其是對(duì)于高維導(dǎo)航模型濾波問題來說實(shí)時(shí)性難以滿足要求。針對(duì)以上存在的問題，本文將球面單形sigma點(diǎn)變換和迭代UKF相結(jié)合，使用狀態(tài)的估計(jì)值代替預(yù)測(cè)值重新進(jìn)行UT變換，得到濾波參數(shù)并再次利用觀測(cè)值進(jìn)一步改善狀態(tài)估計(jì)。球面單形sigma點(diǎn)變換只需要n+2個(gè)點(diǎn)［5-6］，相比傳統(tǒng)UT變換大大減小了計(jì)算量，同時(shí)迭代算法的引入提高了狀態(tài)估計(jì)的精度。

1　導(dǎo)航系統(tǒng)方程

在借鑒以往研究成果的基礎(chǔ)上，文中導(dǎo)航系統(tǒng)誤差模型采用四元數(shù)法建立捷聯(lián)慣性導(dǎo)航系統(tǒng)SINS/多普勒測(cè)速儀DVL誤差模型［7-8］，建立系統(tǒng)狀態(tài)方程和觀測(cè)方程如下：

狀態(tài)方程中狀態(tài)變量為：

將SINS系統(tǒng)速度誤差與DVL測(cè)速誤差之差作為系統(tǒng)量測(cè)值如下：

2　SS-IUKF算法

對(duì)于式（3）和式（4）確定的非線性系統(tǒng)，經(jīng)過改進(jìn)后的球面單形迭代UKF（SS-IUKF）的具體算法如下：

狀態(tài)變量和方差初始化：

對(duì)于k-1測(cè)量時(shí)刻計(jì)算sigma點(diǎn)：

（a）選取權(quán)值W0，且滿足0≤W0≤1。

（c）初始化sigma點(diǎn)向量序列：

建立SS-IUKF的時(shí)間更新方程如下：

SS-IUKF的量測(cè)更新方程如下：

對(duì)于，t=0，…，M，

其中，式（8）中SSUT表示對(duì)參數(shù)進(jìn)行計(jì)算球面單形sigma點(diǎn)；M表示迭代次數(shù)。則SS-IUKF的濾波估計(jì)輸出為：

以上是SS-IUKF算法的實(shí)現(xiàn)。SS-IUKF算法，僅需n+2點(diǎn)的SSUT，相比傳統(tǒng)的2n+1點(diǎn)UT方法，減少了近一半的運(yùn)算量。在測(cè)量更新階段，SS-IUKF相比UKF算法增加了M次迭代運(yùn)算［9］（M一般取值較?。?，迭代計(jì)算所產(chǎn)生的計(jì)算量相比sigma點(diǎn)計(jì)算節(jié)省的計(jì)算量來說可以忽略不計(jì)。綜上，組合導(dǎo)航系統(tǒng)采用SS-IUKF算法濾波比UKF算法節(jié)省近一半的計(jì)算量。

3　算法仿真與分析

對(duì)于SINS/DVL導(dǎo)航系統(tǒng)，水平面內(nèi)誤差不斷增大的內(nèi)在特性是由于傳感器誤差信息的積累（包括SINS隨機(jī)游走誤差）引起的。初始仿真參數(shù)中，SINS陀螺常值漂移0.1°/h，加速度計(jì)常值漂移±500 μg；ADCP測(cè)速誤差水平速度誤差0.2%，垂直測(cè)速誤差0.6%。初始緯度60°，經(jīng)度120°。100次蒙特卡洛仿真計(jì)算的結(jié)果如下頁(yè)圖1～圖4所示，分別采用EKF、UKF、SR-UKF以及SS-IUKF 4種算法。

從仿真結(jié)果可以看出，EKF算法的結(jié)果與采用線性模型的卡爾曼濾波算法精度相當(dāng)。UKF及其改進(jìn)算法的精度均比EKF估計(jì)精度有所提高，SS-IUKF的精度相比UKF以及SR-UKF的精度也略有提高。因此，仿真算法精度按從高至低的排序依 次 是SS-IUKF＞SR-UKF＞UKF＞EKF。SS-IUKF、SR-UKF、UKF以及EKF雖然具有相同的計(jì)算復(fù)雜度O（n3），但計(jì)算效率卻相差很大，SS-IUKF的計(jì)算量大約為UKF的50%左右。經(jīng)仿真計(jì)算，4種算法的計(jì)算運(yùn)行時(shí)間量依次為UKF＞SR-UKF＞EKF＞SS-IUKF。因此，文中改進(jìn)提出的SS-IUKF算法不僅可以提高濾波精度，同時(shí)減小了計(jì)算量。

圖1　位置誤差

圖2　姿態(tài)誤差角

圖3　陀螺漂移誤差

4　結(jié)論

針對(duì)水下航行器SINS/DVL組合導(dǎo)航系統(tǒng)中采用四元數(shù)法建立的非線性導(dǎo)航模型，文中提出了SS-IUKF非線性濾波算法并應(yīng)用于上述模型。仿真結(jié)果有效地證明SS-IUKF用于組合導(dǎo)航系統(tǒng)的非線性誤差模型，相比UKF、平方根UKF（SR-UKF）兩種算法不僅提高了估計(jì)精度，還大大降低了計(jì)算量。對(duì)于組合導(dǎo)航系統(tǒng)這種高維濾波問題具有一定的應(yīng)用價(jià)值。

圖4　多普勒誤差

參考文獻(xiàn)：

［1］張麗，趙春霞.一種基于迭代EKF的FastSLAM算法［J］.山東大學(xué)學(xué)報(bào)：工學(xué)版，2012，42（4）：41-47.

［2］石波，盧秀山，陳允芳.應(yīng)用EKF平滑算法提高GPS/INS定位定姿精度［J］.測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，2012，29（5）：334-338.

［3］BELLAIRE R L，KAMEN E W，ZABIN S M.A new nonlinear iterated filter with applications to target tracking［C］//Proc of the SPIE，Atlantic City，1995：240-251.

［4］KERR T H.Streamlining measurement iteration for EKF target tracking［J］.IEEE Trans Aerospace and Electronic Systems，1991，27（2）：408-421.

［5］JULIER S J.The spherical simplex unscented transformation ［C］//Proc Amer Control Conf，2003，3：2430-2435.

［6］JULIER S J，UHLMANN J K.Reduced sigma point filter for the propagation of means and covariances through nonlinear transformations［C］//Proc of American Control Conf.Jefferson City，2002：887-892.

［7］李厚全，李恒，唐勁松，等.Unscented粒子濾波算法在合成孔徑聲納組合導(dǎo)航中的應(yīng)用研究［J］.海軍工程大學(xué)學(xué)報(bào)，2010，22（3）：26-31：

［8］曹娟娟，房建成，盛蔚，等.低成本多傳感器組合導(dǎo)航系統(tǒng)在小型無(wú)人機(jī)自主飛行中的研究和應(yīng)用［J］.航空學(xué)報(bào)，2009，30（10）：1923-1929.

［9］KERR T H.Streamlining measurement iteration for EKF target tracking［J］.IEEE Trans Aerospace and Electronic Systems，1991，27（2）：408-421.

［10］姬曉琴，高曉穎.低軌衛(wèi)星緊組合導(dǎo)航UKF方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2012，44（7）：135-138.

中圖分類號(hào)：U675.7

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1002-0640（2016）04-0154-03

收稿日期：2015-03-17修回日期：2015-05-07

*基金項(xiàng)目：博士后基金資助（2014M552658）

作者簡(jiǎn)介：李厚全（1981-），男，山東費(fèi)縣人，博士后，講師。研究方向：水聲工程，武器系統(tǒng)仿真。

Research on SS-IUKF Algorithm and Inertial Navigation System Underwater used in Imaging Sonar

LI Hou-quan1，2，LI Wei-gang1，SUN Rong-guang1，LIU Hai-guang1，DING Hao1
（1.Navy Submarine Academy，Qingdao 266042，China；2.Naval Aeronautical Engineering Institute，Yantai 264001，China）

Abstract：Inertial navigation system with high precision is needed when seabed is to be imaged by UUV.This paper proposes a new algorithm named the spherical simplex square root unscented Kalman filter（SS-IUKF）base on the UKF arithmetic and sigma transformation which is applied to the combinational navigation system based on SINS and ADCP.The result of simulation indicates that the new algorithm can improve the navigation precision and enhance the computation efficiency.

Key words：unscented kalman filter（SS-IUKF），combinational navigation,nonlinear filter, underwater vehicle