采用相對位置量測的緊耦合式多無人機協(xié)同定位算法

摘要：針對僅使用慣性測量單元（IMU）的自主定位導(dǎo)航存在嚴(yán)重誤差累積的問題，將IMU解算和置信傳播（BP）算法進(jìn)行緊耦合，在擴展卡爾曼濾波器（EKF）的框架下，使用無人機（UAV）間的相對位置量測直接更新大地坐標(biāo)系下各無人機的位置狀態(tài)，提出了一種適用于大地坐標(biāo)系多無人機定位與導(dǎo)航的緊耦合的分布式協(xié)同定位算法（EKF-BP）。首先，實現(xiàn)了基于IMU量測和無人機間相對位置量測的集中式協(xié)同定位算法；接著，將BP算法擴展到大地坐標(biāo)系，并與基于IMU的自主導(dǎo)航緊耦合，完成衛(wèi)星拒止環(huán)境中多無人機系統(tǒng)的協(xié)同定位與導(dǎo)航；最后，將所提出的方法與基于協(xié)方差交叉的協(xié)同定位進(jìn)行比較。仿真結(jié)果表明，分布式協(xié)同定位算法具有明顯的計算效率優(yōu)勢，與基于協(xié)方差交叉的分布式協(xié)同定位方法相比，所提EKF-BP算法提高了多無人機系統(tǒng)在衛(wèi)星拒止環(huán)境中的定位導(dǎo)航精度，是協(xié)方差交叉方法提升量的10倍左右。所提的分布式EKF-BP算法能夠顯著地抑制僅依賴IMU數(shù)據(jù)所導(dǎo)致的定位導(dǎo)航精度隨時間迅速變差的問題。

關(guān)鍵詞：集中式協(xié)同定位；分布式協(xié)同定位；置信傳播；協(xié)方差交叉

中圖分類號：TP29；V249.3"文獻(xiàn)標(biāo)志碼：A

DOI：10.7652/xjtuxb202410016"文章編號：0253-987X（2024）10-0178-10

Tightly Coupled Cooperative Localization Algorithm for Multiple Unmanned

Aerial Vehicles Based on Relative Position Measurements

LIU Shuqing， GAO Yongxin， HAN Deqiang

（Faculty of Electronic and Information Engineering， Xi’an Jiaotong University， Xi’an 710049， China）

Abstract：To tackle the error-accumulation issue of autonomous localization and navigation utilizing inertial measurement unit （IMU）， a tightly coupled approach that integrates IMU estimation and belief propagation （BP） algorithm within the framework of the extended Kalman filter （EKF） is proposed. The proposed EKF-BP method directly updates the position states of multiple unmanned aerial vehicles （UAV） in the geodetic coordinate system based on relative position measurements. Firstly， a centralized cooperative localization algorithm is implemented based on IMU measurements and relative position measurements among UAVs. Then， the BP algorithm is extended to the geodetic coordinate system and tightly coupled with IMU-based autonomous navigation to achieve cooperative localization and navigation of multiple UAV systems in satellite-denied environments. Finally， the proposed method is compared with the cooperative localization based on covariance intersection. The simulation results show that the proposed distributed cooperative localization method has an obvious advantage in computing efficiency as the number of UAVs increases. In comparison to the distributed cooperative localization based on covariance intersection， EKF-BP achieves enhanced precision in localization and navigation in satellite-denied environments， which is about 10 times the improvement of the covariance intersection method. The proposed EKF-BP method effectively suppresses the rapid accuracy degradation of dead reckoning relying merely on IMU measurements.

Keywords：centralized cooperative localization; distributed cooperative localization; belief propagation; covariance intersection

精確的定位與導(dǎo)航對無人機的大多數(shù)應(yīng)用至關(guān)重要［1］。通常情況下，無人機可通過全球?qū)Ш叫l(wèi)星系統(tǒng)［2］（global navigation satellite system， GNSS）或信標(biāo)獲取絕對位置。然而GNSS在衛(wèi)星拒止環(huán)境中不可用［3］，基于信標(biāo)的定位則需要預(yù)先布置，這一要求對于軍事應(yīng)用很難滿足。基于慣性測量單元（inertial measurement unit， IMU）或里程計的自主定位導(dǎo)航不依賴外部信息，但由于誤差累積效應(yīng)，其導(dǎo)航定位精度會隨時間迅速變差［4］。因此，拒止環(huán)境下高精度、低成本的定位導(dǎo)航算法一直是該領(lǐng)域的研究熱點。

