熊 智，李欣童，熊 駿，陳明星，劉建業(yè)

(1. 南京航空航天大學(xué)自動化學(xué)院導(dǎo)航研究中心，南京 211106； 2. 先進(jìn)飛行器導(dǎo)航、控制與健康管理工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室，南京 211106)

近年來，無人機(jī)飛行任務(wù)已經(jīng)從單一無人機(jī)向集群無人機(jī)的方向發(fā)展，無人機(jī)集群通過信息的相互合作與傳輸，執(zhí)行多個(gè)無人機(jī)的特殊編隊(duì)任務(wù)[1]。它不是簡單意義上“量”的增加，而是在智能上有“質(zhì)”的突破，具備在復(fù)雜動態(tài)環(huán)境下能實(shí)現(xiàn)大規(guī)模自主性的集群協(xié)同效應(yīng)的能力，有著任務(wù)分散化、分布式智能化、去中心自主化等潛在價(jià)值。

無人機(jī)任務(wù)執(zhí)行中需要高精度定位信息，在近距離編隊(duì)飛行系統(tǒng)中，無人機(jī)需要準(zhǔn)確、實(shí)時(shí)的定位信息來進(jìn)行避碰、任務(wù)調(diào)度和精確控制[2][3]。受成本與無人機(jī)機(jī)載的約束，只有少量無人機(jī)能攜帶高性能傳感器[4]。針對大規(guī)模集群無人機(jī)的定位精度問題，國內(nèi)外學(xué)者對協(xié)同定位算法進(jìn)行了探究。與傳統(tǒng)的僅依賴單個(gè)無人機(jī)傳感器的多源融合方法相比，協(xié)同定位充分利用了協(xié)同網(wǎng)絡(luò)中所有節(jié)點(diǎn)之間的幾何關(guān)系和所有機(jī)載傳感器的觀測數(shù)據(jù)[5,6]。文獻(xiàn)[7]針對非線性模型，設(shè)計(jì)了一種動態(tài)非參數(shù)置信傳播算法獲得無人機(jī)位置信息，但計(jì)算量較大，只對二維空間進(jìn)行了仿真，不適合大規(guī)模集群無人機(jī)使用。文獻(xiàn)[8]提出了一種基于相對位置估計(jì)和置信傳播的協(xié)同定位框架。利用消息傳遞算法的優(yōu)勢結(jié)合卡爾曼濾波，實(shí)現(xiàn)低成本近距無人機(jī)編隊(duì)協(xié)同定位。

實(shí)時(shí)性是集群無人機(jī)協(xié)同定位的一項(xiàng)技術(shù)痛點(diǎn)，網(wǎng)絡(luò)中存在大量過剩冗余的協(xié)同信息，對定位精度貢獻(xiàn)度不高，導(dǎo)致計(jì)算負(fù)荷大且無法滿足精度及實(shí)時(shí)性需求。如何對節(jié)點(diǎn)的協(xié)同信息進(jìn)行篩選，找到穩(wěn)定可靠的信源無人機(jī)是集群無人機(jī)中需要解決的難題。集群的協(xié)同精度不僅與自身的傳感器設(shè)備有關(guān)，還與飛行構(gòu)型有關(guān)，構(gòu)型的優(yōu)劣可以用幾何精度因子（GDOP）量化，GDOP 越小，對集群定位就越有利。文獻(xiàn)[9]分析了節(jié)點(diǎn)數(shù)量和傳感器精度對無線傳感器網(wǎng)絡(luò)定位精度的影響，提出了定位精度下限的計(jì)算方法。文獻(xiàn)[10]使用無人車輔助無人機(jī)的導(dǎo)航，無人車位置設(shè)計(jì)用于減少無人車和無人機(jī)之間的GDOP，仿真結(jié)果顯示大幅降低了無人機(jī)的定位誤差。因此對集群無人機(jī)導(dǎo)航中的信源節(jié)點(diǎn)進(jìn)行篩選，可以簡化協(xié)同定位算法的過程。麻雀搜索算法（Sparrow Search Algortihm, SSA）是2020 年由shen 提出的一種最新的群智優(yōu)化算法，比傳統(tǒng)的優(yōu)化算法具有精度高，穩(wěn)定性好等特點(diǎn)[11]，常用于處理非線性復(fù)雜系統(tǒng)的優(yōu)化問題，適用于集群編隊(duì)這類實(shí)時(shí)性要求高的應(yīng)用場景。將SSA 算法應(yīng)用到信源無人機(jī)的篩選中，利用GDOP作為篩選標(biāo)準(zhǔn)，可以選擇出最優(yōu)的組合方式。因此研究基于SSA 優(yōu)化的無人機(jī)集群協(xié)同定位方法，從而提高導(dǎo)航系統(tǒng)性能，具有重要的研究意義。

