基于距離測量的多無人機(jī)協(xié)同目標(biāo)定位方法

屈耀紅, 張峰, 谷任能, 袁冬莉

(西北工業(yè)大學(xué) 自動化學(xué)院, 陜西 西安 710072)

戰(zhàn)場目標(biāo)的精確定位是現(xiàn)代化戰(zhàn)爭中的關(guān)鍵技術(shù)之一[1]。隨著無人機(jī)技術(shù)的快速發(fā)展，利用多架無人機(jī)協(xié)同對目標(biāo)定位，已成為當(dāng)前軍事科技領(lǐng)域的研究熱點(diǎn)[2]。

目前，由于無線電測距技術(shù)發(fā)展成熟，其精度高且成本低，因此基于距離測量的目標(biāo)定位方法的研究受到了普遍關(guān)注。文獻(xiàn)[3]提出了一種基于約束總體最小二乘(constrained total least-squares,CTLS)的固定觀測站時(shí)差定位算法(time-difference-of-arrival,TDOA)，在忽略觀測站坐標(biāo)誤差時(shí)，其定位精度較高，但若觀測點(diǎn)坐標(biāo)測量存在隨機(jī)誤差時(shí)其定位性能急劇下降。文獻(xiàn)[4]提出了一種線性校正到達(dá)時(shí)間差的定位算法，在假設(shè)傳感器位置信息精確已知的條件下，該方法可以獲得很精確的定位結(jié)果，但如果傳感器位置信息存在隨機(jī)誤差，該方法就會嚴(yán)重降低目標(biāo)的定位精度。文獻(xiàn)[5]提出利用斜距離解析目標(biāo)位置的定位方法，同樣，該方法并未考慮觀測機(jī)的坐標(biāo)測量誤差，當(dāng)該誤差較大時(shí)，其定位性能急劇下降。

另外，幾何定位精度因子(geometric dilution precision,GDOP)是衡量一個(gè)定位系統(tǒng)精度的重要指標(biāo)之一，其取值與各觀測點(diǎn)之間不同的幾何分布相關(guān)[6]。

綜上分析，本文提出了一種利用多無人機(jī)與目標(biāo)之間的距離以及無人機(jī)站址坐標(biāo)估算目標(biāo)位置的協(xié)同定位算法，該方法克服了站址誤差不可忽略時(shí)定位效果急劇下降的不足，同時(shí)，利用GDOP優(yōu)化了編隊(duì)隊(duì)形，進(jìn)一步提高了定位效果。受到了參考文獻(xiàn)[7]的啟發(fā)，本文提出的方法同時(shí)保證了較高的定位精度和較強(qiáng)的適應(yīng)性。

1 定位模型及定位方法

1.1 定位條件

如圖1所示，至少需要3架無人機(jī)來對目標(biāo)進(jìn)行協(xié)同定位。假定每個(gè)無人機(jī)均裝備有GPS和測距傳感器，并圍繞同一個(gè)目標(biāo)飛行，目標(biāo)大致位于編隊(duì)中心點(diǎn)下方。無人機(jī)可利用GPS獲得自身的位置坐標(biāo)，利用測距傳感器測量自身到目標(biāo)的距離。考慮到信號時(shí)間的同步性，做出如下假設(shè)：

若目標(biāo)為移動目標(biāo)，在多架無人機(jī)進(jìn)行定位作業(yè)之前，以機(jī)載GPS時(shí)鐘信號為基準(zhǔn)，完成3架無人機(jī)時(shí)鐘初始校準(zhǔn)；其次，將3架無人機(jī)獲取的位置信息及距離信息標(biāo)志時(shí)間戳，選取時(shí)間戳差值較小的3組數(shù)據(jù)，且假定目標(biāo)移動較慢，即目標(biāo)在信號偏差時(shí)間內(nèi)的位移量可忽略；最后，利用選取時(shí)間戳相近的3組數(shù)據(jù)完成目標(biāo)坐標(biāo)解算。

