光电吊舱三轴陀螺仪漂移实时修正方法与流程

本发明涉及光电吊舱自动控制技术领域，具体涉及光电吊舱三轴陀螺仪漂移实时修正方法。

背景技术：

光电吊舱安装于飞行器，进行对目标的搜索，跟踪。飞行器的飞行环境复杂且变化巨大，导致光电吊舱的外部环境温度变化情况复杂。光电吊舱使用陀螺测量任务载荷空间角速率，但陀螺仪零位随温度和时间发生漂移，导致任务载荷空间角速率发生偏差，视频图像任务设备画面中心漂移。

现有吊舱陀螺仪解决温度漂移的办法大多采用两种方案：一是采用手动陀螺仪漂移补偿，吊舱操作人员在发现吊舱陀螺仪漂移时，观察任务载荷实时画面，分析出吊舱漂移大小和方向，通过吊舱控制手柄，输入陀螺仪漂移修正值，当画面稳定不再漂移时，即完成一次手动补偿。另一种方案是，将吊舱整体平台放入高低温箱中温度校准补偿，实时采集记录每个温度状态下陀螺仪的漂移偏差，最后使用高阶曲线拟合出温度与漂移的函数，并将该函数记录在吊舱内置计算机中。使用时，根据吊舱陀螺仪实时温度，实时根据“温度与漂移函数”计算出漂移补偿值进行漂移补偿。手动补偿方案需要操作员实时操作，耗费操作员精力与时间，耽误正常作业，且精度低。整体平台温度校准，需要大容量校准箱，且校准时间长，生产效率低下。

技术实现要素：

为了解决现有吊舱中陀螺仪漂移问题，本发明提供了一种光电吊舱三轴陀螺仪漂移实时修正方法。本发明用于实现光电吊舱自动实时在线完成陀螺仪漂移补偿，代替目前吊舱陀螺仪漂移手动补偿和温度校准补偿。

本发明通过下述技术方案实现：

光电吊舱三轴陀螺仪漂移实时修正方法，该方法包括以下步骤：

步骤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_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表示系统运行间隔时间。

优选的，所述步骤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*h

r＝10*h

mp＝h

式中，h表示单位矩阵，q表示过程噪声，r表示测量噪声，mx表示状态矩阵，mp表示初始协方差矩阵；

步骤(2)计算状态估计矩阵mxp和协方差

mxp＝ma*mx

式中，maⁱ表示ma的逆矩阵；

步骤(3)计算三轴陀螺仪姿态估计角的估计增益矩阵k：

式中，hⁱ表示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、本发明将高精度载体姿态数据实时引入光电吊舱，通过吊舱方位平台转角和俯仰平台转角得到光电吊舱陀螺仪的高精度姿态角及估计姿态角，通过控制器追踪得到实时陀螺仪漂移值，并实时修正了三轴陀螺仪的漂移。

2、本发明的无需对陀螺仪进行温度校准，也无需对吊舱整体平台进行温度校准，可以同时补偿温度漂移和随机漂移，补偿延迟低，精度高，无需人工干预。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。在附图中：

图1为本发明的方法流程图。

具体实施方式

在下文中，可在本发明的各种实施例中使用的术语“包括”或“可包括”指示所发明的功能、操作或元件的存在，并且不限制一个或更多个功能、操作或元件的增加。此外，如在本发明的各种实施例中所使用，术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

在本发明的各种实施例中，表述“或”或“a或/和b中的至少一个”包括同时列出的文字的任何组合或所有组合。例如，表述“a或b”或“a或/和b中的至少一个”可包括a、可包括b或可包括a和b二者。

在本发明的各种实施例中使用的表述(诸如“第一”、“第二”等)可修饰在各种实施例中的各种组成元件，不过可不限制相应组成元件。例如，以上表述并不限制所述元件的顺序和/或重要性。以上表述仅用于将一个元件与其它元件区别开的目的。例如，第一用户装置和第二用户装置指示不同用户装置，尽管二者都是用户装置。例如，在不脱离本发明的各种实施例的范围的情况下，第一元件可被称为第二元件，同样地，第二元件也可被称为第一元件。

应注意到：如果描述将一个组成元件“连接”到另一组成元件，则可将第一组成元件直接连接到第二组成元件，并且可在第一组成元件和第二组成元件之间“连接”第三组成元件。相反地，当将一个组成元件“直接连接”到另一组成元件时，可理解为在第一组成元件和第二组成元件之间不存在第三组成元件。

