CN109945859A - 一种自适应h∞滤波的运动学约束捷联惯性导航方法 - Google Patents

一种自适应h∞滤波的运动学约束捷联惯性导航方法 Download PDF

Info

Publication number
CN109945859A
CN109945859A CN201910255304.5A CN201910255304A CN109945859A CN 109945859 A CN109945859 A CN 109945859A CN 201910255304 A CN201910255304 A CN 201910255304A CN 109945859 A CN109945859 A CN 109945859A
Authority
CN
China
Prior art keywords
filtering
inertial navigation
formula
strapdown inertial
state
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
Application number
CN201910255304.5A
Other languages
English (en)
Other versions
CN109945859B (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN201910255304.5A priority Critical patent/CN109945859B/zh
Publication of CN109945859A publication Critical patent/CN109945859A/zh
Application granted granted Critical
Publication of CN109945859B publication Critical patent/CN109945859B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Navigation (AREA)

Abstract

本发明公开了一种自适应H滤波的运动学约束捷联惯性导航方法,主要步骤包括:定义解算所需的坐标系,根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量,建立运动学约束H滤波模型,利用滑动窗口内的滤波新息构造自适应H滤波方法,估计出速度误差并对系统进行修正,完成运动学约束惯性导航。本发明能够在不增加传感器且不影响捷联惯性导航系统隐蔽性等其他优点的基础上,有效提高捷联惯性导航系统导航精度。

Description

一种自适应H∞滤波的运动学约束捷联惯性导航方法
技术领域
本发明属于导航技术领域,特别涉及一种自适应H滤波的运动学约束捷联惯性导航方法。
背景技术
捷联式惯性导航系统具有自主性强、隐蔽性好、体积小、结构简单、易于维护等优点,在航天、车载、航海领域都得到了广泛应用。如何在保留捷联式惯性导航系统优点的基础上,提高导航精度是急需解决的关键问题。
运动学约束惯性导航是一种无需增加传感器,利用自身运动学特征的导航方法,目前运动学约束惯性导航方法都采用了卡尔曼滤波技术,卡尔曼滤波通常需要测量噪声的功率谱密度和精确的系统模型来最小化状态估计误差的方差,但实际系统的噪声统计特性不准确,系统模型也有一定的不确定性,因此在实际系统中卡尔曼滤波的很难达到理想的估计精度。随着H控制理论的出现,不对信号频谱特性做任何假设且具有更优鲁棒性的H滤波方法发展起来,H滤波将H范数引入滤波问题,使构建的滤波器从干扰输入到滤波误差输出的H范数最小化,这种滤波方法能使最坏干扰情况下的估计误差最小,但传统的H滤波性能受限于初始设定的滤波参数。
发明内容
发明目的:针对上述缺陷,本发明提供一种能够在不增加传感器且不影响捷联惯性导航系统隐蔽性等其他优点的基础上,有效提高捷联惯性导航系统导航精度的自适应H滤波的运动学约束捷联惯性导航方法。
技术方案:本发明提出一种自适应H滤波的运动学约束捷联惯性导航方法,包括如下步骤:
(1)定义解算所需的坐标系并选取滤波状态量和量测量;
(2)建立运动学约束H滤波模型;
(3)利用滑动窗口内的滤波新息构造自适应H滤波方法。
进一步的,所述步骤(1)中定义解算所需的坐标系并选取滤波状态量和量测量的具体步骤如下:
坐标系选择如下:
n系—导航坐标系,x轴、y轴和z轴分别指向载体所在地理位置的东北天方向;
b系—载体坐标系,x轴、y轴和z轴分别指向载体的右前上方向;
根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量;选取系统状态量为:
式中:δvE,δvN分别为捷联惯性导航系统东向、北向速度误差;φE,φN,φU分别为系统东向、北向和天向失准角误差;分别为载体坐标系x轴,y轴方向的加速度计常值偏置;εx,εy,εz分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移;[·]T表示矩阵转置;X为系统状态量组成的列向量;
选取系统量测量为:
y=[δvx,δvz]T
式中:δvx,δvz分别为捷联惯性导航系统载体系右向、上向的速度误差;y为系统量测量组成的列向量。
进一步的,所述步骤(2)中建立运动学约束H滤波模型的具体步骤如下:
2.1)状态方程为:
式中:为系统状态向量的微分,F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵,Γ为噪声驱动阵,W为由陀螺和加速度计噪声构成的系统噪声阵;
2.2)量测方程为:
y=HX+V
式中:V为量测噪声向量,H为量测矩阵,形式如下:
式中,Cij(i,j=1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素;vE,vN,vU分别为捷联惯性导航系统东向、北向和天向速度。
进一步的,所述步骤(3)中利用滑动窗口内的滤波新息构造自适应H滤波方法的具体步骤如下:
对步骤(2)中的模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
H滤波估计状态的任意线性组合:
Zk=LkXk
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
式中:γ是调节H滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为下式:
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
自适应H滤波不断进行时间更新和量测更新,估计出捷联惯性导航系统的东向和北向速度误差后,对系统输出的东向和北向速度进行补偿,直至导航结束。
本发明提出一种自适应H滤波的运动学约束捷联惯性导航方法,该方法建立了捷联惯性导航系统运动学约束的H滤波模型,给出了一种自适应调节H滤波参数的方法,将该自适应H滤波方法应用于运动学约束导航算法,该方法能够有效提高导航精度。
本发明采用上述技术方案,具有以下有益效果:
本发明采用自适应H滤波,利用滑动窗口内的新息构造滤波参数调节模型,使得滤波参数能够根据外界干扰或系统状态的变化进行自适应调节,兼顾滤波精度与系统的鲁棒性,能够提高运动学约束惯性导航算法的精度。
附图说明
图1为本发明的流程图;
图2为具体实施例中载体行驶轨迹图;
图3为具体实施例中滤波参数γ变化图;
图4为具体实施例中3种方法定位误差和载体系速度误差对比曲线图。
具体实施方式
下面结合具体实施例,进一步阐明本发明,如图1至图4所示,本发明所述的一种自适应H滤波的运动学约束捷联惯性导航方法,具体步骤如下:
步骤1)定义解算所需的坐标系并选取滤波状态量和量测量;
坐标系选择如下:
n系—导航坐标系,x轴、y轴和z轴分别指向载体所在地理位置的东北天方向;
b系—载体坐标系,x轴、y轴和z轴分别指向载体的右前上方向;
根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量。选取系统状态量为:
式中:δvE,δvN分别为捷联惯性导航系统东向、北向速度误差;φE,φN,φU分别为系统东向、北向和天向失准角误差;分别为载体坐标系x轴,y轴方向的加速度计常值偏置;εx,εy,εz分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移;[·]T表示矩阵转置;X为系统状态量组成的列向量;
选取系统量测量为:
y=[δvx,δvz]T (2)
式中:δvx,δvz分别为捷联惯性导航系统载体系右向、上向的速度误差;y为系统量测量组成的列向量;
步骤2)建立运动学约束H滤波模型;
2.1)状态方程为:
式中:为系统状态向量的微分,F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵,Γ为噪声驱动阵,W为由陀螺和加速度计噪声构成的系统噪声阵;
2.2)量测方程为:
y=HX+V (4)
式中:V为量测噪声向量,H为量测矩阵,形式如下:
式中,Cij(i,j=1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素;vE,vN,vU分别为捷联惯性导航系统东向、北向和天向速度;
步骤3)利用滑动窗口内的滤波新息构造自适应H滤波方法:
对上述模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
H滤波可以估计状态的任意线性组合:
Zk=LkXk (7)
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
式中:γ是调节H滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为式(11):
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
根据式(6)至式(11),自适应H滤波不断进行时间更新和量测更新,估计出捷联惯性导航系统的东向和北向速度误差后,对系统输出的东向和北向速度进行补偿,直至导航结束。
本发明的可行性通过如下半物理仿真加以验证:
(1)陀螺仪随机常值漂移0.01°/h,加速度计常值偏置0.1mg;
(2)载体以10km/h的速度匀速行驶,行驶时长为5h,行驶轨迹如图2所示;
(3)取得陀螺与加速度实测数据后,用3种方法进行半物理仿真导航结果对比,分别是不采用运动学约束导航、传统H滤波运动学约束导航和自适应H滤波运动学约束导航方法。
通过半物理仿真,验证了滤波参数随系统的自适应调节功能(如图3所示),本方法的导航精度较另两种方法有显著提高(如图4所示)。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种自适应H滤波的运动学约束捷联惯性导航方法,其特征在于,包括如下步骤:
(1)定义解算所需的坐标系并选取滤波状态量和量测量;
(2)建立运动学约束H滤波模型;
(3)利用滑动窗口内的滤波新息构造自适应H滤波方法。
2.根据权利要求1所述的一种自适应H滤波的运动学约束捷联惯性导航方法,其特征在于,所述步骤(1)中定义解算所需的坐标系并选取滤波状态量和量测量的具体步骤如下:
坐标系选择如下:
n系—导航坐标系,x轴、y轴和z轴分别指向载体所在地理位置的东北天方向;
b系—载体坐标系,x轴、y轴和z轴分别指向载体的右前上方向;
根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量;选取系统状态量为:
式中:δvE,δvN分别为捷联惯性导航系统东向、北向速度误差;φE,φN,φU分别为系统东向、北向和天向失准角误差;分别为载体坐标系x轴,y轴方向的加速度计常值偏置;εx,εy,εz分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移;[·]T表示矩阵转置;X为系统状态量组成的列向量;
选取系统量测量为:
y=[δvx,δvz]T
式中:δvx,δvz分别为捷联惯性导航系统载体系右向、上向的速度误差;y为系统量测量组成的列向量。
3.根据权利要求1所述的一种自适应H滤波的运动学约束捷联惯性导航方法,其特征在于,所述步骤(2)中建立运动学约束H滤波模型的具体步骤如下:
3.1)状态方程为:
式中:为系统状态向量的微分,F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵,Γ为噪声驱动阵,W为由陀螺和加速度计噪声构成的系统噪声阵;
3.2)量测方程为:
y=HX+V
式中:V为量测噪声向量,H为量测矩阵,形式如下:
式中,Cij(i,j=1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素;vE,vN,vU分别为捷联惯性导航系统东向、北向和天向速度。
4.根据权利要求1所述的一种自适应H滤波的运动学约束捷联惯性导航方法,其特征在于,所述步骤(3)中利用滑动窗口内的滤波新息构造自适应H滤波方法的具体步骤如下:
对步骤(2)中的模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
H滤波估计状态的任意线性组合:
Zk=LkXk
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
式中:γ是调节H滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为下式:
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
自适应H滤波不断进行时间更新和量测更新,估计出捷联惯性导航系统的东向和北向速度误差后,对系统输出的东向和北向速度进行补偿,直至导航结束。
CN201910255304.5A 2019-04-01 2019-04-01 一种自适应h∞滤波的运动学约束捷联惯性导航方法 Active CN109945859B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910255304.5A CN109945859B (zh) 2019-04-01 2019-04-01 一种自适应h∞滤波的运动学约束捷联惯性导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910255304.5A CN109945859B (zh) 2019-04-01 2019-04-01 一种自适应h∞滤波的运动学约束捷联惯性导航方法

Publications (2)

Publication Number Publication Date
CN109945859A true CN109945859A (zh) 2019-06-28
CN109945859B CN109945859B (zh) 2022-12-06

Family

ID=67013318

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910255304.5A Active CN109945859B (zh) 2019-04-01 2019-04-01 一种自适应h∞滤波的运动学约束捷联惯性导航方法

Country Status (1)

Country Link
CN (1) CN109945859B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375773A (zh) * 2019-07-29 2019-10-25 兰州交通大学 Mems惯导系统姿态初始化方法
CN111121764A (zh) * 2019-12-14 2020-05-08 国网上海市电力公司 一种基于形态学滤波的惯导载体运行轨迹修正方法
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN112904393A (zh) * 2021-01-19 2021-06-04 江苏大学 一种导航路径几何约束辅助的农业机械自主导航方法
CN114111843A (zh) * 2021-11-24 2022-03-01 东南大学 一种捷联惯导航系统最优动基座初始对准方法
CN114136310A (zh) * 2021-10-29 2022-03-04 北京自动化控制设备研究所 一种惯性导航系统误差自主抑制系统及方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216321A (zh) * 2008-01-04 2008-07-09 南京航空航天大学 捷联惯性导航系统的快速精对准方法
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
CN103674064A (zh) * 2013-12-06 2014-03-26 广东电网公司电力科学研究院 捷联惯性导航系统的初始标定方法
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN109029454A (zh) * 2018-07-13 2018-12-18 哈尔滨工程大学 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216321A (zh) * 2008-01-04 2008-07-09 南京航空航天大学 捷联惯性导航系统的快速精对准方法
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
CN103674064A (zh) * 2013-12-06 2014-03-26 广东电网公司电力科学研究院 捷联惯性导航系统的初始标定方法
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN109029454A (zh) * 2018-07-13 2018-12-18 哈尔滨工程大学 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王其等: "《自适应联邦H∞滤波在水下组合导航系统中的应用》", 《系统仿真学报》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375773A (zh) * 2019-07-29 2019-10-25 兰州交通大学 Mems惯导系统姿态初始化方法
CN111121764A (zh) * 2019-12-14 2020-05-08 国网上海市电力公司 一种基于形态学滤波的惯导载体运行轨迹修正方法
CN111121764B (zh) * 2019-12-14 2023-09-01 国网上海市电力公司 一种基于形态学滤波的惯导载体运行轨迹修正方法
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN112904393A (zh) * 2021-01-19 2021-06-04 江苏大学 一种导航路径几何约束辅助的农业机械自主导航方法
CN112904393B (zh) * 2021-01-19 2023-11-10 江苏大学 一种导航路径几何约束辅助的农业机械自主导航方法
CN114136310A (zh) * 2021-10-29 2022-03-04 北京自动化控制设备研究所 一种惯性导航系统误差自主抑制系统及方法
CN114136310B (zh) * 2021-10-29 2023-10-13 北京自动化控制设备研究所 一种惯性导航系统误差自主抑制系统及方法
CN114111843A (zh) * 2021-11-24 2022-03-01 东南大学 一种捷联惯导航系统最优动基座初始对准方法

Also Published As

Publication number Publication date
CN109945859B (zh) 2022-12-06

Similar Documents

Publication Publication Date Title
CN109945859A (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN103822633B (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN106597017B (zh) 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置
CN101660914B (zh) 耦合惯性位置误差的机载星光和惯性组合的自主导航方法
CN110207691B (zh) 一种基于数据链测距的多无人车协同导航方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN106500693A (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN101900573B (zh) 一种实现陆用惯性导航系统运动对准的方法
CN104635743A (zh) 一种高速无人机超低空全程自主飞行控制系统
CN104698485A (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN107063245A (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN110763872A (zh) 一种多普勒测速仪多参数在线标定方法
CN108121890A (zh) 一种基于线性卡尔曼滤波的航姿信息融合方法
CN114964226B (zh) 噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法

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