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

摘要

本发明提供一种水下机器人多声呐数据融合方法，在对声呐数据和姿态传感器融合的基础上，提高测量精度为目的，基于改进SH自适应滤波算法进行优化。本发明基于测距声呐的测距误差模型针对不同传感器进行时间对准，通过对AUV测距距离，艏向角，横摇角等传感器信号的测量数据融合，采用SH自适应卡尔曼滤波对数据进行修正，并针对算法鲁棒性较差的情况，对系统噪声进行自适应调整并适当减少对量测噪声估计的无偏性，实现对声呐测距数据的准确估计，并改进遗忘因子算法，增强系统鲁棒性。本发明较传统方法具有很大的优点，可用于各类水下机器人，具有深远的应用前景。

主权项

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‑1Z_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)^‑1P_k＝(I‑K_kH)P_k‑1其中，d_k为加权系数，为时间k处的后验状态估计，为过程噪声的协方差矩阵，P_k为误差协方差矩阵，为量测噪声的均值，为过程噪声的均值，是量测噪声协方差矩阵，K_k为增益矩阵，C_k为中间变量；步骤7：应用SH自适应扩展卡尔曼滤波算法并在其基础上进行如下改进：(1)去掉过程噪声的估算，将状态预测方程优化为⑵适当牺牲量测噪声协方差矩阵估计的无偏性：原测量噪声为：改进后的量测噪声变为：步骤8：采取预报残差法求取遗忘因子：步骤9：将得到的遗忘因子带入，对测量数据进行滤波，得到精确的滤波数据，完成机器人传感器数据的融合。