本發(fā)明涉及無人機(jī)平臺(tái)仿真算法領(lǐng)域，具體是一種無人機(jī)載光電穩(wěn)定轉(zhuǎn)臺(tái)跟蹤態(tài)建模仿真方法。

背景技術(shù)：

無人機(jī)載晝夜光電穩(wěn)定轉(zhuǎn)臺(tái)(簡(jiǎn)稱光電轉(zhuǎn)臺(tái))為可見光攝像機(jī)、前視紅外儀、激光測(cè)距/指示器、光學(xué)穩(wěn)定瞄準(zhǔn)裝備的承載機(jī)構(gòu)，具有隔離無人機(jī)角運(yùn)動(dòng)對(duì)承載設(shè)備影響、自適應(yīng)實(shí)現(xiàn)姿態(tài)控制功能的高技術(shù)精密裝備。無人機(jī)部隊(duì)官兵在裝備飛行訓(xùn)練過程中發(fā)現(xiàn)，光電轉(zhuǎn)臺(tái)處于跟蹤態(tài)時(shí)對(duì)地面目標(biāo)的跟蹤操作過程，由于天氣能見度、無人機(jī)較高航高等因素的影響，操作過程非常復(fù)雜，操作手的技術(shù)熟練度要求高，對(duì)實(shí)裝飛行過程的依賴度較高。由于無人機(jī)飛行空域較寬，空域協(xié)調(diào)較為復(fù)雜，無人機(jī)部隊(duì)開展實(shí)裝飛行的條件受到諸多因素的限制，而且，無人機(jī)裝備的使用壽命有限，不允許頻繁地開展實(shí)裝飛行訓(xùn)練，因此，迫切需要基于三維視景仿真技術(shù)的替代訓(xùn)練設(shè)備，來解決無人機(jī)部隊(duì)視頻偵察目標(biāo)跟蹤的訓(xùn)練難題。國內(nèi)外并沒有針對(duì)無人機(jī)載光電轉(zhuǎn)臺(tái)硬件設(shè)備三維視景應(yīng)用的全數(shù)字仿真建模研究。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種無人機(jī)載光電穩(wěn)定轉(zhuǎn)臺(tái)跟蹤態(tài)建模仿真方法，以解決現(xiàn)有技術(shù)軟件仿真條件下的無人機(jī)裝備訓(xùn)練的高逼真度仿真難以實(shí)現(xiàn)的問題。

為了達(dá)到上述目的，本發(fā)明所采用的技術(shù)方案為：

無人機(jī)載光電穩(wěn)定轉(zhuǎn)臺(tái)跟蹤態(tài)建模仿真方法，其特征在于：

首先將無人機(jī)載光電穩(wěn)定轉(zhuǎn)臺(tái)從功能上簡(jiǎn)化為由內(nèi)環(huán)組件、外環(huán)組件和支座組成的二軸二框架結(jié)構(gòu)，其中內(nèi)環(huán)組件安裝視點(diǎn)，內(nèi)環(huán)組件通過旋轉(zhuǎn)軸安裝在外環(huán)組件上，可隨外環(huán)組件一起繞外環(huán)軸做方位旋轉(zhuǎn)，內(nèi)環(huán)組件繞旋轉(zhuǎn)軸相對(duì)外環(huán)轉(zhuǎn)動(dòng)，其轉(zhuǎn)動(dòng)角記為模型高低角，外環(huán)組件的旋轉(zhuǎn)軸固定在支座上，可繞外環(huán)軸相對(duì)于支座做方位旋轉(zhuǎn)，其轉(zhuǎn)動(dòng)角記為模型方位角，支座為安裝固定裝置，可固定于無人機(jī)等運(yùn)動(dòng)或靜止平臺(tái)上；

接著建立無人機(jī)N系坐標(biāo)，其坐標(biāo)原點(diǎn)設(shè)定在無人機(jī)的質(zhì)心上，X_n軸指向地理北，Z_n軸為重力方向，Y_n軸為向東；

建立航跡坐S系坐標(biāo)，航跡坐S系坐標(biāo)原點(diǎn)在無人機(jī)質(zhì)心處，X_s軸與無人機(jī)規(guī)劃航向同向，Z_s軸為重力方向，Y_s軸與X_s軸和Z_s軸為右手定則關(guān)系；

