徐曉蘇，董 亞，童金武，代 維

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

基于5階球面最簡相徑的改進(jìn)型容積卡爾曼濾波在SINS/DVL組合導(dǎo)航中的應(yīng)用

徐曉蘇1,2，董 亞1,2，童金武1,2，代 維1,2

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

為提高水下SINS/DVL組合導(dǎo)航系統(tǒng)的精度，建立了捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)的非線性誤差模型，并建立多普勒測速儀的誤差方程，以SINS為主導(dǎo)航設(shè)備建立SINS/DVL組合導(dǎo)航系統(tǒng)模型。設(shè)計了5階球面最簡相徑容積卡爾曼濾波器，采用了球面最簡相徑采樣規(guī)則改進(jìn)容積卡爾曼濾波，并應(yīng)用于SINS/DVL組合導(dǎo)航系統(tǒng)中。通過數(shù)學(xué)平臺仿真驗證了5階球面最簡相徑容積卡爾曼濾波方法有效性，仿真結(jié)果表明：該方法能夠有效提高SINS/DVL組合導(dǎo)航系統(tǒng)的精度，且穩(wěn)定性好。

組合導(dǎo)航；非線性系統(tǒng)；球面最簡相徑；容積卡爾曼濾波

水下自主航行器（AUV）無論在軍用還是民用領(lǐng)域都有著廣泛的應(yīng)用前景[1]。單一的慣性導(dǎo)航系統(tǒng)（INS）雖然具有強(qiáng)自主性，強(qiáng)隱蔽性等優(yōu)點，但其系統(tǒng)誤差會隨時間積累，長時間工作會導(dǎo)致其導(dǎo)航精度下降而無法滿足導(dǎo)航精度要求。組合導(dǎo)航技術(shù)是AUV研究的一個重要分支已經(jīng)越來越被重視。SINS/DVL組合導(dǎo)航系統(tǒng)利用DVL速度信息對SINS導(dǎo)航誤差進(jìn)行抑制，能夠獲得較高的導(dǎo)航精度，因此SINS/DVL組合導(dǎo)航系統(tǒng)常被應(yīng)用于水下AUV中[2]。

SINS/DVL組合導(dǎo)航系統(tǒng)一般采用線性卡爾曼濾波（KF）、擴(kuò)展卡爾曼濾波（EKF）、容積卡爾曼濾波（CKF）[2-3]。在很多環(huán)境中，例如在姿態(tài)失準(zhǔn)角較大時，經(jīng)典的線性誤差模型和標(biāo)準(zhǔn)卡爾曼濾波方法的使用具有局限性[4-5]，線性卡爾曼濾波技術(shù)在處理線性系統(tǒng)時能保證精度，但是當(dāng)系統(tǒng)的數(shù)學(xué)模型是非線性模型時，線性卡爾曼濾波技術(shù)的精度無法滿足。EKF只采用其非線性函數(shù)泰勒級數(shù)展開的第一項，忽略其他高階項，從而達(dá)到函數(shù)線性近似的目的，但是這樣會引入高階項帶來的誤差，需要計算雅各比矩陣，精度受到了很大限制[6,8]。CKF采用的是卡爾曼濾波結(jié)構(gòu)的高斯濾波方程，根據(jù)容積采樣規(guī)則解決高斯濾波中的多維積分問題，但5階標(biāo)準(zhǔn)CKF會導(dǎo)致計算量過大，且當(dāng)狀態(tài)變量維數(shù)增加時其魯棒性不是很好[7,12]。

本研究針對以上問題，采用球面最簡相徑（SSR）采樣規(guī)則改進(jìn)標(biāo)準(zhǔn)的CKF算法，并選用5階改進(jìn)的CKF，進(jìn)一步提高了組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。將標(biāo)準(zhǔn)CKF和改進(jìn)后的CKF濾波后的導(dǎo)航精度進(jìn)行仿真比較，結(jié)果表明，改進(jìn)后的CKF比標(biāo)準(zhǔn)CKF濾波效果更好，系統(tǒng)的導(dǎo)航精度更高[10-11]。

1 SINS/DVL組合導(dǎo)航系統(tǒng)模型

