查 月，尤婷婷

(1. 中國人民解放軍92941部隊，遼寧 葫蘆島 125001；2. 中國船舶重工集團有限公司，北京 100097)

0 引 言

自主式水下航行器（AUV）是新一代水下航行器，具有活動范圍大、機動性好等優(yōu)點，在民用領域，可用于海底考察、海底施工，水下設備維護與維修等任務；在軍用領域可用于偵察、掃雷、援潛和救生等任務。要使AUV完成預定的任務，離不開水下導航技術，水下導航與空中導航相比，具有工作時間長、信息源少、隱蔽性高等特點，它是決定AUV技術發(fā)展與應用的瓶頸問題[1–2]。

由加速計和陀螺儀構成的捷聯(lián)式慣性導航系統(tǒng)（SINS）是AUV定位導航的傳統(tǒng)傳感器，它可以在短時間提供精確的定位信息。由于慣性器件的固有誤差，慣性測量裝置（IMU）的誤差會隨著時間累積增大，這種累積會帶來定位誤差的發(fā)散，因此需要多個導航傳感器提供外信息對SINS誤差進行補償[3]。目前，用于AUV的組合導航系統(tǒng)主要有SINS/DVL，INS/GPS，SINS/GPS/DVL等。

GPS是一種具有全方位、全天候、全時段、高精度的衛(wèi)星導航系統(tǒng)，能為全球用戶提供低成本、高精度的三維位置、速度和精確定時等導航信息，但是它只能在陸地、水面和空中使用[4]。若采用GPS進行水下導航則僅限于淺水航行，AUV必須定期浮出水面才能利用GPS更新其定位信息。因此，必須設計一種適合AUV水下航行的新型導航方法。若沒有外信息輸入，SINS和DVL組合也很難提供高精確的導航信息。聲學定位系統(tǒng)無累積誤差，但存在高頻誤差，且更新率很低。由于地形輔助導航系統(tǒng)能夠克服水下聲學定位中存在的缺陷，本文研究了包含捷聯(lián)式慣性導航系統(tǒng)（SINS）、多普勒計程儀（DVL）和磁羅經(jīng)（MCP）與地形輔助導航系統(tǒng)（TAN）的組合導航系統(tǒng)?？柭鼮V波（KF）技術是目前組合導航系統(tǒng)中應用最廣泛的，并且它也是一直有效的最佳估計算法。在噪聲干擾下，經(jīng)典的KF會發(fā)散，無法滿足AUV的導航精度。神經(jīng)網(wǎng)絡具有結構簡單、可操作性強、模擬輸入輸出隨機非線性關系等特點，在非線性系統(tǒng)中有著廣泛的應用[5]。因此，近年來，KF與神經(jīng)網(wǎng)絡相結合的方法得到了廣泛研究，本文提出了一種基于神經(jīng)網(wǎng)絡自適應濾波的組合導航方法。

1 組合系統(tǒng)的結構配置

本文提出的SINS/DVL/MCP/TAN組合導航系統(tǒng)是一種較好的解決方案，可以提供比傳統(tǒng)導航系統(tǒng)性能優(yōu)越的導航系統(tǒng)。SINS通過測量加速度和角速度計算AUV的姿態(tài)、位置和速度。DVL和MCP分別提供高精度的AUV相對海底的絕對速度信息和航向信息。在高精度的測深儀和地圖匹配算法配合下，安裝在AUV上的TAN能夠間歇地提供定位信息。

1.1 捷聯(lián)式慣性導航系統(tǒng)（SINS）

SINS沒有實質(zhì)性的平臺，陀螺儀和加速計都是直接安裝在AUV載體上。

SINS的姿態(tài)角誤差方程可以表示為：

速度誤差方程可以表示為：

緯度、經(jīng)度和高度的誤差方程可以表示為：

式中：分別為東，北，天向的速度誤差；分別為東、北、天向姿態(tài)角誤差；分別為緯度，經(jīng)度和高度誤差；分別為X，Y，Z軸上的加速計偏差；分別為X，Y，Z軸上的陀螺儀偏差；為慣性坐標系內(nèi)的地轉(zhuǎn)速度；為導航坐標相對地球坐標系的角速率；為作用于導航坐標內(nèi)比力信息。