建立無人機(jī)U系坐標(biāo)，無人機(jī)U系與無人機(jī)機(jī)體固聯(lián)，其坐標(biāo)原點(diǎn)O_u為無人機(jī)的質(zhì)心，X_u軸為無人機(jī)的機(jī)頭方向，Z_u軸垂直于機(jī)身平面并指向下方，Y_u軸與X_u軸和Z_u軸構(gòu)成右手定則關(guān)系；無人機(jī)偏航角為無人機(jī)U系X_u軸投影在無人機(jī)N系X_nO_nY_n平面上的方位角；無人機(jī)俯仰角為無人機(jī)U系的X_uO_uY_u平面與無人機(jī)N系的X_nO_nY_n平面的夾角，向上為正；無人機(jī)滾轉(zhuǎn)角為無人機(jī)U系的X_uO_uZ_u平面與無人機(jī)N系的X_nO_nZ_n平面的夾角，右轉(zhuǎn)為正；

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

建立像平面M系坐標(biāo)，像平面M系坐標(biāo)原點(diǎn)O_m為像主點(diǎn)，Z_m軸與光軸平行且指向目標(biāo)，當(dāng)光電轉(zhuǎn)臺(tái)三個(gè)姿態(tài)角均為零時(shí)，像平面M系Y_m軸垂直飛行方向向右，X_m軸垂直于Y_m軸向上，像平面M系的Z坐標(biāo)為焦距f；

建立攝像機(jī)C系坐標(biāo)，攝像機(jī)C系坐標(biāo)原點(diǎn)O_c位于攝像機(jī)光心即像主點(diǎn)處，X_c軸平行于像平面M系的X_m軸，Y_c軸平行于像平面M系的Y_m軸，Z_c軸平行于像平面M系的X_m軸；

建立地面站K系、無人機(jī)K系坐標(biāo)，地面站K系、無人機(jī)K系坐標(biāo)原點(diǎn)為分別為地面站和無人機(jī)質(zhì)心，X_k軸向東，Y_k輛向北，Z_k與重力反向；

令被跟蹤目標(biāo)點(diǎn)在地面站K系坐標(biāo)為T:[x_t y_t z_t]^T，無人機(jī)瞬時(shí)位置在地面站K系坐標(biāo)為U:[x_u y_u z_u]^T，令在地面站K系中無人機(jī)瞬時(shí)位置至被跟蹤目標(biāo)至的連線為跟蹤方向線，功能模型對(duì)目標(biāo)跟蹤的實(shí)時(shí)姿態(tài)角(α,β)，其中α、β分別為模型方位角和模型高低角，模型視軸線與地面的交點(diǎn)即地主點(diǎn)O的攝像機(jī)C系坐標(biāo)O_c:[0 0 L]^T，其中L為視軸線斜距；

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

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

包括跟蹤方向線無人機(jī)K系坐標(biāo)解算、跟蹤方向線方向余弦解算和跟蹤方向線坐標(biāo)變換等三個(gè)計(jì)算步驟；

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

則跟蹤方向線的方向余弦r_k:[r_kx r_ky r_kz]^T無人機(jī)K系坐標(biāo)分量表達(dá)式如式(2)所示：

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

為跟蹤方向線的無人機(jī)N系方向余弦，M_en為無人機(jī)K系至無人機(jī)N系的旋轉(zhuǎn)變換矩陣，其表達(dá)式如式(4)所示：

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

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

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

使用擬牛頓數(shù)值迭代算法來解算式(7)中的模型姿態(tài)角，具體使用DFP算法來實(shí)現(xiàn)，為了達(dá)到快速收斂提高算法的效率目的，模型姿態(tài)角DFP迭代算法需要合適的初值，因此將前一時(shí)刻的模型姿態(tài)角做為當(dāng)前時(shí)刻的迭代初值，這是基于擾動(dòng)的影響使正確解不會(huì)偏離前一時(shí)刻太遠(yuǎn)的緣故，再以當(dāng)前時(shí)刻的跟蹤方向線的方向余弦[r_nx r_ny r_nz]^T賦予當(dāng)前模型視軸線方向余弦，以當(dāng)前模型姿態(tài)角為迭代初始值，當(dāng)前無人機(jī)姿態(tài)角、無人機(jī)航向角為已知參量，使用DFP擬牛頓法迭代解算模型姿態(tài)角即模型方位角和模型高低角。

與已有技術(shù)相比，本發(fā)明的有益效果體現(xiàn)在：

