[发明专利]无人机载光电稳定转台跟踪态建模仿真方法

摘要

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

主权项

无人机载光电稳定转台跟踪态建模仿真方法，其特征在于：首先将无人机载光电稳定转台从功能上简化为由内环组件、外环组件和支座组成的二轴二框架结构，其中内环组件安装视点，内环组件通过旋转轴安装在外环组件上，可随外环组件一起绕外环轴做方位旋转，内环组件绕旋转轴相对外环转动，其转动角记为模型高低角，外环组件的旋转轴固定在支座上，可绕外环轴相对于支座做方位旋转，其转动角记为模型方位角，支座为安装固定装置，可固定于无人机等运动或静止平台上；接着建立无人机N系坐标，其坐标原点设定在无人机的质心上，Xn轴指向地理北，Zn轴为重力方向，Yn轴为向东；建立航迹坐S系坐标，航迹坐S系坐标原点在无人机质心处，Xs轴与无人机规划航向同向，Zs轴为重力方向，Ys轴与Xs轴和Zs轴为右手定则关系；建立无人机U系坐标，无人机U系与无人机机体固联，其坐标原点Ou为无人机的质心，Xu轴为无人机的机头方向，Zu轴垂直于机身平面并指向下方，Yu轴与Xu轴和Zu轴构成右手定则关系；无人机偏航角为无人机U系Xu轴投影在无人机N系XnOnYn平面上的方位角；无人机俯仰角为无人机U系的XuOuYu平面与无人机N系的XnOnYn平面的夹角，向上为正；无人机滚转角为无人机U系的XuOuZu平面与无人机N系的XnOnZn平面的夹角，右转为正；建立光电转台坐标系，光电转台坐标系包括基座坐标系、方位环A系、高低环F系和横滚环R系，基座B系，坐标原点Ob为转轴中心，Zb轴由镜头指向目标，Xb轴、Yb轴分别平行于无人机U系的Xu轴、Yu轴；方位环A系与光电转台的方位环固联，相对基座B系，只能绕Zb轴旋转，产生光电转台方位角θa；高低环F系与高低环固联，Yf轴沿高低环轴与方位环A系Ya轴同向；高低环F系，为方位环A系绕Ya轴旋转高低角θf而得到；横滚环R系与横滚环固联，Xr轴沿横滚环轴并与高低环Xf轴同向，横滚环R系，为高低F系，绕Xf轴旋转横滚角θr得到；建立像平面M系坐标，像平面M系坐标原点Om为像主点，Zm轴与光轴平行且指向目标，当光电转台三个姿态角均为零时，像平面M系Ym轴垂直飞行方向向右，Xm轴垂直于Ym轴向上，像平面M系的Z坐标为焦距f；建立摄像机C系坐标，摄像机C系坐标原点Oc位于摄像机光心即像主点处，Xc轴平行于像平面M系的Xm轴，Yc轴平行于像平面M系的Ym轴，Zc轴平行于像平面M系的Xm轴；建立地面站K系、无人机K系坐标，地面站K系、无人机K系坐标原点为分别为地面站和无人机质心，Xk轴向东，Yk辆向北，Zk与重力反向；令被跟踪目标点在地面站K系坐标为T:|xt yt zt|T，无人机瞬时位置在地面站K系坐标为U:[xu yu zu]T，令在地面站K系中无人机瞬时位置至被跟踪目标至的连线为跟踪方向线，功能模型对目标跟踪的实时姿态角(α,β)，其中α、β分别为模型方位角和模型高低角，模型视轴线与地面的交点即地主点O的摄像机C系坐标Oc:[0 0 L]T，其中L为视轴线斜距；最后进行非线性方向线跟踪解算，包括解算跟踪方向线方向余弦、解算模型视轴线的无人机N系方向余弦和DFP解算模型姿态角三个步骤，具体过程如下：(1)、解算跟踪方向线方向余弦：包括跟踪方向线无人机K系坐标解算、跟踪方向线方向余弦解算和跟踪方向线坐标变换等三个计算步骤；计算跟踪方向线在无人机K系中的坐标分量如式(1)所示：Δx=xt-xuΔy=yt-yuΔz=zt-zu---(1),]]>则跟踪方向线的方向余弦rk:[rkx rky rkz]T无人机K系坐标分量表达式如式(2)所示：rkx=ΔxΔx2+Δy2+Δz2rky=ΔyΔx2+Δy2+Δz2rkz=ΔzΔx2+Δy2+Δz2---(2),]]>将跟踪方向线的方向余弦rk由无人机K系变换为无人机N系，变换式如式(3)所示：rnxrnyrnz=Menrkxrkyrkz---(3),]]>为跟踪方向线的无人机N系方向余弦，Men为无人机K系至无人机N系的旋转变换矩阵，其表达式如式(4)所示：Men=01010000-1---(4);]]>(2)、解算模型视轴线的无人机N系方向余弦：基于坐标变换原理，地主点O由摄像机C系至无人机N系的坐标变换过程如式(5)所示：xnynzn=(Mns)T(Msu)T(Mba)T(Maf)T00L---(5),]]>其中，和分别为无人机N系至航迹S系、航迹S系至无人机U系、基座B系至方位环A系、方位环A系至高低环F系的旋转变换矩阵，分别对应于无人机的航向角φhx、包括偏航角俯仰角和滚转角的无人机姿态角、包括方位角θfw、高低角θgd的光电转台姿态角，其表达式如式(5.1)～(5.4)所示：Mns(φhx)=Mz(φhx)---(5.1),]]>Mba=Mz(θfw)---(5.3),]]>Maf=My(θgd)---(5.4),]]>其中，Mx、My、Mz分别绕X轴、Y轴和Z轴的基本旋转矩阵，则模型视轴线的无人机N系的方位余弦表达式如式(6)所示：rkx=xnxn2+yn2+zn2rky=ynxn2+yn2+zn2rkz=zxn2+yn2+zn2---(6),]]>其中，上式中的xn、yn和zn表达式如式(6.1)、(6.2)、(6.3)所示：则即是此时模型视轴线的无人机N系方向余弦；(3)、DFP解算模型姿态角：令模型视轴线始终与跟踪方向线重合，实现了功能模型视角对地面感兴趣目标的跟踪，即确保模型视轴线方向余弦与跟踪方向线方向余弦实时相等条件下，已知模型视轴线方向余弦反向解算模型方位角和模型高低角，这就是非线性方向线跟踪算法的基本思路；在三维视景仿真或新型光电转台理论验证时，无人机的航向角、姿态角、无人机位置和地面目标位置均为已知量，可计算出跟踪方向线的方向余弦，并将其赋予模型视轴线方向线，解算模型方位角和模型高低角为模型视轴线解算逆向过程，为典型的非线性解算过程，很难直接用解析表达式计算，需要使用数值迭代法求解，数值迭代解算的代价函数J(·)表达式如式(7)所示：J(θfw,θgd)=rkut-rkey=0---(7),]]>使用拟牛顿数值迭代算法来解算式(7)中的模型姿态角，具体使用DFP算法来实现，为了达到快速收敛提高算法的效率目的，模型姿态角DFP迭代算法需要合适的初值，因此将前一时刻的模型姿态角做为当前时刻的迭代初值，这是基于扰动的影响使正确解不会偏离前一时刻太远的缘故，再以当前时刻的跟踪方向线的方向余弦[rnx rny rnz]T赋予当前模型视轴线方向余弦，以当前模型姿态角为迭代初始值，当前无人机姿态角、无人机航向角为已知参量，使用DFP拟牛顿法迭代解算模型姿态角即模型方位角和模型高低角。