郭鵬軍， 張 睿， 高關根， 許 斌

(1. 西北工業(yè)大學 自動化學院， 西安 710072； 2. 航空工業(yè)西安飛行自動控制研究所， 西安 710065)

近幾年來隨著科學技術不斷發(fā)展，無人機(Unmanned Aerial Vehicle, UAV)在軍事和民用領域應用越來越廣泛.單個無人機執(zhí)行任務能力有限，相比之下，無人機編隊協(xié)同工作具有任務完成率高、功能性強和優(yōu)化任務分配等優(yōu)點.實時獲取高精度導航信息是無人機編隊飛行的可靠保障[1-2].目前，主流的無人機編隊導航方式是全球導航衛(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)和慣性導航系統(tǒng)(Inertial Navigation System, INS)組合導航，利用GNSS信號修正慣性設備的漂移誤差.但復雜戰(zhàn)場環(huán)境或外界遮擋環(huán)境可能導致GNSS不可用，其慣性導航在長時間運行中會導致誤差積累，若編隊都采用高精度的慣性器件，飛行成本會大幅增加.

無人機協(xié)同導航利用各成員自身導航信息以及各成員之間的相對導航信息共享來抑制慣性導航的解算誤差.根據(jù)編隊內的信息交互架構，可將無人機編隊方式分為平行式[3-4]和主從式[5-6].平行式以去中心化為思想，各成員任務等級相同；而主從式編隊中，成員分為長機和僚機，在全球定位系統(tǒng)(Global Positioning System, GPS)拒止環(huán)境中，長機和僚機通過機載數(shù)據(jù)鏈進行導航信息的交互[7]，長機可在敵方防控區(qū)域外進行巡航，而由僚機進入拒止區(qū)域進行偵察和打擊[8].協(xié)同導航技術主要涉及相對導航方式[9-10]、編隊構型、集群信息交互和定位導航算法等技術[11-12].目前針對協(xié)同導航已有一定研究，文獻[13]依據(jù)無線信號的接收角度和信號強度得到方位距離，并基于幾何關系進行求解，但該模型為二維陸地機器人編隊模型，無法應用于高機動特性的三維無人機編隊；文獻[14]基于聯(lián)邦濾波提出無人機集群分層式協(xié)同導航模型，提高了編隊導航的容錯能力，但其協(xié)同導航模型只對其速度和位置誤差進行估計，并未考慮相對傳感器誤差以及僚機失準角的校正，無法滿足長航時的導航需求.

本研究針對主從式無人機編隊形式提出一種相對速度和位置輔助的無人機協(xié)同導航方案.首先，在考慮相對導航傳感器誤差的前提下，結合慣性導航的誤差方程建立協(xié)同導航系統(tǒng)的狀態(tài)方程；其次，將相對導航信息與絕對導航信息進行空間統(tǒng)一，建立相對速度和相對位置綜合量測模型；最后，利用卡爾曼濾波器估計僚機的導航誤差并進行誤差補償，提升各僚機的導航精度.

1 協(xié)同導航系統(tǒng)方案設計

協(xié)同導航系統(tǒng)方案基本思路如圖1所示.圖中：M為長機；Sn為僚機，n∈N；SINS為捷聯(lián)慣導系統(tǒng).編隊中長機通過組合導航提供自身高精度的導航信息，并通過機載數(shù)據(jù)鏈與各個僚機進行導航信息共享.然后，以僚機INS的誤差模型、多普勒測速誤差模型以及激光測角和測距誤差模型組合建立系統(tǒng)的狀態(tài)方程，僚機通過相對導航傳感器可測得自身與長機的相對速度和位置信息，結合接收到的長機導航信息可推算出僚機的速度及位置信息，將其與僚機慣導輸出的速度位置作差，從而建立協(xié)同導航系統(tǒng)的量測方程.利用協(xié)同導航濾波器估計出僚機INS的誤差以及相對傳感器的測量誤差，通過反饋對僚機INS以及相對導航傳感器進行補償，抑制僚機慣導的漂移，從而提升無人機集群的導航精度.

圖1 無人機協(xié)同導航方案框圖Fig.1 Diagram of UAV cooperative navigation scheme

2 相對導航傳感器誤差模型

2.1 多普勒測速誤差模型

根據(jù)多普勒測速原理以及相應的誤差來源可將機載三維多普勒測速裝置的速度測量值表示為

vd=(I+δA)(I+δK)(vt+δv)

(1)