光電轉(zhuǎn)臺(tái)跟蹤態(tài)是無人機(jī)執(zhí)行作戰(zhàn)任務(wù)時(shí)的重要工作模式，主要用于完成承載的攝像機(jī)/紅外儀/激光指示器/光學(xué)瞄準(zhǔn)器視軸線對(duì)地面目標(biāo)的視覺跟蹤任務(wù)。本發(fā)明著重解決光電轉(zhuǎn)臺(tái)對(duì)地面目標(biāo)視覺跟蹤的數(shù)字功能建模問題，力求不采購光電轉(zhuǎn)臺(tái)的實(shí)物裝備，僅使用計(jì)算機(jī)軟件仿真實(shí)現(xiàn)光電轉(zhuǎn)臺(tái)硬件對(duì)地面目標(biāo)的視頻/圖像/光學(xué)跟蹤任務(wù)。發(fā)明擬要解決的技術(shù)問題包括實(shí)裝光電轉(zhuǎn)臺(tái)操作層面功能抽象化建模和跟蹤態(tài)仿真算法設(shè)計(jì)，完成對(duì)光電轉(zhuǎn)臺(tái)跟蹤態(tài)功能的高逼真度仿真實(shí)現(xiàn)。光電轉(zhuǎn)臺(tái)操作層面功能抽象化建模，著重從光電轉(zhuǎn)臺(tái)任務(wù)執(zhí)行功能層面進(jìn)行抽象化構(gòu)模，設(shè)計(jì)實(shí)現(xiàn)同實(shí)裝完全一致的操作自由度、承載設(shè)備安裝位置(攝像機(jī)/前視紅外儀/激光指示器/光學(xué)瞄準(zhǔn)裝置的視角位置)、設(shè)備響應(yīng)參數(shù)、安裝位置等等。光電轉(zhuǎn)臺(tái)跟蹤態(tài)軟件仿真算法建模，是本發(fā)明的核心內(nèi)容，擬使用智能計(jì)算方法和數(shù)字建模算法，解決對(duì)光電轉(zhuǎn)臺(tái)硬件跟蹤態(tài)的功能仿真，力爭(zhēng)在三維仿真訓(xùn)練軟硬件環(huán)境下，用以替代光電轉(zhuǎn)臺(tái)硬件對(duì)地面目標(biāo)的視頻/圖像/光學(xué)跟蹤難題，解決軟件仿真條件下的無人機(jī)裝備訓(xùn)練的高逼真度仿真實(shí)現(xiàn)問題。

本發(fā)明工作原理：將實(shí)裝光電轉(zhuǎn)臺(tái)復(fù)雜的機(jī)械伺服結(jié)構(gòu)、陀螺儀穩(wěn)定回路和圖像控制儀自適應(yīng)控制回路，簡(jiǎn)化抽象為由視點(diǎn)(視軸線)、內(nèi)環(huán)、外環(huán)組成的二自由度功能模型，通過將模型視軸線適時(shí)跟隨跟蹤方向法，使用擬牛頓數(shù)據(jù)值迭代算法來實(shí)時(shí)解算功能模型的方位角和高低角，實(shí)現(xiàn)非線性方向線跟蹤算法的設(shè)計(jì)，解決了在三維視景仿真模擬訓(xùn)練和新型光電轉(zhuǎn)臺(tái)理論驗(yàn)證中的光電轉(zhuǎn)臺(tái)跟蹤態(tài)的數(shù)字建模仿真關(guān)鍵技術(shù)問題。

本發(fā)明構(gòu)造了光電轉(zhuǎn)臺(tái)二自由度功能模型，設(shè)計(jì)實(shí)現(xiàn)了非線性方向線跟蹤算法，很好地解決了三維視景仿真測(cè)試光電轉(zhuǎn)臺(tái)的功能建模和跟蹤算法建模難題，具有很好的工程可用性。

本發(fā)明很好了解決三維仿真環(huán)下對(duì)光電轉(zhuǎn)臺(tái)跟蹤態(tài)的數(shù)字建模仿真問題。軟件仿真測(cè)試表明，軟件仿真收斂速度快、計(jì)算精度高。將模型和算法加載到三維仿真場(chǎng)景中測(cè)試后，模型能夠很好地實(shí)現(xiàn)對(duì)視點(diǎn)的控制，與實(shí)裝光電轉(zhuǎn)臺(tái)的效果逼真度高，當(dāng)控制對(duì)地面目標(biāo)的跟蹤時(shí)，無論無人機(jī)做何種方式機(jī)動(dòng)時(shí)，模型搭載的視場(chǎng)中心始終對(duì)準(zhǔn)被跟蹤目標(biāo)，計(jì)算機(jī)資源占用率不高，三維畫面流暢穩(wěn)定。

