基于幾何特性的多無人機協(xié)同導(dǎo)航算法

潘瑞鴻, 徐勝紅

(海軍航空工程學(xué)院 a.研究生管理大隊; b.控制工程系， 山東 煙臺 264001)

【裝備理論與裝備技術(shù)】

基于幾何特性的多無人機協(xié)同導(dǎo)航算法

潘瑞鴻a, 徐勝紅b

(海軍航空工程學(xué)院 a.研究生管理大隊; b.控制工程系， 山東 煙臺 264001)

針對在集群無人機不使用GPS(Global Positioning System)和高精度慣性導(dǎo)航系統(tǒng)(INS)的情況下如何提高集群成員導(dǎo)航定位精度的問題，提出一種基于幾何特性的多無人機協(xié)同導(dǎo)航算法；以無人機的位置和速度信息為狀態(tài)量，建立三架無人機所組成系統(tǒng)的運動學(xué)模型，利用以無人機為頂點構(gòu)成的幾何三角形的形狀、大小及其在空間中的姿態(tài)作為量測量，通過卡爾曼濾波，估計系統(tǒng)中各無人機的位置信息；仿真結(jié)果表明：所提的協(xié)同導(dǎo)航算法能夠提高集群無人機的定位精度，優(yōu)于航位推算，而且定位誤差不隨時間積累。

協(xié)同導(dǎo)航；集群無人機；幾何三角形；法向量

無論在軍用還是民用領(lǐng)域，單架無人機執(zhí)行任務(wù)的能力始終有限，而集群無人機則具備單架無人機無法比擬的優(yōu)勢[1]，如功能分布化、高效費交換比、大范圍態(tài)勢感知、高任務(wù)完成率[2]等。因此，無人機集群技術(shù)是未來無人機技術(shù)發(fā)展的一個重要趨勢，受到了國內(nèi)外研究人員的廣泛關(guān)注。

無人機之間的協(xié)同導(dǎo)航是無人機集群的必要條件之一，一定精度以上的導(dǎo)航信息也是隊形設(shè)計、編隊控制等關(guān)鍵技術(shù)的基礎(chǔ)。目前諸如有人機、高空長航時大型無人機等飛行器多采用INS/GPS組合導(dǎo)航系統(tǒng)，該系統(tǒng)的兩個子導(dǎo)航系統(tǒng)優(yōu)勢互補，能夠很好地滿足實時、連續(xù)、高精度導(dǎo)航定位的要求[3-4]。然而，一方面GPS系統(tǒng)受制于人，戰(zhàn)時無法使用，另一方面，由于集群無人機體積、成本等條件的約束，無法裝備大型高精度慣性導(dǎo)航設(shè)備，因此如何在有限的條件下，盡可能提高集群無人機的導(dǎo)航精度成為亟待解決的問題。已有研究人員對協(xié)同導(dǎo)航定位相關(guān)問題進行了研究。文獻[5]建立了編隊成員相對導(dǎo)航運動模型及非線性觀測模型，采用量測重構(gòu)技術(shù)構(gòu)造了偽線性觀測模型，基于相對導(dǎo)航結(jié)果提出了一種多平臺INS誤差聯(lián)合修正方法，使收斂速度大幅提高，有效減緩編隊系統(tǒng)INS誤差發(fā)散速度。但算法要求各機載INS誤差近似呈零均值高斯分布。文獻[6]通過分析單領(lǐng)航自主水下航行器(AUV)協(xié)同導(dǎo)航系統(tǒng)的定位原理和運動學(xué)模型，結(jié)合相鄰采樣時刻主、從AUV相對位置的幾何關(guān)系及航位推算信息，利用擴展卡爾曼濾波導(dǎo)航算分實現(xiàn)對從AUV的定位估計。但只能運用于兩艘AUV之間，且要求主AUV導(dǎo)航精度較高。文獻[7]研究了多機器人隊列利用相對觀測信息在未知環(huán)境中進行同時定位的問題，利用觀測信息同時更新整個隊列的位置及協(xié)方差矩陣，得到更精確的位置估計。但只單獨利用了相對距離或者相對方位作為觀測量，信息利用不充分。因此，這些方法都不能很好的適用于集群無人機協(xié)同導(dǎo)航中。