1.2 多普勒計程儀（DVL）

多普勒計程儀是一種高精度測量系統(tǒng)，它可以通過四波束法獲取速度。四波束如圖2所示。

四波束在方向上對稱分布，水平角為，垂直角為分別為四波束聲波方向，AUV的速度方向可以表示為：

1.3 磁羅經(jīng)（MCP）

磁羅經(jīng)由三維磁阻傳感器、角度傳感器和單片機組成，利用磁裝置獲取地球磁場方向，給出AUV的磁航向角[6]。這里，地球磁場矢量可以用正交分量X、Y、Z來描述。地球磁場水平分量可以表示為：

1.4 地形輔助導航系統(tǒng)（TAN）

地形輔助導航的原理是通過AUV實際測量的水深值與已配置的數(shù)字地圖進行匹配，對AUV進行的定位，定位精度與匹配區(qū)域的導航信息和地形匹配算法有關。地形匹配算法是地形輔助導航系統(tǒng)研究的核心技術。迭代最近點（ICP）算法是基于相關分析的大多數(shù)地形匹配方法的研究熱點[7]。

通過計算機對算法進行仿真。仿真的三維水下地形如圖3所示。地形圖存儲格式為90 m×90 m的方形網(wǎng)格。水深儀的測量誤差設置見表1。ICP算法的初始條件和模型參數(shù)如下：導航速度為10 m/s，行進方向角為45°，導航深度為100 m，仿真時間為1 h。ICP算法的經(jīng)緯度曲線誤差如圖4所示，匹配誤差控制在0.3″以內(nèi)。

2 組合導航系統(tǒng)的數(shù)學模型

2.1 狀態(tài)方程

導航坐標系定義為東-北-天地理坐標系，SINS誤差模型為：

表 1 儀器誤差模型Tab. 1 Bathometer error model

其中：，，為速度誤差；，，為SINS失準角；，，為緯度、經(jīng)度和高度誤差；，，為加速度計零偏，，，為陀螺常值漂移誤差。

系統(tǒng)的狀態(tài)方程為：其具體參數(shù)請參見文獻[5]。

2.2 量測方程

AUV的真實位置為TAN測得的位置為則 SINS測得的位置信息為：

TAN測得的位置信息為

式中：為水深；NE，NN，Nh分別為TAN沿東-北-天方向的位置誤差。

位置量測方程為：

DVL測得的速度為：

其中，ηE，ηN，ηU分別為DVL沿北-東方向的測量白噪聲。

速度量測方程為：

假設AUV的真實航向為，MCP顯示的航向為，SINS和MCP的航向為：

航向的量測方程為：

系統(tǒng)的量測方程為：

2.3 聯(lián)合卡爾曼濾波器

對系統(tǒng)的數(shù)學模型離散化，其形式如下：

當為狀態(tài)矢量時，k，為觀測矢量；k，為k–1 t o k時狀態(tài)轉(zhuǎn)換矩陣；為系統(tǒng)噪聲；為k–1時的系統(tǒng)噪聲矩陣；為k時的觀測矩陣；為觀測噪聲[4]。 通常情況下，和為與零均值不相干的白噪聲，方差矩陣為Q和R。但事實上，由于每一子系統(tǒng)的量測誤差，Q會隨著時間的變化而變化，而R卻隨環(huán)境的改變而不同[5]。鑒于海洋中復雜的導航環(huán)境，在仿真實驗中加入了時間相關誤差。

每一子濾波器方程的算法如下：

針對由噪聲模式帶來的不確定性，聯(lián)合濾波器具有極強的可逆性。聯(lián)合濾波器信息分布算法如下：

當和每一子濾波器和聯(lián)合濾波器的誤差協(xié)方差矩陣時，為信息分布的系數(shù)，則必須符合下式：

其中，n=3。

3 BP神經(jīng)網(wǎng)絡模型

3.1 BP神經(jīng)網(wǎng)絡架構

