基于雙向測(cè)距的協(xié)同定位系統(tǒng)誤差分析

徐 望， 陳恒智， 闞 磊

(1.海軍上海航保修理廠，上海 200083； 2.中國(guó)空間技術(shù)研究院 載人航天總體部，北京 100094；3.陸裝駐重慶地區(qū)第六軍代室，重慶 400010)

協(xié)同定位的概念早在20世紀(jì)60年代就已被提出，該思想早期主要被用于衛(wèi)星組網(wǎng)的自主導(dǎo)航系統(tǒng)。隨后物理學(xué)家哈肯又創(chuàng)立了協(xié)同理論，該理論的含義主要是在一個(gè)龐大的系統(tǒng)中，若其中各個(gè)子系統(tǒng)都能夠相互分享信息并協(xié)調(diào)配合，就可使整體系統(tǒng)獲得更強(qiáng)的功能。該理論的提出使多節(jié)點(diǎn)協(xié)同定位技術(shù)作為新的研究方向受到了廣泛的關(guān)注[1-3]，其應(yīng)用的平臺(tái)包括無(wú)人機(jī)、無(wú)人水下潛器以及其他各種陸用平臺(tái)等。此外，各相關(guān)平臺(tái)通過(guò)信息的交互協(xié)同使工作范圍和工作穩(wěn)定性得到大幅提升。例如，在戰(zhàn)爭(zhēng)環(huán)境下，部隊(duì)的通信、指揮、調(diào)度系統(tǒng)時(shí)刻面臨著高時(shí)變、高動(dòng)態(tài)和高隨機(jī)性的挑戰(zhàn)，而部隊(duì)的大部分傳感器依賴于GNSS(Global Navigation Satellite System，全球?qū)Ш叫l(wèi)星系統(tǒng))的輔助，惡劣的戰(zhàn)場(chǎng)環(huán)境很有可能導(dǎo)致GNSS信號(hào)拒止或不可靠。此時(shí)，協(xié)同定位系統(tǒng)通過(guò)組網(wǎng)結(jié)合各傳感器信息，從而提升信息的冗余性，進(jìn)而提高了導(dǎo)航傳感器定位精度和可靠性。正是由于協(xié)同定位系統(tǒng)具有這一優(yōu)點(diǎn)，相關(guān)研究者們希望其在自身的研究領(lǐng)域中有所應(yīng)用。多節(jié)點(diǎn)協(xié)同定位系統(tǒng)有以下諸多優(yōu)勢(shì)[4]：① 系統(tǒng)中部分節(jié)點(diǎn)的高精度定位信息可以傳遞至定位精度較低的節(jié)點(diǎn)，用以提高低精度節(jié)點(diǎn)的定位性能；② 如果系統(tǒng)中的部分節(jié)點(diǎn)的定位誤差有界，那么通過(guò)組網(wǎng)信息共享能夠使系統(tǒng)的每個(gè)節(jié)點(diǎn)的定位誤差都有界；③ 當(dāng)系統(tǒng)中的部分節(jié)點(diǎn)由于不同因素喪失自主定位能力時(shí)，通過(guò)多節(jié)點(diǎn)協(xié)同定位能夠在一定程度上恢復(fù)這些節(jié)點(diǎn)的導(dǎo)航定位能力。

為獲得更高的協(xié)同定位精度，國(guó)內(nèi)外學(xué)者進(jìn)行了有針對(duì)性的研究。Tang等[5]提出了一種采用激光雷達(dá)和視覺(jué)傳感器的協(xié)同導(dǎo)航方案，實(shí)現(xiàn)了較高的定位精度，但其設(shè)備高昂的價(jià)格限制了該方案的實(shí)際推廣。Lim等[6]通過(guò)組合低成本GPS和基于超寬帶的雙向測(cè)距(Two-Way Ranging,TWR)進(jìn)行協(xié)同定位，實(shí)現(xiàn)了1 m以內(nèi)的定位精度，具有較好的大規(guī)模工程應(yīng)用前景，因此本文在該方案的硬件配置基礎(chǔ)上進(jìn)行研究分析。