本文提出了一種新的利用以無人機為頂點構(gòu)成的幾何三角形的形狀、大小以及其在空間中的姿態(tài)構(gòu)造系統(tǒng)量測信息的協(xié)同導(dǎo)航方法，充分利用多組機間相對位置信息，通過卡爾曼濾波對系統(tǒng)中無人機的位置進行估計[8]，獲得較精確的導(dǎo)航定位信息，仿真結(jié)果驗證了算法的有效性。

1 多無人機系統(tǒng)建模

假設(shè)每架無人機都裝備有微型慣性測量單元(MIMU)/航位推算系統(tǒng)等導(dǎo)航系統(tǒng)，但這些設(shè)備的精度較低，對加速度的測量會引入誤差。選取地球固連坐標(biāo)系為導(dǎo)航坐標(biāo)系，對三架無人機組成的系統(tǒng)進行建模：

選取狀態(tài)量為

X=[x1,y1,z1,vx1,vy1,vz1,x2,y2,z2,

vx2,vy2,vz2,x3,y3,z3,vx3,vy3,vz3]T

(1)

其中[xi,yi,zi]T，i=1,2,3，為無人機i的位置信息，[vxi,vyi,vzi]T，i=1,2,3，為無人機i在3個方向上的速度信息。下標(biāo)1、2、3分別表示無人機1、2、3。

則系統(tǒng)運動學(xué)模型即狀態(tài)方程為

(2)

離散化為

X(k)=Φ(k,k-1)X(k-1)+Γ(k-1)W(k-1)

(3)

式中：系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣：

系統(tǒng)噪聲輸入矩陣

W(k)=[wx1,wy1,wz1,wx2,wy2,wz2,wx3,wy3,wz3]T為系統(tǒng)過程噪聲，即加速度計噪聲。假定其具有統(tǒng)計特性E[W(t)]=0，

該模型本質(zhì)上是利用航位推算原理。由于加速度計噪聲的存在以及系統(tǒng)模型的遞推性質(zhì)，采用航位推算方法計算無人機位置時每一個采樣時刻的加速度計噪聲都會疊加到下一時刻的狀態(tài)量上，從而導(dǎo)致定位誤差隨時間積累，因此只能在一定時間內(nèi)提供較低精度的導(dǎo)航信息[9-10]。在不使用GPS的情況下，如何校正此導(dǎo)航信息，使定位誤差不隨時間積累，提高導(dǎo)航精度，成為了難點技術(shù)問題。

2 協(xié)同導(dǎo)航算法分析

不同于單架無人機，集群無人機不僅有自身的絕對位置，還有與其他協(xié)同成員的相對位置，而無人機之間測距和測角信息的精度是比較高的。從信息的角度看，對于兩種相關(guān)但精度不同的信息，可以利用精度高的信息去校正精度低的信息。因此，利用無人機之間的相對位置信息去校正各無人機自身導(dǎo)航設(shè)備輸出的絕對位置信息是合理的?？紤]構(gòu)造一種新的量測量，能夠包含協(xié)同成員之間所有的相對位置信息，通過卡爾曼濾波，對狀態(tài)量進行估計。

假設(shè)無人機之間為理想通信情況，且不考慮通信延遲，即無人機兩兩之間的相對距離、相對角度以及自身導(dǎo)航系統(tǒng)輸出參數(shù)等信息都能夠?qū)崟r傳輸。系統(tǒng)總體流程簡圖如圖1。

量測量的構(gòu)造及相應(yīng)的協(xié)同導(dǎo)航算法具體描述如下。不考慮共線情況，以3架無人機為頂點總是可以構(gòu)成一個三角形，該三角形3條邊的邊長即為無人機之間的相對距離，而三角形在空間中所處的姿態(tài)則由無人機之間的相對角度決定，表示為三角形所在平面的法向量。這個三角形能夠完全反映3架無人機之間的相對位置關(guān)系。

圖1 系統(tǒng)總體流程簡圖

