動態(tài)網(wǎng)絡(luò)協(xié)同定位方法研究

摘 要：協(xié)同定位技術(shù)可以通過多個節(jié)點之間的時空協(xié)同來提高系統(tǒng)及節(jié)點的整體定位精度，在固有導航定位設(shè)施無法覆蓋的區(qū)域有著廣闊的應(yīng)用需求，但在實際應(yīng)用過程中，系統(tǒng)中的節(jié)點始終處于運動狀態(tài)，常常會面臨場景切換問題，可能出現(xiàn)錨節(jié)點與普通節(jié)點角色變化、節(jié)點之間的測距值的消失或產(chǎn)生、節(jié)點隨機接入與退出網(wǎng)絡(luò)等情況，導致協(xié)同定位算法精度下降乃至無法正常工作的情況。針對上述問題，開展了協(xié)同定位方法研究，并提出了自適應(yīng)調(diào)整觀測誤差協(xié)方差矩陣的濾波方法與聯(lián)邦濾波方法融合，實現(xiàn)了在移動節(jié)點間錨節(jié)點與普通節(jié)點變換，節(jié)點間測距值消失與出現(xiàn)、節(jié)點的隨機接入與退出等情況下協(xié)同定位系統(tǒng)正常工作，仿真結(jié)果表明，由5個普通節(jié)點組成的協(xié)同系統(tǒng)定位精度達到了3.6m，相比于航位推算定位精度10.702m提升了197.2%，并通過仿真設(shè)定，驗證了所提動態(tài)協(xié)同定位方法的有效性。

關(guān)鍵詞：動態(tài)網(wǎng)絡(luò)；協(xié)同；導航定位；卡爾曼濾波

DOI：10.15938/j.jhust.2024.06.012

中圖分類號： TP751

文獻標志碼： A

文章編號： 1007-2683（2024）06-0124-10

Research on Cooperative Localization Method in Dynamic Networks

ZHANG Zhihua1， SUN Bin1， LI Bingchen1， ZHOU Zhongfu1， SHEN Feng2

（1.Transmission Branch， Yunnan Power Grid Co.， Ltd.， Kunming 650000， China;

2.School of Instrumentation Science and Engeering Harbin Institute of Technology， Harbin 150001， China）

Abstract：Cooperative localization technology can improve the overall positioning accuracy of the system and nodes through spatial and temporal cooperation among multiple nodes， and it has a broad application demand in the areas that cannot be covered by the inherent navigation and positioning facilities. However， in the actual application process， the nodes in the system are always in motion， and often face the problem of scenario switching. This may lead to changes in roles between anchor nodes and ordinary nodes， the disappearance or emergence of ranging values between nodes， as well as nodes randomly accessing or exiting the network. These circumstances can result in a decrease in the accuracy of cooperative localization algorithms or even cause them to malfunction. In order to address the above problems， this paper carries out the research on cooperative localization method， and proposes the filtering method of adaptive adjustment of observation error covariance matrix and the fusion of the federated filtering method， which enables the cooperative localization system to work normally in the case of the change of anchor nodes and ordinary nodes among mobile nodes， the disappearance or emergence of ranging values between nodes， and the random access and exit of nodes， etc. Simulation results show that the cooperative localization system composed of five ordinary nodes achieves a localization accuracy of 3.6m， which represents a 197.2% improvement compared to the dead reckoning localization accuracy of 10.702 meters.， and verifies the validity of the proposed dynamic cooperative localization method through the simulation setting.

Keywords：dynamic network; collaboration; navigation and localization; Kalman filtering

收稿日期： 2023-03-30

基金項目： 國家自然科學基金（61673128；61573117）.

作者簡介：

張智華（1980—），男，碩士，高級工程師；

孫 斌（1988—），男，工程師.

通信作者：

沈 鋒（1981—），男，博士，教授，E-mail：sf407@126.com.

0 引 言

目前智能無人設(shè)備發(fā)展迅速，在復雜、陌生環(huán)境中得到越來越多的應(yīng)用，其中高精度定位能力又是環(huán)境感知的基礎(chǔ)，其精度將直接影響系統(tǒng)執(zhí)行任務(wù)的效率。諸如全球?qū)Ш叫l(wèi)星系統(tǒng)（global navigation satellite system，GNSS）、基站定位等固有導航定位基礎(chǔ)設(shè)施將無法使用［1-2］，而不依賴外界設(shè)施的高精度慣性導航系統(tǒng)存在體積大、成本高、存在累計誤差等問題［3］，無法在每個無人平臺上部署。因此，如何充分利用各智能體之間搭載的傳感器，通過協(xié)同定位技術(shù)實現(xiàn)節(jié)點間的時空合作與信息融合［4］，提高復雜惡劣環(huán)境下無人平臺的定位精度，對保證任務(wù)執(zhí)行重要意義［5-7］。

目前常見的導航定位系統(tǒng)包括有我國建設(shè)的北斗衛(wèi)星導航系統(tǒng)（beidou navigation satellite system，BDS）、美國建設(shè)的全球定位系統(tǒng)等為代表的全球?qū)Ш叫l(wèi)星系統(tǒng)、以捷聯(lián)慣導系統(tǒng)（strap-down inertial navigation system，SINS）為代表的慣性導航系統(tǒng)、越發(fā)受到重視的超寬帶（ultra wide band，UWB）定位系統(tǒng)等。