1.1 SINS誤差數(shù)學(xué)模型

導(dǎo)航坐標(biāo)系(n系)選用東-北-天地理坐標(biāo)系，載體坐標(biāo)系為b系，計算導(dǎo)航坐標(biāo)系記為n′系。真實姿態(tài)角φ=[φE,φN,φU]，真實速度，真實地理坐標(biāo)P=[L,λ,H]。

1.2 DVL誤差數(shù)學(xué)模型

多普勒測速儀(DVL)是根據(jù)多普勒效應(yīng)，利用超聲換能器發(fā)射的超聲波來測量載體速度的儀器[5]。DVL的測速誤差主要包含標(biāo)度因數(shù)誤差δKc、偏流角誤差δ?和隨機(jī)測量誤差δVDVL，考慮到偏流角誤差對DVL測速誤差影響較小，不考慮偏流角誤差，可得速度方程為：

式中：βV、βK分別表示DVL隨機(jī)測量誤差和刻度系數(shù)誤差的相關(guān)時間；wV、wK為其對應(yīng)的零均值高斯白噪聲。

1.3 組合導(dǎo)航濾波模型

由于慣導(dǎo)系統(tǒng)天向通道不穩(wěn)定，且DVL對天向的測速誤差較大忽略高度通道的狀態(tài)量[6]，對于SINS系統(tǒng)，選取12維狀態(tài)變量為：

對于DVL選取標(biāo)度因數(shù)誤差和隨機(jī)常值測量誤差作為狀態(tài)量，即：。將系統(tǒng)的非線性狀態(tài)方程簡記為：

式中：w(t)為系統(tǒng)的過程噪聲且w( t)～N[0,Q( t)δ(t-τ)]，Q(t)為系統(tǒng)過程噪聲向量的方差強(qiáng)度矩陣。

選取系統(tǒng)的觀測量為導(dǎo)航坐標(biāo)系下捷聯(lián)解算速度和DVL的速度的差值，并且由于慣導(dǎo)系統(tǒng)天向通道不穩(wěn)定，且DVL對天向的測速誤差較大，本文只考慮水平通道，即只取速度的前兩維。

式中：Vn為導(dǎo)航坐標(biāo)系下速度的真實值，δKc為DVL的刻度系數(shù)誤差，δVDVL為DVL的隨機(jī)測量誤差。因此量測方程為：

將此量測方程簡記為：

式中：u(t)為系統(tǒng)的觀測噪聲，且u(t)～N[0,R(t)δ(t-τ)]，R(t)為觀測向量的方差強(qiáng)度矩陣。

以采樣周期Ts作為濾波周期，并以Ts為步長，對系統(tǒng)的狀態(tài)方程和量測方程進(jìn)行離散化，得到系統(tǒng)的非線性濾波模型，并簡記為：

式中：wk為隨機(jī)系統(tǒng)過程噪聲并且wk～N(0,Qk)，Qk為系統(tǒng)過程噪聲序列的方差陣；uk為系統(tǒng)的隨機(jī)觀測噪聲并且uk～N (0,Rk)，Rk為系統(tǒng)的觀測噪聲序列的方差陣。

2 五階SSRCKF濾波算法

經(jīng)典的容積卡爾曼濾波（CKF）將積分式用權(quán)值與相應(yīng)采樣點函數(shù)值的乘積來近似，避免了計算機(jī)進(jìn)行高維積分運算，可顯著減少計算機(jī)的計算量，提高濾波效率。對于精度要求不高的系統(tǒng)，標(biāo)準(zhǔn)3階CKF可以滿足，且采樣點數(shù)量較少，濾波效率較高，但如果要求更高階的系統(tǒng)精度，需要5階CKF或更高階的濾波器，此時采樣點的數(shù)量會隨之增加，這就導(dǎo)致計算量增加，濾波效率變低。此外文獻(xiàn)[8]指出，當(dāng)系統(tǒng)的狀態(tài)量維度較高時，標(biāo)準(zhǔn)5階CKF的部分采樣點權(quán)值會變成負(fù)值，這種情況可能導(dǎo)致濾波器發(fā)散，影響濾波效果。為解決上述問題，提高濾波的精度和效率，本文在經(jīng)典CKF的基礎(chǔ)上，采用SSR容積采樣規(guī)則，對標(biāo)準(zhǔn)CKF進(jìn)行改進(jìn)。

