CN112034866A - 一种水下机器人跟踪制导的方法及装置 - Google Patents

Abstract

本发明提供了一种水下机器人跟踪制导的方法及装置，涉及跟踪制导技术领域，包括：获取水下机器人的水下深度，以及跟随设备与水下机器人的相对距离；根据水下深度和相对距离，确定水下机器人所属的跟踪区域；根据水下机器人所属的跟踪区域，控制跟随设备当前时刻的速度和/或艏向角。本发明利用水下深度和相对距离准确反映跟随设备和水下机器人的相对关系的变化，根据两者之间不同的相对关系，相应控制跟随设备当前时刻的速度和/或艏向角，以此保证对运动轨迹未知的水下机器人进行实时有效的跟踪。

Description

一种水下机器人跟踪制导的方法及装置

技术领域

本发明涉及跟踪制导技术领域，具体而言，涉及一种水下机器人跟踪制导的方法及装置。

背景技术

水下机器人(Autonomous Underwater Vehicle，简称为AUV)因其灵活性、智能性等特点，被广泛用于深海探索的多种任务之中。为了实时确定AUV位置，通常采用超短基线辅助导航定位AUV，但需要有海面的母船配合进行实时跟随，以保证水面与水下建立实时的通信联系。然而直接利用母船进行跟踪往往需要人工成本，且母船在进行AUV的跟踪期间难以同时进行其他任务，导致工作效率较大程度地降低。

水面无人艇(Unmanned Surface Vessel，简称为USV)作为海上的智能体应运而生，利用其优越的工作性能有效代替了母船，完成与AUV的协同工作，充当AUV的通信中介，以便给岸上的工作人员实时传输AUV获取的相关水下信息。USV作为AUV的通信中介，保持高效通信传输的关键就是USV能针对AUV进行准确的跟踪制导，保证两者之间良好的跟随状态。然而，在现有USV对AUV的跟踪制导控制方法中，往往是按照AUV预先规定的航迹路线或预先设置的虚拟引力点对USV进行路径规划。但仅仅让AUV和USV都按同样的设定路线前行，忽略了海洋复杂的环境对USV和AUV之间的相对状态的影响，两者之间极容易因为不可控的环境因素而无法保持良好的跟随状态，从而使USV无法完成通信传输。因而，如何自主保持USV对AUV高效的跟踪制导是亟待解决的问题。

发明内容

本发明解决的技术问题是如何自主保持USV对AUV高效的跟踪制导，为达上述目的，第一方面，本发明提供了一种水下机器人跟踪制导的方法，其包括：

获取水下机器人的水下深度，以及跟随设备与所述水下机器人的相对距离；

根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域；

根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角，以对所述水下机器人进行跟踪。

由此，本发明提供的水下机器人跟踪制导的方法利用了水下深度判断了当前水下机器人的运动状态，并利用相对距离判断跟随设备与水下机器人之间的相对运动状态，以此动态反映两者之间相对运动的关系。依据水下机器人的运动状态和两者之间的相对运动的关系，确定水下机器人实际所属的跟踪区域，依据不同的所属的跟踪区域，调节跟随设备当前时刻的速度和/或艏向角，以此实时依据实际两者的运动状态进行有效跟踪，实现跟随设备对水下机器人实时、高效的追踪。

进一步地，所述跟踪区域包括稳定跟踪域、过渡跟踪域和追逐域，所述根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域包括：

当所述相对距离小于第一半径时，所述水下机器人所属的跟踪区域为所述稳定跟踪域；

当所述相对距离介于所述第一半径与第二半径之间时，所述水下机器人所属的跟踪区域为所述过渡跟踪域；

当所述相对距离介于所述第二半径与第三半径之间时，所述水下机器人所属的跟踪区域为所述追逐域；

其中，根据所述水下深度确定所述第一半径、所述第二半径以及第三半径，所述第一半径小于所述第二半径，所述第二半径小于所述第三半径。

由此，本发明依据相对距离，划分不同的跟踪区域，以便后续针对不同的跟踪区域进行有效的控制，实时依据实际两者的运动状态进行有效跟踪。

进一步地，所述根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角包括：

根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点；

根据所述驱动引力点当前时刻的经纬度坐标和所述跟随设备当前时刻的经纬度坐标，确定相应的经度差和纬度差；

根据所述经度差和所述纬度差之商的反正切函数值，确定所述跟随设备当前时刻的艏向角。

由此，利用水下机器人所属的跟踪区域设置驱动引力点，以此根据水下机器人的运动状态设置跟随设备下一时刻需要到达的位置，再利用驱动引力点和跟随设备当前时刻的位置点计算相关的角度，以便跟随设备实现实时有效的跟踪。

进一步地，所述根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点包括：

当所述水下机器人所属的跟踪区域为所述稳定跟踪域时，所述驱动引力点为所述水下机器人当前时刻所处位置点；

当所述水下机器人所属的跟踪区域为所述追逐域时，所述驱动引力点为所述水下机器人下一时刻对应的位置预测点；

其中，根据预设的水平距离参数、预设的相对位置角度参数和所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述位置预测点的经纬度坐标，所述水平距离参数用于确定水下机器人与下一时刻预测位置的水平距离，所述相对位置角度用于确定水下机器人与下一时刻预测位置的相对位置方向角度。

由此，当水下机器人处于稳定跟踪域时，说明跟随设备和水下机器人之间的距离较小，不需过多地调整艏向角，只需要保证跟随设备艏向角使其始终指向水下机器人的当前位置即可，以便水下机器人脱离稳定跟踪域时能迅速反应机动。当水下机器人处于追逐域时，由于水下机器人处于较为危险的脱离追踪的区域，跟随设备将采用追逐行为策略机动跟踪目标，将水下机器人视为追逐的目标，追逐方向始终朝向水下机器人的下一时刻预测位置，即位置预测点，即可有效保证跟随设备在追逐域对水下机器人的跟踪制导。

进一步地，所述根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点包括：

当所述水下机器人所属的跟踪区域为所述过渡跟踪域时，判断所述水下机器人当前时刻的速度和所述水下机器人上一时刻的速度的方向是否一致；

若不一致，则所述驱动引力点为所述水下机器人当前时刻对应的第一虚拟引力点；