對于工作在衛(wèi)星拒止環(huán)境中的多無人機系統(tǒng)，研究利用無人機間的相對量測來降低誤差累積效應(yīng)，提高導(dǎo)航定位精度的問題稱為協(xié)同定位（cooperative localization， CL）。協(xié)同定位能夠很好地規(guī)避傳統(tǒng)定位方法所需的高信號傳輸功率和高節(jié)點部署密度問題［2］?，F(xiàn)有的協(xié)同定位研究可以根據(jù)估計算法分為基于優(yōu)化的方法、基于概率圖的方法和基于數(shù)據(jù)融合的方法?；趦?yōu)化的算法將協(xié)同定位集中建模為非線性最小二乘問題求解，通常會帶來較大的通信負(fù)擔(dān)［5］。與之相比，基于概率圖的方法［6］將全局估計問題分解為多個局部估計函數(shù)的乘積，降低了整體估計的計算復(fù)雜度。其中，基于因子圖的置信傳播（belief propagation， BP）算法［7］由于其天然的分布式架構(gòu)、高精度估計和低成本計算而備受關(guān)注，但其涉及概率密度函數(shù)（probability density function， PDF）的積分、乘積運算以及傳輸，在實際應(yīng)用中較為困難。而高斯BP［8］、SPBP［9］和非參數(shù)BP［10］，通過將Kalman濾波器、無跡濾波器和粒子濾波器推廣到更一般的因子結(jié)構(gòu)，提供了較低復(fù)雜度的BP近似方案［9］。針對非線性量測問題，文獻(xiàn)［11］將后驗線性化濾波器［12］與高斯BP算法結(jié)合，提出了后驗線性BP算法，提高線性化的精度，從而提高CL性能。粒子濾波由于不受系統(tǒng)模型的線性和高斯假設(shè)約束，在非高斯系統(tǒng)的協(xié)同定位問題上得到了廣泛的研究和應(yīng)用［10］。考慮到大量粒子帶來的通信和計算負(fù)擔(dān)，文獻(xiàn)［13］提出了BP-MF算法，該方法兼具BP的高精度定位和平均場近似的低通信成本優(yōu)勢。在標(biāo)準(zhǔn)BP中，節(jié)點向任一鄰居發(fā)送的消息均需單獨計算，文獻(xiàn)［14］提出的SPAWN算法使用置信度代替?zhèn)鞒鱿ⅲ构?jié)點在每次迭代中對所有鄰居的傳出消息都相同，因此可以使用廣播代替點對點通信，極大地降低了通信需求。CIBP算法［15］利用網(wǎng)絡(luò)中的連通性信息評價節(jié)點位置估計的可靠性，去除可靠性低的節(jié)點信息，降低了SPAWN算法的計算負(fù)擔(dān)并提高了定位性能。對于存在環(huán)路的復(fù)雜網(wǎng)絡(luò)，BP算法可能無法收斂到全局最優(yōu)［16］。在分布式協(xié)同定位中，由共同的過程噪聲和先驗導(dǎo)致的潛在相關(guān)性很難獲?。?7］，基于協(xié)方差交叉（covariance intersection， CI）的方法能夠在節(jié)點間信息相關(guān)性未知的情況下融合多個估計，同時保持估計的一致性［18］?？紤]到節(jié)點間通信受阻的可能性，文獻(xiàn)［19］令各節(jié)點跟蹤整個系統(tǒng)的狀態(tài)并使用自身量測更新，基于鄰居信息的CI融合只在通信可用時獨立進(jìn)行，從而降低了通信故障對定位性能的影響。文獻(xiàn)［20］利用CI、逆CI提出了一種同時定位與跟蹤的分布式算法，可有效降低信息傳輸和處理的成本，并保持了估計的魯棒性。但是，基于CI的方法需要額外的計算時間來優(yōu)化均值和協(xié)方差的組合權(quán)重［21］。上述已有的協(xié)同定位研究均針對固定的笛卡爾坐標(biāo)系展開，并且大多僅限于固定坐標(biāo)系的小場景，這樣過度的簡化使其無法直接應(yīng)用于多無人機協(xié)同定位與導(dǎo)航問題。文獻(xiàn)［22］提出的EKF-CI算法雖然能直接更新大地坐標(biāo)系的位置，但受CI算法固有局限性的影響，無法獲得高精度的定位結(jié)果。

目前，對于位置定義在大地坐標(biāo)系，速度和姿態(tài)定義在局部東北天坐標(biāo)系的協(xié)同定位與導(dǎo)航的相關(guān)研究相對較少，尚未發(fā)現(xiàn)切實有效的解決辦法，潛在的間接求解的方式也無法達(dá)到直接更新的效率和精度。本文將BP算法擴展到大地坐標(biāo)系，并與基于IMU的自主導(dǎo)航緊耦合完成衛(wèi)星拒止環(huán)境中多無人機系統(tǒng)的協(xié)同定位與導(dǎo)航。通過建立觀測方程，在擴展卡爾曼濾波器（extended Kalman filter， EKF）的框架下，使用相對位置量測直接更新大地坐標(biāo)系下無人機的位置狀態(tài)，實現(xiàn)了EKF-BP的分布式協(xié)同定位。通過仿真實驗對比EKF-BP、EKF-CI的定位精度，驗證了EKF-BP的計算效率和估計性能。

1"IMU演化與相對量測模型

多無人機協(xié)同定位如圖1所示，由虛線連接的無人機互為鄰居，鄰居之間可以進(jìn)行相對位置量測，通過在鄰居間交換信息來提高整個多無人機系統(tǒng)的定位導(dǎo)航精度，每架無人機均配置IMU。

1.1"IMU狀態(tài)傳播模型

各無人機的IMU狀態(tài)向量定義如下

x=（re）T（vl）T（El）T（bg）T（ba）TT（1）

式中：re=φλhT為大地坐標(biāo)系的緯經(jīng)高；vl=vEvNvUT為東北天（ENU）坐標(biāo)系中的速度；El為俯仰、橫滾、偏航Euler角；bg、ba分別為陀螺儀偏置和加速度計漂移。

