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

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

Info

Publication number
CN109945859B
CN109945859B CN201910255304.5A CN201910255304A CN109945859B CN 109945859 B CN109945859 B CN 109945859B CN 201910255304 A CN201910255304 A CN 201910255304A CN 109945859 B CN109945859 B CN 109945859B
Authority
CN
China
Prior art keywords
filtering
formula
inertial navigation
strapdown inertial
axis
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
CN201910255304.5A
Other languages
English (en)
Other versions
CN109945859A (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

Images

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轴分别指向载体的右前上方向;
根据捷联惯性导航***状态和载体运动学约束条件选取状态量和量测量;选取***状态量为:
Figure BDA0002013535280000021
式中:δvE,δvN分别为捷联惯性导航***东向、北向速度误差;φE,φN,φU分别为***东向、北向和天向失准角误差;
Figure BDA0002013535280000022
分别为载体坐标系x轴,y轴方向的加速度计常值偏置;εx,εy,εz分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移;[·]T表示矩阵转置;X为***状态量组成的列向量;
选取***量测量为:
y=[δvx,δvz]T
式中:δvx,δvz分别为捷联惯性导航***载体系右向、上向的速度误差;y为***量测量组成的列向量。
进一步的,所述步骤(2)中建立运动学约束H滤波模型的具体步骤如下:
2.1)状态方程为:
Figure BDA0002013535280000023
式中:
Figure BDA0002013535280000024
为***状态向量的微分,F为按捷联惯导***误差方程建立的10 ×10维状态转移矩阵,Γ为噪声驱动阵,W为由陀螺和加速度计噪声构成的***噪声阵;
2.2)量测方程为:
y=HX+V
式中:V为量测噪声向量,H为量测矩阵,形式如下:
Figure BDA0002013535280000025
式中,Cij(i,j=1,2,3)为导航系至载体系姿态转换矩阵
Figure BDA0002013535280000026
的第i行第j列的元素;vE,vN,vU分别为捷联惯性导航***东向、北向和天向速度。
进一步的,所述步骤(3)中利用滑动窗口内的滤波新息构造自适应H滤波方法的具体步骤如下:
对步骤(2)中的模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
Figure BDA0002013535280000031
H滤波估计状态的任意线性组合:
Zk=LkXk
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
Figure BDA0002013535280000032
式中:γ是调节H滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
Figure BDA0002013535280000033
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk
Figure BDA0002013535280000034
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为下式:
Figure BDA0002013535280000035
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
自适应H滤波不断进行时间更新和量测更新,估计出捷联惯性导航***的东向和北向速度误差后,对***输出的东向和北向速度进行补偿,直至导航结束。
本发明提出一种自适应H滤波的运动学约束捷联惯性导航方法,该方法建立了捷联惯性导航***运动学约束的H滤波模型,给出了一种自适应调节H滤波参数的方法,将该自适应H滤波方法应用于运动学约束导航算法,该方法能够有效提高导航精度。
本发明采用上述技术方案,具有以下有益效果:
本发明采用自适应H滤波,利用滑动窗口内的新息构造滤波参数调节模型,使得滤波参数能够根据外界干扰或***状态的变化进行自适应调节,兼顾滤波精度与***的鲁棒性,能够提高运动学约束惯性导航算法的精度。
附图说明
图1为本发明的流程图;
图2为具体实施例中载体行驶轨迹图;
图3为具体实施例中滤波参数γ变化图;
图4为具体实施例中3种方法定位误差和载体系速度误差对比曲线图。
具体实施方式
下面结合具体实施例,进一步阐明本发明,如图1至图4所示,本发明所述的一种自适应H滤波的运动学约束捷联惯性导航方法,具体步骤如下:
步骤1)定义解算所需的坐标系并选取滤波状态量和量测量;
坐标系选择如下:
n系—导航坐标系,x轴、y轴和z轴分别指向载体所在地理位置的东北天方向;
b系—载体坐标系,x轴、y轴和z轴分别指向载体的右前上方向;
根据捷联惯性导航***状态和载体运动学约束条件选取状态量和量测量。选取***状态量为:
Figure BDA0002013535280000041
式中:δvE,δvN分别为捷联惯性导航***东向、北向速度误差;φE,φN,φU分别为***东向、北向和天向失准角误差;
Figure BDA0002013535280000042
分别为载体坐标系x轴,y轴方向的加速度计常值偏置;εx,εy,εz分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移;[·]T表示矩阵转置;X为***状态量组成的列向量;
选取***量测量为:
y=[δvx,δvz]T (2)
式中:δvx,δvz分别为捷联惯性导航***载体系右向、上向的速度误差;y为***量测量组成的列向量;
步骤2)建立运动学约束H滤波模型;
2.1)状态方程为:
Figure BDA0002013535280000051
式中:
Figure BDA0002013535280000052
为***状态向量的微分,F为按捷联惯导***误差方程建立的10 ×10维状态转移矩阵,Γ为噪声驱动阵,W为由陀螺和加速度计噪声构成的***噪声阵;
2.2)量测方程为:
y=HX+V (4)
式中:V为量测噪声向量,H为量测矩阵,形式如下:
Figure BDA0002013535280000053
式中,Cij(i,j=1,2,3)为导航系至载体系姿态转换矩阵
Figure BDA0002013535280000054
的第i行第j列的元素;vE,vN,vU分别为捷联惯性导航***东向、北向和天向速度;
步骤3)利用滑动窗口内的滤波新息构造自适应H滤波方法:
对上述模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
Figure BDA0002013535280000055
H滤波可以估计状态的任意线性组合:
Zk=LkXk (7)
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
Figure BDA0002013535280000061
式中:γ是调节H滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
Figure BDA0002013535280000062
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk
Figure BDA0002013535280000063
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为式(11):
Figure BDA0002013535280000064
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
根据式(6)至式(11),自适应H滤波不断进行时间更新和量测更新,估计出捷联惯性导航***的东向和北向速度误差后,对***输出的东向和北向速度进行补偿,直至导航结束。
本发明的可行性通过如下半物理仿真加以验证:
(1)陀螺仪随机常值漂移0.01°/h,加速度计常值偏置0.1mg;
(2)载体以10km/h的速度匀速行驶,行驶时长为5h,行驶轨迹如图2所示;
(3)取得陀螺与加速度实测数据后,用3种方法进行半物理仿真导航结果对比,分别是不采用运动学约束导航、传统H滤波运动学约束导航和自适应H滤波运动学约束导航方法。
通过半物理仿真,验证了滤波参数随***的自适应调节功能(如图3所示),本方法的导航精度较另两种方法有显著提高(如图4所示)。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。

Claims (1)

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

Families Citing this family (6)

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

Family Cites Families (5)

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

Also Published As

Publication number Publication date
CN109945859A (zh) 2019-06-28

Similar Documents

Publication Publication Date Title
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110398257B (zh) Gps辅助的sins***快速动基座初始对准方法
CN110207691B (zh) 一种基于数据链测距的多无人车协同导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN110440830B (zh) 动基座下车载捷联惯导***自对准方法
CN112945225A (zh) 基于扩展卡尔曼滤波的姿态解算***及解算方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航***的自适应滤波导航方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN114111843B (zh) 一种捷联惯导***最优动基座初始对准方法
CN113175926B (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN112284388B (zh) 一种无人机多源信息融合导航方法
CN111707292B (zh) 一种自适应滤波的快速传递对准方法
CN110375773B (zh) Mems惯导***姿态初始化方法
CN113029173A (zh) 车辆导航方法及装置
CN115014321B (zh) 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
CN114061574B (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