CN106777489A - 无人机载光电稳定转台跟踪态建模仿真方法 - Google Patents

Abstract

本发明公开了一种无人机载光电稳定转台跟踪态建模仿真方法，基于数字建模算法仿真实现光电转台跟踪态功能，用于搭载光电稳定转台无人机装备的三维视景虚拟仿真训练，或新型光电稳定转台的理论验证使用，主要包括光电转台功能化构模、光电转台非线性方向线跟踪算法实现两部分。本发明构造了光电转台二自由度功能模型，设计实现了非线性方向线跟踪算法，很好地解决了三维视景仿真测试光电转台的功能建模和跟踪算法建模难题，具有很好的工程可用性。

Description

无人机载光电稳定转台跟踪态建模仿真方法

技术领域

本发明涉及无人机平台仿真算法领域，具体是一种无人机载光电稳定转台跟踪态建模仿真方法。

背景技术

无人机载昼夜光电稳定转台(简称光电转台)为可见光摄像机、前视红外仪、激光测距/指示器、光学稳定瞄准装备的承载机构，具有隔离无人机角运动对承载设备影响、自适应实现姿态控制功能的高技术精密装备。无人机部队官兵在装备飞行训练过程中发现，光电转台处于跟踪态时对地面目标的跟踪操作过程，由于天气能见度、无人机较高航高等因素的影响，操作过程非常复杂，操作手的技术熟练度要求高，对实装飞行过程的依赖度较高。由于无人机飞行空域较宽，空域协调较为复杂，无人机部队开展实装飞行的条件受到诸多因素的限制，而且，无人机装备的使用寿命有限，不允许频繁地开展实装飞行训练，因此，迫切需要基于三维视景仿真技术的替代训练设备，来解决无人机部队视频侦察目标跟踪的训练难题。国内外并没有针对无人机载光电转台硬件设备三维视景应用的全数字仿真建模研究。

发明内容

本发明的目的是提供一种无人机载光电稳定转台跟踪态建模仿真方法，以解决现有技术软件仿真条件下的无人机装备训练的高逼真度仿真难以实现的问题。

为了达到上述目的，本发明所采用的技术方案为：

无人机载光电稳定转台跟踪态建模仿真方法，其特征在于：

首先将无人机载光电稳定转台从功能上简化为由内环组件、外环组件和支座组成的二轴二框架结构，其中内环组件安装视点，内环组件通过旋转轴安装在外环组件上，可随外环组件一起绕外环轴做方位旋转，内环组件绕旋转轴相对外环转动，其转动角记为模型高低角，外环组件的旋转轴固定在支座上，可绕外环轴相对于支座做方位旋转，其转动角记为模型方位角，支座为安装固定装置，可固定于无人机等运动或静止平台上；

接着建立无人机N系坐标，其坐标原点设定在无人机的质心上，X_n轴指向地理北，Z_n轴为重力方向，Y_n轴为向东；

建立航迹坐S系坐标，航迹坐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系坐标为T:[x_t y_t z_t]^T，无人机瞬时位置在地面站K系坐标为U:[x_u y_u z_u]^T，令在地面站K系中无人机瞬时位置至被跟踪目标至的连线为跟踪方向线，功能模型对目标跟踪的实时姿态角(α,β)，其中α、β分别为模型方位角和模型高低角，模型视轴线与地面的交点即地主点O的摄像机C系坐标O_c:[0 0 L]^T，其中L为视轴线斜距；

最后进行非线性方向线跟踪解算，包括解算跟踪方向线方向余弦、解算模型视轴线的无人机N系方向余弦和DFP解算模型姿态角三个步骤，具体过程如下：

(1)、解算跟踪方向线方向余弦：

包括跟踪方向线无人机K系坐标解算、跟踪方向线方向余弦解算和跟踪方向线坐标变换等三个计算步骤；

计算跟踪方向线在无人机K系中的坐标分量如式(1)所示：

则跟踪方向线的方向余弦r_k:[r_kx r_ky r_kz]^T无人机K系坐标分量表达式如式(2)所示：

