CN109828296A - 一种ins/usbl非线性紧组合综合校正方法 - Google Patents

一种ins/usbl非线性紧组合综合校正方法 Download PDF

Info

Publication number
CN109828296A
CN109828296A CN201910177074.5A CN201910177074A CN109828296A CN 109828296 A CN109828296 A CN 109828296A CN 201910177074 A CN201910177074 A CN 201910177074A CN 109828296 A CN109828296 A CN 109828296A
Authority
CN
China
Prior art keywords
error
usbl
ins
navigation
under
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.)
Pending
Application number
CN201910177074.5A
Other languages
English (en)
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910177074.5A priority Critical patent/CN109828296A/zh
Publication of CN109828296A publication Critical patent/CN109828296A/zh
Pending legal-status Critical Current

Links

Landscapes

  • Navigation (AREA)

Abstract

本发明属于卫星导航领域,具体涉及一种INS/USBL非线性紧组合综合校正方法,包括以下步骤:利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程,超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型,与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程,使得入射角误差及测距误差得到补偿。本发明能够有效解决INS/USBL松组合导航利用绝对位置做观测信息存在导航精度较低,且噪声异常引起抗干扰能力弱的问题。

Description

一种INS/USBL非线性紧组合综合校正方法
技术领域
本发明属于卫星导航领域,具体涉及一种INS/USBL非线性紧组合综合校正方法。
背景技术
水下航行器在校正导航信息时需上浮接收全球定位系统(Global NavigationSatellite System,GNSS)信号,这不仅使得水下航行器需要消耗一定的能量和时间,并且会大幅增加暴露的可能性。而INS/USBL综合校正技术可通过超短基线定位系统(UltraShort Base Line,USBL)在水下校正惯性导航系统(Inertial Navigation System,INS)而不需上浮接收GNSS信号即可获得连续、高精度的信息,从而节省了水下航行器的能量和时间,保证了水下航行器的工作效率。但常规USBL与INS的组合常用位置作为量测信息,这需要USBL测得入射角和斜距后再解算出相应的位置或角度做量测量,使USBL解算频率更低,且此设计中滤波参数设置无针对性,最终导致滤波精度不高,且在实际工程应用中很难避免非线性问题。
《Design and Experimental Validation of a USBL Underwater AcousticPositioning System》(发表于期刊《Sensors》,2016年,16期,9卷)一文中,提出建立INS/USBL组合导航的非线性模型,并利用扩展卡尔曼滤波(Extended Kalman Filters,EKF)解决非线性问题,但EKF无法避免模型线性化带来的截断误差问题,当系统出现强非线性时,EKF滤波精度会大幅下降。且此方法仍无法解决利用角度或位置做量测量使USBL解算频率降低、滤波参数设置无针对性的问题。除此之外,水下环境复杂,INS/USBL非线性紧组合综合校正量测值也常出现野值,量测噪声受到环境影响易出现异常,将不准确的噪声统计特性应用到常规滤波中时,则会出现较大的估计误差。
发明内容
针对现有技术中存在的问题,本发明提供一种INS/USBL非线性紧组合综合校正方法。
一种INS/USBL非线性紧组合综合校正方法,包括下述步骤:
(1)利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程,超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型,与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程,使得入射角误差及测距误差得到补偿;
(2)利用惯导解算的应答器相对于惯导在基阵坐标系下的入射角、斜距信息与超短基线输出的入射角、斜距信息之差作为量测量构建非线性量测方程;
(3)在改进的Sage-Husa算法基础上,采用容积规则设计适用于非线性系统的自适应容积卡尔曼滤波估计器,使系统在量测噪声发生异常时滤波估计器可稳定输出。
所述非线性量测方程为:
其中:
其中,分别为惯导解算的应答器相对于惯导在载体系下x轴、y轴的入射角及斜距,分别是其误差项;分别为USBL直接输出的基阵系下的x轴、y轴的入射角及斜距,分别为其误差项;δKd为超短基线的测距精度误差;R为斜距真值;为导航系到载体系的转换矩阵;为地球坐标系到导航坐标系下的转换矩阵;rn为应答器与INS在导航坐标系下的相对位置;δp为位置误差;φ为姿态角误差;re=(xe ye ze)T为应答器与惯导在地球坐标系下的相对位置; 分别为应答器相对于惯导在载体坐标系下xb、yb、zb方向的相对位置关系;λ、h分别为经度、纬度、高度;RN为地球半径。
所述非线性系统下的量测噪声递推公式为:
其中,b1为遗忘因子,0<b1<1。
本发明的有益效果在于:
(1)本发明设计的利用USBL直接输出的入射角和斜距作为量测方程中的量测信息,使在滤波器设计时能根据USBL的测量精度对量测噪声进行设计,使定位精度更高、滤波更平稳。
(2)本发明设计的自适应容积卡尔曼滤波算法在INS/USBL综合校正的量测噪声受到干扰的条件下,能够有效消除噪声干扰对滤波结果造成的影响,并使滤波具备抗外界干扰的能力,使导航性能得到全面的提升。
(3)本发明能够有效解决INS/USBL松组合导航利用绝对位置做观测信息存在导航精度较低,且噪声异常引起抗干扰能力弱的问题。
附图说明
图1为INS/USBL非线性紧组合综合校正流程图。
图2为INS/USBL非线性紧组合综合校正结构图。
图3为松组合与紧组合的定位误差对比图。
图4为量测噪声异常时CKF算法与ACKF算法的东向定位误差对比图。
图5为量测噪声异常时CKF算法与ACKF算法的北向定位误差对比图。
图6为量测噪声异常时CKF算法与ACKF算法的天向定位误差对比图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明提出一种INS/USBL非线性紧组合综合校正方法,其流程图如图1所示,该方法的主要步骤如下:
步骤一:为减小USBL基阵中心ob相对INS中心ou的位置杆臂,以及USBL基阵坐标系ou-xuyuzu到INS载体坐标系ob-xbybzb的安装误差角,将INS与USBL系统设计为一体化结构,此设计具有安装方便、操作简单、精度高等特点,INS/USBL非线性紧组合综合校正的结构图如图2所示。
步骤二:将USBL的入射角误差δθxmU、δθymU,测距精度误差δKd加入到传统的INS/USBL综合校正系统的状态量中建立USBL的误差模型,与INS的非线性误差模型共同构成INS/USBL非线性紧组合综合校正系统的状态方程。
步骤三:利用惯导解算的应答器相对于惯导在基阵坐标系下的入射角、斜距信息与超短基线输出的入射角、斜距信息之差作为量测量,并忽略USBL基阵中心相对INS中心的位置杆臂以及USBL基阵坐标系到INS载体坐标系的安装误差角(即b系与u系重合)建立非线性量测方程。
所涉及的非线性量测方程为
式中
式中,分别为INS解算的应答器相对于INS在载体系下x轴、y轴的入射角及斜距,分别是其误差项;分别为USBL直接输出的基阵系下的x轴、y轴的入射角及斜距,分别为其误差项;δKd为超短基线的测距精度误差;R为斜距真值;为导航系到载体系的转换矩阵;为地球坐标系到导航坐标系下的转换矩阵;rn为应答器与INS在导航坐标系下的相对位置;δp为位置误差;φ为姿态角误差;re=(xe ye ze)T为应答器与INS在地球坐标系下的相对位置; 分别为应答器相对于INS在载体坐标系下xb、yb、zb方向的相对位置关系;λ、h分别为经度、纬度、高度;RN为地球半径。
步骤四:将改进的Sage-Husa算法与容积规则相结合,推导出非线性系统下量测噪声协方差阵R的非线性表示形式,设计出自适应容积卡尔曼滤波算法。
所涉及的非线性系统下的量测噪声递推公式为
式中,b1为遗忘因子,0<b1<1。
为了验证本发明方法的合理性与可行性,利用MATLAB程序对所发明的INS/USBL非线性紧组合综合校正导航方法进行仿真验证。
仿真条件及仿真结果如下所示:
(1)载体运动及初始条件设置
航行器的初始位置为45.6°N,126.5°E,高度为0m,应答器的位置为45.6796°N,126.5705°E,高度-50m;航行器经初始对准后的失准角分别为:纵横摇角误差为0.1°,方位角误差为15°。
航行器以5n mile/h的速度围绕应答器螺旋向下行驶600s;INS的解算频率为100Hz,USBL的解算频率为0.5Hz。
(2)各传感器误差如表1所示
表1传感器误差
(3)仿真结果
依上述(1)(2)仿真条件,对所发明的INS/USBL非线性紧组合综合校正导航方法进行仿真,验证紧组合时的优越性。在上述仿真条件下分别以绝对位置与相对信息作为量测量进行仿真,两者的定位误差比较图如图3所示。将图3中各定位误差的均方根值(RMS)进行统计整理,如表2所示。
表2位置估计误差(RMS)
从图3、表2可以看出,本发明设计的非线性紧组合综合校正算法定位精度大约2.4倍优于松组合的综合校正算法定位精度,而且滤波更加平稳。
依上述(1)(2)仿真条件,以紧组合方式对所发明的ACKF算法对抗外界干扰的能力进行仿真验证,在上述仿真条件下模拟测量噪声在导航150s和200s之间受到干扰而增大,将入射角随机白噪声的标准差增大至10°,将斜距随机白噪声的标准差增大至20m。利用CKF算法对定位误差做滤波估计,定位误差估计曲线如图4-图6所示。为验证ACKF算法的优越性,在量测噪声增大条件下仿真,其中,ACKF算法中遗忘因子取值为b1=0.2,并与常规CKF做比较,ACKF算法下的定位误差如图4-图6中所示。将图4-图6中各定位误差的均方根值(RMS)进行统计整理,如表3所示。
表3位置估计误差(RMS)
由图4-图6、表3可以看出,CKF算法由于缺少噪声估计环节,在量测噪声受干扰而增大期间仍使用噪声协方差初值,使滤波结果受到影响,导致滤波出现较大不平稳误差,即使在干扰噪声消失后,滤波也无法重新收敛。而ACKF算法通过在线估计噪声,并利用相对准确的噪声估计值进行滤波解算,保障了导航解算不受噪声变化的影响。在噪声增大期间,滤波过程始终保持平稳和收敛,且其精度与无噪声变化时的精度和稳定性相当。由此可见,ACKF算法可以有效地抑制噪声变化对滤波精度的影响,并能使滤波精度水平提高。
结合上述分析,得到如下的结果:本发明的INS/USBL非线性紧组合综合校正导航方法,不仅使在滤波器设计时能根据USBL的测量精度对量测噪声进行设计,使定位精度更高、滤波更平稳,而且在量测噪声受到干扰的条件下,能够有效消除噪声干扰对滤波结果造成的影响,并使滤波具备抗外界干扰的能力,使导航性能得到全面的提升。
本发明的目的在于提供一种精度高,滤波稳定的非线性综合校正导航方法。
为实现上述目的,本发明采用的技术方案包括下列步骤:
步骤一:以惯导传统误差量,即速度误差δv,位置误差δp,姿态误差角φ,加速度计零偏陀螺仪随机漂移ε作为INS状态量,扩充超短基线状态量,即基阵坐标系xu轴入射角误差δψxmU,yu轴入射角误差δψymU,测距精度误差δKd,利用INS状态量及USBL状态量共同构建INS/USBL综合校正的非线性状态方程。
步骤二:以惯导解算的应答器相对于惯导在载体系下的xb轴入射角yb轴入射角斜距与USBL直接输出基阵系下的xu轴入射角yu轴入射角斜距之差作为量测量,并忽略USBL基阵中心相对INS中心的位置杆臂以及USBL基阵坐标系到INS载体坐标系的安装误差角(即载体系与基阵系重合)建立非线性量测方程。
所涉及观测方程为
式中
步骤三:将改进的Sage-Husa算法结合容积规则,推导出非线性系统下量测噪声协方差阵R的非线性表示形式,设计出自适应容积卡尔曼滤波算法。
所涉及的自适应容积卡尔曼滤波时间更新与量测更新分别为
时间更新
量测更新
应理解,这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解,在阅读了本发明讲授的内容之后,本领域技术人员可以对本发明作各种改动或修改,这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (3)

