基于自適應殘差加權的AUV集群分布式協同定位算法

何桂萍，王正偉，陳 洲

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

0 引 言

協同思想在多機器人定位、無線傳感網絡定位以及無線移動網絡定位等領域得到了廣泛的研究和發展。隨著水下聲學通信技術的發展，基于水聲通信網絡的多自主水下航行器（AUV）協同定位技術得到了越來越多的關注。GNSS信號在水下環境高度衰減，水深超過幾米的情況下無法使用。隨著AUV技術的發展以及某些特殊的使命任務需求，進一步增強AUV的導航自主性和隱蔽性，減少利用水面錨節點，采用水下航行器進行協同定位正逐漸成為當前水下定位導航領域的研究熱點。

采用AUV協同定位不僅可以提高AUV集群整體位置精度和定位魯棒性，而且實現簡單、成本低、可靠性好，且不受AUV活動區域限制，為多AUV協同應用提供重要的技術保障，具有重要的應用價值。

本文提出一種基于自適應殘差加權的AUV集群分布式協同定位算法（ARW- UWASN）。集群網絡內領航AUV可獲得高精度的位置信息，AUV之間可進行通信和距離測量，采用協同定位算法對網絡內其余攜帶低精度導航裝置的普通AUV進行定位，更新低精度AUV位置信息，從而逐步確定網絡中每個AUV節點的精確位置。該算法可以提高AUV集群網絡范圍內普通AUV節點的定位精度和定位覆蓋率，減小節點能量消耗，為水下移動節點協同定位導航的應用與發展提供理論支撐。

1 水下移動節點定位技術

1.1 水下移動節點定位方法

根據水下定位技術的原理不同，目前水下定位方法主要分為3類：基于水下聲學傳感器網絡（underwater acoustic sensor network，UWASN）的聲學定位、基于地理信息匹配的定位（simultaneous localization and mapping，SLAM）和基于慣性導航系統的定位（inertial navigation system，INS）。基本上，水聲定位技術有兩大類，即基于距離的定位和無距離的定位。基于距離的定位算法，首先使用TDOA，TOA，AOA和RSSI估計到一些錨定傳感器節點的距離或角度，然后通過三邊測量（2D）或四邊形測量（3D）將這些距離估計轉換為位置信息；無距離算法查詢傳感器節點的局部拓撲和位置估計，這些位置估計是從附近的錨傳感器節點獲得。

本文采用基于距離測量的UWASN聲學定位技術，定位算法分為普通AUV節點定位和領航AUV節點定位。普通AUV節點接收來自領航AUV節點的信息，領航AUV通過浮出水面接收GNSS信息或者與水面浮標或海底潛標通信進行自定位。

1.2 協同定位網絡結構

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

AUV利用航位推算方法可以在水下自定位，領航AUV采用昂貴的慣性導航進行航位推算，在短時間內可以保持高精度的位置信息。本文協同定位是一種分布式的、基于距離估計的定位協議。在AUV運行周期中，領航AUV每間隔時間廣播其位置信息，廣播格式定義為{Id；SendTime；Type；Weight；Location}，其中Id為節點的唯一身份標識，SendTime為廣播時刻，Type為AUV節點類型，Weight為節點權重，Location為節點位置信息。假設網絡內節點是同步的，在接收到多個領航AUV信息后，普通AUV節點可以估計它們自身的位置坐標。

2 AUV集群節點分布式逐級協同定位方案

由于水下AUV節點分布區域的廣泛性和節點的移動性，不是所有普通節點都能有效地接收到領航節點的位置廣播信息，當普通AUV節點不能接收到足夠的領航AUV節點廣播信息時，通常采取的方法為：

1）等待接收足夠數量領航AUV節點位置信息再定位；

2）直接將已完成定位的普通AUV節點升級為具有精確位置信息的參考節點，接收其位置信息進行定位。

