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

说明书

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

技术领域

本发明涉及的是一种水下机器人多传感器数据处理方法，属于水下机器人技术领域。

技术背景

水下机器人在水下探测和水下作业任务中的应用越来越广泛。在机器人以对输水明渠的环境状态进行检测为任务，对机器人的测距声呐测量精度有很高要求。

机器人工作时，在推进器作用下距墙壁指定距离，平行于墙壁航行。这对声呐的测量数据精度有很高要求，可是，受到水流及推进器动作等影响，机器人在航行中总存在着一定的艏摇和横摇，这增大了声呐测得的数据误差，增加了数据处理的难度；同时，由于声呐与姿态传感器工作频率不同，产生的数据不同步，也加大了数据处理的难度。因此，需采用自适应卡尔曼滤波算法对多传感器的数据进行处理，完成AUV距墙壁距离的估计。

传统的自适应卡尔曼滤波利用量测数据进行实时估计并调整噪声统计特性参数，解决了系统过程噪声和量测噪声难以确定的问题，但其算法本身不能保证测量噪声协方差阵的正定性，容易导致大计算量和滤波发散。

发明内容

本发明目的在于提供一种在对声呐数据和姿态传感器融合的基础上，能提高测量精度为目的水下机器人多声呐数据融合方法。

本发明的目的通过以下步骤实现：

步骤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自适应扩展卡尔曼滤波算法如下