基于自適應(yīng)殘差加權(quán)的AUV集群分布式協(xié)同定位算法

何桂萍，王正偉，陳 洲

(四川九洲電器集團(tuán)有限責(zé)任公司，四川 成都 610000)

0 引 言

協(xié)同思想在多機(jī)器人定位、無(wú)線傳感網(wǎng)絡(luò)定位以及無(wú)線移動(dòng)網(wǎng)絡(luò)定位等領(lǐng)域得到了廣泛的研究和發(fā)展。隨著水下聲學(xué)通信技術(shù)的發(fā)展，基于水聲通信網(wǎng)絡(luò)的多自主水下航行器（AUV）協(xié)同定位技術(shù)得到了越來(lái)越多的關(guān)注。GNSS信號(hào)在水下環(huán)境高度衰減，水深超過(guò)幾米的情況下無(wú)法使用。隨著AUV技術(shù)的發(fā)展以及某些特殊的使命任務(wù)需求，進(jìn)一步增強(qiáng)AUV的導(dǎo)航自主性和隱蔽性，減少利用水面錨節(jié)點(diǎn)，采用水下航行器進(jìn)行協(xié)同定位正逐漸成為當(dāng)前水下定位導(dǎo)航領(lǐng)域的研究熱點(diǎn)。

采用AUV協(xié)同定位不僅可以提高AUV集群整體位置精度和定位魯棒性，而且實(shí)現(xiàn)簡(jiǎn)單、成本低、可靠性好，且不受AUV活動(dòng)區(qū)域限制，為多AUV協(xié)同應(yīng)用提供重要的技術(shù)保障，具有重要的應(yīng)用價(jià)值。

本文提出一種基于自適應(yīng)殘差加權(quán)的AUV集群分布式協(xié)同定位算法（ARW- UWASN）。集群網(wǎng)絡(luò)內(nèi)領(lǐng)航AUV可獲得高精度的位置信息，AUV之間可進(jìn)行通信和距離測(cè)量，采用協(xié)同定位算法對(duì)網(wǎng)絡(luò)內(nèi)其余攜帶低精度導(dǎo)航裝置的普通AUV進(jìn)行定位，更新低精度AUV位置信息，從而逐步確定網(wǎng)絡(luò)中每個(gè)AUV節(jié)點(diǎn)的精確位置。該算法可以提高AUV集群網(wǎng)絡(luò)范圍內(nèi)普通AUV節(jié)點(diǎn)的定位精度和定位覆蓋率，減小節(jié)點(diǎn)能量消耗，為水下移動(dòng)節(jié)點(diǎn)協(xié)同定位導(dǎo)航的應(yīng)用與發(fā)展提供理論支撐。

1 水下移動(dòng)節(jié)點(diǎn)定位技術(shù)

1.1 水下移動(dòng)節(jié)點(diǎn)定位方法

根據(jù)水下定位技術(shù)的原理不同，目前水下定位方法主要分為3類：基于水下聲學(xué)傳感器網(wǎng)絡(luò)（underwater acoustic sensor network，UWASN）的聲學(xué)定位、基于地理信息匹配的定位（simultaneous localization and mapping，SLAM）和基于慣性導(dǎo)航系統(tǒng)的定位（inertial navigation system，INS）?；旧?，水聲定位技術(shù)有兩大類，即基于距離的定位和無(wú)距離的定位?；诰嚯x的定位算法，首先使用TDOA，TOA，AOA和RSSI估計(jì)到一些錨定傳感器節(jié)點(diǎn)的距離或角度，然后通過(guò)三邊測(cè)量（2D）或四邊形測(cè)量（3D）將這些距離估計(jì)轉(zhuǎn)換為位置信息；無(wú)距離算法查詢傳感器節(jié)點(diǎn)的局部拓?fù)浜臀恢霉烙?jì)，這些位置估計(jì)是從附近的錨傳感器節(jié)點(diǎn)獲得。