式中：I為3×3的單位矩陣；vt為速度真實值；δA為三軸安裝誤差；δK為測速刻度因數(shù)；δv為三維速度隨機誤差，相應值為

由于式(1)中誤差都為小量，展開后忽略耦合項可得：

vd=vt+δAvt+δKvt+δv

(2)

因此，選擇多普勒測速儀的測速誤差狀態(tài)為

XDVL=

[δKxδKyδKzδAxδAyδAzδvxδvyδvz]T

(3)

式中：δKi和δAi為隨機常量，δvi為隨機測速誤差，i=x,y,z，可由一階馬爾可夫過程近似描述.

多普勒測速儀的誤差狀態(tài)方程為

(4)

式中：

τx,τy,τz和wx,wy,wz分別為多普勒測速誤差模型一階馬爾可夫過程的相關時間和白噪聲.

2.2 激光測距和測角誤差模型

激光雷達采用激光飛行時間法測距，測距公式如下：

(5)

式中：Lt為距離測量真實值；Δt為激光來回的傳播時間；c為激光束在真空中傳播的速度；n為大氣折射率；N為激光束的脈沖總數(shù)；T0為激光的脈沖長度.當考慮回波強度對測量距離產生的隨機測量誤差時，激光測距的相對距離測量值可表示為

L=Ltkl+δl

(6)

式中：kl為修正系數(shù)，是隨機常數(shù)；δl為波強的隨機測量誤差.

激光測角使用波達角度傳感器進行測量，如激光測角傳感器等，其測量值可表示為

αc=αt+δα

(7)

式中：αt為角度真實值；δα為角度測量的隨機誤差.

因此，選擇激光測角和測距傳感器誤差狀態(tài)量為

XR=[klδlδα]T

(8)

式中：δl，δα由一階馬爾可夫過程近似描述.因此激光測角和測距誤差狀態(tài)方程為

(9)

式中：

τl,τα和wl,wα分別為激光測距和測角誤差模型一階馬爾可夫相關過程時間和驅動噪聲.

3 相對導航矢量模型計算

由于相對導航傳感器的輸出是載體坐標系(b系)下的相對導航信息，而慣導解算出的信息為導航坐標系(n系)下的絕對導航信息，所以需將相對導航信息進行空間統(tǒng)一.

3.1 b系下相對導航矢量模型

選用地理坐標系為導航坐標系，僚機搭載的相對導航傳感器在b系測得的相對導航信息可根據(jù)測距、測角和測速模型分別表示為

(10)

3.2 相對導航矢量模型空間轉換

(11)

將式(10)代入式(11)，同時忽略二階小量，可得：

(12)

式中：δpbx,δpby,δpbz為偽量測噪聲.

(13)

圖2 僚機b系下相對導航矢量Fig.2 Relative navigation vector in b coordinates of slave

3.3 相對位置矢量和慣導輸出位置轉換

僚機INS輸出的三維位置信息為緯、經(jīng)、高度，而相對導航傳感器輸出的三維位置信息為3個方向的距離.由于無人機編隊間距遠小于地球直徑，所以長、僚機間的直線距離可近似為以地心為圓心過二者的弧線長度，則相對導航推算的僚機緯、經(jīng)、高度信息可表示為

(14)

式中：R為地球半徑；LM,λM,hM為長機緯、經(jīng)、高度.

4 協(xié)同導航系統(tǒng)的數(shù)學模型

4.1 協(xié)同導航系統(tǒng)的狀態(tài)方程

聯(lián)立多普勒測速的誤差模型、激光測距誤差模型，并參考文獻[15]中的僚機慣導誤差方程，得到協(xié)同導航系統(tǒng)的狀態(tài)方程：

(15)

式中：

4.2 協(xié)同導航系統(tǒng)的量測方程

在n系中，僚機通過機載數(shù)據(jù)鏈接收長機的導航信息，與僚機測量的相對導航信息作差可得僚機的推算導航信息，再通過僚機自身INS信息與推算的僚機導航信息相減得到量測結果.因為長機定位較為精確，所以對其誤差進行忽略，即

(16)

4.2.1速度量測矩陣 由b系下的速度誤差模型可知：

(17)

(18)

式中：Φ×為失準角矢量Φ的反對稱矩陣，即

(19)

聯(lián)立式(18)和式(13)，并忽略二階耦合項，n系下相對速度的測量值可表示為

(20)

(21)

則速度觀測量可表示為

(22)

將Φ×,δA,δK分別代入式(22)可得：

Zv=HvX+Vv

(23)