隨著BDS的三期建設(shè)完成，其開始為全球用戶提供全天候、全天時、高精度的定位、導航、授時服務(wù)。目前，在BDS信號接收良好的環(huán)境下，全球位置精度因子可用性優(yōu)于85%，空間信號測距誤差可以達到0.48m（RMS）［9］。BDS作為我國的重要時空基礎(chǔ)設(shè)施，在軍民領(lǐng)域發(fā)揮著不可替代的作用［10］。然而衛(wèi)星導航定位系統(tǒng)仍存在其局限性，在城市環(huán)境下，高樓之間的城市峽谷效應(yīng)、隧道、高架橋等因素嚴重影響衛(wèi)星信號質(zhì)量，導致定位精度下降乃至無法使用；而在復雜環(huán)境下，電磁干擾、峽谷、叢林等環(huán)境使得衛(wèi)星信號極易被干擾，對單位導航定位安全提出了嚴峻考驗。SINS有陀螺儀與加速度計組成，其敏感載體相對于慣性空間的三軸角速度和三軸加速度，通過計算得到載體的姿態(tài)角、速度以及位置信息［11］。其導航僅依賴于載體本身，不受外界環(huán)境影響，自主性強。但受限于SINS 工作原理，其在解算時會產(chǎn)生累計誤差，因此如果單獨長時間使用慣導，需要保證慣性測量單元具有極高的精度，目前高精度的慣性測量單元有光纖陀螺、激光陀螺等，例如，iXblue公司研制的MARINS M11光纖陀螺慣導系統(tǒng)實現(xiàn)了1N·m/15天的定位精度，光纖陀螺精度達到了2×5-5°/h，但是其重量達到了62kg，尺寸約為［12］，因此受限于其價格、體積、重量等因素，無法實現(xiàn)大規(guī)模應(yīng)用，而目前低成本的MEMS慣導設(shè)備誤差大，無法達到導航精度，目前主要作為航子航向基準系統(tǒng)為載體提供姿態(tài)等信息［12］。UWB技術(shù)開始于20世紀60年代，主要用于軍事雷達和低截獲率的無線系統(tǒng)中，得益于UWB信號在時域上的持續(xù)時間非常短，往往處在納秒級別，因此其具備傳輸速度快、低功率、抗干擾等優(yōu)點，在通訊與測距方面受到重視。目前，市面上成熟的超寬帶定位系統(tǒng)可以實現(xiàn)在理想環(huán)境下20cm的定位精度，同時具備近距離6m/s、遠距離800kb/s的信息傳輸能力，這在軍事環(huán)境應(yīng)用具備十分大的優(yōu)勢，但是，在復雜的應(yīng)用環(huán)境下，UWB定位仍面臨在多徑、非視距（no line of sight， NOLS）等因素干擾下，測距精度下降的問題，同時目前普遍使用的UWB定位技術(shù)需要提前布設(shè)基站，這在陌生的戰(zhàn)場環(huán)境無法輕易實現(xiàn)。

目前國內(nèi)外學者開展了一定的協(xié)同定位技術(shù)研究，文［13］提出了一種MDS協(xié)同定位方法，其將協(xié)同中各節(jié)點之間的距離信息發(fā)送到中心節(jié)點，在中心節(jié)點中建立位置解算方程，進而獲得各節(jié)點的位置信息，但是其存在計算量大，僅可以獲取節(jié)點間相對位置信息等問題；文［14］在此基礎(chǔ)上提出了一種MDS-MAP方法，其可以在MDS基礎(chǔ)上，通過錨節(jié)點位置階段得到其他節(jié)點的絕對位置；文［15］提出了一種在衛(wèi)星矩陣環(huán)境下多無人機協(xié)同定位方法，分析了基于多無人機集群協(xié)同定位的集中式卡爾曼濾波結(jié)構(gòu)。

但是在面對復雜環(huán)境以及無人平臺的快速機動，往往會導致傳感器信號存在各類偏置及誤差、節(jié)點及節(jié)點間的測觀量會突然消失或出現(xiàn)、節(jié)點隨機接入與退出協(xié)同系統(tǒng)，這些動態(tài)變化將會導致協(xié)同定位系統(tǒng)產(chǎn)生較大的誤差，甚至無法正常運行［16-19］。

因此針對上述問題，本文首先建立了協(xié)同定位算法，并提出了自適應(yīng)調(diào)整觀測誤差協(xié)方差矩陣的濾波方法與聯(lián)邦濾波方法融合的動態(tài)系統(tǒng)定位方法，實現(xiàn)移動節(jié)點間錨節(jié)點與普通節(jié)點變換，節(jié)點間測距值消失與出現(xiàn)、節(jié)點的隨機接入與退出等情況下協(xié)同定位系統(tǒng)正常工作。

1 協(xié)同定位算法