若目標(biāo)靜止不動，則不需要考慮信號時(shí)間的同步性，利用3架無人機(jī)位置信息及距離信息，構(gòu)成定位方程即可實(shí)現(xiàn)目標(biāo)定位解算。

圖1 多無人機(jī)協(xié)同定位實(shí)例

1.2 定位方法

本文提出的算法主要由三大部分組成：首先，利用傳統(tǒng)距離定位算法求出目標(biāo)的大致位置x0;然后,將距離測量的非線性方程在x0處一階Taylor展開轉(zhuǎn)化為量測距離信息加上無人機(jī)的站址誤差以及測距傳感器的測距誤差信息方程,求出定位解的表達(dá)式;最后,求出定位解中參變量的值,完成目標(biāo)位置的估算。接下來是對每個(gè)部分的詳細(xì)介紹。

1.2.1 目標(biāo)大致位置的求解

假定多機(jī)協(xié)同定位模型處于地心空間直角坐標(biāo)系中,目標(biāo)的真實(shí)坐標(biāo)為xt=[xt,yt,zt]T,無人機(jī)觀測站的坐標(biāo)為xi=[xi,yi,zi]T,i=1,2,3。各無人機(jī)到目標(biāo)之間的距離為

(1)

令

(2)

式中，r為目標(biāo)所屬區(qū)域與地球質(zhì)心之間的距離,對(1)式平方并聯(lián)立(2)式可以得到

(3)

即

至此,傳統(tǒng)距離定位方程如(3)式所示,將其寫為矩陣形式有

A1xt=B1

(4)

式中

計(jì)算上式,目標(biāo)的大致位置x0為

(5)

將其作為Taylor展開點(diǎn),完成后續(xù)的算法。

1.2.2 目標(biāo)坐標(biāo)的求解

引入測距誤差信息(ερi)構(gòu)建定位方程為

ρi=‖xi-xt‖+ερi,i=1～3

(6)

(7)

至此得到初步定位方程,寫成矩陣的形式如下

Ay=B

(8)

式中:

類似于(4)式,可以求出初步定位方程的解

(9)

(10)

式中:

結(jié)合(10)～(14),可以得到誤差方程為

(A-ΔA)y=(B-ΔB)

(15)

令

(16)

令

(17)

則有

A*y=B*

(18)

ΔA,ΔB可以表示為

(19)

令Hy=xG1+yG2+zG3-G4,其中(x,y,z)的值為初步定位方程的解y,目標(biāo)的定位解為:

(20)

聯(lián)立(19)、(20)式可得

(21)

為了求解yTLS,對(19)式求一階導(dǎo)并運(yùn)算可得

(22)

1.2.3 參數(shù)λ的求解

令

yTLS=y*+δy

(23)

即δy為真實(shí)值與定位解的差值,將(21)式代入(19)式可以得到δy的表達(dá)式

(24)

其均方誤差為

(25)

參數(shù)λ的估計(jì)值為

(26)

定義

(27)

(28)

D=diag[(μ1+λ)-2,(μ2+λ)-2,(μN(yùn)+λ)-2]

(29)

將(26)和(27)式代入(25)式可得

(30)

(31)

將(26)、(30)和(31)式聯(lián)立得到參數(shù)λ為

(32)

根據(jù)參考文獻(xiàn)[8],選取λ的值為

(33)

將參數(shù)λ的值代入定位解的表達(dá)式得到定位解為

(34)

1.3 幾何精度因子(GDOP)的求解

從方程(10)可以得到

(35)

式中,ε為等效距離誤差,δx=(δxt,δyt,δzt)為無人機(jī)站址誤差,將(35)式展開

δρ=Hδx+ε

(36)

式中

近似地,可以得到

δρ=Hδx

(39)

對(39)式等號兩邊均左乘以H-1有

δx=H-1δρ

(40)

誤差協(xié)方差為

