一种INS/USBL非线性紧组合综合校正方法技术

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

AN INS/USBL NONLINEAR COMPACT COMPREHENSIVE CORRECTION METHOD

The invention belongs to the field of satellite navigation, and specifically relates to an INS/USBL non-linear tightly integrated correction method, which includes the following steps: using the velocity error, position error, attitude angle error of inertial navigation and the incident angle error and ranging accuracy error of ultra-short baseline as state variables, establishing a non-linear state equation, and adding ultra-short baseline parameters into the state variables of INS/USBL integrated correction system. The error model of USBL is established, and the state equation of INS/USBL integrated correction system is constructed with the non-linear error model of INS, so that the incident angle error and ranging error can be compensated. The invention can effectively solve the problem of low navigation accuracy and weak anti-interference ability caused by abnormal noise in INS/USBL loose integrated navigation using absolute position as observation information.

【技术实现步骤摘要】
一种INS/USBL非线性紧组合综合校正方法
本专利技术属于卫星导航领域，具体涉及一种INS/USBL非线性紧组合综合校正方法。
技术介绍
水下航行器在校正导航信息时需上浮接收全球定位系统(GlobalNavigationSatelliteSystem，GNSS)信号，这不仅使得水下航行器需要消耗一定的能量和时间，并且会大幅增加暴露的可能性。而INS/USBL综合校正技术可通过超短基线定位系统(UltraShortBaseLine，USBL)在水下校正惯性导航系统(InertialNavigationSystem，INS)而不需上浮接收GNSS信号即可获得连续、高精度的信息，从而节省了水下航行器的能量和时间，保证了水下航行器的工作效率。但常规USBL与INS的组合常用位置作为量测信息，这需要USBL测得入射角和斜距后再解算出相应的位置或角度做量测量，使USBL解算频率更低，且此设计中滤波参数设置无针对性，最终导致滤波精度不高，且在实际工程应用中很难避免非线性问题。《DesignandExperimentalValidationofaUSBLUnderwaterAcousticPositioningSystem》(发表于期刊《Sensors》，2016年，16期，9卷)一文中，提出建立INS/USBL组合导航的非线性模型，并利用扩展卡尔曼滤波(ExtendedKalmanFilters，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＝(xeyeze)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＝(xeyeze)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°。航行器以5nmile/h的速度围绕应答器螺旋向下行驶600s；INS的解算频率本文档来自技高网...

【技术保护点】
1.一种INS/USBL非线性紧组合综合校正方法，其特征在于，包括下述步骤：(1)利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程，超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型，与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程，使得入射角误差及测距误差得到补偿；(2)利用惯导解算的应答器相对于惯导在基阵坐标系下的入射角、斜距信息与超短基线输出的入射角、斜距信息之差作为量测量构建非线性量测方程；(3)在改进的Sage‑Husa算法基础上，采用容积规则设计适用于非线性系统的自适应容积卡尔曼滤波估计器，使系统在量测噪声发生异常时滤波估计器可稳定输出。

【技术特征摘要】
1.一种INS/USBL非线性紧组合综合校正方法，其特征在于，包括下述步骤：(1)利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程，超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型，与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程，使得入射角误差及测距误差得到补偿；(2)利用惯导解算的应答器相对于惯导在基阵坐标系下的入射角、斜距信息与超短基线输出的入射角、斜距信息之差作为量测量构建非线性量测方程；(3)在改进的Sage-Husa算法基础上，采用容积规则设计适用于非线性系统的自适应容积卡尔曼滤波估计器，使系统在量测噪声发生异常时滤波估计器可稳定输出。2.根据权利要求1所述的一种INS/USBL非线性紧组合综合校正方法，...

【专利技术属性】
技术研发人员：程建华，董萍，李亮，范世龙，王诺，朱宇轩，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23