基于高斯粒子濾波與消息傳遞的協(xié)同導(dǎo)航方法

陳明星，熊 智,2，王 融,2，劉建業(yè),2，熊 駿，田世偉

(1.南京航空航天大學(xué)自動化學(xué)院導(dǎo)航研究中心，南京 211106；2.先進飛行器導(dǎo)航、控制與健康管理工業(yè)和信息化部重點實驗室，南京 211106；3.南京郵電大學(xué)物聯(lián)網(wǎng)學(xué)院，南京 210023；4.國防創(chuàng)新研究院，北京 100000)

近年來無人機集群由于其高效率、功能分布化等優(yōu)勢在軍事和民用領(lǐng)域都取得了廣泛的關(guān)注。導(dǎo)航信息是無人機可靠飛行的關(guān)鍵，尤其是在密集編隊飛行環(huán)境下，無人機需要高精度的位置、速度信息來實現(xiàn)精確控制和防撞[1]。慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)是目前最廣泛應(yīng)用于無人機的導(dǎo)航手段，基于衛(wèi)星導(dǎo)航的RTK技術(shù)可以為無人機提供厘米級的定位精度[2]。但為集群中所有無人機配備RTK 設(shè)備的成本較高，且在某些受限環(huán)境下如城市、森林等，GNSS 接收機可能因為無法搜索到足夠的衛(wèi)星而無法提供精確可靠的導(dǎo)航信息[3]。

為了充分利用集群的優(yōu)勢，國內(nèi)外學(xué)者開始研究融合無人機機間量測信息的協(xié)同導(dǎo)航方法。文獻[4][5]通過卡爾曼濾波實現(xiàn)本地量測與協(xié)同網(wǎng)絡(luò)中相對量測信息的融合。以卡爾曼濾波為基礎(chǔ)的分布式協(xié)同導(dǎo)航方法實現(xiàn)簡單，但僅對相鄰節(jié)點的協(xié)同信息進行了融合，沒有充分利用網(wǎng)絡(luò)中非相鄰節(jié)點的約束關(guān)系。因子圖為協(xié)同導(dǎo)航問題的處理提供了一個有效的框架，基于因子圖框架的貝葉斯協(xié)同導(dǎo)航方法是目前協(xié)同導(dǎo)航領(lǐng)域研究的熱點。文獻[6]在GNSS 定位信息和無人機之間的距離信息基礎(chǔ)上建立協(xié)同導(dǎo)航因子圖，通過集群中少量配備RTK 設(shè)備的無人機輔助，提升了整個無人機群的定位精度。不過這種方法需要無人機群存在高精度定位節(jié)點，且每架無人機都能進行GNSS 定位。文獻[7]提出了一種基于因子圖和粒子化消息傳遞的混合和積算法（Hybrid Sum-product Algorithm for Wireless Network,H-SPAWN），能夠?qū)⑿l(wèi)星偽距和用戶間量測信息融合進行協(xié)同定位。該方法可以在衛(wèi)星受限的情況下取得較好的定位性能，但該方法通過單一運動模型來描述載體的運動，不適用于復(fù)雜動態(tài)的無人機飛行場景，也無法提供連續(xù)可靠的導(dǎo)航信息。

本文提出了一種因子圖框架下基于高斯粒子濾波和消息傳遞的協(xié)同導(dǎo)航方法，將INS 加入到因子圖框架以描述無人機的動態(tài)過程，可以分布式地融合INS、氣壓計等本地傳感器數(shù)據(jù)、衛(wèi)星單差偽距以及機間測距信息，并通過無人機之間消息的迭代傳遞實現(xiàn)整個無人機群協(xié)同導(dǎo)航信息的有效融合，從而提升無人機群的整體導(dǎo)航性能。

1 協(xié)同導(dǎo)航因子圖框架

