李厚全，劉莫塵，伍志海，李偉剛

（1. 海軍潛艇學(xué)院，青島 266044；2. 海軍航空工程學(xué)院，煙臺 264000；3. 山東農(nóng)業(yè)大學(xué)，泰安 271018）

球面單形平方根無跡粒子濾波在拖曳合成孔徑聲納組合導(dǎo)航中的應(yīng)用

李厚全1，2，劉莫塵3，伍志海1，李偉剛1

（1. 海軍潛艇學(xué)院，青島 266044；2. 海軍航空工程學(xué)院，煙臺 264000；3. 山東農(nóng)業(yè)大學(xué)，泰安 271018）

慣性導(dǎo)航設(shè)備修正回波數(shù)據(jù)作為合成孔徑聲納（SAS）運動補(bǔ)償?shù)囊环N重要方法，其中的關(guān)鍵技術(shù)是研究和利用高精度的濾波算法以及組合導(dǎo)航系統(tǒng)為運動補(bǔ)償提供導(dǎo)航數(shù)據(jù)信息。在標(biāo)準(zhǔn)粒子濾波算法的基礎(chǔ)上進(jìn)行改進(jìn)，提出了一種球面單形平方根無跡粒子濾波算法（SS-SRUPF），并將之應(yīng)用于SINS、多普勒計程儀（DVL）組成的水下組合導(dǎo)航系統(tǒng)，輸出的導(dǎo)航數(shù)據(jù)再傳輸給SAS成像系統(tǒng)。仿真結(jié)果表明，SS-SRUPF算法可將一個合成孔徑時間內(nèi)的組合導(dǎo)航位置均方誤差精度提高到 0.013 m，小于SAS距離向分辨力0.019 m，滿足SAS運動補(bǔ)償?shù)囊蟆?/p>

合成孔徑聲納；組合導(dǎo)航；粒子濾波；運動補(bǔ)償

SAS的運動補(bǔ)償通常可采用基于慣性測量系統(tǒng)的運動補(bǔ)償和基于回波數(shù)據(jù)的運動補(bǔ)償兩種方法。基于慣性測量系統(tǒng)的運動補(bǔ)償?shù)年P(guān)鍵技術(shù)之一是利用高精度水下組合導(dǎo)航方法對安裝在拖體上的 SAS進(jìn)行實時的姿態(tài)測量和運動誤差估計。導(dǎo)航系統(tǒng)測量并給出SAS姿態(tài)和位置，計算給出平均理想直線航跡[1]，以確保運動補(bǔ)償相對此理想航跡進(jìn)行，根據(jù)測量數(shù)據(jù)計算出陣元相位中心的實際位置，然后補(bǔ)償?shù)魧嶋H位置相對理想直線航跡的偏差[2]。水下拖曳式合成孔徑通常包含三個子系統(tǒng)：拖曳子系統(tǒng)、信號處理子系統(tǒng)、顯控子系統(tǒng)。其中，拖曳子系統(tǒng)包括絞車、沉魚和拖體。拖體內(nèi)包含聲納子系統(tǒng)、導(dǎo)航子系統(tǒng)和數(shù)據(jù)采集與處理子系統(tǒng)，拖體內(nèi)的配置見圖1。SAS導(dǎo)航系統(tǒng)配置包括SINS、聲學(xué)多普勒計程儀和深度計。正是基于上述SAS導(dǎo)航子系統(tǒng)的導(dǎo)航方案，研究了由SINS、聲學(xué)多普勒計程儀以及高精度石英深度計等設(shè)備組成的的姿態(tài)和位移測量組合導(dǎo)航系統(tǒng)。

當(dāng)導(dǎo)航姿態(tài)誤差角較大時，組合導(dǎo)航系統(tǒng)方程采用非線性方程更為精確，因此采用四元數(shù)法建立大姿態(tài)誤差角下SINS/DVL組合導(dǎo)航系統(tǒng)方程。系統(tǒng)方程的濾波算法即成為一高維非線性系統(tǒng)濾波問題。

粒子濾波算法（PF）是近些年備受關(guān)注的一種處理非線性問題的貝葉斯?fàn)顟B(tài)估計算法，在導(dǎo)航系統(tǒng)中已有諸多報道[3-7]。它通過帶有歸一化權(quán)重的粒子集來近似表示后驗概率密度[8-9]，采用擴(kuò)展卡爾曼濾波（EKF）、無跡卡爾曼濾波（UKF）等算法結(jié)合最新觀測值產(chǎn)生建議密度分布[10-11]的方法解決粒子退化問題都存在弊端，如EKF只能精確到一階近似，UKF無跡粒子濾波（UPF）對高維濾波問題計算量偏大。

