CN115096321A - 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统 - Google Patents

一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统 Download PDF

Info

Publication number
CN115096321A
CN115096321A CN202210720421.6A CN202210720421A CN115096321A CN 115096321 A CN115096321 A CN 115096321A CN 202210720421 A CN202210720421 A CN 202210720421A CN 115096321 A CN115096321 A CN 115096321A
Authority
CN
China
Prior art keywords
information
measurement
axis
filtering
alignment
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
CN202210720421.6A
Other languages
English (en)
Other versions
CN115096321B (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.)
63921 Troops of PLA
Original Assignee
63921 Troops of PLA
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 63921 Troops of PLA filed Critical 63921 Troops of PLA
Priority to CN202210720421.6A priority Critical patent/CN115096321B/zh
Publication of CN115096321A publication Critical patent/CN115096321A/zh
Application granted granted Critical
Publication of CN115096321B publication Critical patent/CN115096321B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明属于车载定位导航技术领域，公开了一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统，采用信息滤波框架下的方法‑‑无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化。本发明提出的RUIKF方法适用于陆上GNSS信号可用性差或遮蔽条件下载体系速度辅助车载捷联惯导初始对准中，而且还适用于GNSS被拒止的水下载体系速度辅助捷联惯导初始对准，可进一步推广应用至组合导航领域。本发明提出的RUIKF可在非线性、非高斯条件下实现SINS的动态初始对准，进而提高车辆的快速启动和反应能力。

Description