協(xié)同定位系統(tǒng)的基礎(chǔ)組成單元為移動(dòng)節(jié)點(diǎn)，移動(dòng)節(jié)點(diǎn)根據(jù)系統(tǒng)控制信號(hào)可分別作為錨節(jié)點(diǎn)[7]和標(biāo)簽節(jié)點(diǎn)，其中錨節(jié)點(diǎn)是自身位置準(zhǔn)確已知的移動(dòng)節(jié)點(diǎn)，標(biāo)簽節(jié)點(diǎn)是待定位移動(dòng)節(jié)點(diǎn)，該類節(jié)點(diǎn)可利用與錨節(jié)點(diǎn)間的距離通過(guò)三角定位等方法來(lái)完成自身定位。另外，協(xié)同定位系統(tǒng)可以通過(guò)改變移動(dòng)節(jié)點(diǎn)工作模式來(lái)擴(kuò)展錨節(jié)點(diǎn)分布和定位范圍，例如當(dāng)一個(gè)標(biāo)簽節(jié)點(diǎn)通過(guò)錨節(jié)點(diǎn)或GPS獲取其準(zhǔn)確位置后，該標(biāo)簽節(jié)點(diǎn)的工作模式即可更改為錨節(jié)點(diǎn)。

錨節(jié)點(diǎn)工作模式下的移動(dòng)節(jié)點(diǎn)在測(cè)量與標(biāo)簽節(jié)點(diǎn)間的距離進(jìn)行定位的過(guò)程中，會(huì)將自身定位誤差傳播給其定位的標(biāo)簽節(jié)點(diǎn)。而即使忽略測(cè)距噪聲的影響，錨節(jié)點(diǎn)的位置誤差仍然會(huì)在雙向測(cè)距過(guò)程中帶來(lái)額外的誤差。因此，研究如何降低基于雙向測(cè)距的協(xié)同定位網(wǎng)絡(luò)中標(biāo)簽節(jié)點(diǎn)的定位誤差必須考慮精度因子(Dilution of Precision，DOP)和錨節(jié)點(diǎn)幾何分布的影響。同時(shí)，由于協(xié)同定位系統(tǒng)中錨節(jié)點(diǎn)的幾何分布一般是時(shí)變的，因而集群編隊(duì)控制[8-9]也是協(xié)同定位的關(guān)鍵問(wèn)題之一。

本文主要分析了基于雙向測(cè)距的協(xié)同定位系統(tǒng)誤差傳播機(jī)理。首先，對(duì)距離量測(cè)的誤差源進(jìn)行了總結(jié)。另外，分析推導(dǎo)了由測(cè)距誤差帶來(lái)的協(xié)同定位系統(tǒng)中的定位誤差。最后，通過(guò)在不同場(chǎng)景下進(jìn)行仿真展現(xiàn)了錨節(jié)點(diǎn)幾何分布對(duì)系統(tǒng)誤差傳播的影響。

1 協(xié)同定位系統(tǒng)誤差源

以無(wú)人機(jī)集群編隊(duì)飛行需確定精確的相對(duì)位置關(guān)系為背景，對(duì)基于雙向測(cè)距的協(xié)同定位系統(tǒng)的誤差進(jìn)行了研究。由于二維空間條件下誤差問(wèn)題的分析可以較為容易地推廣至三維空間，因此文中的分析主要從二維空間展開(kāi)。不同場(chǎng)景下協(xié)同定位系統(tǒng)示意圖如圖1所示。

圖1 不同場(chǎng)景下協(xié)同定位系統(tǒng)示意圖

從圖1場(chǎng)景1中可以看出，所有移動(dòng)節(jié)點(diǎn)均通過(guò)GPS修正自身位置，隨后場(chǎng)景2中移動(dòng)節(jié)點(diǎn)5進(jìn)入GPS拒止區(qū)域。在場(chǎng)景3中，移動(dòng)節(jié)點(diǎn)5通過(guò)與其他已知確切位置的移動(dòng)節(jié)點(diǎn)完成雙向距離測(cè)量，并確定自身位置成為錨節(jié)點(diǎn)。在場(chǎng)景4中，移動(dòng)節(jié)點(diǎn)1進(jìn)入GPS拒止區(qū)域，并利用錨節(jié)點(diǎn)5及其他節(jié)點(diǎn)的測(cè)距信息定位自身位置。

1.1 測(cè)距誤差

