[发明专利]一种适用于自主水下机器人的海底目标搜寻方法

权利要求书

1.一种适用于自主水下机器人的海底目标搜寻方法，其特征在于，包括以下步骤：

当自主水下机器人检测到目标方位后，进行分时方位量测路径的规划；

在分时方位量测路径上，利用自主水下机器人在一个周期内对目标的方位测距，计算目标位置的粗估计；

所述分时方位量测路径满足以下公式：

其中，点X_i和点X_i+1分别表示自主水下机器人在相邻时刻t_i和t_i+1的位置；c是线性化判定常数；是方位角α_i的标准差，i为第i个时间点，表示方位角α_i的单位向量；

根据自主水下机器人在一个周期内对目标位置的粗估计，反馈校正自主水下机器人累积航行误差，获得一个周期内自主水下机器人位置的优化估计集合；

在优化估计集合中更新自主水下机器人位置，重新计算新的周期内目标位置；

评估新的周期目标位置精度；

将各周期目标位置和精度估计进行循环数据融合，计算多周期条件下的目标位置以及对应精度；

自主规划水下机器人的目标搜索路径，实时估计各周期的目标位置，同时数据融合多周期目标位置估计和精度，计算出全局条件下目标位置估计和精度。

2.根据权利要求1所述的一种适用于自主水下机器人的海底目标搜寻方法，其特征在于，所述一个周期为相邻5个时间点。

3.根据权利要求1所述的一种适用于自主水下机器人的海底目标搜寻方法，其特征在于，所述优化估计集合通过以下方程组获得：

其中，表示自主水下机器人从时刻t_i到时刻t_i+4位置的优化集合，即到分别表示自主水下机器人时刻t_i到时刻t_i+4位置的优化估计；H_i表示时刻t_i目标方位角α_i与自主水下机器人位置的相关量测向量；H_List,i表示时刻t_i到时刻t_i+4目标方位角与自主水下机器人位置的相关量测向量集合；Z_i表示时刻t_i目标方位角α_i和自主水下机器人位置X_i构建的量测；X_i(1)和X_i(2)分别表示自主水下机器人位置X_i的北向位置、东向位置；X_ob,i(1)和X_ob,i(2)分别表示目标位置粗估计X_ob,i的北向位置、东向位置；r_i表示自主水下机器人位置X_i和目标位置X_ob,i的欧式距离；Z_List,i表示时刻t_i到时刻t_i+4的自主水下机器人位置和目标位置构建的量测集合。

4.根据权利要求1所述的一种适用于自主水下机器人的海底目标搜寻方法，其特征在于，所述评估新的周期目标位置精度具体为：

定义P_ob,i表示自主水下机器人基于从时刻t_i到时刻t_i+4方位角计算目标位置的精度，

其中，j表示索引序号，对应时刻是t_j；目标方位角α_j表示时刻t_j的目标方位角；r_j表示自主水下机器人位置X_j和目标位置X_ob,i的欧式距离；方差是舷侧阵声呐的设备工作参数。

5.根据权利要求1所述的一种适用于自主水下机器人的海底目标搜寻方法，其特征在于，所述计算多周期条件下的目标位置以及对应精度通过以下公式：

X_next＝[I-P_next(P_next+P_ob,i)^-1]X_next+P_next(P_next+P_ob,i)^-1*X_ob,i

P_next＝[I-P_next(P_next+P_ob,i)^-1]P_next

其中，X_ob，1和P_ob，1分别表示自主水下机器人从时刻t₁到时刻t₅对目标的位置估计和精度估计；X_next和P_next分别表示自主水下机器人当前对目标的位置估计和精度估计；I表示单位矩阵。