CN116519011A - 基于Psi角误差修正模型的长航时双惯导协同标定方法 - Google Patents
基于Psi角误差修正模型的长航时双惯导协同标定方法 Download PDFInfo
- Publication number
- CN116519011A CN116519011A CN202310230516.4A CN202310230516A CN116519011A CN 116519011 A CN116519011 A CN 116519011A CN 202310230516 A CN202310230516 A CN 202310230516A CN 116519011 A CN116519011 A CN 116519011A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- degrees
- representing
- error
- stops
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000012937 correction Methods 0.000 title claims abstract description 29
- 238000009434 installation Methods 0.000 claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 44
- 230000008569 process Effects 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 9
- 230000036544 posture Effects 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 9
- 230000007246 mechanism Effects 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000017105 transposition Effects 0.000 claims description 4
- 230000009977 dual effect Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 6
- 239000000969 carrier Substances 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
Classifications
-
- 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
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明属于导航技术领域,公开了基于Psi角误差修正模型的长航时双惯导协同标定方法,适用于无外界基准信息时长航时双惯导***的协同标定。本发明通过对速度误差模型的修正,避免动态情况下比力不准影响标定精度,在此基础上以两套惯导***的相对速度、相对位置为约束观测,建立了基于Psi角误差修正模型的联合误差状态卡尔曼滤波器,在无外界基准信息的情况下,对待标定惯导***的陀螺标度因数误差、加速度计标度因数误差、安装误差进行标定估计。本发明提出的标定方法完全自主,不受外界环境干扰,标定精度不受正常工作惯导***绝对误差的影响,在运动状态下也能进行标定,具有重要工程意义。
Description
技术领域
本发明属于导航技术领域,涉及惯导***的外场标定方法,特别涉及基于Psi角误差修正模型的长航时双惯导协同标定方法,适用于两套及以上带有双轴或三轴转位机构的惯导***间的联合标定。
背景技术
误差标定技术是惯性导航***的关键技术之一,根据标定的环境不同可分为实验室标定和外场标定。实验室标定是指在实验室内使用转台等设备对惯导***的误差参数进行标定,外场标定则是将惯导***安装于载体后进行的标定。惯导***的误差参数会随着***使用或存放时间的推移而改变,因此需要对惯导***进行定期标定。由于外场标定技术不需要将***反复拆卸,减少了日常维护的工作量,是近几年惯导***标定技术的研究重点。
传统的外场标定技术以外界准确参考信息作为观测,利用卡尔曼滤波实现***级标定。然而对于缺少外界参考信息的情况,如水下环境、GNSS拒止环境等,外场标定技术的使用会受到限制。对于具备外场标定条件的平台而言,通常会搭载多套带有转位机构的惯导***,例如船载冗余配置的双轴旋转调制惯导***、车载双轴旋转调制主惯导***和“三自”子惯导***等。利用多套惯导***的冗余信息、合理设计惯导***联合转位次序能够使得惯导***的***性误差得到估计。
在无外界参考信息的应用环境下,载体的导航定位由惯导***完成。惯导***的定位误差会随时间积累,而Psi角误差模型定义在计算坐标系,与位置误差解耦,更适合长航时导航***。此外,传统误差方程中导航坐标系下的比力项无法直接测得,需要微分获得,导致在动态环境下***协方差矩阵误差较大,影响标定精度。
本发明针对目前存在的问题,提出基于Psi角误差修正模型的长航时双惯导协同标定方法,将速度误差方程修正后避免误差方程中存在比力项,解决了动态环境下误差方程不准确的问题,将两套惯导的速度、位置投影至同一坐标系后以其相对值作为约束观测,建立基于Psi角误差修正模型的长航时双惯导***联合状态卡尔曼滤波器,对待标定惯导***的全误差参数进行在线标定,解决了无外界基准信息时惯导***的外场标定难题;以两套惯导***间的相对误差为观测量,标定精度不受惯导***绝对误差的影响,在运动状态下也能进行自主标定。
发明内容
本发明提出基于Psi角误差修正模型的长航时双惯导协同标定方法,对速度误差方程进行修正,消除了比力项,解决了在动态环境下误差方程不准确的问题,实现了在无外界基准信息时,对具备自标定能力的惯导***的陀螺标度因数、加速度计标度因数、安装误差角的外场标定。本标定方案不受载体运动状态的影响,在静基座、动基座条件下均能完成标定;不受参考惯导***绝对误差的影响。本发明标定精度能够满足导航级惯导***的需求,具有重要工程实用价值。
为解决上述技术问题,本发明提出的解决方案为:
基于Psi角误差修正模型的长航时双惯导协同标定方法,所述方法包括以下步骤:
(1)构建两套惯导***的误差模型;
定义正常工作的双轴旋转调制惯导***为惯导1,其体坐标系b1定义为“右-前-上”,待标定的惯导***为惯导2,其体坐标系b2定义为“右-前-上”;
惯导1的标度因数误差及安装误差很小进而忽略,将惯导1的误差模型定义为:
其中,
式中,表示惯导1的陀螺组件误差,/>表示惯导1的加速度计组件误差,/>表示惯导1的x轴陀螺漂移,/>表示惯导1的y轴陀螺漂移,/>表示惯导1的z轴陀螺漂移,/>表示惯导1的x轴加速度计零偏,/>表示惯导1的y轴加速度计零偏,/>表示惯导1的z轴加速度计零偏,/>表示惯导1的陀螺漂移,/>表示惯导1的加速度计零偏,/>为惯导1的陀螺噪声,/>为惯导1加速度计噪声;
考虑标度因数误差、安装角误差及零偏误差,将惯导2的误差模型定义为:
其中,
式中,表示惯导2的陀螺组件误差,/>表示惯导2的加速度计组件误差,/>表示惯导2陀螺组件输出的理论角速度矢量,/>表示惯导2加速度计组件测得的理论比力矢量,/>表示惯导2的x轴陀螺漂移,/>表示惯导2的y轴陀螺漂移,/>表示惯导2的z轴陀螺漂移,/>表示惯导2的x轴加速度计零偏,/>表示惯导2的y轴加速度计零偏,/>表示惯导2的z轴加速度计零偏,/>表示惯导2的陀螺漂移,/>表示惯导2的加速度计零偏,/>为惯导2的陀螺噪声,/>为惯导2加速度计噪声;δκg和δμg表示陀螺的标度因数误差矩阵和安装误差矩阵,δκa和δμa表示加速度计的标度因数误差矩阵和安装误差矩阵;
确定δκg和δκa:
式中,δκgx、δκgy和δκgz分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差,δκax、δκay和δκaz分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差;
确定δμg和δμa:
式中,δμgyx、δμgzx和δμgzy表示陀螺组件的三个安装误差角,δμayx、δμazx、δμazy、δμaxy、δμaxz和δμayz表示加速度计组件的六个安装误差角;
(2)利用两套惯导***输出的姿态、速度、位置相关信息,建立基于Psi角误差修正模型的联合状态卡尔曼滤波器,具体步骤为:
(2.1)确定***联合误差方程:
式中,ψ1=[ψE1 ψN1 ψU1]T表示惯导1的漂移误差角,ψE1、ψN1、ψU1分别表示惯导1东向、北向、天向的漂移误差角,表示平台坐标系下误差修正后惯导1的速度误差矢量,/>分别表示误差修正后惯导1在东向、北向、天向的速度误差,/>表示惯导1的位置误差,/>表示惯导1的东向误差,/>表示惯导1的北向误差,/>表示惯导1的天向误差,/>表示惯导1计算坐标系下的地球自转角速度,/>表示惯导1计算坐标系下的转移角速度,/>表示惯导1体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导1计算坐标系下的重力矢量,ψ2=[ψE2 ψN2 ψU2]T表示惯导2的漂移误差角,ψE2、ψN2、ψU2分别表示惯导2东向、北向、天向的漂移误差角,/>表示平台坐标系下误差修正后惯导2的速度误差矢量,/> 分别表示误差修正后惯导2在东向、北向、天向的速度误差,/>表示惯导2的位置误差,表示惯导2的东向误差,/>表示惯导2的北向误差,/>表示惯导2的天向误差,/>表示惯导2计算坐标系下的地球自转角速度,/>表示惯导2计算坐标系下的转移角速度,/>表示惯导2体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导2计算坐标系下的重力矢量,vp表示载体在平台坐标系下的速度;
(2.2)确定联合状态方程:
其中,
式中,0i×j表示i行j列的零矩阵,表示平台坐标系下的惯导1东向、北向、天向速度,L1、h1表示惯导1输出的载***置的纬度和高度,/>表示惯导1速度矢量的反对称矩阵,RN1、RE1表示惯导1输出位置处的子午圈、卯酉圈半径,/>表示惯导1输出位置处的重力加速度的值,/>表示平台坐标系下的惯导2东向、北向、天向速度,L2、h2表示惯导2输出的载***置的纬度和高度,/>表示惯导2速度矢量的反对称矩阵,RN2、RE2表示惯导2输出位置处的子午圈、卯酉圈半径,/>表示惯导2输出位置处的重力加速度的值,ωie表示地球自转角速度,C23表示/>矩阵的第二、第三列,C3表示/>矩阵的第三列,分别表示惯导2的x、y、z轴陀螺的输出值,/>分别表示惯导2的x、y、z轴加速度计的输出值;
将状态向量x(t)表示为:
将噪声分布矩阵及噪声矩阵表示为:
(2.3)确定状态约束观测方程:
将惯导1、惯导2***输出的速度、位置分别表示为:
式中,和/>分别表示惯导1和惯导2输出的平台坐标系下的速度信息,/>和/>分别表示惯导1、惯导2输出的位置信息,惯导1与惯导2之间的外杆臂参数在***安装完毕后测量标定获得,rc表示公共点的位置真值,/>表示两套惯导间的外杆臂,/>表示两套惯导间的外杆臂在惯导2体坐标系下的投影,/>表示惯导2体坐标系相对平台坐标系的旋转角速度矢量;
由于两套***反映的是同一载体的速度信息、位置信息,观测量构成了惯导1、惯导2各自速度误差、位置误差的约束,表示为:
式中,υv、υr为相应的速度观测噪声、位置观测噪声;在双惯导的应用环境中,基于高度信息的外界观测确定高度观测方程:
式中,为惯导1输出的高度值,υh为高度观测的噪声;将观测方程表示为:
z(t)=H(t)x(t)+υ(t)
其中,
H1=[0 0 1]
υ(t)=[(υv)T (υr)T υh]T
式中,I3×3表示3行3列的单位矩阵;
(3)确定两套惯导***的转位次序:
惯导1的转位次序为双轴16次序,具体转位流程如下:
次序1:z轴以9°/s正向旋转180°,转停100s;
次序2:y轴以9°/s反向旋转180°,转停100s;
次序3:z轴以9°/s正向旋转180°,转停100s;
次序4:y轴以9°/s反向旋转180°,转停100s;
次序5:y轴以9°/s反向旋转180°,转停100s;
次序6:z轴以9°/s正向旋转180°,转停100s;
次序7:y轴以9°/s反向旋转180°,转停100s;
次序8:z轴以9°/s正向旋转180°,转停100s;
次序9:z轴以9°/s反向旋转180°,转停100s;
次序10:y轴以9°/s正向旋转180°,转停100s;
次序11:z轴以9°/s反向旋转180°,转停100s;
次序12:y轴以9°/s正向旋转180°,转停100s;
次序13:y轴以9°/s正向旋转180°,转停100s;
次序14:z轴以9°/s反向旋转180°,转停100s;
次序15:y轴以9°/s正向旋转180°,转停100s;
次序16:z轴以9°/s反向旋转180°,转停100s;
惯导2的转位次序为18次序,具体转位流程如下:
次序1:y轴以9°/s正向旋转90°,转停180s;
次序2:y轴以9°/s正向旋转180°,转停180s;
次序3:y轴以9°/s正向旋转180°,转停180s;
次序4:z轴以9°/s正向旋转90°,转停180s;
次序5:z轴以9°/s正向旋转180°,转停180s;
次序6:z轴以9°/s正向旋转180°,转停180s;
次序7:x轴以9°/s正向旋转90°,转停180s;
次序8:x轴以9°/s正向旋转180°,转停180s;
次序9:x轴以9°/s正向旋转180°,转停180s;
次序10:x轴以9°/s正向旋转90°,转停180s;
次序11:x轴以9°/s正向旋转90°,转停180s;
次序12:x轴以9°/s正向旋转90°,转停180s;
次序13:z轴以9°/s正向旋转90°,转停180s;
次序14:z轴以9°/s正向旋转90°,转停180s;
次序15:z轴以9°/s正向旋转90°,转停180s;
次序16:y轴以9°/s正向旋转90°,转停180s;
次序17:y轴以9°/s正向旋转90°,转停180s;
次序18:y轴以9°/s正向旋转90°,转停180s;
基于联合转位方式,惯导1处于双轴旋转调制导航状态,惯导2处于标定状态,其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励,根据步骤(2)所述方案,建立联合状态卡尔曼滤波器即实现惯导2的外场在线标定。
进一步的,本发明方法对载体的运动状态无要求,载体处于系泊状态或运动状态均能实现在线标定;对载体所处环境无要求,在水下环境、GNSS拒止环境下均适用。
进一步的,惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到,惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。
进一步的,步骤(3)所述的联合转位次序适用于两套及以上具有双轴转位机构的惯导***间的在线标定,对于双轴和三轴惯导***之间、多套三轴惯导***间的在线标定也适用。
进一步的,步骤(3)所述的联合转位次序仅为基于两套具有双轴转位机构的惯导***的优选方案,对于其他旋转调制次序与标定次序之间的联合转位方案,也属于本发明的范畴。
综上所述,本发明的优点及积极效果为:本发明通过两套惯导***协同转位,将两套惯导***的输出信息融合完成了外场标定,打破了传统外场自标定方案对于载体运动状态以及外界基准信息的限制,通过Psi角误差修正模型解决了动态环境下误差方程不准确的问题,适用于长航时惯导***的标定,具有重要工程实践意义。
附图说明
图1是本发明实施例提供的流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
在一些特殊的应用环境,如水下环境、GNSS拒止环境等载体缺少准确、可靠外界参考信息,此时载体的导航定位依赖于惯性导航***。当惯导***需要定期标定或者故障后更换了部分组件需要重新标定,传统的外场标定技术不能满足这一需求,这会影响惯导***的精度。此外,传统的速度误差模型存在比力项,在动态环境下会存在比力计算不准导致***协方差矩阵误差增大的问题,并且由于缺乏外界参考信息对惯导***的误差进行校正,惯导***的误差会随着时间累积。为解决上述技术问题,本发明提出基于Psi角误差修正模型的长航时双惯导协同标定方法,所述方法如图1所示,具体实施方法如下:
(1)构建两套惯导***的误差模型;
定义正常工作的双轴旋转调制惯导***为惯导1,其体坐标系b1定义为“右-前-上”,待标定的惯导***为惯导2,其体坐标系b2定义为“右-前-上”;
惯导1的标度因数误差及安装误差很小进而忽略,将惯导1的误差模型定义为:
其中,
式中,表示惯导1的陀螺组件误差,/>表示惯导1的加速度计组件误差,/>表示惯导1的x轴陀螺漂移,/>表示惯导1的y轴陀螺漂移,/>表示惯导1的z轴陀螺漂移,/>表示惯导1的x轴加速度计零偏,/>表示惯导1的y轴加速度计零偏,/>表示惯导1的z轴加速度计零偏,/>表示惯导1的陀螺漂移,/>表示惯导1的加速度计零偏,/>为惯导1的陀螺噪声,为惯导1加速度计噪声;
考虑标度因数误差、安装角误差及零偏误差,将惯导2的误差模型定义为:
其中,
式中,表示惯导2的陀螺组件误差,/>表示惯导2的加速度计组件误差,/>表示惯导2陀螺组件输出的理论角速度矢量,/>表示惯导2加速度计组件测得的理论比力矢量,/>表示惯导2的x轴陀螺漂移,/>表示惯导2的y轴陀螺漂移,/>表示惯导2的z轴陀螺漂移,/>表示惯导2的x轴加速度计零偏,/>表示惯导2的y轴加速度计零偏,/>表示惯导2的z轴加速度计零偏,/>表示惯导2的陀螺漂移,/>表示惯导2的加速度计零偏,/>为惯导2的陀螺噪声,/>为惯导2加速度计噪声;δκg和δμg表示陀螺的标度因数误差矩阵和安装误差矩阵,δκa和δμa表示加速度计的标度因数误差矩阵和安装误差矩阵;
确定δκg和δκa:
式中,δκgx、δκgy和δκgz分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差,δκax、δκay和δκaz分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差;
确定δμg和δμa:
式中,δμgyx、δμgzx和δμgzy表示陀螺组件的三个安装误差角,δμayx、δμazx、δμazy、δμaxy、δμaxz和δμayz表示加速度计组件的六个安装误差角;
(2)利用两套惯导***输出的姿态、速度、位置相关信息,建立基于Psi角误差修正模型的联合状态卡尔曼滤波器,具体步骤为:
(2.1)确定***联合误差方程:
式中,ψ1=[ψE1 ψN1 ψU1]T表示惯导1的漂移误差角,ψE1、ψN1、ψU1分别表示惯导1东向、北向、天向的漂移误差角,表示平台坐标系下误差修正后惯导1的速度误差矢量,/>分别表示误差修正后惯导1在东向、北向、天向的速度误差,/>表示惯导1的位置误差,/>表示惯导1的东向误差,/>表示惯导1的北向误差,/>表示惯导1的天向误差,/>表示惯导1计算坐标系下的地球自转角速度,/>表示惯导1计算坐标系下的转移角速度,/>表示惯导1体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导1计算坐标系下的重力矢量,ψ2=[ψE2 ψN2 ψU2]T表示惯导2的漂移误差角,ψE2、ψN2、ψU2分别表示惯导2东向、北向、天向的漂移误差角,/>表示平台坐标系下误差修正后惯导2的速度误差矢量,/> 分别表示误差修正后惯导2在东向、北向、天向的速度误差,/>表示惯导2的位置误差,表示惯导2的东向误差,/>表示惯导2的北向误差,/>表示惯导2的天向误差,/>表示惯导2计算坐标系下的地球自转角速度,/>表示惯导2计算坐标系下的转移角速度,/>表示惯导2体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导2计算坐标系下的重力矢量,vp表示载体在平台坐标系下的速度;
(2.2)确定联合状态方程:
其中,
/>
式中,0i×j表示i行j列的零矩阵,表示平台坐标系下的惯导1东向、北向、天向速度,L1、h1表示惯导1输出的载***置的纬度和高度,/>表示惯导1速度矢量的反对称矩阵,RN1、RE1表示惯导1输出位置处的子午圈、卯酉圈半径,/>表示惯导1输出位置处的重力加速度的值,/>表示平台坐标系下的惯导2东向、北向、天向速度,L2、h2表示惯导2输出的载***置的纬度和高度,/>表示惯导2速度矢量的反对称矩阵,RN2、RE2表示惯导2输出位置处的子午圈、卯酉圈半径,/>表示惯导2输出位置处的重力加速度的值,ωie表示地球自转角速度,C23表示/>矩阵的第二、第三列,C3表示/>矩阵的第三列,分别表示惯导2的x、y、z轴陀螺的输出值,/>分别表示惯导2的x、y、z轴加速度计的输出值;
将状态向量x(t)表示为:
将噪声分布矩阵及噪声矩阵表示为:
/>
(2.3)确定状态约束观测方程:
将惯导1、惯导2***输出的速度、位置分别表示为:
式中,和/>分别表示惯导1和惯导2输出的平台坐标系下的速度信息,/>和/>分别表示惯导1、惯导2输出的位置信息,惯导1与惯导2之间的外杆臂参数在***安装完毕后测量标定获得,rc表示公共点的位置真值,/>表示两套惯导间的外杆臂,/>表示两套惯导间的外杆臂在惯导2体坐标系下的投影,/>表示惯导2体坐标系相对平台坐标系的旋转角速度矢量;
由于两套***反映的是同一载体的速度信息、位置信息,观测量构成了惯导1、惯导2各自速度误差、位置误差的约束,表示为:
式中,υv、υr为相应的速度观测噪声、位置观测噪声;在双惯导的应用环境中,基于高度信息的外界观测确定高度观测方程:
式中,为惯导1输出的高度值,υh为高度观测的噪声;将观测方程表示为:
z(t)=H(t)x(t)+υ(t)
其中,
H1=[0 0 1]
υ(t)=[(υv)T (υr)T υh]T
式中,I3×3表示3行3列的单位矩阵;
(3)确定两套惯导***的转位次序:
惯导1的转位次序为双轴16次序,具体转位流程如下:
次序1:z轴以9°/s正向旋转180°,转停100s;
次序2:y轴以9°/s反向旋转180°,转停100s;
次序3:z轴以9°/s正向旋转180°,转停100s;
次序4:y轴以9°/s反向旋转180°,转停100s;
次序5:y轴以9°/s反向旋转180°,转停100s;
次序6:z轴以9°/s正向旋转180°,转停100s;
次序7:y轴以9°/s反向旋转180°,转停100s;
次序8:z轴以9°/s正向旋转180°,转停100s;
次序9:z轴以9°/s反向旋转180°,转停100s;
次序10:y轴以9°/s正向旋转180°,转停100s;
次序11:z轴以9°/s反向旋转180°,转停100s;
次序12:y轴以9°/s正向旋转180°,转停100s;
次序13:y轴以9°/s正向旋转180°,转停100s;
次序14:z轴以9°/s反向旋转180°,转停100s;
次序15:y轴以9°/s正向旋转180°,转停100s;
次序16:z轴以9°/s反向旋转180°,转停100s;
惯导2的转位次序为18次序,具体转位流程如下:
次序1:y轴以9°/s正向旋转90°,转停180s;
次序2:y轴以9°/s正向旋转180°,转停180s;
次序3:y轴以9°/s正向旋转180°,转停180s;
次序4:z轴以9°/s正向旋转90°,转停180s;
次序5:z轴以9°/s正向旋转180°,转停180s;
次序6:z轴以9°/s正向旋转180°,转停180s;
次序7:x轴以9°/s正向旋转90°,转停180s;
次序8:x轴以9°/s正向旋转180°,转停180s;
次序9:x轴以9°/s正向旋转180°,转停180s;
次序10:x轴以9°/s正向旋转90°,转停180s;
次序11:x轴以9°/s正向旋转90°,转停180s;
次序12:x轴以9°/s正向旋转90°,转停180s;
次序13:z轴以9°/s正向旋转90°,转停180s;
次序14:z轴以9°/s正向旋转90°,转停180s;
次序15:z轴以9°/s正向旋转90°,转停180s;
次序16:y轴以9°/s正向旋转90°,转停180s;
次序17:y轴以9°/s正向旋转90°,转停180s;
次序18:y轴以9°/s正向旋转90°,转停180s;
基于联合转位方式,惯导1处于双轴旋转调制导航状态,惯导2处于标定状态,其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励,根据步骤(2)所述方案,建立联合状态卡尔曼滤波器即实现惯导2的外场在线标定。
本发明方法对载体的运动状态无要求,载体处于系泊状态或运动状态均能实现在线标定;对载体所处环境无要求,在水下环境、GNSS拒止环境下均适用。
惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到,惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。
所述步骤(3)中的联合转位次序适用于两套及以上具有双轴转位机构的惯导***间的在线标定,对于双轴和三轴惯导***之间、多套三轴惯导***间的在线标定也适用。
所述步骤(3)中的联合转位次序仅为基于两套具有双轴转位机构的惯导***的优选方案,对于其他旋转调制次序与标定次序之间的联合转位方案,也属于本发明的范畴。
以上所述仅是本发明的优选实施方式,并不用以限制本发明,凡属于本发明思路下的技术方案均属于本发明的保护范围。在不脱离本发明原理前提下的若干改进和润饰等,这些改进和润饰也应视为本发明的保护范围。
Claims (4)
1.基于Psi角误差修正模型的长航时双惯导协同标定方法,其特征在于,所述方法包括以下步骤:
(1)构建两套惯导***的误差模型;
定义正常工作的双轴旋转调制惯导***为惯导1,其体坐标系b1定义为“右-前-上”,待标定的惯导***为惯导2,其体坐标系b2定义为“右-前-上”;
惯导1的标度因数误差及安装误差很小进而忽略,将惯导1的误差模型定义为:
其中,
式中,表示惯导1的陀螺组件误差,/>表示惯导1的加速度计组件误差,/>表示惯导1的x轴陀螺漂移,/>表示惯导1的y轴陀螺漂移,/>表示惯导1的z轴陀螺漂移,/>表示惯导1的x轴加速度计零偏,/>表示惯导1的y轴加速度计零偏,/>表示惯导1的z轴加速度计零偏,/>表示惯导1的陀螺漂移,/>表示惯导1的加速度计零偏,/>为惯导1的陀螺噪声,/>为惯导1加速度计噪声;
考虑标度因数误差、安装角误差及零偏误差,将惯导2的误差模型定义为:
其中,
式中,表示惯导2的陀螺组件误差,/>表示惯导2的加速度计组件误差,/>表示惯导2陀螺组件输出的理论角速度矢量,/>表示惯导2加速度计组件测得的理论比力矢量,/>表示惯导2的x轴陀螺漂移,/>表示惯导2的y轴陀螺漂移,/>表示惯导2的z轴陀螺漂移,/>表示惯导2的x轴加速度计零偏,/>表示惯导2的y轴加速度计零偏,/>表示惯导2的z轴加速度计零偏,/>表示惯导2的陀螺漂移,/>表示惯导2的加速度计零偏,/>为惯导2的陀螺噪声,/>为惯导2加速度计噪声;δκg和δμg表示陀螺的标度因数误差矩阵和安装误差矩阵,δκa和δμa表示加速度计的标度因数误差矩阵和安装误差矩阵;
确定δκg和δκa:
式中,δκgx、δκgy和δκgz分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差,δκax、δκay和δκaz分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差;
确定δμg和δμa:
式中,δμgyx、δμgzx和δμgzy表示陀螺组件的三个安装误差角,δμayx、δμazx、δμazy、δμaxy、δμaxz和δμayz表示加速度计组件的六个安装误差角;
(2)利用两套惯导***输出的姿态、速度、位置相关信息,建立基于Psi角误差修正模型的联合状态卡尔曼滤波器,具体步骤为:
(2.1)确定***联合误差方程:
式中,ψ1=[ψE1 ψN1 ψU1]T表示惯导1的漂移误差角,ψE1、ψN1、ψU1分别表示惯导1东向、北向、天向的漂移误差角,表示平台坐标系下误差修正后惯导1的速度误差矢量,/>分别表示误差修正后惯导1在东向、北向、天向的速度误差,表示惯导1的位置误差,/>表示惯导1的东向误差,/>表示惯导1的北向误差,/>表示惯导1的天向误差,/>表示惯导1计算坐标系下的地球自转角速度,表示惯导1计算坐标系下的转移角速度,/>表示惯导1体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导1计算坐标系下的重力矢量,ψ2=[ψE2 ψN2 ψU2]T表示惯导2的漂移误差角,ψE2、ψN2、ψU2分别表示惯导2东向、北向、天向的漂移误差角,/>表示平台坐标系下误差修正后惯导2的速度误差矢量,/> 分别表示误差修正后惯导2在东向、北向、天向的速度误差,/>表示惯导2的位置误差,/>表示惯导2的东向误差,/>表示惯导2的北向误差,/>表示惯导2的天向误差,/>表示惯导2计算坐标系下的地球自转角速度,/>表示惯导2计算坐标系下的转移角速度,/>表示惯导2体坐标系至平台坐标系的方向余弦矩阵,/>表示惯导2计算坐标系下的重力矢量,vp表示载体在平台坐标系下的速度;
(2.2)确定联合状态方程:
其中,
式中,0i×j表示i行j列的零矩阵,表示平台坐标系下的惯导1东向、北向、天向速度,L1、h1表示惯导1输出的载***置的纬度和高度,/>表示惯导1速度矢量的反对称矩阵,RN1、RE1表示惯导1输出位置处的子午圈、卯酉圈半径,/>表示惯导1输出位置处的重力加速度的值,/>表示平台坐标系下的惯导2东向、北向、天向速度,L2、h2表示惯导2输出的载***置的纬度和高度,/>表示惯导2速度矢量的反对称矩阵,RN2、RE2表示惯导2输出位置处的子午圈、卯酉圈半径,/>表示惯导2输出位置处的重力加速度的值,ωie表示地球自转角速度,C23表示/>矩阵的第二、第三列,C3表示/>矩阵的第三列,分别表示惯导2的x、y、z轴陀螺的输出值,/>分别表示惯导2的x、y、z轴加速度计的输出值;
将状态向量x(t)表示为:
将噪声分布矩阵及噪声矩阵表示为:
(2.3)确定状态约束观测方程:
将惯导1、惯导2***输出的速度、位置分别表示为:
式中,和/>分别表示惯导1和惯导2输出的平台坐标系下的速度信息,/>和/>分别表示惯导1、惯导2输出的位置信息,惯导1与惯导2之间的外杆臂参数在***安装完毕后测量标定获得,rc表示公共点的位置真值,/>表示两套惯导间的外杆臂,/>表示两套惯导间的外杆臂在惯导2体坐标系下的投影,/>表示惯导2体坐标系相对平台坐标系的旋转角速度矢量;
由于两套***反映的是同一载体的速度信息、位置信息,观测量构成了惯导1、惯导2各自速度误差、位置误差的约束,表示为:
式中,υv、υr为相应的速度观测噪声、位置观测噪声;在双惯导的应用环境中,基于高度信息的外界观测确定高度观测方程:
式中,为惯导1输出的高度值,υh为高度观测的噪声;将观测方程表示为:
z(t)=H(t)x(t)+υ(t)
其中,
H1=[0 0 1]
υ(t)=[(υv)T (υr)T υh]T
式中,I3×3表示3行3列的单位矩阵;
(3)确定两套惯导***的转位次序:
惯导1的转位次序为双轴16次序,具体转位流程如下:
次序1:z轴以9°/s正向旋转180°,转停100s;
次序2:y轴以9°/s反向旋转180°,转停100s;
次序3:z轴以9°/s正向旋转180°,转停100s;
次序4:y轴以9°/s反向旋转180°,转停100s;
次序5:y轴以9°/s反向旋转180°,转停100s;
次序6:z轴以9°/s正向旋转180°,转停100s;
次序7:y轴以9°/s反向旋转180°,转停100s;
次序8:z轴以9°/s正向旋转180°,转停100s;
次序9:z轴以9°/s反向旋转180°,转停100s;
次序10:y轴以9°/s正向旋转180°,转停100s;
次序11:z轴以9°/s反向旋转180°,转停100s;
次序12:y轴以9°/s正向旋转180°,转停100s;
次序13:y轴以9°/s正向旋转180°,转停100s;
次序14:z轴以9°/s反向旋转180°,转停100s;
次序15:y轴以9°/s正向旋转180°,转停100s;
次序16:z轴以9°/s反向旋转180°,转停100s;
惯导2的转位次序为18次序,具体转位流程如下:
次序1:y轴以9°/s正向旋转90°,转停180s;
次序2:y轴以9°/s正向旋转180°,转停180s;
次序3:y轴以9°/s正向旋转180°,转停180s;
次序4:z轴以9°/s正向旋转90°,转停180s;
次序5:z轴以9°/s正向旋转180°,转停180s;
次序6:z轴以9°/s正向旋转180°,转停180s;
次序7:x轴以9°/s正向旋转90°,转停180s;
次序8:x轴以9°/s正向旋转180°,转停180s;
次序9:x轴以9°/s正向旋转180°,转停180s;
次序10:x轴以9°/s正向旋转90°,转停180s;
次序11:x轴以9°/s正向旋转90°,转停180s;
次序12:x轴以9°/s正向旋转90°,转停180s;
次序13:z轴以9°/s正向旋转90°,转停180s;
次序14:z轴以9°/s正向旋转90°,转停180s;
次序15:z轴以9°/s正向旋转90°,转停180s;
次序16:y轴以9°/s正向旋转90°,转停180s;
次序17:y轴以9°/s正向旋转90°,转停180s;
次序18:y轴以9°/s正向旋转90°,转停180s;
基于联合转位方式,惯导1处于双轴旋转调制导航状态,惯导2处于标定状态,其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励,根据步骤(2)所述方案,建立联合状态卡尔曼滤波器即实现惯导2的外场在线标定。
2.如权利要求1所述的基于Psi角误差修正模型的长航时双惯导协同标定方法,其特征在于,本发明方法对载体的运动状态无要求,载体处于系泊状态或运动状态均能实现在线标定;对载体所处环境无要求,在水下环境、GNSS拒止环境下均适用。
3.如权利要求1所述的基于Psi角误差修正模型的长航时双惯导协同标定方法,其特征在于,惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到,惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。
4.如权利要求1所述的基于Psi角误差修正模型的长航时双惯导协同标定方法,其特征在于,所述步骤(3)中的联合转位次序适用于两套及以上具有双轴转位机构的惯导***间的在线标定,对于双轴和三轴惯导***之间、多套三轴惯导***间的在线标定也适用。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310230516.4A CN116519011B (zh) | 2023-03-11 | 2023-03-11 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310230516.4A CN116519011B (zh) | 2023-03-11 | 2023-03-11 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116519011A true CN116519011A (zh) | 2023-08-01 |
CN116519011B CN116519011B (zh) | 2024-03-01 |
Family
ID=87407094
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310230516.4A Active CN116519011B (zh) | 2023-03-11 | 2023-03-11 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116519011B (zh) |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09318382A (ja) * | 1996-05-27 | 1997-12-12 | Nec Corp | 姿勢角検出装置及び方法 |
CN103471615A (zh) * | 2013-08-30 | 2013-12-25 | 北京自动化控制设备研究所 | 一种双冗余惯导***快速故障检测方法 |
US20140168264A1 (en) * | 2012-12-19 | 2014-06-19 | Lockheed Martin Corporation | System, method and computer program product for real-time alignment of an augmented reality device |
CN103900611A (zh) * | 2014-03-28 | 2014-07-02 | 北京航空航天大学 | 一种惯导天文高精度复合两位置对准及误差标定方法 |
CN104897178A (zh) * | 2015-07-06 | 2015-09-09 | 中国人民解放军国防科学技术大学 | 一种双惯导联合旋转调制导航与在线相对性能评估方法 |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
CN105806365A (zh) * | 2016-03-15 | 2016-07-27 | 北京航空航天大学 | 一种基于自抗扰控制的车载惯导行进间快速初始对准方法 |
CN107543545A (zh) * | 2017-10-30 | 2018-01-05 | 中国人民解放军国防科技大学 | 极区双航海惯性导航***定位信息融合方法 |
CN110285834A (zh) * | 2019-07-08 | 2019-09-27 | 哈尔滨工程大学 | 基于一点位置信息的双惯导***快速自主重调方法 |
CN110296668A (zh) * | 2019-04-24 | 2019-10-01 | 合肥工业大学 | 一种基于bp神经网络的圆光栅传感器测角误差修正方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN113029199A (zh) * | 2021-03-15 | 2021-06-25 | 中国人民解放军国防科技大学 | 一种激光陀螺惯导***的***级温度误差补偿方法 |
WO2021227011A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种陀螺随钻测量***及方法 |
CN114353831A (zh) * | 2021-12-29 | 2022-04-15 | 中国卫星海上测控部 | 一种船载usb测角误差修正模型及其实现方法 |
CN114877915A (zh) * | 2022-06-09 | 2022-08-09 | 中国人民解放军国防科技大学 | 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法 |
CN115143993A (zh) * | 2022-07-01 | 2022-10-04 | 中国人民解放军国防科技大学 | 基于三轴转台的激光陀螺惯导***g敏感性误差标定方法 |
-
2023
- 2023-03-11 CN CN202310230516.4A patent/CN116519011B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09318382A (ja) * | 1996-05-27 | 1997-12-12 | Nec Corp | 姿勢角検出装置及び方法 |
US20140168264A1 (en) * | 2012-12-19 | 2014-06-19 | Lockheed Martin Corporation | System, method and computer program product for real-time alignment of an augmented reality device |
CN103471615A (zh) * | 2013-08-30 | 2013-12-25 | 北京自动化控制设备研究所 | 一种双冗余惯导***快速故障检测方法 |
CN103900611A (zh) * | 2014-03-28 | 2014-07-02 | 北京航空航天大学 | 一种惯导天文高精度复合两位置对准及误差标定方法 |
CN104897178A (zh) * | 2015-07-06 | 2015-09-09 | 中国人民解放军国防科学技术大学 | 一种双惯导联合旋转调制导航与在线相对性能评估方法 |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
CN105806365A (zh) * | 2016-03-15 | 2016-07-27 | 北京航空航天大学 | 一种基于自抗扰控制的车载惯导行进间快速初始对准方法 |
CN107543545A (zh) * | 2017-10-30 | 2018-01-05 | 中国人民解放军国防科技大学 | 极区双航海惯性导航***定位信息融合方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN110296668A (zh) * | 2019-04-24 | 2019-10-01 | 合肥工业大学 | 一种基于bp神经网络的圆光栅传感器测角误差修正方法 |
CN110285834A (zh) * | 2019-07-08 | 2019-09-27 | 哈尔滨工程大学 | 基于一点位置信息的双惯导***快速自主重调方法 |
WO2021227011A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种陀螺随钻测量***及方法 |
CN113029199A (zh) * | 2021-03-15 | 2021-06-25 | 中国人民解放军国防科技大学 | 一种激光陀螺惯导***的***级温度误差补偿方法 |
CN114353831A (zh) * | 2021-12-29 | 2022-04-15 | 中国卫星海上测控部 | 一种船载usb测角误差修正模型及其实现方法 |
CN114877915A (zh) * | 2022-06-09 | 2022-08-09 | 中国人民解放军国防科技大学 | 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法 |
CN115143993A (zh) * | 2022-07-01 | 2022-10-04 | 中国人民解放军国防科技大学 | 基于三轴转台的激光陀螺惯导***g敏感性误差标定方法 |
Non-Patent Citations (1)
Title |
---|
梁钟泓等: "基于协方差变换的INS/DVL 全球组合导航算法", 中国惯性技术学报, vol. 30, no. 2, pages 159 - 167 * |
Also Published As
Publication number | Publication date |
---|---|
CN116519011B (zh) | 2024-03-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110160554B (zh) | 一种基于寻优法的单轴旋转捷联惯导***标定方法 | |
CN109211269B (zh) | 一种双轴旋转惯导***姿态角误差标定方法 | |
CN107167134B (zh) | 冗余配置激光陀螺航海惯导协同定位方法 | |
CN109974697A (zh) | 一种基于惯性***的高精度测绘方法 | |
CN116481564B (zh) | 基于Psi角误差修正模型的极地双惯导协同标定方法 | |
CN112595350B (zh) | 一种惯导***自动标定方法及终端 | |
CN116734887B (zh) | 基于速度误差修正模型的极地双惯导协同标定方法 | |
CN110243377B (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN111351508B (zh) | 一种mems惯性测量单元***级批量标定方法 | |
CN104344836A (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN112710328B (zh) | 一种四轴冗余惯导***的误差标定方法 | |
CN112129321B (zh) | 陀螺零偏标定值确定方法、装置及计算机存储介质 | |
CN113203415B (zh) | 一种原子陀螺导航***及其导航解算方法 | |
CN116222618B (zh) | 一种极地环境下的双惯导协同标定方法 | |
CN111207734B (zh) | 一种基于ekf的无人机组合导航方法 | |
CN111141285B (zh) | 一种航空重力测量装置 | |
CN111024128B (zh) | 一种机载光电吊舱光轴稳定状态传递对准方法 | |
CN110488853B (zh) | 一种降低转轴涡动影响的混合式惯导***稳定控制指令的计算方法 | |
CN116519011B (zh) | 基于Psi角误差修正模型的长航时双惯导协同标定方法 | |
CN116242397B (zh) | 一种速度误差修正模型下的双惯导协同标定方法 | |
CN114370885B (zh) | 一种惯性导航***误差补偿方法及*** | |
CN116222619B (zh) | 一种双惯导***外场协同在线标定方法 | |
CN112304339A (zh) | 一种卫星移动通信天线的惯导校准方法 | |
CN113970344B (zh) | 一种惯导***的陀螺和加速度计刻度系数非对称性误差标定方法 | |
Shu-zhi et al. | Optimization design and calibration of installation error coefficients for gyroscope-free strapdown inertial measurement unit |
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 |