如圖2所示，無人機i在地球固連坐標(biāo)系下的坐標(biāo)為(xi,yi,zi)，i=1,2,3，無人機之間的相對距離分別記為r12,r13,r23，以3個坐標(biāo)點為頂點構(gòu)成一個三角形，該三角形所在平面的法向量記為

n=[nx,ny,nz]

(4)

選取無人機構(gòu)成的三角形的一種全等條件和三角形在空間中的姿態(tài)為量測量。這里全等條件選擇邊邊邊，三角形姿態(tài)由其所在平面的法向量表示。

圖2 3架無人機空間位置示意圖

3邊邊長即無人機之間的相對距離，可由測距傳感器直接測得，表示為

且i≠j

(5)

法向量無法直接測定，但可由無人機之間的相對角度推算得到，相對角度由測角傳感器測得，表示為

(6)

(7)

i,j=1,2,3且i≠j

假定相對距離、俯仰角和方位角的量測噪聲具有統(tǒng)計特性：E[vrij]=E[vθij]=E[vφij]=0，Var[vrij]=(σrij)2，Var[vθij]=(σθij)2，Var[vφij]=(σφij)2，且三者兩兩互不相關(guān)，即E[vrijvθij]=E[vrijvφij]=E[vθijvφij]=0，i,j=1,2,3且i≠j。

法向量的表達式為

n=[nx,ny,nz]=

[r12cosθ12cosφ12,r12cosθ12sinφ12,r12sinθ12]×

[r13cosθ13cosφ13,r13cosθ13sinφ13,r13sinθ13]

(8)

量測量與狀態(tài)量的具體關(guān)系如下：

(9)

n=[nx,ny,nz]=([x2,y2,z2]-[x1,y1,z1])×

([x3,y3,z3]-[x1,y1,z1])+[vnx,vny,vnz]

(10)

式(10)中vnx,vny,vnz的推導(dǎo)可參考文獻[5]，表達式如下：

vnx=vθ12r12r13(-sinθ12sinφ12sinθ13-cosθ12cosθ13sinφ13)+

vθ13r12r13(cosθ12sinφ12cosθ13+sinθ12sinθ13sinφ13)+

vφ12r12r13(cosθ12cosφ12sinθ13)+

vφ13r12r13(-sinθ12cosθ13cosφ13)+

vr12r13(cosθ12sinφ12sinθ13-sinθ12cosθ13sinφ13)+

vr13r12(cosθ12sinφ12sinθ13-sinθ12cosθ13sinφ13)

(11)

vny=vθ12r12r13(-cosθ12cosθ13cosφ13+cosθ12cosθ13cosφ13)+

vθ13r12r13(-sinθ12sinθ13cosφ13-cosθ12cosφ12cosθ13)+

vφ12r12r13(+cosθ12sinφ12sinθ13)+

vφ13r12r13(-sinθ12cosθ13sinφ13)+

vr12r13(sinθ12cosθ13cosφ13-cosθ12cosφ12sinθ13)+

vr13r12(sinθ12cosθ13cosφ13-cosθ12cosφ12sinθ13)

(12)

vnz=vθ12r12r13(-sinθ12cosφ12cosθ13sinφ13+sinθ12sinφ12cosθ13cosφ13)+

vθ13r12r13(-cosθ12cosφ12sinθ13sinφ13+cosθ12sinφ12sinθ13cosφ13)+

vφ12r12r13(-cosθ12sinφ12cosθ13sinφ13-cosθ12cosφ12cosθ13cosφ13)+

vφ13r12r13(cosθ12cosφ12cosθ13cosφ13+cosθ12sinφ12cosθ13sinφ13)+

vr12r13(cosθ12cosφ12cosθ13sinφ13-cosθ12sinφ12cosθ13cosφ13)+

vr13r12(cosθ12cosφ12cosθ13sinφ13-cosθ12sinφ12cosθ13cosφ13)

(13)

即量測方程為

Z(t)=h[X(t)]+V(t)

(14)

離散化為

Z(k)=H(k)X(k)+V(k)

(15)