4.2.2位置量測矩陣 n系下相對位置信息可表示為真實信息和誤差之和：

(24)

將式(24)帶入式(14)，可得到僚機的相對導航推算的緯、經(jīng)、高度信息：

(25)

此外，僚機慣導輸出的位置信息可表示為真實緯、經(jīng)、高度信息與對應誤差之和：

(26)

將式(25)和(26)代入式(16)，可得到位置量測矩陣為

Zp=HpX+Vp

(27)

4.2.3協(xié)同導航系統(tǒng)的量測方程 聯(lián)立式(23)和式(27)，協(xié)同導航系統(tǒng)的觀測矩陣為

(28)

根據(jù)狀態(tài)方程和量測方程，利用卡爾曼濾波對相對導航傳感器以及僚機慣性導航的誤差進行估計并校正.

5 仿真及結果分析

為驗證所提協(xié)同導航算法的有效性，搭建長機、僚機無人機編隊協(xié)同導航驗證平臺.為有效描述無人機集群間的相對位姿，長機、僚機的相對位姿都由編隊參考點給出，其參考點導航信息取(34°, 108°, 100 m)，相對導航傳感器配置如表1所示.此外，實際主從式無人機編隊中，長機搭載高精度慣性導航，而僚機搭載低精度的慣性導航.因此，在仿真中長機配置高精度INS，比僚機INS精度高10～20倍，長機、僚機的導航參數(shù)配置如表2所示.

表1 相對導航傳感器配置Tab.1 Configuration of relative navigation sensor

表2 編隊成員導航參數(shù)配置

根據(jù)無人機編隊要求，設置單長機三僚機的無人機編隊，通過軌跡發(fā)生器設計編隊飛行軌跡，編隊先后進行加速、爬升、勻速、轉彎等機動形式.此外，為保持編隊隊形，設計僚機飛行軌跡與長機同步，飛行過程中編隊構型不發(fā)生改變，僚機的運動軌跡與長機同步，設置仿真時長為 1 800 s，其在導航坐標系中的軌跡曲線如圖3所示，曲線均為無人機相對于參考點的真實飛行軌跡.

圖3 長機和僚機真實軌跡Fig.3 Real trajectory of leader and slave

編隊以圖3設計的軌跡飛行，其中長機配備表2中對應的高精度慣性器件和GPS進行組合導航，其組合導航在各方向上的速度誤差小于0.02 m/s，定位誤差小于3 m；為與所提協(xié)同導航方案進行對比，首先各僚機S1, S2, S3僅采用表2中對應的低精度INS進行導航，經(jīng)過 1 800 s 飛行，得到誤差曲線如圖4～6所示，圖中：t為仿真時間；δA為純慣導解算的失準角誤差；δv為純慣導解算的速度誤差；δp為純慣導解算的位置誤差；δθ，δγ，δψ分別為僚機東、北、天方向的失準角誤差；δve，δvn，δvu分別為僚機東、北、天方向的速度誤差；δL，δλ，δh分別為僚機北、東、天方向的位置誤差.其純慣導解算東向、北向速度誤差發(fā)散至1 m/s和0.5 m/s，東向、北向位置誤差發(fā)散至600 m和400 m，僚機的慣性導航天向速度、位置與氣壓高度計利用卡爾曼濾波進行融合，使其誤差收斂，從整體精度來看各僚機純慣性導航定位精度無法滿足長時間編隊飛行導航的要求.

圖4 僚機S1慣性導航解算誤差Fig.4 Error of inertial navigation solution of Slave S1

圖5 僚機S2慣性導航解算誤差Fig.5 Error of inertial navigation solution of Slave S2

圖6 僚機S3慣性導航解算誤差Fig.6 Error of inertial navigation solution of Slave S3

圖7 僚機S1協(xié)同導航誤差Fig.7 Error of cooperative navigation of Slave S1

圖8 僚機S2協(xié)同導航誤差Fig.8 Error of cooperative navigation of Slave S2

圖9 僚機S3協(xié)同導航誤差Fig.9 Error of cooperative navigation of Slave S3

6 結語

針對主從式無人機編隊形式提出一種無人機協(xié)同導航方案，利用僚機的慣導誤差量以及相對傳感器的誤差量建立協(xié)同導航的系統(tǒng)模型，對相對速度以及相對位置構建量測方程.在此基礎上，通過卡爾曼濾波對誤差進行估計，以此對僚機慣導以及相對導航傳感器誤差進行補償，為無人機編隊提供穩(wěn)定可靠的導航信息.