基于5階球面最簡相徑的改進型容積卡爾曼濾波在SINS/DVL組合導航中的應用

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

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

基于5階球面最簡相徑的改進型容積卡爾曼濾波在SINS/DVL組合導航中的應用

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

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

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

組合導航；非線性系統；球面最簡相徑；容積卡爾曼濾波

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

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

本研究針對以上問題，采用球面最簡相徑（SSR）采樣規則改進標準的CKF算法，并選用5階改進的CKF，進一步提高了組合導航系統的導航精度。將標準CKF和改進后的CKF濾波后的導航精度進行仿真比較，結果表明，改進后的CKF比標準CKF濾波效果更好，系統的導航精度更高[10-11]。

1 SINS/DVL組合導航系統模型

1.1 SINS誤差數學模型

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

1.2 DVL誤差數學模型

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

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

1.3 組合導航濾波模型

由于慣導系統天向通道不穩定，且DVL對天向的測速誤差較大忽略高度通道的狀態量[6]，對于SINS系統，選取12維狀態變量為：

對于DVL選取標度因數誤差和隨機常值測量誤差作為狀態量，即：。將系統的非線性狀態方程簡記為：

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

選取系統的觀測量為導航坐標系下捷聯解算速度和DVL的速度的差值，并且由于慣導系統天向通道不穩定，且DVL對天向的測速誤差較大，本文只考慮水平通道，即只取速度的前兩維。

式中：Vn為導航坐標系下速度的真實值，δKc為DVL的刻度系數誤差，δVDVL為DVL的隨機測量誤差。因此量測方程為：

將此量測方程簡記為：

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

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

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

2 五階SSRCKF濾波算法

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

2.1 SSR容積采樣規則

高斯濾波積分可歸納成：

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

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

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

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

2.2 SSRCKF濾波器

基于SSR采樣規則改進的CKF在估計精度上明顯優于標準CKF，尤其在計算量上。5階SSRCKF既能使得濾波精度達到5階，并且其采樣點數目較少，能夠顯著提高濾波效率。改進后的五階SSRCKF濾波算法如下：

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

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

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

3 仿真實驗與分析

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

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

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

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

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

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

從圖4的位置誤差曲線仿真圖可以看出：在7200 s的仿真時間內，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組合導航系統的導航精度。

圖2 姿態角誤差仿真曲線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

從以上的仿真實驗結果可知：采用5階SSR采樣規則改進的CKF算法，在繼承標準CKF算法的基礎上，進一步提高了組合導航系統的導航精度，同時解決了標準CKF算法為提高精度而采用高階算法時，其計算量大大增多的問題。相比之下，改進后的5階SSRCKF算法比3階標準CKF算法的導航精度高，更適用于導航系統精度要求較高的系統。

4 結 論

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

（References）：

[1] 李厚全, 劉莫塵, 伍志海, 等. 球面單形平方根無跡粒子濾波在拖曳合成孔徑聲納組合導航中的應用[J]. 中國慣性技術學報, 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]. 哈爾濱:哈爾濱工程大學, 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] 徐曉蘇, 潘永飛, 鄒海軍. 基于自適應濾波SINS/DVL組合導航系統[J].華中科技大學學報(自然科學版),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

國家自然科學基金項目（51175082，61473085）

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