1.一种INS/USBL非线性紧组合综合校正方法,其特征在于,包括下述步骤:
(1)利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程,超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型,与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程,使得入射角误差及测距误差得到补偿;
(2)利用惯导解算的应答器相对于惯导在基阵坐标系下的入射角、斜距信息与超短基线输出的入射角、斜距信息之差作为量测量构建非线性量测方程;
(3)在改进的Sage-Husa算法基础上,采用容积规则设计适用于非线性系统的自适应容积卡尔曼滤波估计器,使系统在量测噪声发生异常时滤波估计器可稳定输出。
2.根据权利要求1所述的一种INS/USBL非线性紧组合综合校正方法,其特征在于,所述非线性量测方程为:
其中:
其中,分别为惯导解算的应答器相对于惯导在载体系下x轴、y轴的入射角及斜距,分别是其误差项;分别为USBL直接输出的基阵系下的x轴、y轴的入射角及斜距,分别为其误差项;δKd为超短基线的测距精度误差;R为斜距真值;为导航系到载体系的转换矩阵;为地球坐标系到导航坐标系下的转换矩阵;rn为应答器与INS在导航坐标系下的相对位置;δp为位置误差;φ为姿态角误差;re=(xe ye ze)T为应答器与惯导在地球坐标系下的相对位置; 分别为应答器相对于惯导在载体坐标系下xb、yb、zb方向的相对位置关系;λ、h分别为经度、纬度、高度;RN为地球半径。
3.根据权利要求1所述的一种INS/USBL非线性紧组合综合校正方法,其特征在于,所述非线性系统下的量测噪声递推公式为:
其中,b1为遗忘因子,0<b1<1。
CN201910177074.5A 2019-03-08 2019-03-08 一种ins/usbl非线性紧组合综合校正方法 Pending CN109828296A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910177074.5A CN109828296A (zh) 2019-03-08 2019-03-08 一种ins/usbl非线性紧组合综合校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910177074.5A CN109828296A (zh) 2019-03-08 2019-03-08 一种ins/usbl非线性紧组合综合校正方法