基于無(wú)線電測(cè)距方式的實(shí)現(xiàn)一般包括單向測(cè)距和雙向測(cè)距。假設(shè)需要完成移動(dòng)節(jié)點(diǎn)A和移動(dòng)節(jié)點(diǎn)B之間的距離測(cè)量，單向測(cè)距方式是通過(guò)移動(dòng)節(jié)點(diǎn)A將數(shù)據(jù)包發(fā)送到移動(dòng)節(jié)點(diǎn)B，同時(shí)計(jì)算信號(hào)飛行時(shí)間，實(shí)現(xiàn)該方法需要全局同步高精度晶振，因此價(jià)格較為高昂。而雙向測(cè)距方式具體的實(shí)現(xiàn)流程如圖2所示。定位標(biāo)簽會(huì)在定位開(kāi)始時(shí)發(fā)送一個(gè)數(shù)據(jù)包，同時(shí)記錄下此刻的發(fā)送時(shí)間T1，安裝在某一確定位置的定位基站在正常的工作模式下會(huì)接收到這個(gè)數(shù)據(jù)包，等待一段時(shí)間后會(huì)發(fā)送應(yīng)答數(shù)據(jù)包，該數(shù)據(jù)包包含了這段等待時(shí)間T_replyB，標(biāo)簽在接收到應(yīng)答包后，記錄此時(shí)的時(shí)間T2，得到T_RoundA=T2-T1，然后經(jīng)過(guò)等待時(shí)間T_replyA發(fā)送最終的數(shù)據(jù)，進(jìn)行一次與前一階段相同的通信過(guò)程。該測(cè)距方式利用兩次通信的時(shí)間來(lái)對(duì)時(shí)鐘偏移引入的誤差進(jìn)行補(bǔ)償，解決了時(shí)鐘不統(tǒng)一的問(wèn)題。

圖2 雙向測(cè)距方式示意圖

另外，錨節(jié)點(diǎn)的定位精度也會(huì)對(duì)帶定位標(biāo)簽節(jié)點(diǎn)的定位精度產(chǎn)生影響。若錨節(jié)點(diǎn)定位存在誤差，那么即使是無(wú)測(cè)量噪聲的測(cè)距值，也與標(biāo)簽節(jié)點(diǎn)真實(shí)位置和錨節(jié)點(diǎn)估計(jì)位置之間的標(biāo)稱距離不相等，這一誤差是協(xié)同定位系統(tǒng)中測(cè)距誤差的必要組成部分。Lim[10]將測(cè)距誤差的協(xié)方差矩陣Q表示為：

(1)

式中，Pi為第i個(gè)錨節(jié)點(diǎn)的協(xié)方差矩陣；σ2為距離量測(cè)的方差；IN為N×N維單位矩陣；hi為從標(biāo)簽節(jié)點(diǎn)到第i個(gè)節(jié)點(diǎn)的方向余弦向量，可以表示為

(2)

其中，(xi,yi,zi)為第i個(gè)錨節(jié)點(diǎn)的位置；(x,y,z)為標(biāo)簽節(jié)點(diǎn)位置；ri為兩節(jié)點(diǎn)間的距離。由式(1)可以看出，測(cè)距精度主要取決于測(cè)距噪聲以及錨節(jié)點(diǎn)的位置估計(jì)誤差這兩個(gè)部分。

1.2 位置誤差

標(biāo)簽節(jié)點(diǎn)的定位過(guò)程主要是通過(guò)結(jié)合測(cè)距信息及錨節(jié)點(diǎn)的位置信息來(lái)實(shí)現(xiàn)，其定位誤差的協(xié)方差矩陣P可由以下公式推出：

P=cov(Δx)

=(HTQ-1H)-1HTQ-1cov(Δr){Q-1HT(HTQ-1H)-1}T

=(HTQ-1H)-1

(3)

式中，Δx=(HTQ-1H)-1HTQ-1Δr，H為標(biāo)簽節(jié)點(diǎn)的觀測(cè)矩陣，Q為測(cè)距誤差協(xié)方差矩陣。在協(xié)同定位系統(tǒng)中，Q矩陣通常不是單位矩陣，這與有關(guān)精度因子的假設(shè)有所區(qū)別。將P引用為加權(quán)精度因子，與定位誤差協(xié)方差矩陣取值相同。

1.3 移動(dòng)場(chǎng)景下誤差