協(xié)同定位系統(tǒng)本質(zhì)上是通過節(jié)點間導航定位傳感器信息共享及時空信息融合，對節(jié)點的位置等狀態(tài)信息進行實時濾波的過程，由于節(jié)點及傳感器在運動及測量過程中存在各種噪聲，因此協(xié)同定位本質(zhì)上是從含有噪聲的信息中進行最優(yōu)估計的過程［20-25］。本章針對多節(jié)點協(xié)同定位問題，以集中式擴展卡爾曼濾波為研究對象，建立移動節(jié)點的運動模型，并在此基礎(chǔ)上進行多節(jié)點的協(xié)同定位濾波器設(shè)計。

1.1 協(xié)同網(wǎng)絡(luò)狀態(tài)方程

移動節(jié)點的運動狀態(tài)包括位置信息、姿態(tài)信息以及速度信息，由于本文僅考慮二維平面，系統(tǒng)節(jié)點的狀態(tài)如圖1所示。

2 動態(tài)協(xié)同定位方法實現(xiàn)

雖然建立了協(xié)同定位的擴展卡爾曼濾波算法，但是在面對復雜戰(zhàn)場環(huán)境以及作戰(zhàn)人員和無人平臺的快速機動，往往會導致節(jié)點及節(jié)點間的測觀量會突然消失或出現(xiàn)、節(jié)點隨機接入與退出協(xié)同系統(tǒng)，這些動態(tài)變化將會導致協(xié)同定位系統(tǒng)產(chǎn)生較大的誤差，甚至無法正常運行。因此需要對1.1節(jié)提出的協(xié)同定位算法進行改進，以實現(xiàn)在動態(tài)網(wǎng)絡(luò)下的定位功能。

2.1 動態(tài)協(xié)同網(wǎng)絡(luò)分析

1）網(wǎng)絡(luò)內(nèi)部動態(tài)變化

網(wǎng)絡(luò)內(nèi)部動態(tài)變化，即網(wǎng)絡(luò)中移動節(jié)點數(shù)量不變，但是移動節(jié)點自身以及節(jié)點與節(jié)點之間發(fā)生相對變化。網(wǎng)絡(luò)內(nèi)部節(jié)點動態(tài)變化，具體可以將其分為：錨節(jié)點與普通節(jié)點的角色變化、節(jié)點與節(jié)點之間距離測量的消失與出現(xiàn)，如圖2所示。

根據(jù)協(xié)同網(wǎng)絡(luò)定義，系統(tǒng)中存在錨節(jié)點與普通節(jié)點兩種不同角色的節(jié)點，其中具備BDS信號的節(jié)點為錨節(jié)點，反之為普通節(jié)點。由圖2可以看出，隨著1、2號節(jié)點的運動，其分別進入BDS信號正常及拒止環(huán)境，其節(jié)點身份完成了錨節(jié)點與普通節(jié)點之間的轉(zhuǎn)換。

根據(jù)建立的觀測方程可以得到，當錨節(jié)點轉(zhuǎn)換為普通節(jié)點后，其之前可以獲取得到的BDS信號接收機提供的絕對觀測將會消失，系統(tǒng)的觀測量將會減小兩個維度，相反，普通節(jié)點變換為錨節(jié)點后，系統(tǒng)的觀測變量將會增加兩個維度。而觀測量維度改變將會導致卡爾曼濾波無法繼續(xù)工作。

根據(jù)建立的協(xié)同定位系統(tǒng)觀測方程可以得到，當兩節(jié)點的距離觀測消失時，系統(tǒng)觀測變量會減小一個維度，相反，當距離觀測恢復后，系統(tǒng)觀測變量會增加一個維度，將同樣導致卡爾曼濾波器無法正常工作。

2）節(jié)點隨機加入與退出

網(wǎng)絡(luò)外部動態(tài)變化，即網(wǎng)絡(luò)中移動節(jié)點數(shù)量變化，網(wǎng)絡(luò)外部節(jié)點動態(tài)變化示意圖如圖3所示，在第1時刻系統(tǒng)存在1，2號節(jié)點，第2時刻3號節(jié)點出現(xiàn)加入系統(tǒng)，第3個時刻1號節(jié)點退出系統(tǒng)。

根據(jù)建立的協(xié)同定位系統(tǒng)狀態(tài)模型以及觀測模型，當協(xié)同中節(jié)點退出系統(tǒng)后，狀態(tài)向量減少4個維度，與該節(jié)點相關(guān)的觀測量也全部消失；當有新的節(jié)點加入系統(tǒng)后，狀態(tài)向量增加4個維度，觀測向量同樣相對應(yīng)地增加。

卡爾曼濾波是一個連續(xù)性濾波，其要求濾波的狀態(tài)向量以及觀測向量維度固定，而通過列文對動態(tài)網(wǎng)絡(luò)節(jié)點變化的具體情況分析，可以發(fā)現(xiàn)網(wǎng)絡(luò)的動態(tài)變化都會導致協(xié)同定位系統(tǒng)觀測量、甚至狀態(tài)量的維數(shù)變化，導致卡爾曼濾波無法正常工作。但是，對于節(jié)點固定的協(xié)同定位系統(tǒng)而言，其狀態(tài)量維度為4×N，當所有節(jié)點均為錨節(jié)點時，系統(tǒng)有最大的觀測量維度：2N+（N+1）N/2，此時系統(tǒng)狀態(tài)量與觀測量維數(shù)固定，因此可以通過建立協(xié)同定位系統(tǒng)最大觀測維度的卡爾曼濾波方程，并通過一定方法對消失的觀測量進行預(yù)測及誤差方差修正，即可適應(yīng)動態(tài)情況下協(xié)同定位的卡爾曼濾波實現(xiàn)。