這2種常規方案都有其固有的缺點：方案1，當網絡中領航AUV節點數量較少時，可能存在大量普通AUV節點出現不能定位的情況；方案2，直接利用已完成定位的普通節點對待定位節點進行定位，可能會因為已定位的普通節點存在較大的誤差，導致待定位節點出現更大的自定位誤差。

因此，針對上述缺點，為了使待定位AUV獲得更高的定位精度和更大的定位覆蓋率，本文提出一種分布式逐級定位方案，采用基于自適應殘差加權的AUV集群分布式協同定位算法（ARW- UWASN），利用所有可以獲得的領航AUV和位置誤差投影統計值達標的已定位AUV節點（參考節點）位置信息對待定位AUV進行位置估計，每3組領航AUV和參考AUV節點可定位估計出一組待定位AUV的潛在位置，一個待定位AUV節點的位置可以表示為所有潛在位置估計的加權平均值，使用殘差權重來計算最終的定位結果。

AUV集群網絡分布式逐級定位方案的結構如圖1所示。其中的節點主要包括：領航AUV節點、普通待定位AUV節點、參考AUV節點。領航AUV攜帶高精度的導航裝備，可在一定時間內獲得自身高精度參考位置，在一定時間后位置信息精度降低時，到達水面從GPS或者北斗接收定位信息，對自身位置信息進行校準，然后再潛入水中進行航位推算，在水下進行自定位導航。同時領航AUV可以直接與水面浮標和海底潛標進行通信，獲得實時高精度位置信息，從而進行自身位置估計。普通AUV價格便宜，復雜度低，不能浮出水面進行GNSS同步，也不能直接與浮標或潛標通信，只能與相鄰的AUV節點通信。

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

AUV集群網絡范圍內每個待定位AUV節點的定位過程如圖2所示。

圖2 網絡中普通AUV節點的定位過程Fig. 2 Localization process of ordinary AUV nodes in network

該算法主要包括兩部分：

1）可以直接與領航AUV通信的普通AUV節點的定位

可直接接收到領航節點信息的定位方式如圖1中待定位AUV節點A所處區域所示，待定位節點A偵收到領航節點AUV1，AUV2，AUV3發送的廣播信息，直接根據其接收到的信息計算自身的位置坐標值。

2）無法直接與領航AUV通信的普通AUV節點的定位

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

這樣，先將距離領航節點通信半徑范圍內的待定位節點完成定位，隨后定位出下一通信半徑距離的待定位節點，直至網絡內所有待定位節點完成定位。

3 基于自適應殘差加權的AUV集群協同定位算法

3.1 待定位節點潛在位置估計

潛在位置估計是利用領航或參考AUV節點與待定位AUV節點的測距求解位置信息。根據速度和廣播信息的始末時間計算出節點的距離后，可列出如下方程：

式中： (x,y,z) 為第個領航節點或參考節點的坐標，(,,)為待定位的普通節點的坐標，為待定位節點能夠接收到的節點廣播的數量，d為 參考節點與待定位節點的距離。

AUV深度可由深度計獲得，上述方程只需要3組便可解得一組待定位普通AUV節點的位置坐標。

3.2 自適應殘差加權協同定位算法

對于維測量殘差序列ζ和預測殘差序列λ，采用加權函數后的均值和協方差分別表示為：

式中：ω和 ω分別為第個潛在位置測量殘差和預測殘差的權值系數，采用投影統計法計算位置誤差統計值。具體方法如下：

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

2）計算每一個單獨數據向量h相對于中位數的單位向量

3）標準化投影

對于每一個單位方向向量v

①將數據向量h投影到每一個單位向量v

②對于每一個單位方向上的投影值進行中位數求取

③計算中位數絕對偏差

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

④計算標準化投影

4）計算投影統計

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

利用加權估計，待定位節點的位置估計為：

如果待定位節點收到的領航和參考節點的信息數量少于3時，則無法通過以上算法估算出位置信息，需要等待下一個定位周期，待收到領航和參考節點的信息數大于或等于3時再進行定位。

4 仿真與結果分析

利用Matlab仿真軟件對提出的ARW- UWASN定位算法進行仿真，評估算法性能，并與常規非加權自適應定位算法（UWASN）進行比較。

