CN109945859B - 一种自适应h∞滤波的运动学约束捷联惯性导航方法 - Google Patents
一种自适应h∞滤波的运动学约束捷联惯性导航方法 Download PDFInfo
- 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
Links
Images
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种自适应H∞滤波的运动学约束捷联惯性导航方法,主要步骤包括:定义解算所需的坐标系,根据捷联惯性导航***状态和载体运动学约束条件选取状态量和量测量,建立运动学约束H∞滤波模型,利用滑动窗口内的滤波新息构造自适应H∞滤波方法,估计出速度误差并对***进行修正,完成运动学约束惯性导航。本发明能够在不增加传感器且不影响捷联惯性导航***隐蔽性等其他优点的基础上,有效提高捷联惯性导航***导航精度。
Description
技术领域
本发明属于导航技术领域,特别涉及一种自适应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)状态方程为:
2.2)量测方程为:
y=HX+V
式中:V为量测噪声向量,H为量测矩阵,形式如下:
进一步的,所述步骤(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)状态方程为:
2.2)量测方程为:
y=HX+V (4)
式中:V为量测噪声向量,H为量测矩阵,形式如下:
步骤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 (1)
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为***量测量组成的列向量;
(2)建立运动学约束H∞滤波模型;
其中,建立运动学约束H∞滤波模型的具体步骤如下:
3.1)状态方程为:
3.2)量测方程为:
y=HX+V
式中:V为量测噪声向量,H为量测矩阵,形式如下:
vN,vU分别为捷联惯性导航***东向、北向和天向速度;
(3)利用滑动窗口内的滤波新息构造自适应H∞滤波方法;
其中,利用滑动窗口内的滤波新息构造自适应H∞滤波方法的具体步骤如下:
对步骤(2)中的模型进行离散化,k时刻的状态量由k-1时刻的状态量递推得到,形式如下:
H∞滤波估计状态的任意线性组合:
Zk=LkXk
式中:Zk为线性组合后的状态量,取Lk为单位阵;
利用自适应H∞滤波算法对状态量中的速度误差进行估计,其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下:
式中:γ是调节H∞滤波鲁棒性和精度的因子;
新息ηk可以反映滤波器的性能:
式中:新息ηk为2维列向量,对ηk进行相关运算得到ωk:
式中,N为新息序列的采样长度,m为计数序号;
可构造γ为下式:
式中:系数μ>0,μ根据实际情况确定,λ(·)为矩阵的最大特征值;
自适应H∞滤波不断进行时间更新和量测更新,估计出捷联惯性导航***的东向和北向速度误差后,对***输出的东向和北向速度进行补偿,直至导航结束。
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)
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)
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 | 哈尔滨工程大学 | 一种基于卡尔曼滤波的横坐标系捷联惯导***阻尼算法 |
-
2019
- 2019-04-01 CN CN201910255304.5A patent/CN109945859B/zh active Active
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 |