本文提出了一種基于改進(jìn)SSA 的無人機(jī)集群置信傳播（Belief Propagation, BP）協(xié)同定位方法，利用BP 框架不斷進(jìn)行信息更新推斷，在無人機(jī)進(jìn)行信息交互時(shí)，提出了以GDOP 值為適應(yīng)度函數(shù)的改進(jìn)SSA，尋優(yōu)得到最優(yōu)幾何構(gòu)型，與該最優(yōu)構(gòu)型中的無人機(jī)進(jìn)行信息交互，從而實(shí)現(xiàn)了對低貢獻(xiàn)度信息的排除和篩選，在確保精度的基礎(chǔ)上減少計(jì)算量，最終仿真驗(yàn)證本方法的有效性。

1 集群無人機(jī)置信傳播協(xié)同定位方法

假設(shè)有N架無人機(jī)，無人機(jī)i從0 時(shí)刻到t時(shí)刻的所有狀態(tài)信息量觀測量包括衛(wèi)星和相鄰無人機(jī)的相對測距信息。假設(shè)具有一階馬爾可夫性，即根據(jù)貝葉斯定理，預(yù)測和更新過程為

式(1)為預(yù)測過程，t時(shí)刻之前所有觀測量式(2)為更新過程，通過t時(shí)刻獲得的最新觀測信息按照式(2)更新。初始化條件為因此可得后驗(yàn)概率分布為

通過式(3)構(gòu)建因子圖，進(jìn)而采用置信傳播算法求解。無人機(jī)群定位問題通過因子圖模型的形式進(jìn)行建模，即可求得任一節(jié)點(diǎn)的狀態(tài)，協(xié)同定位問題即為計(jì)算t時(shí)刻前機(jī)載導(dǎo)航信息和觀測信息的后驗(yàn)概率分布，即位置置信度(Positional Belief, PB)：

根據(jù)集群協(xié)同定位的特征，將與變量有關(guān)的因子分為預(yù)測因子和修正因子。其中修正因子包含兩類：自身觀測信息,iselfz和與鄰節(jié)點(diǎn)相互觀測信息(j屬于i節(jié)點(diǎn)的一個(gè)鄰節(jié)點(diǎn))。由此建立無人機(jī)集群協(xié)同定位過程的因子圖模型[12]。如圖1 為BP 圖模型推理。

圖1 編隊(duì)無人機(jī)協(xié)同定位因子圖Fig.1 FG of swarm UAVs localization system

結(jié)合集群無人機(jī)的實(shí)際情況，給出下列合理假設(shè)：

① 所有無人機(jī)的初始位置信息和狀態(tài)信息更

新過程是相互獨(dú)立：

③ 無人機(jī)自身的觀測信息在不同時(shí)刻相互獨(dú)立的，且只和無人機(jī)當(dāng)前時(shí)刻以及前一時(shí)刻的狀態(tài)有關(guān)：

④ 第i架無人機(jī)的自身觀測量只和它自身的狀態(tài)量有關(guān)，即不同無人機(jī)之間的自身觀測量是相互獨(dú)立的；并且相鄰無人機(jī)之間的相對觀測信息是條件獨(dú)立的：

基于以上假設(shè)，結(jié)合式(3)任一時(shí)刻協(xié)同網(wǎng)絡(luò)中后驗(yàn)概率分布可分解為式(10)[8]。