2.1 SSR容積采樣規(guī)則

高斯濾波積分可歸納成：

采用SSR容積采樣規(guī)則，求解球面積分S( R)。對于擁有n維狀態(tài)量的系統(tǒng)，求解過程如下：取一組n維向

將向量ai的中點投影到球面Un得到以下序列：

文獻(xiàn)[8]指出，標(biāo)準(zhǔn)5階CKF在系統(tǒng)維度n＞4時會出現(xiàn)權(quán)值為負(fù)值的現(xiàn)象，這將導(dǎo)致濾波器發(fā)散。相比較而言，雖然通過SSR采樣規(guī)則改進(jìn)的5階SSRCKF方法在n＞7時權(quán)值也可能為負(fù)值，但當(dāng)n→∞時，上述權(quán)值點將趨于零，因此5階SSRCKF具有較強(qiáng)的魯棒性。本文系統(tǒng)的狀態(tài)變量選用16維，表1[8]給出了當(dāng)n=16時，3階CKF、5階CKF、3階SSRCKF和5階SSRCKF四種算法的采樣點數(shù)量，顯然5階SSRCKF算法與5階CKF算法相比，采樣點數(shù)目大幅度減少，同時可以使濾波器達(dá)到5階精度。因此本文選用5階SSRCKF算法。

表1 四種算法的采樣點數(shù)目Tab.1 Point numbers of different algorithm

2.2 SSRCKF濾波器

基于SSR采樣規(guī)則改進(jìn)的CKF在估計精度上明顯優(yōu)于標(biāo)準(zhǔn)CKF，尤其在計算量上。5階SSRCKF既能使得濾波精度達(dá)到5階，并且其采樣點數(shù)目較少，能夠顯著提高濾波效率。改進(jìn)后的五階SSRCKF濾波算法如下：

式(9)和(13)已經(jīng)給出本文組合導(dǎo)航系統(tǒng)的非線性濾波方程，且系統(tǒng)過程噪聲和觀測噪聲已假設(shè)為高斯白噪聲，可用均值和方差完全表達(dá)成高斯分布，因此可以使用高斯濾波過程處理，其一般形式如下：

式(20)種的各變量可用如下積分形式表示：

上述變量的運算可歸納成統(tǒng)一的多維積分形式I( f)，因此只需求得該積分的近似解就可完成上述高斯濾波過程，而I( f)可通過SSR溶劑采樣規(guī)則近似。以下給出5階SSRCKF的濾波過程：

3 仿真實驗與分析

本文SINS/DVL組合導(dǎo)航濾波過程采用間接濾波法，即采用SINS和DVL水平方向速度的差值為觀測量，觀測量通過5階SSRCKF進(jìn)行最優(yōu)估計，將估計后的系統(tǒng)誤差對SINS系統(tǒng)進(jìn)行反饋校正，最終輸出導(dǎo)航參數(shù)。具體流程如圖1所示。

圖1 SINS/DVL組合導(dǎo)航濾波原理框圖Fig.1 Schematic of SINS/DVL integrated navigation

針對此SINS/DVL的組合導(dǎo)航系統(tǒng)分別采用3階標(biāo)準(zhǔn)CKF算法和改進(jìn)后的5階SSRCKF算法進(jìn)行了2h的MATLAB仿真。設(shè)定系統(tǒng)工作的初始位置為東經(jīng)118°，北緯32°；①SINS系統(tǒng)：系統(tǒng)陀螺的常值偏移和隨機(jī)偏移均設(shè)為0.05 (°)/h，加速度計的常值漂移和隨機(jī)漂移均設(shè)為50μ，水平初始姿態(tài)角為0°，航向角設(shè)為北偏東30°，初始姿態(tài)角誤差分別為1°、1°、10°，北向速度和東向速度的初始偏差為0 m/s；②DVL系統(tǒng)：速度偏移誤差δVDVL= 0.10 m/s，標(biāo)度因數(shù)誤差δKc=0.001，DVL的速度偏移的相關(guān)時間和刻度系數(shù)誤差的相關(guān)時間分別取為100 s。SINS的采樣周期為0.01 s，DVL的采樣周期為1 s，CKF的濾波周期設(shè)置為1 s。載體以5 m/s的速度沿直線勻速行駛一段時間后，繞圓弧依次進(jìn)行90°的左轉(zhuǎn)和右轉(zhuǎn)，轉(zhuǎn)向時間為120 s，兩次轉(zhuǎn)向間的運動時間為1000 s，兩次轉(zhuǎn)向后載體繼續(xù)沿直線運動，仿真時間為7200 s。