本文采用基于距離測(cè)量的UWASN聲學(xué)定位技術(shù)，定位算法分為普通AUV節(jié)點(diǎn)定位和領(lǐng)航AUV節(jié)點(diǎn)定位。普通AUV節(jié)點(diǎn)接收來(lái)自領(lǐng)航AUV節(jié)點(diǎn)的信息，領(lǐng)航AUV通過(guò)浮出水面接收GNSS信息或者與水面浮標(biāo)或海底潛標(biāo)通信進(jìn)行自定位。

1.2 協(xié)同定位網(wǎng)絡(luò)結(jié)構(gòu)

水下AUV節(jié)點(diǎn)由于受到通信距離和供能的影響，需要采取有效的定位方式，在有限的通信范圍內(nèi)，不過(guò)多消耗節(jié)點(diǎn)能量的同時(shí)保證定位的精度和覆蓋范圍。因此本文采用分布式定位方案，每個(gè)傳感器節(jié)點(diǎn)單獨(dú)定位或節(jié)點(diǎn)自由定位，然后將這些數(shù)據(jù)分別發(fā)送給參考節(jié)點(diǎn)。

AUV利用航位推算方法可以在水下自定位，領(lǐng)航AUV采用昂貴的慣性導(dǎo)航進(jìn)行航位推算，在短時(shí)間內(nèi)可以保持高精度的位置信息。本文協(xié)同定位是一種分布式的、基于距離估計(jì)的定位協(xié)議。在AUV運(yùn)行周期中，領(lǐng)航AUV每間隔時(shí)間廣播其位置信息，廣播格式定義為{Id；SendTime；Type；Weight；Location}，其中Id為節(jié)點(diǎn)的唯一身份標(biāo)識(shí)，SendTime為廣播時(shí)刻，Type為AUV節(jié)點(diǎn)類型，Weight為節(jié)點(diǎn)權(quán)重，Location為節(jié)點(diǎn)位置信息。假設(shè)網(wǎng)絡(luò)內(nèi)節(jié)點(diǎn)是同步的，在接收到多個(gè)領(lǐng)航AUV信息后，普通AUV節(jié)點(diǎn)可以估計(jì)它們自身的位置坐標(biāo)。

2 AUV集群節(jié)點(diǎn)分布式逐級(jí)協(xié)同定位方案

由于水下AUV節(jié)點(diǎn)分布區(qū)域的廣泛性和節(jié)點(diǎn)的移動(dòng)性，不是所有普通節(jié)點(diǎn)都能有效地接收到領(lǐng)航節(jié)點(diǎn)的位置廣播信息，當(dāng)普通AUV節(jié)點(diǎn)不能接收到足夠的領(lǐng)航AUV節(jié)點(diǎn)廣播信息時(shí)，通常采取的方法為：

1）等待接收足夠數(shù)量領(lǐng)航AUV節(jié)點(diǎn)位置信息再定位；

2）直接將已完成定位的普通AUV節(jié)點(diǎn)升級(jí)為具有精確位置信息的參考節(jié)點(diǎn)，接收其位置信息進(jìn)行定位。

這2種常規(guī)方案都有其固有的缺點(diǎn)：方案1，當(dāng)網(wǎng)絡(luò)中領(lǐng)航AUV節(jié)點(diǎn)數(shù)量較少時(shí)，可能存在大量普通AUV節(jié)點(diǎn)出現(xiàn)不能定位的情況；方案2，直接利用已完成定位的普通節(jié)點(diǎn)對(duì)待定位節(jié)點(diǎn)進(jìn)行定位，可能會(huì)因?yàn)橐讯ㄎ坏钠胀ü?jié)點(diǎn)存在較大的誤差，導(dǎo)致待定位節(jié)點(diǎn)出現(xiàn)更大的自定位誤差。