式(15)中:Z=[r12,r13,r23,nx,ny,nz]T，為系統(tǒng)量測量，V=[vr12,vr13,vr23,vnx,vny,vnz]T，為系統(tǒng)量測噪聲，根據(jù)其統(tǒng)計特性，可推導(dǎo)出量測噪聲的協(xié)方差矩陣為

(16)

顯然，量測方程是一個非線性方程，需要對其進行線性化，即對式(9)和式(10)進行一階泰勒展開，得到H的Jacobian矩陣為：

(17)

式(17)中

對于以相對距離為量測值的量測方程，可對其進行線性化[7,11,12]，說明由此產(chǎn)生的誤差在允許范圍內(nèi)，再對比式(9)和式(10)，易知Hn的線性化誤差要小于Hr的，因此這里使用卡爾曼濾波是合理的。以式(3)為狀態(tài)方程，式(15)為量測方程，利用卡爾曼濾波，可得到3架無人機的位置估計值。每一次迭代時，所構(gòu)造的量測信息都會通過濾波對系統(tǒng)狀態(tài)量進行校正，提高了導(dǎo)航精度，并且使定位誤差不隨時間積累。

3 仿真分析

使用Matlab進行仿真，取3架無人機的初始位置分別為(10,10,10)，(200,100,50)，(100,150,100)，均采用勻速運動模型，3個方向的速度均為100 m/s。各個方向系統(tǒng)噪聲標(biāo)準(zhǔn)差為0.001 m/s2，相對距離量測噪聲標(biāo)準(zhǔn)差為0.1 m，俯仰角和方位角量測噪聲標(biāo)準(zhǔn)差為1′。仿真時間為3 000 s，采樣周期為0.1 s。圖3、圖4、圖5給出了采用本文所提的協(xié)同導(dǎo)航算法的3架無人機位置誤差曲線，圖6、圖7、圖8給出了采用航位推算的3架無人機位置誤差曲線。

從圖3、圖4、圖5可以看出，采用協(xié)同導(dǎo)航算法的3架無人機各個方向上的定位誤差在仿真時間內(nèi)都穩(wěn)定在10 m范圍以內(nèi)，無人機2的x方向，無人機3的z、y方向上的定位誤差始終小于1 m。而且到仿真結(jié)束前，誤差曲線均出現(xiàn)過零現(xiàn)象，可以判斷出協(xié)同導(dǎo)航的定位誤差不隨時間積累。

從圖6～圖8可以看出，采用航位推算的3架無人機各個方向上的定位誤差大體上隨仿真時間逐漸增大，仿真結(jié)束時達到25 m左右，無人機3的x、y方向上的定位誤差甚至達到50 m以上，而且誤差總體呈發(fā)散趨勢，除了無人機3的z方向，定位誤差曲線均沒有出現(xiàn)過零現(xiàn)象，說明航位推算的定位誤差隨時間積累。

圖3 協(xié)同導(dǎo)航位置誤差曲線(無人機1)

圖4 協(xié)同導(dǎo)航位置誤差曲線(無人機2)

圖5 協(xié)同導(dǎo)航位置誤差曲線(無人機3)

分別對比圖3和圖6，圖4和圖7，圖5和圖8，本文所提的協(xié)同導(dǎo)航的定位誤差明顯小于航位推算的定位誤差，且不隨時間積累，說明所提算法達到了預(yù)期效果。

圖6 航位推算位置誤差曲線(無人機1)

圖7 航位推算位置誤差曲線(無人機2)

圖8 航位推算位置誤差曲線(無人機3)

4 結(jié)論

本文所提的多無人機協(xié)同導(dǎo)航算法利用無人機兩兩之間的相對距離和相對角度測量信息構(gòu)造了一種新的量測量，仿真結(jié)果表明所得到的定位結(jié)果精度要高于航位推算方法，而且估計誤差不隨時間積累。該方法的優(yōu)點在于充分利用了多架無人機之間的相對位置關(guān)系信息，從數(shù)學(xué)算法上提高導(dǎo)航定位精度。