考慮如圖1所示無人機群工作場景，集群中所有無人機配備衛(wèi)星接收機、IMU、氣壓高度計等導(dǎo)航設(shè)備，同時配備UWB 進行機間距離測量。在飛行過程中，無人機通過無線數(shù)據(jù)鏈與其他無人機交互導(dǎo)航信息，從而形成協(xié)同導(dǎo)航網(wǎng)絡(luò)。

圖1 無人機群協(xié)同工作場景Fig.1 Cooperative work scenario of UAV swarm

協(xié)同導(dǎo)航網(wǎng)絡(luò)中導(dǎo)航狀態(tài)的相互依存關(guān)系可以通過因子圖來表示。以無人機的導(dǎo)航參數(shù)和IMU 的誤差構(gòu)成因子圖的變量節(jié)點，無人機i在k時刻的變量集可定義為：

無人機在k時刻接收到的距離觀測集和偽距觀測集定義為：

其中：表示無人機i與j之間的UWB 距離觀測，Ui為與無人機i存在距離觀測的無人機集合，表示導(dǎo)航衛(wèi)星sn（i）與無人機i之間的偽距觀測，Si為無人機i搜索到的導(dǎo)航衛(wèi)星集合。

由此可得無人機群導(dǎo)航狀態(tài)的聯(lián)合后驗概率分布：

Zk表示k時刻無人機群接收到的所有觀測信息：

根據(jù)無人機群的飛行特性和傳感器特性，聯(lián)合后驗概率分布可被因式分解為如下形式：

其中，p（X0）表示導(dǎo)航狀態(tài)的先驗信息。結(jié)合式(5)和機載傳感器類型，可定義四種因子：

? 慣性因子：

慣性因子連接無人機兩個時刻的變量節(jié)點，表征導(dǎo)航狀態(tài)的變化。

? 測距因子：

測距因子表征無人機i與j之間的距離關(guān)系。

? 高度因子：

高度因子表征無人機所在海拔高度與測得氣壓高度的關(guān)系，假設(shè)無人機飛行環(huán)境氣壓分布標準，則氣壓高度計得到的氣壓高度經(jīng)過處理可視為海拔高度[8]。

? 單差偽距因子：

考慮到在城市等環(huán)境中無人機不一定能觀測到足夠的衛(wèi)星實現(xiàn)定位，因此利用偽距作為觀測信息。對于機載衛(wèi)星接收機收到的偽距信息，其誤差項中包含電離層誤差和對流層誤差在內(nèi)的信號傳播誤差和衛(wèi)星鐘差通?？刹捎孟鄳?yīng)修正模型或星基增強系統(tǒng)播發(fā)的消息來消除，多徑或非視距誤差可通過故障識別的方式進行剔除，處理后的衛(wèi)星偽距可建模為：

結(jié)合所定義的變量節(jié)點和因子節(jié)點，可將式(5)無人機群導(dǎo)航狀態(tài)的聯(lián)合后驗概率分布分解和描述為圖2所示因子圖。在建立起協(xié)同導(dǎo)航的因子圖框架，并完成變量節(jié)點和因子節(jié)點的建模后，本文結(jié)合高斯粒子濾波和消息傳遞算法設(shè)計協(xié)同導(dǎo)航方法，從而獲得因子圖中變量的最大后驗概率估計。

圖2 無人機群協(xié)同導(dǎo)航因子圖Fig.2 Cooperative navigation factor graph of UAV swarm

2 協(xié)同導(dǎo)航方法設(shè)計

協(xié)同導(dǎo)航因子圖通過相鄰節(jié)點之間的消息傳遞來實現(xiàn)概率分布的更新。本文利用一系列高斯粒子及其權(quán)重來對消息進行描述。因此，變量節(jié)點的概率分布估計問題轉(zhuǎn)變?yōu)榱Ｗ又岛蜋?quán)重的更新問題。

2.1 高斯粒子濾波

