光电吊舱三轴陀螺仪漂移实时修正方法技术

本发明专利技术公开了光电吊舱三轴陀螺仪漂移实时修正方法，本发明专利技术将高精度载体姿态数据实时引入光电吊舱，通过吊舱方位平台转角和俯仰平台转角得到光电吊舱陀螺仪的高精度姿态角及估计姿态角，通过控制器追踪得到实时陀螺仪漂移值，并实时修正了三轴陀螺仪的漂移。

Real time correction method for drift of three-axis gyroscope in photoelectric pod

【技术实现步骤摘要】
光电吊舱三轴陀螺仪漂移实时修正方法
本专利技术涉及光电吊舱自动控制
，具体涉及光电吊舱三轴陀螺仪漂移实时修正方法。
技术介绍
光电吊舱安装于飞行器，进行对目标的搜索，跟踪。飞行器的飞行环境复杂且变化巨大，导致光电吊舱的外部环境温度变化情况复杂。光电吊舱使用陀螺测量任务载荷空间角速率，但陀螺仪零位随温度和时间发生漂移，导致任务载荷空间角速率发生偏差，视频图像任务设备画面中心漂移。现有吊舱陀螺仪解决温度漂移的办法大多采用两种方案：一是采用手动陀螺仪漂移补偿，吊舱操作人员在发现吊舱陀螺仪漂移时，观察任务载荷实时画面，分析出吊舱漂移大小和方向，通过吊舱控制手柄，输入陀螺仪漂移修正值，当画面稳定不再漂移时，即完成一次手动补偿。另一种方案是，将吊舱整体平台放入高低温箱中温度校准补偿，实时采集记录每个温度状态下陀螺仪的漂移偏差，最后使用高阶曲线拟合出温度与漂移的函数，并将该函数记录在吊舱内置计算机中。使用时，根据吊舱陀螺仪实时温度，实时根据“温度与漂移函数”计算出漂移补偿值进行漂移补偿。手动补偿方案需要操作员实时操作，耗费操作员精力与时间，耽误正常作业，且精度低。整体平台温度校准，需要大容量校准箱，且校准时间长，生产效率低下。
技术实现思路
为了解决现有吊舱中陀螺仪漂移问题，本专利技术提供了一种光电吊舱三轴陀螺仪漂移实时修正方法。本专利技术用于实现光电吊舱自动实时在线完成陀螺仪漂移补偿，代替目前吊舱陀螺仪漂移手动补偿和温度校准补偿。本专利技术通过下述技术方案实现：光电吊舱三轴陀螺仪漂移实时修正方法，该方法包括以下步骤：步骤1，将光电吊舱和惯性姿态传感器均安装在载体上，光电吊舱基座相对于惯性姿态传感器静止，光电吊舱基座体轴坐标系平行于惯性姿态传感器体轴坐标系，且光电吊舱基座体轴坐标系X轴与惯性姿态传感器体轴坐标系X轴同向；步骤2，惯性姿态传感器实时输出测量的姿态数据，包括俯仰姿态角ES_pitch，滚转姿态角ES_roll和偏航姿态角ES_pan；步骤3，根据步骤2的姿态数据计算得到光电吊舱三轴陀螺仪姿态角，包括俯仰姿态角L，滚转姿态角EL_roll和偏航姿态角EL_pan；步骤4，根据陀螺仪三轴原始角速率与陀螺仪三轴角速率漂移估计值计算得到光电吊舱三轴陀螺仪三轴估计角速率矩阵MA；步骤5，根据光电吊舱三轴陀螺仪姿态角计算三轴陀螺仪姿态角四元素矩阵MZ；步骤6，根据光电吊舱三轴陀螺仪估计角速率矩阵MA和三轴陀螺仪姿态角四元素矩阵MZ计算得到三轴陀螺仪姿态估计角；步骤7，根据三轴陀螺仪姿态角和陀螺仪三轴角速率漂移估计值，得到当前陀螺仪三轴角速率漂移估计值；步骤8，根据陀螺仪三轴原始角速率与步骤7得到的当前陀螺仪三轴角速度漂移估计值，得到漂移修正后的陀螺仪三轴角速率。优选的，所述步骤3具体通过下式计算得到光电吊舱三轴陀螺仪姿态角：式中，θA表示光电吊舱方位平台旋转角度，θE表示光电吊舱俯仰平台旋转角度，载体姿态角绕方位平台轴旋转θA得到方位平台姿态角，包括俯仰姿态角EA_pitch，滚转姿态角EA_roll和偏航姿态角EA_pan，方位平台姿态角绕俯仰平台轴旋转θE得到俯仰平台姿态角即三轴陀螺仪姿态角，包括俯仰姿态角EE_pitch，滚转姿态角EE_roll和偏航姿态角EE_pan；因惯性姿态传感器与载体相对静止，即惯性姿态传感器输出的俯仰姿态角ES_pitch、滚转姿态角ES_roll和偏航姿态角ES_pan分别于载体俯仰姿态角EB_pitch、载体滚转姿态角EB_roll和载体偏航姿态角相等EB_pan。优选的，所述步骤4通过下式计算得到吊舱三轴陀螺仪估计角速率矩阵MA：PQREsti_roll＝PQRL_roll-DL_rollPQREsti_pitch＝PQRL_pitch-DL_pitchPQREsti_pan＝PQRL_pan-DL_pan式中，PQRL_roll表示三轴陀螺仪原始滚转角速率，PQRL_pitch表示三轴陀螺仪原始俯仰角速率和PQRL_pan表示三轴陀螺仪原始偏航角速率；DL_roll表示三轴陀螺仪滚转轴角速率漂移估计值，DL_pitch表示三轴陀螺仪俯仰轴角速率漂移估计值，DL_pan表示三轴陀螺仪偏航轴角速率漂移估计值，dt表示系统运行间隔时间。优选的，所述步骤5通过下式计算得到三轴陀螺仪姿态四元素矩阵MZ：sinPhi＝sin(EL_roll/2)cosPhi＝cos(EL_roll/2)sinTheta＝sin(EL_pitch/2)cosTheta＝cos(EL_pitch/2)sinPsi＝sin(EL_pan/2)cosPsi＝cos(EL_pan/2)式中，Phi表示三轴陀螺仪滚转角，Theta表示三轴陀螺仪俯仰角，Psi表示三轴陀螺仪偏航角。优选的，所述步骤6具体包括以下步骤：系统运行时，执行步骤(1)～(5)；系统正常运行时，执行步骤(2)～(5)：步骤(1)系统开始运行，初始化矩阵Q、R、H、MX和MP：Q＝0.0001*HR＝10*HMP＝H式中，H表示单位矩阵，Q表示过程噪声，R表示测量噪声，MX表示状态矩阵，MP表示初始协方差矩阵；步骤(2)计算状态估计矩阵MXP和协方差MXP＝MA*MX式中，MAI表示MA的逆矩阵；步骤(3)计算三轴陀螺仪姿态估计角的估计增益矩阵K：式中，HI表示H的逆矩阵；步骤(4)计算三轴陀螺仪姿态估计角四元素矩阵MX与更新协方差矩阵MP：MX＝MXP+K*(MZ-H*MXP)步骤(5)计算三轴陀螺仪姿态估计角：Eesti_L_roll＝atan2(2*(MX[3]*MX[4]+MX[1]*MX[2])，1-2*(MX[2]*MX[2]+MX[3]*MX[3]))Eesti_L_pitch＝-asin(2*(MX[2]*MX[4]-MX[1]*MX[3]))Eesti_L_pan＝atan2(2*(MX[2]*MX[3]+MX[1]*MX[4])，1-2*(MX[3]*MX[3]+MX[4]*MX[4]))优选的，所述步骤7具体由三轴陀螺仪姿态角与陀螺仪三轴速率漂移估计值的代数和，然后通过一个PI控制器输出得到当前陀螺仪三轴角速率漂移估计值，包括俯仰轴漂移估计值DL_pitch，滚转轴漂移估计值DL_roll和偏航轴漂移DL_pan。优选的，所述PI控制器的传递函数为：。优选的，所述步骤8具体由陀螺仪三轴角速率值与当前陀螺仪三轴角速率漂移估计值的代数和，再通过一个低通滤波器得到漂移修正后的陀螺仪三轴角速率。优选的，所述低通滤波器截止频率为0.1Hz。优选的，所述步骤3～步骤8均由光电吊舱内置数本文档来自技高网...

