冀大雄 鄧嘯

摘 ?要：水下無人潛航器（Underwater Unmanned Vehicle，UUV）的導(dǎo)航定位主要依靠測距聲信標(biāo)或便攜式低精度導(dǎo)航傳感器。這些設(shè)備觀測信息單一且精度低，載體非線性運動及復(fù)雜海流等因素均對導(dǎo)航定位精度有較大的影響。因此，文章設(shè)計了基于擴展卡爾曼濾波（Extended Kalman Filtering，EKF）的導(dǎo)航算法，結(jié)果表明該算法適用范圍廣且具有好的收斂性和精度。

關(guān)鍵詞：水下潛航器;導(dǎo)航定位;擴展卡爾曼濾波

中圖分類號：TN966 ? ? 文獻標(biāo)識碼：A

Abstract：The navigation and positioning of underwater unmanned vehicles（UUV） mainly relies on the ranging acoustic beacon or portable low-precision navigation sensor.The observation information of these devices is single and the accuracy is low.The nonlinear motion of the carrier and the complex ocean currents have a great influence on the navigation and positioning accuracy.Therefore，a navigation algorithm based on extended Kalman filter（EKF）is designed in this manuscript.The results show that the algorithm has a wide range of applications and good convergence and accuracy.

Key words：Underwater unmanned vehicles;Navigation and positioning;Extended Kalman filter

水下無人潛航器（UUV）具有隱蔽性好、綜合效益高等優(yōu)點，已在多個領(lǐng)域得到了發(fā)展和應(yīng)用[1]。UUV協(xié)同探測能夠完成單UUV很難完成或無法完成的任務(wù)，發(fā)揮更大的作用[2]。UUV面臨的主要瓶頸是水下導(dǎo)航系統(tǒng)的設(shè)計，該系統(tǒng)要能精準(zhǔn)定位并提供速度、姿態(tài)等信息。同時，受質(zhì)量、體積限制及水介質(zhì)等多種因素影響，UUV的精確導(dǎo)航仍然是該領(lǐng)域內(nèi)的難點[3]。

傳統(tǒng)的導(dǎo)航方法是以航位推算為基礎(chǔ)，將多普勒測速儀和慣性導(dǎo)航系統(tǒng)融合，但誤差會隨時間積累而越來越大。EKF導(dǎo)航算法是一種基于模型近似線性化和高斯假設(shè)的次優(yōu)濾波法[4]。曹方方等利用EKF算法估計出UUV相對信標(biāo)的位置，通過優(yōu)化算法，可有效降低定位過程中的累積位置誤差，設(shè)計的水下航行體導(dǎo)航定位系統(tǒng)，利用卡爾曼濾波與GPS異常點剔除算法，提高了自動控制實時性且增強了航跡控制精確性[5]。筆者基于EKF算法，通過分析和實驗證明了其可應(yīng)用于三信標(biāo)、固定單信標(biāo)和移動單信標(biāo)三種條件下的UUV導(dǎo)航定位場景，同時航位估計和水流估計精度很高。

一、載體非線性運動模型

首先建立東—北直角坐標(biāo)系EON，然后以載體重心G為原點建立載體坐標(biāo)系xOy，如圖1所示。假設(shè)載體在海流速度為v的海水中沿水平面運動，并忽略橫搖擾動的影響。另外，載體裝備的電子羅盤能顯示載體艏向角Ψ，裝備的計程儀可計算出沿載體艏向的速度u。載體重心G的速度V可由海流與艏向速度的合成速度求得，載體ON向坐標(biāo)軸的夾角稱為記為γ，其物理含義是航跡角。海流作用使得載體以一定的角度航行，該角度記為β，名稱為漂角。通過下式可以求得載體在水平面的艏向速度：

載體內(nèi)安裝有定位聲吶測量載體，可以每隔一段時間，通過測量載體到聲信標(biāo)的聲傳播時間，求得距離，使載體在純距離方式下導(dǎo)航。令為載體東—北坐標(biāo)系下的坐標(biāo)，為海流東、北向分量，為渦輪式計程儀測量的速度，和為載體的艏向角和縱傾角。對于攜帶壓力傳感器的UUV，僅需估計載體的二維坐標(biāo)?？紤]到渦輪式計程儀測量的是相對海流的速度，在導(dǎo)航算法中還應(yīng)估計海流速度。因此，采用狀態(tài)—參數(shù)聯(lián)系估計，設(shè)濾波器狀態(tài)為，對載體的非線性運動進行離散化，建立如下運動學(xué)微分模型：

剩下的問題是設(shè)計有效的導(dǎo)航算法，滿足UUV在弱觀測條件下的導(dǎo)航需要。文章采用基于Kalman濾波器的導(dǎo)航算法，根據(jù)聲信標(biāo)數(shù)量及運動狀態(tài)，分成三種情況予以研究。

二、不同條件下的運動學(xué)分析

（一）三信標(biāo)條件下的運動學(xué)分析