3階標(biāo)準(zhǔn)CKF以及本文改進(jìn)的5階SSRCKF的仿真的姿態(tài)角、運動線速度和位置誤差如圖2～4所示，圖中φx、φy、φz分別表示縱搖、橫搖和航向失準(zhǔn)角，δVE、δVN分別為東向和北向速度誤差，δpE、δpN分別為東向和北向（經(jīng)緯度方向）的位置誤差。

從圖2的姿態(tài)角誤差仿真曲線可以看出，3階CKF算法和改進(jìn)后的5階SSRCKF算法的水平失準(zhǔn)角都有較高的精度和穩(wěn)定性，但改進(jìn)后的5階SSRCKF算法的精度明顯高于3階CKF算法的精度，其水平誤差角穩(wěn)定在±0.01°以內(nèi)，航向角誤差穩(wěn)定在±0.1°以內(nèi)。

從圖3的速度誤差曲線可以看出，改進(jìn)后的5階SSRCKF算法速度誤差穩(wěn)定性和精度都高于3階CKF算法，其誤差量穩(wěn)定在±0.25 m/s之間。

從圖4的位置誤差曲線仿真圖可以看出：在7200 s的仿真時間內(nèi)，3階CKF的北向位置誤差在-150～100 m之間，5階SSRCKF算法的北向位置誤差范圍在-50～50 m之間，后者精度顯著提高；3階CKF的東向位置誤差在-150～10 m之間，5階SSRCKF算法東向位置誤差在-50～10 m之間，5階SSRCKF算法的精度明顯高于3階CKF算法。

綜上所述，5階SSRCKF顯著提高了SINS/DVL組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。

圖2 姿態(tài)角誤差仿真曲線Fig.2 Simulation curves of attitude angle errors

圖3 水平方向速度誤差仿真曲線Fig.3 Simulation curves of horizontal velocity errors

圖4 位置誤差仿真曲線Fig.4 Simulation curves of position errors

從以上的仿真實驗結(jié)果可知：采用5階SSR采樣規(guī)則改進(jìn)的CKF算法，在繼承標(biāo)準(zhǔn)CKF算法的基礎(chǔ)上，進(jìn)一步提高了組合導(dǎo)航系統(tǒng)的導(dǎo)航精度，同時解決了標(biāo)準(zhǔn)CKF算法為提高精度而采用高階算法時，其計算量大大增多的問題。相比之下，改進(jìn)后的5階SSRCKF算法比3階標(biāo)準(zhǔn)CKF算法的導(dǎo)航精度高，更適用于導(dǎo)航系統(tǒng)精度要求較高的系統(tǒng)。

4 結(jié) 論

本文為了提高SINS/DVL組合導(dǎo)航系統(tǒng)精度，根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）的非線性誤差模型和多普勒測速儀（DVL）的測速原理及其誤差方程，以SINS為主導(dǎo)航設(shè)備且以DVL輔助導(dǎo)航，建立SINS/DVL組合導(dǎo)航系統(tǒng)模型。采用球面最簡相徑（SSR）采樣規(guī)則改進(jìn)CKF濾波方法，通過改變其采樣規(guī)則，減少了CKF算法的采樣點數(shù)量，該方法相比于3階CKF導(dǎo)航精度較高，相比于5階CKF計算量小，計算效率顯著增加。在數(shù)學(xué)平臺下進(jìn)行7200 s的動基座仿真，仿真結(jié)果表明：5階SSR改進(jìn)型CKF方法能夠有效提高SINS/DVL組合導(dǎo)航系統(tǒng)的精度，且穩(wěn)定性好。

