CN110986928B - 光电吊舱三轴陀螺仪漂移实时修正方法 - Google Patents
光电吊舱三轴陀螺仪漂移实时修正方法 Download PDFInfo
- Publication number
- CN110986928B CN110986928B CN201911037314.8A CN201911037314A CN110986928B CN 110986928 B CN110986928 B CN 110986928B CN 201911037314 A CN201911037314 A CN 201911037314A CN 110986928 B CN110986928 B CN 110986928B
- Authority
- CN
- China
- Prior art keywords
- gyroscope
- attitude
- angle
- triaxial
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000012937 correction Methods 0.000 title claims description 6
- 239000011159 matrix material Substances 0.000 claims description 45
- 238000005096 rolling process Methods 0.000 claims description 11
- 230000005693 optoelectronics Effects 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 8
- 230000003068 static effect Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- XOOUIPVCVHRTMJ-UHFFFAOYSA-L zinc stearate Chemical compound [Zn+2].CCCCCCCCCCCCCCCCCC([O-])=O.CCCCCCCCCCCCCCCCCC([O-])=O XOOUIPVCVHRTMJ-UHFFFAOYSA-L 0.000 claims description 3
- 239000000470 constituent Substances 0.000 description 14
- 230000006870 function Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000014509 gene expression Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Abstract
本发明公开了光电吊舱三轴陀螺仪漂移实时修正方法,本发明将高精度载体姿态数据实时引入光电吊舱,通过吊舱方位平台转角和俯仰平台转角得到光电吊舱陀螺仪的高精度姿态角及估计姿态角,通过控制器追踪得到实时陀螺仪漂移值,并实时修正了三轴陀螺仪的漂移。
Description
技术领域
本发明涉及光电吊舱自动控制技术领域,具体涉及光电吊舱三轴陀螺仪漂移实时修正方法。
背景技术
光电吊舱安装于飞行器,进行对目标的搜索,跟踪。飞行器的飞行环境复杂且变化巨大,导致光电吊舱的外部环境温度变化情况复杂。光电吊舱使用陀螺测量任务载荷空间角速率,但陀螺仪零位随温度和时间发生漂移,导致任务载荷空间角速率发生偏差,视频图像任务设备画面中心漂移。
现有吊舱陀螺仪解决温度漂移的办法大多采用两种方案:一是采用手动陀螺仪漂移补偿,吊舱操作人员在发现吊舱陀螺仪漂移时,观察任务载荷实时画面,分析出吊舱漂移大小和方向,通过吊舱控制手柄,输入陀螺仪漂移修正值,当画面稳定不再漂移时,即完成一次手动补偿。另一种方案是,将吊舱整体平台放入高低温箱中温度校准补偿,实时采集记录每个温度状态下陀螺仪的漂移偏差,最后使用高阶曲线拟合出温度与漂移的函数,并将该函数记录在吊舱内置计算机中。使用时,根据吊舱陀螺仪实时温度,实时根据“温度与漂移函数”计算出漂移补偿值进行漂移补偿。手动补偿方案需要操作员实时操作,耗费操作员精力与时间,耽误正常作业,且精度低。整体平台温度校准,需要大容量校准箱,且校准时间长,生产效率低下。
发明内容
为了解决现有吊舱中陀螺仪漂移问题,本发明提供了一种光电吊舱三轴陀螺仪漂移实时修正方法。本发明用于实现光电吊舱自动实时在线完成陀螺仪漂移补偿,代替目前吊舱陀螺仪漂移手动补偿和温度校准补偿。
本发明通过下述技术方案实现:
光电吊舱三轴陀螺仪漂移实时修正方法,该方法包括以下步骤:
步骤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:
sin Phi=sin(EL_roll/2)
cos Phi=cos(EL_roll/2)
sin Theta=sin(EL_pitch/2)
cos Theta=cos(EL_pitch/2)
sin Psi=sin(EL_pan/2)
cos Psi=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表示初始协方差矩阵;
MXP=MA*MX
式中,MA I表示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、本发明将高精度载体姿态数据实时引入光电吊舱,通过吊舱方位平台转角和俯仰平台转角得到光电吊舱陀螺仪的高精度姿态角及估计姿态角,通过控制器追踪得到实时陀螺仪漂移值,并实时修正了三轴陀螺仪的漂移。
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。
sin Phi=sin(EL_roll/2)
cos Phi=cos(EL_roll/2)
sin Theta=sin(EL_pitch/2)
cos Theta=cos(EL_pitch/2)
sin Psi=sin(EL_pan/2)
cos Psi=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表示初始协方差矩阵;
MXP=MA*MX
式中,MA I表示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:计算当前陀螺仪三轴角速率漂移估计值(俯仰轴漂移DL_pitch,滚转轴漂移DL_roll和及偏航轴漂移DL_pan)。由三轴陀螺仪姿态角与陀螺仪三轴角速率漂移估计值的代数和,然后通过一个PI控制输出得到当前螺仪三轴角速率漂移估计值。
步骤8:由陀螺仪三轴角速率值与陀螺仪三轴角速率漂移估计值的代数和,通过一个低通滤波器得到漂移修正后的陀螺仪三轴角速率。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和 /或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和 /或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (4)
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得到的当前陀螺仪三轴角速度漂移估计值,得到漂移修正后的陀螺仪三轴角速率;所述步骤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_rolt-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:
sin Phi=sin(EL_roll/2)
cos Phi=cos(EL_roll/2)
sin Theta=sin(EL_pitch/2)
cos Theta=cos(EL_pitch/2)
sin Psi=sin(EL_pan/2)
cos Psi=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表示初始协方差矩阵;
MXP=MA*MX
式中,MA I表示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]))
Eesti_L_pitch表示俯仰姿态估计角,Eesti_L_roll表示滚转姿态估计角,Eesti_L_pan表示偏航姿态估计角;所述步骤7具体由三轴陀螺仪姿态角与陀螺仪三轴速率漂移估计值的代数和,然后通过一个PI控制器输出得到当前陀螺仪三轴角速率漂移估计值,包括俯仰轴漂移估计值DL_pitch,滚转轴漂移估计值DL_roll和偏航轴漂移DL_pan;
所述步骤8具体由陀螺仪三轴角速率值与当前陀螺仪三轴角速率漂移估计值的代数和,再通过一个低通滤波器得到漂移修正后的陀螺仪三轴角速率。
3.根据权利要求1所述的光电吊舱三轴陀螺仪漂移实时修正方法,其特征在于,所述低通滤波器截止频率为0.1Hz。
4.根据权利要求1-3任一项所述的光电吊舱三轴陀螺仪漂移实时修正方法,其特征在于,所述步骤3~步骤8均由光电吊舱内置数据处理装置执行。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911037314.8A CN110986928B (zh) | 2019-10-29 | 2019-10-29 | 光电吊舱三轴陀螺仪漂移实时修正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911037314.8A CN110986928B (zh) | 2019-10-29 | 2019-10-29 | 光电吊舱三轴陀螺仪漂移实时修正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110986928A CN110986928A (zh) | 2020-04-10 |
CN110986928B true CN110986928B (zh) | 2021-08-20 |
Family
ID=70082697
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911037314.8A Active CN110986928B (zh) | 2019-10-29 | 2019-10-29 | 光电吊舱三轴陀螺仪漂移实时修正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110986928B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111722452B (zh) * | 2020-06-29 | 2022-07-12 | 烟台艾睿光电科技有限公司 | 一种红外产品的防抖稳像方法、装置、设备及存储介质 |
CN112558051A (zh) * | 2020-11-30 | 2021-03-26 | 湖南傲英创视信息科技有限公司 | 基于ukf的光电吊舱稳定平台被动测距方法 |
CN113091740B (zh) * | 2021-03-22 | 2023-05-16 | 浙江兆晟科技股份有限公司 | 一种基于深度学习的稳定云台陀螺仪漂移实时修正方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105371871A (zh) * | 2015-12-02 | 2016-03-02 | 中国矿业大学 | 井下采煤机捷联惯导系统的组合初始对准系统及对准方法 |
EP3179211A1 (en) * | 2015-12-07 | 2017-06-14 | Atlantic Inertial Systems Limited | Inertial measurement system for projectiles with corrections of roll angle and scale factor |
CN107702710A (zh) * | 2017-08-17 | 2018-02-16 | 上海航天控制技术研究所 | 一种多陀螺表头常值漂移实时估计方法 |
CN109508033A (zh) * | 2018-12-17 | 2019-03-22 | 成都纵横自动化技术股份有限公司 | 一种无人机控制方法、装置、自动驾驶仪及无人机 |
CN109724579A (zh) * | 2019-03-13 | 2019-05-07 | 中国工程物理研究院电子工程研究所 | 一种陀螺罗经标定方法、装置、计算设备及存储介质 |
CN110092006A (zh) * | 2019-04-04 | 2019-08-06 | 深圳市斯贝达电子有限公司 | 一种提高光电吊舱视轴稳定精度的系统及方法 |
CN110361031A (zh) * | 2019-07-05 | 2019-10-22 | 东南大学 | 一种基于回溯理论的imu全参数误差快速标定方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106706003A (zh) * | 2017-02-15 | 2017-05-24 | 重庆邮电大学 | 一种基于三轴mems陀螺仪的寻北旋转在线校准方法 |
CN107677295B (zh) * | 2017-11-22 | 2023-09-26 | 马玉华 | 一种飞行器惯性导航系统误差校准系统和方法 |
-
2019
- 2019-10-29 CN CN201911037314.8A patent/CN110986928B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105371871A (zh) * | 2015-12-02 | 2016-03-02 | 中国矿业大学 | 井下采煤机捷联惯导系统的组合初始对准系统及对准方法 |
EP3179211A1 (en) * | 2015-12-07 | 2017-06-14 | Atlantic Inertial Systems Limited | Inertial measurement system for projectiles with corrections of roll angle and scale factor |
CN107702710A (zh) * | 2017-08-17 | 2018-02-16 | 上海航天控制技术研究所 | 一种多陀螺表头常值漂移实时估计方法 |
CN109508033A (zh) * | 2018-12-17 | 2019-03-22 | 成都纵横自动化技术股份有限公司 | 一种无人机控制方法、装置、自动驾驶仪及无人机 |
CN109724579A (zh) * | 2019-03-13 | 2019-05-07 | 中国工程物理研究院电子工程研究所 | 一种陀螺罗经标定方法、装置、计算设备及存储介质 |
CN110092006A (zh) * | 2019-04-04 | 2019-08-06 | 深圳市斯贝达电子有限公司 | 一种提高光电吊舱视轴稳定精度的系统及方法 |
CN110361031A (zh) * | 2019-07-05 | 2019-10-22 | 东南大学 | 一种基于回溯理论的imu全参数误差快速标定方法 |
Non-Patent Citations (2)
Title |
---|
Gyro online correctionmethod based on;Hua Gao 等;《2015 Fifth International Conference on Instrumentation and Measurement, Computer, Communication and Control》;20150920;第1144-1179页 * |
基于惯性测量单元旋转的陀螺漂移估计和补偿方法;吴哲明 等;《清华大学学报》;20141231;第54卷(第9期);第1143-1147页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110986928A (zh) | 2020-04-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110986928B (zh) | 光电吊舱三轴陀螺仪漂移实时修正方法 | |
CN105698765B (zh) | 双imu单目视觉组合测量非惯性系下目标物位姿方法 | |
US9915550B2 (en) | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer | |
WO2020253260A1 (zh) | 时间同步处理方法、电子设备及存储介质 | |
CN109000612A (zh) | 设备的角度估算方法、装置、摄像组件及飞行器 | |
US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
WO2021027638A1 (zh) | 一种偏航角的融合方法、装置及飞行器 | |
CN106813679B (zh) | 运动物体的姿态估计的方法及装置 | |
CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
CN107621266B (zh) | 基于特征点跟踪的空间非合作目标相对导航方法 | |
CN108318027B (zh) | 载体的姿态数据的确定方法和装置 | |
CN113155129B (zh) | 一种基于扩展卡尔曼滤波的云台姿态估计方法 | |
CN105910606A (zh) | 一种基于角速度差值的方向修正方法 | |
CN110873563B (zh) | 一种云台姿态估计方法及装置 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
CN112985384A (zh) | 一种抗干扰磁航向角优化系统 | |
CN112797979B (zh) | 一种应用于agv的惯性姿态导航系统 | |
CN116608853B (zh) | 载体动态姿态估计方法、设备、存储介质 | |
Parikh et al. | Comparison of attitude estimation algorithms with IMU under external acceleration | |
CN110645976B (zh) | 一种移动机器人的姿态估计方法及终端设备 | |
CN110530400A (zh) | 陀螺仪漂移修正方法、装置、光电吊舱及飞行器 | |
CN114252073B (zh) | 一种机器人姿态数据融合方法 | |
CN110285811A (zh) | 卫星定位和惯性导航的融合定位方法和装置 | |
CN116125789A (zh) | 一种基于四元数的姿态算法参数自动匹配系统及方法 | |
CN110470294B (zh) | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CP03 | Change of name, title or address |
Address after: 7 / F, area a, building 6, No. 200, Tianfu 5th Street, high tech Zone, Chengdu, Sichuan 610000 Patentee after: CHENGDU JOUAV AUTOMATION TECHNOLOGY Co.,Ltd. Country or region after: China Address before: 610000 room 801-805, 8th floor, block a, building 3, No. 200, Tianfu Fifth Street, Chengdu hi tech Zone, China (Sichuan) pilot Free Trade Zone, Chengdu, Sichuan Patentee before: CHENGDU JOUAV AUTOMATION TECHNOLOGY Co.,Ltd. Country or region before: China |
|
CP03 | Change of name, title or address |