E(δxδxT)=H-1E(δρδρT)(HT)-1

(41)

因?yàn)镚DOP僅跟對角元素有關(guān),可假設(shè)E(δρδρT)互不相關(guān)且方差相同均為1,則有

E(δxδxT)=(HTH)-1

(42)

(43)

2 仿真實(shí)驗(yàn)

2.1 仿真條件

為驗(yàn)證所提定位方法的有效性及適應(yīng)性,本文提出以下假設(shè)條件:

1) 3架無人機(jī)定高并圍繞目標(biāo)飛行,每架無人機(jī)的距離測量誤差方差及站址測量誤差方差滿足如下關(guān)系:

2) 文中的坐標(biāo)系統(tǒng)為WGS-84地心空間直角坐標(biāo)系,坐標(biāo)原點(diǎn)為地球質(zhì)心。

2.2 仿真結(jié)果

2.2.1 定位算法的有效性及適應(yīng)性分析

目標(biāo)的真實(shí)位置為xtrue=(6 371,20,0.2),3架無人機(jī)的初始位置如表1所示,單位為km?；诒疚奶岢龅亩ㄎ凰惴?令每次試驗(yàn)中站址量測誤差取值于區(qū)間[0,0.01 km],目標(biāo)的大致位置x0=[x0,y0,z0]T及目標(biāo)的求解坐標(biāo)xt=[xt,yt,zt]T如圖2所示。

表1 無人機(jī)的初始位置

圖2 目標(biāo)的大致位置及定位解

隨著站址誤差的增大,目標(biāo)的大致位置和求解坐標(biāo)均不同程度的偏離真實(shí)坐標(biāo),顯然,xt更接近真實(shí)位置;此外,即使目標(biāo)的大致位置與真實(shí)位置偏差較大時(shí),本算法依然能解算到較為理想的坐標(biāo)。

為分析定位算法的有效性及適應(yīng)性,將本方法與傳統(tǒng)距離定位方法進(jìn)行了兩方面比較:當(dāng)無人機(jī)觀測站的站址誤差可以忽略時(shí),改變距離測量誤差的取值,比較二者的定位精度,仿真如圖3所示;此外,在距離測量誤差不變的前提下,比較二者對于無人機(jī)站址測量誤差變化的適應(yīng)性,仿真如圖4所示。

圖3 定位誤差隨距離量測誤差變化的比較圖

如圖3所示,隨著距離測量誤差的增大，本算法保持著較好的定位精度。顯然,當(dāng)無人機(jī)站址量測誤差為零時(shí),本算法具有較好的定位效果。從傳統(tǒng)的距離算法可以清楚地看出,其定位精度受到地球半徑精度的影響,在算法過程中引入了更多的誤差。綜上,本方法更具有效性。

將距離量測誤差的值設(shè)為σ2=0.002 km,并引入無人機(jī)觀測站的站址量測誤差,2個(gè)算法的定位誤差隨站址量測誤差變化的比較如圖4所示:

圖4 定位誤差隨站址量測誤差變化的比較圖

由圖4可知,若站址量測誤差不可忽略,顯然本算法適應(yīng)性更強(qiáng),精度更高;結(jié)合圖3與圖4可以看出,本方法的定位精度明顯優(yōu)于傳統(tǒng)定位方法。

2.2.2 無人機(jī)編隊(duì)隊(duì)形分析

選取最小GDOP對應(yīng)的編隊(duì)隊(duì)形作為理想編隊(duì)隊(duì)形,以提高定位精度?；趫D1所提出的多機(jī)協(xié)同定位模型,本文利用3架無人機(jī)的初始位置表示其組成的編隊(duì)隊(duì)形,編隊(duì)隊(duì)形的鳥瞰圖如圖5所示,多無人機(jī)定高并圍繞目標(biāo)飛行,其航跡共圓且目標(biāo)位置的投影包含于圓中?；?.3節(jié)所提出的GDOP的求解方法,利用在線枚舉法將無人機(jī)編隊(duì)的所有隊(duì)形所映射的GDOP值求解并找出最小GDOP值所映射的編隊(duì)隊(duì)形。無人機(jī)的編隊(duì)隊(duì)形的表示如下:

圖5 無人機(jī)編隊(duì)隊(duì)形鳥瞰圖

1) UAV1和UAV2之間的距離d保持不變,UAV3的位置沿著航跡圓運(yùn)動,得到d條件下的所有隊(duì)形,如表2所示。

表2 d=1.8時(shí)無人機(jī)編隊(duì)隊(duì)形

2) 令d在區(qū)間(0,2r)中自小到大依次取值,同時(shí),UAV3的位置沿著航跡圓運(yùn)動,得到無人機(jī)編隊(duì)的所有隊(duì)形,如表3所示。

表3 無人機(jī)編隊(duì)的所有隊(duì)形

對表2所述的無人機(jī)隊(duì)形進(jìn)行GDOP求解,其仿真結(jié)果如圖6所示。

圖6 GDOP隨無人機(jī)編隊(duì)部分隊(duì)形變化圖

如圖6所示,GDOP的值隨著UAV3橫坐標(biāo)x增大而先減小后增大,有一個(gè)明顯的極小值,此時(shí)極小值所映射的無人機(jī)編隊(duì)隊(duì)形就是在d=1.8條件下的最優(yōu)編隊(duì)隊(duì)形,該隊(duì)形是由(6 370.1,19.5,1),(6 371.9,19.5,1),(6 371,21,1)這3個(gè)點(diǎn)所確定的一個(gè)銳角三角形。

對于如表3所述的所有的無人機(jī)編隊(duì)隊(duì)形,依次求解隊(duì)形所映射的GDOP,結(jié)果如圖7所示。

圖7中GDOP有一個(gè)十分明顯的極小值,位于d=1.71,x=6 371.2處,無人機(jī)編隊(duì)的位置為：(6 370.145,19.481,1),(6 371.855,19.481,1),(6 371.2,20.980,1),即無人機(jī)的最佳編隊(duì)隊(duì)形為正三角形,其目標(biāo)位于正三角形中心正下方附近時(shí)GDOP最小。

圖7 GDOP隨無人機(jī)編隊(duì)的所有隊(duì)形變化圖

2.3 仿真結(jié)果分析

從圖2可知，在不考慮無人機(jī)觀測站的站址誤差時(shí)，本文提出的定位方法效果優(yōu)于傳統(tǒng)距離定位算法。究其原因，傳統(tǒng)距離定位方法精度受到地球半徑精度的限制，引入了新的誤差項(xiàng)。對比圖2和圖3可知，引入無人機(jī)站址誤差后，傳統(tǒng)方法的定位效果會差一些。綜上，本方法具有良好的有效性和適應(yīng)性。

基于本文所建立的多機(jī)協(xié)同定位模型，表述出3架無人機(jī)航跡共圓的所有隊(duì)形，利用在線枚舉法尋求最小GDOP值對應(yīng)的編隊(duì)隊(duì)形，如圖5～6所示，保持無人機(jī)編隊(duì)隊(duì)形為正三角形，盡量使無人機(jī)編隊(duì)的中心在地面的投影靠近目標(biāo)的位置，可提高目標(biāo)定位精度。

3 結(jié) 論

本文提出了一種多機(jī)協(xié)同地面目標(biāo)定位方法，綜合考慮了無人機(jī)觀測站的量測誤差和距離測量誤差，定位效果較傳統(tǒng)的距離定位方法來說更好，通過仿真驗(yàn)證了其有效性和適用性。

對于本文所建立的多無人機(jī)協(xié)同定位模型，目標(biāo)的位置處于正三角形的中心正下方附近，應(yīng)保證編隊(duì)隊(duì)形為正三角形，以提高無人機(jī)定位精度。