IMU狀態(tài)演化模型為

ellb=D－1vlRlbfb－（2Ωlie+Ωlel）vl+gl

12qlbωblb（2）

式中：D－1參考文獻(xiàn)［23］；fb、gl分別為比力和ENU系的重力加速度。旋轉(zhuǎn)矩陣Rlb和四元數(shù)qlb描述載體坐標(biāo)系到ENU坐標(biāo)系的轉(zhuǎn)換，均由姿態(tài)角變換得到；反對稱矩陣Ωlie、Ωlel由角速度向量ωlie、ωlll變換得到。為了方便計算，姿態(tài)的解算由四元數(shù)方程描述，表示四元數(shù)和向量的乘積。

IMU量測模型為

ωm=ωbib+bg+ng（3）

fm=fb+ba+na（4）

陀螺儀偏置bg和加速度計漂移ba為兩個隨機游走過程，有如下演化形式

ga=nwgnwa（5）

設(shè)nwg、nwa、ng、na都為零均值的白噪聲，那么比力fb和角速度ωbib可以由下式估計得到，即

b=fm－a（6）bib=ωm－g（7）

獲得比力fb和角速度ωbib的估計后，可以通過對式（2）的數(shù)值積分得到僅基于IMU數(shù)據(jù)的位姿估計，此過程即航位推算（DR）。為了使用相對量測對IMU的狀態(tài)估計進(jìn)一步更新，需要知道IMU狀態(tài)估計所對應(yīng)的均方根誤差（MSE），可通過誤差狀態(tài)模型來獲得。

1.2"誤差狀態(tài)傳播模型

IMU誤差狀態(tài)向量定義為

=（e）T（l）T（l）T（g）T（a）TT（8）

IMU的位置、速度和偏差的誤差定義為估計和真值的差，姿態(tài)的誤差定義則需要在旋轉(zhuǎn)矩陣的基礎(chǔ)上給出。若有旋轉(zhuǎn)矩陣的估計值bl、真值Rbl，姿態(tài)誤差可由誤差旋轉(zhuǎn)矩陣bl描述，三者的關(guān)系為

Rlb=lblb（9）

根據(jù)Euler角與旋轉(zhuǎn)矩陣的轉(zhuǎn)換關(guān)系，可得到對應(yīng)的姿態(tài)誤差l。

IMU誤差狀態(tài)的線性化連續(xù)時間模型為

x～·=F+Gn（10）

式中n=nTgnTwgnTanTwaT表示系統(tǒng)噪聲，對應(yīng)的協(xié)方差矩陣為Q，矩陣F、G分別為

F=0D－100000－Fl0－Rlb0F320－Rlb00000000000（11）

G=000000Rlb0Rlb0000I300000I3（12）

式中：0為具有合適維度的零矩陣；I3為3×3的單位矩陣；Fl為ENU坐標(biāo)系中加速度計量測的反對稱矩陣；F32的定義參考文獻(xiàn)［23］。

對連續(xù)時間系統(tǒng)離散化，可以得到誤差傳播模型的離散系統(tǒng)，以便于得到誤差協(xié)方差矩陣的傳播方程，即

（tk）=Φ（tk，tk－1）（tk－1）+G（tk－1）n（13）

Φ（tk，tk－1）=e∫tktk－1F（τ）dτ（14）

則誤差的協(xié)方差矩陣（即狀態(tài)估計對應(yīng)的MSE矩陣）可由下式計算

Pkk－1=Φ（tk，tk－1）Pk－1k－1ΦT（tk，tk－1）+Qd（15）

Qd=∫tktk－1Φ（tk，τ）G（τ）Q（τ）GT（τ）ΦT（tk，τ）dτ（16）

1.3"相對量測模型及其線性化

將N架無人機的集合表示為。如果在tk時刻，無人機i能夠獲得對無人機j的相對量測（i，j∈），則稱無人機j為無人機i的鄰居，記作j∈ki。無人機i獲得的相對量測zi，jk（區(qū)分zi，jk和zj，ik）被建模為地心地固（ECEF）笛卡爾坐標(biāo)系下節(jié)點間的相對位置，即

zi，jk=c（xjk）－c（xik）+vi，jk，"i∈， j∈ik（17）

式中：xlk（l=i，j）為無人機l在tk時刻的狀態(tài)；c（xlk）由無人機位置re和地球的第一偏心率e計算，表示大地坐標(biāo)系到笛卡爾坐標(biāo)系的轉(zhuǎn)換，即

c（xlk）=xceycezce=（RN+h）cosφcosλ

（RN+h）cosφsinλ

RN（1－e2）+hsinφ（18）

關(guān)于xlk的雅可比矩陣記作Hl，posk；vi，jk表示量測噪聲，是均值為零、協(xié)方差為Ri，jk的高斯白噪聲，且假設(shè)所有量測噪聲、過程噪聲和初始狀態(tài)相互獨立。將量測函數(shù)在tk時刻的狀態(tài)預(yù)測lkk－1（l=i，j）處泰勒展開，可得如下的線性量測模型

zi，jk=i，jk|k－1+Hik（xik－ik|k－1）+

Hjk（xjk－jk|k－1）+O（x）（19）