在對(duì)標(biāo)簽節(jié)點(diǎn)的位置進(jìn)行估計(jì)的過(guò)程中，錨節(jié)點(diǎn)是瞬態(tài)靜止的，直至完成對(duì)標(biāo)簽節(jié)點(diǎn)間的距離量測(cè)與定位后方可自由移動(dòng)，而這一節(jié)點(diǎn)靜止和移動(dòng)狀態(tài)的轉(zhuǎn)換過(guò)程即移動(dòng)場(chǎng)景。在移動(dòng)場(chǎng)景下，移動(dòng)標(biāo)簽節(jié)點(diǎn)借助錨節(jié)點(diǎn)完成了自身定位。同時(shí)，由錨節(jié)點(diǎn)移動(dòng)帶來(lái)的錨節(jié)點(diǎn)定位范圍移動(dòng)將使加權(quán)精度因子發(fā)生變化。為更好地定量描述這一變化，在此將這一變化定義為移動(dòng)精度因子，該定義值可通過(guò)測(cè)距誤差與移動(dòng)節(jié)點(diǎn)定位誤差之比計(jì)算得到。

范數(shù)的均方根增益可以表示為Frobenius范數(shù)，且每一個(gè)范數(shù)均是凸函數(shù)。由于均方根誤差的最小值點(diǎn)一般落在錨節(jié)點(diǎn)定位范圍的內(nèi)部，移動(dòng)標(biāo)簽節(jié)點(diǎn)的定位誤差會(huì)隨著與錨節(jié)點(diǎn)定位范圍之間的距離增加而線性增加。從這一重要特性可以看出，由于移動(dòng)標(biāo)簽定位誤差的非線性增長(zhǎng)， 當(dāng)錨節(jié)點(diǎn)的定位范圍固定不變時(shí)，將移動(dòng)標(biāo)簽節(jié)點(diǎn)直接移至最終目標(biāo)位置不是最優(yōu)的方案。

2 仿真分析

為驗(yàn)證基于雙向測(cè)距的協(xié)同定位系統(tǒng)相對(duì)于傳統(tǒng)基于雙程測(cè)距定位系統(tǒng)的優(yōu)勢(shì)，下面分別在兩種場(chǎng)景下進(jìn)行仿真分析并對(duì)比，具體場(chǎng)景設(shè)計(jì)如圖3所示。在場(chǎng)景1中，待定位標(biāo)簽節(jié)點(diǎn)5初始時(shí)位于錨節(jié)點(diǎn)1～錨節(jié)點(diǎn)4的定位范圍內(nèi)，隨后標(biāo)簽節(jié)點(diǎn)5按照黑色實(shí)線方向移動(dòng)出先前錨節(jié)點(diǎn)的定位范圍。在場(chǎng)景2中，首先節(jié)點(diǎn)6和節(jié)點(diǎn)7從初始位置移動(dòng)至錨節(jié)點(diǎn)1～錨節(jié)點(diǎn)4所構(gòu)成的定位范圍之外，隨后標(biāo)簽節(jié)點(diǎn)5移動(dòng)出錨節(jié)點(diǎn)1、錨節(jié)點(diǎn)2、錨節(jié)點(diǎn)6、錨節(jié)點(diǎn)7構(gòu)成的定位范圍，其中錨節(jié)點(diǎn)3和錨節(jié)點(diǎn)4在此過(guò)程中未被標(biāo)簽節(jié)點(diǎn)5使用。

圖3 協(xié)同定位系統(tǒng)動(dòng)態(tài)場(chǎng)景對(duì)比

在此基礎(chǔ)上，設(shè)置仿真條件如下：場(chǎng)景區(qū)域?yàn)?0 m×40 m方形范圍，錨節(jié)點(diǎn)具體分布位置坐標(biāo)如圖4所示，節(jié)點(diǎn)間測(cè)距標(biāo)準(zhǔn)差為0.3 m，通過(guò)節(jié)點(diǎn)的真實(shí)位置可以計(jì)算出標(biāo)簽節(jié)點(diǎn)與4個(gè)錨節(jié)點(diǎn)的真實(shí)距離，并將此距離加上給定的誤差 0.3 m作為定位系統(tǒng)的原始測(cè)量距離。標(biāo)簽節(jié)點(diǎn)的定位解算方法采用CHAN算法[11]，迭代次數(shù)設(shè)為5000次，該算法能夠以很小的計(jì)算量解算出高精度的位置坐標(biāo)，理論上可達(dá)到克拉美羅下限。此外，錨節(jié)點(diǎn)的位置用空心點(diǎn)表示，解算出的標(biāo)簽節(jié)點(diǎn)位置在圖4中用藍(lán)點(diǎn)表示。

