[发明专利]一种水下机器人多声呐数据融合方法

权利要求书

1.一种水下机器人多声呐数据融合方法，其特征在于：包括以下步骤：

步骤1，机器人收到原始声呐测量数据r_sf1，r_sb1；

步骤2，采用最小二乘时间配准算法进行传感器时间配准，得到姿态传感器当前时刻融合值：

步骤3：进行如下判断：

若满足声呐数据无效，重复步骤3，

若满足声呐数据有效，转至步骤4，

其中θ为侧向声呐声波反射点到水下机器人中心距离连线与其顶部声呐声波反射点距离连线之间的夹角，β是水下机器人的横摇误差角；

步骤4：对声呐测量数据结合姿态传感数据进行初步融合，得到机器人距墙壁距离：

d=((r_sf1+r_sb1+2R₁)cosαcosβ)/2；

其中，d是机器人距离墙的实际距离，R₁是机器人在声呐位置处半径，r_sf1，r_sb1分别是前左和后左侧测距声呐的测量距离，α是水下机器人的艏向误差角；

步骤5：建立多声呐数据融合系统模型：

X_k＝ΦX_k-1+Γ_k-1W_k-1

Z_k＝HX_k+V_k

其中，X_k为系统状态变量，Z_k为观测变量，Φ为状态转移矩阵，H为量测系数矩阵，Γ_k-1为状态噪声系数矩阵，W_k-1为过程噪声，V_k为量测噪声；

步骤6：应用SH自适应扩展卡尔曼滤波算法，SH自适应扩展卡尔曼滤波算法如下

d_k＝(1-b)/(1-b^k)

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k-1)^-1

P_k＝(I-K_kH)P_k-1

其中，d_k为加权系数，为时间k处的后验状态估计，为过程噪声的协方差矩阵，P_k为误差协方差矩阵，为量测噪声的均值，为过程噪声的均值，是量测噪声协方差矩阵，K_k为增益矩阵，C_k为中间变量；

步骤7：应用SH自适应扩展卡尔曼滤波算法并在其基础上进行如下改进：

(1)去掉过程噪声的估算，将状态预测方程优化为

⑵适当牺牲量测噪声协方差矩阵估计的无偏性：

原测量噪声为：

改进后的量测噪声变为：

步骤8：采取预报残差法求取遗忘因子：

步骤9：将得到的遗忘因子带入，对测量数据进行滤波，得到精确的滤波数据，完成机器人传感器数据的融合。