根據(jù)三圓交匯原理，當(dāng)有距離信息到來時，代入定位方程解算載體位置。載體攜帶的導(dǎo)航傳感器（電子羅盤和計程儀）和位置信息分別構(gòu)成了兩個線性化觀測方程，分別由式（9）和式（10）表示：

（9）

（10）

其中，式（9）為沒有位置信息時的觀測方程;式（10）為位置信息到來時的觀測方程。

載體的全部運動過程由部分可觀性動態(tài)系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組成。每隔固定時間載體更新一次位置信息，在固定時段內(nèi)載體根據(jù)觀測量速度和艏向推算航位，能夠在時間區(qū)間段意義上，實現(xiàn)系統(tǒng)的完全可觀。

（二）固定單信標(biāo)條件下的運動學(xué)分析

單信標(biāo)條件下載體不能直接得出自身的位置坐標(biāo)。導(dǎo)航傳感器更新率小于位置更新率，因此需要的觀測方程有兩個，即：

單信標(biāo)條件下載體不能直接得出自身的位置坐標(biāo)。載體的全部運動過程由部分可觀性系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組成。每隔固定時間載體更新一次距離信息，在固定時段內(nèi)載體根據(jù)觀測量速度和艏向推算航位。將上述可觀性動態(tài)系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組合起來，使載體機動航行（改變航向或航速非線性改變），能實現(xiàn)系統(tǒng)在時段區(qū)間意義上的完全可觀。

（三）移動單信標(biāo)條件下的運動學(xué)分析

移動單信標(biāo)條件下載體不能直接得出自身的位置坐標(biāo)。導(dǎo)航傳感器更新率小于位置更新率，因此需要的觀測方程有兩個，同式（11）與式（12）。

移動單信標(biāo)條件下載體不能直接得出自身的位置坐標(biāo)。載體的全部運動過程由部分可觀性系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組成。每隔固定時間載體更新一次距離信息，在固定時段內(nèi)載體根據(jù)觀測量速度和艏向推算航位。將上述可觀性動態(tài)系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組合起來，使信標(biāo)或載體機動航行（改變航跡），能夠?qū)崿F(xiàn)系統(tǒng)在時段區(qū)間意義上的完全可觀。

（四）主從式多UUV群體的運動學(xué)分析

研究由一個主UUV和兩個從UUV構(gòu)成的主從式多UUV群體，如圖2。主UUV與從UUV時間同步。主UUV攜帶高精度導(dǎo)航傳感器，能夠?qū)崟r得到準(zhǔn)確的地理坐標(biāo)，具備自主導(dǎo)航能力。同時，主UUV還安裝了水聲Modem，將自身的實時位置數(shù)據(jù)定時發(fā)送給從UUV。從UUV攜帶低成本、精度較低的導(dǎo)航傳感器，通過安裝的水聲Modem接收主UUV的位置數(shù)據(jù)，同時測量從UUV到主UUV的聲傳播時間以得到距離。

在這種模式下從UUV不能直接得出自身的位置坐標(biāo)。導(dǎo)航傳感器更新率小于位置更新率，因此需要的觀測方程有兩個，即

其他從UUV的觀測方程同上。從UUV的全部運動過程由部分可觀性動態(tài)系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組成。每隔固定時間從UUV更新一次距離信息，在固定時段內(nèi)從UUV根據(jù)觀測量速度和艏向推算航位。將上述可觀性動態(tài)系統(tǒng)和完全可觀性動態(tài)系統(tǒng)組合起來，使主UUV或從UUV機動航行（改變航跡），能夠?qū)崿F(xiàn)系統(tǒng)在時段區(qū)間意義上的完全可觀。

三、導(dǎo)航算法

由于載體運動狀態(tài)方程是非線性的，對于單信標(biāo)（固定、移動、主從UUV）情況，觀測方程也是非線性的，考慮到海流等參數(shù)，需要對狀態(tài)和參數(shù)進行聯(lián)合估計，因此采用基于EKF的導(dǎo)航算法。

（一）算法一

將上一時刻狀態(tài)變量的預(yù)測值作為當(dāng)前時刻的載體坐標(biāo)，代入觀測方程計算觀測值。對于三信標(biāo)使用條件，當(dāng)位置更新信息到來時，作為當(dāng)前時刻載體坐標(biāo)的觀測值，如圖3;對于單信標(biāo)使用條件，當(dāng)距離更新信息到來時，作為當(dāng)前時刻距離的觀測值，如圖4。

（二）算法二

采用兩個EKF。對于三信標(biāo)情況，位置更新信息沒有到來時，以導(dǎo)航傳感器給出的艏向角和速度作為觀測向量，當(dāng)位置更新信息到來時，在原觀測向量中增加位置信息，如圖5;對于單信標(biāo)，距離信息沒有到來時，以導(dǎo)航傳感器給出的艏向角和速度作為觀測向量，當(dāng)距離信息到來時，在原觀測向量中增加位置信息，如圖6。