因此，針對(duì)上述缺點(diǎn)，為了使待定位AUV獲得更高的定位精度和更大的定位覆蓋率，本文提出一種分布式逐級(jí)定位方案，采用基于自適應(yīng)殘差加權(quán)的AUV集群分布式協(xié)同定位算法（ARW- UWASN），利用所有可以獲得的領(lǐng)航AUV和位置誤差投影統(tǒng)計(jì)值達(dá)標(biāo)的已定位AUV節(jié)點(diǎn)（參考節(jié)點(diǎn)）位置信息對(duì)待定位AUV進(jìn)行位置估計(jì)，每3組領(lǐng)航AUV和參考AUV節(jié)點(diǎn)可定位估計(jì)出一組待定位AUV的潛在位置，一個(gè)待定位AUV節(jié)點(diǎn)的位置可以表示為所有潛在位置估計(jì)的加權(quán)平均值，使用殘差權(quán)重來(lái)計(jì)算最終的定位結(jié)果。

AUV集群網(wǎng)絡(luò)分布式逐級(jí)定位方案的結(jié)構(gòu)如圖1所示。其中的節(jié)點(diǎn)主要包括：領(lǐng)航AUV節(jié)點(diǎn)、普通待定位AUV節(jié)點(diǎn)、參考AUV節(jié)點(diǎn)。領(lǐng)航AUV攜帶高精度的導(dǎo)航裝備，可在一定時(shí)間內(nèi)獲得自身高精度參考位置，在一定時(shí)間后位置信息精度降低時(shí)，到達(dá)水面從GPS或者北斗接收定位信息，對(duì)自身位置信息進(jìn)行校準(zhǔn)，然后再潛入水中進(jìn)行航位推算，在水下進(jìn)行自定位導(dǎo)航。同時(shí)領(lǐng)航AUV可以直接與水面浮標(biāo)和海底潛標(biāo)進(jìn)行通信，獲得實(shí)時(shí)高精度位置信息，從而進(jìn)行自身位置估計(jì)。普通AUV價(jià)格便宜，復(fù)雜度低，不能浮出水面進(jìn)行GNSS同步，也不能直接與浮標(biāo)或潛標(biāo)通信，只能與相鄰的AUV節(jié)點(diǎn)通信。

圖1 AUV集群分布式逐級(jí)協(xié)同定位方案Fig. 1 AUV cluster distributed hierarchical cooperative localization method

AUV集群網(wǎng)絡(luò)范圍內(nèi)每個(gè)待定位AUV節(jié)點(diǎn)的定位過(guò)程如圖2所示。

圖2 網(wǎng)絡(luò)中普通AUV節(jié)點(diǎn)的定位過(guò)程Fig. 2 Localization process of ordinary AUV nodes in network

該算法主要包括兩部分：

1）可以直接與領(lǐng)航AUV通信的普通AUV節(jié)點(diǎn)的定位

可直接接收到領(lǐng)航節(jié)點(diǎn)信息的定位方式如圖1中待定位AUV節(jié)點(diǎn)A所處區(qū)域所示，待定位節(jié)點(diǎn)A偵收到領(lǐng)航節(jié)點(diǎn)AUV1，AUV2，AUV3發(fā)送的廣播信息，直接根據(jù)其接收到的信息計(jì)算自身的位置坐標(biāo)值。

2）無(wú)法直接與領(lǐng)航AUV通信的普通AUV節(jié)點(diǎn)的定位

不是每個(gè)待定位節(jié)點(diǎn)都能接收到3個(gè)或更多領(lǐng)航節(jié)點(diǎn)信息，在定位了普通待定位節(jié)點(diǎn)，普通節(jié)點(diǎn)位置誤差統(tǒng)計(jì)值小于位置精度閾值后，可將其升級(jí)為新的參考節(jié)點(diǎn)對(duì)周圍其他待定位普通節(jié)點(diǎn)進(jìn)行定位。如圖1中AUV節(jié)點(diǎn)B所處區(qū)域所示，節(jié)點(diǎn)B與其他領(lǐng)航節(jié)點(diǎn)的距離超過(guò)通信半徑范圍，因此在逐級(jí)定位的過(guò)程中，將已完成定位的普通節(jié)點(diǎn)AUV4和AUV5升級(jí)為參考節(jié)點(diǎn)，利用1個(gè)領(lǐng)航節(jié)點(diǎn)和2個(gè)參考節(jié)點(diǎn)，根據(jù)與領(lǐng)航節(jié)點(diǎn)和參考節(jié)點(diǎn)的相對(duì)關(guān)系計(jì)算出自身位置值。