附圖說明

圖1為抽象化功能模型示意。

圖2為無人機(jī)N系坐標(biāo)圖。

圖3為航跡S系坐標(biāo)圖。

圖4為無人機(jī)U系坐標(biāo)圖。

圖5為光電轉(zhuǎn)臺(tái)坐標(biāo)系圖。

圖6為像平面M系與攝像機(jī)C系坐標(biāo)圖。

圖7為非線性方向線跟蹤算法流程框圖。

圖8為解算跟蹤方向線方向余弦流程框圖。

圖9為攝像機(jī)C系至無人機(jī)N系坐標(biāo)變換過程框圖。

圖10為DFP解算模型姿態(tài)角流程框圖。

圖11為模型方位角變化曲線圖。

圖12為模型態(tài)高低角變化曲線圖。

圖13為某目標(biāo)跟蹤截圖一。

圖14為某目標(biāo)跟蹤截圖二。

圖15為某目標(biāo)跟蹤截圖三。

圖16為某目標(biāo)跟蹤截圖四。

圖17為某目標(biāo)跟蹤截圖五。

具體實(shí)施方式

本發(fā)明主要包括兩部分內(nèi)容，其一是光電轉(zhuǎn)臺(tái)功能化構(gòu)模，其二是光電轉(zhuǎn)臺(tái)非線性方向線跟蹤算法實(shí)現(xiàn)。

一、光電轉(zhuǎn)臺(tái)抽象化功能構(gòu)模：

實(shí)裝光電轉(zhuǎn)臺(tái)為兩軸四框架硬件由機(jī)械伺服結(jié)構(gòu)、陀螺儀穩(wěn)定結(jié)構(gòu)和圖像控制儀自適應(yīng)控制結(jié)構(gòu)組成，本發(fā)明從功能上將其簡(jiǎn)化為二軸二框架結(jié)構(gòu)(簡(jiǎn)記為功能模型)，如圖1所示，由內(nèi)環(huán)組件、外環(huán)組件和支座組成。內(nèi)環(huán)組件安裝視點(diǎn)(包括攝像機(jī)/前視紅外儀/激光指示/瞄準(zhǔn)器/光學(xué)瞄準(zhǔn)裝置，視點(diǎn)的虛擬光軸線簡(jiǎn)記為模型視軸線)，內(nèi)環(huán)組件旋轉(zhuǎn)軸安裝在外環(huán)組件上，可隨外環(huán)組件一起繞外環(huán)軸做方位旋轉(zhuǎn)，內(nèi)環(huán)組件繞旋轉(zhuǎn)軸相對(duì)外環(huán)轉(zhuǎn)動(dòng)，其轉(zhuǎn)動(dòng)角為高低角(簡(jiǎn)記為模型高低角)。外環(huán)組件的旋轉(zhuǎn)軸固定在支座上，可繞外環(huán)軸相對(duì)于支座做方位旋轉(zhuǎn)，其轉(zhuǎn)動(dòng)角為方位角(簡(jiǎn)記為模型方位角)。支座為安裝固定裝置，可固定于無人機(jī)等運(yùn)動(dòng)或靜止平臺(tái)上。

二、非線性方向線跟蹤算法設(shè)計(jì)

算法原理

光電轉(zhuǎn)臺(tái)跟蹤態(tài)，主要用于控制搭載的攝像機(jī)/紅外儀/激光指示器/光學(xué)瞄準(zhǔn)裝置對(duì)地面目標(biāo)進(jìn)行視頻/圖像/光學(xué)跟蹤，主要利用圖像匹配算法、陀螺儀測(cè)量穩(wěn)定回路、圖像控制儀自適應(yīng)控制回路，控制實(shí)現(xiàn)攝像機(jī)/紅外儀/激光指示器/光學(xué)瞄準(zhǔn)裝置視軸線持續(xù)對(duì)準(zhǔn)地面目標(biāo)。

