[發明專利]無人機載光電穩定轉臺跟蹤態建模仿真方法

有效

• 申請號: 201611029070.5
• 申請日: 2016-11-22
• 公開（公告）號: CN106777489B
• 公開（公告）日: 2021-04-06
• 發明（設計）人: 趙溫波;徐從東;趙孔金;陳永科;朱國春;王東;張明義;黃克明;王濤
• 申請（專利權）人: 中國人民解放軍陸軍軍官學院
• 主分類號: G06F30/20
• 分類號: G06F30/20;G06T17/00;G09B9/00
• 代理公司: 安徽合肥華信知識產權代理有限公司 34112
• 代理人: 余成俊

1.無人機載光電穩定轉臺跟蹤態建模仿真方法，其特征在于：

首先將無人機載光電穩定轉臺從功能上簡化為由內環組件、外環組件和支座組成的二軸二框架結構，其中內環組件安裝視點，內環組件通過旋轉軸安裝在外環組件上，可隨外環組件一起繞外環軸做方位旋轉，內環組件繞旋轉軸相對外環轉動，其轉動角記為模型高低角，外環組件的旋轉軸固定在支座上，可繞外環軸相對于支座做方位旋轉，其轉動角記為模型方位角，支座為安裝固定裝置，可固定于無人機平臺上；

接著建立無人機N系坐標，其坐標原點設定在無人機的質心上，X_n軸指向地理北，Z_n軸為重力方向，Y_n軸為向東；

建立航跡S系坐標原點在無人機質心處，X_s軸與無人機規劃航向同向，Z_s軸為重力方向，Y_s軸與X_s軸和Z_s軸為右手定則關系；

建立無人機U系坐標，無人機U系與無人機機體固聯，其坐標原點O_u為無人機的質心，X_u軸為無人機的機頭方向，Z_u軸垂直于機身平面并指向下方，Y_u軸與X_u軸和Z_u軸構成右手定則關系；偏航角為無人機U系X_u軸投影在無人機N系X_nO_nY_n平面上的方位角；俯仰角為無人機U系的X_uO_uY_u平面與無人機N系的X_nO_nY_n平面的夾角，向上為正；滾轉角為無人機U系的X_uO_uZ_u平面與無人機N系的X_nO_nZ_n平面的夾角，右轉為正；

建立光電轉臺坐標系，光電轉臺坐標系包括基座坐標系、方位環A系、高低環F系和橫滾環R系，基座B系，坐標原點O_b為轉軸中心，Z_b軸由鏡頭指向目標，X_b軸、Y_b軸分別平行于無人機U系的X_u軸、Y_u軸；方位環A系與光電轉臺的方位環固聯，相對基座B系，只能繞Z_b軸旋轉，產生光電轉臺方位角θ_a；高低環F系與高低環固聯，Y_f軸沿高低環軸與方位環A系Y_a軸同向；高低環F系，為方位環A系繞Y_a軸旋轉高低角θ_f而得到；橫滾環R系與橫滾環固聯，X_r軸沿橫滾環軸并與高低環X_f軸同向，橫滾環R系為高低環F系，繞X_f軸旋轉橫滾角θ_r得到；

建立像平面M系坐標，像平面M系坐標原點O_m為像主點，Z_m軸與光軸平行且指向目標，當光電轉臺三個姿態角均為零時，像平面M系Y_m軸垂直飛行方向向右，X_m軸垂直于Y_m軸向上，像平面M系的Z坐標為焦距f；

建立攝像機C系坐標，攝像機C系坐標原點O_c位于攝像機光心即像主點處，X_c軸平行于像平面M系的X_m軸，Y_c軸平行于像平面M系的Y_m軸，Z_c軸平行于像平面M系的X_m軸；

建立地面站K系、無人機K系坐標，地面站K系、無人機K系坐標原點為分別為地面站和無人機質心，X_k軸向東，Y_k軸向北，Z_k與重力反向；