一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统
技术领域
本发明属于车载定位导航技术领域，尤其涉及一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统。
背景技术
目前，时间和空间是宇宙万物运行的基本属性，现代人类社会的发展对精准可靠的定位导航授时(Positioning Navigation and Timing，PNT)信息高度依赖。卫星导航以其全天候、全天时、低成本的优势，是目前人类社会使用最广泛的定位导航授时手段。受无线电信号传输特性的影响，北斗卫星导航系统在室内、水下、地下、深空等特殊环境应用受限。同时，卫星信号经过两万多公里的传输，到达用户终端时已经极其微弱，极易受到电磁环境的影响，导致服务性能下降，尤其是受到恶意干扰和欺骗时，存在安全隐患。捷联式惯性导航系统(Strapdown Inertial Navigation System，SINS)具有自主性强、体积小、成本低、隐蔽性高、抗干扰能力强等独特优势，在航空、航天、航海等领域受到广泛关注，是北斗卫星导航受到干扰或拒止时的重要备份/保底导航手段。
车载定位导航是全球卫星导航系统(Global satellite navigation system，GNSS)应用的重要领域之一。但在城市、峡谷等GNSS信号受到遮蔽，或在复杂电磁环境GNSS信号受到干扰的复杂情形，车载卫星信号接收机的定位效果将会受到较严重影响。此时，采用外部信息辅助的方法将SINS与里程计(Odometer，OD)等外部导航传感器结合起来构成的组合导航系统可以增强定位导航效果，也可以作为一种应急备份定位导航手段。
初始对准是SINS进入导航解算之前的必经阶段，常规方法是先进行粗对准，再利用卡尔曼滤波(Kalman Filter，KF)进行精对准。粗对准主要目的是在短时间内获取粗略的姿态，使失准角满足小角度、可线性化条件；精对准是在粗对准的基础上进一步获取更加准确的姿态。但是，在实际应用中，SINS不具备充分的静态对准条件，粗对准效果可能达不到精对准的小失准角要求，而采用非线性模型更能真实、有效的反映误差的传播特性。无迹卡尔曼滤波(Unscented Kalman Filter，UKF)通过采样少量的sigma点即可对非线性模型状态量的概率密度函数进行较好的近似，实现3阶精度的数值积分近似，在实际应用中需要较为准确的状态量先验信息，但SINS初始失准角往往是未知的，这将会导致滤波过程中状态误差协方差矩阵的初始值较难确定。
通过上述分析，现有技术存在的问题及缺陷为：
(1)采用“解析式粗对准+KF精对准”两阶段传统对准方法时，在某些场合粗对准阶段所得到的姿态角并不一定能够满足后续精对准小失准角(小于1°)的要求。
(2)在实用过程中，辅助传感器输出易受非高斯噪声污染，导致基于标准KF框架下的滤波对准方法精度下降，甚至滤波发散。
(3)现有无迹卡尔曼滤波方法中，SINS初始失准角难以具备准确的先验信息且量测噪声未知且时变，这就需要状态误差协方差矩阵在滤波过程中能够具备实时自适应能力。
发明内容
针对现有技术存在的问题，本发明提供了一种车载捷联惯导系统鲁棒无迹信息滤波对准方法、系统、介质、设备及终端。
本发明是这样实现的，一种车载捷联惯导系统鲁棒无迹信息滤波对准方法，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法包括：
采用信息滤波框架下的UKF方法--无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
进一步，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法包括以下步骤：
步骤一，进行SINS初始对准模型的构建；
步骤二，进行UIKF算法的时间更新；
步骤三，计算观测量与观测新息之间马氏距离，并计算观测新息对应权值；
步骤四，基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
步骤五，利用修正后的R阵进行UIKF算法量测更新，实现RIKF量测更新；
步骤六，利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
进一步，所述步骤一中的SINS初始对准模型的构建包括：
(1)坐标系的定义
1)惯性坐标系O-xiyizi，i系
以地球的中心为坐标原点，xi轴指向春分点，zi轴与地球自转轴重合，yi轴与xi轴、zi轴符合右手螺旋定则，构成右手直角坐标系。
2)地球坐标系O-xeyeze，e系
以地球中心为原点，xe轴指向格林威治子午线，ze轴与地球自转轴重合，ye轴与xe轴、ze轴符合右手螺旋定则，构成右手直角坐标系；O-xeyeze坐标系与地球固联，随地球转动，相对惯性空间的转动角速度就是地球自转角速度ωie。
3)导航坐标系O-xnynzn，n系
导航坐标系是INS在导航解算过程中求解导航参数所使用的坐标系，选取当地地理坐标系为导航坐标系。
4)载体坐标系O-xbybzb，b系
以载体的质心为坐标原点，xb轴与载体主轴重合；zb轴垂直于载体向上，yb轴与xb轴、zb轴符合右手螺旋定则，构成右手直角坐标系；载体坐标系与载体直接固联，xb轴为俯仰轴，yb轴为横滚轴，zb轴为偏航轴；载体绕xb轴、yb轴和zb轴旋转将分别得到俯仰角θ、横滚角
Figure BDA0003711068320000031
和航向角ψ。
(2)SINS非线性对准模型的构建
SINS初始对准采用闭环对准CA的方式。在CA结构中，滤波估计结果用于实时补偿SINS的解算误差；建立SINS的动态误差模型；根据SINS的误差模型和OD/DVL提供的观测数据，采用非线性滤波方法实时估计状态向量；b系速度通过SINS输出的姿态矩阵转化为n系速度并作为非线性滤波方法的观测量。
1)状态模型
SINS动基座初始对准非线性状态空间模型如下式所示：
Figure BDA0003711068320000032
式中，δfb为加速度计的量测误差，此处设置为
Figure BDA0003711068320000033
gn为n系下的重力矢量，δgn在实际应用中近似为0；
Figure BDA0003711068320000034
表示y系相对于x系的旋转角速度在z系上的投影；vn＝[vE vNvU]T为地速，是运载体b系相对于e系的速度在n系上的投影；fb为加速度计测得的比力；L为纬度；ωie为地球自转角速度；Re为地球半径。
2)量测模型
选取东向速度误差δvE和北向速度误差δvN作为观测量；当外部辅助速度为DVL或OD提供的b系速度时，观测模型公式如下式所示：
Figure BDA0003711068320000035
式中，
Figure BDA0003711068320000036
为DVL/OD测得的载体系速度；
Figure BDA0003711068320000037
为DVL/OD的安装误差矩阵，在实际应用中通过事先标定的方式获取，观测量z的表达式为：
Figure BDA0003711068320000038
式中，[·]2×1表示向量[·]3×1的前两行。
进一步，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：
基于如下离散时间非线性动态对准状态空间模型：
Figure BDA0003711068320000041
式中，f(·)表示非线性的过程模型函数；h(·)表示非线性观测模型函数；
Figure BDA0003711068320000042
表示n维状态向量；
Figure BDA0003711068320000043
表示m维观测向量；ωk表示系统噪声向量，服从高斯分布ωk～N(0,Qk)，Qk为系统噪声协方差矩阵；υk表示量测噪声向量，服从高斯分布υk～N(0,Rk)，Rk为量测噪声协方差矩阵。
Figure BDA0003711068320000044
式中，δkl为Kronecker函数。
RUIKF滤波过程分为时间更新和量测更新：
(1)时间更新
计算Sigma点χk-1|k-1和传递Sigma点
Figure BDA0003711068320000045
Figure BDA0003711068320000046
Figure BDA0003711068320000047
计算状态量的先验估计
Figure BDA0003711068320000048
状态误差协方差的先验估计Pk|k-1：
Figure BDA0003711068320000049
Figure BDA00037110683200000410
Figure BDA00037110683200000411
(2)量测更新
计算Sigma点如下式所示，计算传递Sigma点
Figure BDA00037110683200000412
Figure BDA00037110683200000413
计算观测量的先验估计
Figure BDA00037110683200000414
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure BDA00037110683200000415
Figure BDA0003711068320000051
Figure BDA0003711068320000052
Figure BDA0003711068320000053
Figure BDA0003711068320000054
Figure BDA0003711068320000055
Figure BDA0003711068320000056
Figure BDA0003711068320000057
Figure BDA0003711068320000058
Figure BDA0003711068320000059
进一步，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：
当在非线性系统中系统的量测方程中的量测向量表示成状态量的线性形式，对于离散时间非线性动态对准状态空间模型所示的系统，系统量测方程中的量测向量表示成系统状态向量的线性表示方式：
Figure BDA00037110683200000510
式中，Hk表示量测矩阵。引入UKF的信息滤波形式，信息状态
Figure BDA00037110683200000511
和信息矩阵IPk|k定义如下：
Figure BDA00037110683200000512
Figure BDA00037110683200000513
由信息卡尔曼滤波IKF的状态更新方程可知，量测量zk对信息状态
Figure BDA00037110683200000514
和信息矩阵IPk|k的贡献ik和Ik表示如下：
Figure BDA00037110683200000515
Figure BDA00037110683200000516
UKF的量测更新方程表示如下：
Figure BDA00037110683200000517
Figure BDA00037110683200000518
对于UIKF，若系统的量测方程为系统状态向量的线性表示，则系统的观测协方差矩阵和互协方差矩阵表示如下：
Figure BDA0003711068320000061
Figure BDA0003711068320000062
将公式等号两边同时左乘
Figure BDA0003711068320000063
和右乘
Figure BDA0003711068320000064
并用Pxz,k|k-1取代
Figure BDA0003711068320000065
得无迹信息滤波中量测向量zk对信息状态
Figure BDA0003711068320000066
的贡献形式：
Figure BDA0003711068320000067
无迹信息滤波中量测向量zk对信息矩阵IPk|k的贡献形式--信息状态的增量形式ik表示如下：
Figure BDA0003711068320000068
得对应的系统量测更新方程如下：
Figure BDA0003711068320000069
式中，
Figure BDA00037110683200000610
证毕。
进一步，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：引入放大因子λ以抑制干扰噪声对初始对准结果的影响。
RUIKF的量测更新如下：
计算Sigma点如下式所示，计算传递Sigma点
Figure BDA00037110683200000611
Figure BDA00037110683200000612
计算观测量的先验估计
Figure BDA00037110683200000613
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure BDA0003711068320000071
Figure BDA0003711068320000072
Figure BDA0003711068320000073
Figure BDA0003711068320000074
Figure BDA0003711068320000075
选择k时刻的观测量
Figure BDA0003711068320000076
与观测量的先验估计
Figure BDA0003711068320000077
间的马氏距离作为评判指标，则k时刻评判指标
Figure BDA00037110683200000724
的定义如下式所示：
Figure BDA0003711068320000078
式中，
Figure BDA0003711068320000079
为马氏距离；Pzz,k|k-1为量测误差协方差阵的先验估计；μk为观测新息向量；对于真实的观测量
Figure BDA00037110683200000710
若评判指标
Figure BDA00037110683200000725
满足
Figure BDA00037110683200000711
则
Figure BDA00037110683200000712
将被标记为正常观测量；若评判指标
Figure BDA00037110683200000726
满足
Figure BDA00037110683200000713
则
Figure BDA00037110683200000714
将被标记为异常观测量，通过引入放大因子λk用于放大量测噪声协方差阵Rk：
Figure BDA00037110683200000715
Figure BDA00037110683200000716
转化为求解λk的非线性问题：
Figure BDA00037110683200000717
式中，λk通过牛顿迭代法求解；λk(i+1)与λk(i)的关系表示为：
Figure BDA00037110683200000718
式中，
Figure BDA00037110683200000719
且λk(i)初始值为λk(0)＝1；当评判指标满足
Figure BDA00037110683200000720
时，迭代终止；将概率参数α设置为0.99，鲁棒滤波方法的效率为99％，2自由度卡方分布值为
Figure BDA00037110683200000721
Figure BDA00037110683200000722
Kk＝(IPk|k-1+Ik)-1IPk|k-1Pxz,k|k-1(λkRk)-1；
Figure BDA00037110683200000723
Figure BDA0003711068320000081
Figure BDA0003711068320000082
Figure BDA0003711068320000083
本发明的另一目的在于提供一种应用所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法的车载捷联惯导系统鲁棒无迹信息滤波对准系统，所述车载捷联惯导系统鲁棒无迹信息滤波对准系统包括：
初始对准模型构建模块，用于进行SINS初始对准模型的构建；
算法时间更新模块，用于进行UIKF算法的时间更新；
马氏距离计算模块，用于计算观测量与观测新息之间马氏距离，并计算观测新息对应权值；
R阵修正模块，用于基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
量测更新模块，用于利用修正后的R阵进行UIKF算法量测更新，实现RIKF量测更新；
误差修正模块，用于利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
采用信息滤波框架下的无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
采用信息滤波框架下的无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端用于实现所述的车载捷联惯导系统鲁棒无迹信息滤波对准系统。
结合上述的技术方案和解决的技术问题，请从以下几方面分析本发明所要保护的技术方案所具备的优点及积极效果为：
第一、针对上述现有技术存在的技术问题以及解决该问题的难度，紧密结合本发明的所要保护的技术方案以及研发过程中结果和数据等，详细、深刻地分析本发明技术方案如何解决的技术问题，解决问题之后带来的一些具备创造性的技术效果。具体描述如下：
本发明采用信息滤波框架下的UKF方法，即无迹信息卡尔曼滤波方法(UnscentedInformation Kalman Filter，UIKF)，通过统计学中的卡方检验实现对异常观测量的有效辨识，并引入放大因子λ对异常观测时的量测噪声协方差阵进行膨胀，进而实现UIKF的鲁棒化，得到鲁棒UIKF(Robust UIKF，RUIKF)。UIKF可有效克服动态、非线性条件下，非高斯噪声对捷联惯导系统初始初始对准的影响，有效提高捷联惯导系统在非线性、非高斯条件下的适应、鲁棒和快速启动能力
本发明提出的RUIKF方法不仅适用于陆上GNSS信号脆弱或遮挡条件下载体系速度辅助车载捷联惯导系统初始对准中，而且还能适用于GNSS被拒止的水下载体系速度辅助船载捷联惯导系统初始对准，并可进一步推广应用至组合导航领域。
第二，把技术方案看做一个整体或者从产品的角度，本发明所要保护的技术方案具备的技术效果和优点，具体描述如下：
本发明提供的RUIKF方法可在非线性、非高斯条件下实现SINS的动态初始对准，进而提高运载体的快速启动能力。
第三，作为本发明的权利要求的创造性辅助证据，还体现在以下几个重要方面：
(1)本发明的技术方案转化后的预期收益和商业价值为：
本发明的基于载体系速度辅助车载捷联惯导系统初始对准方法通过技术方案转化后，与现阶段常规初始对准方法相比，可以统一两阶段对准过程，实现先验信息未知情况下的任意失准角一步对准，有效缩短了整体对准时间并提高了最终对准精度。
(2)本发明的技术方案是否解决了人们一直渴望解决、但始终未能获得成功的技术难题：
长期以来，捷联惯性导航系统作为运载体一种重要的自主导航方式，但其导航精度很大程度上依赖于初始对准的效果，动态复杂环境下的SINS初始对准问题是惯性导航领域长期面临的难点问题，而将稳定性好的里程计作为辅助传感器为SINS系统提供速度量测信息进行滤波，可解决长期以来始终未能解决的SINS初始对准问题。
(3)本发明的技术方案是否克服了技术偏见：
本发明克服了长期以来SINS两阶段初始对准方法的惯性思维，指出了实际应用中状态量先验信息未知且量测噪声时变，造成对准过程中滤波参数值难以确定，而通过非线性信息滤波方法，可以满足不需要准确已知状态量的初始信息条件并提升量测更新效率，同时引入放大因子实现滤波方法的鲁棒化，提高了SINS初始对准的精度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波对准方法流程图；
图2是本发明实施例提供的b系速度辅助SINS闭环对准结构框图；
图3是本发明实施例提供的OD输出速度示意图；
图4是本发明实施例提供的俯仰角对准误差的RMSE及局部放大图；
图5是本发明实施例提供的俯仰角对准误差的RMSE及局部放大图；
图6是本发明实施例提供的航向角对准误差的RMSE及局部放大图；
图7是本发明实施例提供的东向速度对准误差的RMSE及局部放大图；
图8是本发明实施例提供的北向速度对准误差的RMSE及局部放大图；
图9是本发明实施例提供的纬度对准误差的RMSE及局部放大图；
图10是本发明实施例提供的经度对准误差的RMSE及局部放大图；
图11是本发明实施例提供的俯仰角估计误差及局部放大图；
图12是本发明实施例提供的横滚角估计误差及局部放大图；
图13是本发明实施例提供的航向角估计误差及局部放大图；
图14是本发明实施例提供的东向速度估计误差及局部放大图；
图15是本发明实施例提供的北向速度估计误差及局部放大图；
图16是本发明实施例提供的纬度估计误差及局部放大图；
图17是本发明实施例提供的经度估计误差及局部放大图；
图18是本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波对准系统结构框图；
图中：1、初始对准模型构建模块；2、算法时间更新模块；3、马氏距离计算模块；4、R阵修正模块；5、量测更新模块；6、误差修正模块。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统，下面结合附图对本发明作详细的描述。
一、解释说明实施例。为了使本领域技术人员充分了解本发明如何具体实现，该部分是对权利要求技术方案进行展开说明的解释说明实施例。
本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波初始对准方法，以UKF方法为基本框架并引入信息滤波，提出无迹信息卡尔曼滤波方法(Unscented InformationKalman Filter，UIKF)，同时，在滤波过程中，通过统计学方法实时对量测信息进行有效辨识以检测异常量测值，利用放大因子λ对异常观测时的量测噪声协方差阵进行放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF(Robust UIKF，RUIKF)。所提出的RUIKF可在非线性、非高斯条件下实现车载SINS的动态初始对准，提高运载体的快速反应和精确导航能力。
如图1所示，本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波对准方法包括以下步骤：
S101，进行SINS初始对准模型的构建；
S102，进行UIKF算法的时间更新；
S103，计算观测量与观测新息之间马氏距离，进行观测新息对应权值计算；
S104，基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
S105，利用修正后的R阵进行UIKF算法量测更新，实现RIKF的量测更新；
S106，利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
作为优选实施例，本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波对准方法具体包括：
一、无迹信息卡尔曼滤波方法(UIKF)
考虑如下离散时间非线性动态对准状态空间模型：
Figure BDA0003711068320000111
式中，f(·)表示非线性的过程模型函数；h(·)表示非线性观测模型函数；
Figure BDA0003711068320000112
表示n维状态向量；
Figure BDA0003711068320000113
表示m维观测向量；ωk表示系统噪声向量，一般服从高斯分布，即ωk～N(0,Qk)，Qk为系统噪声协方差矩阵；υk表示量测噪声向量，一般服从高斯分布，即υk～N(0,Rk)，Rk为量测噪声协方差矩阵。
Figure BDA0003711068320000114
式中，δkl为Kronecker函数。与UKF类似，RUIKF滤波过程分为时间更新和量测更新，具体如下：
(a)时间更新
计算Sigma点χk-1|k-1和传递Sigma点
Figure BDA0003711068320000115
Figure BDA0003711068320000116
Figure BDA0003711068320000117
计算状态量的先验估计
Figure BDA0003711068320000118
状态误差协方差的先验估计Pk|k-1：
Figure BDA0003711068320000119
Figure BDA00037110683200001110
Figure BDA00037110683200001111
(b)量测更新
计算Sigma点如式(8)所示，计算传递Sigma点
Figure BDA00037110683200001112
Figure BDA00037110683200001113
计算观测量的先验估计
Figure BDA00037110683200001114
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure BDA00037110683200001115
Figure BDA00037110683200001116
Figure BDA0003711068320000121
Figure BDA0003711068320000122
Figure BDA0003711068320000123
Figure BDA0003711068320000124
Figure BDA0003711068320000125
Figure BDA0003711068320000126
Figure BDA0003711068320000127
Figure BDA0003711068320000128
为证明式(8)～(18)的合理性，假设在非线性系统中系统的量测方程中的量测向量可以表示成状态量的线性形式，具体如下。
假设1对于式(1)所示的系统，系统量测方程中的量测向量可以表示成系统状态向量的线性表示方式，即式(1)可转化为：
Figure BDA0003711068320000129
式中，Hk表示量测矩阵。对于假设1，由于系统状态模型较为复杂，通常为非线性系统，但对于量测系统，实际中可以将其表示为状态向量的线性表示形式。例如在组合导航系统中，若选取状态向量为三轴位置矢量，则可以通过量测系统直接获取三轴位置信息，即可表示为系统状态向量的线性函数表示形式，因而假设1在实际系统中是合理的。以下对上述无迹卡尔曼滤波过程进行证明。
此处引入UKF的信息滤波形式，信息状态
Figure BDA00037110683200001210
和信息矩阵IPk|k定义如下：
Figure BDA00037110683200001211
Figure BDA00037110683200001212
由信息卡尔曼滤波(IKF)的状态更新方程可知，量测量zk对信息状态
Figure BDA00037110683200001213
和信息矩阵IPk|k的贡献ik和Ik表示如下：
Figure BDA00037110683200001214
Figure BDA00037110683200001215
因而，UKF的量测更新方程表示如下：
Figure BDA00037110683200001216
IPk|k＝IPk|k-1+Ik (25)
对于UIKF，若系统量测方程满足假设1，即系统的量测方程为系统状态向量的线性表示，则系统的观测协方差矩阵和互协方差矩阵可表示如下：
Figure BDA0003711068320000131
Figure BDA0003711068320000132
将式(23)等号两边同时左乘
Figure BDA0003711068320000133
和右乘
Figure BDA0003711068320000134
并用Pxz,k|k-1取代
Figure BDA0003711068320000135
可得如下形式：
Figure BDA0003711068320000136
式中，Pk|k-1可通过式(6)计算得到，Pxz,k|k-1可通过式(11)计算得到。与此类似，信息状态的增量形式ik可表示如下：
Figure BDA0003711068320000137
式(28)和式(29)即为无迹信息滤波中量测向量zk对信息状态
Figure BDA0003711068320000138
和信息矩阵IPk|k的贡献形式。因而可得对应的系统量测更新方程如下：
Figure BDA0003711068320000139
式中，
Figure BDA00037110683200001310
证毕。
二、无迹鲁棒信息卡尔曼滤波方法(RUIKF)
在实用过程中，OD输出难免会受到非高斯噪声污染，为此，引入放大因子λ以抑制干扰噪声对初始对准结果的影响，具体如下。
RUIKF的量测更新如下，计算Sigma点如式(32)所示，计算传递Sigma点
Figure BDA0003711068320000141
Figure BDA0003711068320000142
计算观测量的先验估计
Figure BDA0003711068320000143
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure BDA0003711068320000144
Figure BDA0003711068320000145
Figure BDA0003711068320000146
Figure BDA0003711068320000147
Figure BDA0003711068320000148
当观测量受到野值等非高斯噪声污染时，观测新息向量μk会出现异常，此时对形如式(19)的模型作高斯分布假设将会导致IUKF滤波性能下降。为了实现IUKF的鲁棒化，选择k时刻的观测量
Figure BDA0003711068320000149
与观测量的先验估计
Figure BDA00037110683200001410
之间的马氏距离作为评判指标，则k时刻评判指标
Figure BDA00037110683200001421
的定义如式(38)所示：
Figure BDA00037110683200001411
式中，
Figure BDA00037110683200001412
为马氏距离；Pzz,k|k-1为量测误差协方差阵的先验估计，可根据式(34)求得；μk为观测新息向量。对于真实的观测量
Figure BDA00037110683200001413
若其评判指标
Figure BDA00037110683200001422
满足
Figure BDA00037110683200001414
则
Figure BDA00037110683200001415
将被标记为正常观测量；反之，若其评判指标
Figure BDA00037110683200001423
满足
Figure BDA00037110683200001416
则
Figure BDA00037110683200001417
将被标记为异常观测量，此时通过引入放大因子λk用以放大量测噪声协方差阵Rk，即：
Figure BDA00037110683200001418
将式(39)带入式(38)可得：
Figure BDA00037110683200001419
式(40)可以转化为求解λk的非线性问题，如式(41)所示：
Figure BDA00037110683200001420
式中，λk可以通过牛顿迭代法求解。因此，λk(i+1)与λk(i)的关系可表示为：
Figure BDA0003711068320000151
式中，
Figure BDA0003711068320000152
且λk(i)初始值为λk(0)＝1。当评判指标满足
Figure BDA0003711068320000153
时，迭代终止。本发明将概率参数α设置为0.99，即鲁棒滤波方法的效率为99％，2自由度卡方分布值为
Figure BDA0003711068320000154
Figure BDA0003711068320000155
Figure BDA0003711068320000156
Figure BDA0003711068320000157
Figure BDA0003711068320000158
IPk|k＝IPk|k-1+Ik (47)
Figure BDA0003711068320000159
式(32)～(48)即为RUIKF的量测更新过程。可以看出，RUIKF的时间更新过程与UIKF一致，即为式(3)～(7)。利用RUIKF进行初始对准的流程如图1所示。
三、初始对准模型
3.1坐标系的定义
(1)惯性坐标系(O-xiyizi，i系)
以地球的中心为坐标原点，xi轴指向春分点，zi轴与地球自转轴重合，yi轴与xi轴、zi轴符合右手螺旋定则，构成右手直角坐标系。
(2)地球坐标系(O-xeyeze，e系)
以地球中心为原点，xe轴指向格林威治子午线，ze轴与地球自转轴重合，ye轴与xe轴、ze轴符合右手螺旋定则，构成右手直角坐标系。O-xeyeze坐标系与地球固联，随地球转动，其相对惯性空间的转动角速度就是地球自转角速度ωie。
(3)导航坐标系(O-xnynzn，n系)
导航坐标系是INS在导航解算过程中求解导航参数所使用的坐标系，一般选取当地地理坐标系为导航坐标系。
(4)载体坐标系(O-xbybzb，b系)
以载体的质心为坐标原点，xb轴与载体主轴重合；zb轴垂直于载体向上，yb轴与xb轴、zb轴符合右手螺旋定则，构成右手直角坐标系。载体坐标系与载体直接固联，xb轴为俯仰轴，yb轴为横滚轴，zb轴为偏航轴，即载体绕xb轴、yb轴和zb轴旋转将会分别得到俯仰角θ、横滚角
Figure BDA00037110683200001510
和航向角ψ。
3.2SINS非线性对准模型
SINS初始对准一般分为开环对准(Open-loop Alignment，OA)和闭环对准(Closed-loop Alignment，CA)两种方式。在OA结构中，滤波估计结果仅用于修正SINS的解算误差，由此所得的对准方法也称为开环滤波。相反的，在CA结构中，滤波估计结果用于实时补偿SINS的解算误差，实用中通常采用该对准结构。OD或多普勒计程仪(DopplerVelocity Log，DVL)输出的b系下速度辅助SINS闭环对准结构框图如图2所示。
在如图2所示的初始对准方式中，应首先建立SINS的动态误差模型；然后根据SINS的误差模型和OD/DVL提供的观测数据，采用非线性滤波方法实时估计状态向量。需要注意的是，OD/DVL提供b系下的辅助速度不能直接用来作为初始对准的观测量。由图2可以看出，b系速度首先通过SINS输出的姿态矩阵转化为n系速度才可作为非线性滤波方法的观测量。
(a)状态模型
SINS动基座初始对准非线性状态空间模型如式(49)所示：
Figure BDA0003711068320000161
式中，δfb为加速度计的量测误差，此处设置为
Figure BDA0003711068320000162
gn为n系下的重力矢量，δgn在实际应用中近似为0；
Figure BDA0003711068320000163
表示y系相对于x系的旋转角速度在z系上的投影；vn＝[vE vNvU]T为地速，即运载体b系相对于e系的速度在n系上的投影；fb为加速度计测得的比力；L为纬度；ωie为地球自转角速度；Re为地球半径。
(b)量测模型
选取东向速度误差δvE和北向速度误差δvN作为观测量。当外部辅助速度为DVL或OD提供的b系速度时，观测模型公式如式(50)所示：
Figure BDA0003711068320000164
式中，
Figure BDA0003711068320000165
为DVL/OD测得的载体系速度；
Figure BDA0003711068320000166
为DVL/OD的安装误差矩阵，在实际应用中可通过事先标定的方式获取。因此，观测量z的表达式为：
Figure BDA0003711068320000171
式中，[·]2×1表示向量[·]3×1的前两行。OD提供b系测速辅助SINS进行初始对准或组合导航的问题与DVL提供b系测速辅助SINS进行初始对准或组合导航的问题具有一致性。
如图18所示，本发明实施例提供的车载捷联惯导系统鲁棒无迹信息滤波对准系统包括：
初始对准模型构建模块1，用于进行SINS初始对准模型的构建；
算法时间更新模块2，用于进行UIKF算法的时间更新；
马氏距离计算模块3，用于计算观测量与观测新息之间马氏距离，并计算观测新息对应权值；
R阵修正模块4，用于基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
量测更新模块5，用于利用修正后的R阵进行UIKF算法量测更新，实现RIKF量测更新；
误差修正模块6，用于利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
二、应用实施例。为了证明本发明的技术方案的创造性和技术价值，该部分是对权利要求技术方案进行具体产品上或相关技术上的应用实施例。
本发明针对标准KF方法性能在大失准角、量测信息易受非高斯噪声污染等情况下大幅下降的问题，提出了一种基于信息滤波和马氏距离的RUIKF方法，并将其应用于车载SINS/OD初始对准。在本发明所设计的RUIKF方法更新过程中，利用信息滤波方法减小未知状态量未知先验信息对滤波结果的影响，同时通过计算新息的马氏距离并设置阈值来增强算法的鲁棒能力。本发明所提方法不仅适用于陆上GNSS信号可用性差或遮蔽条件下载体系速度辅助车载SINS初始对准中，而且还能适用于GNSS被拒止的水下载体系速度辅助船载SINS初始对准中，并可进一步推广应用至多传感器组合导航领域。
需要强调的是，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
三、实施例相关效果的证据。本发明实施例在研发或者使用过程中取得了一些积极效果，和现有技术相比的确具备很大的优势，下面内容结合试验过程的数据、图表等进行描述。
为验证本发明所提方法的有效性，利用车载实验平台采集实测数据，车载实验系统由一台导航级激光捷联惯导、里程计(OD)和一套单天线GPS接收机组成，实验系统使用各个传感器的特性参数如下：
(1)IMU(导航级激光陀螺捷联惯导)：陀螺漂移约为0.007°/h(1σ)，加速度计零偏约为5×10-5g(1σ)，采样率为125Hz；
(2)里程计(OD)：脉冲当量为0.08194m/脉冲，采样率为125Hz；
(3)GPS：采样率为1Hz。
实验过程设计为：当SINS开机时，载车保持1200s的静止状态；然后驶出，保持1800s的运动状态。采集的实验数据包含：IMU(陀螺仪和加速度计)的原始数据，GPS输出的速度、位置以及OD的输出。通过SINS/GPS组合导航建立姿态参考基准、速度参考基准。选取900s车载实测数据对UIKF和RUIKF的性能进行对比验证，对应OD的输出如图3所示。
结合实际应用，为验证本发明RUIKF应用于非线性、非高斯条件下SINS动态初始对准中的可行性，考虑如下两类非高斯噪声污染情形：1)混合高斯分布噪声；2)野值。
1.混合高斯分布情形
基于维纳近似定理，任何非高斯噪声分布可用已知概率密度的高斯噪声分布的有限和来表示或充分近似。假设观测噪声的实际概率分布如式(52)所示：
ρactual＝(1-α)N(0,Rc)+αN(0,Rp) (52)
式中，干扰因子α满足0≤α≤0.1；Rc为DVL输出速度数据的量测噪声协方差阵，Rp＝κRc(κ为比例系数)为具有较大标准偏差的干扰噪声协方差阵。试验中，设置Rp＝200·Rc，人为地将式(52)引入图3，分别利用UIKF和RUIKF进行25次蒙特卡洛仿真试验。选取姿态误差、速度误差和位置误差的均方根误差(Root mean-square errors，RMSE)作为评判滤波性能的指标，定义如下：
Figure BDA0003711068320000181
Figure BDA0003711068320000182
Figure BDA0003711068320000183
式中，M为蒙特卡洛仿真试验总次数，即M＝25；
Figure BDA0003711068320000184
分别为第m次蒙特卡洛试验姿态、速度和位置误差真值，
Figure BDA0003711068320000185
分别为第m次蒙特卡洛试验姿态、速度和位置误差估计值。初始对准结果分别如图4～10所示。
从图6～10可以明显看出，当观测量受到干扰噪声污染时，与标准UIKF相比，本发明提出的RUIKF在姿态误差估计、速度误差估计和位置误差估计都具有更好的性能。由试验结果可知，UIKF在整个初始对准过程中都是发散的，这也说明了UIKF对干扰噪声不具备鲁棒性。在初始对准最后150s，RUIKF对水平姿态角估计误差的RMSE收敛到区间(0°，0.018°)，对航向角估计误差的RMSE收敛到区间(0.08°，0.25°)；对速度估计误差的RMSE收敛到区间(0m/s，0.1m/s)；对纬度估计误差的RMSE收敛到区间(10m，34m)，对经度估计误差的RMSE收敛到区间(1m，17m)。由此可得出结论，RUIKF对混合高斯分布噪声具有较好的鲁棒性，能够有效抑制干扰噪声对初始对准的影响。
2.野值情形
在实用中，OD的输出还会受到野值的污染，因此，每间隔15s人为地引入幅值为15m/s的速度野值，分别利用UIKF和RUIKF进行初始对准，结果分别如图11～17所示。
由图11～17可以明显看出，当OD输出受到野值污染时，UIKF对姿态、速度和位置随准误差曲线呈现发散趋势，也就是说UIKF对于野值没有鲁棒性。相比于UIKF，RUIKF得到的对准误差曲线更加稳定。由图13可知，RUIKF对航向角的对准误差曲线在400s时刻开始收敛。这也说明了RUIKF能够抑制野值对初始对准结果的影响。试验结果进一步验证了RUIKF应用于非线性、非高斯条件下SINS/OD动态初始对准中的可行性和有效性。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法包括：
采用信息滤波框架下的UKF方法--无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
2.如权利要求1所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法包括以下步骤：
步骤一，进行SINS初始对准模型的构建；
步骤二，进行UIKF算法的时间更新；
步骤三，计算观测量与观测新息之间马氏距离，并计算观测新息对应权值；
步骤四，基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
步骤五，利用修正后的R阵进行UIKF算法量测更新，实现RIKF量测更新；
步骤六，利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
3.如权利要求2所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述步骤一中的SINS初始对准模型的构建包括：
(1)坐标系的定义
1)惯性坐标系O-xiyizi，i系
以地球的中心为坐标原点，xi轴指向春分点，zi轴与地球自转轴重合，yi轴与xi轴、zi轴符合右手螺旋定则，构成右手直角坐标系；
2)地球坐标系O-xeyeze，e系
以地球中心为原点，xe轴指向格林威治子午线，ze轴与地球自转轴重合，ye轴与xe轴、ze轴符合右手螺旋定则，构成右手直角坐标系；O-xeyeze坐标系与地球固联，随地球转动，相对惯性空间的转动角速度就是地球自转角速度ωie；
3)导航坐标系O-xnynzn，n系
导航坐标系是INS在导航解算过程中求解导航参数所使用的坐标系，选取当地地理坐标系为导航坐标系；
4)载体坐标系O-xbybzb，b系
以载体的质心为坐标原点，xb轴与载体主轴重合；zb轴垂直于载体向上，yb轴与xb轴、zb轴符合右手螺旋定则，构成右手直角坐标系；载体坐标系与载体直接固联，xb轴为俯仰轴，yb轴为横滚轴，zb轴为偏航轴；载体绕xb轴、yb轴和zb轴旋转将分别得到俯仰角θ、横滚角
Figure FDA0003711068310000021
和航向角ψ；
(2)SINS非线性对准模型的构建
SINS初始对准采用闭环对准CA的方式；在CA结构中，滤波估计结果用于实时补偿SINS的解算误差；建立SINS的动态误差模型；根据SINS的误差模型和OD/DVL提供的观测数据，采用非线性滤波方法实时估计状态向量；b系速度通过SINS输出的姿态矩阵转化为n系速度并作为非线性滤波方法的观测量；
1)状态模型
SINS动基座初始对准非线性状态空间模型如下式所示：
Figure FDA0003711068310000022
式中，δfb为加速度计的量测误差，此处设置为
Figure FDA0003711068310000023
gn为n系下的重力矢量，δgn在实际应用中近似为0；
Figure FDA0003711068310000024
表示y系相对于x系的旋转角速度在z系上的投影；vn＝[vE vN vU]T为地速，是运载体b系相对于e系的速度在n系上的投影；fb为加速度计测得的比力；L为纬度；ωie为地球自转角速度；Re为地球半径；
2)量测模型
选取东向速度误差δvE和北向速度误差δvN作为观测量；当外部辅助速度为DVL或OD提供的b系速度时，观测模型公式如下式所示：
Figure FDA0003711068310000031
式中，
Figure FDA0003711068310000032
为DVL/OD测得的载体系速度；
Figure FDA0003711068310000033
为DVL/OD的安装误差矩阵，在实际应用中通过事先标定的方式获取，观测量z的表达式为：
Figure FDA0003711068310000034
式中，[·]2×1表示向量[·]3×1的前两行。
4.如权利要求1所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：
基于如下离散时间非线性动态对准状态空间模型：
Figure FDA0003711068310000035
式中，f(·)表示非线性的过程模型函数；h(·)表示非线性观测模型函数；
Figure FDA0003711068310000036
表示n维状态向量；
Figure FDA0003711068310000037
表示m维观测向量；ωk表示系统噪声向量，服从高斯分布ωk～N(0,Qk)，Qk为系统噪声协方差矩阵；υk表示量测噪声向量，服从高斯分布υk～N(0,Rk)，Rk为量测噪声协方差矩阵；
Figure FDA0003711068310000038
式中，δkl为Kronecker函数；
RUIKF滤波过程分为时间更新和量测更新：
(1)时间更新
计算Sigma点χk-1|k-1和传递Sigma点
Figure FDA0003711068310000039
Figure FDA00037110683100000310
Figure FDA0003711068310000041
计算状态量的先验估计
Figure FDA0003711068310000042
状态误差协方差的先验估计Pk|k-1：
Figure FDA0003711068310000043
Figure FDA0003711068310000044
Figure FDA0003711068310000045
(2)量测更新
计算Sigma点如下式所示，计算传递Sigma点
Figure FDA0003711068310000046
Figure FDA0003711068310000047
计算观测量的先验估计
Figure FDA0003711068310000048
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure FDA0003711068310000049
Figure FDA00037110683100000410
Figure FDA00037110683100000411
Figure FDA00037110683100000412
Figure FDA00037110683100000413
Figure FDA00037110683100000414
Figure FDA00037110683100000415
Figure FDA00037110683100000416
Figure FDA00037110683100000417
IPk|k＝IPk|k-1+Ik，
Figure FDA00037110683100000418
5.如权利要求4所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：
当在非线性系统中系统的量测方程中的量测向量表示成状态量的线性形式，对于离散时间非线性动态对准状态空间模型所示的系统，系统量测方程中的量测向量表示成系统状态向量的线性表示方式：
Figure FDA0003711068310000051
式中，Hk表示量测矩阵；引入UKF的信息滤波形式，信息状态
Figure FDA0003711068310000052
和信息矩阵IPk|k定义如下：
Figure FDA0003711068310000053
Figure FDA0003711068310000054
由信息卡尔曼滤波IKF的状态更新方程可知，量测量zk对信息状态
Figure FDA0003711068310000055
和信息矩阵IPk|k的贡献ik和Ik表示如下：
Figure FDA0003711068310000056
Figure FDA0003711068310000057
UKF的量测更新方程表示如下：
Figure FDA0003711068310000058
IPk|k＝IPk|k-1+Ik；
对于UIKF，若系统的量测方程为系统状态向量的线性表示，则系统的观测协方差矩阵和互协方差矩阵表示如下：
Figure FDA0003711068310000059
Figure FDA00037110683100000510
将公式等号两边同时左乘
Figure FDA00037110683100000511
和右乘
Figure FDA00037110683100000512
并用Pxz,k|k-1取代
Figure FDA00037110683100000513
得无迹信息滤波中量测向量zk对信息状态
Figure FDA00037110683100000514
的贡献形式：
Figure FDA0003711068310000061
无迹信息滤波中量测向量zk对信息矩阵IPk|k的贡献形式--信息状态的增量形式ik表示如下：
Figure FDA0003711068310000062
得对应的系统量测更新方程如下：
Figure FDA0003711068310000063
式中，
Figure FDA0003711068310000064
证毕。
6.如权利要求1所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准方法还包括：引入放大因子λ以抑制干扰噪声对初始对准结果的影响；
RUIKF的量测更新如下：
计算Sigma点如下式所示，计算传递Sigma点
Figure FDA0003711068310000065
Figure FDA0003711068310000066
计算观测量的先验估计
Figure FDA0003711068310000067
量测误差协方差的先验估计Pzz,k|k-1，新息协方差的先验估计Pee,k|k-1，以及状态量和观测量互协方差的先验估计Pxz,k|k-1：
Figure FDA0003711068310000071
Figure FDA0003711068310000072
Figure FDA0003711068310000073
Figure FDA0003711068310000074
Figure FDA0003711068310000075
选择k时刻的观测量
Figure FDA0003711068310000076
与观测量的先验估计
Figure FDA0003711068310000077
间的马氏距离作为评判指标，则k时刻评判指标
Figure FDA00037110683100000722
的定义如下式所示：
Figure FDA0003711068310000078
式中，
Figure FDA0003711068310000079
为马氏距离；Pzz,k|k-1为量测误差协方差阵的先验估计；μk为观测新息向量；对于真实的观测量
Figure FDA00037110683100000710
若评判指标
Figure FDA00037110683100000723
满足
Figure FDA00037110683100000711
则
Figure FDA00037110683100000712
将被标记为正常观测量；若评判指标
Figure FDA00037110683100000724
满足
Figure FDA00037110683100000713
则
Figure FDA00037110683100000714
将被标记为异常观测量，通过引入放大因子λk用于放大量测噪声协方差阵Rk：
Figure FDA00037110683100000715
Figure FDA00037110683100000716
转化为求解λk的非线性问题：
Figure FDA00037110683100000717
式中，λk通过牛顿迭代法求解；λk(i+1)与λk(i)的关系表示为：
Figure FDA00037110683100000718
式中，
Figure FDA00037110683100000719
且λk(i)初始值为λk(0)＝1；当评判指标满足
Figure FDA00037110683100000720
时，迭代终止；将概率参数α设置为0.99，鲁棒滤波方法的效率为99％，2自由度卡方分布值为
Figure FDA00037110683100000721
Figure FDA0003711068310000081
Kk＝(IPk|k-1+Ik)-1IPk|k-1Pxz,k|k-1(λkRk)-1；
Figure FDA0003711068310000082
Figure FDA0003711068310000083
IPk|k＝IPk|k-1+Ik；
Figure FDA0003711068310000084
7.一种应用如权利要求1～6任意一项所述的车载捷联惯导系统鲁棒无迹信息滤波对准方法的车载捷联惯导系统鲁棒无迹信息滤波对准系统，其特征在于，所述车载捷联惯导系统鲁棒无迹信息滤波对准系统包括：
初始对准模型构建模块，用于进行SINS初始对准模型的构建；
算法时间更新模块，用于进行UIKF算法的时间更新；
马氏距离计算模块，用于计算观测量与观测新息之间马氏距离，并计算观测新息对应权值；
R阵修正模块，用于基于马氏距离算法，引入放大因子λk，对估计出的R阵进行修正；
量测更新模块，用于利用修正后的R阵进行UIKF算法量测更新，实现RIKF量测更新；
误差修正模块，用于利用RUIKF估计出状态误差对SINS导航解算误差进行修正。
8.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
采用信息滤波框架下的无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
9.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
采用信息滤波框架下的无迹信息卡尔曼滤波方法UIKF，并利用统计学方法实现异常观测量的有效辨识；通过引入放大因子λ实现异常观测时对量测噪声协方差阵的放大，进而实现UIKF的鲁棒化，得到鲁棒UIKF。
10.一种信息数据处理终端，其特征在于，所述信息数据处理终端用于实现如权利要求7所述的车载捷联惯导系统鲁棒无迹信息滤波对准系统。
CN202210720421.6A 2022-06-23 2022-06-23 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统 Active CN115096321B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210720421.6A CN115096321B (zh) 2022-06-23 2022-06-23 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210720421.6A CN115096321B (zh) 2022-06-23 2022-06-23 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统