式中：i，jk|k－1為量測的預(yù)測；

Hik=－Hi，posk0; Hjk=Hj，posk0（20）

2"集中式協(xié)同定位

集中式協(xié)同定位僅是一個狀態(tài)和量測都擴維的濾波問題。如果忽略濾波方法上的近似，集中式協(xié)同定位由于使用所有原始量測并充分利用狀態(tài)間的相關(guān)性，因而具有最佳的估計性能。換而言之，集中式協(xié)同定位提供了所有協(xié)同定位算法的精度上限，因此通常作為評估分布式算法性能的基準(zhǔn)。因此，本文描述集中式協(xié)同定位問題，并實現(xiàn)大地坐標(biāo)系下基于相對位置量測的集中式協(xié)同定位算法，用于評估本文所提出的分布式算法。

集中式協(xié)同定位可描述為在每個tk時刻使用測量zi，jk（i∈，j∈ik）確定所有節(jié)點的后驗分布的問題，即

f（Xk|Z1：k）∝f（Zk|Xk）f（Xk|Z1：k－1）（21）

f（Xk|Z1：k－1）=∫f（Xk|Xk－1）f（Xk－1|Z1：k－1）dXk－1（22）

式中：Xk為tk時刻的所有節(jié)點狀態(tài)；Z1：k為直到tk時刻獲得的所有可用測量，關(guān)系式如下

Xk［（x1k）T，…，（xNk）T］T（23）

Z1：k［（Z1）T，…，（Zk）T］T（24）

Zk［（Z1k）T，…，（ZNk）T］T（25）

Zik［zi，j1k，…，zi，jnk］T，jn∈ik（26）

設(shè)各無人機先驗狀態(tài)獨立并且進(jìn)行無記憶的獨立運動。注意到式（21）對應(yīng)于貝葉斯濾波器的更新步驟，而式（22）則對應(yīng)于預(yù)測步驟，下面討論無人機集中式協(xié)同定位的具體實現(xiàn)。

各無人機的狀態(tài)通過IMU模型進(jìn)行一步演化，將演化后未被相對量測更新的估計記為ik|k－1（i∈），即一步預(yù)測，類似式（23）將所有節(jié)點的狀態(tài)預(yù)測進(jìn)行向量堆疊得到skk－1，其MSE矩陣記作Pskk－1，傳播模型為

Pskk－1=ΦsPsk－1k－1（Φs）T+Qsd（27）

式中：Φs、Qsd分別為以Φi（tk，tk－1）、Qid作為對角線的矩陣，上標(biāo)i表示第i架無人機，且i∈。

類似式（25）、（26），將tk時刻無人機間的所有相對位置量測和基于狀態(tài)預(yù)測得到的量測預(yù)測分別進(jìn)行向量堆疊。使用堆疊量測Zsk基于EKF中的更新步驟計算sk來校正狀態(tài)估計。位置和速度的更新結(jié)果為skk－1、sk的差，姿態(tài)更新則需要引入旋轉(zhuǎn)矩陣計算，即

sk=Kk（sk－Zsk）（28）

Kk=Psk|k－1（Hsk）T（HskPsk|k－1（Hsk）T+Rsk）－1（29）

Hsk=［（Hi，jk）Ti∈，j∈ik］T（30）

Hi，jk=0…Hik…Hjk…0（31）

式中：Rsk為以Ri，jk作為對角線構(gòu)造的矩陣；Kk為濾波增益矩陣；矩陣Hik和Hjk由式（20）得到。狀態(tài)估計對應(yīng)的MSE矩陣更新如下

Psk|k=（I－KkHsk）Psk|k－1（I－KkHsk）T+

KkRsk（Kk）T（32）

集中式協(xié)同定位假設(shè)存在融合中心，且所有數(shù)據(jù)都需傳送到融合中心［24］，當(dāng)節(jié)點數(shù)較多且通信資源受限時很難實現(xiàn)。

3"基于BP的分布式協(xié)同定位

考慮到集中式算法的局限性，結(jié)合IMU狀態(tài)演化和節(jié)點間相對位置量測，本節(jié)給出無人機分布式協(xié)同定位的因子圖，并完成IMU演化與BP緊耦合的協(xié)同定位。

3.1"BP算法

基于Z1：k進(jìn)行貝葉斯推理的目標(biāo)是獲得每個狀態(tài)xik的邊緣后驗PDFf（xik|Z1：k）。因子圖能夠?qū)⒙?lián)合概率密度函數(shù)因式分解并用圖形化的模型描述［25］，將復(fù)雜的全局運算分解為簡單的局部運算，計算效率高，且方便增減因子節(jié)點，在協(xié)同定位與導(dǎo)航中得到廣泛運用，因此在因子圖中迭代執(zhí)行BP來近似邊緣后驗PDF?；讵毩⑿约僭O(shè)，聯(lián)合PDFf（X0：k|Z1：k）可以如下的因式分解，其中X0：k表示直到tk時刻的所有節(jié)點狀態(tài)，即

f（X0：k|Z1：k）∝

∏ni∈f（xi0）∏kt=1∏i1∈{f（xi1t|xi1t－1）f（zi1t|xi1t，xi1t－1）·

∏j∈i1kf（zi1，jt|xi1t，xjt）}（33）