【技术保护点】
1.光电吊舱三轴陀螺仪漂移实时修正方法，其特征在于，该方法包括以下步骤：/n步骤1，将光电吊舱和惯性姿态传感器均安装在载体上，光电吊舱基座相对于惯性姿态传感器静止，光电吊舱基座体轴坐标系平行于惯性姿态传感器体轴坐标系，且光电吊舱基座体轴坐标系X轴与惯性姿态传感器体轴坐标系X轴同向；/n步骤2，惯性姿态传感器实时输出测量的姿态数据，包括俯仰姿态角E

【技术特征摘要】
1.光电吊舱三轴陀螺仪漂移实时修正方法，其特征在于，该方法包括以下步骤：
步骤1，将光电吊舱和惯性姿态传感器均安装在载体上，光电吊舱基座相对于惯性姿态传感器静止，光电吊舱基座体轴坐标系平行于惯性姿态传感器体轴坐标系，且光电吊舱基座体轴坐标系X轴与惯性姿态传感器体轴坐标系X轴同向；
步骤2，惯性姿态传感器实时输出测量的姿态数据，包括俯仰姿态角ES_pitch，滚转姿态角ES_roll和偏航姿态角ES_pan；
步骤3，根据步骤2的姿态数据计算得到光电吊舱三轴陀螺仪姿态角，包括俯仰姿态角L，滚转姿态角EL_roll和偏航姿态角EL_pan；
步骤4，根据陀螺仪三轴原始角速率与陀螺仪三轴角速率漂移估计值计算得到光电吊舱三轴陀螺仪三轴估计角速率矩阵MA；
步骤5，根据光电吊舱三轴陀螺仪姿态角计算三轴陀螺仪姿态角四元素矩阵MZ；
步骤6，根据光电吊舱三轴陀螺仪估计角速率矩阵MA和三轴陀螺仪姿态角四元素矩阵MZ计算得到三轴陀螺仪姿态估计角；
步骤7，根据三轴陀螺仪姿态角和陀螺仪三轴角速率漂移估计值，得到当前陀螺仪三轴角速率漂移估计值；
步骤8，根据陀螺仪三轴原始角速率与步骤7得到的当前陀螺仪三轴角速度漂移估计值，得到漂移修正后的陀螺仪三轴角速率。


