CN113916222B - 基于卡尔曼滤波估计方差约束的组合导航方法 - Google Patents

基于卡尔曼滤波估计方差约束的组合导航方法 Download PDF

Info

Publication number
CN113916222B
CN113916222B CN202111078675.4A CN202111078675A CN113916222B CN 113916222 B CN113916222 B CN 113916222B CN 202111078675 A CN202111078675 A CN 202111078675A CN 113916222 B CN113916222 B CN 113916222B
Authority
CN
China
Prior art keywords
value
error
kalman filtering
moment
navigation
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
Application number
CN202111078675.4A
Other languages
English (en)
Other versions
CN113916222A (zh
Inventor
胡奇林
吴亮华
刘玉祝
于杏
李瑞贤
高晓波
危志英
王秋帆
杨槊
盛雪莲
王欣
张吉
张泽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN202111078675.4A priority Critical patent/CN113916222B/zh
Publication of CN113916222A publication Critical patent/CN113916222A/zh
Application granted granted Critical
Publication of CN113916222B publication Critical patent/CN113916222B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。应用本发明的技术方案，能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。

Description

基于卡尔曼滤波估计方差约束的组合导航方法
技术领域
本发明涉及惯性导航技术领域，尤其涉及一种基于卡尔曼滤波估计方差约束的组合导航方法。
背景技术
在组合导航系统的使用过程中，在天向通道上由于存在重力，进行卡尔曼滤波器估计时，在系统进行长时间组合导航经常会出现高度通道与卫星高度之间存在一个缓慢增大的偏差，这对高精度定点打击或者精密无人机悬停的影响很大。
针对卡尔曼滤波过程进行分析总结，发现系统的某一个状态量观测性极强的情况下，卡尔曼滤波器的方差会快速的收敛，而系统在后续的组合导航过程中，加速度计零位或者标定因素发生变化时，滤波器无法再进行跟踪，导致组合导航的天向通道出现偏差，具体表现为长时间组合导航过程中高度与卫星高度慢慢出现偏差值；当系统长时间不进行卡尔曼滤波的量测更新时，由于天向通道及其容易发散，卡尔曼滤波过程中对应高度与天速状态量的方差变得极大，比如高度发散到10000000m，而陀螺漂移设定值为0.01°/h，在初期滤波过程中收敛至0.001°/h，转换为标准单位4.85e-9rad/s，当系统再次具备量测更新的条件时，在解算运算过程中，由于计算机的精度有限，容易出现“大数吃小数”的现象，导致滤波出现异常。
发明内容
本发明旨在至少解决现有技术中存在的技术问题之一。
本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。
进一步地，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值包括：根据惯性器件误差设定卡尔曼滤波器的误差协方差Pk+1/k+1的对角线元素的最大值PMax和最小值Pmin，其中，最大值PMax和最小值Pmin均为一维数组，维数与误差协方差Pk+1/k+1的对角线元素个数以及状态量的维数相同，最大值PMax和最小值Pmin中的各元素与状态量/>中的各变量一一对应，最大值PMax中的各元素均为元素最大值，最小值Pmin中的各元素均为元素最小值；在进行卡尔曼滤波计算的量测更新过程中，若误差协方差Pk+1/k+1的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k+1的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；在进行卡尔曼滤波计算的时间更新过程中，若误差协方差Pk+1/k的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；其中，Pk+1/k和Pk+1/k+1分别为k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，/>为k时刻系统状态量估计值，k为计算时刻，k＝1,2,...n，n为整数。
进一步地，基于卡尔曼滤波估计方差约束的组合导航方法可根据构建系统误差模型，其中，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Qk，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为Rk，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量/>δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δVN、δVU、δVE分别为惯导北、天、东速度误差，φN、φU、φE分别为惯导系统地理坐标系内北、天、东三个方向的失准角，和/>分别为x加速度计、y加速度计和z加速度计的加速度计零偏，εx、εy和εz分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。
进一步地，设定卡尔曼滤波器初值包括：设定卡尔曼滤波的初始协方差矩阵P0、系统噪声方差阵、系统误差状态初值、量测噪声方差阵和卡尔曼滤波计算周期。
进一步地，卡尔曼滤波计算中时间更新公式为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，/>不进行量测更新时Kk+1＝0，其中，Φk+1,k为k时刻到k+1时刻的状态转移矩阵，/> 分别为k时刻系统状态量估计值、k到k+1时刻系统状态量一步预测估计值、k+1时刻系统状态量量测更新后估计值，Pk/k为k时刻的误差协方差值，Zk+1表示k+1时刻系统的量测量，γk表示新息值，Kk+1表示量测更新过程中的增益值，Qk、Rk+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。
应用本发明的技术方案，提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。
附图说明
所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出了根据本发明的具体实施例提供的基于卡尔曼滤波估计方差约束的组合导航方法的流程示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
如图1所示，根据本发明的具体实施例提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法包括：对惯导系统进行粗对准；根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。
应用此种配置方式，提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。
进一步地，在本发明中，为了实现基于卡尔曼滤波估计方差约束的组合导航，首先对惯导系统进行粗对准。作为本发明的一个具体实施例，在惯导系统开机后，利用陀螺角速率和加速度计比力/>进行粗对准，确定初始姿态。
此外，在本发明中，在粗对准结束后，根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度。
进一步地，在本发明中，在完成惯性导航解算后，建立系统误差模型。作为本发明的一个具体实施例，基于卡尔曼滤波估计方差约束的组合导航方法可根据构建系统误差模型，其中，k为计算时刻，k＝1,2,...n，n为整数，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Qk，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为Rk，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δVN、δVU、δVE分别为惯导北、天、东速度误差，φN、φU、φE分别为惯导系统地理坐标系内北、天、东三个方向的失准角。/>和/>分别为x加速度计、y加速度计和z加速度计的加速度计零偏，εx、εy和εz分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。在该实施例中，可根据实际情况设定计算时刻k的周期，例如可设定为1.0s。在本发明中，根据导航计算结果和系统误差模型解算系统状态转移矩阵。
此外，在本发明中，在建立系统误差模型后，选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值。作为本发明的一个具体实施例，在本发明中，选取惯导位置和速度为卡尔曼滤波的量测量，设定卡尔曼滤波的初始协方差矩阵P0，其参数可根据初始误差的量级进行设置；设定系统噪声方差阵Q，可根据实际系统噪声大小进行设置；设定系统误差状态初值X0，维数为15×1，一般设为零矩阵；设定量测噪声方差阵R；设定卡尔曼滤波计算周期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，/>不进行量测更新时Kk+1＝0，其中，Φ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表示新息值，Kk+1表示量测更新过程中的增益值，Qk、Rk+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。通过上述卡尔曼滤波计算能够获取组合导航数据直至组合导航任务结束。
此外，在本发明中，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值具体包括：
根据惯性器件误差设定卡尔曼滤波器的误差协方差Pk+1/k+1的对角线元素的最大值PMax和最小值Pmin，其中，最大值PMax和最小值Pmin均为一维数组，维数与误差协方差Pk+1/k+1的对角线元素个数以及状态量的维数相同，最大值PMax和最小值Pmin中的各元素与状态量/>中的各变量一一对应，最大值PMax中的各元素均为元素最大值，最小值Pmin中的各元素均为元素最小值；
在进行卡尔曼滤波计算的量测更新过程中，若误差协方差Pk+1/k+1的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k+1的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；
在进行卡尔曼滤波计算的时间更新过程中，若误差协方差Pk+1/k的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最大值PMax中对应的元素最大值。
通过上述约束方法可将卡尔曼滤波器的误差协方差的取值限定在一定范围内，能够明显提高长时间组合导航的精度，尤其是可观测性极强的高度通道，组合导航的高度不会与基准高度存在固定偏差。
本发明针对我国组合导航系统的发展需求，在现阶段对初始对准方法认识的基础上，分析系统卡尔曼滤波模型中不同观测量对滤波估计的影响程度，对惯组误差参数的种类及特性进行更加准确的分析与建模，同时针对卡尔曼滤波过程进行分析总结，提出了一种基于卡尔曼滤波估计方差约束的组合导航方法。
本发明的基于卡尔曼滤波估计方差约束的组合导航方法适用于长时间工作的组合导航系统，能够在消除在长时间组合导航过程中由于前期可观测性强的状态量在后期的组合导航过程中，状态量因环境等原因发生变化时滤波器无法跟踪的情况，提升组合导航性能，同时能够避免在受外界量测信息的影响而一直工作在时间更新状态一段时间后再次进行量测更新时出现滤波发散的问题，进一步提升了滤波器的稳定性；解决了传统对准方案的长时间组合导航精度受限及可能出现滤波发散的问题。
本发明的基于卡尔曼滤波估计方差约束的组合导航方法通过对组合导航滤波过程中的误差协方差的上下限进行约束，在现有组合导航技术的基础上进一步提升了长时间组合导航的精度，能够防止长时间工作条件下的估计效果明显下降，提高系统模型的可观测性，能够对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高了导航系统的组合性能。
为了对本发明有进一步地了解，下面对本发明的一个具体实施例进行详细说明。
在该具体实施例中，以系统静基座对准为例，进行长时间组合导航，组合导航30min后，人为的增加天向加速度计的误差系统，模拟弹用或者机载惯导系统在一段时间组合导航后受环境温度或者飞行高度的影响而产生的加速度计零位、刻度系数差产生影响的情况，状态如下所示：系统北天东放置，即滚动角、方位角、俯仰角均为0°，设定粗对准后系统导航坐标系与地理坐标系之间失准角均为1°，惯导系统陀螺常值漂移0.01°/h,陀螺随机游走系数天向加速度计常值零位100ug，工作0.5h后，后续的1.5h中天向加速度计零位由100ug变化到200ug,当地纬度39.8133°，系统在组合导航2h，卡尔曼滤波周期为1s。
基于卡尔曼滤波估计方差约束的组合导航方法包括以下步骤。
步骤一，惯导系统开机，利用陀螺角速率和加速度计比力/>进行粗对准，确定初始姿态。
步骤二，粗对准结束后，根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度。
步骤三，根据构建系统误差模型。
步骤四，选取惯导位置和速度为卡尔曼滤波的量测量，设定卡尔曼滤波的初始协方差矩阵P0、系统噪声方差阵Q、系统误差状态初值X0、量测噪声方差阵R和卡尔曼滤波计算周期Tk。
其中，初始协方差矩阵P0参考值可设定为
其中，diag表示误差协方差Pk+1/k+1的对角线元素合集，RM和RN分别为地球子午面和卯酉面半径，μg为0.00000980147m/s2。
初始协方差矩阵P0最大值限幅设定为
最小值限幅设定为/>
上述参数设定仅供参考，不同系统需按照应用需求及产品精度进行综合设计。
步骤五，根据时间更新公式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，/>不进行量测更新时Kk+1＝0进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束。
在卡尔曼滤波计算过程中，根据惯性器件误差设定卡尔曼滤波器的误差协方差Pk+1/k+1的对角线元素的最大值PMax和最小值Pmin，其中，最大值PMax和最小值Pmin均为一维数组，维数与误差协方差Pk+1/k+1的对角线元素个数以及状态量的维数相同，最大值PMax和最小值Pmin中的各元素与状态量/>中的各变量一一对应，最大值PMax中的各元素均为元素最大值，最小值Pmin中的各元素均为元素最小值；
在进行卡尔曼滤波计算的量测更新过程中，若误差协方差Pk+1/k+1的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k+1的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；
在进行卡尔曼滤波计算的时间更新过程中，若误差协方差Pk+1/k的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最大值PMax中对应的元素最大值。
按以上步骤组合导航2h，其组合导航误差结果如下表1中所示。
表1组合导航过程中位置误差最大值
由结果可以看出，在未进行方差约束时，由于初始0.5h组合导航过程中，可观测性强的天向加速度计P已经快速收敛，在后续1.5h过程中随着加速度计零位的变化，卡尔曼滤波估计的效果变弱，导致天向通道(即高度方向)出现常值误差。采用本方法后，天向通道能够正常估计，高度跟踪良好。本发明能够消除在长时间组合导航过程中由于前期可观测性强的状态量在后期的组合导航过程中，状态量因环境等原因发生变化时滤波器无法跟踪的情况，提升组合导航性能，同时能够避免在受外界量测信息的影响而一直工作在时间更新状态一段时间后再次进行量测更新时出现滤波发散的问题，进一步提升了滤波器的稳定性；解决了传统对准方案的长时间组合导航精度受限及可能出现滤波发散的问题。
综上所述，本发明提供了一种基于卡尔曼滤波估计方差约束的组合导航方法，该基于卡尔曼滤波估计方差约束的组合导航方法通过在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，能够防止长时间工作条件下的滤波估计效果明显下降的缺陷，提高系统模型的可观测性，对系统变化环境条件下加表零位与陀螺漂移进行精确估计，提高系统的组合性能。与现有技术相比，本发明的技术方案能够解决现有技术组合导航过程中滤波器无法跟踪，使得导航精度受限，以及长时间组合导航存在滤波发散的技术问题。
为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。
此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，所述基于卡尔曼滤波估计方差约束的组合导航方法包括：
对惯导系统进行粗对准；
根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；
选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；
进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值，具体包括：
根据惯性器件误差设定卡尔曼滤波器的误差协方差Pk+1/k+1的对角线元素的最大值PMax和最小值Pmin，其中，最大值PMax和最小值Pmin均为一维数组，维数与误差协方差Pk+1/k+1的对角线元素个数以及状态量的维数相同，最大值PMax和最小值Pmin中的各元素与状态量中的各变量一一对应，最大值PMax中的各元素均为元素最大值，最小值Pmin中的各元素均为元素最小值；
在进行卡尔曼滤波计算的量测更新过程中，若误差协方差Pk+1/k+1的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k+1的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k+1的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；
在进行卡尔曼滤波计算的时间更新过程中，若误差协方差Pk+1/k的对角线元素中的任一元素小于最小值Pmin中对应的元素最小值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最小值Pmin中对应的元素最小值；若误差协方差Pk+1/k的对角线元素中的任一元素大于最大值PMax中对应的元素最大值，则将当前误差协方差Pk+1/k的对角线元素中的该元素设置为最大值PMax中对应的元素最大值；
其中，Pk+1/k和Pk+1/k+1分别为k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，为k时刻系统状态量估计值，k为计算时刻，k＝1,2,...n，n为整数。
2.根据权利要求1所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，基于卡尔曼滤波估计方差约束的组合导航方法根据构建系统误差模型，其中，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Qk，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为Rk，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量/>δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δVN、δVU、δVE分别为惯导北、天、东速度误差，φN、φU、φE分别为惯导系统地理坐标系内北、天、东三个方向的失准角，和/>分别为x加速度计、y加速度计和z加速度计的加速度计零偏，εx、εy和εz分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。
3.根据权利要求1所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，设定卡尔曼滤波器初值包括：设定卡尔曼滤波的初始协方差矩阵P0、系统噪声方差阵、系统误差状态初值、量测噪声方差阵和卡尔曼滤波计算周期。
4.根据权利要求1至3中任一项所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，卡尔曼滤波计算中时间更新公式为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，/>不进行量测更新时Kk+1＝0，其中，Φk+1,k为k时刻到k+1时刻的状态转移矩阵，/> 分别为k时刻系统状态量估计值、k到k+1时刻系统状态量一步预测估计值、k+1时刻系统状态量量测更新后估计值，Pk/k为k时刻的误差协方差值，Zk+1表示k+1时刻系统的量测量，γk表示新息值，Kk+1表示量测更新过程中的增益值，Qk、Rk+1分别表示k时刻系统噪声方差阵和k+1时刻量测噪声方差阵。
CN202111078675.4A 2021-09-15 2021-09-15 基于卡尔曼滤波估计方差约束的组合导航方法 Active CN113916222B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111078675.4A CN113916222B (zh) 2021-09-15 2021-09-15 基于卡尔曼滤波估计方差约束的组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111078675.4A CN113916222B (zh) 2021-09-15 2021-09-15 基于卡尔曼滤波估计方差约束的组合导航方法

Publications (2)

Publication Number Publication Date
CN113916222A CN113916222A (zh) 2022-01-11
CN113916222B true CN113916222B (zh) 2023-10-13

Family

ID=79234946

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111078675.4A Active CN113916222B (zh) 2021-09-15 2021-09-15 基于卡尔曼滤波估计方差约束的组合导航方法

Country Status (1)

Country Link
CN (1) CN113916222B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114993296B (zh) * 2022-04-19 2024-03-15 北京自动化控制设备研究所 一种制导炮弹高动态组合导航方法
CN114963873B (zh) * 2022-04-25 2023-07-14 北京自动化控制设备研究所 一种基于加速度信息的旋转弹快速对准方法
CN117191049A (zh) * 2023-08-10 2023-12-08 北京航空航天大学 一种航空导航网络机载定位方法及系统
CN117390498B (zh) * 2023-12-12 2024-04-30 四川腾盾科技有限公司 一种基于Transformer模型的固定翼集群无人机飞行能力评估方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001264105A (ja) * 2000-03-22 2001-09-26 Toshiba Corp 慣性航法装置、慣性航法装置の初期化方法及び記録媒体
JP2011227017A (ja) * 2010-04-23 2011-11-10 Univ Of Tokyo 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN110006423A (zh) * 2019-04-04 2019-07-12 北京理工大学 一种自适应惯导和视觉组合导航方法
CN110455287A (zh) * 2019-07-24 2019-11-15 南京理工大学 自适应无迹卡尔曼粒子滤波方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109253726B (zh) * 2018-06-22 2020-05-05 东南大学 一种水下滑翔器导航定位系统及上浮精度校正方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001264105A (ja) * 2000-03-22 2001-09-26 Toshiba Corp 慣性航法装置、慣性航法装置の初期化方法及び記録媒体
JP2011227017A (ja) * 2010-04-23 2011-11-10 Univ Of Tokyo 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN110006423A (zh) * 2019-04-04 2019-07-12 北京理工大学 一种自适应惯导和视觉组合导航方法
CN110455287A (zh) * 2019-07-24 2019-11-15 南京理工大学 自适应无迹卡尔曼粒子滤波方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
多站无源跟踪边缘化卡尔曼滤波算法;徐征;曲长文;王昌海;;信号处理(第08期);949-955 *

Also Published As

Publication number Publication date
CN113916222A (zh) 2022-01-11

Similar Documents

Publication Publication Date Title
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN113916222B (zh) 基于卡尔曼滤波估计方差约束的组合导航方法
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN105737828B (zh) 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN103913181B (zh) 一种基于参数辨识的机载分布式pos传递对准方法
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN112595350B (zh) 一种惯导系统自动标定方法及终端
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN102680004A (zh) 一种挠性陀螺位置姿态测量系统pos的标度因数误差标定与补偿方法
CN104374388A (zh) 一种基于偏振光传感器的航姿测定方法
Leutenegger et al. A low-cost and fail-safe inertial navigation system for airplanes
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN105091907A (zh) Sins/dvl组合中dvl方位安装误差估计方法
Ayres-Sampaio et al. A comparison between three IMUs for strapdown airborne gravimetry
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN114877915B (zh) 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
Condomines Nonlinear Kalman Filter for Multi-Sensor Navigation of Unmanned Aerial Vehicles: Application to Guidance and Navigation of Unmanned Aerial Vehicles Flying in a Complex Environment
CN105466423A (zh) 一种无人机导航系统及其运行方法
CN107702718B (zh) 一种基于瞬间可观测度模型的机载pos机动优化方法与装置
CN113465570B (zh) 一种基于高精度imu的气浮台初始对准方法及系统
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN110110347B (zh) 一种基于点质量法的航空重力矢量向下延拓方法及系统
Wagner GNSS/INS integration: still an attractive candidate for automatic landing systems?

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