CN113503895B - 基于卡尔曼滤波的三自惯组加速度计尺寸估计方法 - Google Patents
基于卡尔曼滤波的三自惯组加速度计尺寸估计方法 Download PDFInfo
- Publication number
- CN113503895B CN113503895B CN202110650825.8A CN202110650825A CN113503895B CN 113503895 B CN113503895 B CN 113503895B CN 202110650825 A CN202110650825 A CN 202110650825A CN 113503895 B CN113503895 B CN 113503895B
- Authority
- CN
- China
- Prior art keywords
- accelerometer
- inertial navigation
- error
- inertial
- inner frame
- 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
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该基于卡尔曼滤波的三自惯组加速度计尺寸估计方法包括:对惯导***进行粗对准;根据陀螺角速率和比力信息进行惯性导航解算以完成姿态四元数、位置和速度的更新;惯导***的内框和外框按照预设转动次序和转动角度进行转动,获取转动过程中陀螺角速率和加速度计比力;根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型;初始化卡尔曼滤波器参数;采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计。应用本发明的技术方案,能够解决现有技术中惯性器件误差与加速度计尺寸误差耦合,无法精确分离导致影响导航精度的技术问题。
Description
技术领域
本发明涉及惯性导航技术领域,尤其涉及一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法。
背景技术
在惯性导航***中,由于机械结构及安装原因,三个加速度计敏感质量质心不重合或者经过模型量取的尺寸进行补偿后仍然存在较大偏差。当载体进行振动或者大角运动时,会引起加速度计测量组件的尺寸效应误差。对于通过旋转方式进行对准的三自惯组,尺寸效应误差产生的速度误差以及对准过程中等效的加表零位误差已不能忽略,需要进行参数标定与补偿。以往加速度计的尺寸通常是通过模型设计保证,当出现偏差时,一般是利用角振动激励的速度误差通过最小二乘的方式辨识相关参数,当惯性导航***的器件误差未进行精确补偿时,惯性器件误差与加速度计尺寸误差耦合,无法精确分离。
发明内容
本发明提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,能够解决现有技术中惯性器件误差与加速度计尺寸误差耦合,无法精确分离导致影响导航精度的技术问题。
本发明提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该基于卡尔曼滤波的三自惯组加速度计尺寸估计方法包括:对惯导***进行粗对准以获取惯导初始姿态;根据陀螺角速率和比力信息进行惯性导航解算以完成姿态四元数、位置和速度的更新;惯导***的内框和外框按照预设转动次序和转动角度进行转动,获取转动过程中陀螺角速率和加速度计比力;根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型;初始化卡尔曼滤波器参数;根据转动过程中陀螺角速率、加速度计比力以及***误差模型采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计以完成基于卡尔曼滤波的三自惯组加速度计尺寸估计。
进一步地,惯导***的内框和外框的预设转动次序和转动角度具体为:内框:0°外框:0°、内框:0°外框:90°、内框:0°外框:180°、内框:0°外框:270°、内框:0°外框:180°、内框:0°外框:90°、内框:0°外框:0°、内框:90°外框:0°、内框:90°外框:90°、内框:90°外框:180°、内框:90°外框:270°、内框:90°外框:180°、内框:90°外框:90°、内框:90°外框:0°、内框:180°外框:0°、内框:270°外框:0°、内框:180°外框:0°、内框:90°外框:0°和内框:0°外框:0°。
进一步地,根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型具体包括:根据惯导器件误差和加速度计尺寸构建惯导***的状态变量;根据状态变量和惯性导航解算结果构建***误差模型。
进一步地,根据构建惯导***的状态变量,其中,Xk表示k时刻的状态变量,δL、δh和δλ分别表示惯导***的纬度误差、高度误差和经度误差,δVN、δVU和δVE分别表示惯导***北速度误差、天速度误差和东速度误差,φN、φU和φE分别表示惯导***地理坐标系内北、天和东三个方向的失准角,/>和/>分别表示惯导***测量坐标系内X、Y和Z三个方向的加速度计零位误差,εx、εy和εz分别表示惯导***测量坐标系内X、Y和Z三个方向的陀螺常值漂移,kax、kay和kaz分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的标度因数误差,kayx、kazx和kazy分别表示惯导***测量坐标系内x加速度计相对于Y方向的安装误差、x加速度计相对于Z方向的安装误差和y加速度计相对于Z方向的安装误差,kax2、kay2和kaz2分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的二次项系数误差,kgx、kgy和kgz分别表示惯导***测量坐标系内x陀螺、y陀螺和z陀螺的标度因数误差,kgxy、kgxz、kgyx、kgyz、kgzx和kgzy分别表示惯导***测量坐标系内y陀螺相对于X方向的安装误差、z陀螺相对于X方向的安装误差、x陀螺相对于Y方向的安装误差、z陀螺相对于Y方向的安装误差、x陀螺相对于Z方向的安装误差、y陀螺相对于Z方向的安装误差。
进一步地,根据构建***误差模型,其中,Φk.k-1为***k-1时刻到k时刻的状态转移矩阵,Xk-1为k-1时刻的状态变量,Wk-1表示***k-1时刻的***噪声,Zk为k时刻的***观测量,Hk为k时刻的观测矩阵,/>I为单位矩阵,vk为k时刻的观测噪声。
进一步地,根据获取状态转移矩阵Φk,k-1,其中,Tn为导航周期,Te为滤波周期,At为t时刻的状态转移矩阵,
B4=03×3,/>
ωie为地球自转角速率,VN、VU和VE分别为惯导北向、天向和东向速度,L和h分别为惯导纬度和高度,RM和RN分别为地球子午面和卯酉面半径,/>为b系到n系的姿态转换矩阵,/>和/>分别为惯导体坐标系下x陀螺、y陀螺和z陀螺的陀螺输出角速率,/>和/>为惯导体坐标系下x加速度计、y加速度计和z加速度计的加表输出比力,/>和/>为陀螺输出角速率通过微分得到的x陀螺、y陀螺和z陀螺的角加速度。
进一步地,根据Zk=[L-L0 h-h0 λ-λ0 VN VU VE]T获取***观测量Zk,其中,λ为惯导经度,L0、h0和λ0分别为初始装订纬度、高度和经度。
进一步地,初始化卡尔曼滤波器参数具体包括:设定卡尔曼滤波的初始协方差矩阵P0、***噪声方差阵Q、***误差状态初值X0和卡尔曼滤波计算周期Tk。
进一步地,根据Pk+1/k=Φk+1,kPk/kΦT k+1,k+Qk,Kk+1=Pk+1/kHT k+1[Hk+1Pk+1/kHT k+1+Rk+1]-1,Pk+1/k+1=[I-Kk+1Hk+1]Pk+1/k,进行卡尔曼滤波,其中,Φk+1,k为k时刻到k+1时刻的状态转移矩阵,和/>分别为k时刻***状态量估计值、k到k+1时刻***状态量一步预测估计值和k+1时刻***状态量量测更新后估计值,Pk/k、Pk+1/k和Pk+1/k+1分别为k时刻、k到k+1一步预测时刻和k+1量测更新时刻的误差协方差值,Zk+1表示k+1时刻的***观测量,γk表示k时刻的新息值,Kk+1表示k+1时刻的量测更新过程中的增益值,Qk和Rk+1分别表示k时刻的***噪声方差阵和k+1时刻的量测噪声方差阵。
应用本发明的技术方案,提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该方法通过将加速度计尺寸误差加入到***误差模型中,在合适的转位路径下实现了惯性器件误差与加速度计尺寸误差的分离,简化了加速度计尺寸误差的辨识难度,提高了惯导***的导航精度。与现有技术相比,本发明的技术方案能够解决现有技术中惯性器件误差与加速度计尺寸误差耦合,无法精确分离导致影响导航精度的技术问题。
附图说明
所包括的附图用来提供对本发明实施例的进一步的理解,其构成了说明书的一部分,用于例示本发明的实施例,并与文字描述一起来阐释本发明的原理。显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1示出了根据本发明的具体实施例提供的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法的流程示意图;
图2示出了根据本发明的具体实施例提供的加速度计的三维坐标示意图;
图3示出了根据本发明的具体实施例提供的加速度计尺寸估计数据图。
具体实施方式
需要说明的是,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的,决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本申请的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
除非另外具体说明,否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时,应当明白,为了便于描述,附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论,但在适当情况下,所述技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有示例中,任何具体值应被解释为仅仅是示例性的,而不是作为限制。因此,示例性实施例的其它示例可以具有不同的值。应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步讨论。
如图1所示,根据本发明的具体实施例提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该基于卡尔曼滤波的三自惯组加速度计尺寸估计方法包括:对惯导***进行粗对准以获取惯导初始姿态;惯导***的内框和外框按照预设转动次序和转动角度进行转动,获取转动过程中陀螺角速率和加速度计比力;根据惯导器件误差和加速度计尺寸构建***误差模型;初始化卡尔曼滤波器参数;根据转动过程中陀螺角速率、加速度计比力以及***误差模型采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计以完成基于卡尔曼滤波的三自惯组加速度计尺寸估计。
应用此种配置方式,提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该方法通过将加速度计尺寸误差加入到***误差模型中,在合适的转位路径下实现了惯性器件误差与加速度计尺寸误差的分离,简化了加速度计尺寸误差的辨识难度,提高了惯导***的导航精度。与现有技术相比,本发明的技术方案能够解决现有技术中惯性器件误差与加速度计尺寸误差耦合,无法精确分离导致影响导航精度的技术问题。
进一步地,在本发明中,为了实现基于卡尔曼滤波的三自惯组加速度计尺寸估计,首先,对惯导***进行粗对准以获取惯导初始姿态。
作为本发明的一个具体实施例,惯导***开机后旋转机构处于锁紧状态,惯导***在初始位置静止若干时长后,利用惯性测量组合敏感的加速度与角速度信息,即角速率和比力信息/>进行粗对准,确定初始姿态。在获取初始姿态后,***解锁。在该实施例中,惯导***在初始位置静止时长不少有30s。
此外,在本发明中,在完成粗对准后,根据陀螺角速率和比力信息进行惯性导航解算以完成姿态四元数、位置和速度的更新。
进一步地,在本发明中,在完成惯性导航结算后,惯导***的内框和外框按照预设转动次序和转动角度进行转动,获取转动过程中陀螺角速率和加速度计比力。
作为本发明的一个具体实施例,惯导***的转位机构内框和外框可按照表1中的转动次序和转动角度进行转动,在每个位置的停止时间大于等于等于180s,从一个位置转动至下一位置的转动角速度大于等于15°/s。
表1惯导***内框和外框的转动次序和转动角度表
此外,在本发明中,在获取转动过程中陀螺角速率和加速度计比力后,根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型。
作为本发明的一个具体实施例,在本发明中,根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型具体包括:根据惯导器件误差和加速度计尺寸构建惯导***的状态变量;根据状态变量和惯性导航解算结果构建***误差模型。
在三自惯组中,通常将旋转中心(即内环轴与外环轴相交的中心点)当成测量坐标系原点,加速度计的尺寸即为加速度计敏感中心在测量坐标系中三维坐标。如图2中三个圆柱体表示为加速度计,加速度计的尺寸参数定义为三个加速度计敏感中心相对测量坐标系原点的三维坐标。三个加速度计在测量坐标系下的坐标为其中,Rx、Ry和Rz分别表示x加速度计、y加速度计和z加速度计的三维坐标,Rxx、Rxy和Rxz分别表示在测量坐标系下x加速度计相对旋转中心在X、Y和Z三个方向的坐标值,Ryx、Ryy和Ryz分别表示在测量坐标系下y加速度计相对旋转中心在X、Y和Z三个方向的坐标值,Rzx、Rzy和Rzz分别表示在测量坐标系下z加速度计相对旋转中心在X、Y和Z三个方向的坐标值。将Rxx、Rxy、Rxz、Ryx、Ryy、Ryz、Rzx、Rzy和Rzz扩充至惯导***误差模型中。
当转位机构旋转时,可以得到加速度计敏感到尺寸效应产生的偏差,其中,fx、fy和fz分别表示x加速度计、y加速度计和z加速度计的敏感到尺寸效应产生的偏差,ω表示陀螺敏感得到的三维角速度,惯性组合可以视为一个刚体,在刚体上任意位置角速度是一致的,因此,加速度的角速度也为ω,/>表示角加速度。
结合转位路径以及转动速度对惯性器件误差及加速度计尺寸误差的激励,结合惯导***导航误差(速度误差和姿态误差)以及惯性器件误差,扩充9维加速度计尺寸至状态变量中。具体地,根据构建惯导***的状态变量,其中,Xk表示k时刻的状态变量,δL、δh和δλ分别表示惯导***的纬度误差、高度误差和经度误差,δVN、δVU和δVE分别表示惯导***北速度误差、天速度误差和东速度误差,φN、φU和φE分别表示惯导***地理坐标系内北、天和东三个方向的失准角,▽x、▽y和▽z分别表示惯导***测量坐标系内X、Y和Z三个方向的加速度计零位误差,εx、εy和εz分别表示惯导***测量坐标系内X、Y和Z三个方向的陀螺常值漂移,kax、kay和kaz分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的标度因数误差,kayx、kazx和kazy分别表示惯导***测量坐标系内x加速度计相对于Y方向的安装误差、x加速度计相对于Z方向的安装误差和y加速度计相对于Z方向的安装误差,kax2、kay2和kaz2分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的二次项系数误差,kgx、kgy和kgz分别表示惯导***测量坐标系内x陀螺、y陀螺和z陀螺的标度因数误差,kgxy、kgxz、kgyx、kgyz、kgzx和kgzy分别表示惯导***测量坐标系内y陀螺相对于X方向的安装误差、z陀螺相对于X方向的安装误差、x陀螺相对于Y方向的安装误差、z陀螺相对于Y方向的安装误差、x陀螺相对于Z方向的安装误差、y陀螺相对于Z方向的安装误差。
进一步地,在本发明中,在完成状态变量的构建后,根据构建***误差模型,其中,Φk.k-1为***k-1时刻到k时刻的状态转移矩阵,Xk-1为k-1时刻的状态变量,Wk-1表示***k-1时刻的***噪声,Zk为k时刻的***观测量,Hk为k时刻的观测矩阵,I为单位矩阵,vk为k时刻的观测噪声。
在本发明中,状态转移矩阵Φk,k-1可根据获取,其中,Tn为导航周期,Te为滤波周期。在该实施例中,可取Tn=0.005s,Te=1s。At为t时刻的状态转移矩阵,每个滤波周期开始时t=0。
B4=03×3,/>
其中,ωie为地球自转角速率,VN、VU和VE分别为惯导北向、天向和东向速度,L和h分别为惯导纬度和高度,RM和RN分别为地球子午面和卯酉面半径,/>为b系到n系的姿态转换矩阵,/>和分别为惯导体坐标系下x陀螺、y陀螺和z陀螺的陀螺输出角速率,/>和/>为惯导体坐标系下x加速度计、y加速度计和z加速度计的加表输出比力,/>和/>为陀螺输出角速率通过微分得到的x陀螺、y陀螺和z陀螺的角加速度。
在本发明中,***观测量可根据Zk=[L-L0 h-h0 λ-λ0 VN VU VE]T获取,其中,λ为惯导经度,L0、h0和λ0分别为初始装订纬度、高度和经度。
此外,在本发明中,为了降低导航误差,在完成***误差模型的构建后,初始化卡尔曼滤波器参数。作为本发明的一个具体实施例,设定卡尔曼滤波的初始协方差矩阵P0,其参数可根据初始误差的量级进行设置;设定***噪声方差阵Q,可根据实际***噪声大小进行设置;设定***误差状态初值X0,维数为42×1,一般设为零矩阵;设定卡尔曼滤波计算周期Tk。
进一步地,在本发明中,在初始化卡尔曼滤波器参数后,根据转动过程中陀螺角速率、加速度计比力以及***误差模型采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计以完成基于卡尔曼滤波的三自惯组加速度计尺寸估计。
作为本发明的一个具体实施例,可根据Pk+1/k=Φk+1,kPk/kΦT k+1,k+Qk,/>Kk+1=Pk+1/kHT k+1[Hk+1Pk+1/kHT k+1+Rk+1]-1,Pk+1/k+1=[I-Kk+ 1Hk+1]Pk+1/k,/>进行卡尔曼滤波,其中,Φk+1,k为k时刻到k+1时刻的状态转移矩阵,/>和/>分别为k时刻***状态量估计值、k到k+1时刻***状态量一步预测估计值和k+1时刻***状态量量测更新后估计值,Pk/k、Pk+1/k和Pk+1/k+1分别为k时刻、k到k+1一步预测时刻和k+1量测更新时刻的误差协方差值,Zk+1表示k+1时刻的***观测量,γk表示k时刻的新息值,Kk+1表示k+1时刻的量测更新过程中的增益值,Qk和Rk+1分别表示k时刻的***噪声方差阵和k+1时刻的量测噪声方差阵。
本发明的目的在于提供一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,对于带双轴或者三轴转位机构的惯导***,一般来说,均具备自标定的功能,本发明在自标定方法的基础上,对***误差模型进行扩维,将加速度计的尺寸误差加入到器件误差模型当中,并通过可观测型分析,在合适的转位路径及转速条件下,可以实现惯性器件误差与加速度计尺寸误差的分离。解决了传统方案中通过随机振动或者角振动实现加速度计尺寸误差分离的复杂过程,大大简化了加速度计尺寸误差的辨识难度。采用本方案的算法,仅需在***进行自标定的同时就可完成加速度计尺寸的误差估计,降低了工程应用中的辨识难度。
本发明针对带双轴或者三轴转位机构的惯导******,采用基于“速度+位置”匹配的卡尔曼滤波技术,解决了因机械结构及安装原因而导致的加速度尺寸与预先设计尺寸存在出入的问题;通过卡尔曼滤波估计的方式精确的完成加速度尺寸的估计,提高三自惯组在振动条件下的导航精度,降低加速度尺寸误差在单轴正反转寻北过程中锯齿速度误差对寻北精度的影响。
为了对本发明有进一步地了解,下面结合图1至图3对本发明的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法进行详细说明。
在该实施例中,预设定***三个加速度计尺寸分别为单位为m。
如图1至图3所示,根据本发明的具体实施例提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该方法具体包括以下步骤。
步骤一,惯导***开机,旋转机构处于锁紧状态,利用角速率和比力信息/>进行粗对准,获取惯导初始姿态,***解锁。
步骤二,根据陀螺角速率和比力信息进行惯性导航解算以完成姿态四元数、位置和速度的更新。
步骤三,转位机构开始按照表1设定的转位路径以及预设转动速度运行,测量转动过程中的陀螺角速率和加速度计比力。
步骤四,根据建立***误差模型,并将加速度计尺寸加入***误差模型中,状态变量为
步骤五,初始化卡尔曼滤波器参数。
步骤六,根据转动过程中陀螺角速率、加速度计比力以及***误差模型采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计以完成基于卡尔曼滤波的三自惯组加速度计尺寸估计。
在整个转动周期停止后,输出惯导***器件误差及加速度计尺寸。按以上步骤进行加速度计的尺寸误差估计,其估计尺寸结果与设定尺寸对比如表2所示。
表2加速度计尺寸估计结果统计(单位:m)
由理论仿真分析可知,表2中的加速度计尺寸估计精度已达到0.00001,加速度计尺寸设定值与尺寸估计值基本无区别;由于本方法相对于传统的加速度计尺寸误差估计,在标定同时对惯导器件误差进行了估计,加速度计尺寸误差与惯导***器件误差进行了分离,不同与传统的利用最小二乘的方式通过随机振动或者角振动的方式进行辨识,提高了加速度尺寸误差的辨识精度。
本发明中所采用的坐标系定义如下:
s系:测量坐标系(oxsyszs),测量坐标系与惯性测量组合固联,坐标系方向与IMU惯性测量单元/惯性器件三个敏感轴方向一致。
综上所述,本发明提供了一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,该方法通过将加速度计尺寸误差加入到***误差模型中,在合适的转位路径下实现了惯性器件误差与加速度计尺寸误差的分离,简化了加速度计尺寸误差的辨识难度,提高了惯导***的导航精度。与现有技术相比,本发明的技术方案能够解决现有技术中惯性器件误差与加速度计尺寸误差耦合,无法精确分离导致影响导航精度的技术问题。
为了便于描述,在这里可以使用空间相对术语,如“在……之上”、“在……上方”、“在……上表面”、“上面的”等,用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是,空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如,如果附图中的器件被倒置,则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而,示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位),并且对这里所使用的空间相对描述作出相应解释。
此外,需要说明的是,使用“第一”、“第二”等词语来限定零部件,仅仅是为了便于对相应零部件进行区别,如没有另行声明,上述词语并没有特殊含义,因此不能理解为对本发明保护范围的限制。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,所述基于卡尔曼滤波的三自惯组加速度计尺寸估计方法包括:
对惯导***进行粗对准以获取惯导初始姿态;
根据陀螺角速率和比力信息进行惯性导航解算以完成姿态四元数、位置和速度的更新;
惯导***的内框和外框按照预设转动次序和转动角度进行转动,获取转动过程中陀螺角速率和加速度计比力;
根据惯导器件误差、加速度计尺寸和惯性导航解算结果构建***误差模型,具体根据构建所述***误差模型;
初始化卡尔曼滤波器参数;
根据转动过程中所述陀螺角速率、所述加速度计比力以及所述***误差模型采用离散卡尔曼滤波方程进行惯性器件误差估计和加速度计尺寸估计以完成基于卡尔曼滤波的三自惯组加速度计尺寸估计;
其中,Xk表示k时刻的状态变量,δL、δh和δλ分别表示惯导***的纬度误差、高度误差和经度误差,δVN、δVU和δVE分别表示惯导***北速度误差、天速度误差和东速度误差,φN、φU和φE分别表示惯导***地理坐标系内北、天和东三个方向的失准角,/>和/>分别表示惯导***测量坐标系内X、Y和Z三个方向的加速度计零位误差,εx、εy和εz分别表示惯导***测量坐标系内X、Y和Z三个方向的陀螺常值漂移,kax、kay和kaz分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的标度因数误差,kayx、kazx和kazy分别表示惯导***测量坐标系内x加速度计相对于Y方向的安装误差、x加速度计相对于Z方向的安装误差和y加速度计相对于Z方向的安装误差,kax2、kay2和kaz2分别表示惯导***测量坐标系内x加速度计、y加速度计和z加速度计的二次项系数误差,kgx、kgy和kgz分别表示惯导***测量坐标系内x陀螺、y陀螺和z陀螺的标度因数误差,kgxy、kgxz、kgyx、kgyz、kgzx和kgzy分别表示惯导***测量坐标系内y陀螺相对于X方向的安装误差、z陀螺相对于X方向的安装误差、x陀螺相对于Y方向的安装误差、z陀螺相对于Y方向的安装误差、x陀螺相对于Z方向的安装误差、y陀螺相对于Z方向的安装误差;Rxx、Rxy和Rxz分别表示在测量坐标系下x加速度计相对旋转中心在X、Y和Z三个方向的坐标值,Ryx、Ryy和Ryz分别表示在测量坐标系下y加速度计相对旋转中心在X、Y和Z三个方向的坐标值,Rzx、Rzy和Rzz分别表示在测量坐标系下z加速度计相对旋转中心在X、Y和Z三个方向的坐标值;Φk.k-1为***k-1时刻到k时刻的状态转移矩阵,Tn为导航周期,Te为滤波周期,At为t时刻的状态转移矩阵,Xk-1为k-1时刻的状态变量,Wk-1表示***k-1时刻的***噪声,Zk为k时刻的***观测量,Hk为k时刻的观测矩阵,/>I为单位矩阵,vk为k时刻的观测噪声。
2.根据权利要求1所述的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,所述惯导***的内框和外框的预设转动次序和转动角度具体为:内框:0°外框:0°、内框:0°外框:90°、内框:0°外框:180°、内框:0°外框:270°、内框:0°外框:180°、内框:0°外框:90°、内框:0°外框:0°、内框:90°外框:0°、内框:90°外框:90°、内框:90°外框:180°、内框:90°外框:270°、内框:90°外框:180°、内框:90°外框:90°、内框:90°外框:0°、内框:180°外框:0°、内框:270°外框:0°、内框:180°外框:0°、内框:90°外框:0°和内框:0°外框:0°。
3.根据权利要求1所述的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,
ωie为地球自转角速率,VN、VU和VE分别为惯导北向、天向和东向速度,L和h分别为惯导纬度和高度,RM和RN分别为地球子午面和卯酉面半径,/>为b系到n系的姿态转换矩阵,/>和/>分别为惯导体坐标系下x陀螺、y陀螺和z陀螺的陀螺输出角速率,/>和/>为惯导体坐标系下x加速度计、y加速度计和z加速度计的加表输出比力,/>和/>为陀螺输出角速率通过微分得到的x陀螺、y陀螺和z陀螺的角加速度,fE、fU和fN分别为比力在地理系东向、天向、北向的投影。
4.根据权利要求1所述的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,根据Zk=[L-L0 h-h0 λ-λ0 VN VU VE]T获取所述***观测量Zk,其中,λ为惯导经度,L0、h0和λ0分别为初始装订纬度、高度和经度。
5.根据权利要求1所述的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,初始化卡尔曼滤波器参数具体包括:设定卡尔曼滤波的初始协方差矩阵P0、***噪声方差阵Q、***误差状态初值X0和卡尔曼滤波计算周期Tk。
6.根据权利要求1至5中任一项所述的基于卡尔曼滤波的三自惯组加速度计尺寸估计方法,其特征在于,根据Pk+1/k=Φk+1,kPk/kΦT k+1,k+Qk,Kk+1=Pk+1/kHT k+1[Hk+1Pk+1/kHT k+1+Rk+1]-1,Pk+1/k+1=[I-Kk+1Hk+1]Pk+1/k,进行卡尔曼滤波,其中,Φk+1,k为k时刻到k+1时刻的状态转移矩阵,和/>分别为k时刻***状态量估计值、k到k+1时刻***状态量一步预测估计值和k+1时刻***状态量量测更新后估计值,Pk/k、Pk+1/k和Pk+1/k+1分别为k时刻、k到k+1一步预测时刻和k+1量测更新时刻的误差协方差值,Zk+1表示k+1时刻的***观测量,γk表示k时刻的新息值,Kk+1表示k+1时刻的量测更新过程中的增益值,Qk和Rk+1分别表示k时刻的***噪声方差阵和k+1时刻的量测噪声方差阵,Hk+1为k+1时刻的观测矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110650825.8A CN113503895B (zh) | 2021-06-10 | 2021-06-10 | 基于卡尔曼滤波的三自惯组加速度计尺寸估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110650825.8A CN113503895B (zh) | 2021-06-10 | 2021-06-10 | 基于卡尔曼滤波的三自惯组加速度计尺寸估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113503895A CN113503895A (zh) | 2021-10-15 |
CN113503895B true CN113503895B (zh) | 2023-08-15 |
Family
ID=78009862
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110650825.8A Active CN113503895B (zh) | 2021-06-10 | 2021-06-10 | 基于卡尔曼滤波的三自惯组加速度计尺寸估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113503895B (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104697553A (zh) * | 2015-03-13 | 2015-06-10 | 哈尔滨工程大学 | 光纤陀螺捷联惯导***加速度计内杆臂标定方法 |
CN106482746A (zh) * | 2016-11-25 | 2017-03-08 | 北京航空航天大学 | 一种用于混合式惯导***的加速度计内杆臂标定与补偿方法 |
CN107830858A (zh) * | 2017-09-30 | 2018-03-23 | 南京航空航天大学 | 一种基于重力辅助的手机航向估计方法 |
AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
-
2021
- 2021-06-10 CN CN202110650825.8A patent/CN113503895B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104697553A (zh) * | 2015-03-13 | 2015-06-10 | 哈尔滨工程大学 | 光纤陀螺捷联惯导***加速度计内杆臂标定方法 |
CN106482746A (zh) * | 2016-11-25 | 2017-03-08 | 北京航空航天大学 | 一种用于混合式惯导***的加速度计内杆臂标定与补偿方法 |
CN107830858A (zh) * | 2017-09-30 | 2018-03-23 | 南京航空航天大学 | 一种基于重力辅助的手机航向估计方法 |
AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
Non-Patent Citations (1)
Title |
---|
游金川 ; 秦永元 ; 杨鹏翔 ; 严恭敏.捷联惯导加速度计尺寸效应误差建模及其标定.宇航学报.2012,第33卷(第03期),311-317. * |
Also Published As
Publication number | Publication date |
---|---|
CN113503895A (zh) | 2021-10-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
CN109459044B (zh) | 一种gnss双天线辅助的车载mems惯导组合导航方法 | |
CN108051866B (zh) | 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法 | |
CN107270893B (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
CN101413800B (zh) | 导航/稳瞄一体化***的导航、稳瞄方法 | |
CN103808331B (zh) | 一种mems三轴陀螺仪误差标定方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN111121766B (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
CN106969783B (zh) | 一种基于光纤陀螺惯性导航的单轴旋转快速标定技术 | |
CN106482746B (zh) | 一种用于混合式惯导***的加速度计内杆臂标定与补偿方法 | |
CN110887507B (zh) | 一种快速估计惯性测量单元全部零偏的方法 | |
CN112595350B (zh) | 一种惯导***自动标定方法及终端 | |
CN104880189B (zh) | 一种动中通天线低成本跟踪抗干扰方法 | |
CN112504275B (zh) | 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法 | |
CN113503894B (zh) | 基于陀螺基准坐标系的惯导***误差标定方法 | |
CN114858189B (zh) | 捷联惯导***陀螺漂移等效补偿方法 | |
CN108627152B (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
CN112857398B (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
CN104833375B (zh) | 一种借助星敏感器的imu两位置对准方法 | |
CN111486870B (zh) | 一种斜置捷联惯组***级标定方法 | |
CN112902956A (zh) | 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质 | |
CN112880669A (zh) | 一种航天器星光折射和单轴旋转调制惯性组合导航方法 | |
CN110849360A (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
Carratù et al. | IMU self-alignment in suspensions control system | |
CN113503895B (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 |