Publications (1)

Publication Number Publication Date
CN109828296A true CN109828296A (zh) 2019-05-31

Family

ID=66865770

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910177074.5A Pending CN109828296A (zh) 2019-03-08 2019-03-08 一种ins/usbl非线性紧组合综合校正方法

Country Status (1)

Country Link
CN (1) CN109828296A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111380520A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl松组合导航定位方法
CN111380517A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
CN111380519A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN111578936A (zh) * 2020-05-09 2020-08-25 东南大学 基于imm-ukf的惯性/超短基线多参数标定方法
CN114279443A (zh) * 2021-10-29 2022-04-05 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN116337115A (zh) * 2023-05-31 2023-06-27 山东大学 一种基于声呐标校惯性传感器的方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105486313A (zh) * 2016-02-03 2016-04-13 东南大学 一种基于usbl辅助低成本sins系统的定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105486313A (zh) * 2016-02-03 2016-04-13 东南大学 一种基于usbl辅助低成本sins系统的定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
董萍 等: "基于相对信息观测量的INS/USBL非线性组合导航方法" *
董萍等: "基于相对信息观测量的INS/USBL非线性", 《系统工程与电子技术》 *
董萍等: "基于相对信息观测量的INS/USBL非线性", 《系统工程与电子技术》, vol. 41, no. 2, 28 February 2019 (2019-02-28), pages 402 - 408 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111380520A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl松组合导航定位方法
CN111380517A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
CN111380519A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN111380517B (zh) * 2020-03-04 2021-10-29 哈尔滨工程大学 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
CN111578936A (zh) * 2020-05-09 2020-08-25 东南大学 基于imm-ukf的惯性/超短基线多参数标定方法
CN111578936B (zh) * 2020-05-09 2022-08-02 东南大学 基于imm-ukf的惯性/超短基线多参数标定方法
CN114279443A (zh) * 2021-10-29 2022-04-05 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN114279443B (zh) * 2021-10-29 2024-01-02 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN116337115A (zh) * 2023-05-31 2023-06-27 山东大学 一种基于声呐标校惯性传感器的方法及系统
CN116337115B (zh) * 2023-05-31 2023-08-29 山东大学 一种基于声呐标校惯性传感器的方法及系统

