CN113916220B - 一种具有协方差反馈控制的动态自适应导航定位方法 - Google Patents

一种具有协方差反馈控制的动态自适应导航定位方法 Download PDF

Info

Publication number
CN113916220B
CN113916220B CN202111007377.6A CN202111007377A CN113916220B CN 113916220 B CN113916220 B CN 113916220B CN 202111007377 A CN202111007377 A CN 202111007377A CN 113916220 B CN113916220 B CN 113916220B
Authority
CN
China
Prior art keywords
error
state
navigation system
inertial
beidou
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
CN202111007377.6A
Other languages
English (en)
Other versions
CN113916220A (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202111007377.6A priority Critical patent/CN113916220B/zh
Publication of CN113916220A publication Critical patent/CN113916220A/zh
Application granted granted Critical
Publication of CN113916220B publication Critical patent/CN113916220B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • 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
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Medical Informatics (AREA)
  • Evolutionary Computation (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种具有协方差反馈控制的动态自适应导航定位方法，涉及导航定位技术领域，将系统后验状态及其误差协方差信息反馈至滤波过程中，构成CKF协方差传播的闭环结构；基于极大似然准则，利用估计窗口内的反馈状态及其误差协方差信息，建立了一种先验状态误差协方差在线反馈控制策略。提出的方法在调整过程中能够保证先验状态误差协方差的正定性，克服了传统噪声统计估计方法需要对负定结果进行额外修正的缺陷，从而有效抑制了过程噪声方差不确定性对滤波解的影响，提高了CKF用于无人机INS/BDS组合导航解算时的动态自适应能力。

Description

一种具有协方差反馈控制的动态自适应导航定位方法
技术领域
本发明涉及导航定位技术领域，特别涉及一种具有协方差反馈控制的动态自适应导航定位方法。
背景技术
惯性导航/北斗卫星导航系统(INS/BDS)组合已成为无人机应用中最常用的导航策略。然而，其非线性本质以及动态属性严重限制了无人机在复杂任务环境下的应用。
信息融合是实现组合导航的关键技术，由于组合导航系统的非线性本质，线性卡尔曼滤波KF已很难适用，特别是在复杂应用环境中。扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF是常用的非线性滤波器，然而，由于系统模型的线性化，EKF对于强非线性系统的估计精度较差，UKF在无迹变换中权值会出现负值，导致滤波结果不稳定，特别是对于3维以上的高维非线性系统，例如INS/BDS组合导航系统。
容积卡尔曼滤波CKF是一种新兴的非线性高斯滤波策略，其利用球面径向容积法则逼近贝叶斯后验概率密度函数的数值积分，使得状态的后验均值和协方差至少具有二阶近似精度。在初始误差大、可观测性弱的情况下，CKF比EKF具有更好的性能。此外，现有技术已经证实，对于高维滤波问题，CKF相比UKF具有更好的稳定性和精度。CKF的计算负担也小于UKF，因为它包含2n个容积点，而UKF包含2n+1个sigma点，从而提高了系统的计算性能。因此，对于系统维数至少为15维的INS/BDS系统，CKF是一个更优的滤波选择。
然而，KF框架中的高斯滤波器有一个共同的局限性，其滤波解算严重依赖于系统噪声统计参数的精确性，CKF也是如此。如果在滤波过程中使用有偏的系统噪声统计，滤波结果会导致较大的估计误差，甚至发散。因此，可以确定，系统噪声的统计特性是提高INS/BDS组合导航定位精度的关键。在INS/BDS组合导航系统中，量测噪声方差R与BDS接收机的导航误差有关。借助先进的北斗导航技术，使用高精度的BDS观测设备或大量冗余的观测数据可以保证R的精度。然而，由于状态模型的噪声统计量总是与系统动力学具有强耦合的关系，所以很难获取过程噪声方差矩阵Q的精确信息。因此，如何抑制有偏过程噪声方差对INS/BDS组合系统解算精度的影响，提高系统的动态自适应能力是十分必要的。
针对INS/BDS组合系统过程噪声方差Q具有不确定性，本发明提出了一种具有协方差反馈控制的动态自适应导航定位方法，将系统后验状态和误差协方差信息反馈至滤波过程中，基于最大似然准则，利用估计窗口内的反馈状态和误差协方差信息，建立了先验状态误差协方差在线反馈控制策略，以提高无人机INS/BDS组合导航对过程噪声方差的动态自适应能力。
发明内容
本发明的目的在于提供一种具有协方差反馈控制的动态自适应导航定位方法，将系统后验状态和误差协方差信息反馈至滤波过程中，基于最大似然准则，利用估计窗口内的反馈状态和误差协方差信息，建立了先验状态误差协方差在线反馈控制策略，以提高无人机INS/BDS组合导航对过程噪声方差的动态自适应能力。
本发明提供了一种具有协方差反馈控制的动态自适应导航定位方法，包括以下步骤：
获取惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数，组合惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数作为北斗/惯性组合导航系统的状态量，组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型；
采用北斗卫星导航系统和惯性导航系统之间的速度差和位置差，作为北斗/惯性组合导航系统的量测量，根据量测量与状态量之间线性关系，建立北斗/惯性组合导航系统的量测模型；
设置初始状态估计及其误差协方差矩阵对滤波器进行初始化，将北斗/惯性组合导航系统的状态量和量测量输入滤波器，滤波器基于状态模型执行容积卡尔曼滤波进行时间更新，获得北斗/惯性组合导航系统的先验状态及其误差协方差矩阵；
根据先验状态及其误差协方差矩阵，滤波器基于量测模型执行容积卡尔曼滤波进行量测更新，得到北斗/惯性组合导航系统的后验状态及其误差协方差矩阵；
将后验状态及其误差协方差矩阵反馈至滤波器中，滤波器基于极大似然准则，调整先验状态误差协方差矩阵，采用调整后的先验状态误差协方差矩阵重新基于量测模型进行量测更新，获得最终的状态估计及其误差协方差矩阵，修正北斗/惯性组合导航系统的导航误差。
进一步地，所述滤波器基于极大似然准则，调整先验状态误差协方差矩阵，包括：
定义滤波器的新息向量；
基于固定长度估计窗口内的新息向量，建立先验状态误差协方差矩阵的似然函数；
求取似然函数的对数关于先验状态误差协方差矩阵的偏导数，并令其为0，使得似然函数最大化；
估计窗口内的滤波过程达到稳态时，先验状态误差协方差矩阵逐渐收敛，得到先验状态误差协方差矩阵的反馈控制策略。
进一步地，所述惯性导航系统的误差参数包括：姿态误差、速度误差、位置误差；
所述北斗/惯性组合导航系统的状态量包括姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差。
进一步地，所述惯性导航系统的误差参数和陀螺仪与加速度计的器件误差采用四元数描述，表示为：
Figure GDA0003408307330000041
其中，
Figure GDA0003408307330000042
表示加性姿态误差四元数，/>
Figure GDA0003408307330000043
为速度误差，[δL δλ δh]T为位置误差，/>
Figure GDA0003408307330000044
为陀螺仪的常值漂移，
Figure GDA0003408307330000045
为加速度计的零偏。
进一步地，所述组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型，包括：
获取姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差的传播方程；
组合姿态误差的传播方程、速度误差的传播方程、位置误差的传播方程、陀螺的输出误差方程和加速度计的输出误差方程，形成北斗/惯性组合导航系统的状态模型。
进一步地，所述量测量与状态量之间线性关系为：
zk＝Hkxk+vk
其中，
Figure GDA0003408307330000048
为量测噪声矩阵，vv和v0为北斗卫星导航系统的速度误差和位置误差，/>
Figure GDA0003408307330000047
为系统的量测矩阵，其中
Figure GDA0003408307330000051
量测模型中的量测矢量zk用于表征北斗卫星导航系统和惯性导航系统的速度差和位置差，量测矢量zk表示为：
zk＝[vEJ-vEB vXJ-vXB VVJ-vVB λJ-λB LJ-LB hJ-hB]T
其中，[vEJ NNJ vVJ]T和[λJ LJ hy]T为惯性导航系统输出的无人机速度和位置，[vEBvNB vVB]和[λB LB hB]T为北斗卫星导航系统输出的无人机速度和位置。
与现有技术相比，本发明具有如下显著优点：
本发明提出的一种具有协方差反馈控制的动态自适应导航定位方法，能够提高无人机INS/BDS组合导航对过程噪声方差的动态自适应能力。该方法利用系统后验状态及其误差协方差信息，对先验状态误差协方差进行反馈调整控制，有效抑制了过程噪声方差不确定性对滤波解的影响，并保证了调整过程中先验状态误差协方差的正定性，从而提高了CKF用于无人机INS/BDS组合导航解算时的自适应能力，并通过仿真无人机INS/BDS组合导航对该方法的有效性进行评估。
附图说明
图1为本发明实施例提供的协方差反馈控制流程图；
图2为本发明实施例提供的无人机飞行轨迹仿真图；
图3为本发明实施例提供的经典CKF、ASCKF和提出的自适应CKF得到的无人机位置RMSE曲线图；
图4为本发明实施例提供的经典CKF、ASCKF和提出的自适应CKF得到的无人机姿态RMSE曲线图；
图5为本发明实施例提供的无人机在(300s,700s)内的位置RMSE均值比较图；
图6为本发明实施例提供的无人机在(300s,700s)内的姿态RMSE均值比较图。
具体实施方式
下面结合本发明中的附图，对本发明实施例的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
自适应滤波是处理有偏过程噪声统计影响的常用方法，基于不同的自适应策略，如协方差匹配技术、Sage-Husa估计、贝叶斯推断理论等，学者们设计了大量的自适应滤波，以抑制有偏过程噪声方差对系统滤波解算的影响。协方差匹配技术是一种通过保持新息协方差与其理论值一致，从而在线估计过程噪声方差的自适应策略。然而，该方法在估计过程中会产生一定的稳态估计误差，导致滤波精度改善效果不佳。Sage-Husa估计器基于最大后验原理获取过程噪声的统计信息。然而，其不能保证收敛到正确的过程噪声方差，因此可能导致滤波发散。贝叶斯推断理论自适应滤波器是基于时不变动态误差假设下建立的，并且需要大量的计算负荷，因而不适用于INS/BDS组合导航系统。
对于提高CKF滤波器自适应能力的研究，现有技术基于Sage-Husa估计器，建立了一种迭代CKF算法，通过过程噪声统计估计来抑制过程噪声不确定性对滤波解算的影响。然而，该方法中遗忘因子的选取严重依赖于人工经验。通过将极大似然准则与期望最大化方法相结合，现有技术还提出了一种自适应平方根CKF(ASCKF)，用于在线估计和调整过程噪声方差矩阵。然而，该方法的计算量会随着时间的推移而无限增加，不适用于长时间的无人机导航。更重要的是，上述自适应方法不能避免过程噪声方差估计的负定结果，需要额外的措施来确保过程噪声方差估计值的半正定性，这在一定程度上是有效的，但在滤波精度上是折衷的。总体而言，在INS/BDS组合精确导航方面，如何更好地抑制过程噪声方差对CKF解算的影响还缺乏有效的解决方案。
参照图1-6，本发明提供了一种具有协方差反馈控制的动态自适应导航定位方法，包括以下步骤：
利用四元数描述惯性导航系统的误差参数(姿态误差、速度误差、位置误差)和陀螺仪与加速度计的器件误差参数，组合惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数获得北斗/惯性组合导航系统的状态量，根据状态量获得北斗/惯性组合导航系统的状态模型；
采用北斗卫星导航系统和惯性导航系统之间的速度差和位置差，作为北斗/惯性组合导航系统的量测量，根据量测量与状态量之间线性关系，建立北斗/惯性组合导航系统的量测模型；
设置初始状态估计及其误差协方差矩阵对滤波器进行初始化，将北斗/惯性组合导航系统的状态量和量测量输入滤波器，滤波器基于状态模型执行容积卡尔曼滤波进行时间更新，获得北斗/惯性组合导航系统的先验状态及其误差协方差矩阵；
根据先验状态及其误差协方差矩阵，滤波器基于量测模型执行容积卡尔曼滤波进行量测更新，得到北斗/惯性组合导航系统的后验状态及其误差协方差矩阵；
将后验状态及其误差协方差矩阵反馈至滤波器中，滤波器基于极大似然准则，调整先验状态误差协方差矩阵，采用调整后的先验状态误差协方差矩阵重新基于量测模型进行量测更新，获得最终的状态估计及其误差协方差矩阵，修正北斗/惯性组合导航系统的导航误差。
当先验状态误差协方差矩阵得到修正后，有偏过程噪声方差矩阵的影响将会降低，从而导致北斗/惯性组合导航系统的导航误差减小，实现了动态环境下自适应导航定位的功能。
实施例1
所述对基于北斗/惯性组合导航系统状态模型获得的先验状态误差协方差进行自适应反馈调节采用具有协方差反馈控制的容积卡尔曼滤波方法，所述容积卡尔曼滤波方法的步骤包括：
设置滤波器的初始状态估计
Figure GDA0003408307330000081
及其误差协方差矩阵/>
Figure GDA0003408307330000082
Figure GDA0003408307330000083
其中，x0表示状态初值，E[·]表示期望函数；
滤波器执行容积卡尔曼滤波时间更新，得到先验状态估计
Figure GDA0003408307330000084
及其误差协方差矩阵/>
Figure GDA0003408307330000085
Figure GDA0003408307330000086
Figure GDA0003408307330000087
其中，n为系统的状态维数；
滤波器执行容积卡尔曼滤波量测更新，得到北斗/惯性组合导航系统的后验状态估计
Figure GDA0003408307330000088
及其误差协方差矩阵/>
Figure GDA0003408307330000089
Figure GDA00034083073300000810
Figure GDA00034083073300000811
Figure GDA0003408307330000091
Figure GDA0003408307330000092
Figure GDA0003408307330000093
Figure GDA0003408307330000094
其中，
Figure GDA0003408307330000095
和/>
Figure GDA0003408307330000096
表示量测的预测及其误差协方差矩阵，/>
Figure GDA0003408307330000097
表示量测与状态的互协方差矩阵；Kk为滤波器增益矩阵；
将后验状态估计及其误差协方差矩阵反馈至滤波过程中，对先验状态误差协方差矩阵进行反馈调整控制：
Figure GDA0003408307330000098
Figure GDA0003408307330000099
且
Figure GDA00034083073300000910
其中，g为比例系数且满足1≤g＜N0≤N；N0为选取的一个较小的正数；权重
Figure GDA00034083073300000911
满足/>
Figure GDA00034083073300000912
根据调整后的先验状态误差协方差矩阵，滤波器重新进行容积卡尔曼滤波的量测更新，实现对北斗/惯性组合导航系统状态估计的修正。
实施例2
现有技术中，经典CKF算法的计算过程如下所示：
采用改进的Euler方法对INS/BDS(惯性/北斗)组合导航系统的状态模型进行离散化，得到一般形式的非线性离散系统模型：
Figure GDA0003408307330000101
其中，xk∈Rn和zk∈Rn表示在k时刻系统的状态向量和量测向量；wk和vk为不相关的零均值高斯白噪声序列，其方差矩阵为
Figure GDA0003408307330000102
和/>
Figure GDA0003408307330000103
f(·)为描述状态模型的非线性函数，Hk是量测矩阵。
CKF是一种高斯域的贝叶斯滤波器，其通过容积法则近似一个随机变量经过非线性函数传播后的均值和协方差，即：
Figure GDA0003408307330000104
其中，
Figure GDA0003408307330000105
为P的平方根分解，并满足/>
Figure GDA0003408307330000106
2n个容积点集合由|ξi|给出，其中ξi是下列矩阵的第i个列向量：
Figure GDA0003408307330000107
基于容积法则，经典CKF算法的计算过程如下所示：
步骤1：初始化，通过设置初始状态估计及其误差协方差矩阵对滤波器进行初始化：
Figure GDA0003408307330000111
步骤2：时间更新，应用容积法则(22)近似先验状态及其误差协方差：
Figure GDA0003408307330000112
Figure GDA0003408307330000113
其中，容积点
Figure GDA0003408307330000114
通过前一时刻的状态估计/>
Figure GDA0003408307330000115
及其误差协方差矩阵/>
Figure GDA0003408307330000116
生成：
Figure GDA0003408307330000117
Figure GDA0003408307330000118
步骤3：量测更新，由于量测模型为线性模型，则更新过程与卡尔曼滤波一致。
Figure GDA0003408307330000119
Figure GDA00034083073300001110
Figure GDA00034083073300001111
Figure GDA00034083073300001112
Figure GDA00034083073300001113
Figure GDA00034083073300001114
步骤4：返回步骤2，直到处理完所有样本。
实施例3
滤波器基于极大似然准则，调整先验状态误差协方差矩阵，包括：
定义滤波器的新息向量为：
Figure GDA0003408307330000121
令
Figure GDA0003408307330000122
表示窗口N内的一组历史新息序列。然后，基于
Figure GDA0003408307330000123
先验状态误差协方差的似然函数可表示为：
Figure GDA0003408307330000124
式中，
Figure GDA0003408307330000125
表示已知/>
Figure GDA0003408307330000126
时，高斯序列/>
Figure GDA0003408307330000127
的概率密度函数，其可以描述为零均值高斯分布：
Figure GDA0003408307330000128
式中，m为量测向量的维数，|·|表示矩阵的行列式，
Figure GDA0003408307330000129
为新息向量的协方差矩阵，可由(20)获得。
在一个固定长度的估计窗口中，对似然函数(27)的对数求取关于
Figure GDA00034083073300001210
的偏导数，并应用矩阵的导数理论，得出：
Figure GDA00034083073300001211
式中，s和t分别表示矩阵的第s行和第t列。
令
Figure GDA00034083073300001212
得：
Figure GDA00034083073300001213
由式(29)可以看出，上述极大似然估计问题已被转化为计算新息协方差关于
Figure GDA0003408307330000131
的偏导数。
将(20)代入(29)得到：
Figure GDA0003408307330000132
当估计窗口内的滤波过程达到稳态时，
Figure GDA0003408307330000133
将趋于收敛，(30)中的微分项将趋于1，因此可得：
Figure GDA0003408307330000134
对(31)左乘和右乘
Figure GDA0003408307330000135
并使用(20)和(21)中的关系，可以得到
Figure GDA0003408307330000136
由(23)和(24)，得出：
Figure GDA0003408307330000137
Figure GDA0003408307330000138
因此，可以得到：
Figure GDA0003408307330000139
由于滤波器到达稳态时，
Figure GDA00034083073300001310
会趋于收敛，因此可以得到如下先验状态误差协方差矩阵反馈控制策略：
Figure GDA0003408307330000141
进一步地，由于估计窗口N内的所有反馈信息都等效地贡献了对先验状态误差协方差的控制信息，因此所得到的反馈控制可能不能准确地描述真实特征，导致估计结果有偏差或恶化。我们在上述状态误差协方差控制方案中嵌入了一个比例系数g，以生成一组新的权重，放大最新反馈信息的贡献，从而产生一个新的比例反馈控制方案来调节之前的先验状态误差协方差。
Figure GDA0003408307330000142
Figure GDA0003408307330000143
且
Figure GDA0003408307330000144
式中，g为比例系数且满足1≤g＜N0≤N；权重
Figure GDA0003408307330000145
满足
Figure GDA0003408307330000146
实施例4
选用东北天地理坐标系g系作为无人机导航坐标系n系。将机体坐标系表示为b系，惯性坐标系表示为i系，地球坐标系表示为e系。四元数描述惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数表示为：
Figure GDA0003408307330000147
其中，
Figure GDA0003408307330000148
表示加性姿态误差四元数，/>
Figure GDA0003408307330000149
为速度误差，[δL δλ δh]T为位置误差，[δhx δhyδhz]T为陀螺仪的常值漂移，/>
Figure GDA0003408307330000151
为加速度计的零偏。
惯性导航系统的误差参数包括：姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计输出误差；
姿态误差的传播方程为：
Figure GDA0003408307330000152
其中，
Figure GDA0003408307330000153
表示姿态误差四元数，/>
Figure GDA0003408307330000154
表示相应的微分，/>
Figure GDA0003408307330000155
表示姿态四元数，/>
Figure GDA0003408307330000156
表示3个陀螺测量得到的机体角速率，/>
Figure GDA0003408307330000157
表示陀螺误差，
Figure GDA0003408307330000158
表示n系相对于i系的旋转角速率，/>
Figure GDA0003408307330000159
是n系相对于i系的旋转角速率相应的误差，
Figure GDA00034083073300001510
和/>
Figure GDA00034083073300001511
为四元数姿态误差方程的过程参数；
速度误差的传播方程为：
Figure GDA00034083073300001512
其中，
Figure GDA0003408307330000161
表示速度误差，δVn为速度误差的微分，Vn＝[vE vN vU]T表示无人机在东、北、天三个方向的速度，/>
Figure GDA0003408307330000162
表示姿态误差耦合项，具有很强的非线性特征，fn表示加速度计比力量测，δfn表示加速度计误差，/>
Figure GDA0003408307330000163
表示b系到n系的姿态旋转矩阵，/>
Figure GDA0003408307330000164
为地球旋转角速率在n系中的投影，/>
Figure GDA0003408307330000165
表示n系相对于e系的旋转角速率，
Figure GDA0003408307330000166
和/>
Figure GDA0003408307330000167
表示其相应的误差；
位置误差的传播方程为：
Figure GDA0003408307330000168
其中，δPn＝[δL δλ δh]T表示纬度、经度和高度方向上的位置误差，δPh表示位置误差的微分；RM和RN表示子午圈和卯酉圈的曲率半径，L和h表示无人机的纬度和高度；
陀螺的输出误差方程为：
Figure GDA0003408307330000169
Figure GDA0003408307330000171
其中，δh为陀螺的常值漂移，wh是陀螺的白噪声；
加速度计的输出误差方程为：
Figure GDA0003408307330000172
Figure GDA0003408307330000173
其中，
Figure GDA0003408307330000174
为加速度计零偏，wh是加速度计的白噪声。
实施例5
北斗/惯性组合导航系统的状态模型为：
Figure GDA0003408307330000175
其中，
Figure GDA0003408307330000176
为连续形式的非线性系统函数，w(i)为系统过程噪声。
北斗/惯性组合导航系统的量测模型表示为：
zk＝Hkxk+vk (47)
其中，
Figure GDA0003408307330000179
为量测噪声矩阵，vv和v0为北斗卫星导航系统的速度误差和位置误差，/>
Figure GDA0003408307330000178
为系统的量测矩阵，其中/>
Figure GDA00034083073300001710
量测模型中的量测矢量zk用于表征北斗卫星导航系统和惯性导航系统的速度误差和位置误差，量测矢量zk表示为：
zk＝[vEJ-vEB vNJ-vVB vEJ-vVB λJ-λBLJ-LB hJ-hB]T (48)
其中，[vEJ vXJ vVJ]T和[λJ LJ hJ]T为惯性导航系统输出的无人机速度和位置，[vEBvNB vVB]T和[λB LBhB]T为北斗卫星导航系统输出的无人机速度和位置。
实施例6
通过蒙特卡洛仿真评估验证自适应调节的效果，具体包括：
设置包含平飞、匀速、加速、转弯、爬升、下降多运动状态的飞行轨迹图；
设置飞行参数，初始姿态为(0，0，0)，初始速度为(0m/s.150m/s.0m/s)，初始位置为东经108.997、北纬34.246、高度5000m；
对飞行过程进行模拟，验证具有协方差反馈控制的容积卡尔曼滤波方法在过程噪声方差存在偏差或不匹配情形时的滤波效果，得出评估结果。如图2所示，在(300s，700s)内，滤波器使用的过程噪声方差突变为25×Qk-1，即：
Figure GDA0003408307330000181
导航系统仿真参数设置如表1所示，滤波周期1s，蒙特卡洛次数20次。
表1仿真参数设置
Figure GDA0003408307330000182
Figure GDA0003408307330000191
参照图3和图4，其给出了分别采用经典CKF、ASCKF和提出的自适应CKF算法得到的无人机位置和姿态的均方根误差(RMSE)曲线。分析图3和图4得到：
i)在(300s,700s)内，滤波器中使用的过程噪声方差突变，即过程噪声统计存在不确定性。经典CKF由于自适应能力较差，因而估计的导航参数误差最大，明显大于过程噪声统计精确已知的时间段；而ASCKF能够通过噪声统计的在线估计弱化过程噪声统计突变的影响，并提高经典CKF的滤波精度，其估计误差明显小于经典CKF；与上述两种方法相比，提出的自适应CKF通过先验状态误差协方差的自适应反馈控制，可以获得比ASCKF更高的导航精度。这是由于提出的方法有效避免了对过程噪声方差负定估计结果的额外处理，保证了先验状态误差协方差的正定性，从而提高了无人机导航系统的自适应能力和导航精度。
ii)在(0s,300s)和(700s,1000s)内，由于过程噪声方差未出现突变，相比于ASCKF和提出的自适应CKF算法，经典CKF具有最小的估计误差；而ASCKF和提出的自适应CKF由于具有次优性，其估计误差略大于经典CKF。
图5和图6描绘了经典CKF、ASCKF和提出的自适应CKF算法分别在(300s,700s)内得到的无人机位置和姿态的RMSE均值的直观比较。图3和图4中的统计结果同样表明，本发明提出的自适应CKF显著改善了经典CKF的自适应性，从而减小了INS/BDS组合导航系统的导航误差，提高了无人机导航系统的自适应能力。
以上公开的仅为本发明的几个具体实施例，但是，本发明实施例并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (6)