這樣，先將距離領(lǐng)航節(jié)點(diǎn)通信半徑范圍內(nèi)的待定位節(jié)點(diǎn)完成定位，隨后定位出下一通信半徑距離的待定位節(jié)點(diǎn)，直至網(wǎng)絡(luò)內(nèi)所有待定位節(jié)點(diǎn)完成定位。

3 基于自適應(yīng)殘差加權(quán)的AUV集群協(xié)同定位算法

3.1 待定位節(jié)點(diǎn)潛在位置估計(jì)

潛在位置估計(jì)是利用領(lǐng)航或參考AUV節(jié)點(diǎn)與待定位AUV節(jié)點(diǎn)的測(cè)距求解位置信息。根據(jù)速度和廣播信息的始末時(shí)間計(jì)算出節(jié)點(diǎn)的距離后，可列出如下方程：

式中： (x,y,z) 為第個(gè)領(lǐng)航節(jié)點(diǎn)或參考節(jié)點(diǎn)的坐標(biāo)，(,,)為待定位的普通節(jié)點(diǎn)的坐標(biāo)，為待定位節(jié)點(diǎn)能夠接收到的節(jié)點(diǎn)廣播的數(shù)量，d為 參考節(jié)點(diǎn)與待定位節(jié)點(diǎn)的距離。

AUV深度可由深度計(jì)獲得，上述方程只需要3組便可解得一組待定位普通AUV節(jié)點(diǎn)的位置坐標(biāo)。

3.2 自適應(yīng)殘差加權(quán)協(xié)同定位算法

對(duì)于維測(cè)量殘差序列ζ和預(yù)測(cè)殘差序列λ，采用加權(quán)函數(shù)后的均值和協(xié)方差分別表示為：

式中：ω和 ω分別為第個(gè)潛在位置測(cè)量殘差和預(yù)測(cè)殘差的權(quán)值系數(shù)，采用投影統(tǒng)計(jì)法計(jì)算位置誤差統(tǒng)計(jì)值。具體方法如下：

1）求取矩陣 [···h]列空間中位數(shù)

2）計(jì)算每一個(gè)單獨(dú)數(shù)據(jù)向量h相對(duì)于中位數(shù)的單位向量

3）標(biāo)準(zhǔn)化投影

對(duì)于每一個(gè)單位方向向量v

①將數(shù)據(jù)向量h投影到每一個(gè)單位向量v

②對(duì)于每一個(gè)單位方向上的投影值進(jìn)行中位數(shù)求取

③計(jì)算中位數(shù)絕對(duì)偏差

式中：的值為1.482 6，是高斯噪聲分布下的費(fèi)舍爾一致性校正因子，用以保證結(jié)果的無(wú)偏性。

④計(jì)算標(biāo)準(zhǔn)化投影

4）計(jì)算投影統(tǒng)計(jì)

得到的投影統(tǒng)計(jì)值表示結(jié)果偏離樣本的程度大小，通常取χ分布97.5%置信度為異常判斷閾，在進(jìn)行位置估計(jì)結(jié)果異常值判斷的時(shí)候，如果計(jì)算得到的位置誤差投影統(tǒng)計(jì)值大于判斷閾，可以認(rèn)為該位置估計(jì)結(jié)果為一異常值，將不能用于下一步的定位計(jì)算。

利用加權(quán)估計(jì)，待定位節(jié)點(diǎn)的位置估計(jì)為：

