基于幾何特性的多無人機協同導航算法

潘瑞鴻, 徐勝紅

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

【裝備理論與裝備技術】

基于幾何特性的多無人機協同導航算法

潘瑞鴻a, 徐勝紅b

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

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

協同導航；集群無人機；幾何三角形；法向量

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

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

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

1 多無人機系統建模

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

選取狀態量為

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個方向上的速度信息。下標1、2、3分別表示無人機1、2、3。

則系統運動學模型即狀態方程為

(2)

離散化為

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

(3)

式中：系統狀態轉移矩陣：

系統噪聲輸入矩陣

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

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

2 協同導航算法分析

不同于單架無人機，集群無人機不僅有自身的絕對位置，還有與其他協同成員的相對位置，而無人機之間測距和測角信息的精度是比較高的。從信息的角度看，對于兩種相關但精度不同的信息，可以利用精度高的信息去校正精度低的信息。因此，利用無人機之間的相對位置信息去校正各無人機自身導航設備輸出的絕對位置信息是合理的。考慮構造一種新的量測量，能夠包含協同成員之間所有的相對位置信息，通過卡爾曼濾波，對狀態量進行估計。

假設無人機之間為理想通信情況，且不考慮通信延遲，即無人機兩兩之間的相對距離、相對角度以及自身導航系統輸出參數等信息都能夠實時傳輸。系統總體流程簡圖如圖1。

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

圖1 系統總體流程簡圖

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

n=[nx,ny,nz]

(4)

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

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

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

且i≠j

(5)

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

(6)

(7)

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

假定相對距離、俯仰角和方位角的量測噪聲具有統計特性：E[vrij]=E[vθij]=E[vφij]=0，Var[vrij]=(σrij)2，Var[vθij]=(σθij)2，Var[vφij]=(σφij)2，且三者兩兩互不相關，即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)

量測量與狀態量的具體關系如下：

(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的推導可參考文獻[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，為系統量測量，V=[vr12,vr13,vr23,vnx,vny,vnz]T，為系統量測噪聲，根據其統計特性，可推導出量測噪聲的協方差矩陣為

(16)

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

(17)

式(17)中

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

3 仿真分析

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

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

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

圖3 協同導航位置誤差曲線(無人機1)

圖4 協同導航位置誤差曲線(無人機2)

圖5 協同導航位置誤差曲線(無人機3)

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

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

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

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

4 結論

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

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

[1] 周紹磊,康宇航,秦亮,等.多無人機協同控制的研究現狀與主要挑戰[J].飛航導彈,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] 張共愿,程詠梅,程承,等.基于相對導航的多平臺INS誤差聯合修正方法[J].航空學報,2011(2):271-280.

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

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

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

[9] 秦永元.慣性導航[M].北京:科學出版社,2005.

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

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

[12] 劉俊成,張京娟,馮培德.基于相互測距信息的機群組網協同定位技術[J].北京航空航天大學學報,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—)，男，碩士研究生，主要從事飛行器綜合導航技術研究。

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

10.11809/scbgxb2017.10.012

本文引用格式：潘瑞鴻,徐勝紅.基于幾何特性的多無人機協同導航算法[J].兵器裝備工程學報，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

(責任編輯周江川)