潘 爽，施建禮，聶永芳，王希彬

(1.海軍潛艇學(xué)院 戰(zhàn)略導(dǎo)彈與水中兵器系，山東 青島 266041；2.海軍航空大學(xué) 航空基礎(chǔ)學(xué)院，山東 煙臺(tái) 264001)

0 引 言

自主水下航行器（Autonomous Underwater Vehicle，AUV）具有自主性高、機(jī)動(dòng)性好、探測(cè)地域大、隱蔽性強(qiáng)等顯著優(yōu)勢(shì)，在水下環(huán)境中得到廣泛應(yīng)用，而要實(shí)現(xiàn)AUV的這些優(yōu)勢(shì)，最關(guān)鍵的是要實(shí)現(xiàn)AUV的自主導(dǎo)航與探測(cè)。然而，精準(zhǔn)的AUV自主導(dǎo)算法需要系統(tǒng)提供遠(yuǎn)距離、長(zhǎng)時(shí)間的精準(zhǔn)速度、位置和姿態(tài)信息，而這一技術(shù)主要利用SLAM算法完成AUV的自主定位與制圖。該算法可以描述為[1]：AUV從初始的位置出發(fā)，在未知環(huán)境中運(yùn)動(dòng)，依靠自身所攜帶的傳感器對(duì)未知環(huán)境進(jìn)行感知，處理計(jì)算機(jī)對(duì)傳感器數(shù)據(jù)進(jìn)行處理并創(chuàng)建地圖，同時(shí)依據(jù)此地圖對(duì)AUV進(jìn)行導(dǎo)航。

SLAM算法最先由國(guó)外學(xué)者提出[2]，并在國(guó)外發(fā)展迅速，由于在機(jī)器人的自主定位中廣泛應(yīng)用，算法的種類(lèi)也越來(lái)越多，技術(shù)較為成熟。國(guó)內(nèi)SLAM算法研究起步較晚，目前已在地面機(jī)器人、空中無(wú)人機(jī)和水下航行器等領(lǐng)域展開(kāi)應(yīng)用。本文提出一種基于EKF的水下SLAM算法[3]，通過(guò)對(duì)特征進(jìn)行2次探測(cè)，提高了AUV和特征的定位精度，以便更好實(shí)現(xiàn)AUV的自主精確導(dǎo)航與探測(cè)。

1 水下航行器模型構(gòu)建

航行器的運(yùn)動(dòng)模型和環(huán)境探測(cè)模型是SLAM系統(tǒng)的關(guān)鍵部分，AUV在水下的運(yùn)動(dòng)狀態(tài)由航行器系統(tǒng)的運(yùn)動(dòng)模型體現(xiàn)，AUV裝載的聲吶傳感器由環(huán)境探測(cè)模型表示。模型的創(chuàng)建需要在一致的坐標(biāo)系內(nèi)，本文采用全局坐標(biāo)系作為航行器系統(tǒng)模型的基礎(chǔ)坐標(biāo)系。

1.1 AUV水下運(yùn)動(dòng)模型

AUV相對(duì)于全局坐標(biāo)系的位置和姿態(tài)，均可以通過(guò)運(yùn)動(dòng)坐標(biāo)系相對(duì)全局坐標(biāo)系的坐標(biāo)值和運(yùn)動(dòng)坐標(biāo)系相對(duì)于全局坐標(biāo)系的旋轉(zhuǎn)角確定。AUV在水下的運(yùn)動(dòng)模型為六自由度，則AUV的運(yùn)動(dòng)方程可表示為：

式中：為前進(jìn)速度，m/s；為側(cè)向速度，m/s；為升降速度，m/s；為滾轉(zhuǎn)角速度，rad/s；為偏航角速度，rad/s；為俯仰角速度，rad/s。

1.2 水下觀測(cè)模型

聲吶的探測(cè)信息是AUV測(cè)量環(huán)境特征位置、方向的信息來(lái)源[4]，特征模型在全局坐標(biāo)系[5]中的表[示，需要]對(duì)特征模型在傳感器坐標(biāo)系中的位置和方向Z=ρ,ψs,θsT進(jìn)行坐標(biāo)變換。已知傳感器安裝位置到AUV重心的距離，即傳感器坐標(biāo)原點(diǎn)到運(yùn)動(dòng)坐標(biāo)系的坐標(biāo)原點(diǎn)距離已知，假設(shè)兩坐標(biāo)系坐標(biāo)原點(diǎn)在同一水平面上且不考慮AUV滾轉(zhuǎn)運(yùn)動(dòng)，AUV的觀測(cè)模型如圖1所示。

則觀測(cè)方程可以表示為：