四、觀測值的確定

（一）三信標(biāo)

以載體位置、導(dǎo)航傳感器給出的速度及艏向角作為觀測值，根據(jù)聲雙程傳播時間和平均聲速計算載體到信標(biāo)的距離，再利用聲信標(biāo)與載體的高度差計算水平距離，考慮到載體在傳播時間內(nèi)位置的偏移，將其全部代入定位方程，即可算出載體坐標(biāo)。也可利用二次多項式法得到較高精度的水平距離。不管采用哪種方式，在聲傳播時間中都應(yīng)減去信標(biāo)的檢測延時。

（二）單信標(biāo)

載體到信標(biāo)的水平距離、導(dǎo)航傳感器給出的速度及艏向角作為觀測值。在應(yīng)答方式下，采用與三信標(biāo)相同的方法得到載體到信標(biāo)的距離和水平距離，再代入定位方程求解，也可利用二次多項式法得到較高精度的水平距離。不管采用哪種方式，在聲傳播時間中都應(yīng)減去信標(biāo)的檢測延時。在同步方式下，根據(jù)聲單程傳播時間和平均聲速計算載體到信標(biāo)的距離，再利用聲信標(biāo)與載體的高度差計算水平距離，也可利用二次多項式法或本征聲線法得到較高精度的水平距離。

（三）異常值剔除

采用馬氏距離法判斷距離測量值的野點，根據(jù)距離預(yù)測值與測量值的差確定異常點：

其中為第時刻的距離觀測值，，是樣本的協(xié)方差矩陣，是一個可調(diào)參數(shù)。當(dāng)上式左邊大于時，則該時刻距離測量值野點，予以剔除。

五、實驗結(jié)果與分析

僅考慮載體在水平面內(nèi)的運動狀態(tài)。導(dǎo)航傳感器誤差服從零均值的高斯分布，計程儀與電子羅盤誤差方差分別為5%和2°。載體坐標(biāo)分量測量方差為1米，載體到信標(biāo)的距離測量誤差方差為1米。導(dǎo)航傳感器數(shù)據(jù)更新率為1秒，位置或距離更新率30秒。海流分量從0.5米跳躍到0.8米，仿真時間3000秒。規(guī)定以與實際初始位置偏差50米的坐標(biāo)分量作為狀態(tài)分量初始值，其他狀態(tài)分量初始值均為零。分別給出三信標(biāo)（圖7、8）、固定單信標(biāo)（圖9、10）、移動單信標(biāo)（圖11、12）、主從UUV（圖13）條件下航位估計和水流估計的仿真結(jié)果。

六、結(jié)論

理論分析和仿真實驗表明，EKF導(dǎo)航算法不僅能夠應(yīng)用于三信標(biāo)條件下攜帶低精度導(dǎo)航傳感器的UUV導(dǎo)航定位，同樣適用于固定單信標(biāo)和移動單信標(biāo)條件下的導(dǎo)航定位，航位估計和水流估計精度很高。不同的是在三信標(biāo)條件下載體不用機動航行，而單信標(biāo)載體需要進行一定的機動航行，以提高系統(tǒng)可觀測性。仿真實驗表明設(shè)計的濾波器是漸進穩(wěn)定的，也就是濾波性能不受初始值的影響，只要濾波時間足夠長，總能以任意精度逼近實際值。仿真實驗還表明導(dǎo)航算法二的濾波性能優(yōu)于導(dǎo)航算法一，具有更好的收斂性和精度。

參考文獻：

[1]金建海，陳偉華，張波，等.UUV集群技術(shù)概述[C]//中國造船工程學(xué)會，西北工業(yè)大學(xué)，中國船舶重工集團有限公司第七〇五研究所.2018年水下無人系統(tǒng)技術(shù)高峰論壇論文集.西安：《水下無人系統(tǒng)學(xué)報》編輯部，2017：92-96.

[2]周宏坤，葛錫云，邱中梁，等.UUV集群協(xié)同探測與數(shù)據(jù)融合技術(shù)研究[J].艦船科學(xué)技術(shù)，2017（12）：70-75.

[3]張立川，劉明雍，徐德民，等.基于水聲傳播延遲的主從式多無人水下航行器協(xié)同導(dǎo)航定位研究[J].兵工學(xué)報，2009（12）：1674-1678.

[4]劉國海，李沁雪，施維，等.動態(tài)卡爾曼濾波在導(dǎo)航試驗狀態(tài)估計中的應(yīng)用[J].儀器儀表學(xué)報，2009（02）：396-400.

[5]曹方方，劉衛(wèi)東，李娟麗.基于EKF的UUV單信標(biāo)水聲導(dǎo)航定位方法研究[J].計算機測量與控制，2011（09）：2234-2236+2244.

[6]張晉斌.基于卡爾曼濾波的UUV導(dǎo)航定位系統(tǒng)GPS異常點剔除方法[J].電子世界，2014（15）：95.