若一致，则所述跟随设备当前时刻的艏向角为所述水下机器人当前时刻的速度的方向角度；

其中，根据所述第一半径和所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述第一虚拟引力点的经纬度坐标。

由此，当水下机器人处于过渡跟踪域时，说明跟随设备和水下机器人虽脱离了稳定跟踪域，但两者之间的相对距离适中，在过渡跟踪域仍对水下机器人进行稳定的跟踪即可，但在确保水下机器人处在过渡跟踪域的同时，考虑到水下机器人进行机动转弯的情况(水下机器人当前时刻的速度和上一时刻的速度的方向不一致)，也要尽可能利用短的路径进行跟踪，因而根据水下机器人的运动状态确定相应的第一虚拟引力点，此时跟随设备的艏向角指向第一虚拟引力点，即可有效保证跟随设备在过渡跟踪域对水下机器人的跟踪制导。除此之外，当水下机器人处于过渡跟踪域时，考虑到水下机器人进行直线行驶的情况(水下机器人当前时刻的速度和上一时刻的速度的方向一致)，此时跟随设备的艏向角和水下机器人当前时刻的速度方向一致，即可有效保证跟随设备在过渡跟踪域对水下机器人的跟踪制导。

进一步地，所述根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角包括：

根据所述水下机器人所属的跟踪区域，设置多个边界比较距离；

根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度。

由此，在不同的跟踪区域，对应设置多个边界比较距离，以便依据不同的跟踪区域进行分段式控制速度，保证跟随设备对水下机器人的高效追踪。

进一步地，当所述水下机器人所属的跟踪区域为所述追逐域时，所述边界比较距离包括所述第二半径和危险边界半径，所述危险边界半径为所述第二半径与预设的半径调整量之和，所述第二半径小于所述危险边界半径，所述危险边界半径小于所述第三半径，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的艏向角包括：

当所述相对距离介于所述第二半径和所述危险边界半径之间时，所述跟随设备当前时刻的速度为所述水下机器人当前时刻的速度；

当所述相对距离大于所述危险边界半径且所述跟随设备上一时刻的速度小于预设的最大跟随设备速度时，所述跟随设备当前时刻的速度为所述水下机器人当前时刻的速度与预设的速度调整量之和；

当所述水下机器人当前时刻的速度与所述速度调整量之和大于所述最大跟随设备速度，则所述跟随设备当前时刻的速度保持为所述最大跟随设备速度。

由此，当水下机器人处于追逐域时，跟随设备需要采用强机动进行追逐，速度根据水下机器人速度和水下机器人所处边界位置而确定，距离较远则加速，距离较近则减速。当水下机器人刚离开过渡跟踪域、刚进入追逐域时，说明此刻需要加速，但若此时速度与距离直接成线性关系，则会在边界位置震动不平稳，故在设危险边界半径以此确定危险边界位置，此时跟随设备当前时刻的速度保持为水下机器人当前时刻的速度即可。但当水下机器人离开危险边界位置时，则说明两者相对距离越来越远，此时需要开始加速以拉近两者距离，因而跟随设备当前时刻的速度为水下机器人当前时刻的速度与预设的速度调整量之和即可。因此，有效根据实际运动状态进行追逐。当水下机器人当前时刻的速度和预设的速度调整量之和已经达到了最大跟随设备速度，说明此时水下机器人仍在高速运行，此时为了避免两者相对距离持续拉大，因而控制跟随设备当前时刻的速度保持最大，全速追逐即可。

进一步地，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度包括：

当所述水下机器人所属的跟踪区域为所述稳定跟踪域时，控制所述跟随设备当前时刻的速度为零。

由此，当水下机器人处于稳定跟踪域，说明两者相对距离较小，处于较安全的区域，此时跟随设备没有跟丢的风险，令跟随设备速度为0，以便更好地监控水下机器人的任务情况。

进一步地，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度包括：

当所述水下机器人所属的跟踪区域为所述过渡跟踪域时，根据所述水下机器人当前时刻对应的第一虚拟引力点和所述水下机器人下一时刻对应的第二虚拟引力点之间的距离差，确定第一距离；

根据当前时刻和下一时刻的时间差，确定相隔时间；

根据所述第一距离和所述相隔时间之商，确定所述跟随设备当前时刻的速度；

其中，根据所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述第一虚拟引力的经纬度坐标；根据所述水下机器人下一时刻对应的位置预测点的经纬度坐标，确定所述第二虚拟引力点的经纬度坐标。

由此，当水下机器人处于过渡跟踪域，说明两者相对距离适中，需要警惕相对距离的变大，开始加速尽量避免水下机器人脱离该区域到达追逐域，依据水下机器人的运动状态设置两个虚拟引力点，将此时跟随设备的速度设为两个虚拟路径点的位移与相隔时间的比值，即可保证一定的加速，避免两者相对距离持续增大。

为达上述目的，第二方面，本发明提供了一种水下机器人跟踪制导的装置，包括：

获取单元：用于获取水下机器人的水下深度，以及跟随设备与所述水下机器人的相对距离；

处理单元：用于根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域；

控制单元：用于根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角，以对所述水下机器人进行跟踪。

本发明提供的水下机器人跟踪制导的装置，具有与本发明第一方面提供的水下机器人跟踪制导的方法具有类似的有益效果，在此不再赘述。

附图说明

图1所示为本发明实施例的水下机器人跟踪制导的方法的流程示意图；

图2所示为本发明实施例的水下深度对应有效定位区域的区域示意图；

图3所示为本发明实施例的划分跟踪区域的区域示意图；

图4所示为本发明实施例的控制艏向角的流程示意图；

图5所示为本发明实施例的依据第一虚拟引力点控制艏向角的流程示意图；

图6所示为本发明实施例的确定第一虚拟引力点的流程示意图；

图7所示为本发明实施例的确定位置预测点的流程示意图；

图8所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图一；

图9所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图二；

图10所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图三；

图11所示为本发明实施例的控制速度的流程示意图一；

图12所示为本发明实施例的控制速度的流程示意图二；

图13所示为本发明实施例的控制速度的分段示意图；

图14所示为本发明实施例的水下机器人跟踪制导的装置的结构示意图。

具体实施方式

