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

摘要

本发明公开了一种无人机载光电稳定转台惯性态建模仿真方法，首先将光电转台抽象化功能模型，然后利用视轴线惯性稳定递推迭代算法解算的模型方位角和模型高低角。本发明构造了光电转台二自由度功能模型，设计实现了视轴线惯性稳定递推迭代算法，很好地解决了三维视景仿真光电转台的功能建模和惯性态算法建模难题，具有很好的工程可用性。

主权项

无人机载光电稳定转台惯性态建模仿真方法，其特征在于：包括以下步骤：(1)、光电转台抽象化功能构模：从功能上将光电稳定转台简化为二轴二框架结构，该二轴二框架结构由内环组件、外环组件和支座组成，内环组件安装视点，视点的虚拟光轴线记为模型视轴线，内环组件旋转轴安装在外环组件上，可随外环组件一起运动，内环组件绕旋转轴相对外环组件转动，其转动角为高低角，记为模型高低角，外环组件的旋转轴固定在支座上，可绕外环轴相对于支座做方位旋转，其转动角为方位角，记为模型方位角，支座为安装固定装置，可固定于无人机等运动或静止平台上；(2)、视轴线惯性稳定递推迭代算法设计：(2.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平面的夹角，右转为正；确定光电转台坐标系，光电转台坐标系包括基座B系、方位环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系，坐标原点为分别为地面站和无人机质心，X_k轴向东，Y_k轴向北，Z_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，功能模型对的实时姿态角(a,β)，a,β分别为模型方位角和模型高低角，模型视轴线与地面的交点即地主点O的摄像机C系坐标O_c:[0 0 L]^T，其中L为视轴线斜距；(2.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、包括偏航角俯仰角和滚转角的无人机姿态角、包括模型方位角模型高低角的模型姿态角的输入角参量，其表达式如式(5.1)～(5.4)所示：$M_n^s\left({\phi }_{hx}\right)=M_z\left({\phi }_{hx}\right)---\left(5.1\right),$$M_b^a=M_z\left({\theta }_{fw}\right)---\left(5.3\right),$$M_a^f=M_y\left({\theta }_{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}{\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_n}{\sqrt{x_n^2+y_n^2+z_n^2}}\end{array}\right.---\left(6\right),$其中，上式中的x_n、y_n和z_n表达式如下：则即为当前时刻k的模型视轴线的无人机N系方向余弦；(2.3)、DFP解算模型姿态角：DFP解算模型姿态角可使得模型视轴线在采样各时刻保持方向不变，具体来说，确保当前模型视轴线方向余弦与前一时刻的视轴线方向余弦相等，根据当前时刻来反向解算模型方位角和模型高低角；根据视轴线方向余弦的解算过程可知，解算模型方位角和模型高低角为模型视轴线解算逆向过程，为典型的非线性过程，很难直接用解析表达式计算，需要使用数值迭代法求解，其代价函数J(·)如式(7)：$J({\theta }_k^{fw},{\theta }_k^{gd})=r_k^{ey}-r_{k-1}^{ey}=0---\left(7\right),$使用DFP拟牛顿数值迭代算法来解算式(7)中的模型姿态角，为了达到快速收敛提高算法的效率目的，将前一时刻的模型视轴线方向余弦赋予当前模型视轴线方向余弦，并把前一时刻解算的模型姿态角作为DFP解算的迭代输入，基于当前无人机姿态角、无人机航向角为已知参量，使用DFP拟牛顿法迭代解算当前时刻的包括模型方位角和模型高低角的模型姿态角。