式中：X0：k（X0）T…（Xk）T。將式（33）映射到如圖2所示的因子圖中。因子圖是一種表示因式分解結(jié)構(gòu)的二部圖，包含兩種類型的節(jié)點，其中，每個狀態(tài)變量節(jié)點xik由圓形表示，每個因子節(jié)點fjk由方形表示。圖2中fik=f（zik|xik，xik－1）f（xik|xik－1），fi，jk=f（zi，jk，zj，ik|xik，xjk）。當(dāng)且僅當(dāng)xik是fjk的一個參數(shù)時，因子圖中對應(yīng)的因子節(jié)點和變量節(jié)點用邊連接。在因子圖中執(zhí)行BP算法，沿著因子圖的邊傳播消息，并計算各節(jié)點的傳入消息、傳出消息和置信度，變量節(jié)點x的置信度即為其邊緣概率密度，記作b（x）。

節(jié)點接收到的消息將沿著連接的邊發(fā)送給除了傳入方向外的所有方向。當(dāng)整個因子圖中的消息傳遞完成后，變量節(jié)點對應(yīng)的置信度等于所有傳入消息的乘積。

圖2中，xik在第p次迭代時的置信度為

bp（xik）∝m→k（xik）∏j∈ikmpj→i（xik），i∈（34）

m→k（xik）∝

∫f（zik|xik，xik－1）f（xik|xik－1）bp（xik－1）dxik－1（35）

式中：m→k（xik）為從變量節(jié)點xik－1傳遞到xik的消息。

在第p次迭代中，從因子節(jié)點f（zi，jk|xik，xjk）傳遞到變節(jié)點的xik消息mpj→i（xik），j∈ik為mpj→i（xik）∝∫f（xi，jk|xik，xjk）np－1j→i（xjk）dxjk（36）

np－1j→i（xjk）∝m→k（xjk）∏l∈j→ikmp－1l→j（xjk）（37）

式中：np－1j→i（xik）為在第p－1次迭代中，從變量節(jié)點xjk傳遞到因子節(jié)點f（zi，jk|xik，xjk）的消息。

對于包含環(huán)路的因子圖，需要對BP進(jìn)行多次迭代才能獲得較好的近似解。本文僅考慮信息沿時間的前向傳播，不考慮回溯，這樣雖會造成一定的性能損失，但是極大地降低了算法復(fù)雜度和計算量。在實際計算資源允許時，可以采用擴維方法使用少量步數(shù)的回溯以進(jìn)一步提高定位性能［24］。

3.2"EKF-BP算法

本節(jié)使用EKF實現(xiàn)BP算法，并在大地坐標(biāo)系直接更新各節(jié)點的狀態(tài)，提出EKF-BP算法。設(shè)式（35）中的消息m→k（xik）是高斯密度函數(shù)，記作m→k（xik）∝（xik;ik|k－1，Pik|k－1），其中ik|k－1和Pik|k－1通過IMU狀態(tài)及誤差傳播模型遞推得到。同樣地，式（37）的消息np－1j→i（xjk）∝（xjk;j，p－1k|k－1，Pj，p－1k|k－1），其中j，p－1k|k－1和Pj，p－1k|k－1由無人機j進(jìn)行一步預(yù)測，并接收其鄰居l（l∈j→ik）的消息進(jìn)行p－1次迭代得到。置信度bp（xik）∝（xik;ik|k，Pik|k），其中ik|k和Pik|k為無人機i執(zhí)行p次迭代后的估計結(jié)果。迭代從無人機j的一步預(yù)測開始，即n0j→i（xjk）=m→k（xjk）。

式（34）本質(zhì)上是貝葉斯濾波器的更新步驟，在EKF的框架下實現(xiàn)此更新。類似式（23），將無人機i的狀態(tài)ik|k－1和鄰居j發(fā)送的第p－1次迭代后的狀態(tài)j，p－1k|k－1（j∈ik）進(jìn)行堆疊，記作i，sk|k－1；以對應(yīng)的MSE矩陣構(gòu)造塊對角線矩陣，記作Pi，sk|k－1。濾波增益計算式為

Kik=Pi，sk|k－1（Hik）T（Hi，skPi，sk|k－1（Hi，sk）T+Rik）－1（38）

設(shè)無人機之間不共享相對量測，無人機i的狀態(tài)更新只依賴于自身產(chǎn)生的量測zi，jk（j∈ik）。因此，堆疊量測Zik和堆疊量測預(yù)測ik可如式（26）得到，對應(yīng)的量測噪聲矩陣Rik以Ri，jk（j∈ik）為對角線構(gòu)造，雅可比矩陣Hi，sk由式（31）所示的Hi，jk堆疊得到

Hi，sk=［（Hi，jk）Tj∈ik］（39）

根據(jù)EKF公式更新堆疊狀態(tài)估計i，sk|k－1及其對應(yīng)的MSE矩陣Pi，sk|k－1，其中姿態(tài)更新通過旋轉(zhuǎn)矩陣實現(xiàn)，即

i，sk=i，sk|k－1－Kik（ik－Zik）（40）

Pi，sk=（I－KikHi，sk）Pi，sk|k－1（I－KikHi，sk）T+

KikRik（Kik）T（41）