将跟踪方向线的方向余弦r_k由无人机K系变换为无人机N系，变换式如式(3)所示：

为跟踪方向线的无人机N系方向余弦，M_en为无人机K系至无人机N系的旋转变换矩阵，其表达式如式(4)所示：

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

令模型视轴线始终与跟踪方向线重合，实现了功能模型视角对地面感兴趣目标的跟踪，即确保模型视轴线方向余弦与跟踪方向线方向余弦实时相等条件下，已知模型视轴线方向余弦反向解算模型方位角和模型高低角，这就是非线性方向线跟踪算法的基本思路；

在三维视景仿真或新型光电转台理论验证时，无人机的航向角、姿态角、无人机位置和地面目标位置均为已知量，可计算出跟踪方向线的方向余弦，并将其赋予模型视轴线方向线，解算模型方位角和模型高低角为模型视轴线解算逆向过程，为典型的非线性解算过程，很难直接用解析表达式计算，需要使用数值迭代法求解，数值迭代解算的代价函数J(·)表达式如式(7)所示：

使用拟牛顿数值迭代算法来解算式(7)中的模型姿态角，具体使用DFP算法来实现，为了达到快速收敛提高算法的效率目的，模型姿态角DFP迭代算法需要合适的初值，因此将前一时刻的模型姿态角做为当前时刻的迭代初值，这是基于扰动的影响使正确解不会偏离前一时刻太远的缘故，再以当前时刻的跟踪方向线的方向余弦[r_nx r_ny r_nz]^T赋予当前模型视轴线方向余弦，以当前模型姿态角为迭代初始值，当前无人机姿态角、无人机航向角为已知参量，使用DFP拟牛顿法迭代解算模型姿态角即模型方位角和模型高低角。

与已有技术相比，本发明的有益效果体现在：

光电转台跟踪态是无人机执行作战任务时的重要工作模式，主要用于完成承载的摄像机/红外仪/激光指示器/光学瞄准器视轴线对地面目标的视觉跟踪任务。本发明着重解决光电转台对地面目标视觉跟踪的数字功能建模问题，力求不采购光电转台的实物装备，仅使用计算机软件仿真实现光电转台硬件对地面目标的视频/图像/光学跟踪任务。发明拟要解决的技术问题包括实装光电转台操作层面功能抽象化建模和跟踪态仿真算法设计，完成对光电转台跟踪态功能的高逼真度仿真实现。光电转台操作层面功能抽象化建模，着重从光电转台任务执行功能层面进行抽象化构模，设计实现同实装完全一致的操作自由度、承载设备安装位置(摄像机/前视红外仪/激光指示器/光学瞄准装置的视角位置)、设备响应参数、安装位置等等。光电转台跟踪态软件仿真算法建模，是本发明的核心内容，拟使用智能计算方法和数字建模算法，解决对光电转台硬件跟踪态的功能仿真，力争在三维仿真训练软硬件环境下，用以替代光电转台硬件对地面目标的视频/图像/光学跟踪难题，解决软件仿真条件下的无人机装备训练的高逼真度仿真实现问题。

本发明工作原理：将实装光电转台复杂的机械伺服结构、陀螺仪稳定回路和图像控制仪自适应控制回路，简化抽象为由视点(视轴线)、内环、外环组成的二自由度功能模型，通过将模型视轴线适时跟随跟踪方向法，使用拟牛顿数据值迭代算法来实时解算功能模型的方位角和高低角，实现非线性方向线跟踪算法的设计，解决了在三维视景仿真模拟训练和新型光电转台理论验证中的光电转台跟踪态的数字建模仿真关键技术问题。

本发明构造了光电转台二自由度功能模型，设计实现了非线性方向线跟踪算法，很好地解决了三维视景仿真测试光电转台的功能建模和跟踪算法建模难题，具有很好的工程可用性。

本发明很好了解决三维仿真环下对光电转台跟踪态的数字建模仿真问题。软件仿真测试表明，软件仿真收敛速度快、计算精度高。将模型和算法加载到三维仿真场景中测试后，模型能够很好地实现对视点的控制，与实装光电转台的效果逼真度高，当控制对地面目标的跟踪时，无论无人机做何种方式机动时，模型搭载的视场中心始终对准被跟踪目标，计算机资源占用率不高，三维画面流畅稳定。