BP神經(jīng)網(wǎng)絡是一個多層網(wǎng)絡，是一種按誤差反向傳播訓練的多層前饋網(wǎng)絡，如圖5所示。它有3層或更多層，含輸入層、隱層和輸出層。BP算法是一種監(jiān)督式的學習算法，通過輸入學習樣本，使用反向傳播算法對網(wǎng)絡的權值和偏差進行反復的調(diào)整訓練，使輸出的向量與期望向量盡可能地接近，當網(wǎng)絡輸出層的誤差平方和小于指定的誤差時訓練完成，保存網(wǎng)絡的權值和偏差[6]。

3.2 BP神經(jīng)網(wǎng)絡算法

在BP神經(jīng)網(wǎng)絡中，假設輸入向量為輸出向量為為連接權。訓練過程描述如下：首先，所有的權值和闕值都首先(設為最)小隨機值。其次，為每個樣本設定實際輸出值最后，調(diào)整權值以便使多層網(wǎng)絡輸出產(chǎn)生的誤差值達到最小，整個網(wǎng)絡的誤差測量可以通過以下目標函數(shù)計算：

誤差的反向傳播采用梯度下降法調(diào)整各層間的連接權值，使目標函數(shù)達到最小。

通過計算梯度下降后的可得結果

這里為增益系數(shù)。

在BP算法中，權值校正所需的誤差按正常網(wǎng)絡傳播方向反向進行，從輸出層向輸入層的神經(jīng)元傳播[10]。采用迭代算法直到目標函數(shù)小于設定值。

4 基于BP神經(jīng)網(wǎng)絡的自適應濾波算法

采用聯(lián)合卡爾曼濾波對組合導航系統(tǒng)進行狀態(tài)估計的方法對系統(tǒng)的模型要求嚴格，若由于復雜環(huán)境所引起的對干擾統(tǒng)計特性無法準確獲得時，得到的狀態(tài)估計值就很難滿足定位精度的要求，因此本文采用基于BP神經(jīng)網(wǎng)絡的自適應濾波算法，實現(xiàn)組合導航系統(tǒng)的狀態(tài)估計。

針對AUV水下組合導航系統(tǒng)，提出了基于BP神經(jīng)網(wǎng)絡的自適應濾波方案?；贐P神經(jīng)網(wǎng)絡的自適應濾波方案見圖6。在反饋回路，將BP網(wǎng)絡輸出的修正加入到濾波算法不準確的狀態(tài)向量估計中，提高了系統(tǒng)的導航性能。

本文為AUV組合導航系統(tǒng)設計了基于BP神經(jīng)網(wǎng)絡的自適應濾波算法。采用離線訓練的BP網(wǎng)絡對實際動態(tài)環(huán)境下系統(tǒng)模型的不準確性進行校正，使其具有足夠的精度。該方案以減小導航誤差為目標，選取對誤差有直接影響的參數(shù)作為BP網(wǎng)絡的輸入，選擇觀測向量與一步預測測量向量之差作為唯一輸入。BP神經(jīng)網(wǎng)絡的輸出理想地描述了由子濾波器預測的姿態(tài)與AUV的真實姿態(tài)之間的精確差。根據(jù)以上給出的BP神經(jīng)網(wǎng)絡算法，采用300對樣本進行離線訓練，訓練結果見圖7。

5 仿真實驗

假設AUV在一個平面上，以一定的深度航行，系統(tǒng)仿真時間為5 h。根據(jù)AUV的使用環(huán)境，系統(tǒng)仿真條件如下：AUV速度為4 kn，初始姿態(tài)角均為0，初始航向角為45°，初始位置為165°E，32°N。組合導航系統(tǒng)的傳感器誤差模型如表2所示。

為了比較本文提出的方法效果，在相同的初始條件下，對經(jīng)典卡爾曼濾波器和基于BP自適應濾波器進行仿真實驗，仿真結果見圖8和圖9。仿真實驗結果表明：基于BP神經(jīng)網(wǎng)絡的自適應濾波算法通過 BP神經(jīng)網(wǎng)絡對卡爾曼濾波輸出修正，提高了卡爾曼濾波器的估計精度，使組合導航系統(tǒng)定位精度均有所提高。