從降低計算量、提高系統(tǒng)估計精度的角度出發(fā)，將UPF算法進(jìn)行改進(jìn)，提出了一種球面單形平方根無跡粒子濾波算法（SS-SRUPF），并應(yīng)用于大姿態(tài)誤差角情況下基于四元數(shù)法的SINS/DVL組合導(dǎo)航系統(tǒng)，仿真結(jié)果表明，提出的SS-SRUPF算法相比常規(guī)UPF算法大大降低了計算量，提高了系統(tǒng)估計精度。

1 SAS運動估計的導(dǎo)航系統(tǒng)方程

SAS所用拖體內(nèi)的配置[12-13]如圖1所示，拖體長約4.4 m，線列陣長約2 m。導(dǎo)航方案采用SINS、DVL和深度計組合。對于SAS影響最大的主要是水平面內(nèi)信息（側(cè)擺和偏航角）的影響，橫滾角對InSAS（干涉合成孔徑）影響較大。因此，水平面內(nèi)的導(dǎo)航信息對SAS可采用組合導(dǎo)航模式，深度方向采用深度計測量。

圖1 導(dǎo)航系統(tǒng)在SAS拖體系統(tǒng)中的配置Fig.1 Navigation system in the towfish of SAS

針對上述設(shè)計的SAS導(dǎo)航方案，組合導(dǎo)航系統(tǒng)采用四元數(shù)法建立SINS/DVL系統(tǒng)誤差方程[14]，系統(tǒng)狀態(tài)方程和觀測方程建立如下：

上述系統(tǒng)狀態(tài)方程中，狀態(tài)變量為：

式中，δL、δλ、δh分別為緯度、經(jīng)度、高度誤差；δVx、δVy、δVz分別為相應(yīng)的速度誤差；δ q0、δ q1、δ q2、δ q3為計算四元數(shù)與真實四元數(shù)的差；εx、εy、εz為SINS的系統(tǒng)隨機(jī)誤差；δVd是DVL采用底跟蹤模式時的觀測誤差，主要是指速度偏移誤差，δK為刻度系數(shù)誤差，δΔ偏流角誤差。

將SINS計算出的系統(tǒng)速度誤差與DVL測速誤差之差作為系統(tǒng)的量測值，得系統(tǒng)的量測方程為：

2 SS-SRUPF算法

一般離散非線性系統(tǒng)可表述如下：

式中， xk為狀態(tài)向量；vk為系統(tǒng)噪聲，其方差為 Pv；nk為量測噪聲，其方差為 Pn。

對于上述系統(tǒng)，提出了球面單形平方根無跡粒子濾波算法（SS-SRUPF），該算法選用SS-SRUKF作為粒子濾波算法的建議密度，其參加計算的sigma點約為 UPF算法中 UKF所用 sigma點數(shù)量的一半，且SR-UKF本身也比UKF計算效率也有較大的提升。

SS-SRUPF詳細(xì)算法如下：

①初始化： k=0

從先驗分布 p(x0)中采樣粒子集，i =1,…, Ns。并設(shè)為如下初始條件：

② 對于 k-1測量時刻計算 sigma點，選取權(quán)值W0，且滿足 0 ≤ W0≤ 1。確定權(quán)值序列，滿足：Wl=(1 - W0)(n +1)， l = 1,… ,n+1。

初始化sigma點向量序列：

迭代計算sigma點向量，擴(kuò)展維數(shù) j =2,… ,n：

式中，

SS-SRUKF時間更新方程：

SS-SRUKF量測更新方程如下：

式中，Pv為過程噪聲向量， Pn為測量噪聲向量。

③ 以SS-SRUKF得到的狀態(tài)估計和方差為建議密度分布，從中采樣 Ns個粒子：

④ 重采樣：

⑤ 狀態(tài)輸出和方差：

上述過程為SS-SRUPF的完整算法，采樣得到的每個粒子傳遞均值和方差信息，采用的算法為 n+ 2球面單形sigma點的UKF算法。為驗證SS-SRUPF算法的優(yōu)越性，第3部分進(jìn)行了仿真算法驗證。

3 SS-SRUPF仿真與分析