附图说明

图1为抽象化功能模型示意。

图2为无人机N系坐标图。

图3为航迹S系坐标图。

图4为无人机U系坐标图。

图5为光电转台坐标系图。

图6为像平面M系与摄像机C系坐标图。

图7为非线性方向线跟踪算法流程框图。

图8为解算跟踪方向线方向余弦流程框图。

图9为摄像机C系至无人机N系坐标变换过程框图。

图10为DFP解算模型姿态角流程框图。

图11为模型方位角变化曲线图。

图12为模型态高低角变化曲线图。

图13为某目标跟踪截图一。

图14为某目标跟踪截图二。

图15为某目标跟踪截图三。

图16为某目标跟踪截图四。

图17为某目标跟踪截图五。

具体实施方式

本发明主要包括两部分内容，其一是光电转台功能化构模，其二是光电转台非线性方向线跟踪算法实现。

一、光电转台抽象化功能构模：

实装光电转台为两轴四框架硬件由机械伺服结构、陀螺仪稳定结构和图像控制仪自适应控制结构组成，本发明从功能上将其简化为二轴二框架结构(简记为功能模型)，如图1所示，由内环组件、外环组件和支座组成。内环组件安装视点(包括摄像机/前视红外仪/激光指示/瞄准器/光学瞄准装置，视点的虚拟光轴线简记为模型视轴线)，内环组件旋转轴安装在外环组件上，可随外环组件一起绕外环轴做方位旋转，内环组件绕旋转轴相对外环转动，其转动角为高低角(简记为模型高低角)。外环组件的旋转轴固定在支座上，可绕外环轴相对于支座做方位旋转，其转动角为方位角(简记为模型方位角)。支座为安装固定装置，可固定于无人机等运动或静止平台上。

二、非线性方向线跟踪算法设计

算法原理

光电转台跟踪态，主要用于控制搭载的摄像机/红外仪/激光指示器/光学瞄准装置对地面目标进行视频/图像/光学跟踪，主要利用图像匹配算法、陀螺仪测量稳定回路、图像控制仪自适应控制回路，控制实现摄像机/红外仪/激光指示器/光学瞄准装置视轴线持续对准地面目标。

非线性方向线跟踪算法为本发明的核心内容，主要使用智能计算方法和数字建模算法来仿真光电转台硬件工作时对地面目标的视频/图像/光学跟踪控制过程，即保持光电转台搭载的摄像机/前视红外仪/激光指示器/光学瞄准装置的视轴线持续对准地面感兴趣目标的过程，主要包括构建实装光电转台在跟踪态时其陀螺仪稳定机构、机械伺服机构和图像控制仪的自适应控制过程的功能仿真模型和算法。

具体来说，利用非线性方向线跟踪算法解算的方位角和高低角，控制本发明提出的功能模型，以实现对模型视点视轴线的实时控制。非线性方向线跟踪算法基本原理是通过设定飞机/目标的指向线(简称跟踪方向线)，使模型视轴线始终与跟踪方向线保持一致，并通过拟牛顿数值迭代算法来非线性解算模型方位角和模型高低角，从而实现对功能模型视角的自适用控制。

算法流程

如图2所示，无人机N系，其坐标原点设定在无人机的质心上，X_n轴指向地理北，Z_n轴为重力方向，Y_n轴为向东。

如图3所示，航迹坐S系坐标原点在无人机质心处，X_s轴与无人机规划航向同向，Z_s轴为重力方向，Y_s轴与X_s轴和Z_s轴为右手定则关系。

如图4所示，无人机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系。如图5所示，基座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得到。

如图6所示，像平面M系，坐标原点O_m为像主点，Z_m轴与光轴平行且指向目标，当光电转台三个姿态角均为零时，像平面M系Y_m轴垂直飞行方向向右，X_m轴垂直于Y_m轴向上。像平面M系的Z坐标为焦距f。

