[发明专利]一种利用水面机器人跟踪水下机器人的方法

说明书

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种利用水面机器人跟踪水下机器人的方法，实现水面机器人自主执行跟踪水下机器人。 

背景技术

水下机器人在水下航行时，陆上操作人员需要一种跟踪水下机器人的装置和方法，实时了解水下机器人的航行状态，确认其工作是否正常。常规的跟踪装置和方法是在有人参与的情况下，用母船执行跟踪任务。该方法利用船载超短基线定位系统或者船载水声通信机得到的位置信息，然后母船上的操作人员据此调整母船航速和航向，使其跟踪水下机器人。由于受水深、航程、设施、人员成本等条件限制，常规跟踪方法往往对船只、人员配备等要求较高且使用复杂，成本非常高昂；由于受母船噪声影响，跟踪性能也较差，因此常规方法不能满足稳定跟踪的需要。 

发明内容

为了克服现有方法的不足，本发明要解决的技术问题是提供一种不需要人参与的自主跟踪水下机器人的方法。该方法不用配备船只和人员，利用水面机器人实现自主跟踪的目的。 

本发明为实现上述目的所采用的技术方案是：一种利用水面机器人跟踪水下机器人的方法，包括以下步骤： 

水下机器人启动安装在其背部的水声通信机MA，按周期向安装在水面机器人底部的水声通信机MB发射水下机器人的航行状态信息； 

水声通信机MB将所述航行状态信息发送给水面机器人计算机； 

水面机器人计算机计算并输出航向角控制指令和速度控制指令，使水面机器人按照该控制指令运动。 

所述航行状态信息包括水下机器人的航速、航向和地理坐标数据。 

所述水面机器人计算机通过自主跟踪算法计算并输出航向角控制指令，具 体算法为： 

计算水面机器人与水下机器人之间的距离： 

d＝[(x2-x1)²+(y2-y1)²+(z2-z1)²]^1/2

其中，水面机器人的位置为： 

x1＝Rsin[(90-lati1)·λ]cos(long1·λ) 

y1＝Rsin[(90-lati1)·λ]sin(long1·λ)； 

z1＝Rcos[(90-lati1)·λ] 

水下机器人的位置为： 

x2＝Rsin[(90-lati2)·λ]cos(long2·λ) 

y2＝Rsin[(90-lati2)·λ]sin(long2·λ)； 

z2＝Rcos[(90-lati2)·λ] R为地球半径，水面机器人的经纬度为(long1，lati1)，水下机器人的经纬度为(long2，lati2)； 

如果d＜300米，计算航向角α：如果|lati2-lati1|＜0.00001，在long2-long1＞0的情况下，α＝90度；否则，α＝270度；如果|lati2-lati1|≥0.00001，计算γ＝tan^-1[(long2-long1)cos(lati2·λ)，(lati2-lati1)]/λ：如果γ＜0，则有α＝γ+360；如果γ＞360，α＝γ-360； 

如果d≥300米，计算shead＝sin(|long2-long1|·λ)sin[(90-lati2)·λ]/sin(d/R)，计算航向角： 

i)如果|long2-long1|≥0且|lati2-lati1|≥0，则α＝sin^-1(shead)/λ，转到v)；否则转到ii)： 

ii)如果|long2-long1|＜0且|lati2-lati1|≥0，则α＝-sin^-1(shead)/λ，转到v)；否则转到iii)： 

iii)如果|long2-long1|＜0且|lati2-lati1|＜0，则α＝-90-cos^-1(shead)/λ，转到v)；否则转到iv)： 

iv)α＝90+cos^-1(shead)/λ，转到v)： 

v)如果α＜0，则有α＝α+360；如果α＞360，α＝α-360。 

所述水面机器人计算机通过自主跟踪算法计算并输出速度控制指令，具体算法为：

如果水面机器人与水下机器人的距离大于最大通信距离R_max，当航向角控制指令与水面机器人当前航向角之差β是锐角时，水面机器人加速航行，当航向角控制指令与水面机器人当前航向角之差β不是锐角时，水面机器人减速航行；