SAS由艦船牽引在水下運動，其運動規(guī)律用速度方程和姿態(tài)角表達(dá)如下[7]：

其中，VE、VN、VU分別為東北天坐標(biāo)系下的速度分量；V為SAS所在載體軸向速度設(shè)定為3 m/s；θ、γ、φ為SAS載體縱傾角、橫滾角和方位角；ωθ、ωγ、ωφ為姿態(tài)角角頻率，其相應(yīng)周期分別設(shè)為12 s、6 s、10 s；Aθ、Aγ、Aφ為姿態(tài)角搖擺幅度，分別設(shè)為6°、4°、3°；φθ、φλ、φφ分別為姿態(tài)角初始相位，均設(shè)為45°，φ0為SAS初始航向設(shè)為45°。SINS陀螺常值漂移0.1 (°)/h，加速度計常值漂移± 500 μg；DVL測速誤差垂直測速誤差0.6%，水平速度誤差0.2%。初始經(jīng)度120°，緯度60°。

為驗證所提出算法的性能，依據(jù)SAS水下試驗方案，在同一仿真條件下，分別采用SS-SRUPF以及PF、UPF三種算法對SINS/DVL導(dǎo)航系統(tǒng)方程進(jìn)行濾波處理。在CPU為AMD 4核 2.0 GHz，內(nèi)存為2.0GB的PC機(jī)上，分別對三種算法進(jìn)行50次蒙特卡洛仿真，選取粒子數(shù)目1200。

仿真結(jié)果如圖2、圖3、圖4以及表1所示。由圖中結(jié)果可以看出，SS-SRUPF算法濾波精度高于標(biāo)準(zhǔn)粒子濾波算法（PF）和UPF算法，由此看見粒子濾波算法中建議密度的合理選擇對提升粒子濾波的性能有著至關(guān)重要的作用，仿真結(jié)果與前面的理論分析完好的吻合。SS-SRUPF與UPF相比，濾波精度提高，且計算效率較UPF算法提高一倍。

圖2 位置誤差Fig.2 Position error

圖3 速度誤差Fig.3 Velocity error

圖4 方向誤差角Fig.4 Attitude angle error

表1 PF、UPF以及SS-SRUPF估計精度Tab.1 Precision comparing among PF, UPF and SS-SRUPF algorithms

由表1水平面內(nèi)位置估計精度可以計算得到PF、 UPF以及SS-SRUPF的航路估計（航速3 m/s）誤差精度分別達(dá)到航程的0.19%、0.14%、0.13%。此外，偏流角的誤差估計精度，三種算法都在0.1°以內(nèi)。

對于系統(tǒng)參數(shù)如表2所示的SAS成像系統(tǒng)，經(jīng)計算其距離向分辨力為：

其中，c表示水聲傳播速度， Br為信號帶寬。

一個合成孔徑時間為：

其中， r0為成像距離，λ為波長，d為聲納基陣寬度。

表2 系統(tǒng)仿真參數(shù)Tab.2 System and simulation parameters

4 結(jié) 論

針對非線性導(dǎo)航系統(tǒng)誤差方程的濾波為一高維非線性濾波問題，改進(jìn)提出的采用球面單形平方根無跡粒子濾波算法，相比UPF算法和PF算法能夠有效提高組合導(dǎo)航系統(tǒng)的估計精度，計算效率也比UPF大大提高，其濾波精度完全滿足合成孔徑聲納運動補(bǔ)償對導(dǎo)航數(shù)據(jù)的要求，為合成孔徑聲納運動補(bǔ)償提供了數(shù)據(jù)和技術(shù)支持。

（References）：

[1] 張遠(yuǎn)彪，朱三文. 多子陣合成孔徑聲納空變運動補(bǔ)償[J].計算機(jī)應(yīng)用，2014，34(S1)：352-355.

ZHANG Yuan-biao, ZHU San-wen. Space-variant motion compensation for multi-element synthetic aperture sonar[J]. Journal of Computer Applications, 2014, 34(S1): 352-355.

[2] Hayes M P, Gough P T. Synthetic aperture sonar: A review of current status[J]. IEEE J. Oceanic Eng., 2009, 34(3): 207-224.

[3] Ali J, Mirza M R U B. Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution[J]. Nonlinear Dynamics, 2010, 61(3): 491-502.

[4] 熊劍，郭杭，熊智，等. GPS/INS 組合導(dǎo)航系統(tǒng)中的高斯粒子濾波混和算法[J]. 中國慣性技術(shù)學(xué)報，2012，20(2)：596-600.