（References）：

[1] 李厚全, 劉莫塵, 伍志海, 等. 球面單形平方根無跡粒子濾波在拖曳合成孔徑聲納組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2014, 22(4): 531-535.Li Hou-quan, Liu Mo-chen, Wu Zhi-hai, et al. Spherical simplex squareroot unscented particle filter used in integrated navigation system of synthetic aperture sonar[J]. Journal of Chinese Inertial Technology, 2014，22(4): 531-535.

[2] 黃鶯. 基于CKF的SINS/DVL組合導(dǎo)航系統(tǒng)設(shè)計與仿真[D]. 哈爾濱:哈爾濱工程大學(xué), 2013.Huang Ying. Design and simulation of SINS/DVL integrated navigation system based on CKF[D]. Harbin: Harbin Engineering University, 2013.

[3] Cao Jie, Liu Fan-ming, Chen Qin, et al.SINS/DVL integrated navigation technology in autonomous underwater vehicle[J]. Navigation of China, 2004, 59(2):55-59.

[4] Yang Bo, Xu Xiao-su, Zhang Tao, et al.Novel SINS initial alignment method under large misalignment angles and uncertain noise based on nonlinear filter[J]. Mathematical Problems in Engineering, 2017: 1-14.

[5] 徐曉蘇, 潘永飛, 鄒海軍. 基于自適應(yīng)濾波SINS/DVL組合導(dǎo)航系統(tǒng)[J].華中科技大學(xué)學(xué)報(自然科學(xué)版),2015(3): 95-99, 106.Xu Xiao-su, Pan Yong-fei, Zou Hai-jun.SINS/DVL integrated navigation system based on adaptive filtering[J].Journal of Chinese Inertial Technology,2015(3): 95-99, 106.

[6] Gao Wei, Li Jing-chun. Adaptive Kalman filtering for the integrated SINS/DVL system[J]. Journal of Computational Information Systems, 2013, 16(9):6443- 6450.

[7] Wang Xiao-xu, Liang Yan, Pan Quan, et al. Gaussian filter for nonlinear systems with one-step randomly delayed measurements[J]. Automatica, 2013, 49(4):976-986.

[8] Wang Shi-yuan, Feng Jiu-chao. Spherical simplex-radial cubature Kalman filter[J]. IEEE Signal Processing Letters,2014, 21(1): 43-46.

[9] Jia B, Xin M, Cheng Y. High-degree cubature Kalman filter[J]. Automatica,2013, 49(2): 510-518.

[10] Zhang Yong-gang, Huang Yu-long, Wu Zhe-min, et al. Seventh-degree spherical simplex-radial cubature Kalman filter[C]//Proceedings of the 33rd Chinese Control Conference. 2014: 2513-2517.

[11] Arasaratnam I, Haykin S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[12] Wang Lei, Cheng Xiang-hong, Li Shuangxi, et al. Adaptive interacting multiple model filter for AUV integrated navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 511- 515.

Improved fifth-degree spherical simplex sadial cubature Kalman filter in SINS/DVL integrated navigation

XU Xiao-su1,2, DONG Ya1,2, TONG Jin-wu1,2, DAI Wei1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University,Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

In order to improve the accuracy of SINS/DVL integrated navigation system, a SINS/DVL integrated navigation system model is built based on establishing the nonlinear error model of SINS and the error equation of the Doppler velocity log (DVL). To further improve the navigation accuracy, a fifth-degree cubature Kalman filter is applied. In view that conventional fifth-degree cubature Kalman filter has too large computation amount and relative poor robustness, a fifth-degree spherical simplex radial (SSR) cubature Kalman filter is designed, which adopts the SSR rule to change the sampling rule and reduce the number of sampling points. Simulation results show that the proposed method can effectively improve the accuracy of the SINS/DVL integrated navigation system and has better stability.

integrated navigation; nonlinear system; spherical simplex radial; cubature Kalman filter

U666.1

：A

1005-6734(2017)03-0343-06

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

2017-03-07；

：2017-05-25

國家自然科學(xué)基金項目（51175082，61473085）

徐曉蘇（1961—），男，博士生導(dǎo)師，從事測控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail: xxs@seu.edu.cn