雖然式（40）本質(zhì)上是使用zi，jk更新無人機i和j的狀態(tài)估計，但是該更新在無人機i完成，因此僅保留更新后無人機i的狀態(tài)估計ik|k，即i，sk的前15維。對應(yīng)的MSE矩陣Pik|k為Pi，sk的前15×15維。

4"基于EKF-CI的分布式CL算法

BP算法可以在樹狀結(jié)構(gòu)的因子圖中提供精確的估計。然而，如果因子圖中存在環(huán)路，BP算法不一定保證收斂［16， 26］。EKF-CI算法由于其不考慮相關(guān)性而融合的特點，在增加魯棒性的同時，會失去一定的估計精度。本節(jié)實現(xiàn)大地坐標(biāo)系下基于EKF-CI的協(xié)同定位算法，以便于與EKF-BP算法進(jìn)行比較。

當(dāng)無人機i與鄰居j通信，共享位置信息時，首先基于相對量測zi，jk和鄰居j在tk時刻的狀態(tài)估計jkk－1獲得對無人機i在ECEF笛卡爾坐標(biāo)系的位置估計（ik|k－1）rel，使用CI算法融合ikk－1和（ik|k－1）rel，并完成對無人機i的狀態(tài)更新，即

ikk=ikk－1－Kk（ik|k－1－（ik|k－1）rel）（42）

基于量測的估計（ik|k－1）rel為

（ik|k－1）rel=zi，jk－jkk－1（43）

對應(yīng)的MSE為

Prelk|k－1=HjkPjk|k－1（Hjk）T+Rk（44）

濾波增益Kk的計算式為

Kk=Pik（Hik）T（HikPik（Hik）T+Pjk）－1（45）

協(xié)方差矩陣Pik、Pjk為

Pik=Pik|k－1Wik， Pjk=Prelk|k－11－Wik（46）

參數(shù)Wik∈［0，1］是如下優(yōu)化問題的解

minWik［tr（Pikk）］（47）

式中：tr（P）為矩陣P的跡；Pikk為融合狀態(tài)估計的對應(yīng)MSE，計算式為

Pikk=（I－KkHik）Pik（I－KkHik）T+

KkRk（Kk）T（48）

5"仿真實驗與結(jié)果分析

3架可互相通信的無人機在高度500m處編隊飛行，各無人機在大地坐標(biāo)系的初始位置分別為

r10=34.2501°108.6594°500mT

r20=34.2601°108.6594°500mT

r30=34.2701°108.6594°500mT

3架無人機在東北天坐標(biāo)系下的初始速度均為

vi0=70m/s70m/s70m/sT，i=1，2，3

初始姿態(tài)角、加速度計及陀螺儀偏置均為0。根據(jù)各無人機的初始狀態(tài)生成真實軌跡，在真實軌跡的基礎(chǔ)上使用IMU模型生成IMU量測，基于相對量測模型生成相對位置量測。仿真中涉及的噪聲均為0均值的高斯白噪聲，其中，相對量測噪聲的協(xié)方差設(shè)置為10－2I3，IMU量測噪聲協(xié)方差分別設(shè)置為

nig=nia=10－8I3，niwa=10－12I3，i=1，2，3

n1wg=10－14I3，n2wg=n3wg=10－13I3

仿真持續(xù)時長為100s，IMU量測的數(shù)據(jù)率為100Hz，相對量測的數(shù)據(jù)率為1Hz。每次獲得相對量測值時分別采用集中式、EKF-BP和EKF-CI對IMU演化的狀態(tài)估計執(zhí)行更新，其中，仿真對BP算法進(jìn)行2次迭代。仿真共執(zhí)行了500次蒙特卡羅，使用各節(jié)點的均方根誤差（RMSE）的平均值來比較估計精度。圖3～圖5給出各算法的絕對位置、絕對速度和姿態(tài)的節(jié)點平均RMSE隨時間變化的曲線。圖3顯示，CL算法的絕對定位精度在各個位置維度都比DR高，精度最低的EKF-CI算法也能在DR的基礎(chǔ)上提高定位精度。

表1給出各CL方法在100s時獲得的位置估計的節(jié)點平均RMSE，并以DR作為基準(zhǔn)計算各算法對定位精度的提升量。仿真結(jié)果印證了CL的優(yōu)越性，能夠通過協(xié)同來抑制IMU的累計誤差，其中集中式算法由于對量測和節(jié)點狀態(tài)相關(guān)性的充分利用，提供了所有CL算法的精度上限。本文所提出的EKF-BP算法作為一種分布式算法，其絕對定位精度的提升量能達(dá)到集中式方法的約65%，是分布式EKF-CI提升量的10倍左右，并且能夠很好地利用相對量測抑制DR的誤差累積，改善整個網(wǎng)絡(luò)的定位精度。此外，從圖4、5可以看出，盡管量測中只包含相對位置信息，但是EKF-BP的速度和姿態(tài)精度明顯高于DR的結(jié)果，說明位置精度的提高同樣有助于改善其他維度的精度。