如图6所示，摄像机C系坐标原点O_c位于摄像机光心(即像主点处)，X_c轴平行于像平面M系的X_m轴，Y_c轴平行于像平面M系的Y_m轴，Z_c轴平行于像平面M系的X_m轴。

地面站K系、无人机K系，坐标原点为分别为地面站和无人机质心，X_k轴向东，Y_k轴向北，Z_k与重力反向。

令被跟踪目标点坐标为T:[x_t y_t z_t]^T(地面站K系)，无人机瞬时位置为U:[x_u y_uz_u]^T(地面站K系)，令在地面站K系中无人机瞬时位置至被跟踪目标至的连线为跟踪方向线，功能模型对目标跟踪的实时姿态角(α,β)(α、β分别为模型方位角和模型高低角)，模型视轴线与地面的交点(地主点O)的摄像机C系坐标O_c:[0 0 L]^T(L为视轴线斜距)。

本发明提出的非线性方向线跟踪算法，计算流程如图7所示，主要包括解算跟踪方向线方向余弦、解算模型视轴线的无人机N系方向余弦和DFP解算模型姿态角三个步骤。

(1)解算跟踪方向线方向余弦

解算流程见图8，主要包括跟踪方向线无人机K系坐标解算、跟踪方向线方向余弦解算和跟踪方向线坐标变换等三个计算步骤。

计算跟踪方向线在无人机K系中的坐标分量。

则跟踪方向线的方向余弦r_k:[r_kx r_ky r_kz]^T无人机K系坐标分量表达式如下：

将跟踪方向线的方向余弦r_k由无人机K系变换为无人机N系，变换式如下：

为跟踪方向线的无人机N系方向余弦。M_en为无人机K系至无人机N系的旋转变换矩阵，其表达式如下：

(2)解算模型视轴线的无人机N系方向余弦

基于坐标变换原理(变换过程如图9所示)，地主点O由摄像机C系至无人机N系的坐标变换过程如下。

其中，和分别为无人机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系的方位余弦表达式如下。

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

则即是此时模型视轴线的无人机N系方向余弦。

(3)DFP解算模型姿态角

令模型视轴线始终与跟踪方向线重合就实现了功能模型视角对地面感兴趣目标的跟踪，即确保模型视轴线方向余弦与跟踪方向线方向余弦实时相等条件下，已知模型视轴线方向余弦反向解算模型方位角和模型高低角，这就是非线性方向线跟踪算法的基本思路。解算流程如图10所示。

在三维视景仿真或新型光电转台理论验证时，无人机的航向角、姿态角、无人机位置和地面目标位置均为已知量，可计算出跟踪方向线的方向余弦，并将其赋予模型视轴线方向线，根据4.2节模型视轴线解算过程可知，解算模型方位角和模型高低角为模型视轴线解算逆向过程，为典型的非线性解算过程，很难直接用解析表达式计算，需要使用数值迭代法求解，数值迭代解算的代价函数J(·)表达式如下：

本发明使用拟牛顿数值迭代算法来解算式(7)中的模型姿态角，具体使用DFP算法来实现。为了达到快速收敛提高算法的效率目的，模型姿态角DFP迭代算法需要合适的初值，本发明将前一时刻的模型姿态角做为当前时刻的迭代初值，这是基于扰动的影响使正确解不会偏离前一时刻太远的缘故，再以当前时刻的跟踪方向线的方向余弦[r_nx r_ny r_nz]^T赋予当前模型视轴线方向余弦，以当前模型姿态角为迭代初始值，当前无人机姿态角、无人机航向角为已知参量，使用DFP拟牛顿法迭代解算模型姿态角(模型方位角和模型高低角)。

为验证本专利提出的光电转台跟踪态仿真建模技术的正确性，仿真了飞机绕地面某点为中心做大圆圈飞行(飞机航向角做大幅度变化)，模型视点跟踪大圆圈的中心点，利用非线性方向线跟踪算法，来解算模型姿态角的实时变化，即模型方位角和模型高低角调整变化过程，来验证模型本专利提出的功能模型和跟踪算法的正确性。