2.2 動態(tài)協(xié)同定位方法

卡爾曼狀態(tài)估計方程可以寫為［27-29］

xk=xk，k－1+Pk，k－1HTk（HkPk，k－1HTk+Rk）（zk－Hkxk，k－1）（6）

由于系統(tǒng)各節(jié)點之間的觀測量互相獨立，因此觀測方程協(xié)方差矩陣Rk的分對角線元素均為0，對角線元素依次對應(yīng)觀測量的高斯白噪聲方差，因此由式（6）可以看出：當系統(tǒng)觀測方程Rk中某一對角線元素趨近于無窮大時，卡爾曼濾波增益對應(yīng)元素趨近于0，即對應(yīng)系統(tǒng)對應(yīng)的狀態(tài)估計值等于系統(tǒng)狀態(tài)預(yù)測值，

當Rk中某一對角線元素趨近于無窮小時，卡爾曼濾波增益對應(yīng)元素趨近于1，即對應(yīng)系統(tǒng)對應(yīng)的狀態(tài)估計值等于系統(tǒng)對應(yīng)觀測值，其體現(xiàn)出卡爾曼濾波的數(shù)據(jù)融合思想。

通過上述分析可以得到，對于一個節(jié)點數(shù)固定為N的協(xié)同定位網(wǎng)絡(luò)，節(jié)點及節(jié)點間的動態(tài)變化對系統(tǒng)狀態(tài)量沒有影響，其維數(shù)固定為4×N，而會造成觀測量的維度變化，而對于該系統(tǒng)，假設(shè)節(jié)點間所有測距信息都存在，且所有節(jié)點都可以接收到BDS衛(wèi)星，其觀測量可以表示為

zk=［z1，2k…z1，Nkz2，3k…zN-1，Nkz1k…zNk］T

其觀測量維度為N（N+1）/2+2N。

其對應(yīng)系統(tǒng)觀測誤差協(xié)方差矩陣Rk為對角線為觀測誤差方差、非對角線元素為0，維度為（N（N+1）/2+2N）×（N（N+1）/2+2N）的矩陣。

假設(shè)系統(tǒng)在k時刻得到的觀測信息集合為E，則對于系統(tǒng)觀測量是否存在可以用判斷矩陣Ak進行描述，有：

Ak={aij}∈R（N（N+1）/2+2N）×（N（N+1）/2+2N）（8）

其中

aij=1，i=j;i，j∈E

0，i=j;i，jE

0，i≠j

因此，改寫卡爾曼增益計算公式：

Kk=Pk，k－1HTk（HkPk，k－1HTk+Fanti（AkRk））（9）

其中：

Fanti（xi，j）=xi，j，xi，j≠0

x+∞，xi，j=0

針對系統(tǒng)觀測量，當某一觀測量消失后，直接使用狀態(tài)預(yù)測值代替觀測值，即有：

k=Frep（Akzk）（10）

Frep（xi）=xi，xi≠0

（xk，k－1）i，xi=0

因此，節(jié)點及節(jié)點間動態(tài)變化的協(xié)同定位卡爾曼濾波算法可以改寫為：

①計算狀態(tài)預(yù)測：

xk，k－1=Φk，k－1xk－1（11）

Pk，k－1=Φk，k－1Pk－1ΦTk，k－1+Γk－1Qk－1ΓTk－1（12）

②計算卡爾曼濾波增益：

k=Pk，k－1HTk（HkPk，k－1HTk+anti（AkRk））（13）

③更新系統(tǒng)觀測量：

k=Frep（Akzk）（14）

④計算狀態(tài)后驗：

xk=xk，k－1+k（k－Hkxk，k－1）（15）

Pk=（I－KkHk）Pk，k－1（16）

通過上述分析可以得到，當有節(jié)點接入或退出時，協(xié)同定位網(wǎng)絡(luò)中節(jié)點數(shù)增加或者減少，該變化將會導致系統(tǒng)增加或減少4個狀態(tài)量，并且會造成系統(tǒng)最大觀測量相應(yīng)的增加或減少，因此僅通過改變觀測誤差協(xié)方差矩陣以及觀測量對應(yīng)的值也無法適應(yīng)此種情況，系統(tǒng)必不可免的需要重新構(gòu)建狀態(tài)方程以及觀測方程。

對于卡爾曼濾波而言，需要給定系統(tǒng)的初始狀態(tài)量x0以及初始狀態(tài)誤差協(xié)方差矩陣P0，但是一般情況下，無法獲得節(jié)點確切的初始狀態(tài)量，因此通常在協(xié)同狀態(tài)大致區(qū)間內(nèi)隨機選取，并將初始狀態(tài)誤差協(xié)方差陣取值設(shè)置較大，雖然初值誤差較大，但是經(jīng)過卡爾曼濾波逐次迭代，偏差會急劇縮小，最后收斂于最優(yōu)解。