設置領航AUV節點自定位位置誤差為2 m。將全部領航AUV節點與待定位AUV節點隨機的分布于1000 m×1000 m的區域。設置網絡中AUV節點總數量為200個，其中，領航AUV節點比例為0.2（數量為40個），待定位普通AUV節點數量為160個。網絡中節點部署情況和節點鄰居關系圖如圖3和圖4所示。

根據上述AUV網絡節點部署情況，對普通待定位節點進行定位仿真，采用平均定位誤差來評估本文算法的定位性能，平均定位誤差定義為估計位置到真實位置的歐式距離與通信半徑的比值，計算公式如下：

圖3 網絡中節點部署情況Fig. 3 Node deployment diagram on the network

圖4 網絡中節點鄰居關系示意圖Fig. 4 Node neighbor relationship in network

式中：_為平均定位誤差；為待定位AUV節點數量；為通信半徑。

從不同的領航AUV節點比例數量、節點的通信半徑大小和網絡內節點總數3個方面，分析定位算法對定位精度和定位覆蓋范圍的影響，仿真條件同上。

1）領航AUV節點數量比例的影響

圖5為領航AUV節點比例對平均定位誤差的影響，圖6為領航AUV節點比例對不可定位AUV節點數量的影響。

可以看出，在領航AUV節點比例逐漸增加時，2種算法的定位誤差都隨著領航節點比例增加而減小，但本文算法的定位誤差明顯小于常規算法，常規算法的平均定位誤差由0.011 4下降至0.003 0，而本文算法的平均定位誤差由0.005 2下降至0.001 8；本文算法不可定位節點數接近0，常規算法在領航AUV比例較小時，不可定位節點數較多。在領航AUV比例較小的情況下，本文的算法在定位精度和定位范圍上優勢更為明顯。

2）AUV節點通信半徑的影響

圖5 節點比例對平均定位誤差的影響Fig. 5 Impact of the ratio of nodes on average localization error

圖6 節點比例對不可定位節點數量的影響Fig. 6 Impact of node ratio on the number of unlocatable nodes

圖7為通信半徑對本文算法與常規算法的平均定位誤差影響圖，圖8為不可定位節點數對比圖。

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

3）節點總數的影響

圖9為節點總數對本文算法與常規算法的平均定位誤差的影響，圖10為不可定位節點數對比圖。

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

圖8 通信半徑對不可定位節點數量的影響Fig. 8 Impact of communication radius on the number of unlocatable nodes

圖9 節點總數對平均定位誤差的影響Fig. 9 Impact of total number of nodes on average localization error

圖10 節點總數對不可定位節點數量的影響Fig. 10 Impact of the total number of nodes on the number of unlocatable nodes

可以看出，隨著節點總數的增加，領航節點的數增多，待定位節點收到的參考節點的廣播信息增多，潛在位置估計次數上升，平均定位誤差逐漸降低。在節點數小于200時，雖然常規算法平均誤差小于本文算法，但是本文算法幾乎可以定位網絡內所有節點；常規算法在節點總數較少時，不可定位節點數量較多；本文算法在節點總數達到200左右，定位誤差趨于穩定。

綜上仿真結果表明，本文ARW-UWASN算法與常規UWASN算法相比，在上述3種影響因素下，本文算法均具有較高的定位精度和較好的定位覆蓋率，節點網絡內定位精度平均可提高約30%。

5 結 語

本文提出一種基于自適應殘差加權的分布式協同定位算法（ARW-UWASN），解決了常規算法如果領航AUV節點密度太小，部分待定位普通節點無法接收到領航節點信號，導致部分待定位節點始終無法定位的問題。本文采用逐級定位解決領航AUV節點密度稀疏導致無法定位的問題，同時采用自適應殘差加權算法，結合參考節點位置信息實現對待定位節點位置的加權估計。仿真結果表明，本文算法與常規算法相比，減小了節點能量消耗，有效提高了定位精度和定位覆蓋率。