其中， (xs,ys,zs)為特征模型在觀測(cè)坐標(biāo)系中的坐標(biāo)表示，根據(jù)聲吶坐標(biāo)系與全局坐標(biāo)系和位置坐標(biāo)系間的方位關(guān)系，可以得出觀測(cè)模型。

2 基于 EKF 的水下 SLAM 算法

在建立水下自主航行器（AUV）系統(tǒng)模型基礎(chǔ)上，提出采用基于捷聯(lián)慣導(dǎo)系統(tǒng)與聲吶傳感器的水下自主航行器同時(shí)定位與制圖算法EKF-SLAM算法。捷聯(lián)慣導(dǎo)的導(dǎo)航原理[6]是通過(guò)慣性器件測(cè)量水下自主航行器的角速度和3個(gè)敏感軸向的加速度，并通過(guò)四元數(shù)法解算出自主水下航行器的位姿和速度，但是計(jì)算所得到的是近似值，存在計(jì)算誤差，隨時(shí)間增長(zhǎng)導(dǎo)航偏差會(huì)不斷增大?；诮萋?lián)慣導(dǎo)系統(tǒng)與聲吶傳感器的水下自主航行器同時(shí)定位與制圖技術(shù)的EKF-SLAM算法，利用捷聯(lián)慣導(dǎo)的輸出參數(shù)與聲吶傳感器探測(cè)的環(huán)境特征估計(jì)定位，通過(guò)算法的遞推更新濾除誤差，提高導(dǎo)航定位精度[7]。

2.1 狀態(tài)方程

AUV的位置、姿態(tài)信息是通過(guò)對(duì)慣性導(dǎo)航器件的輸出進(jìn)行積分運(yùn)算得到的，慣導(dǎo)器件的輸出參數(shù)含有隨機(jī)誤差，將會(huì)影響到AUV的定位精度，且隨著時(shí)間的累積，誤差的影響也明顯，最終導(dǎo)致AUV迷失方向。本文解決這一問(wèn)題的方法是通過(guò)在AUV的狀態(tài)向量中引入慣導(dǎo)器件的高斯誤差[8]，通過(guò)濾波器消除這一誤差。

水下自主航行器的狀態(tài)方程可以表示成：式中：為時(shí)刻的狀態(tài)向量；為狀態(tài)轉(zhuǎn)換矩陣；為時(shí)刻的控制向量；為高斯白噪聲向量，設(shè)其均值為零，協(xié)方差為，可以表示為

其中，為AUV在全局坐標(biāo)系中的位置；為AUV各坐標(biāo)線(xiàn)速度和轉(zhuǎn)向角速度；為AUV相對(duì)于前一時(shí)刻的坐標(biāo)位置。

2.2 觀測(cè)方程

AUV利用裝載在機(jī)身上的主動(dòng)聲吶，觀測(cè)環(huán)境信息從而獲得特征模型和探測(cè)聲吶之間的幾何位置關(guān)系，并以此為參考校正AUV的坐標(biāo)。聲吶探測(cè)模型是通過(guò)構(gòu)建聲吶傳感器同特征模型之間的距離和方向獲得的，則第i個(gè)特征的觀測(cè)方程為：

已知是特征模型的狀態(tài)向量，即，那么

即為表示在時(shí)刻的AUV狀態(tài)向量和特征模型狀態(tài)向量函數(shù)。設(shè)AUV在運(yùn)動(dòng)坐標(biāo)系中的狀態(tài)向量為，則觀測(cè)方程可以表示為:

由于是非線(xiàn)性的，只有對(duì)觀測(cè)方程進(jìn)行線(xiàn)性化，才能通過(guò)濾波器實(shí)現(xiàn)SLAM算法。利用對(duì)AUV狀態(tài)向量進(jìn)行雅克比替代[9]，如下：

便可實(shí)現(xiàn)觀測(cè)方程的線(xiàn)性化，觀測(cè)方程可寫(xiě)為：

3 仿真試驗(yàn)與分析

AUV工作環(huán)境為100 m×100 m水下模擬環(huán)境，以特征點(diǎn)表示水下信標(biāo)、海丘、海嶺等特征明顯的海底地貌，仿真圖用*表示特征點(diǎn)。AUV是以3±0.3 m/s的恒定航速做勻速運(yùn)動(dòng)，初始角為0°，最大轉(zhuǎn)彎角30±3°，最大轉(zhuǎn)彎角速度為20 rad/s。聲吶探測(cè)的最遠(yuǎn)距離為30 m，測(cè)量誤差為0.1 m，則狀態(tài)方程的誤差協(xié)方差，觀測(cè)方程的誤差協(xié)方差。將初始化后的系統(tǒng)參數(shù)載入EKF-SLAM算法的工作環(huán)境，開(kāi)始進(jìn)行仿真。檢驗(yàn)算法在環(huán)境特征點(diǎn)為50個(gè)時(shí)做圓周運(yùn)動(dòng)的定位與制圖精度，如圖2所示。