綜上所述， 對于節(jié)點退出而言，利用方差上界技術(shù)，新的卡爾曼濾波方程初始值可以使用舊卡爾曼濾波的狀態(tài)估計值與狀態(tài)誤差協(xié)方差值；對于節(jié)點接入情況而言，其節(jié)點初始狀態(tài)以及狀態(tài)誤差的協(xié)方差都未知，一般而言，新節(jié)點初始狀態(tài)取一個大致范圍，并將其對應(yīng)的狀態(tài)誤差的協(xié)方差設(shè)定為一個較大值，因此建立的新的卡爾曼濾波方程在開始階段將會收到新節(jié)點初始狀態(tài)誤差的干擾，造成其他節(jié)點狀態(tài)及狀態(tài)誤差協(xié)方差的波動。因此，為了降低新節(jié)點造成的定位結(jié)果波動，實現(xiàn)兩個卡爾曼濾波之間平滑切換，在此期間建立基于聯(lián)邦卡爾曼濾波的動態(tài)協(xié)同定位方法，并在新的卡爾曼濾波趨于穩(wěn)定后，停止原卡爾曼濾波的解算。

動態(tài)協(xié)同定位的聯(lián)邦卡爾曼濾波由一個主系統(tǒng)和新舊兩個子系統(tǒng)構(gòu)成如圖4所示。使用無反饋模式，每個自濾波器并行獨立，并采用方差上界技術(shù)，分別得到系統(tǒng)的局部狀態(tài)估計及誤差均方差，然后輸入到主系統(tǒng)中，作為主系統(tǒng)的觀測量進行全局濾波，獲得協(xié)同系統(tǒng)的全局狀態(tài)估計，并在新系統(tǒng)輸出穩(wěn)定后，關(guān)閉舊系統(tǒng)濾波，實現(xiàn)節(jié)點加入后系統(tǒng)狀態(tài)平滑過度。

動態(tài)協(xié)同定位系統(tǒng)的聯(lián)邦卡爾曼濾波器可以表示為

g=Pg（P－10，k0，k+P－11，k1，k）

Pg=（P－10，k+P－11，k）－1（17）

因此，結(jié)合協(xié)同網(wǎng)絡(luò)節(jié)點及節(jié)點間動態(tài)變化、節(jié)點接入與退出定位方法，動態(tài)網(wǎng)絡(luò)的協(xié)同定位方法流程如圖5所示。

3 仿真實驗結(jié)果分析

3.1 定位精度評估

通過建立協(xié)同定位系統(tǒng)的擴展卡爾曼濾波方程，可以得到系統(tǒng)中各移動節(jié)點在k時刻的狀態(tài)估計以及誤差協(xié)方差矩陣，因此，為了定量的衡量協(xié)同定位系統(tǒng)的定位效果，需要定義精度評價指標，其可以為絕對定位精度。

對于單個移動節(jié)點來講，其絕對定位精度有：

Δrik=（xik－xik，0）2+（yik－yik，0）（18）

式中：xik，0，yik，0為i節(jié)點在k時刻的在協(xié)同坐標系下的真實坐標。

利用均方根誤差（root mean square error，RMSE）作為衡量單個節(jié)點在整個移動過程中的定位精度衡量指標，有：

SRMSEi=1L∑Lk=1Δrik（19）

式中：SRMSEi為第i個節(jié)點位置的均方根誤差。

因此，整個系統(tǒng)的定位精度可以表示為

SRMSE=1N∑Ni=1SRMSEi（20）

3.2 協(xié)同定位算法仿真驗證

選取5個移動節(jié)點組成一個協(xié)同定位網(wǎng)絡(luò)，用以驗證所提算法的有效性，并分析不同錨節(jié)點、相對距離測量等不同因素對協(xié)同精度的影響。

基于python3.8.12，令仿真時間800s，由于使用協(xié)同定位坐標系，以某一節(jié)點初始位置作為坐標系原點，因此不考慮初始狀態(tài)誤差，忽略BDS信號在坐標系轉(zhuǎn)換中產(chǎn)生的誤差，假設(shè)錨節(jié)點得到的BDS定位信號直接為協(xié)同坐標系中的x，y軸坐標，且各節(jié)點導航傳感器誤差為高斯白噪聲，具體數(shù)值如下：

1）0-5號節(jié)點從0時刻開始以1m/s2的加速度加速到3m/s，并分別以-15°、-5°、0°、5°、15°初始方位行駛，并隨機1-4號節(jié)點初始位置。

2）SINS陀螺儀測量零偏為7°/h，隨機漂移為10°/h，節(jié)點速度測量精度0.1m/s。

3）UWB測距精度10cm，BDS定位精度1m。

4）采樣頻率為10Hz。

3.2.1 錨節(jié)點數(shù)量對系統(tǒng)定位精度影響

錨節(jié)點可以獲取得到自身的絕對位置信息，并通過協(xié)同定位系統(tǒng)信息共享將自身絕對位置作為約束提高普通節(jié)點的定位精度，因此研究錨節(jié)點數(shù)量對協(xié)同定位系統(tǒng)精度影響是十分有必要的。

