一种水下机器人多声呐数据融合方法技术

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

A Multi-sonar Data Fusion Method for Underwater Vehicles

【技术实现步骤摘要】
一种水下机器人多声呐数据融合方法
本专利技术涉及的是一种水下机器人多传感器数据处理方法，属于水下机器人
技术背景水下机器人在水下探测和水下作业任务中的应用越来越广泛。在机器人以对输水明渠的环境状态进行检测为任务，对机器人的测距声呐测量精度有很高要求。机器人工作时，在推进器作用下距墙壁指定距离，平行于墙壁航行。这对声呐的测量数据精度有很高要求，可是，受到水流及推进器动作等影响，机器人在航行中总存在着一定的艏摇和横摇，这增大了声呐测得的数据误差，增加了数据处理的难度；同时，由于声呐与姿态传感器工作频率不同，产生的数据不同步，也加大了数据处理的难度。因此，需采用自适应卡尔曼滤波算法对多传感器的数据进行处理，完成AUV距墙壁距离的估计。传统的自适应卡尔曼滤波利用量测数据进行实时估计并调整噪声统计特性参数，解决了系统过程噪声和量测噪声难以确定的问题，但其算法本身不能保证测量噪声协方差阵的正定性，容易导致大计算量和滤波发散。
技术实现思路
本专利技术目的在于提供一种在对声呐数据和姿态传感器融合的基础上，能提高测量精度为目的水下机器人多声呐数据融合方法。本专利技术的目的通过以下步骤实现：步骤1，机器人收到原始声呐测量数据rsf1，rsb1；步骤2，采用最小二乘时间配准算法进行传感器时间配准，得到姿态传感器当前时刻融合值：步骤3：进行如下判断：若满足声呐数据无效，重复步骤3，若满足声呐数据有效，转至步骤4，其中θ为侧向声呐声波反射点到水下机器人中心距离连线与其顶部声呐声波反射点距离连线之间的夹角，β是水下机器人的横摇误差角；步骤4：对声呐测量数据结合姿态传感数据进行初步融合，得到机器人距墙壁距离：d＝((rsf1+rsb1+2R1)cosαcosβ)/2；其中，d是机器人距离墙的实际距离，R1是机器人在声呐位置处半径，rsf1，rsb1分别是前左和后左侧测距声呐的测量距离，α是水下机器人的艏向误差角；步骤5：建立多声呐数据融合系统模型：Xk＝ΦXk-1+Γk-1Wk-1Zk＝HXk+Vk其中，Xk为系统状态变量，Zk为观测变量，Φ为状态转移矩阵，H为量测系数矩阵，Γk-1为状态噪声系数矩阵，Wk-1为过程噪声，Vk为量测噪声；步骤6：应用SH自适应扩展卡尔曼滤波算法，SH自适应扩展卡尔曼滤波算法如下dk＝(1-b)/(1-bk)Kk＝Pk/k-1HT(HPk/k-1HT+Rk-1)-1Pk＝(I-KkH)Pk-1其中，dk为加权系数，为时间k处的后验状态估计，为过程噪声的协方差矩阵，Pk为误差协方差矩阵，为量测噪声的均值，为过程噪声的均值，是量测噪声协方差矩阵，Kk为增益矩阵，Ck为中间变量；步骤7：应用SH自适应扩展卡尔曼滤波算法并在其基础上进行如下改进：⑴去掉过程噪声的估算，将状态预测方程优化为⑵适当牺牲量测噪声协方差矩阵估计的无偏性：原测量噪声为：改进后的量测噪声变为：步骤8：采取预报残差法求取遗忘因子：步骤9：将得到的遗忘因子带入，对测量数据进行滤波，得到精确的滤波数据，完成机器人传感器数据的融合。本专利技术针对传统算法鲁棒性较差的情况，对系统噪声进行自适应调整并适当减少对量测噪声估计的无偏性，实现对声呐测距数据的准确估计，并改进遗忘因子算法，增强系统鲁棒性，能更准确、稳定地完成机器人传感器数据的融合。与现有技术相比，本专利技术的优点在于：1、本专利技术提供一种多声呐数据融合方法，提高了声呐测距的精度。传统水下机器人声呐数据由于水下环境因素等影响，存在较大误差。2、本专利技术基于测距声呐的测距误差模型针对不同传感器进行时间对准，通过对AUV测距距离，艏向角，横摇角等传感器信号的测量数据融合，采用SH自适应卡尔曼滤波对数据进行修正，并针对算法鲁棒性较差的情况，对系统噪声进行自适应调整并适当减少对量测噪声估计的无偏性，实现对声呐测距数据的准确估计，并改进遗忘因子算法，增强系统鲁棒性。3、改进的SH自适应卡尔曼滤波算法重视每一时刻得到的新观测数据在对应时刻滤波中的地位，同时也对观测噪声的变化给予特别的重视。因此能够有效地剔除系统中的部分干扰，使得滤波结果更加精确，并且保证了改进算法的稳定性。附图说明图1是本专利技术的传感器布置示意图。图2是本专利技术的流程图。图3是本专利技术坐标系定义图。图4是本专利技术的误差分析示意图。具体实施方式如附图1所示，在机器人的左侧装有测距声呐①、②，当机器人左侧距离探测墙壁较近时，选取左侧两测距声呐的测量值进行分析，①、②位于机器人中横剖面上，且纵向距离机器人中心的距离相等，所以一般取两声呐的算术平均值作为声呐初始数据。④是机器人内部的姿态传感器，结合已知的墙壁角度可以测得机器人的航向角，还有横摇角和纵倾角，与声呐数据融合可得到接近真实值的容和数据。同理，当机器人右侧侧距离探测墙壁较近时，选取右侧两测距声呐③、⑤的测量值进行分析，测距声呐③、⑤与①、②关于机器人纵剖面对称，测量过程与左侧一致。该水下机器人的坐标系定义如下：如图3所示，一般的在潜艇和AUV的研究中，采用国际水池会议(ITTC)推荐的两种右手坐标系，如图2为AUV的坐标系示意图，其中E-ξηξ为大地坐标系，G-xyz为艇体坐标系，其中G为AUV的中心，AUV的速度U在G-xyz轴的投影分别为纵向速度u,横向速度v和垂向速度w。K，M，N分别为AUV的横摇力矩，纵倾力矩和转艏力矩。图2为基于改进SH自适应卡尔曼滤波方法的多声呐数据融合流程图，包括以下几个步骤：步骤1，机器人收到的原始声呐测量数据rsf1，rsb1和姿态数据α，β步骤2，采用最小二乘时间配准算法进行传感器时间配准，记声呐的采样周期为τ，而姿态传感器的采样周期为T，采样周期的比例系数为整数n，如果声呐最近一次目标状态的估计时刻记为(k-1)τ，则当前时刻可以表示为kτ＝[(k-1)τ+nT]。Zn＝(z1，z2...zn)T为k-1时刻到k时刻姿态传感器的测量序列，zn表示声呐的测量值，利用表示n个状态估计值的融合值及其倒数构成的测量集合。可以得到姿态传感器测量新值的表达式为：vi表示量测噪声。姿态传感器的量测向量表示为：Zn=WnU+Vn。向量Vn的均值为0，方差为若则利用最小二乘准则使最小需要对求偏导数等于0，得到：得到姿态传感器当前时刻融合值：步骤3：考虑到当机器人距水面较近工作且有较大横摇角时，侧向声呐声波会打到水面而不是侧壁从而出现无效数据，进行如下判断：若满足声呐数据无效，重复步骤3，若满足声呐数据有效，转至步骤4。其中θ为侧向声呐声波反射点到AUV中心距离连线与其顶部声呐声波反射点距离连线之间的夹角，β是机器人的横摇误差角。步骤4：结合图4，对有效声呐测量数据结合姿态传感数据进行初步融合，得到机器人距墙壁距离：d=((rsf1+rsb1+2R1)cosαcosβ)/2其中，d是机器人距离墙的实际距离，R1是机器人在声呐位置处半径，rsf1，rsb1分别是前左和后左侧测距声呐的测量距离，α是AUV的艏向误差角。步骤5：建立多声呐数据融合系统模型：Xk＝ΦXk-1+Γk-1Wk-1Zk＝HXk+Vk其中，Xk为系统状态变量，Zk为观测变量，Φ为状态转移矩阵，H为量测系数矩阵，Γk-1为状态噪声系数矩阵，Wk-1为过程噪声，Vk为量测噪声。步骤6：应用SH自适应扩展卡尔曼滤本文档来自技高网...