但是本文在研究過程中假設(shè)了理想的通信條件，沒有考慮模型誤差，3架無人機作相同的勻速運動，這3方面不具有普適性。因此，這是下一步研究的方向。

[1] 周紹磊,康宇航,秦亮,等.多無人機協(xié)同控制的研究現(xiàn)狀與主要挑戰(zhàn)[J].飛航導(dǎo)彈,2015(7):31-35.

[2] ZOU Y F,PAGILLA P R and RATLIFF R T.Distributed Formation Flight Control Using Constraint Forces[J].Journal of Guidance,Control,and Dynamics，2009,32(1):112-120.

[3] WILLIAMSON W,MIN J,SPEYER J L,FARRELL J.A Comparison of State Space,Range Space,and Carrier Phase Differential GPS/INS Relative Navigation[C]//Proceedings of the American Control Conference,2000.

[4] ALI J,MIRZA MRUB.Performance Comparison among Some Nonlinear Filters for a Low Cost SINS/GPS Integrated Solution[J].Nonlinear Dynamics,2010,61(3):491-502.

[5] 張共愿,程詠梅,程承,等.基于相對導(dǎo)航的多平臺INS誤差聯(lián)合修正方法[J].航空學(xué)報,2011(2):271-280.

[6] 李聞白,劉明雍,張立川,等.單領(lǐng)航者相對位移測量的多自主水下航行器協(xié)同導(dǎo)航[J].兵工學(xué)報,2011(8):1002-1007.

[7] 王玲,邵金鑫,萬建偉.基于相對觀測量的多機器人定位[J].國防科技大學(xué)學(xué)報,2006(2):67-72.

[8] 秦永元，張洪鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社,1998.

[9] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2005.

[10] 馬培圣.基于MEMS的微小型GPS/SINS組合測姿系統(tǒng)研究[D].南京:南京航空航天大學(xué),2012.

[11] 程承,楊峰,張共愿,等.一種機群相對定位方法[J].火力與指揮控制,2012(3):13-15,19.

[12] 劉俊成,張京娟,馮培德.基于相互測距信息的機群組網(wǎng)協(xié)同定位技術(shù)[J].北京航空航天大學(xué)學(xué)報,2012(4):541-545.

Multi-UAVCooperativeNavigationAlgorithmBasedonGeometricCharacteristics

PAN Ruihonga, XU Shenghongb

(a.Graduate Students’ Brigade; b.Department of Control Engineering， Naval Aeronautical and Astronautical University, Yantai 264001, China)

Aiming at the problem of how to improve the navigation and positioning accuracy of cluster members under the condition that the cluster unmanned aerial vehicle(UAV) do not use GPS and high precision inertial navigation system(INS), a multi-UAV cooperative navigation algorithm based on geometric characteristics is proposed. The position and velocity information of the UAV is used as the state quantity, and the kinematic model of the system composed of three UAVs is established. The shape and size of the geometric triangle formed by the UAV as the vertex and its attitude in space is used as the measurement quantity, and the position information of each UAV in the system is estimated by using Kalman filtering. The simulation results show that the proposed cooperative navigation algorithm can improve the positioning accuracy of the cluster UAV, which is better than the dead reckoning, and the positioning error does not accumulate with time.

cooperative navigation; cluster UAV; geometric triangle; normal vector

2017-05-23；

2017-06-20

潘瑞鴻(1993—)，男，碩士研究生，主要從事飛行器綜合導(dǎo)航技術(shù)研究。

徐勝紅(1974—)，男，博士，副教授，碩士生導(dǎo)師，主要從事導(dǎo)航、制導(dǎo)與控制研究。

10.11809/scbgxb2017.10.012

本文引用格式：潘瑞鴻,徐勝紅.基于幾何特性的多無人機協(xié)同導(dǎo)航算法[J].兵器裝備工程學(xué)報，2017(10):55-59，96.

formatPAN Ruihong, XU Shenghong.Multi-UAV Cooperative Navigation Algorithm Based on Geometric Characteristics[J].Journal of Ordnance Equipment Engineering,2017(10):55-59，96.

V249.32+8

A

2096-2304(2017)10-0055-05

(責(zé)任編輯周江川)