[發(fā)明專利]一種雙無人機跟蹤測距方法

有效

• 申請?zhí)枺?/td>: 201711083451.6
• 申請日: 2017-11-07
• 公開（公告）號: CN107884781B
• 公開（公告）日: 2020-06-02
• 發(fā)明（設(shè)計）人: 陳峰;劉芊;張宇;高文龍
• 申請（專利權(quán)）人: 北京電子工程總體研究所
• 主分類號: G01S17/66
• 分類號: G01S17/66;G01S17/50
• 代理公司: 北京正理專利代理有限公司 11257
• 代理人: 付生輝

1.一種雙無人機跟蹤測距方法，其特征在于，兩架裝載有光電跟蹤設(shè)備的無人機同時跟蹤一個目標(biāo)，將視線的方位值和俯仰值傳送給地面控制系統(tǒng)，控制系統(tǒng)使用空間解析算法計算得到目標(biāo)的坐標(biāo)；其中，

所述控制系統(tǒng)的計算過程包括以下步驟：

1)、獲得跟蹤狀態(tài)下兩無人機的視線的方位值和俯仰值，并在視線上找到距無人機單位距離的點，求得該點在無人機機體坐標(biāo)系下的三維坐標(biāo)；

其中，ε_TS和β_TS分別是無人機視線的方位值和俯仰值，X_TS、Y_TS、Z_TS分別是單位距離的點在無人機機體坐標(biāo)系下的坐標(biāo)；

2)、將單位距離的點的坐標(biāo)從無人機機體坐標(biāo)系轉(zhuǎn)換到無人機地理坐標(biāo)系；

無人機通過光電跟蹤設(shè)備獲得的無人機機體坐標(biāo)系WZ下的坐標(biāo)，轉(zhuǎn)換到無人機地理坐標(biāo)系下；

式中X_GWZ、Y_GWZ、Z_GWZ是單位距離點在無人機地理坐標(biāo)系下的坐標(biāo)，X_TS、Y_TS、Z_TS是單位距離點在無人機機體坐標(biāo)系下位置坐標(biāo)；

φ_WZ為：無人機的航向角；

θ_WZ為：無人機的俯仰角；

γ_WZ為：無人機的傾斜角；

3)、將無人機在大地坐標(biāo)系中的坐標(biāo)轉(zhuǎn)換到地心直角坐標(biāo)系；

無人機位置在大地坐標(biāo)系中的坐標(biāo)為(L,B,H)，在地心直角坐標(biāo)系中的坐標(biāo)為(x_GX0,y_GX,z_GX0)，兩者之間的轉(zhuǎn)換關(guān)系：

大地坐標(biāo)系使用WGS-84大地坐標(biāo)系，用(L,B,H)表示；

原點為格林尼治子午線與地球赤道的交點；

L為大地經(jīng)度；

B為大地緯度；

H為從參考橢球面量起的法向距離，即高度；經(jīng)度、緯度以“弧度”為單位進行計算，高度以“米”為單位進行計算；

WGS-84大地坐標(biāo)系橢球采用的常用基本參數(shù)：橢球長半軸：a＝6378137m；橢球短半軸：b＝6356752.3142m；橢球扁率：第一偏心率平方：

式中N為無人機所在位置的卯酉圈曲率半徑；a為橢球長半軸；e為第一偏心率；

4)、將單位距離的點在無人機地理坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到地心直角坐標(biāo)系下；

式中(L₀,B₀)為：無人機地理坐標(biāo)系原點在大地坐標(biāo)系中的經(jīng)度和緯度；

(X_GX0,Y_GX0,Z_GX0)為：無人機地理坐標(biāo)系原點即無人機在地心直角坐標(biāo)系下的位置坐標(biāo)；

(X_GX,Y_GX,Z_GX)為：地心直角坐標(biāo)系下單位距離的點的位置坐標(biāo)；

(X_GWZ,Y_GWZ,Z_GWZ)為：無人機地理坐標(biāo)系下的單位距離的點位置坐標(biāo)；

5)、判斷兩無人機視線在空間的平行、異面和相交情況；

根據(jù)步驟3)中的無人機在地心直角坐標(biāo)系中的坐標(biāo)(x_GX0,y_GX0,z_GX0)，和步驟4)中的單位距離的點在地心直角坐標(biāo)系的坐標(biāo)(X_GX,Y_GX,Z_GX)，使用向量空間的方法得到兩視線的單位向量v10、v20和兩無人機連線向量t；

根據(jù)v10、v20和t判斷兩無人機視線在空間的平行、異面和相交情況：

6)、如果步驟5)的判斷結(jié)果為相交，求出兩直線的交點坐標(biāo)即為目標(biāo)坐標(biāo)；

7)、如果步驟5)的判斷結(jié)果為異面，求出這兩條視線的最短距離，以及在這個距離上的兩視線最接近點的坐標(biāo)；兩最接近點的中點的坐標(biāo)即為目標(biāo)坐標(biāo)；

8)、根據(jù)目標(biāo)坐標(biāo)求得目標(biāo)和無人機距離。