下面将参照附图详细描述根据本发明的实施例，描述涉及附图时，除非另有表示，不同附图中的相同附图标记表示相同或相似的要素。要说明的是，以下示例性实施例中所描述的实施方式并不代表本发明的所有实施方式。它们仅是与如权利要求书中所详述的、本发明公开的一些方面相一致的装置和方法的例子，本发明的范围并不局限于此。在不矛盾的前提下，本发明各个实施例中的特征可以相互组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

AUV广泛用于深海探索的多种任务之中，诸如水下安全搜救、水下目标观察、水下管道系统检修等。在AUV多种应用中，都需要将探测信息回传，维持其与工作人员之间的实时通讯是必不可少的。为了保持AUV的实时通信，采用海面的母船配合进行实时跟随，充当通信中介，以保证水面与水下建立实时的通信联系。当母船对AUV进行跟踪时，由于母船的操作复杂，往往对人员的配备要求较高。同时，母船在进行AUV的跟踪期间难以同时进行其他任务，导致工作效率较大程度地降低。因而，采用母船跟踪AUV的方式人工成本高、使用效率低下。

USV作为海上的智能体，在许多危险或者不必需要人力的情况下执行任务能起到很出色的效果。将USV与AUV配合工作，可以改善任务的整体运作、对任务的执行起到重大作用，有效克服了母船跟踪方式的种种缺陷。USV作为AUV的通信中介，给岸上的工作人员实时传输AUV获取的相关水下信息，在此过程中，保持高效通信传输的关键就是USV能针对AUV进行准确的跟踪制导，保证两者之间良好的跟随状态。

然而，在现有USV对AUV的跟踪制导控制方法中，往往是按照AUV预先规定的航迹路线或预先设置的虚拟引力点对USV进行路径规划。具体而言分两种方式，一种是AUV按照预先规定的航迹路线进行作业，USV沿着该路径针对AUV进行跟随任务，另一种则是已知AUV运动路线，并提前设计好虚拟路径点，要求USV与AUV在汇合面有一次握手协议确定两者的位置，AUV在进行工作时按照预期路线巡航，USV也按照预定义的虚拟路径点进行跟踪，一旦目标消失就前往下一个规划点提前等候。在上述现有技术中，仅仅让AUV和USV都按同样的设定路线前行，忽略了海洋复杂的环境对USV和AUV之间的相对状态的影响，比如，USV虽按预先设置的路线行驶到虚拟引力点，但由于障碍物、气流等因素的影响，AUV没有行驶到既定位置，此时，由于两者相对距离过大会导致通讯中断。综上，AUV和USV之间极容易因为不可控的环境因素而无法保持良好的跟随状态，从而使USV无法完成通信传输。因而，如何自主保持USV对AUV高效的跟踪制导是亟待解决的问题。

本发明第一方面的实施例提供了一种水下机器人跟踪制导的方法，结合图1来看，图1所示为本发明实施例的水下机器人跟踪制导的方法的流程示意图，包括步骤S1至S3，其中：

在步骤S1中，获取水下机器人的水下深度，以及跟随设备与水下机器人的相对距离。由此，利用获取的水下机器人的水下深度反映当前水下机器人的运动状态，并利用相对距离反映跟随设备与水下机器人之间的相对运动状态。

可选地，通过跟随设备上的多种传感装置获取水下机器人的水下深度，以及跟随设备与水下机器人的相对距离；或通过水下机器人向跟随设备发送自身的深度信息，跟随设备接收到深度信息后获取水下机器人的水下深度以及跟随设备与水下机器人的相对距离。以此有效利用传感装置或通信传递进行信息获取。

在步骤S2中，根据水下深度和相对距离，确定水下机器人所属的跟踪区域。由此，有效依据水下机器人的运动状态和两者之间的相对运动的关系，划分不同的跟踪区域，确定水下机器人实际所属的跟踪区域，以便后续针对不同的跟踪区域进行有效的控制，实时依据实际两者的运动状态进行有效跟踪。

可选地，跟踪区域包括稳定跟踪域、过渡跟踪域和追逐域。其中，稳定跟踪域为水下机器人和跟随设备之间的相对距离较小的区域，过渡跟踪域为水下机器人和跟随设备之间的相对距离适中的区域，追逐域为水下机器人和跟随设备之间的相对距离较大的区域。由此，分区域以便后续进行相应的控制。

可选地，跟随设备包括但不限于水面无人艇(USV)，以此高效完成和水下机器人的协同工作。可以理解的是，本发明实施例提供的跟随设备包括多种海面智能工作设备，只要能实现对水下机器人的跟踪制导即可。

可选地，步骤S2具体包括：当相对距离小于第一半径时，水下机器人所属的跟踪区域为稳定跟踪域。由此，设置第一半径有效区分水下机器人和跟随设备之间相对距离较小的情况，以便进行相应的控制。

可选地，步骤S2具体还包括：当相对距离介于第一半径与第二半径之间时，水下机器人所属的跟踪区域为过渡跟踪域。由此，设置第一半径和第二半径有效区分水下机器人和跟随设备之间相对距离适中的情况，以便进行相应的控制。

可选地，步骤S2具体还包括：当相对距离介于第二半径与第三半径之间时，水下机器人所属的跟踪区域为追逐域。由此，设置第二半径和第三半径有效区分水下机器人和跟随设备之间相对距离较大的情况，以便进行相应的控制。

其中，第一半径小于第二半径，第二半径小于第三半径。由此，设置第一半径、第二半径、第三半径的相对关系，有效划分稳定跟踪域、过渡跟踪域和追逐域。

其中，根据声学理论设置稳定跟踪域、过渡跟踪域和追逐域的范围，即根据水下深度确定第一半径、第二半径以及第三半径，具体包括：第一半径设置为水下机器人的水下深度的三分之一，第二半径设置为水下机器人的水下深度的三分之二，第三半径设置为水下机器人的水下深度。以此有效根据相对距离划分相应的区域。

在本发明实施例中，根据水下机器人的水下深度R_h设置第一半径R_st、第二半径R_tr以及第三半径R_ft，分别表示为下式(1)、(2)和(3)：

R_fr＝R_h (3)