表 2 組合導航系統(tǒng)的傳感器誤差模型Tab. 2 Sensor model in the integrated navigation system

6 結 語

本文提出了一種新的地形組合導航系統(tǒng)，能夠滿足AUV長期保持高穩(wěn)定和高精度導航的需求。基于神經(jīng)網(wǎng)絡輔助的聯(lián)合卡爾曼濾波器比經(jīng)典聯(lián)合卡爾曼濾波器具有更好的導航性能。該方法利用離線訓練的BP神經(jīng)網(wǎng)絡對經(jīng)典聯(lián)邦卡爾曼濾波估計中的誤差進行校正。AUV定位誤差大大減小，水下導航精度大大提高。

[1] 張濤, 石宏飛, 陳立平, 等. 基于UKF的SINS/LBL水下AUV緊組合定位技術[J]. 中國慣性技術學報, 2016, 24(10):639–642.ZHANG Tao, SHI Hong-fei, CHEN Li-pin, et al. An underwater positioning technology based on tightly coupled SINS/LBL for AUV[J]. Journal of Chinese Inertial Technology, 2016, 24(10): 639–642.

[2] TITTERTON D H, WESTON J L. Strapdown inertial navigation technology[Z]. London, U. K.: Peter Pegerinus,1997: 19–56, 259–320, and 385–392.

[3] 周保軍, 劉碩. 水下導航技術研究[J]. 現(xiàn)代導航, 2012, 2(1):19–23.ZHOU Bao-jun, LIU Shuo. Research of underwater navigation technology[J]. Modern Navigation, 2012, 2(1): 19–23.

[4] ALCOCER A, OLIVEIRA P, PASCOAL A. Study and implementation of an EKF GIB-based underwater positioning system, in Proc[Z]. IFAC Conf. Control Appl. Mar. Syst.(CAMS), Ancona, Italy. 2004.

[5] HOLTZHAUSEN S, MATSEBE O, TLALE N S, et al.Autonomous underwater vehicle motion tracking using a Kalman filter for sensor fusion[J]. 15th International conference on Mechatronics and Machine Vision in Practice,2008: 103–108.

[6] 葛錫云, 申高展, 潘瓊文, 等. 基于慣性導航/地磁的水下潛器組合導航定位方法[J]. 艦船科學技術, 2014, 36(11):120–123.GE Xi-yun, SHEN Gao-zhan, PAN Qiong-wen , et al. The underwater integrated navigation method of underwater vehicle based on INS / geomagnetic[J]. Ship Science and Technology,2014, 36(11): 120–123.

[7] 徐遵義, 晏磊, 劉光軍, 等. 海底地形輔助導航全面仿真技術研究[J]. 艦船科學技術, 2007, 29(1): 123–128.XUN Zun-yi, YAN Lei, LIU Guang-jun, et al. Study on comprehensive simulation technology for seabed terrain-aided navigation[J]. Ship Science and Technology, 2007, 29(1):123–128.

[8] 郭俊. 神經(jīng)網(wǎng)絡與非線性濾波在景象匹配輔助導航中的應用研究[D]. 南京: 南京航空航天大學, 2009: 15–21, 33–37.GUO Jun. Application of BP neural network and nonlinear filtering in scene matching aided navigation. [D]. Nanjing:Nanjing University of Aeronautics & Astronautics, 2009:15–21, 33–37.

[9] 徐曉蘇, 周峰, 張濤, 等. 遺傳算法優(yōu)化的神經(jīng)網(wǎng)絡在SINS/GPS中的應用[J]. 中國慣性技術學報, 2015, 23(3):322–327.XU Xiao-su, ZHOU Feng, ZHANG Tao, et al. Application of neural network by genetic algorithm optimization in SINS/GPS[J]. Journal of Chinese Inertial Technology, 2016, 24(10):639–642.

[10] HECHT-NIELEN R. Theory of the back propagation neural network[J]. International Joint Conference on Neural Networks, 2015: 322–327.