XIONG Jian, GUO Hang, XIONG Zhi, et al. Gaussian particle filtering hybrid algorithm for GPS/INS integrated navigation system[J]. Journal of Chinese Inertial Technology, 2012, 20(2): 596-600.

[5] Scharcanski J, Oliveira A B, Cavalcanti P G, Yari Y. A particle filtering approach for vehicular tracking adaptive to occlusions[J]. IEEE Transactions on Vehicular Technology, 2011, 60(2): 381-389.

[6] Jacques G, Tashfeen K, Umar I, et al. Enhanced MEMSIMU/odometer/GPS integration using mixture particle filter[J]. GPS Solutions, 2011, 15(3): 239-252.

[7] 吳鑫輝，黃高明，高俊. 未知噪聲統(tǒng)計下多模型概率假設(shè)密度粒子濾波算法[J]. 控制與決策，2014，29(2)：1-6.

WU Xin-hui, HUANG Gao-ming, GAO Jun. Multiplemodel probability hypothesis density filter for multi-target tracking without the statistics of noise parameters[J]. Control and Decision, 2014, 29(2): 1-6.

[8] Stordal A S, Karlsen H A, N?vdal G, et AL. Bridging the ensemble Kalman filter and particle filters: the adaptive Gaussian mixture filter[J]. Computational Geosciences, 2011, 15(2): 293-305.

[9] Kim J, Vaddi S S, Menon P K, Ohlmeyer E J. Comparison between nonlinear filtering techniques for spiraling ballistic missile state estimation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(1): 313- 328.

[10] Stordal A S, Karlsen H A, N?vdal G, et al. Bridging the ensemble Kalman filter and particle filters: the adaptive Gaussian mixture filter[J]. Computational Geosciences, 2011, 15(2): 293-305.

[11] Julier S J. The spherical simplex unscented transformation [C]//Proc Amer Control Conf, 2003, vol.3: 2430-2430.

[12] 張森，唐勁松，陳鳴，等. 干涉合成孔徑聲納海試樣機(jī)研制與試驗[J]. 聲學(xué)技術(shù)，2012，31(2): 167-173.

ZHANG Sen, TANG Jing-song, CHEN Ming, et al. Development and sea trial of interferometric synthetic aperture sonar[J]. Technical Acoustics, 2012, 31(2): 167-173.

[13] Zhang Sen, Tang Jingsong. Image autocoregistration and interferogram estimation using extended COMET-EXIP method[J]. IEEE Transactions on Geoscience and Remote Sensing, 2010, 48(12): 4204-4218.

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

LI Hou-quan, LI Heng, TANG Jin-song et al. The research of Unscented particle filter used in the navigation of SAS[J]. Journal of Naval University of Engineering, 2010, 22(3): 26-31.

Spherical simplex square-root unscented particle filter used in integrated navigation system of synthetic aperture sonar

LI Hou-quan1,2, LIU Mo-chen3, WU Zhi-hai1, LI Wei-gang1
(1. Navy Submarine Academy, Qingdao 266044, China; 2. Naval Aeronautical Engineering Institute, Yantai 264000, China; 3. Shandong Agricultural University, Taian 271018, China)

In motion compensation of synthetic aperture sonar(SAS), one of important methods is using inertial navigation system to correct the echo data. In this paper, a high-precision filter algorithm and integrated navigation system were studied and utilized for providing navigation data to realize the motion compensation. A new algorithm, i.e. spherical simplex square root unscented particle filter(SS-SRUPF), is proposed and applied into the integrated SINS/DVL navigation system. The navigation data from the integrated system is then transferred to the SAS imaging system. Simulation results indicate that the integrated navigation precision in a synthetic aperture time is increased to 0.013 m(RMS) which is smaller than the SAS range differentiability of 0.019 m, and the computation efficiency is more than twice the UPF’s, meeting the motion compensation requirement of SAS.

synthetic aperture sonar; inertial navigation; particle filter; motion compensation

李厚全（1981—），男，博士，講師，從事組合導(dǎo)航、水聲信號處理研究。E-mail： lhqqtxy@163.com

1005-6734(2014)04-0531-05

10.13695/j.cnki.12-1222/o3.2014.04.020

U675.7

A

2014-04-25；

2014-07-24

國家高技術(shù)研究發(fā)展計劃（863計劃）（2007AA091101）；中國博士后基金資助（2014M552658）