CN112445243B - 一种适用于自主水下机器人的海底目标搜寻方法 - Google Patents

Abstract

本发明涉及水下机器人目标搜索技术领域，尤其涉及一种自主水下机器人的海底目标搜寻方法，本发明通过观测性分析技术实现搜索路径的在线规划，同时利用目标的位置估计反馈校正自主水下机器人累积导航误差，抑制平台对目标位置估计的干扰，实现自主水下机器人对目标位置的精确估计。本方法能够有效地处理水下目标搜索，自主规划搜寻路径，改善系统可观测性；采用自主水下机器人导航位置和目标位置滚动优化策略，抑制自主水下机器人导航累积误差对目标估计的干扰，提高了目标位置搜索精度，具有较强的工程应用价值；本方法移植方便，扩展性强，也适用于无人船、半潜式自主水下机器人等的海底目标搜索应用领域。

Description

一种适用于自主水下机器人的海底目标搜寻方法

技术领域

本发明涉及水下机器人目标搜索技术领域，尤其涉及一种无人自主水下机器人(简称AUV)的海底目标搜寻方法，实现自主水下机器人基于方位测量对海底目标搜寻并定位。

背景技术

在海洋工程、海洋调查、海洋搜救过程中，自主水下机器人发挥越来越重要的作用。近年来，在马航MH370残骸搜寻事件中，自主水下机器人充当了深海目标搜寻的重要角色。传统基于声学图像的目标探测是使用有源声学探测载荷，而自主水下机器人有限的能源制约了有源声学探测载荷半径的提升，所以需要研究一种基于低能耗、高探测效率探测载荷的自主水下机器人目标搜寻技术。舷侧阵声呐是一种无源目标探测载荷，能够测量具有声源特征的水下目标相对方位，具有低能耗、探测半径大的技术优势。研究适用于自主水下机器人平台基于目标方位测量的目标搜寻技术，即基于舷侧阵声呐的自主水下机器人目标搜寻技术，是提高水下目标搜寻效率的关键技术。传统适用于自主水下机器人的基于目标方位测量的搜索技术，将自主水下机器人与搜索目标的方位角作为直接观测量，根据分时目标方位角组成的观测方程组来间接计算目标位置，实现自主水下机器人平台成功搜索目标并估计目标位置。传统适用于自主水下机器人的基于方位测量的目标搜索技术存在以下缺点:(1)基于目标方位角的观测方程是非线性间接观测方程，该特征制约了目标位置估计精度的进一步提高，有时甚至存在目标位置估计无法收敛的技术风险；(2)传统模型假设在分时方位测量期间，自主水下机器人机动航行不会引入自身的导航误差，而现实环境中自主水下机器人本体运动产生的导航误差是不可忽略的，该误差是影响目标位置估计精度的关键要素。所以，适用于自主水下机器人的基于方位测量的目标搜索技术需要解决两个关键技术难题，第一，如何保证目标观测方程的可观测性，如何高效融合目标方位角测量进而获得精确的目标位置估计，即保证基于方位测量的目标可观测性，实现对目标位置的实时可估计；第二，如何抑制观测平台自身运动而引入的导航误差，防止目标位置估计精度随着时间的增加而下降，即抑制平台运动噪声对目标位置估计的干扰，实现对目标位置的精确估计。

发明内容

本发明涉及水下机器人目标搜索技术领域，尤其涉及一种自主水下机器人(简称AUV)的海底目标搜寻方法，针对改善目标可观测性、提高搜索目标的位置估计精度等关键技术问题，提出了一种适用于自主水下机器人的基于方位测量的目标搜索方法。

本发明为实现上述目的所采用的技术方案是：

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

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

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

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

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

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

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

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

所述一个周期为相邻5个时间点。

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

其中，点X_i和点X_i+1分别表示自主水下机器人在相邻时刻t_i和t_i+1的位置；c是线性化判定常数；

是方位角α_i的标准差，i为第i个时间点。

所述优化估计集合通过以下方程组获得：

其中，

表示自主水下机器人从时刻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的自主水下机器人位置和目标位置构建的量测集合。

所述评估新的周期目标位置精度具体为：

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

其中，j表示索引序号，对应时刻是t_j；目标方位角α_j表示时刻t_j的目标方位角；r_j表示自主水下机器人位置X_j和目标位置X_ob,i的欧式距离；方差

是舷侧阵声呐的设备工作参数。

所述计算多周期条件下的目标位置以及对应精度通过以下公式：

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表示单位矩阵。

本发明具有以下有益效果及优点：