测试具体参数如下：功能模型初始姿态角度[0°,0°]^T，模拟无人机初始姿态角[0°,0°,0°]^T，模拟无人机初始航向为正北，飞行高度为1000米，航速为190公里/小时，圆周飞行大圈半径为1.5公里。非线性方向线跟踪算法解算的模型方位角和模型高低角变化曲线如图11和图12所示。

仿真结果显示，当飞机做大圆周运动，并设定圆周的中心为视频跟踪目标时，由于无人机航向角做连续匀速率(无人机匀速率转弯运动)变化，为了实现对目标的有效跟踪，非线性方向线跟踪算法实时调整了功能模型的姿态，模型方位角呈线性变化，补偿了无人机航向角匀速率变化，模型高低角呈非线性变化；图中所示的断点处为模型方位等角/模型高低角在0度或360度处的数值跳变，并不是真正意义的断点。可以看出，本发明提出的功能模型和非线性方向线跟踪算法，在理论上是完全正确的。

将能模型加载到三维仿真场景，并把非线性方向线跟踪算法软件工程化后用于控制该三维场景视角，软件工程化测试结果截图如图13～图17所示，完全实现了对场景中给定某目标的视频跟踪仿真，无论无人机做何种运动方式，也无论运动到任何地点，只要视场可见目标，视场中心始终对准被跟踪目标，计算机资源占用率不高，三维画面流畅稳定，不仅说明技术理论正确，而且还证明了该技术具有较好的工程可用性。

Claims (1)

1.无人机载光电稳定转台跟踪态建模仿真方法，其特征在于：

首先将无人机载光电稳定转台从功能上简化为由内环组件、外环组件和支座组成的二轴二框架结构，其中内环组件安装视点，内环组件通过旋转轴安装在外环组件上，可随外环组件一起绕外环轴做方位旋转，内环组件绕旋转轴相对外环转动，其转动角记为模型高低角，外环组件的旋转轴固定在支座上，可绕外环轴相对于支座做方位旋转，其转动角记为模型方位角，支座为安装固定装置，可固定于无人机等运动或静止平台上；

接着建立无人机N系坐标，其坐标原点设定在无人机的质心上，X_n轴指向地理北，Z_n轴为重力方向，Y_n轴为向东；

建立航迹坐S系坐标，航迹坐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系坐标为T:|x_t y_t z_t|^T，无人机瞬时位置在地面站K系坐标为U:[x_u y_u z_u]^T，令在地面站K系中无人机瞬时位置至被跟踪目标至的连线为跟踪方向线，功能模型对目标跟踪的实时姿态角(α,β)，其中α、β分别为模型方位角和模型高低角，模型视轴线与地面的交点即地主点O的摄像机C系坐标O_c:[0 0 L]^T，其中L为视轴线斜距；

最后进行非线性方向线跟踪解算，包括解算跟踪方向线方向余弦、解算模型视轴线的无人机N系方向余弦和DFP解算模型姿态角三个步骤，具体过程如下：

(1)、解算跟踪方向线方向余弦：

包括跟踪方向线无人机K系坐标解算、跟踪方向线方向余弦解算和跟踪方向线坐标变换等三个计算步骤；

计算跟踪方向线在无人机K系中的坐标分量如式(1)所示：

$\left\{\begin{array}{c}\mathrm{\Δ}x=x_t-x_u\\ \mathrm{\Δ}y=y_t-y_u\\ \mathrm{\Δ}z=z_t-z_u\end{array}\right.---\left(1\right),$

则跟踪方向线的方向余弦r_k:[r_kx r_ky r_kz]^T无人机K系坐标分量表达式如式(2)所示：

$\left\{\begin{array}{c}{r}_{kx}=\frac{\mathrm{\Δ}x}{\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2}}\\ r_{ky}=\frac{\mathrm{\Δ}y}{\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2}}\\ r_{kz}=\frac{\mathrm{\Δ}z}{\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2}}\end{array}\right.---\left(2\right),$