非線性方向線跟蹤算法為本發(fā)明的核心內(nèi)容，主要使用智能計(jì)算方法和數(shù)字建模算法來仿真光電轉(zhuǎn)臺(tái)硬件工作時(shí)對(duì)地面目標(biāo)的視頻/圖像/光學(xué)跟蹤控制過程，即保持光電轉(zhuǎn)臺(tái)搭載的攝像機(jī)/前視紅外儀/激光指示器/光學(xué)瞄準(zhǔn)裝置的視軸線持續(xù)對(duì)準(zhǔn)地面感興趣目標(biāo)的過程，主要包括構(gòu)建實(shí)裝光電轉(zhuǎn)臺(tái)在跟蹤態(tài)時(shí)其陀螺儀穩(wěn)定機(jī)構(gòu)、機(jī)械伺服機(jī)構(gòu)和圖像控制儀的自適應(yīng)控制過程的功能仿真模型和算法。

具體來說，利用非線性方向線跟蹤算法解算的方位角和高低角，控制本發(fā)明提出的功能模型，以實(shí)現(xiàn)對(duì)模型視點(diǎn)視軸線的實(shí)時(shí)控制。非線性方向線跟蹤算法基本原理是通過設(shè)定飛機(jī)/目標(biāo)的指向線(簡(jiǎn)稱跟蹤方向線)，使模型視軸線始終與跟蹤方向線保持一致，并通過擬牛頓數(shù)值迭代算法來非線性解算模型方位角和模型高低角，從而實(shí)現(xiàn)對(duì)功能模型視角的自適用控制。

算法流程

如圖2所示，無人機(jī)N系，其坐標(biāo)原點(diǎn)設(shè)定在無人機(jī)的質(zhì)心上，X_n軸指向地理北，Z_n軸為重力方向，Y_n軸為向東。

如圖3所示，航跡坐S系坐標(biāo)原點(diǎn)在無人機(jī)質(zhì)心處，X_s軸與無人機(jī)規(guī)劃航向同向，Z_s軸為重力方向，Y_s軸與X_s軸和Z_s軸為右手定則關(guān)系。

如圖4所示，無人機(jī)U系與無人機(jī)機(jī)體固聯(lián)，其坐標(biāo)原點(diǎn)O_u為無人機(jī)的質(zhì)心，X_u軸為無人機(jī)的機(jī)頭方向，Z_u軸垂直于機(jī)身平面并指向下方，Y_u軸與X_u軸和Z_u軸構(gòu)成右手定則關(guān)系；無人機(jī)偏航角為無人機(jī)U系X_u軸投影在無人機(jī)N系X_nO_nY_n平面上的方位角；無人機(jī)俯仰角為無人機(jī)U系的X_uO_uY_u平面與無人機(jī)N系的X_nO_nY_n平面的夾角，向上為正；無人機(jī)滾轉(zhuǎn)角為無人機(jī)U系的X_uO_uZ_u平面與無人機(jī)N系的X_nO_nZ_n平面的夾角，右轉(zhuǎn)為正；

光電轉(zhuǎn)臺(tái)坐標(biāo)系包括基座坐標(biāo)系、方位環(huán)A系、高低環(huán)F系和橫滾環(huán)R系。如圖5所示，基座B系，坐標(biāo)原點(diǎn)O_b為轉(zhuǎn)軸中心，Z_b軸由鏡頭指向目標(biāo)，X_b軸、Y_b軸分別平行于無人機(jī)U系的X_u軸、Y_u軸。方位環(huán)A系與光電轉(zhuǎn)臺(tái)的方位環(huán)固聯(lián)，相對(duì)基座B系，只能繞Z_b軸旋轉(zhuǎn)，產(chǎn)生光電轉(zhuǎn)臺(tái)方位角θ_a。高低環(huán)F系與高低環(huán)固聯(lián)，Y_f軸沿高低環(huán)軸與方位環(huán)A系Y_a軸同向。高低環(huán)F系，為方位環(huán)A系繞Y_a軸旋轉(zhuǎn)高低角θ_f而得到。橫滾環(huán)R系與橫滾環(huán)固聯(lián)，X_r軸沿橫滾環(huán)軸并與高低環(huán)X_f軸同向，橫滾環(huán)R系，為高低F系，繞X_f軸旋轉(zhuǎn)橫滾角θ_r得到。