1.一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，包括以下步骤：
获取惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数，组合惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数作为北斗/惯性组合导航系统的状态量，组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型；
采用北斗卫星导航系统和惯性导航系统之间的速度差和位置差，作为北斗/惯性组合导航系统的量测量，根据量测量与状态量之间线性关系，建立北斗/惯性组合导航系统的量测模型；
设置初始状态估计及其误差协方差矩阵对滤波器进行初始化，将北斗/惯性组合导航系统的状态量和量测量输入滤波器，滤波器基于状态模型执行容积卡尔曼滤波进行时间更新，获得北斗/惯性组合导航系统的先验状态及其误差协方差矩阵；
根据先验状态及其误差协方差矩阵，滤波器基于量测模型执行容积卡尔曼滤波进行量测更新，得到北斗/惯性组合导航系统的后验状态及其误差协方差矩阵；
将后验状态及其误差协方差矩阵反馈至滤波器中，滤波器基于极大似然准则，调整先验状态误差协方差矩阵，采用调整后的先验状态误差协方差矩阵重新基于量测模型进行量测更新，获得最终的状态估计及其误差协方差矩阵，修正北斗/惯性组合导航系统的导航误差。
2.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述滤波器基于极大似然准则，调整先验状态误差协方差矩阵，包括：
定义滤波器的新息向量；
基于固定长度估计窗口内的新息向量，建立先验状态误差协方差矩阵的似然函数；
求取似然函数的对数关于先验状态误差协方差矩阵的偏导数，并令其为0，使得似然函数最大化；
估计窗口内的滤波过程达到稳态时，先验状态误差协方差矩阵逐渐收敛，得到先验状态误差协方差矩阵的反馈控制策略。
3.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述惯性导航系统的误差参数包括：姿态误差、速度误差、位置误差；
所述北斗/惯性组合导航系统的状态量包括姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差。
4.如权利要求3的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述惯性导航系统的误差参数和陀螺仪与加速度计的器件误差采用四元数描述，表示为：
Figure FDA0003237468430000021
其中，[δq0 δq1 δq2 δq3]T表示加性姿态误差四元数，[δvE δvN δvU]T为速度误差，[δL δλ δh]T为位置误差，[εbx εby εbz]T为陀螺仪的常值漂移，
Figure FDA0003237468430000022
为加速度计的零偏。
5.如权利要求4的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型，包括：
获取姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差的传播方程；
组合姿态误差的传播方程、速度误差的传播方程、位置误差的传播方程、陀螺的输出误差方程和加速度计的输出误差方程，形成北斗/惯性组合导航系统的状态模型。
6.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述量测量与状态量之间线性关系为：
zk＝Hkxk+vk
其中，zk表示量测量，xk表示状态量，
Figure FDA0003237468430000031
为量测噪声矩阵，vv和vp为北斗卫星导航系统的速度误差和位置误差，/>
Figure FDA0003237468430000032
为系统的量测矩阵，其中Hv＝[03×4 diag(1,1,1) 03×9]、Hp＝[03×7 diag(RM,RNcosL,1) 03×9]；
量测模型中的量测矢量zk用于表征北斗卫星导航系统和惯性导航系统的速度差和位置差，量测矢量zk表示为：
zk＝[vEI-vEB vNI-vNB vUI-vUB λI-λB LI-LB hI-hB]T
其中，[vEI vNI vUI]T和[λI LI hI]T为惯性导航系统输出的无人机速度和位置，[vEB vNBvUB]T和[λB LB hB]T为北斗卫星导航系统输出的无人机速度和位置。
CN202111007377.6A 2021-08-30 2021-08-30 一种具有协方差反馈控制的动态自适应导航定位方法 Active CN113916220B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111007377.6A CN113916220B (zh) 2021-08-30 2021-08-30 一种具有协方差反馈控制的动态自适应导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111007377.6A CN113916220B (zh) 2021-08-30 2021-08-30 一种具有协方差反馈控制的动态自适应导航定位方法