為驗證網(wǎng)絡(luò)拓?fù)鋵Ω鲄f(xié)同定位算法性能的影響，在不同拓?fù)滏溄雍筒煌?guī)模的網(wǎng)絡(luò)中分別實現(xiàn)上述算法，表2給出100s時各CL算法相對于DR結(jié)果的定位精度提升量。在全連接拓?fù)浜痛笮途W(wǎng)絡(luò)規(guī)模的無人機集群中，各無人機能獲得的量測信息量相較于鏈狀拓?fù)浜托⌒途W(wǎng)絡(luò)極大增加。

相較于鏈狀拓?fù)洌诓捎萌B接拓?fù)涞臒o人機集群中，EKF-BP算法能夠獲得更好的定位效果，并且隨著無人機節(jié)點數(shù)量的增加，其定位性能進(jìn)一步得到提升，與集中式算法結(jié)果的差距逐漸縮小，主要原因如下：①非線性的量測模型需通過Taylor展開來完成線性化，用局部導(dǎo)數(shù)代替函數(shù)整體均值，導(dǎo)致估計精度下降；②多無人機分布式協(xié)同定位的因子圖中存在環(huán)路，因此BP算法只能提供近似解，造成一定程度的精度損失；③在全連接的網(wǎng)絡(luò)拓?fù)渲?，可用于協(xié)同定位的節(jié)點量測信息較鏈狀拓?fù)錁O大增加，隨著網(wǎng)絡(luò)規(guī)模的增大，這一差距愈發(fā)明顯。EKF-BP算法的定位性能隨之得到明顯改善，更接近集中式算法的性能。

為驗證所提分布式定位方法的計算效率，表3給出了各CL算法在不同規(guī)模的全連接網(wǎng)絡(luò)中進(jìn)行100次蒙特卡洛的平均計算時間。其中EKF-BP為進(jìn)行一次迭代的結(jié)果，并且此時定位精度較DR仍有明顯提高，甚至接近多次迭代的精度，說明所提EKF-BP算法的收斂速度很快，能夠通過有限的迭代達(dá)到較高的定位精度。

從表3可以看出：隨著節(jié)點增多，集中式優(yōu)化計算時間增長較分布式更加明顯。由于分布式算法在各無人機節(jié)點中并行進(jìn)行，其計算效率明顯優(yōu)于集中式，隨著網(wǎng)絡(luò)規(guī)模的增大，分布式算法的計算優(yōu)勢更為顯著；雖然EKF-CI算法通常具有最低的計算成本，但是其相對于DR的精度提高十分有限。綜上分析可得如下結(jié)論。

（1）在IMU自主定位導(dǎo)航的基礎(chǔ)上，通過CL算法利用無人機之間的相對位置信息，無人機網(wǎng)絡(luò)的導(dǎo)航定位精度得到了提高。由于位置和姿態(tài)之間的相關(guān)性，無人機的姿態(tài)估計精度同樣有所提高。

（2）隨著拓?fù)滏溄訑?shù)和網(wǎng)絡(luò)節(jié)點數(shù)的增加，CL算法對IMU累積誤差的抑制作用愈發(fā)明顯。在大規(guī)模的無人機網(wǎng)絡(luò)中，分布式協(xié)同定位算法具有明顯的計算效率優(yōu)勢，與EKF-CI算法相比，所提的EKF-BP算法擁有更高的定位性能。

（3）所提的分布式EKF-BP算法能夠在較少的迭代次數(shù)下獲得較高的定位精度，較好解決了IMU導(dǎo)航在缺少絕對位置信息時，精度隨時間迅速降低的問題，在大規(guī)模全連接網(wǎng)絡(luò)中具有很好的性能。

6"結(jié)"論

本文從提高IMU定位導(dǎo)航精度的角度出發(fā)，利用網(wǎng)絡(luò)節(jié)點間相對位置信息，將現(xiàn)有BP算法的應(yīng)用場景拓展到大地坐標(biāo)系，研究了一種衛(wèi)星拒止時緊耦合的分布式無人機協(xié)同定位導(dǎo)航算法，與EKF-CI算法相比，本文提出的EKF-BP算法具有更精確的定位結(jié)果，各維度的絕對位置估計精度較DR分別提高了24.5%、30.54%和29.39%。雖然本文的研究是基于相對位置量測來實現(xiàn)的，但其同樣能夠處理實際中常用的極坐標(biāo)量測模型，只需通過量測轉(zhuǎn)換方法對極坐標(biāo)系量測進(jìn)行預(yù)處理，將其轉(zhuǎn)換為本文中的量測模型即可。

參考文獻(xiàn)：

［1］李清華， 高影， 王振桓， 等. 一種動態(tài)分組的多節(jié)點協(xié)同定位編隊構(gòu)型優(yōu)化方法 ［J］. 中國慣性技術(shù)學(xué)報， 2022， 30（6）： 746-751.

LI Qinghua， GAO Ying， WANG Zhenhuan， et al. A dynamic grouping formation configuration optimization method for multi-node cooperative localization ［J］. Journal of Chinese Inertial Technology， 2022， 30（6）： 746-751.

［2］HAN Ke， ZHANG Chongyu， XING Huashuai， et al. An area optimization based cooperative localization algorithm with node selection ［J］. China Communications， 2021， 18（12）： 178-195.

［3］CHOI J， MYUNG H. BRM localization： UAV localization in GNSS-denied environments based on matching of numerical map and UAV images ［C］//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ， USA： IEEE， 2020： 4537-4544.