如圖6所示，像平面M系，坐標(biāo)原點(diǎn)O_m為像主點(diǎn)，Z_m軸與光軸平行且指向目標(biāo)，當(dāng)光電轉(zhuǎn)臺(tái)三個(gè)姿態(tài)角均為零時(shí)，像平面M系Y_m軸垂直飛行方向向右，X_m軸垂直于Y_m軸向上。像平面M系的Z坐標(biāo)為焦距f。

如圖6所示，攝像機(jī)C系坐標(biāo)原點(diǎn)O_c位于攝像機(jī)光心(即像主點(diǎn)處)，X_c軸平行于像平面M系的X_m軸，Y_c軸平行于像平面M系的Y_m軸，Z_c軸平行于像平面M系的X_m軸。

地面站K系、無人機(jī)K系，坐標(biāo)原點(diǎn)為分別為地面站和無人機(jī)質(zhì)心，X_k軸向東，Y_k軸向北，Z_k與重力反向。

令被跟蹤目標(biāo)點(diǎn)坐標(biāo)為T:[x_t y_t z_t]^T(地面站K系)，無人機(jī)瞬時(shí)位置為U:[x_u y_uz_u]^T(地面站K系)，令在地面站K系中無人機(jī)瞬時(shí)位置至被跟蹤目標(biāo)至的連線為跟蹤方向線，功能模型對(duì)目標(biāo)跟蹤的實(shí)時(shí)姿態(tài)角(α,β)(α、β分別為模型方位角和模型高低角)，模型視軸線與地面的交點(diǎn)(地主點(diǎn)O)的攝像機(jī)C系坐標(biāo)O_c:[0 0 L]^T(L為視軸線斜距)。

本發(fā)明提出的非線性方向線跟蹤算法，計(jì)算流程如圖7所示，主要包括解算跟蹤方向線方向余弦、解算模型視軸線的無人機(jī)N系方向余弦和DFP解算模型姿態(tài)角三個(gè)步驟。

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

解算流程見圖8，主要包括跟蹤方向線無人機(jī)K系坐標(biāo)解算、跟蹤方向線方向余弦解算和跟蹤方向線坐標(biāo)變換等三個(gè)計(jì)算步驟。

計(jì)算跟蹤方向線在無人機(jī)K系中的坐標(biāo)分量。

則跟蹤方向線的方向余弦r_k:[r_kx r_ky r_kz]^T無人機(jī)K系坐標(biāo)分量表達(dá)式如下：

將跟蹤方向線的方向余弦r_k由無人機(jī)K系變換為無人機(jī)N系，變換式如下：

為跟蹤方向線的無人機(jī)N系方向余弦。M_en為無人機(jī)K系至無人機(jī)N系的旋轉(zhuǎn)變換矩陣，其表達(dá)式如下：

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

基于坐標(biāo)變換原理(變換過程如圖9所示)，地主點(diǎn)O由攝像機(jī)C系至無人機(jī)N系的坐標(biāo)變換過程如下。

其中，和分別為無人機(jī)N系至航跡S系、航跡S系至無人機(jī)U系、基座B系至方位環(huán)A系、方位環(huán)A系至高低環(huán)F系的旋轉(zhuǎn)變換矩陣，分別對(duì)應(yīng)于無人機(jī)的航向角φ_hx、無人機(jī)姿態(tài)角(偏航角俯仰角和滾轉(zhuǎn)角)、光電轉(zhuǎn)臺(tái)姿態(tài)角(方位角θ_fw、高低角θ_gd)等輸入角參量，其表達(dá)式如式(5.1)～(5.4)所示。

其中，M_x、M_y、M_z分別繞X軸、Y軸和Z軸的基本旋轉(zhuǎn)矩陣，

則模型視軸線的無人機(jī)N系的方位余弦表達(dá)式如下。

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

則即是此時(shí)模型視軸線的無人機(jī)N系方向余弦。

(3)DFP解算模型姿態(tài)角

令模型視軸線始終與跟蹤方向線重合就實(shí)現(xiàn)了功能模型視角對(duì)地面感興趣目標(biāo)的跟蹤，即確保模型視軸線方向余弦與跟蹤方向線方向余弦實(shí)時(shí)相等條件下，已知模型視軸線方向余弦反向解算模型方位角和模型高低角，這就是非線性方向線跟蹤算法的基本思路。解算流程如圖10所示。