1.相对于传统基于方位测量的目标搜索，本方法使用目标搜索算法既能够改善目标的可观测性，也能够高效融合目标方位测量，提高了目标位置估计精度。

2.应用范围广。本发明不但可以应用于AUV平台水下目标搜索，还可以用于其它潜航器的水下目标搜索。

3.为了有效融合目标方位测量，改善目标观测方程的可观测，同时降低AUV运动噪声对目标位置估计精度的影响，本发明将目标搜索定位与自主水下机器人定位优化相结合，有效抑制AUV平台累积误差对目标估计的干扰，提高目标精确搜索。

附图说明

图1a是本发明的组成示意图一；

图1b是本发明的组成示意图二；

图2是本发明的流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

整个系统组成包括1台自主水下机器人、自主水下机器人搭载的多普勒计程仪、深水罗经、舷侧阵声呐等设备。其中自主水下机器人是搭载多普勒计程仪、深水罗经、舷侧阵声呐的运载具，多普勒计程仪、深水罗经是自主水下机器人的内部导航传感器，其中多普勒计程仪的作用是测量自主水下机器人相对于海底的航行速度；深水罗经的作用是测量自主水下机器人的航向角、纵倾角、横滚角等姿态角；舷侧阵声呐是目标探测传感器，它的作用是测量自主水下机器人与目标的方位角，系统组成如图1a～图1b所示。

如图2所示，整个系统按照如下流程工作：

为了描述方便，首先对一些变量进行定义。定义X_i＝(x_i,y_i)^T表示自主水下机器人在时刻t_i的空间位置，其中x_i,,y_i,分别表示北向位置，东向位置,i是时刻t的索引序号，时刻t索引序号的最大值是n。定义α_i表示在时刻t_i自主水下机器人与目标的方位角，定义

表示方位角α_i的单位向量，即

定义

表示方位角α_i的随机误差，它服从方差

的正态分布，即

方位角是舷侧阵声呐对目标的测量值，方差

也是舷侧阵声呐的设备工作参数，该参数由设备厂家提供。为了方便描述矩阵和向量的运算，定义T表示矩阵或向量的转置算子，||·||表示向量的2范数算子。

第一步，规划分时方位量测路径

当自主水下机器人检测到目标方位后，自主水下机器人开始规划分时方位量测路径，对量测路径的要求如下：以当前位置为中心进行机动航行同时测量目标方位，要求量测路径满足可观测条件，测量路径可观测判别依据是如下判别公式：

其中点X_i和点X_i+1分别表示自主水下机器人在相邻时刻t_i和t_i+1的位置；c是线性化判定常数，工程上可以取经验值0.9；

是方位角α_i的标准差，它是舷侧阵声呐的设备工作参数，该参数由设备厂家提供，一般情况下σ_α取值小于0.03弧度。该判别式的物理含义是在考虑方位角误差干扰的前提下，当相邻时刻观测路径满足判别式时，那么该路径对目标具有可观测性。所以在实际工程应用过程中，建议方位角量测路径采用多边形轨迹(边数不少于6条)，相邻分时方位路径点距离不小于100米。

第二步，计算目标位置的粗估计

利用自主水下机器人相邻5个时间点对目标的方位测距，计算目标的粗估计。定义X_ob,i表示自主水下机器人从时刻t_i到时刻t_i+4对目标位置的粗估计，它是通过求解如下方程组获得：

其中B_i表示时刻t_i目标方位角α_i的量测向量，它是中间变量，它的计算方法见公式(3)；B_List,i表示时刻t_i到时刻t_i+4目标方位角的量测向量集合，它是中间变量；Y_i表示时刻t_i目标方位角α_i和自主水下机器人位置X_i构建的量测，它是中间变量，它的计算方法见方程组(4)；Y_List,i表示时刻t_i到时刻t_i+4目标方位角和自主水下机器人位置构建的量测集合，它是中间变量；X_ob,i是方程组中的待求解位置变量。

B_i＝[tan(α_i)-1],for i＝1to n (3)

其中α_i表示时刻t_i目标的方位角，它由是舷侧阵声呐提供，是已知量；n表示对目标的最大测量次数，也是时刻的最大索引，即最后一次对目标方位进行测量的时刻为t_n。