将跟踪方向线的方向余弦r_k由无人机K系变换为无人机N系，变换式如式(3)所示：

$\left[\begin{array}{c}{r}_{nx}\\ r_{ny}\\ r_{nz}\end{array}\right]=M_{en}\left[\begin{array}{c}{r}_{kx}\\ r_{ky}\\ r_{kz}\end{array}\right]---\left(3\right),$

为跟踪方向线的无人机N系方向余弦，M_en为无人机K系至无人机N系的旋转变换矩阵，其表达式如式(4)所示：

$M_{en}=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]---\left(4\right);$

(2)、解算模型视轴线的无人机N系方向余弦：

基于坐标变换原理，地主点O由摄像机C系至无人机N系的坐标变换过程如式(5)所示：

$\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]={\left(M_n^s\right)}^T{\left(M_s^u\right)}^T{\left(M_b^a\right)}^T{\left(M_a^f\right)}^T\left[\begin{array}{c}0\\ 0\\ L\end{array}\right]---\left(5\right),$

其中，和分别为无人机N系至航迹S系、航迹S系至无人机U系、基座B系至方位环A系、方位环A系至高低环F系的旋转变换矩阵，分别对应于无人机的航向角φ_hx、包括偏航角俯仰角和滚转角的无人机姿态角、包括方位角θ_fw、高低角θ_gd的光电转台姿态角，其表达式如式(5.1)～(5.4)所示：

$M_n^s\left({\mathrm{\φ}}_{hx}\right)=M_z\left({\mathrm{\φ}}_{hx}\right)---\left(5.1\right),$

$M_b^a=M_z\left({\mathrm{\θ}}_{fw}\right)---\left(5.3\right),$

$M_a^f=M_y\left({\mathrm{\θ}}_{gd}\right)---\left(5.4\right),$

其中，M_x、M_y、M_z分别绕X轴、Y轴和Z轴的基本旋转矩阵，

则模型视轴线的无人机N系的方位余弦表达式如式(6)所示：

$\left\{\begin{array}{c}{r}_k^x=\frac{x_n}{\sqrt{x_n^2+y_n^2+z_n^2}}\\ r_k^y=\frac{y_n}{\sqrt{x_n^2+y_n^2+z_n^2}}\\ r_k^z=\frac{z}{\sqrt{x_n^2+y_n^2+z_n^2}}\end{array}\right.---\left(6\right),$

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

则即是此时模型视轴线的无人机N系方向余弦；

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

令模型视轴线始终与跟踪方向线重合，实现了功能模型视角对地面感兴趣目标的跟踪，即确保模型视轴线方向余弦与跟踪方向线方向余弦实时相等条件下，已知模型视轴线方向余弦反向解算模型方位角和模型高低角，这就是非线性方向线跟踪算法的基本思路；

在三维视景仿真或新型光电转台理论验证时，无人机的航向角、姿态角、无人机位置和地面目标位置均为已知量，可计算出跟踪方向线的方向余弦，并将其赋予模型视轴线方向线，解算模型方位角和模型高低角为模型视轴线解算逆向过程，为典型的非线性解算过程，很难直接用解析表达式计算，需要使用数值迭代法求解，数值迭代解算的代价函数J(·)表达式如式(7)所示：

$J({\mathrm{\θ}}_{fw},{\mathrm{\θ}}_{gd})=r_k^{ut}-r_k^{ey}=0---\left(7\right),$

使用拟牛顿数值迭代算法来解算式(7)中的模型姿态角，具体使用DFP算法来实现，为了达到快速收敛提高算法的效率目的，模型姿态角DFP迭代算法需要合适的初值，因此将前一时刻的模型姿态角做为当前时刻的迭代初值，这是基于扰动的影响使正确解不会偏离前一时刻太远的缘故，再以当前时刻的跟踪方向线的方向余弦[r_nx r_ny r_nz]^T赋予当前模型视轴线方向余弦，以当前模型姿态角为迭代初始值，当前无人机姿态角、无人机航向角为已知参量，使用DFP拟牛顿法迭代解算模型姿态角即模型方位角和模型高低角。