高斯粒子濾波的基本思想是將非線性系統(tǒng)中被估計狀態(tài)的后驗概率密度函數(shù)近似為多維高斯分布，利用粒子濾波技術(shù)求取高斯分布的相關(guān)參數(shù)并得到濾波結(jié)果[9]。對于一個隨機系統(tǒng)，它的狀態(tài)方程和量測方程的離散形式可以分別描述為：

其中f（·）和h（·）為已知函數(shù)；uk，υk分別為服從一定分布的系統(tǒng)噪聲和量測噪聲。高斯粒子濾波的目的是用一組粒子和權(quán)值來求取系統(tǒng)狀態(tài)的邊緣后驗假設(shè)可由高斯分布近似逼近，高斯粒子濾波可以表述為：

其中Ck為歸一化因子。

對于協(xié)同導(dǎo)航因子圖來說，節(jié)點之間傳遞的消息包含了量測信息和變量的狀態(tài)信息，消息的迭代傳遞過程即粒子的時間更新和量測更新過程。

2.2 消息形式

結(jié)合定義的因子類型，在協(xié)同導(dǎo)航因子圖中需要考慮的消息主要有以下形式：

時進消息是由慣性因子傳遞給變量節(jié)點的消息，表示為：

氣壓高度觀測僅與當前時刻當前位置相關(guān)，因此氣壓高度因子與變量節(jié)點單連接，氣壓高度消息完全來自因子高度消息可表示為：

其中為無人機i在k時刻對應(yīng)的海拔高度，為氣壓高度噪聲方差。

與高度因子類似，由于衛(wèi)星位置已知，單差偽距因子僅與變量節(jié)點之間單連接，因此，單差偽距消息完全繼承自因子可表示為：

對于UWB 測距來說，在視距內(nèi)的量測可建模為：

其中εr是方差為的測距噪聲。由機間測距因子傳遞給變量節(jié)點的消息為：

式中為測距噪聲和無人機j的位置協(xié)方差的疊加：

其中Hj→i為方向余弦：

變量節(jié)點傳遞給慣性因子的狀態(tài)消息實質(zhì)上就是變量節(jié)點自身狀態(tài)的后驗概率分布，其形式如下：

2.3 基于高斯粒子濾波和消息傳遞的協(xié)同導(dǎo)航方法實現(xiàn)過程

基于消息形式和高斯粒子濾波的相關(guān)定義，協(xié)同導(dǎo)航方法的實現(xiàn)過程如下：

①初始化各無人機的慣導(dǎo)系統(tǒng)和高斯粒子濾波器，給出導(dǎo)航狀態(tài)的先驗概率分布

③進行時進消息的傳遞，對所有粒子進行慣導(dǎo)遞推，更新粒子的值，得到

④計算單差偽距消息和高度消息，更新此時的狀態(tài)消息：

權(quán)重更新后對其歸一化；

⑤測距消息的迭代傳遞，主要分為兩步：

3 仿真驗證與分析

3.1 仿真條件設(shè)置

根據(jù)第二節(jié)給出的協(xié)同導(dǎo)航因子圖框架，對基于高斯粒子濾波和消息傳遞的協(xié)同導(dǎo)航方法進行MATLAB仿真驗證。仿真條件如下：仿真10架無人機組成的無人機群，仿真時間3600s，無人機集群的初始位置和其中一架無人機的運動軌跡如圖3-4所示，每架無人機粒子數(shù)目選定為2000，閾值α設(shè)定為0.01。每架無人機均配備IMU、氣壓計、衛(wèi)星接收機和UWB，所有無人機均可與自身60m 范圍內(nèi)的相鄰無人機測距和通信，每架無人機搜索到的衛(wèi)星數(shù)為3顆，表1列出了無人機所搭載設(shè)備的仿真參數(shù)[10]。

表1 傳感器配置與仿真參數(shù)Tab.1 Sensor configuration and simulationparameters

圖3 無人機群初始位置Fig.3 Initial position of UAV swar m