Publications (2)

Publication Number Publication Date
CN115096321A true CN115096321A (zh) 2022-09-23
CN115096321B CN115096321B (zh) 2024-08-06

Family

ID=83293781

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210720421.6A Active CN115096321B (zh) 2022-06-23 2022-06-23 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统

Country Status (1)

Country Link
CN (1) CN115096321B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116559966A (zh) * 2023-03-06 2023-08-08 中国人民解放军国防科技大学 基于sins/ldv组合的重力测量方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160223683A1 (en) * 2014-09-16 2016-08-04 Israel Military Industries Ltd. Gps-aided inertial navigation method and system
US20180274940A1 (en) * 2015-10-13 2018-09-27 Shanghai Huace Navigation Technology Ltd A method for initial alignment of an inertial navigation apparatus
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN113108797A (zh) * 2021-04-19 2021-07-13 上海商汤临港智能科技有限公司 车辆定位方法、装置、电子设备以及计算机可读存储介质
CN113218421A (zh) * 2021-05-11 2021-08-06 中国人民解放军63921部队 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
CN115096302A (zh) * 2022-06-23 2022-09-23 中国人民解放军63921部队 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160223683A1 (en) * 2014-09-16 2016-08-04 Israel Military Industries Ltd. Gps-aided inertial navigation method and system
US20180274940A1 (en) * 2015-10-13 2018-09-27 Shanghai Huace Navigation Technology Ltd A method for initial alignment of an inertial navigation apparatus
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN113108797A (zh) * 2021-04-19 2021-07-13 上海商汤临港智能科技有限公司 车辆定位方法、装置、电子设备以及计算机可读存储介质
CN113218421A (zh) * 2021-05-11 2021-08-06 中国人民解放军63921部队 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
CN115096302A (zh) * 2022-06-23 2022-09-23 中国人民解放军63921部队 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
ZHU BING: "Robust Initial Alignment Based on Unscented Information Kalman Filter", JOURNAL INFORMATION IEEE SENSORS JOURNAL, 21 November 2022 (2022-11-21) *
侯志勇: "捷联惯导初始对准与导航校准技术研究", 中国优秀硕士学位论文全文数据库 (信息科技辑), 15 April 2022 (2022-04-15) *
朱兵;许江宁;吴苗;卞鸿巍;李京书;: "水下动基座初始对准中的鲁棒自适应UKF方法", 仪器仪表学报, no. 02, 15 February 2018 (2018-02-15) *
郭晓艺;吴峻;: "GARUKF算法在无人船对准中的应用", 传感技术学报, no. 02, 15 February 2020 (2020-02-15) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116559966A (zh) * 2023-03-06 2023-08-08 中国人民解放军国防科技大学 基于sins/ldv组合的重力测量方法及系统