具体地，结合图2来看，图2所示为本发明实施例的水下深度对应有效定位区域的区域示意图。在声学理论中，超短基线系统的有效跟踪范围是由射线声学理论确定的声波传播区域和由声呐方程确定的超短基线系统的作用距离综合确定，换能器发出的声脉冲信号的声波传播可看作是一束无数条垂直于等相位面的射线的传播。因而，对于水下深度为R_h(即图中的水下深度h等于R_h)的水下机器人，其有效区域由声线轨迹确定的传播边界(图2中曲线1所示)以及声呐参数确定的声波作用距离(图2中曲线2所示)共同确定，由此，根据水下深度为R_h能有效地利用声学原理，划分跟踪区域。

具体地，结合图3来看，图3所示为本发明实施例的划分跟踪区域的区域示意图，图中，R_st为第一半径，R_tr为第二半径，R_ft为第三半径，当水下机器人与跟随设备相对距离为(0，R_st)时，属于稳定跟踪域；当水下机器人与跟随设备相对距离为(R_st，R_tr)时，属于过渡跟踪域；当水下机器人与跟随设备相对距离为(R_st，R_ft)时，属于追逐域。

在步骤S3中，根据水下机器人所属的跟踪区域，控制跟随设备当前时刻的速度和/或艏向角，以对水下机器人进行跟踪。由此，确定水下机器人实际所属的跟踪区域后，依据不同的所属的跟踪区域，调节跟随设备当前时刻的速度和/或艏向角，以此实时依据实际两者的运动状态进行有效跟踪，实现跟随设备对水下机器人实时、高效的追踪。

可选地，步骤S3具体包括：根据水下机器人所属的跟踪区域控制跟随设备当前时刻的艏向角。结合图4来看，图4所示为本发明实施例的控制艏向角的流程示意图，包括步骤S31至步骤S33，其中：

在步骤S31中，根据水下机器人所属的跟踪区域，设置对应的驱动引力点。由此，根据不同的跟踪区域，设置对应的驱动引力点，使跟随设备指向驱动引力点，以跟随设备当前时刻所处位置点和驱动引力点之间形成的角度确定跟随设备当前时刻的艏向角，以此保证有效的驱动。

可选地，驱动引力点包括水下机器人当前时刻所处位置点。步骤S31具体包括：当水下机器人所属的跟踪区域为稳定跟踪域时，驱动引力点为水下机器人当前时刻所处位置点。由此，当水下机器人处于稳定跟踪域时，说明跟随设备和水下机器人之间的距离较小，不需过多地调整艏向角，只需要保证跟随设备艏向角使其始终指向水下机器人的当前位置即可，以便水下机器人脱离稳定跟踪域时能迅速反应机动。

可选地，驱动引力点包括水下机器人当前时刻对应的第一虚拟引力点，用于驱动水下机器人行驶至当前时刻位置点。结合图5来看，图5所示为本发明实施例的依据第一虚拟引力点控制艏向角的流程示意图，步骤S31具体包括步骤S311和步骤S312，其中：

在步骤S311中，当水下机器人所属的跟踪区域为过渡跟踪域时，判断水下机器人当前时刻的速度和水下机器人上一时刻的速度的方向是否一致。由此，依据判断速度方向的结果，有效确定是否存在机动转弯的情况。在本发明实施例中，水下机器人当前时刻的速度为

水下机器人上一时刻的速度为

比较两个时刻的速度方向，若一致，说明水下机器人当前时刻处于过渡跟踪域且沿直线行驶，若不一致，说明水下机器人当前时刻处于过渡跟踪域但正在进行机动转弯。

在步骤S312中，若方向不一致，则驱动引力点为水下机器人当前时刻对应的第一虚拟引力点。由此，当水下机器人处于过渡跟踪域时，说明跟随设备和水下机器人虽脱离了稳定跟踪域，但两者之间的相对距离适中，在过渡跟踪域仍对水下机器人进行稳定的跟踪即可，但在确保水下机器人处在过渡跟踪域的同时，考虑到水下机器人进行机动转弯的情况(水下机器人当前时刻的速度和上一时刻的速度的方向不一致)，也要尽可能利用短的路径进行跟踪，因而根据水下机器人的运动状态确定相应的第一虚拟引力点，此时，跟随设备当前时刻的艏向角指向第一虚拟引力点，即可有效保证跟随设备在过渡跟踪域对水下机器人的跟踪制导。

其中，根据第一半径和水下机器人当前时刻所处位置点的经纬度坐标，确定第一虚拟引力点当前时刻的经纬度坐标。具体地，结合图6来看，图6所示为本发明实施例的确定第一虚拟引力点的流程示意图，包括步骤S3121至步骤S3122，其中：

在步骤S3121中，根据水下机器人当前时刻所处位置点的经纬度坐标以及水下机器人当前时刻的速度矢量，确定水下机器人当前时刻的指向角度。由此，依据水下机器人当前时刻的运动状态，判断水下机器人指向第一虚拟引力点的角度。

在本发明实施例中，水下机器人当前时刻的指向角度θ表示为下式(4)：

其中，

为水下机器人当前时刻的速度矢量，v_1L为水下机器人沿经度方向的速度分量，v_1F为水下机器人沿经度方向的速度分量，ΔL为水下机器人当前时刻的经度位移，ΔF为水下机器人当前时刻的纬度位移，sgn(x)为符号函数，x<0取-1，x>0取+1，arctan为反正切函数。

在步骤S3122中，根据水下机器人当前时刻的指向角度、水下机器人当前时刻所处位置点的经纬度坐标、第一半径、预设的地球子午圈曲率半径以及预设的地球卯酉圈曲率半径，分别确定第一虚拟引力点的经度坐标和纬度坐标。

其中，第一虚拟引力点的经度坐标的确定具体包括：根据水下机器人当前时刻的指向角度的余弦值和预设的地球子午圈曲率半径之商，确定第一商值；根据第一半径和第一商值的乘积，确定第一乘积；根据水下机器人当前时刻所处位置点的经度坐标和第一乘积之和，确定第一虚拟引力点的经度坐标。由此，有效确定第一虚拟引力点的经度坐标。

其中，第一虚拟引力点的纬度坐标的确定具体包括：根据水下机器人当前时刻的指向角度的正弦值和第二乘积之商，确定第二商值，其中，第二乘积为预设的地球卯酉圈曲率半径和水下机器人当前时刻所处位置点的纬度坐标余弦值的乘积；根据第一半径和第二商值的乘积，确定第三乘积；根据水下机器人当前时刻所处位置点的纬度坐标和第三乘积之和，确定第一虚拟引力点的纬度坐标。由此，有效确定第一虚拟引力点的纬度坐标。