圖4 無人機運動軌跡Fig.4 Trajectory of UAV

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

基于上述仿真條件，對無人機群的定位精度進行仿真分析。為了使驗證具有代表性，將本文算法與無協(xié)同情況下基于卡爾曼濾波的INS/衛(wèi)星偽距/氣壓高度組合、基于EKF的分布式協(xié)同算法以及基于H-SPAWN算法[7]的協(xié)同導(dǎo)航方法進行對比。以其中一架無人機的定位結(jié)果為例，仿真結(jié)果如圖5所示。

由圖5可以看出，本文所提出的協(xié)同導(dǎo)航方法相比無協(xié)同、基于EKF的協(xié)同方法以及基于H-SPAWN的協(xié)同方法定位精度得到明顯的提升，為了定量地分析四種不同方法下的定位誤差，對無人機群定位均方根誤差(RMSE)進行了統(tǒng)計，統(tǒng)計結(jié)果如表2所示。表2中的位置估計誤差的計算公式為：

圖5 定位誤差對比曲線Fig.5 Comparisonon position error

從表2可以看出，通過融合無人機之間的相對導(dǎo)航信息，基于EKF的協(xié)同導(dǎo)航方法提升了無人機群的定位精度，而通過對自身量測以及無人機群約束關(guān)系的充分利用，本文方法相對于基于EKF的協(xié)同導(dǎo)航方法定位精度提升89%，由于融合了INS信息，本文方法的定位精度同樣優(yōu)于基于H-SPAWN的協(xié)同方法，定位精度提升54%。

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

為考察本文所提出方法在可見衛(wèi)星較少情況下的協(xié)同導(dǎo)航性能，在航跡、傳感器配置等條件不變的情況下，設(shè)定無人機群中7架無人機可搜索到2顆衛(wèi)星，初始高度最低的3架無人機無法搜索到衛(wèi)星。圖6給出了其中一架無法搜索到衛(wèi)星的無人機的協(xié)同導(dǎo)航位置誤差對比曲線，表3對比了該條件下的無人機群協(xié)同導(dǎo)航位置誤差統(tǒng)計結(jié)果。

圖6 定位誤差對比曲線(可見星較少情況)Fig.6 Comparison on position error (Fewer visible satellites)

由圖6和表3可以看出，在可見星較少的仿真條件下，無協(xié)同和基于EKF的協(xié)同方法定位結(jié)果已經(jīng)發(fā)散，而基于H-SPAWN 的協(xié)同方法和本文方法由于充分利用了集群協(xié)同信息仍能實現(xiàn)定位。對比表2和表3可以看出，對于通過單一運動模型來描述載體的運動的H-SPAWN，在衛(wèi)星導(dǎo)航信息減少的情況下其定位性能惡化程度要高于融合了INS信息的本文方法。本文方法相對于H-SPAWN方法定位精度提升了60%。

表3 定位誤差統(tǒng)計結(jié)果(可見星較少情況)Tab.3 Statisticsof positionerror (Fewer visible satellites)

4 結(jié)論

本文以衛(wèi)星受限環(huán)境下無人機群協(xié)同導(dǎo)航為背景，研究因子圖框架下基于高斯粒子濾波和消息傳遞的協(xié)同導(dǎo)航方法，該方法利用因子圖來描述無人機群導(dǎo)航狀態(tài)與不同類型量測之間的關(guān)系，并通過高斯粒子濾波來完成因子圖中節(jié)點間的消息傳遞，有效融合INS、氣壓高度、衛(wèi)星單差偽距與無人機群相對量測信息，從而實現(xiàn)導(dǎo)航狀態(tài)的后驗概率估計。仿真結(jié)果表明，本文所提出的方法能夠?qū)崿F(xiàn)無人機群量測信息的精確融合，提升整個無人機群的定位精度，適用于衛(wèi)星受限環(huán)境下無人機協(xié)同編隊場景。