結(jié)合圖1 和式(10)可以進(jìn)一步進(jìn)行因式分解，最終對BP 進(jìn)行最小均方誤差估計(jì)，具體算法執(zhí)行步驟將在2.5 節(jié)詳述。由式(11)和式(12)可以看出，協(xié)同信息和BP 的連續(xù)乘積帶來了大量計(jì)算負(fù)荷，而節(jié)點(diǎn)i流入的協(xié)同信息越多，對狀態(tài)節(jié)點(diǎn)也會有著更好的估計(jì)效果。但協(xié)同信息越多，相應(yīng)的計(jì)算量就越大，因此需要排除低貢獻(xiàn)度信息，對協(xié)同信息進(jìn)行篩選，簡化協(xié)同定位算法的過程，本文提出了一種改進(jìn)的麻雀搜索算法平衡計(jì)算量與精度的問題。

2 改進(jìn)的麻雀搜索算法

2.1 麻雀搜索算法

SSA 算法模擬麻雀尋找食物的過程就是尋優(yōu)的過程，其種群表示如下：

其中，S為麻雀種群矩陣，sF為適應(yīng)度值矩陣，ns代表第n只麻雀，即指代第n種無人機(jī)的組合方式，為麻雀算法中一只麻雀的一維數(shù)組，d是變量的維數(shù)，為第n只麻雀的適應(yīng)度值，即第n種組合方式的GDOP 值。計(jì)算每一只麻雀適應(yīng)度if，選出當(dāng)前最優(yōu)值bf和其對應(yīng)的無人機(jī)組合方式以及當(dāng)前最劣的適應(yīng)度值wf和其對應(yīng)無人機(jī)組合方式

麻雀覓食過程可定義為發(fā)現(xiàn)者-加入者模型，并加入警戒者機(jī)制。按照各自的規(guī)則進(jìn)行迭代，位置更新方程分別如下:

其中iter為迭代次數(shù)，為第j個(gè)麻雀在第k維的無人機(jī)編號，α和Q為隨機(jī)數(shù)，L是一個(gè)一行多維的全一矩陣，R2∈[0,1]和ST∈[0.5,1]分別為預(yù)警值和安全值。當(dāng)預(yù)警值小于安全值時(shí)搜索環(huán)境安全，發(fā)現(xiàn)者可廣泛搜索，帶領(lǐng)種群獲得更高的適應(yīng)度值；當(dāng)預(yù)警值大于安全值表示麻雀遇到了捕食者，迅速調(diào)整策略飛向安全區(qū)。

χ和K為隨機(jī)數(shù)；當(dāng)fj＞fb為警戒者位于種群邊緣，處于危險(xiǎn)狀態(tài)；fj=fb為警戒者處于種群中間意識到危險(xiǎn)，需要趨向其他麻雀群。

2.2 適應(yīng)度函數(shù)

本文假設(shè)測距誤差為零均值高斯分布，無人機(jī)集群保持時(shí)鐘同步，測距值如下形式：

上標(biāo)e表示地心地固ECEF 坐標(biāo)系，對式(17)進(jìn)行泰勒展開并保留一階項(xiàng)，得到可拓展如下形式：

Hi為方向余弦矩陣，載體位置偏差的誤差協(xié)方差為，σ2為測距誤差的方差，GDOP就被定義為的跡的平方根。

2.3 改進(jìn)麻雀搜索算法

標(biāo)準(zhǔn)的麻雀搜索算法中，因?yàn)榇蠖鄶?shù)智能算法的初始值都由隨機(jī)數(shù)產(chǎn)生，會使數(shù)值遠(yuǎn)離最優(yōu)值，影響最終效果；并且種群會在中后期無法跳出局部最優(yōu)，本章采取混沌初始化和自適應(yīng)t 分布變異兩種改進(jìn)方式，避免傳統(tǒng)麻雀搜索算法陷入局部最優(yōu)，提高搜索能力。

2.3.1 混沌初始化

混沌是指在確定性動力學(xué)系統(tǒng)中表現(xiàn)出的不可預(yù)測的、類似隨機(jī)性的運(yùn)動，具有隨機(jī)性、規(guī)律性、在一定范圍內(nèi)不重復(fù)遍歷等特點(diǎn)。Tent Map混沌模型為經(jīng)典的混沌模型，其Tent 映射表達(dá)式如下：

其中Gk+1、Gk為k+1 和k時(shí)刻的狀態(tài)值，為映射參數(shù)。如圖2 所示，藍(lán)色圓圈和紅色星號分別代表了混沌模型和隨機(jī)數(shù)產(chǎn)生的初值結(jié)果。隨機(jī)數(shù)在橢圓框中取值集中，而在矩形框中基本沒有取值，因此采用Tent Map 混沌模型代替隨機(jī)數(shù)初始化，不僅保持種群的多樣性和覆蓋性，而且使得算法脫離局部最優(yōu)值，有更強(qiáng)的全局搜索能力[13]。本文利用混沌初始化模型更新發(fā)現(xiàn)者、追隨者、警戒者。

