CN114279443B - 一种基于最大熵自适应抗差估计的usbl斜距修正方法 - Google Patents

Abstract

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

Description

一种基于最大熵自适应抗差估计的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₁时刻，根据组合的高精度位置，计算发送时刻的斜距：

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

其中，表示SINS与USBL之间的安装误差角，/>表示t₂时刻的姿态转移矩阵，/>表示n系到e系下的转移矩阵，/>表示SINS与USBL之间的杆臂误差；

3.4SINS计算的姿态转移矩阵为则将t₂时刻计算的斜距递推至t₁时刻：

3.5根据步骤3.4所计算的斜距，则t₂时刻的斜距修正为：

步骤4：根据步骤3修正的USBL斜距和步骤2采集的方位角、高度角和SINS解算信息，建立SINS/USBL紧组合系统模型；

在t₂时刻，建立系统模型，步骤如下：

4.1系统状态模型为：

其中，F(k)为状态转移矩阵，W(k)为系统噪声，状态向量X(k)为：

其中，X_USBL＝[θ_x θ_y θ_z δα δβ δD]^T；

其中，δθ＝[θ_x θ_y θ_z]^T表示安装角误差，[δα δβ]^T表示、测角误差，δD表示测距误差，φⁿ＝[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；δV＝[δV_E δV_N δV_N]^T表示载体东向、北向、天向速度误差；δP＝[δL δλ δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；

其中，F_USBL＝0_6×6，

其中，

F₂₂＝(Vⁿ×)F₁₂-(a×)

其中，ω_ie表示地球自转角速率。

4.2系统量测模型为：

Z(k)＝HX(k)+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；根据步骤4.3修正后的斜距以及t₂时刻测量的方位角，USBL量测信息表示为：

利用SINS位置计算得到应答器在a系下的相对位置矢量为则应答器的相对位置矢量/>和方位角、斜距之间的关系表达式为：

对上式求偏微分得：

其中，矢量为表述方便，记

则矩阵A_TS的表达式为：

根据SINS的位置计算的应答器在a系下的相对位置矢量为：

忽略高阶误差小量，则位置矢量误差为：

其中，

其中，C_λ＝cosλ(t₂)，则利用SINS位置及应答器位置计算得到的a系下的斜距方位角信息重新表述为：

其中，[α β R]^T为方位角及斜距的真值，

矩阵分别为

其中，

则系统量测为：

其中，H_u＝[0 0 -1]，δU＝[δα δβ δD]^T；

观测矩阵H的表达式为：

H＝[H_a 0_3×3 H_p0_3×6 H_IH_u]；

步骤5：计算动力学模型误差自适应因子和根据最大熵原则计算增益矩阵；

步骤6：对步骤5的模型进行卡尔曼滤波融合，对SINS进行反馈校正，输出导航结果。重复上述步骤，直至导航结束。

作为本发明进一步改进，步骤5计算增益矩阵，步骤如下：

5.1k时刻，状态预测值为：

5.2k时刻，根据量测方程，最小二乘无偏估计结果为：

5.3预测状态残差向量d_k为：

5.4定义误差判别统计量；

5.5根据IGGIII构造自适应因子α_k：

5.6根据最大熵原则，构造损失函数J(k)：

其中，P_X(k，k-1)表示状态预测向量的权矩阵，P_k，k-1＝F(k)P_k-1F(k)+G(k-1)W[G(k-1)]^T，/>e_ki表示e_k的第i个分量，/>

5.7取β＝σ²，对J(k)关于X(k)求导，并令导数为0，则

将上式对X(k)求解，则：

写成Kalman标准形式，则：

其中，

作为本发明进一步改进，步骤6进行卡尔曼滤波融合，步骤如下：

状态一步预测：

状态一步预测均方误差：

滤波增益：

状态估计：

状态估计均方误差阵：

P_k＝P_k-K_kH_kP_k|(k-1)。

与现有技术相比，本发明的有益效果为：

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

附图说明

图1为USBL发送报文和接收报文示意图；

图2为SINS位置信息递推示意图；

图3为本发明描述的基于最大熵自适应抗差估计的USBL斜距修正方法原理图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

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

一种基于最大熵自适应抗差估计的USBL斜距修正方法，其中USBL发送报文和接收报文示意图如图1所示，SINS位置信息递推示意图如图2所示，基于最大熵自适应抗差估计的USBL斜距修正方法原理图如图3所示：

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

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

步骤1：对USBL斜距进行修正：

(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个采样周期惯导解算的纬度值。

(2)根据(1)中的递推结果，计算的斜距：

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

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

(4)SINS计算的姿态转移矩阵为则将t₂时刻计算的斜距递推至t₁时刻：

(5)根据步骤4.2所计算的斜距，则t₂时刻的斜距修正为：

在t₂时刻，建立系统模型，步骤如下：

(1)系统状态模型为：

其中，F(k)为状态转移矩阵，W(k)为系统噪声。状态向量X(k)为：

其中，

X_USBL＝[θ_x θ_y θ_z δα δβ δD]^T。

其中，δθ＝[θ_x θ_y θ_z]^T表示安装角误差，[δα δβ]^T表示、测角误差，δD表示测距误差。

φⁿ＝[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；δV＝[δV_E δV_N δV_N]^T表示载体东向、北向、天向速度误差；δP＝[δL δλ δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏。

其中，F_USBL＝0_6×6，

其中，

F₂₂＝(Vⁿ×)F₁₂-(a×)

其中，ω_ie表示地球自转角速率。

计算增益矩阵，步骤如下：

(1)k时刻，状态预测值为：

(2)k时刻，根据量测方程，最小二乘无偏估计结果为：

(3)预测状态残差向量d_k为：

(4)定义误差判别统计量

(5)根据IGGIII构造自适应因子α_k：

(6)根据最大熵原则，构造损失函数J(k)：

其中，P_X(k，k-1)表示状态预测向量的权矩阵，P_k，k-1＝F(k)P_k-1F(k)+G(k-1)W[G(k-1)]^T。/>e_ki表示e_k的第i个分量，/>

(7)取β＝σ²，对J(k)关于X(k)求导，并令导数为0，则

将上式对X(k)求解，则：

写成Kalman标准形式，则：

其中，

进行卡尔曼滤波融合，步骤如下：

状态一步预测：

状态一步预测均方误差：

滤波增益：

状态估计：

状态估计均方误差阵：

P_k＝P_k-K_kH_kP_k|(k-1)

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (3)

1.一种基于最大熵自适应抗差估计的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₁时刻，根据组合的高精度位置，计算发送时刻的斜距：

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

其中，表示SINS与USBL之间的安装误差角，/>表示t₂时刻的姿态转移矩阵，/>表示n系到e系下的转移矩阵，/>表示SINS与USBL之间的杆臂误差；

3.4SINS计算的姿态转移矩阵为则将t₂时刻计算的斜距递推至t₁时刻：

3.5根据步骤3.4所计算的斜距，则t₂时刻的斜距修正为：

步骤4：根据步骤3修正的USBL斜距和步骤2采集的方位角、高度角和SINS解算信息，建立SINS/USBL紧组合系统模型；

在t₂时刻，建立系统模型，步骤如下：

4.1系统状态模型为：

其中，F(k)为状态转移矩阵，W(k)为系统噪声，状态向量X(k)为：

其中，

X_USBL＝[θ_x θ_y θ_z δ_α δ_β δ_D]^T；

其中，δθ＝[θ_x θ_y θ_z]^T表示安装角误差，[δα δβ]^T表示、测角误差，δD表示测距误差，φⁿ＝[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；δV＝[δV_E δV_N δV_N]^T表示载体东向、北向、天向速度误差；δP＝[δL δλ δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；

其中，F_USBL＝0_6×6，

其中，

F₂₂＝(Vⁿ×)F₁₂-(a×)

其中，ω_ie表示地球自转角速率；

4.2系统量测模型为：

Z(k)＝HX(k)+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；

根据步骤4.3修正后的斜距以及t₂时刻测量的方位角，USBL量测信息表示为：

利用SINS位置计算得到应答器在a系下的相对位置矢量为则应答器的相对位置矢量/>和方位角、斜距之间的关系表达式为：

对上式求偏微分得：

其中，矢量为表述方便，记

则矩阵A_TS的表达式为：

根据SINS的位置计算的应答器在a系下的相对位置矢量为：

忽略高阶误差小量，则位置矢量误差为：

其中，

其中，C_λ＝cosλ(t₂)，

则利用SINS位置及应答器位置计算得到的a系下的斜距方位角信息重新表述为：

其中，[α β R]^T为方位角及斜距的真值，

矩阵分别为

其中，

则系统量测为：

其中，H_u＝[0 0 -1]，δU＝[δα δβ δD]^T；

观测矩阵H的表达式为：

H＝[H_a 0_3×3 H_p0_3×6 H_I H_u]；

步骤5：计算动力学模型误差自适应因子和根据最大熵原则计算增益矩阵；

步骤6：对步骤5的模型进行卡尔曼滤波融合，对SINS进行反馈校正，输出导航结果，重复上述步骤，直至导航结束。

2.根据权利要求1所述的一种基于最大熵自适应抗差估计的USBL斜距修正方法，其特征在于：

步骤5计算增益矩阵，步骤如下：

5.1k时刻，状态预测值为：

5.2k时刻，根据量测方程，最小二乘无偏估计结果为：

5.3预测状态残差向量d_k为：

5.4定义误差判别统计量

5.5根据IGGⅢ构造自适应因子α_k：

5.6根据最大熵原则，构造损失函数J(k)：

其中，P_X(k,k-1)表示状态预测向量的权矩阵，

P_k,k-1＝F(k)P_k-1F(k)+G(k-1)W[G(k-1)]^T，e_ki表示e_k的第i个分量，/>

5.7取β＝σ²，对J(k)关于X(k)求导，并令导数为0，则

将上式对X(k)求解，则：

写成Kalman标准形式，则：

其中，

3.根据权利要求1所述的一种基于最大熵自适应抗差估计的USBL斜距修正方法，其特征在于：

步骤6进行卡尔曼滤波融合，步骤如下：

状态一步预测：

状态一步预测均方误差：

滤波增益：

状态估计：

状态估计均方误差阵：

P_k＝P_k-K_kH_kP_k|(k-1)。