Similar Documents

Publication Publication Date Title
CN109828296A (zh) 一种ins/usbl非线性紧组合综合校正方法
CN106123921B (zh) 动态干扰条件下捷联惯导系统的纬度未知自对准方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN104344837B (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
Gebre-Egziabher et al. Calibration of strapdown magnetometers in magnetic field domain
CN109029454A (zh) 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法
CN110487301A (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN104344836B (zh) 一种基于姿态观测的冗余惯导系统光纤陀螺系统级标定方法
CN105043415B (zh) 基于四元数模型的惯性系自对准方法
CN108426574A (zh) 一种基于zihr的航向角修正算法的mems行人导航方法
CN106441357B (zh) 一种基于阻尼网络的单轴旋转sins轴向陀螺漂移校正方法
CN103575299A (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN102998685A (zh) 一种惯性/卫星导航系统伪距/伪距率紧组合方法
CN104049269B (zh) 一种基于激光测距和mems/gps组合导航系统的目标导航测绘方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN103792561B (zh) 一种基于gnss通道差分的紧组合降维滤波方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN110207694A (zh) 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
CN113503894B (zh) 基于陀螺基准坐标系的惯导系统误差标定方法
CN102168978B (zh) 一种船用惯性导航系统摇摆基座开环对准方法
CN106802143B (zh) 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法
CN110133692A (zh) 惯导技术辅助的高精度gnss动态倾斜测量系统及方法
CN109737958A (zh) 一种声学测速辅助的极区格网惯性导航误差抑制方法
CN104965102B (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20190531