［4］PAULL L， SAEEDI S， SETO M， et al. AUV navigation and localization： a review ［J］. IEEE Journal of Oceanic Engineering， 2014， 39（1）： 131-149.

［5］PIRES A G， REZECK P A F， CHAVES R A， et al. Cooperative localization and mapping with robotic swarms ［J］. Journal of Intelligent amp; Robotic Systems， 2021， 102（2）： 47.

［6］YAN Zheping， LUAN Zhengkun， LIU Jianbo， et al. A cooperative localization method for multiple unmanned underwater vehicles based on improved factor graph ［C］//2023 IEEE International Conference on Mechatronics and Automation. Piscataway， NJ， USA： IEEE， 2023： 894-899.

［7］WAINWRIGHT M J， JAAKKOLA T S， WILLSKY A S. Tree-based reparameterization framework for analysis of sum-product and related algorithms ［J］. IEEE Transactions on Information Theory， 2003， 49（5）： 1120-1146.

［8］SU Qinliang， WU Y C. Convergence analysis of the variance in gaussian belief propagation ［J］. IEEE Transactions on Signal Processing， 2014， 62（19）： 5119-5131.

［9］MEYER F， HLINKA O， HLAWATSCH F. Sigma point belief propagation ［J］. IEEE Signal Processing Letters， 2014， 21（2）： 145-149.

［10］WIELANDNER L， LEITINGER E， MEYER F， et al. Message passing-based 9-D cooperative localization and navigation with embedded particle flow ［J］. IEEE Transactions on Signal and Information Processing Over Networks， 2023， 9： 95-109.

［11］GARCA-FERNNDEZ F， SVENSSON L， SRKK S. Cooperative localization using posterior linearization belief propagation ［J］. IEEE Transactions on Vehicular Technology， 2018， 67（1）： 832-836.

［12］GARCA-FERNNDEZ F， SVENSSON L， MORELANDE M R， et al. Posterior linearization filter： principles and implementation using sigma points ［J］. IEEE Transactions on Signal Processing， 2015， 63（20）： 5561-5573.

［13］AKMAK B， URUP D N， MEYER F， et al. Cooperative localization for mobile networks： a distributed belief propagation-mean field message passing algorithm ［J］. IEEE Signal Processing Letters， 2016， 23（6）： 828-832.

［14］WYMEERSCH H， LIEN J， WIN M Z. Cooperative localization in wireless networks ［J］. Proceedings of the IEEE， 2009， 97（2）： 427-450.

［15］KIM H， CHOI S W， KIM S. Connectivity information-aided belief propagation for cooperative localization ［J］. IEEE Wireless Communications Letters， 2018， 7（6）： 1010-1013.

［16］CORNELISSEN K， MANTHEY B. Belief propagation for the maximum-weight independent set and minimum spanning tree problems ［J］. Theoretical Computer Science， 2018， 738： 53-64.

［17］FANG Susu， LI Hao， YANG Ming. Adaptive cubature split covariance intersection filter for multi-vehicle cooperative localization ［J］. IEEE Robotics and Automation Letters， 2022， 7（2）： 1158-1165.

［18］SUN Shuli. Multi-sensor optimal information fusion Kalman filters with applications ［J］. Aerospace Science and Technology， 2004， 8（1）： 57-62.

［19］CHANG T K， CHEN K， MEHTA A. Resilient and consistent multirobot cooperative localization with covariance intersection ［J］. IEEE Transactions on Robotics， 2022， 38（1）： 197-208.

［20］LIANG Mingchao， MEYER F. Neural enhanced belief propagation for cooperative localization ［C］//2021 IEEE Statistical Signal Processing Workshop. Piscataway， NJ， USA： IEEE， 2021： 326-330.

［21］楊秀建， 謝永燾. 基于啟發(fā)式調(diào)諧EH∞F的多機器人協(xié)同定位 ［J］. 中國慣性技術(shù)學(xué)報， 2023， 31（1）： 24-32.

YANG Xiujian， XIE Yongtao. Multi-robot cooperative localization based on heuristically tuned extended H∞ filter ［J］. Journal of Chinese Inertial Technology， 2023， 31（1）： 24-32.

［22］GOEL S， KEALYA， GIKAS V， et al. Cooperative localization of unmanned aerial vehicles using GNSS， MEMS inertial， and UWB sensors ［J］. Journal of Surveying Engineering， 2017， 143（4）： 04017007.

［23］NOURELDIN A， KARAMAT T B， GEORGY J. Fundamentals of inertial navigation， satellite-based positioning and their integration ［M］. Berlin， Germany： Heidelberg， 2013.

［24］ZHANG Bolun， GAO Guangen， GAO Yongxin. Cooperative localization based on augmented state belief propagation for mobile agent networks ［J］. Electronics， 2022， 11（13）： 1959.

［25］MA Xiaoshuang， LIU Xixiang， LI Chenlong， et al. Multi-source information fusion based on factor graph in autonomous underwater vehicles navigation systems ［J］. Assembly Automation， 2021， 41（5）： 536-545.

［26］SU Qinliang， WU Y C. On convergence conditions of Gaussian belief propagation ［J］. IEEE Transactions on Signal Processing， 2015， 63（5）： 1144-1155.

（編輯"趙煒）