查 月，尤婷婷

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

0 引 言

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

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

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

1 組合系統(tǒng)的結(jié)構(gòu)配置

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

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

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

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

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

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

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

1.2 多普勒計(jì)程儀（DVL）

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

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

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

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

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

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

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

2 組合導(dǎo)航系統(tǒng)的數(shù)學(xué)模型

2.1 狀態(tài)方程

導(dǎo)航坐標(biāo)系定義為東-北-天地理坐標(biāo)系，SINS誤差模型為：

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

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

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

2.2 量測方程

AUV的真實(shí)位置為TAN測得的位置為則 SINS測得的位置信息為：

TAN測得的位置信息為

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

位置量測方程為：

DVL測得的速度為：

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

速度量測方程為：

假設(shè)AUV的真實(shí)航向?yàn)?，MCP顯示的航向?yàn)椋琒INS和MCP的航向?yàn)椋?/p>

航向的量測方程為：

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

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

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

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

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

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

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

其中，n=3。

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

3.1 BP神經(jīng)網(wǎng)絡(luò)架構(gòu)

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

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

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

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

通過計(jì)算梯度下降后的可得結(jié)果

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

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

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

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

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

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

5 仿真實(shí)驗(yàn)

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

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

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

6 結(jié) 語

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

[1] 張濤, 石宏飛, 陳立平, 等. 基于UKF的SINS/LBL水下AUV緊組合定位技術(shù)[J]. 中國慣性技術(shù)學(xué)報(bào), 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] 周保軍, 劉碩. 水下導(dǎo)航技術(shù)研究[J]. 現(xiàn)代導(dǎo)航, 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] 葛錫云, 申高展, 潘瓊文, 等. 基于慣性導(dǎo)航/地磁的水下潛器組合導(dǎo)航定位方法[J]. 艦船科學(xué)技術(shù), 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] 徐遵義, 晏磊, 劉光軍, 等. 海底地形輔助導(dǎo)航全面仿真技術(shù)研究[J]. 艦船科學(xué)技術(shù), 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)絡(luò)與非線性濾波在景象匹配輔助導(dǎo)航中的應(yīng)用研究[D]. 南京: 南京航空航天大學(xué), 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)絡(luò)在SINS/GPS中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào), 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.