如果待定位節(jié)點(diǎn)收到的領(lǐng)航和參考節(jié)點(diǎn)的信息數(shù)量少于3時(shí)，則無(wú)法通過(guò)以上算法估算出位置信息，需要等待下一個(gè)定位周期，待收到領(lǐng)航和參考節(jié)點(diǎn)的信息數(shù)大于或等于3時(shí)再進(jìn)行定位。

4 仿真與結(jié)果分析

利用Matlab仿真軟件對(duì)提出的ARW- UWASN定位算法進(jìn)行仿真，評(píng)估算法性能，并與常規(guī)非加權(quán)自適應(yīng)定位算法（UWASN）進(jìn)行比較。

設(shè)置領(lǐng)航AUV節(jié)點(diǎn)自定位位置誤差為2 m。將全部領(lǐng)航AUV節(jié)點(diǎn)與待定位AUV節(jié)點(diǎn)隨機(jī)的分布于1000 m×1000 m的區(qū)域。設(shè)置網(wǎng)絡(luò)中AUV節(jié)點(diǎn)總數(shù)量為200個(gè)，其中，領(lǐng)航AUV節(jié)點(diǎn)比例為0.2（數(shù)量為40個(gè)），待定位普通AUV節(jié)點(diǎn)數(shù)量為160個(gè)。網(wǎng)絡(luò)中節(jié)點(diǎn)部署情況和節(jié)點(diǎn)鄰居關(guān)系圖如圖3和圖4所示。

根據(jù)上述AUV網(wǎng)絡(luò)節(jié)點(diǎn)部署情況，對(duì)普通待定位節(jié)點(diǎn)進(jìn)行定位仿真，采用平均定位誤差來(lái)評(píng)估本文算法的定位性能，平均定位誤差定義為估計(jì)位置到真實(shí)位置的歐式距離與通信半徑的比值，計(jì)算公式如下：

圖3 網(wǎng)絡(luò)中節(jié)點(diǎn)部署情況Fig. 3 Node deployment diagram on the network

圖4 網(wǎng)絡(luò)中節(jié)點(diǎn)鄰居關(guān)系示意圖Fig. 4 Node neighbor relationship in network

式中：_為平均定位誤差；為待定位AUV節(jié)點(diǎn)數(shù)量；為通信半徑。

從不同的領(lǐng)航AUV節(jié)點(diǎn)比例數(shù)量、節(jié)點(diǎn)的通信半徑大小和網(wǎng)絡(luò)內(nèi)節(jié)點(diǎn)總數(shù)3個(gè)方面，分析定位算法對(duì)定位精度和定位覆蓋范圍的影響，仿真條件同上。

1）領(lǐng)航AUV節(jié)點(diǎn)數(shù)量比例的影響

圖5為領(lǐng)航AUV節(jié)點(diǎn)比例對(duì)平均定位誤差的影響，圖6為領(lǐng)航AUV節(jié)點(diǎn)比例對(duì)不可定位AUV節(jié)點(diǎn)數(shù)量的影響。

可以看出，在領(lǐng)航AUV節(jié)點(diǎn)比例逐漸增加時(shí)，2種算法的定位誤差都隨著領(lǐng)航節(jié)點(diǎn)比例增加而減小，但本文算法的定位誤差明顯小于常規(guī)算法，常規(guī)算法的平均定位誤差由0.011 4下降至0.003 0，而本文算法的平均定位誤差由0.005 2下降至0.001 8；本文算法不可定位節(jié)點(diǎn)數(shù)接近0，常規(guī)算法在領(lǐng)航AUV比例較小時(shí)，不可定位節(jié)點(diǎn)數(shù)較多。在領(lǐng)航AUV比例較小的情況下，本文的算法在定位精度和定位范圍上優(yōu)勢(shì)更為明顯。

2）AUV節(jié)點(diǎn)通信半徑的影響

圖5 節(jié)點(diǎn)比例對(duì)平均定位誤差的影響Fig. 5 Impact of the ratio of nodes on average localization error