一般情況下，協(xié)同定位系統(tǒng)主要面向峽谷、巷道、隧道、林地等復雜、陌生環(huán)境，在此情景下通常固有導航缺乏，及協(xié)同定位系統(tǒng)中存在甚至所有移動節(jié)點工作在BDS信號拒止情況下，通過節(jié)點間時空信息融合進行定位。若所有節(jié)點均可接收到BDS信號，則協(xié)同定位意義不大。因此，本節(jié)針對5個節(jié)點的協(xié)同定位網(wǎng)絡(luò)情況開展分析，分別對兩個、一個、零個錨節(jié)點情況分析，仿真設(shè)定如表1所示。仿真結(jié)果如圖6所示。

從各節(jié)點及系統(tǒng)x軸及整體的誤差整體趨勢來講，2個錨節(jié)點、1個錨節(jié)點、0個錨節(jié)點的定位誤差逐步增大，但是在y軸方向波動較大，這是由于y軸誤差較小，協(xié)同定位算法對y軸誤差矯正不明顯。同時可以看出，在系統(tǒng)運行前期，節(jié)點航位推算誤差相比于協(xié)同定位誤差更小，在后期誤差才逐步擴大，主要原因有：①仿真忽略了各節(jié)點的初始誤差；②協(xié)同定位算法加入了各節(jié)點間的距離作為約束，各節(jié)點間測距誤差導致了開始時間段相比于航位推算有較大的定位誤差。

在沒有錨節(jié)點的情況下，系統(tǒng)定位誤差波動較大，并在700s左右開始呈現(xiàn)出一定的發(fā)散趨勢，但整體定位精度相比于航位推算有明顯的提升。

不同錨節(jié)點情況下仿真具體數(shù)值結(jié)果如表2所示。

3.2.2 相對測量對系統(tǒng)定位精度影響

除了不同錨節(jié)點數(shù)量會對系統(tǒng)定位精度產(chǎn)生影響之外，節(jié)點與節(jié)點之間距離測量數(shù)量同樣需要考慮，理論上當系統(tǒng)中各節(jié)點之間都存在測距時，系統(tǒng)擁有最大的約束條件，其定位精度最高，但是在實際應(yīng)用中，系統(tǒng)不可能一直處于節(jié)點間全連接情況，當節(jié)點之間距離越遠，或者存在障礙物時，就無法獲取得到兩節(jié)點間的距離觀測，因此研究相對測量對協(xié)同定位系統(tǒng)精度影響同樣是十分有必要的。

基于不同錨節(jié)點數(shù)量對協(xié)同定位精度的仿真結(jié)果，本次仿真隨機選取1個移動節(jié)點作為錨節(jié)點，這樣可以避免因為系統(tǒng)沒有錨節(jié)點產(chǎn)生較大的誤差而影響此次仿真結(jié)果，又避免因系統(tǒng)中錨節(jié)點數(shù)過多定位精度過高的問題。此外，考慮到5個節(jié)點最多擁有10個距離觀測，因此選取全連接（10個測距信息）、隨機8個測距信息、隨機6個測距信息共3種情況下進行仿真實驗。同時，由于節(jié)點數(shù)過多，因此僅給出系統(tǒng)總體誤差，仿真結(jié)果見表2，定位誤差如圖7所示。

由圖7可以看出，當協(xié)同網(wǎng)絡(luò)中各節(jié)點全連接時，系統(tǒng)定位精度最高；當節(jié)點的間測距量減少3個后，定位精度有所下降，但是下降趨勢不明顯，這

是因為8個測距信息相比于5個節(jié)點，仍能提供足夠的距離間約束；當節(jié)點間測距量變?yōu)?個時，協(xié)同定位系統(tǒng)的定位精度有了明顯的下降，這是由于6個測距量提供的約束相比于節(jié)點數(shù)略有不足。但是上述3種情況的定位誤差都遠小于節(jié)點航位推算產(chǎn)生的誤差。

3.3 動態(tài)協(xié)同定位算法仿真

開展10次動態(tài)協(xié)同定位算仿仿真，其時間與錨節(jié)點、測距數(shù)量變化如表3所示，平均誤差結(jié)果如圖8所示。

由圖8可見，在0～300s范圍內(nèi)，系統(tǒng)存在2個錨節(jié)點與全測距信息，其定位誤差維持在較小水平；300～600s內(nèi)，系統(tǒng)錨節(jié)點消失，在BDS信號拒止環(huán)境下，系統(tǒng)誤差成發(fā)散狀態(tài)；600～900s內(nèi)，系統(tǒng)測距信息減少2個，由于約束變少，定位誤差發(fā)散速度增加；在900～1200s內(nèi)，其中一個節(jié)點隨機退出，系統(tǒng)節(jié)點數(shù)變?yōu)?，系統(tǒng)狀態(tài)量與觀測量同時改變，此時通過建立聯(lián)邦卡爾曼濾波進行系統(tǒng)過度，同時其存在6個測距量，節(jié)點間全連接，因此系統(tǒng)整體誤差發(fā)散速度相比于600～900s更??；在1200～1500s期間，新節(jié)點接入系統(tǒng)，且錨節(jié)點數(shù)增加，因此其定位誤差大幅減小，且整體平穩(wěn)不變，但是僅依靠一個錨節(jié)點無法完全矯正系統(tǒng)前期解算誤差，因此在此階段仍存在一個固定誤差；在1500s之后，新加入了一個錨節(jié)點，上一階段存在的誤差被矯正回來，系統(tǒng)定位精度與第1節(jié)點基本相同。