Publications (2)

Publication Number Publication Date
CN113916220A CN113916220A (zh) 2022-01-11
CN113916220B true CN113916220B (zh) 2023-06-23

Family

ID=79233575

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111007377.6A Active CN113916220B (zh) 2021-08-30 2021-08-30 一种具有协方差反馈控制的动态自适应导航定位方法

Country Status (1)

Country Link
CN (1) CN113916220B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107144284A (zh) * 2017-04-18 2017-09-08 东南大学 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
CN110567455A (zh) * 2019-09-25 2019-12-13 哈尔滨工程大学 一种求积更新容积卡尔曼滤波的紧组合导航方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111190207A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于pstcsdref算法的无人机ins bds组合导航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107144284A (zh) * 2017-04-18 2017-09-08 东南大学 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN110567455A (zh) * 2019-09-25 2019-12-13 哈尔滨工程大学 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN111190207A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于pstcsdref算法的无人机ins bds组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
改进的CKF在SINS/GPS组合导航中的应用;黄春梅;王杰;邹慧莹;;长春工业大学学报(第02期);全文 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
CN110109470B (zh) 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
CN109459019B (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN110455287A (zh) 自适应无迹卡尔曼粒子滤波方法
CN104713559B (zh) 一种高精度sins模拟器的设计方法
Liu et al. Variational Bayesian-based robust cubature Kalman filter with application on SINS/GPS integrated navigation system
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
CN110567455A (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN109840517A (zh) 一种mems陀螺噪声估计和滤波方法
Woodbury et al. Adaptive play via estimation in uncertain nonzero-sum orbital pursuit evasion games
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
CN113916220B (zh) 一种具有协方差反馈控制的动态自适应导航定位方法
CN110567490B (zh) 一种大失准角下sins初始对准方法
CN109470251A (zh) 一种用于组合导航系统中的部分反馈滤波方法
CN111290008A (zh) 一种动态自适应扩展卡尔曼滤波容错算法
Ramos et al. Observability informed partial-update schmidt kalman filter
CN113587926B (zh) 一种航天器空间自主交会对接相对导航方法
Chen et al. Dealing with observation outages within navigation data using Gaussian process regression
CN113252039B (zh) 面向地形辅助导航的粒子群快速匹配方法
Gao et al. Improved innovation-based adaptive estimation for measurement noise uncertainty in SINS/GNSS integration system
Xing et al. An adaptive UKF algorithm and its application for satellite attitude determination
Zhao et al. Unscented Kalman filtering for SINS attitude estimation
Basil et al. Adaptive Kalman filter tuning in integration of low-cost MEMS-INS/GPS

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