圖4 移動(dòng)場(chǎng)景下移動(dòng)節(jié)點(diǎn)的定位結(jié)果對(duì)比

從圖4中可以看出，場(chǎng)景2中標(biāo)簽節(jié)點(diǎn)移動(dòng)出錨節(jié)點(diǎn)定位范圍時(shí)的定位誤差與場(chǎng)景1中相比較小。其中，場(chǎng)景1中標(biāo)簽節(jié)點(diǎn)定位標(biāo)準(zhǔn)差為0.76 m，而場(chǎng)景2中定位標(biāo)準(zhǔn)差為0.49 m。因此，基于雙向測(cè)距的協(xié)同定位系統(tǒng)在對(duì)移動(dòng)節(jié)點(diǎn)的定位精度上優(yōu)于傳統(tǒng)基于雙程測(cè)距定位系統(tǒng)。

在此基礎(chǔ)上，為考察基于雙向測(cè)距的協(xié)同定位系統(tǒng)中幾何分布因素對(duì)移動(dòng)節(jié)點(diǎn)定位誤差的影響，下面在仿真實(shí)驗(yàn)中加入距離因子d來(lái)進(jìn)一步對(duì)比分析，距離因子d會(huì)直接影響標(biāo)簽節(jié)點(diǎn)到錨節(jié)點(diǎn)的方向余弦向量，如圖5所示。

圖5 考慮距離因子d的協(xié)同定位系統(tǒng)

標(biāo)簽節(jié)點(diǎn)定位均方根誤差隨距離因子的變化圖如圖6所示。從圖6中可以看出，當(dāng)距離因子d較小時(shí)標(biāo)簽節(jié)點(diǎn)定位均方差較差，同時(shí)標(biāo)簽節(jié)點(diǎn)的定位均方根誤差隨著距離因子d減小而增大，其根本原因是距離因子d較小時(shí)系統(tǒng)的精度因子較差。

圖6 標(biāo)簽節(jié)點(diǎn)定位均方根誤差隨距離因子的變化

另外，如果繼續(xù)增大距離因子d，如圖7所示，可以明顯看到標(biāo)簽節(jié)點(diǎn)的定位均方根誤差隨著距離因子增大至125后大幅增加。這是由于即使增大距離因子d而帶來(lái)較好的精度因子，仍存在雙程測(cè)距方式誤差傳播的因素對(duì)標(biāo)簽節(jié)點(diǎn)定位誤差的影響。

圖7 協(xié)同定位系統(tǒng)中均方根誤差隨距離因子的變化

最后，總結(jié)對(duì)比傳統(tǒng)基于雙向測(cè)距的定位系統(tǒng)與協(xié)同定位系統(tǒng)的誤差源，如表1所示。

表1 不同定位系統(tǒng)的誤差源對(duì)比

3 結(jié)束語(yǔ)

本文主要分析了協(xié)同定位系統(tǒng)中與傳統(tǒng)基于雙向測(cè)距的定位系統(tǒng)相比需額外考慮的測(cè)距誤差傳播和移動(dòng)場(chǎng)景誤差影響。此外，為了更好地評(píng)估移動(dòng)場(chǎng)景的定位效果，提出了移動(dòng)精度因子概念。移動(dòng)精度因子實(shí)質(zhì)上是加權(quán)精度因子與誤差傳播的結(jié)合。同時(shí)，通過(guò)仿真分析了錨節(jié)點(diǎn)幾何分布的距離因子對(duì)標(biāo)簽節(jié)點(diǎn)定位誤差的影響，從仿真結(jié)果中可以看出，若將標(biāo)簽節(jié)點(diǎn)移動(dòng)的過(guò)程控制在合理的移動(dòng)精度因子范圍內(nèi)，可以得到更優(yōu)的移動(dòng)標(biāo)簽節(jié)點(diǎn)定位精度。