在本发明的各种实施例中使用的术语仅用于描述特定实施例的目的并且并非意在限制本发明的各种实施例。如在此所使用，单数形式意在也包括复数形式，除非上下文清楚地另有指示。除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实施例

本实施例提出了一种光电吊舱三轴陀螺仪漂移实时修正方法，如图1所示，包括以下步骤：

步骤1：将光电吊舱安装于载体上，并将惯性姿态传感器也安装于载体上，光电吊舱基座相对惯性姿态传感器静止，光电吊舱基座体轴坐标系平行于惯性姿态传感器体轴坐标系，且光电吊舱基座体轴坐标系x轴与惯性姿态传感器体轴坐标系x轴同向。本实施例的光电吊舱采用但不限于两轴两框架光电吊舱。

步骤2：惯性姿态传感器实时输出测量的姿态数据(俯仰姿态角es_pitch，滚转姿态角es_roll，及偏航姿态角es_pan)，并通过导线传输至光电吊舱内置数据处理装置(优选为计算机)中。

以下步骤3～步骤8均在内置数据处理装置中执行：

步骤3：计算吊舱三轴陀螺仪姿态角(俯仰姿态角el_roll，滚转姿态角el_pitch，及偏航姿态角el_pan)。惯性姿态传感器与载体相对静止，即输出俯仰姿态角es_pitch，滚转姿态角es_roll，及偏航姿态角es_pan分别与载体俯仰姿态角eb_pitch，载体滚转姿态角eb_roll，及载体偏航姿态角eb_pan相等。光电吊舱方位平台旋转角度为θa，光电吊舱俯仰平台旋转角度为θe。载体姿态角绕方位平台旋转θa得到方位平台姿态角(俯仰姿态角ea_pitch，滚转姿态角ea_roll，及偏航姿态角ea_pan)，方位平台姿态角绕俯仰平台轴旋转θe得到俯仰平台姿态角(俯仰姿态角ee_pitch，滚转姿态角ee_roll，及偏航姿态角ee_pan)。俯仰平台与三轴陀螺仪固定连接，俯仰平台姿态角相等于三轴陀螺仪姿态角。

步骤4：计算吊舱三轴陀螺仪估计角速率矩阵ma。由三轴陀螺仪原始角速度率矩阵(滚转角速率pqrl_roll，俯仰角速率pqrl_pitch及偏航角速率pqrl_pan)与三轴陀螺仪角速率漂移估计值(俯仰轴漂移dl_pitch，滚转轴漂移dl_roll及偏航轴漂移dl_pan)的代数和乘以乘积再乘以系统运行间隔时间dt(单位秒)，最后再加上单位矩阵。系统运行间隔时间dt越小，系统计算精度越高，收敛越快。

pqresti_roll＝pqrl_roll-dl_roll

pqresti_pitch＝pqrl_pitch-dl_pitch

pqresti_pan＝pqrl_pan-dl_pan

步骤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：计算三轴陀螺仪姿态估计角，三轴陀螺仪姿态估计角(俯仰姿态估计角eesti_lpitch、滚转姿态估计角eesti_l_roll及偏航姿态估计角eesti_l_pan)。系统运行时，运行步骤为(1)～(5)；系统正常进行时，运行步骤为(2)～(5)。

(1)系统运行初始值时，初始化矩阵q、r、h、mx和mp。

q＝0.0001*h

r＝10*h

mp＝h

式中，h表示单位矩阵，q表示过程噪声，r表示测量噪声，mx表示状态矩阵，mp表示初始协方差矩阵；

(2)计算状态估计矩阵mxp和协方差

mxp＝ma*mx

式中，maⁱ表示ma的逆矩阵；

(3)计算三轴陀螺仪姿态估计角的估计增益矩阵k：

式中，hⁱ表示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：计算当前陀螺仪三轴角速率漂移估计值(俯仰轴漂移dl_pitch，滚转轴漂移dl_roll和及偏航轴漂移dl_pan)。由三轴陀螺仪姿态角与陀螺仪三轴角速率漂移估计值的代数和，然后通过一个pi控制输出得到当前螺仪三轴角速率漂移估计值。

步骤8：由陀螺仪三轴角速率值与陀螺仪三轴角速率漂移估计值的代数和，通过一个低通滤波器得到漂移修正后的陀螺仪三轴角速率。

进一步的优选方案，所述一种两轴两框架光电吊舱三轴陀螺仪漂移实时修正方法，步骤6中的pi控制器的传递函数为

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。