綜上所述，系統(tǒng)在不同情況下的定位表現(xiàn)與前文研究相符，驗證了動態(tài)協(xié)同定位算法的有效性。

4 結(jié) 論

本文提出了一種協(xié)同定位算法，并針對在實際應(yīng)用過程中，節(jié)點始終處于運動狀態(tài)，常常會面臨場景切換問題，可能出現(xiàn)錨節(jié)點與普通節(jié)點角色變化、節(jié)點之間的測距值的消失或產(chǎn)生、節(jié)點隨機接入與退出網(wǎng)絡(luò)等情況，造成協(xié)同定位算法無法正常工作的情況。針對上述問題，本文究首先開展協(xié)同定位方法研究，接著提出自適應(yīng)調(diào)整觀測誤差協(xié)方差矩陣的濾波方法與聯(lián)邦濾波方法融合，實現(xiàn)移動節(jié)點間錨節(jié)點與普通節(jié)點變換，節(jié)點間測距值消失與出現(xiàn)、節(jié)點的隨機接入與退出等情況下協(xié)同定位系統(tǒng)正常工作，最后進行了仿真實驗，仿真結(jié)果顯示，由5個普通節(jié)點組成的協(xié)同系統(tǒng)定位精度達到了3.6m，相比于航位推算定位精度10.702m提升了197.2%，并通過仿真設(shè)定，驗證了所提動態(tài)協(xié)同定位方法的有效性，驗證所提方法的有效性。

參 考 文 獻：

［1］ 原大川， 王濤， 李麗君， 等. 單兵班組多平臺協(xié)同作戰(zhàn)應(yīng)用研究［J］. 火力與指揮控制， 2019， 44（6）： 148.

YUAN Dachuan， WANG Tao， LI Lijun， et al. Cooperative Engagement Operation Research on Soldier Squad and Multiple Platforms［J］. Fire Control amp; Command Control， 2019， 44（6）： 148.

［2］ 張倫東， 張超， 高揚駿. 衛(wèi)星導航欺騙及檢測（一）：典型事件及欺騙技術(shù)發(fā)展［J］. 導航定位學報， 2021， 9（3）： 1.

ZHANG Lundong， ZHANG Chao， GAO Yangjun. GNSS Spoofing and Detection（Ⅰ）：Typical Events and Development of Spoofing Technology［J］. Journal of Navigation and Positioning， 2021， 9（3）： 1.

［3］ 張炎華， 王立端， 戰(zhàn)興群， 等. 慣性導航技術(shù)的新進展及發(fā)展趨勢［J］. 中國造船， 2008， 49（S1）： 134.

ZHANG Yanhua， WANG Liduan， ZHAN Xingqun， et al. The New Advancement and Trend of Inertial Navigation Technology［J］. Shipbuilding of China， 2008， 49（S1）： 134.

［4］ SHEN Y， MAZUELAS S， WIN M Z. Network Navigation： Theory and Interpretation［J］. IEEE Journal on Selected Areas in Communications， 2012， 30（9）： 1823.

［5］ 付夢印， 楊毅， 岳裕豐， 等. 地空協(xié)同無人系統(tǒng)綜述［J］. 國防科技， 2021， 42（3）： 1.

FU Mengyin， YANG Yi， YUE Yufeng， et al. A Review of Ground-air Cooperative Unmanned System［J］. National Defense Technology， 2021， 42（3）： 1.

［6］ 申良強， 蔣秀波， 宋早迪， 等. 單兵信息系統(tǒng)協(xié)同作戰(zhàn)技術(shù)研究［J］. 火力與指揮控制， 2017， 42（8）： 169.

SHEN Liangqiang， JIANG Xiubo， SONG Zaodi， et al. Research on Cooperative Operation Technology of Ihe Individual Information System［J］. Fire Control amp; Command Control， 2017， 42（8）： 169.

［7］ ZHANG Yi， YUAN Ling. A Cooperative Localization Algorithm for Anchor-free Nodes［J］.Electronic Technology Application，2023，49（5）：77.

［8］ 田世偉， 戴衛(wèi)恒， 李廣俠， 等. 協(xié)同定位研究綜述［C］// 第四屆中國衛(wèi)星導航學術(shù)年會論文集-S9 組合導航與導航新方法， 2013： 5.

TIAN Shiwei， DAI Weiheng， LI Guangxia， et al. Synthesis of Cooperative Localization Research［C］// Proceedings of the Fourth Annual China Satellite Navigation Conference-S9 Combined Navigation and New Methods of Navigation， 2013： 5.

［9］ 范世偉. 自主水下航行器協(xié)同定位算法研究［D］. 哈爾濱：哈爾濱工業(yè)大學， 2020.

［10］郭樹人， 蔡洪亮， 孟軼男， 等. 北斗三號導航定位技術(shù)體制與服務(wù)性能［J］. 測繪學報， 2019， 48（7）： 810.