【技术保护点】
1.一种水下机器人多声呐数据融合方法，其特征在于：包括以下步骤：步骤1，机器人收到原始声呐测量数据rsf1，rsb1；步骤2，采用最小二乘时间配准算法进行传感器时间配准，得到姿态传感器当前时刻融合值：

【技术特征摘要】
1.一种水下机器人多声呐数据融合方法，其特征在于：包括以下步骤：步骤1，机器人收到原始声呐测量数据rsf1，rsb1；步骤2，采用最小二乘时间配准算法进行传感器时间配准，得到姿态传感器当前时刻融合值：步骤3：进行如下判断：若满足声呐数据无效，重复步骤3，若满足声呐数据有效，转至步骤4，其中θ为侧向声呐声波反射点到水下机器人中心距离连线与其顶部声呐声波反射点距离连线之间的夹角，β是水下机器人的横摇误差角；步骤4：对声呐测量数据结合姿态传感数据进行初步融合，得到机器人距墙壁距离：d=((rsf1+rsb1+2R1)cosαcosβ)/2；其中，d是机器人距离墙的实际距离，R1是机器人在声呐位置处半径，rsf1，rsb1分别是前左和后左侧测距声呐的测量距离，α是水下机器人的艏向误差角；步骤5：建立多声呐数据融合系统模型：Xk＝ΦXk-1+Γk-1Wk-1Zk＝HXk+Vk其中，Xk为系统状态变量，Zk为...

【专利技术属性】
技术研发人员：张国成，曹建，孙玉山，吴新雨，马陈飞，王相斌，冉祥瑞，徐昊，王元庆，张宸鸣，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23