圖2 混沌模型與隨機(jī)數(shù)初始化對比Fig.2 Comparison of chaos model and random number initialization

2.3.2 自適應(yīng)t 分布變異

設(shè)定變異閾值，當(dāng)小于閾值時(shí)，進(jìn)行自適應(yīng)t 分布變異，定義如下：

2.4 算法步驟

綜上所述，基于改進(jìn)SSA 的BP 無人機(jī)集群協(xié)同定位算法流程如圖3 所示。

圖3 算法流程圖Fig.3 Flowchart of algorithms

步驟1：初始化所有節(jié)點(diǎn)的位置，混沌初始化SSA算法；

步驟5：回到步驟3 進(jìn)行迭代，確保高精度位置信息通過BP 送達(dá)到集群中每架無人機(jī)，直到位置置信度收斂；

步驟6：對后驗(yàn)位置信息，即位置PB 求均值得到對應(yīng)無人機(jī)狀態(tài)信息的最小均方誤差估計(jì)，且成為下一時(shí)刻t+1 時(shí)刻的信息初始值。

3 仿真分析

3.1 仿真條件設(shè)置

設(shè)置如下仿真環(huán)境：以搭載高精度傳感器的長機(jī)為中心，100 m 范圍內(nèi)隨機(jī)生成30 架僚機(jī)，如圖4(a)所示。長機(jī)僚機(jī)飛行軌跡如圖4(b)所示。所有無人機(jī)搭載測距范圍為60 m 的UWB 模塊，考慮無人機(jī)軸距及荷載等原因，將相鄰無人機(jī)之間的安全距離設(shè)為6 m，每一架僚機(jī)選擇與相鄰五架無人機(jī)信息協(xié)同信息的交互。表1 為無人機(jī)機(jī)載傳感器參數(shù)配置，量測噪聲均設(shè)置為高斯噪聲。

圖4 仿真場景和飛行軌跡Fig.4 Simulation scenario and trajectory

表1 傳感器參數(shù)配置Tab.1 Sensor parameters configuration

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

首先探究改進(jìn)麻雀搜索算法性能。種群規(guī)模設(shè)為N= 20，迭代次數(shù)為200，發(fā)現(xiàn)者個(gè)數(shù)和警戒者麻雀個(gè)數(shù)均取種群規(guī)模的20%。選擇某時(shí)刻無人機(jī)i的GDOP，并與原始粒子群算法、原始麻雀搜索算法和改進(jìn)粒子群算法適應(yīng)度值進(jìn)行對比，適應(yīng)度值對比曲線結(jié)果如圖5 所示。

圖5 算法收斂曲線Fig.5 Algorithm convergence curve

圖中經(jīng)典算法粒子群和改進(jìn)粒子群算法過早陷入了局部最優(yōu)解，原始麻雀搜索算法在55 次迭代后收斂精度優(yōu)于前兩種算法。本文的改進(jìn)麻雀搜索算法收斂速度和收斂精度優(yōu)于任何對比算法。

采用本文的改進(jìn)SSA 優(yōu)化的BP 算法，隨機(jī)選取集群系統(tǒng)中兩架僚機(jī)進(jìn)行定位精度的分析，圖6 給出了如下所示的三種不同算法下僚機(jī)在地球坐標(biāo)系下三個(gè)坐標(biāo)方向上的定位誤差對比曲線。

① 本文算法：本文提出的改進(jìn)麻雀搜索算法優(yōu)化的置信傳播無人機(jī)集群協(xié)同定位算法；

② BP 協(xié)同：文獻(xiàn)[8]中提出的協(xié)同導(dǎo)航方法；

③ 無協(xié)同：僚機(jī)采用低精度的INS/GPS 系統(tǒng)進(jìn)行松組合導(dǎo)航，長僚機(jī)之間不進(jìn)行信息交互。