GUO Shuren， CAI Hongliang， MENG Yinan， et al. BDS-3 RNSS Technical Characteristics and Service Performance［J］. Acta Geodaetica et Cartographica Sinica， 2019， 48（7）： 810.

［11］楊元喜. 北斗衛(wèi)星導航系統(tǒng)的進展、貢獻與挑戰(zhàn)［J］. 測繪學報， 2010， 39（1）： 1.

YANG Yuanxi. Progress， Contribution and Challenges of Compass/Beidou Satellite Navigation System［J］. Acta Geodaetica et Cartographica Sinica， 2010， 39（1）： 1.

［12］徐景碩， 周勝明， 蔣華君. 慣性導航系統(tǒng)的發(fā)展及其關(guān)鍵技術(shù)綜述［J］. 科技信息， 2009（35）： 865.

XU Jingshuo， ZHOU Shengming， JIANG Huajun. Survey of the Development and the Key Technology of the Inertial Navigation System［J］. Science amp; Technology Information， 2009（35）： 865.

［13］王鵬飛， 林媛. 高精度光纖陀螺技術(shù)的發(fā)展與思考［J］. 艦船電子工程， 2020， 40（12）： 11.

WANG Pengfei， LIN Yuan. Development and Prospect of High Precision Fiber Optical Gyroscope Technology［J］. Ship Electronic Engineering， 2020， 40（12）： 11.

［14］石斌. 基于超寬帶的室內(nèi)分布式協(xié)同定位技術(shù)研究［D］. 天津：天津理工大學， 2021.

［15］于耕， 任武君. 無線傳感器網(wǎng)絡(luò)節(jié)點定位算法研究［J］. 價值工程， 2018（30）：194.

YU Geng， REN Wujun. Research on Node Localization Algorithm for Wireless Sensor Networks［J］. Value Engineering， 2018（30）：194.

［16］SHANG Y， RUML W， ZHANG Y， et al. Localization from Mere Connectivity［C］//Acm International Symosium on Mobile Ad Hoc Networking amp; Computing. 2003.

［17］田俊曦. GNSS拒止環(huán)境下的多飛行器協(xié)同導航關(guān)鍵技術(shù)研究［D］.綿陽：電子科技大學，2020.

［18］徐亮. 基于改進UKF算法的多AUV協(xié)同導航方法研究［D］. 太原：中北大學， 2020.

［19］谷旭平， 唐大全， 唐管政. 無人機編隊協(xié)同導航研究現(xiàn)狀及進展［J］. 艦船電子工程， 2021， 41（7）： 28.

GU Xuping， TANG Daquan， TANG Guanzheng. Research Status and Progress of UAV Formation Cooperative Navigation［J］. Ship Electronic Engineering， 2021， 41（7）： 28.

［20］ARPIN E， SHANKWITZ C， DONATH M. A High Accuracy Vehicle Positioning System Implemented in a Lane Assistance System when GPS Is Unavailable［J］. 2011.

［21］ALLOTTA B， CAITI A， CHISCI L， et al. An Unscented Kalman Filter Based Navigation Algorithm for Autonomous Underwater Vehicles［J］. Mechatronics， 2016， 39： 185.

［22］WYMEERSCH H， LIEN J， WIN M Z. Cooperative Localization in Wireless Networks［J］. Proceedings of the IEEE， 2009， 97（2）： 427.

［23］LIU J， CAI B， WANG J. Cooperative Localization of Connected Vehicles： Integrating GNSS With DSRC Using a Robust Cubature Kalman Filter［J］. IEEE Transactions on Intelligent Transportation Systems， 2017， 18（8）： 2111.

［24］周巍. 北斗衛(wèi)星導航系統(tǒng)精密定位理論方法研究與實現(xiàn)［D］. 解放軍信息工程大學， 2013.

［25］陳柯勛，張雪英，邱偉.一種改進的GNSS接收機完好性監(jiān)測算法［J］.哈爾濱理工大學學報，2021，26（3）：103.

CHEN Kexun， ZHANG Xueying， QIU Wei. An Improved Integrity Monitoring Algorithm for GNSS Receiver［J］. Journal of Harbin University of Science and Technology， 2021，26（3）：103.

［26］YANG J， JIN T， HUANG Z， et al. New Generation Signal Structure Evaluation System for GNSS［C］// J. Sun， J. Liu， S. Fan， 等. China Satellite Navigation Conference （csnc）. New York： Springer， 2015， 340： 491.

［27］LI Qian， HUANG Hongdian， XU Guipeng， et al. Analysis of Localization Performance and Formation Configuration for AUV Cooperative Navigation［J］.Chinese Journal of Instrument，2023，44（7）：203.

［28］WANG Tao， HUANG Jiacai， TANG Wenjun， et al. Multi-sensor Collaborative Outdoor Positioning Based on Synthesize Kalman Filters［J］.Journal of Nanjing Institute of Technology （Natural Science Edition），2023，21（1）：9.

［29］SUN Wei， CAO Hongyang. KPCA/Improved RBF Neural Network Aided GPS/UWB Cooperative Positioning Method［J］.Journal of Navigation and Positioning，2022，10（6）：112.

（編輯：溫澤宇）