在三維視景仿真或新型光電轉(zhuǎn)臺(tái)理論驗(yàn)證時(shí)，無人機(jī)的航向角、姿態(tài)角、無人機(jī)位置和地面目標(biāo)位置均為已知量，可計(jì)算出跟蹤方向線的方向余弦，并將其賦予模型視軸線方向線，根據(jù)4.2節(jié)模型視軸線解算過程可知，解算模型方位角和模型高低角為模型視軸線解算逆向過程，為典型的非線性解算過程，很難直接用解析表達(dá)式計(jì)算，需要使用數(shù)值迭代法求解，數(shù)值迭代解算的代價(jià)函數(shù)J(·)表達(dá)式如下：

本發(fā)明使用擬牛頓數(shù)值迭代算法來解算式(7)中的模型姿態(tài)角，具體使用DFP算法來實(shí)現(xiàn)。為了達(dá)到快速收斂提高算法的效率目的，模型姿態(tài)角DFP迭代算法需要合適的初值，本發(fā)明將前一時(shí)刻的模型姿態(tài)角做為當(dāng)前時(shí)刻的迭代初值，這是基于擾動(dòng)的影響使正確解不會(huì)偏離前一時(shí)刻太遠(yuǎn)的緣故，再以當(dāng)前時(shí)刻的跟蹤方向線的方向余弦[r_nx r_ny r_nz]^T賦予當(dāng)前模型視軸線方向余弦，以當(dāng)前模型姿態(tài)角為迭代初始值，當(dāng)前無人機(jī)姿態(tài)角、無人機(jī)航向角為已知參量，使用DFP擬牛頓法迭代解算模型姿態(tài)角(模型方位角和模型高低角)。

為驗(yàn)證本專利提出的光電轉(zhuǎn)臺(tái)跟蹤態(tài)仿真建模技術(shù)的正確性，仿真了飛機(jī)繞地面某點(diǎn)為中心做大圓圈飛行(飛機(jī)航向角做大幅度變化)，模型視點(diǎn)跟蹤大圓圈的中心點(diǎn)，利用非線性方向線跟蹤算法，來解算模型姿態(tài)角的實(shí)時(shí)變化，即模型方位角和模型高低角調(diào)整變化過程，來驗(yàn)證模型本專利提出的功能模型和跟蹤算法的正確性。

測(cè)試具體參數(shù)如下：功能模型初始姿態(tài)角度[0°,0°]^T，模擬無人機(jī)初始姿態(tài)角[0°,0°,0°]^T，模擬無人機(jī)初始航向?yàn)檎?，飛行高度為1000米，航速為190公里/小時(shí)，圓周飛行大圈半徑為1.5公里。非線性方向線跟蹤算法解算的模型方位角和模型高低角變化曲線如圖11和圖12所示。

仿真結(jié)果顯示，當(dāng)飛機(jī)做大圓周運(yùn)動(dòng)，并設(shè)定圓周的中心為視頻跟蹤目標(biāo)時(shí)，由于無人機(jī)航向角做連續(xù)勻速率(無人機(jī)勻速率轉(zhuǎn)彎運(yùn)動(dòng))變化，為了實(shí)現(xiàn)對(duì)目標(biāo)的有效跟蹤，非線性方向線跟蹤算法實(shí)時(shí)調(diào)整了功能模型的姿態(tài)，模型方位角呈線性變化，補(bǔ)償了無人機(jī)航向角勻速率變化，模型高低角呈非線性變化；圖中所示的斷點(diǎn)處為模型方位等角/模型高低角在0度或360度處的數(shù)值跳變，并不是真正意義的斷點(diǎn)?？梢钥闯?，本發(fā)明提出的功能模型和非線性方向線跟蹤算法，在理論上是完全正確的。

將能模型加載到三維仿真場(chǎng)景，并把非線性方向線跟蹤算法軟件工程化后用于控制該三維場(chǎng)景視角，軟件工程化測(cè)試結(jié)果截圖如圖13～圖17所示，完全實(shí)現(xiàn)了對(duì)場(chǎng)景中給定某目標(biāo)的視頻跟蹤仿真，無論無人機(jī)做何種運(yùn)動(dòng)方式，也無論運(yùn)動(dòng)到任何地點(diǎn)，只要視場(chǎng)可見目標(biāo)，視場(chǎng)中心始終對(duì)準(zhǔn)被跟蹤目標(biāo)，計(jì)算機(jī)資源占用率不高，三維畫面流暢穩(wěn)定，不僅說明技術(shù)理論正確，而且還證明了該技術(shù)具有較好的工程可用性。