2.根据权利要求1所述的光电吊舱三轴陀螺仪漂移实时修正方法，其特征在于，所述步骤3具体通过下式计算得到光电吊舱三轴陀螺仪姿态角：









式中，θA表示光电吊舱方位平台旋转角度，θE表示光电吊舱俯仰平台旋转角度，载体姿态角绕方位平台轴旋转θA得到方位平台姿态角，包括俯仰姿态角EA_pitch，滚转姿态角EA_roll和偏航姿态角EA_pan，方位平台姿态角绕俯仰平台轴旋转θE得到俯仰平台姿态角即三轴陀螺仪姿态角，包括俯仰姿态角EE_pitch，滚转姿态角EE_roll和偏航姿态角EE_pan；因惯性姿态传感器与载体相对静止，即惯性姿态传感器输出的俯仰姿态角ES_pitch、滚转姿态角ES_roll和偏航姿态角ES_pan分别于载体俯仰姿态角EB_pitch、载体滚转姿态角EB_roll和载体偏航姿态角相等EB_pan。


3.根据权利要求1所述的光电吊舱三轴陀螺仪漂移实时修正方法，其特征在于，所述步骤4通过下式计算得到吊舱三轴陀螺仪估计角速率矩阵MA：
PQREsti_roll＝PQRL_roll-DL_roll
PQREsti_pitch＝PQRL_pitch-DL_pitch
PQREsti_pan＝PQRL_pan-DL_pan



式中，PQRL_roll表示三轴陀螺仪原始滚转角速率，PQRL_pitch表示三轴陀螺仪原始俯仰角速率和PQRL_pan表示三轴陀螺仪原始偏航角速率；DL_roll表示三轴陀螺仪滚转轴角速率漂移估计值，DL_pitch表示三轴陀螺仪俯仰轴角速率漂移估计值，DL_pan表示三轴陀螺仪偏航轴角速率漂移估计值，dt表示系统运行间隔时间。


4.根据权利要求1所述的光电吊舱三轴陀螺仪漂移实时修正方法，其特征在于，所述步骤5通过下式计算得到三轴陀螺仪姿态四元素矩阵...

【专利技术属性】
技术研发人员：刘述超，衡思兰，
申请(专利权)人：成都纵横自动化技术股份有限公司，
类型：发明
国别省市：四川;51