從圖3和圖4可以看出，在環(huán)境特征點(diǎn)為50個(gè)時(shí)，AUV的定位與制圖精度較高，且呈收斂趨勢(shì)，算法具有較好的穩(wěn)定性。AUV在x軸向的定位誤差在沿x軸向運(yùn)動(dòng)時(shí)誤差較小，沿y軸向運(yùn)動(dòng)時(shí)誤差較大；在y軸向的定位誤差與x軸向的情況相反。

為檢驗(yàn)算法的數(shù)據(jù)關(guān)聯(lián)作用，在環(huán)境特征點(diǎn)為50個(gè)時(shí)做2次圓周運(yùn)動(dòng)的定位與制圖精度，如圖5所示。

從圖6和圖7可以看出，AUV采用2次探測(cè)路徑運(yùn)動(dòng)時(shí)，定位誤差明顯降低，其主要原因是由于通過(guò)數(shù)據(jù)關(guān)聯(lián)對(duì)特征點(diǎn)的重復(fù)關(guān)聯(lián)匹配，使特征點(diǎn)位置更好地修正。在AUV的定位誤差較大時(shí)，對(duì)已探測(cè)過(guò)的區(qū)域進(jìn)行重復(fù)探測(cè)，可以提高AUV的定位與制圖精度。

4 結(jié) 語(yǔ)

本文在建立AUV運(yùn)動(dòng)模型與聲吶模型的基礎(chǔ)上，研究了基于EKF的SLAM算法，并采用數(shù)據(jù)關(guān)聯(lián)方法優(yōu)化了濾波算法，且提高算法定位精度。在環(huán)境特征點(diǎn)較多時(shí)，仿真驗(yàn)證了算法在圓形路徑下的定位與制圖精度，結(jié)果表明算法的精度符合實(shí)際要求；對(duì)比相同環(huán)境特征密度下，采用2次探測(cè)路徑，結(jié)果表明該算法可有效提高AUV定位精度，從而驗(yàn)證了該算法的有效性和可行性。實(shí)際應(yīng)用中，外界的三維環(huán)境非常復(fù)雜，可能還包含許多動(dòng)態(tài)目標(biāo)，對(duì)此還需進(jìn)一步深入研究。

[1]劉明雍, 趙濤, 周良榮.SLAM 算法在 AUV 中的應(yīng)用進(jìn)展[J].魚(yú)雷技術(shù), 2010, 18(1): 41–48.

[2]張恒, 樊曉平.移動(dòng)機(jī)器人同步定位與地圖構(gòu)建過(guò)程中的軌跡規(guī)劃研究[J].機(jī)器人, 2006, 28(3): 285–290.

[3]FREDRIK O.Comparison of kalman filter estimation approaches for state space models with nonlinear measurements[C]//Proceedings of Scandinavian Conference on Simulation and Modeling, SIMS, 2005.

[4]曹軍, 曾碧, 何元烈.基于路標(biāo)觀測(cè)的改進(jìn) EKF-SLAM 算法[J].自動(dòng)化與信息工程, 2014, 35(1): 21–31.

[5]黃魯粵.基于慣性傳感器與聲納的水下機(jī)器人定位與地圖構(gòu)建[D].青島: 中國(guó)海洋大學(xué), 2010.

[6]秦永元, 張洪鉞, 汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安: 西北工業(yè)大學(xué)出版社, 2012.198–211.

[7]WEBB T, PRAZENICA R, KURDIAL A, et al.Vision based state estimation for autonomous micro air vehicles[J].Journal of Guidance, Control, and Dynamics, 2007, 30(3): 816–826.

[8]蔡自興, 賀漢根, 陳虹.未知環(huán)境中移動(dòng)機(jī)器人導(dǎo)航控制理論與方法[M].北京: 科學(xué)出版社, 2009.

[9]王希彬.無(wú)人機(jī)視覺(jué)同時(shí)定位與地圖構(gòu)建技術(shù)[D].煙臺(tái): 海軍航空工程學(xué)院, 2012.

[10]王永清.同時(shí)定位與地圖創(chuàng)建中的數(shù)據(jù)關(guān)聯(lián)技術(shù)研究[D].青島: 中國(guó)海洋大學(xué), 2007.

[11]張碧叢, 李琳.馬氏距離在移動(dòng)機(jī)器人導(dǎo)航中的應(yīng)用[J].機(jī)械設(shè)計(jì)與制造, 2012, 5(6): 59–63.