令被跟蹤目標點在地面站K系坐標為[x_t y_t z_t]^T，無人機瞬時位置在地面站K系坐標為[x_u y_u z_u]^T，令在地面站K系中無人機瞬時位置至被跟蹤目標的連線為跟蹤方向線，功能模型對目標跟蹤的實時姿態角(α，β)，其中α、β分別為模型方位角和模型高低角，模型視軸線與地面的交點即地主點O_c的攝像機C系坐標[0 0 L]^T，其中L為視軸線斜距；

最后進行非線性方向線跟蹤解算，包括解算跟蹤方向線方向余弦、解算模型視軸線的無人機N系方向余弦和DFP解算模型姿態角三個步驟，具體過程如下：

(1)、解算跟蹤方向線方向余弦：

包括跟蹤方向線無人機K系坐標解算、跟蹤方向線方向余弦解算和跟蹤方向線坐標變換三個計算步驟；

計算跟蹤方向線在無人機K系中的坐標分量如式(1)所示：

則跟蹤方向線的方向余弦無人機K系坐標分量表達式如式(2)所示：

將跟蹤方向線的方向余弦由無人機K系變換為無人機N系，變換式如式(3)所示：

上式中，為跟蹤方向線的無人機N系方向余弦，其分量分別為r_nx，r_ny，r_nz，M_en為無人機K系至無人機N系的旋轉變換矩陣，其表達式如式(4)所示：

(2)、解算模型視軸線的無人機N系方向余弦：

基于坐標變換原理，地主點O_c由攝像機C系至無人機N系的坐標變換過程如式(5)所示：

其中，和分別為無人機N系至航跡S系、航跡S系至無人機U系、基座B系至方位環A系、方位環A系至高低環F系的旋轉變換矩陣，分別對應于無人機的航向角φ_hx、包括偏航角俯仰角和滾轉角的無人機姿態角、包括方位角θ_fw、高低角θ_gd的光電轉臺姿態角，其表達式如式(5.1)～(5.4)所示：

其中，M_x、M_y、M_z分別為繞X軸、Y軸和Z軸的基本旋轉矩陣，

則模型視軸線的無人機N系的方位余弦表達式如式(6)所示：

其中，上式中的x_n、y_n和z_n表達式如式(6.1)、(6.2)、(6.3)所示：

則即是此時模型視軸線的無人機N系方向余弦；

(3)、DFP解算模型姿態角：

令模型視軸線始終與跟蹤方向線重合，實現了功能模型視角對地面感興趣目標的跟蹤，即確保模型視軸線方向余弦與跟蹤方向線方向余弦實時相等條件下，已知模型視軸線方向余弦反向解算模型方位角和模型高低角，這就是非線性方向線跟蹤算法的基本思路；

在三維視景仿真或光電轉臺理論驗證時，無人機的航向角、姿態角、無人機位置和地面目標位置均為已知量，可計算出跟蹤方向線的方向余弦，并將其賦予模型視軸線方向線，解算模型方位角和模型高低角為模型視軸線解算逆向過程，為典型的非線性解算過程，很難直接用解析表達式計算，需要使用數值迭代法求解，數值迭代解算的代價函數J(·)表達式如式(7)所示：

使用擬牛頓數值迭代算法來解算式(7)中的模型姿態角，具體使用DFP算法來實現，為了達到快速收斂提高算法的效率目的，模型姿態角DFP迭代算法需要合適的初值，因此將前一時刻的模型姿態角做為當前時刻的迭代初值，這是基于擾動的影響使正確解不會偏離前一時刻太遠的緣故，再以當前時刻的跟蹤方向線的方向余弦賦予當前模型視軸線方向余弦以當前模型姿態角為迭代初始值，當前無人機姿態角、無人機航向角為已知參量，使用擬牛頓數值迭代算法迭代解算模型姿態角即模型方位角和模型高低角。