冀大雄 鄧嘯

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

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

中圖分類號(hào)：TN966 ? ? 文獻(xiàn)標(biāo)識(shí)碼：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)點(diǎn)，已在多個(gè)領(lǐng)域得到了發(fā)展和應(yīng)用[1]。UUV協(xié)同探測(cè)能夠完成單UUV很難完成或無法完成的任務(wù)，發(fā)揮更大的作用[2]。UUV面臨的主要瓶頸是水下導(dǎo)航系統(tǒng)的設(shè)計(jì)，該系統(tǒng)要能精準(zhǔn)定位并提供速度、姿態(tài)等信息。同時(shí)，受質(zhì)量、體積限制及水介質(zhì)等多種因素影響，UUV的精確導(dǎo)航仍然是該領(lǐng)域內(nèi)的難點(diǎn)[3]。

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

一、載體非線性運(yùn)動(dòng)模型

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

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

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

二、不同條件下的運(yùn)動(dòng)學(xué)分析

（一）三信標(biāo)條件下的運(yùn)動(dòng)學(xué)分析

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

（9）

（10）

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

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

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

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

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

（三）移動(dòng)單信標(biāo)條件下的運(yùn)動(dòng)學(xué)分析

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

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

（四）主從式多UUV群體的運(yùn)動(dòng)學(xué)分析

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

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

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

三、導(dǎo)航算法

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

（一）算法一

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

（二）算法二

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

四、觀測(cè)值的確定

（一）三信標(biāo)

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

（二）單信標(biāo)

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

（三）異常值剔除

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

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

五、實(shí)驗(yàn)結(jié)果與分析

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

六、結(jié)論

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

參考文獻(xiàn)：

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

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

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

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

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

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