其中α_i表示时刻t_i目标的方位角，它由是舷侧阵声呐提供，是已知量；n表示对目标的最大测量次数，即最后一次对目标方位进行测量的时刻为t_n；Γ(t_i)表示自主水下机器人在时刻t_i的运动迁移矩阵，它是由时刻t_i的航向角ψ(t_i)、纵倾角θ(t_i)、航行速度u(t_i)构建的矩阵，它是中间变量；Δt是自主水下机器人的运动周期，它是系统参数，用户可以自行设定，建议取值为1；ψ(t_i)、θ(t_i)分别表示自主水下机器人在时刻t_i的航向角、纵倾角，它们由自主水下机器人搭载的深水罗经提供，是已知运动变量；u(t_i)表示主水下机器人在时刻t_i的航行速度，它由自主水下机器人搭载的多普勒计程仪提供，是已知运动变量；X₁表示自主水下机器人在时刻t₁的位置，它是已知量；X_i和X_i-1分别表示自主水下机器人在时刻t_i、时刻t_i-1的位置，X_i是需要求解的中间变量，很明显，X_i可以由上一时刻t_i-1的位置X_i-1递推计算，且X₁是已知量，那么任意时刻位置X_i均能从X₁或X_i-1递推求解。

第三步，优化自主水下机器人位置

利用自主水下机器人在相邻5个时间点对目标位置的粗估计和目标在海底保持静止的物理特征，反馈校正自主水下机器人累积航行误差，获得相邻5个时间点自主水下机器人位置的优化估计集合

定义

表示自主水下机器人从时刻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的自主水下机器人位置和目标位置构建的量测集合，它是中间变量；

是方程组中的待求解变量集合。

第四步，反馈优化本周期目标位置

将

中更新自主水下机器人位置，调用步骤二重新计算目标位置X_ob,i，它的计算过程按照表1所示。

表1反馈优化本周期的目标位置

第五步，评估本周期目标位置精度。

定义P_ob,i表示自主水下机器人基于从时刻t_i到时刻t_i+4方位角计算目标位置的精度，它是通过求解如下方程组获得

其中j表示索引序号，对应时刻是t_j；目标方位角α_j表示时刻t_j的目标方位角，它由舷侧阵声呐提供；r_j表示自主水下机器人位置X_j和目标位置X_ob,i的欧式距离，它是中间变量；方差

是舷侧阵声呐的设备工作参数，它是已知量；P_ob,i是方程组中的待求解变量。

第六步，计算多周期目标位置

数据融合各周期目标位置和精度估计，计算多周期条件下的目标位置，同时给出目标位置估计的精度。定义X_ob表示自主水下机器人从时刻t₁到时刻t_n对目标位置的估计，定义P_ob表示自主水下机器人从时刻t₁到时刻t_n对目标位置估计的精度，它们是待求解变量，按照下表方法计算获得：

表2多周期目标位置的数据融合方法

其中，X_ob，1和P_ob，1分别表示自主水下机器人从时刻t₁到时刻t₅对目标的位置估计和精度估计，对于该步骤而言，它们是已知输入量；X_next和P_next分别表示自主水下机器人当前对目标的位置估计和精度估计，它们是中间变量；I表示单位矩阵。

第七步，计算全局目标位置

自主规划水下机器人的目标搜索路径，实时估计各周期的目标位置，同时数据融合多周期目标信息，最后计算出全局条件下目标位置估计和精度。定义X_ob表示自主水下机器人从时刻t₁到时刻t_n对目标位置的估计，即全局目标位置；定义P_ob表示自主水下机器人从时刻t₁到时刻t_n对目标位置估计的精度，即全局目标位置的精度，它们通过下表进行求解。

表3计算全局条件下的目标位置

本发明通过观测性分析技术实现搜索路径的在线规划，同时利用目标的位置估计反馈校正自主水下机器人累积导航误差，抑制平台对目标位置估计的干扰，实现自主水下机器人对目标位置的精确估计。本发明包括：首先，自主水下机器人自主规划量测路径，建立对目标的有效观测；然后，基于当前方位信息对目标位置进行初始估计，同时利用目标运动特征反馈校正自主水下机器人本身导航位置，实现目标位置估计和自主水下机器人位置估计的滚动优化；最后，数据融合多周期目标信息，实现对目标位置估计的全局优化，并给出目标位置的估计精度。本方法能够有效地处理水下目标搜索问题，自主规划搜寻路径，改善系统可观测性；采用自主水下机器人导航位置和目标位置滚动优化策略，抑制自主水下机器人导航累积误差对目标估计的干扰，提高了目标位置搜索精度，具有较强的工程应用价值；本方法移植方便，扩展性强，也适用于无人船、半潜式自主水下机器人等的海底目标搜索应用领域。

Claims (5)

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表示单位矩阵。

Images