圖6 節(jié)點(diǎn)比例對(duì)不可定位節(jié)點(diǎn)數(shù)量的影響Fig. 6 Impact of node ratio on the number of unlocatable nodes

圖7為通信半徑對(duì)本文算法與常規(guī)算法的平均定位誤差影響圖，圖8為不可定位節(jié)點(diǎn)數(shù)對(duì)比圖。

可以看出，隨著通信半徑從的增大，本文算法的平均定位誤差始終優(yōu)于常規(guī)算法；本文算法所有節(jié)點(diǎn)均可定位，常規(guī)算法不可定位節(jié)點(diǎn)數(shù)逐漸減少并趨于0。因此，本文算法在提高定位精度的同時(shí)，可以減小AUV節(jié)點(diǎn)的通信距離，減小節(jié)點(diǎn)能量消耗。

3）節(jié)點(diǎn)總數(shù)的影響

圖9為節(jié)點(diǎn)總數(shù)對(duì)本文算法與常規(guī)算法的平均定位誤差的影響，圖10為不可定位節(jié)點(diǎn)數(shù)對(duì)比圖。

圖7 通信半徑對(duì)平均定位誤差的影響Fig. 7 Impact of communication radius on average localization error

圖8 通信半徑對(duì)不可定位節(jié)點(diǎn)數(shù)量的影響Fig. 8 Impact of communication radius on the number of unlocatable nodes

圖9 節(jié)點(diǎn)總數(shù)對(duì)平均定位誤差的影響Fig. 9 Impact of total number of nodes on average localization error

圖10 節(jié)點(diǎn)總數(shù)對(duì)不可定位節(jié)點(diǎn)數(shù)量的影響Fig. 10 Impact of the total number of nodes on the number of unlocatable nodes

可以看出，隨著節(jié)點(diǎn)總數(shù)的增加，領(lǐng)航節(jié)點(diǎn)的數(shù)增多，待定位節(jié)點(diǎn)收到的參考節(jié)點(diǎn)的廣播信息增多，潛在位置估計(jì)次數(shù)上升，平均定位誤差逐漸降低。在節(jié)點(diǎn)數(shù)小于200時(shí)，雖然常規(guī)算法平均誤差小于本文算法，但是本文算法幾乎可以定位網(wǎng)絡(luò)內(nèi)所有節(jié)點(diǎn)；常規(guī)算法在節(jié)點(diǎn)總數(shù)較少時(shí)，不可定位節(jié)點(diǎn)數(shù)量較多；本文算法在節(jié)點(diǎn)總數(shù)達(dá)到200左右，定位誤差趨于穩(wěn)定。

綜上仿真結(jié)果表明，本文ARW-UWASN算法與常規(guī)UWASN算法相比，在上述3種影響因素下，本文算法均具有較高的定位精度和較好的定位覆蓋率，節(jié)點(diǎn)網(wǎng)絡(luò)內(nèi)定位精度平均可提高約30%。

5 結(jié) 語(yǔ)

本文提出一種基于自適應(yīng)殘差加權(quán)的分布式協(xié)同定位算法（ARW-UWASN），解決了常規(guī)算法如果領(lǐng)航AUV節(jié)點(diǎn)密度太小，部分待定位普通節(jié)點(diǎn)無(wú)法接收到領(lǐng)航節(jié)點(diǎn)信號(hào)，導(dǎo)致部分待定位節(jié)點(diǎn)始終無(wú)法定位的問(wèn)題。本文采用逐級(jí)定位解決領(lǐng)航AUV節(jié)點(diǎn)密度稀疏導(dǎo)致無(wú)法定位的問(wèn)題，同時(shí)采用自適應(yīng)殘差加權(quán)算法，結(jié)合參考節(jié)點(diǎn)位置信息實(shí)現(xiàn)對(duì)待定位節(jié)點(diǎn)位置的加權(quán)估計(jì)。仿真結(jié)果表明，本文算法與常規(guī)算法相比，減小了節(jié)點(diǎn)能量消耗，有效提高了定位精度和定位覆蓋率。