基于相對速度和位置輔助的無人機(jī)編隊協(xié)同導(dǎo)航

郭鵬軍， 張 睿， 高關(guān)根， 許 斌

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

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

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

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

1 協(xié)同導(dǎo)航系統(tǒng)方案設(shè)計

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

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

2 相對導(dǎo)航傳感器誤差模型

2.1 多普勒測速誤差模型

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

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

(1)

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

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

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

(2)

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

XDVL=

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

(3)

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

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

(4)

式中：

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

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

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

(5)

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

L=Ltkl+δl

(6)

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

激光測角使用波達(dá)角度傳感器進(jìn)行測量，如激光測角傳感器等，其測量值可表示為

αc=αt+δα

(7)

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

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

XR=[klδlδα]T

(8)

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

(9)

式中：

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

3 相對導(dǎo)航矢量模型計算

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

3.1 b系下相對導(dǎo)航矢量模型

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

(10)

3.2 相對導(dǎo)航矢量模型空間轉(zhuǎn)換

(11)

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

(12)

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

(13)

圖2 僚機(jī)b系下相對導(dǎo)航矢量Fig.2 Relative navigation vector in b coordinates of slave

3.3 相對位置矢量和慣導(dǎo)輸出位置轉(zhuǎn)換

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

(14)

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

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

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

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

(15)

式中：

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

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

(16)

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

(17)

(18)

式中：Φ×為失準(zhǔn)角矢量Φ的反對稱矩陣，即

(19)

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

(20)

(21)

則速度觀測量可表示為

(22)

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

Zv=HvX+Vv

(23)

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

(24)

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

(25)

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

(26)

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

Zp=HpX+Vp

(27)

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

(28)

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

5 仿真及結(jié)果分析

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

表1 相對導(dǎo)航傳感器配置Tab.1 Configuration of relative navigation sensor

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

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

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

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

圖4 僚機(jī)S1慣性導(dǎo)航解算誤差Fig.4 Error of inertial navigation solution of Slave S1

圖5 僚機(jī)S2慣性導(dǎo)航解算誤差Fig.5 Error of inertial navigation solution of Slave S2

圖6 僚機(jī)S3慣性導(dǎo)航解算誤差Fig.6 Error of inertial navigation solution of Slave S3

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

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

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

6 結(jié)語

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