[发明专利]一种基于最大熵自适应抗差估计的USBL斜距修正方法

说明书

一种基于最大熵自适应抗差估计的USBL斜距修正方法，1)记录USBL发送请求报文时刻的SINS姿态转移矩阵、速度和位置信息，2)记录USBL接收到应答器报文时刻的方位角，SINS姿态、速度和位置信息，3)根据发送时刻和接收时刻SINS的解算结果修正USBL的斜距，4)根据3)修正的USBL斜距和2)采集的方位角、高度角和SINS解算信息，建立SINS/USBL紧组合系统模型，5)计算动力学模型误差自适应因子和根据最大熵原则计算增益矩阵，6)对5)的模型进行卡尔曼滤波融合，对SINS进行反馈校正，输出导航结果。重复上述步骤，直至导航结束，利用捷联惯导系统测量的姿态和位置建立USBL发射和接收时刻之间的变化模型，推导接收时刻的斜距修正值。最后，为了抑制异常噪声对定位精度的影响。

技术领域

本发明涉及水下航行器SINS/USBL组合导航技术，具体为一种基于最大熵自适应抗差估计的USBL斜距修正方法。

背景技术

SINS/USBL组合导航系统是实现水下AUV高精度导航定位的主要途径之一。由于USBL更新频率低，当报文发送和接收之间存在径向速度，传统的斜距计算方法将引入较大误差。并且，由于水下环境的复杂性和水声多径等因素的干扰，USBL的测量通常会包含较大的异常噪声。

发明内容

为解决现有技术中存在的问题，本发明提出了一种基于最大熵自适应抗差估计的USBL斜距修正方法，利用捷联惯导系统(SINS)测量的姿态和位置建立USBL发射和接收时刻之间的变化模型，推导接收时刻的斜距修正值。最后，为了抑制异常噪声对定位精度的影响。

一种基于最大熵自适应抗差估计的USBL斜距修正方法，包括：

步骤1：记录USBL发送请求报文时刻的SINS姿态转移矩阵、速度和位置信息；

在USBL发送请求报文时刻t₁，SINS姿态转移矩阵为速度更新结果为位置更新结果为表示惯导解算地理经度值，表示惯导解算地理纬度值，表示惯导解算高度值；

步骤2：记录USBL接收到应答器报文时刻的方位角，SINS姿态、速度和位置信息；在应答器报文接收时刻t₂,应答器发出的信号到基阵原点的声线与x轴和y轴的夹角分别为α和β，SINS姿态转移矩阵为速度更新结果为表示载体东向、北向、天向速度,位置更新结果为

步骤3：根据发送时刻和接收时刻SINS的解算结果修正USBL的斜距；

对USBL斜距进行修正，包括如下步骤：

3.1在t₂时刻，应答器在n系下的位置为USBL发送t₁时刻与接收时刻t₂之间，SINS更新了m个周期，则将t₂时刻惯性解算位置递推至t₁时刻

其中，M_PV(m)表示第m个采样周期时的SINS位置更新矩阵，

表示第m个采样周期惯性解算的速度信息，T表示SINS采样间隔；R_Nh＝R_N+h(m)，R_Mh＝R_M+h(m),R_e表示地球长半轴半径，e表示地球偏心率，h(m)表示第m个采样周期惯导解算的高度值，L(m)表示第m个采样周期惯导解算的纬度值。

3.2根据上式中的递推结果，计算的斜距：

3.3在t₁时刻，根据组合的高精度位置，计算发送时刻的斜距：

则应答器在α系下的位置表示为：