圖6 表明，低性能僚機(jī)采用本文算法協(xié)同定位方法，極大提高了僚機(jī)的定位誤差精度，誤差曲線明顯優(yōu)于其余兩種對比方法。并且進(jìn)一步對三種算法僚機(jī)的定位誤差精度進(jìn)行定量的分析，對該無人機(jī)定位均方根誤差(RMSE)和精確率（Accuracy）進(jìn)行了統(tǒng)計(jì)，精確率定義為結(jié)果如表2 所示。

根據(jù)圖6 和表2 所示的結(jié)果，本文算法具有較小的RMSE 誤差和較精確的估計(jì)精度，與原始BP 算法相比，估計(jì)更加穩(wěn)定和準(zhǔn)確。根據(jù)定義中的可以推斷出經(jīng)過改進(jìn)SSA 算法優(yōu)化后，最優(yōu)的GDOP值使得協(xié)同信息提高了對僚機(jī)定位的貢獻(xiàn)度。

圖6 定位誤差對比曲線Fig.6 Comparison on position errors

表2 定位誤差統(tǒng)計(jì)結(jié)果Tab.2 Statistics of position error

為了將提出的協(xié)同定位框架與其他框架進(jìn)行比較，無人機(jī)定位估計(jì)誤差的計(jì)算公式如式(23)所示。

為了體現(xiàn)集群中僚機(jī)的整體表現(xiàn)，采用誤差累積分布函數(shù)(CDF)，對比不同方法編隊(duì)飛行中的所有僚機(jī)的定位誤差。如圖7 顯示了累積分布三種方法的CDF，并與長機(jī)進(jìn)行比較。

由圖7 可以看出，本文所提出的方法的定位精度是最為接近長機(jī)定位精度的，并且優(yōu)于原始BP 算法。例如，集群中無協(xié)同定位只有0.3%的定位誤差小于0.5 m，但本文所提出的方法可將該比例提高到97.1%，略低于高精度長機(jī)（98.9%），高于原始BP（81.9%）。

圖7 定位誤差累積分布比較Fig.7 CDF comparison of positioning errors

本文算法對定位估計(jì)精度進(jìn)行了優(yōu)化，針對本文方法的復(fù)雜度進(jìn)行進(jìn)一步的分析。對上述方法的算法效率進(jìn)行了對比，并設(shè)置了對照組：隨機(jī)選擇五條協(xié)同信息CM，與本文算法進(jìn)行精度上和效率上的對比。利用Matlab 的‘tic’和‘toc’函數(shù)獲得記錄每個(gè)周期代碼的運(yùn)行時(shí)間，用來表示算法框架的計(jì)算負(fù)載。結(jié)果如圖8 所示。

圖8 定位誤差與處理時(shí)間對比Fig.8 Comparison of positioning error and processing time

圖8 表示，本文算法計(jì)算負(fù)載遠(yuǎn)小于原始BP 算法，每個(gè)周期的時(shí)間提升為原始BP 的2.8 倍。與對照組相比在計(jì)算量上二者較為近似，在定位精度上優(yōu)于對照組，驗(yàn)證了本文方法的有效性，簡化了協(xié)同定位算法過程的計(jì)算復(fù)雜度，提升了僚機(jī)的整體導(dǎo)航性能。

4 結(jié) 論

針對無人機(jī)集群協(xié)同導(dǎo)航的問題，本文提出利用改進(jìn)SSA 算法優(yōu)化的BP 算法，對基本的麻雀搜索算法進(jìn)行混沌初始化和自適應(yīng)變異改進(jìn)，增加探索能力和種群多樣性，并驗(yàn)證了收斂性和精度。并且研究了基于因子圖的集群無人機(jī)編隊(duì)協(xié)同定位模型，將無人機(jī)定位問題轉(zhuǎn)化為求解后驗(yàn)概率分布問題。仿真結(jié)果表明，改進(jìn)的麻雀搜索算法收斂速度和精度優(yōu)于所有對比算法。本文協(xié)同定位框架能夠提升集群中整體僚機(jī)定位精度，優(yōu)于原始BP 算法，既解決了計(jì)算量大的問題，又提高了協(xié)同導(dǎo)航性能，在保證精度的基礎(chǔ)上簡化了計(jì)算量，且不依賴高精度傳感器，降低了成本，實(shí)現(xiàn)了無人機(jī)集群的高精度實(shí)時(shí)協(xié)同定位。