[发明专利]一种INS/USBL非线性紧组合综合校正方法

说明书

本发明属于卫星导航领域，具体涉及一种INS/USBL非线性紧组合综合校正方法，包括以下步骤：利用惯导的速度误差、位置误差、姿态角误差以及超短基线的入射角误差、测距精度误差作为状态量建立非线性状态方程，超短基线参数加入INS/USBL综合校正系统的状态量中建立USBL的误差模型，与INS的非线性误差模型共同构成INS/USBL综合校正系统的状态方程，使得入射角误差及测距误差得到补偿。本发明能够有效解决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算法基础上，采用容积规则设计适用于非线性系统的自适应容积卡尔曼滤波估计器，使系统在量测噪声发生异常时滤波估计器可稳定输出。

所述非线性量测方程为：

其中：