在本发明实施例中，第一虚拟引力点的经度坐标L₂和纬度坐标F₂表示为下式(5)：

其中，(L₁，F₁)是水下机器人当前时刻所处位置点的经纬度坐标，L₁为水下机器人当前时刻所处位置点的经度坐标，F₁为水下机器人当前时刻所处位置点的纬度坐标，R_st表示为第一半径，R_N为地球的子午圈曲率半径，R_M为地球的卯酉圈曲率半径，θ为水下机器人当前时刻的指向角度。

可选地，当水下机器人当前时刻的速度和水下机器人上一时刻的速度的方向一致时，则跟随设备当前时刻的艏向角为水下机器人当前时刻的速度的方向角度。由此，当水下机器人处于过渡跟踪域时，考虑到水下机器人进行直线行驶的情况(水下机器人当前时刻的速度和上一时刻的速度的方向一致)，此时跟随设备的艏向角和水下机器人当前时刻的速度方向一致，即可有效保证跟随设备在过渡跟踪域对水下机器人的跟踪制导。在本发明实施例中，当水下机器人当前时刻的速度和水下机器人上一时刻的速度的方向一致时，则不设置驱动引力点，水下机器人当前时刻的速度为

则跟随设备当前时刻的艏向角ψ_u为水下机器人当前时刻的速度的方向角度，即

可选地，驱动引力点包括水下机器人下一时刻对应的位置预测点。步骤S31具体包括：当水下机器人所属的跟踪区域为追逐域时，驱动引力点为水下机器人下一时刻对应的位置预测点。由此，当水下机器人处于追逐域时，由于水下机器人处于较为危险的脱离追踪的区域，跟随设备将采用追逐行为策略机动跟踪目标，将水下机器人视为追逐的目标，追逐方向始终朝向水下机器人的下一时刻预测位置，即位置预测点，即可有效保证跟随设备在追逐域对水下机器人的跟踪制导。

其中，根据预设的水平距离参数、预设的相对位置角度参数和水下机器人当前时刻所处位置点的经纬度坐标，确定位置预测点的经纬度坐标，水平距离参数用于确定水下机器人与下一时刻预测位置的水平距离，相对位置角度用于确定水下机器人与下一时刻预测位置的相对位置方向角度。其中，水平距离参数代表水下机器人与下一时刻预测位置的水平距离，相对位置角度为水下机器人与下一时刻预测位置的相对位置方向角度。具体地，结合图7来看，图7所示为本发明实施例的确定位置预测点的流程示意图，包括步骤S3101至步骤S3102，其中：

在步骤S3101中，根据水下机器人当前时刻所处位置点的经度坐标、预设的水平距离参数以及预设的相对位置角度参数，确定位置预测点的经度坐标。由此，有效确定位置预测点的经度坐标。

其中，位置预测点的经度坐标的确定具体包括：根据相对位置角度参数的余弦值和预设的地球子午圈曲率半径之商，确定第三商值；根据水平距离参数和第三商值的乘积，确定第四乘积；根据水下机器人当前时刻所处位置点的经度坐标和第四乘积之和，确定位置预测点的经度坐标。

在步骤S3102中，根据水下机器人当前时刻所处位置点的纬度坐标、预设的水平距离参数以及预设的相对位置角度参数，确定位置预测点的纬度坐标。由此，有效确定位置预测点的纬度坐标。

其中，位置预测点的纬度坐标的确定具体包括：根据相对位置角度参数的正弦值和第五乘积，确定第四商值，其中，第五乘积为预设的地球卯酉圈曲率半径和水下机器人当前时刻所处位置点的纬度坐标余弦值的乘积；根据水平距离参数和第四商值的乘积，确定第六乘积；根据水下机器人当前时刻所处位置点的经度坐标和第六乘积之和，确定位置预测点的纬度坐标。

在本发明实施例中，位置预测点的经度坐标L₃和纬度坐标F₃表示为下式(6)：

其中，(L₁，F₁)是水下机器人当前时刻所处位置点的经纬度坐标，L₁为水下机器人当前时刻所处位置点的经度坐标，F₁为水下机器人当前时刻所处位置点的纬度坐标，R_pre表示为预设的水平距离参数，R_N为地球的子午圈曲率半径，R_M为地球的卯酉圈曲率半径，

为预设的相对位置角度参数。由此，由于位置预测点是水下机器人下一时刻的预测位置点，因而根据预设的水平距离参数R_pre、预设的相对位置角度参数

进行有效预测其经纬度坐标。

在步骤S32中，根据驱动引力点当前时刻的经纬度坐标和跟随设备当前时刻的经纬度坐标，确定相应的经度差和纬度差。由此，有效利用经度差和纬度差反映两者之间的相对位置。

在步骤S33中，根据经度差和纬度差之商的反正切函数值，确定跟随设备当前时刻的艏向角。由此，利用水下机器人所属的跟踪区域设置驱动引力点，以此根据水下机器人的运动状态设置跟随设备下一时刻需要到达的位置，再利用驱动引力点和跟随设备当前时刻的位置点计算相关的角度，以便跟随设备实现实时有效的跟踪。

在本发明实施例中，当水下机器人所属的跟踪区域为稳定跟踪域时，驱动引力点为水下机器人当前时刻所处位置点，跟随设备当前时刻的艏向角ψ_u表示为下式(7)：

其中，(L₀，F₀)是跟随设备当前时刻所处位置点的经纬度坐标，L₀为跟随设备当前时刻所处位置点的经度坐标，F₀为跟随设备当前时刻所处位置点的纬度坐标，(L₁，F₁)是水下机器人当前时刻所处位置点的经纬度坐标，L₁为水下机器人当前时刻所处位置点的经度坐标，F₁为水下机器人当前时刻所处位置点的纬度坐标，arctan为反正切函数。

具体地，结合图8来看，图8所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图一，图中，(L₀，F₀)是跟随设备当前时刻所处位置点的经纬度坐标，(L₁，F₁)是水下机器人当前时刻所处位置点的经纬度坐标，R_st为第一半径，因而，水下机器人当前时刻位于稳定跟踪域内，跟随设备指向水下机器人当前时刻所处位置点，对应的驱动引力点即为水下机器人当前时刻所处位置点，图中所标识的角度ψ_u即为跟随设备当前时刻的艏向角。

