1.一种水声定位自适应平滑滤波方法,包括:步骤S1:根据当前和上一次的解算结果,计算目标当前位置的直角坐标系x、y方向上的速度分量;步骤S2:根据当前位置的直角坐标系x、y方向上速度分量计算目标的下一次的位置预测值;步骤S3:获取目标下一次的位置的方位和距离的定位结果,转化为二维坐标;若获取的定位结果与预测值偏差超过设定阈值,则将预测值作为解算结果;否则将获取的定位结果作为解算结果;根据历史存储的解算结果的方差计算更新定位后目标的速度分量,若使用预测值的次数大于等于1,则使用预测值的次数减1;将解算结果存入历史解算存储数组,并将历史解算存储数组的第一位数据删除;重复执行上述步骤,获取下一次的解算结果;若未获取到定位结果,将预测值作为解算结果,将使用预测值的次数加1,当使用预测值的次数超过设定次数阈值时,认为定位目标已不在水中,不再输出解算结果。
2.根据权利要求1所述的水声定位自适应平滑滤波方法,其特征在于,还包括:检测历史解算存储数组中解算结果的数量,若达到设定数量阈值,则对外输出解算结果。
3.根据权利要求1所述的水声定位自适应平滑滤波方法,其特征在于,所述步骤S1中计算目标的当前直角坐标系x、y方向上的速度分量,表示为: ;其中, 和 分别为目标的当前直角坐标系x、y方向上的速度分量; 和 分别为目标的当前位置的x和y坐标; 和 分别为目标的上一次位置的x和y坐标。
4.根据权利要求3所述的水声定位自适应平滑滤波方法,其特征在于,所述步骤S2中计算目标的下一次的位置预测值,表示为: ;其中, 和 分别为目标的下一次的位置预测值。
5.根据权利要求1所述的水声定位自适应平滑滤波方法,其特征在于,所述步骤S3中获取下一次的方位和距离的定位结果,转化为二维坐标,表示为: ;其中, 和 为目标下一次位置x和y方向上的坐标值; 为目标下一次位置的距离; 为目标下一次位置的方位。
6.根据权利要求1所述的水声定位自适应平滑滤波方法,其特征在于,所述步骤S3中根据历史存储的解算结果的方差计算更新定位后目标的速度分量,包括:计算从当前的解算结果向前设定前向数量的解算结果的x和y坐标的方差 ,和从上一次的解算结果向前设定前向数量的解算结果的x和y坐标的方差 ;计算滤波的x和y方向上的 系数 和 ; ;根据最优选择法计算x和y方向上的速度更新系数 和 ; ;计算目标下一次位置x和y方向上的坐标值 和 ; ;其中, 和 分别为目标的当前位置的x和y坐标;根据计算的速度更新系数,更新定位目标二维直角坐标系下的速度分量 和 的值为 和 : ;其中,T表示一个工作周期的时长。
7.一种水声定位自适应平滑滤波系统,基于权利要求1-6任一所述方法实现,其特征在于,所述系统包括:计算当前位置速度分量模块,用于根据当前和上一次的解算结果,计算目标当前位置的直角坐标系x、y方向上的速度分量;计算下一次位置预测值模块,用于根据当前位置的直角坐标系x、y方向上速度分量计算目标的下一次的位置预测值;和获取下一次位置解算结果模块,用于获取目标下一次的位置的方位和距离的定位结果,转化为二维坐标;若获取的定位结果与预测值偏差超过设定阈值,则将预测值作为解算结果;否则将获取的定位结果作为解算结果;根据历史存储的解算结果的方差计算更新定位后目标的速度分量,若使用预测值的次数大于等于1,则使用预测值的次数减1;将解算结果存入历史解算存储数组,并将历史解算存储数组的第一位数据删除;若未获取到定位结果,将预测值作为解算结果,将使用预测值的次数加1,当使用预测值的次数超过设定次数阈值时,认为定位目标已不在水中,不再输出解算结果。