Also Published As

Publication number Publication date
CN115096321B (zh) 2024-08-06

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
Huang et al. Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
US6859727B2 (en) Attitude change kalman filter measurement apparatus and method
CN111323050B (zh) 一种捷联惯导和多普勒组合系统标定方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
Hu et al. Robust unscented Kalman filter-based decentralized multisensor information fusion for INS/GNSS/CNS integration in hypersonic vehicle navigation
CN111156994A (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN113625324A (zh) 一种在组合导航模式下实现无人机精确定点偏移的欺骗方法
CN110207694A (zh) 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
CN113074757A (zh) 车载惯导安装误差角的标定方法
CN115096321A (zh) 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统
Avzayesh et al. Improved-Performance Vehicle’s State Estimator Under Uncertain Model Dynam
CN115096302A (zh) 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端
CN104121930A (zh) 一种基于加表耦合的mems陀螺漂移误差的补偿方法
CN105928519A (zh) 基于ins惯性导航与gps导航以及磁力计的导航算法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
Zhu et al. Robust initial alignment based on unscented information Kalman filter
Eldesoky et al. Real time localization solution for land vehicle application using low-cost integrated sensors with GPS
Fang et al. Integrating SINS sensors with odometer measurements for land vehicle navigation system
CN118347496B (zh) 一种逆向导航方法及装置
CN118149803B (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