在本发明实施例中，当水下机器人所属的跟踪区域为过渡跟踪域时，驱动引力点为第一虚拟引力点，跟随设备当前时刻的艏向角ψ_u表示为下式(8)：

其中，(L₀，F₀)是跟随设备当前时刻所处位置点的经纬度坐标，L₀为跟随设备当前时刻所处位置点的经度坐标，F₀为跟随设备当前时刻所处位置点的纬度坐标，(L₂，F₂)是第一虚拟引力点的经纬度坐标，L₂为第一虚拟引力点的经度坐标，F₂为第一虚拟引力点的纬度坐标，第一虚拟引力点的经纬度坐标(L₂，F₂)通过式(5)计算得出，arctan为反正切函数。

具体地，结合图9来看，图9所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图二，图中，水下机器人处于过渡跟踪域且进行机动转弯，此时令跟随设备的艏向指向第一虚拟引力点(L₂，F₂)，第一虚拟引力点与水下机器人的距离为第一半径R_st，其与水下机器人当前时刻所处位置点的连线垂直与水下机器人当前时刻的速度，并指向水下机器人上一位置方向。因而，水下机器人当前时刻位于稳定跟踪域内且进行机动转弯时，对应的驱动引力点即为第一虚拟引力点(L₂，F₂)，图中所标识的角度ψ_u即为跟随设备当前时刻的艏向角。

在本发明实施例中，当水下机器人所属的跟踪区域为追逐域时，跟随设备当前时刻的艏向角ψ_u表示为下式(9)：

其中，(L₀，F₀)是跟随设备当前时刻所处位置点的经纬度坐标，L₀为跟随设备当前时刻所处位置点的经度坐标，F₀为跟随设备当前时刻所处位置点的纬度坐标，(L₃，F₃)是位置预测点的经纬度坐标，L₃为位置预测点的经度坐标，F₃为位置预测点的纬度坐标，位置预测点的经纬度坐标(L₃，F₃通过式(6)计算得出。

具体地，结合图10来看，图10所示为本发明实施例的跟随设备当前时刻和驱动引力点的位置示意图三，图中，位置预测点为水下机器人下一时刻的预测位置点，由水下机器人预设的水平距离参数R_pre、预设的相对位置角度参数

决定，以此进行有效地预测位置预测点的经纬度坐标，图中所标识的角度ψ_u即为跟随设备当前时刻的艏向角。

可选地，步骤S3具体包括：根据水下机器人所属的跟踪区域控制跟随设备当前时刻的速度。根据所属的跟踪区域的特点，依据不同的方式计算不同跟踪区域的速度。一般而言，在稳定跟踪区域内，无丢失联系的风险，因而跟随设备当前时刻的速度设置可以为静止，保证更好的通信传递；在过渡跟踪域内，为了防止相对距离进一步地扩大，则要根据水下机器人的方向和速度实时调整跟随设备当前时刻的速度；在追逐域内，两者相对距离已经过大，因而必须根据相对距离的远近不断调整跟随设备当前时刻的速度进行追逐，同时也要保证追逐时加速的平稳。其中，具体地，结合图11来看，图11所示为本发明实施例的控制速度的流程示意图一，包括步骤S34至步骤S35，其中：

在步骤S34中，根据水下机器人所属的跟踪区域，设置多个边界比较距离。为了使边界稳定，因而设置多个边界比较距离，方便分段加速，保证速度的平稳过渡。

可选地，当水下机器人所属的跟踪区域为追逐域时，边界比较距离包括第二半径和危险边界半径，危险边界半径为第二半径与预设的半径调整量之和，第二半径小于危险边界半径，危险边界半径小于第三半径。由此，由于从过渡跟踪域到追逐域意味着相对距离的变大，因而需要加速，为保证加速的平稳性，设置危险边界半径，以此在过渡跟踪域和追逐域之间设置一段速度平稳过渡的路程，保证加速的有效性。在本发明实施例中，边界比较距离设为第二半径R_tr和预设的半径调整量ΔR之和，即R_tr+ΔR。

在步骤S35中，根据相对距离和边界比较距离的大小关系，控制跟随设备当前时刻的速度。由此，在不同的跟踪区域，对应设置多个边界比较距离，以便依据不同的跟踪区域进行分段式控制速度，保证跟随设备对水下机器人的高效追踪。

可选地，步骤S35具体包括：当相对距离介于第二半径和危险边界半径之间时，跟随设备当前时刻的速度为水下机器人当前时刻的速度。由此，当水下机器人处于追逐域时，跟随设备需要采用强机动进行追逐，速度根据水下机器人速度和水下机器人所处边界位置而确定，距离较远则加速，距离较近则减速。当水下机器人刚离开过渡跟踪域、刚进入追逐域时，说明此刻需要加速，但若此时速度与距离直接成线性关系，则会在边界位置震动不平稳，故在设危险边界半径以此确定危险边界位置，此时跟随设备当前时刻的速度保持为水下机器人当前时刻的速度即可。

在本发明实施例中，若相对距离R介于第二半径R_tr和危险边界半径R_tr+ΔR之间，即R_tr<R≤R_tr+ΔR，则跟随设备当前时刻的速度v表示为下式(10)：

其中，

为水下机器人当前时刻的速度标量，

为水下机器人当前时刻的速度矢量，v_1L为水下机器人沿经度方向的速度分量，v_1F为水下机器人沿经度方向的速度分量，

根据v_1L、v_1F的平方和而确定。

可选地，步骤S35具体还包括：当相对距离大于危险边界半径且跟随设备上一时刻的速度小于预设的最大跟随设备速度时，跟随设备当前时刻的速度为水下机器人当前时刻的速度与预设的速度调整量之和。由此，当水下机器人离开危险边界位置时，则说明两者相对距离越来越远，此时需要开始加速以拉近两者距离，因而跟随设备当前时刻的速度为水下机器人当前时刻的速度与预设的速度调整量之和即可，有效根据实际运动状态进行追逐。

在本发明实施例中，若相对距离R大于危险边界半径R_tr+ΔR，且跟随设备上一时刻的速度v′小于预设的最大跟随设备速度v_max，即(R_tr+ΔR<R)&(v′<v_max)，则跟随设备当前时刻的速度v表示为下式(11)：

其中，

为水下机器人当前时刻的速度标量，

为水下机器人当前时刻的速度矢量，v_1L为水下机器人沿经度方向的速度分量，v_1F为水下机器人沿经度方向的速度分量，

根据v_1L、v_1F的平方和而确定，Dv为速度调整量。

可选地，步骤S35具体还包括：当水下机器人当前时刻的速度与预设的速度调整量之和大于最大跟随设备速度，则跟随设备当前时刻的速度保持为最大跟随设备速度。由此，当水下机器人当前时刻的速度和预设的速度调整量之和已经达到了最大跟随设备速度，说明此时水下机器人仍在高速运行，此时为了避免两者相对距离持续拉大，因而控制跟随设备当前时刻的速度保持最大，全速追逐即可。

在本发明实施例中，若水下机器人当前时刻的速度

(标量)与预设的速度调整量Dv之和大于最大跟随设备速度v_max，即

则跟随设备当前时刻的速度v表示为下式(12)：

v＝v_max (12)

其中，v_max为最大跟随设备速度。

可选地，步骤S35具体还包括：当水下机器人所属的跟踪区域为稳定跟踪域时，控制跟随设备当前时刻的速度为零。由此，当水下机器人处于稳定跟踪域，说明两者相对距离较小，处于较安全的区域，此时跟随设备没有跟丢的风险，令跟随设备速度为0，以便更好地监控水下机器人的任务情况。在本发明实施例中，当水下机器人处于稳定跟踪域，则是相对距离小于第一半径R_st，即0≤R≤R_st，此时，跟随设备当前时刻的速度v＝0。

可选地，当水下机器人所属的跟踪区域为过渡跟踪域时，需警惕相对距离的变大。结合图12来看，图12所示为本发明实施例的控制速度的流程示意图二，包括步骤S36至S38，其中：

在步骤S36中，当水下机器人所属的跟踪区域为过渡跟踪域时，根据水下机器人当前时刻对应的第一虚拟引力点和水下机器人下一时刻对应的第二虚拟引力点之间的距离差，确定第一距离。由此，有效确定两点之间的距离差。

在步骤S37中，根据当前时刻和下一时刻的时间差，确定相隔时间。由此，有效确定距离差对应的时间差。

在步骤S38中，根据第一距离和相隔时间之商，确定跟随设备当前时刻的速度。由此，当水下机器人处于过渡跟踪域，说明两者相对距离适中，需要警惕相对距离的变大，开始加速尽量避免水下机器人脱离该区域到达追逐域，依据水下机器人的运动状态设置两个虚拟引力点，将此时跟随设备的速度设为两个虚拟路径点的位移与相隔时间的比值，即可保证一定的加速，避免两者相对距离持续增大。

其中，根据水下机器人当前时刻所处位置点的经纬度坐标，确定第一虚拟引力点当前时刻的经纬度坐标；根据水下机器人下一时刻预测位置点的经纬度坐标，确定第二虚拟引力点的经纬度坐标。第二虚拟引力点的经纬度坐标确定方式与第一虚拟引力点的确定方式一致，具体地，第一虚拟引力点、水下机器人下一时刻位置预测点的经纬度坐标的详细确定方式参见前文，在此不再赘述。

在本发明实施例中，若水下机器人所属的跟踪区域为过渡跟踪域，则相对距离介于第一半径R_st和第二半径R_tr之间，即R_st<R≤R_tr，则跟随设备当前时刻的速度v表示为下式(13)：

其中，(L₂，F₂)是第一虚拟引力点的经纬度坐标，L₂为第一虚拟引力点的经度坐标，F₂为第一虚拟引力点的纬度坐标，(L′₂，F′₂)为第二虚拟引力点的经纬度坐标，L′₂为第二虚拟引力点对应的虚拟引力点的经度坐标，F′₂为第二虚拟引力点的纬度坐标，Δt为当前时刻和下一时刻的时间差，即相隔时间。

在本发明实施例中，结合图13来看，图13所示为本发明实施例的控制速度的分段示意图，其中，对于相对距离对应的不同范围，跟随设备当前时刻的速度v表示为下式(14)：

其中，R为相对距离，R_st为第一半径，R_tr为第二半径，ΔR为半径调整量，R_tr+ΔR为危险边界半径，(L₂，F₂)是第一虚拟引力点的经纬度坐标，L₂为第一虚拟引力点的经度坐标，F₂为第一虚拟引力点的纬度坐标，(L′₂，F′₂)为第二虚拟引力点的经纬度坐标，L′₂为第二虚拟引力点的经度坐标，F′₂为下一时刻预测位置对应的虚拟引力点的纬度坐标，Δt为当前时刻和下一时刻的时间差，即相隔时间，

为水下机器人当前时刻的速度标量，

为水下机器人当前时刻的速度矢量，v_1L为水下机器人沿经度方向的速度分量，v_1F为水下机器人沿经度方向的速度分量，v′为跟随设备上一时刻的速度，

根据v_1L、v_1F的平方和而确定，Dv为速度调整量，v_max为最大跟随设备速度。

本发明第一方面实施例提供的水下机器人跟踪制导的方法利用了水下深度判断了当前水下机器人的运动状态，并利用相对距离判断跟随设备与水下机器人之间的相对运动状态，以此动态反映两者之间相对运动的关系。依据水下机器人的运动状态和两者之间的相对运动的关系，确定水下机器人实际所属的跟踪区域，依据不同的所属的跟踪区域，调节跟随设备当前时刻的速度和/或艏向角，以此实时依据实际两者的运动状态进行有效跟踪，实现跟随设备对水下机器人实时、高效的追踪。

本发明第二方面的实施例还提供了一种水下机器人跟踪制导的装置。图14所示为本发明实施例的水下机器人跟踪制导的装置1400的结构示意图，包括获取单元1401、处理单元1402、以及控制单元1403。

获取单元1401：用于获取水下机器人的水下深度，以及跟随设备与水下机器人的相对距离；

处理单元1402：用于根据水下深度和相对距离，确定水下机器人所属的跟踪区域；

控制单元1403：用于根据水下机器人所属的跟踪区域控制跟随设备当前时刻的速度和/或艏向角，以对水下机器人进行跟踪。

水下机器人跟踪制导的装置1400的各个单元的更具体实现方式可以参见对于本发明的水下机器人跟踪制导方法的描述，且具有与之相似的有益效果，在此不再赘述。

本发明第三方面的实施例提出了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时，实现根本发明第一方面实施例的水下机器人跟踪制导方法。

一般来说，用于实现本发明方法的计算机指令的可以采用一个或多个计算机可读的存储介质的任意组合来承载。非临时性计算机可读存储介质可以包括任何计算机可读介质，除了临时性地传播中的信号本身。

计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言，特别是可以使用适于神经网络计算的Python语言和基于TensorFlow、PyTorch等平台框架。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

本发明第四方面的实施例提供了一种计算设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时，实现本发明第一方面实施例的水下机器人跟踪制导方法。

根据本发明第三、四方面的计算机可读存储介质和计算设备，可以参照根据本发明第一方面实施例具体描述的内容实现，并具有与本发明第一方面实施例的水下机器人跟踪制导方法具有类似的有益效果，在此不再赘述。

尽管上面已经示出和描述了本发明的实施例，应当理解的是，上述实施例是示例性的，不能解释为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种水下机器人跟踪制导的方法，其特征在于，包括：

获取水下机器人所处的水下深度，以及跟随设备与所述水下机器人的相对距离；

根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域；

根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角，以对所述水下机器人进行跟踪。

2.如权利要求1所述的水下机器人跟踪制导的方法，其特征在于，所述跟踪区域包括稳定跟踪域、过渡跟踪域和追逐域，所述根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域包括：

当所述相对距离小于第一半径时，确定所述水下机器人所属的跟踪区域为所述稳定跟踪域；

当所述相对距离介于所述第一半径与第二半径之间时，确定所述水下机器人所属的跟踪区域为所述过渡跟踪域；

当所述相对距离介于所述第二半径与第三半径之间时，确定所述水下机器人所属的跟踪区域为所述追逐域；

其中，根据所述水下深度确定所述第一半径、所述第二半径以及第三半径，所述第一半径小于所述第二半径，所述第二半径小于所述第三半径。

3.如权利要求2所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角包括：

根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点；

根据所述驱动引力点当前时刻的经纬度坐标和所述跟随设备当前时刻的经纬度坐标，确定相应的经度差和纬度差；

根据所述经度差和所述纬度差之商的反正切函数值，确定所述跟随设备当前时刻的艏向角。

4.如权利要求3所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点包括：

当所述水下机器人所属的跟踪区域为所述稳定跟踪域时，所述驱动引力点为所述水下机器人当前时刻所处位置点；

当所述水下机器人所属的跟踪区域为所述追逐域时，所述驱动引力点为所述水下机器人下一时刻对应的位置预测点；

其中，根据预设的水平距离参数、预设的相对位置角度参数和所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述位置预测点的经纬度坐标，所述水平距离参数用于确定水下机器人与下一时刻预测位置的水平距离，所述相对位置角度用于确定水下机器人与下一时刻预测位置的相对位置方向角度。

5.如权利要求3所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述水下机器人所属的跟踪区域，设置对应的驱动引力点包括：

当所述水下机器人所属的跟踪区域为所述过渡跟踪域时，判断所述水下机器人当前时刻的速度和所述水下机器人上一时刻的速度的方向是否一致；

若不一致，则所述驱动引力点为所述水下机器人当前时刻对应的第一虚拟引力点；

若一致，则所述跟随设备当前时刻的艏向角为所述水下机器人当前时刻的速度的方向角度；

其中，根据所述第一半径和所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述第一虚拟引力点的经纬度坐标。

6.如权利要求2所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角包括：

根据所述水下机器人所属的跟踪区域，设置多个边界比较距离；

根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度。

7.如权利要求6所述的水下机器人跟踪制导的方法，其特征在于，当所述水下机器人所属的跟踪区域为所述追逐域时，所述边界比较距离包括所述第二半径和危险边界半径，所述危险边界半径为所述第二半径与预设的半径调整量之和，所述第二半径小于所述危险边界半径，所述危险边界半径小于所述第三半径，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度包括：

当所述相对距离介于所述第二半径和所述危险边界半径之间时，所述跟随设备当前时刻的速度为所述水下机器人当前时刻的速度；

当所述相对距离大于所述危险边界半径且所述跟随设备上一时刻的速度小于预设的最大跟随设备速度时，所述跟随设备当前时刻的速度为所述水下机器人当前时刻的速度与预设的速度调整量之和。

当所述水下机器人当前时刻的速度与所述速度调整量之和大于所述最大跟随设备速度，则所述跟随设备当前时刻的速度保持为所述最大跟随设备速度。

8.如权利要求6所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度包括：

当所述水下机器人所属的跟踪区域为所述稳定跟踪域时，控制所述跟随设备当前时刻的速度为零。

9.如权利要求6所述的水下机器人跟踪制导的方法，其特征在于，所述根据所述相对距离和所述边界比较距离的大小关系，控制所述跟随设备当前时刻的速度包括：

当所述水下机器人所属的跟踪区域为所述过渡跟踪域时，根据所述水下机器人当前时刻对应的第一虚拟引力点和所述水下机器人下一时刻对应的第二虚拟引力点之间的距离差，确定第一距离；

根据当前时刻和下一时刻的时间差，确定相隔时间；

根据所述第一距离和所述相隔时间之商，确定所述跟随设备当前时刻的速度；

其中，根据所述水下机器人当前时刻所处位置点的经纬度坐标，确定所述第一虚拟引力的经纬度坐标；根据所述水下机器人下一时刻对应的位置预测点的经纬度坐标，确定所述第二虚拟引力点的经纬度坐标。

10.一种水下机器人跟踪制导的装置，其特征在于，包括：

获取单元：用于获取水下机器人的水下深度，以及跟随设备与所述水下机器人的相对距离；

处理单元：用于根据所述水下深度和所述相对距离，确定所述水下机器人所属的跟踪区域；

控制单元：用于根据所述水下机器人所属的跟踪区域，控制所述跟随设备当前时刻的速度和/或艏向角，以对所述水下机器人进行跟踪。

Images