一种利用水面机器人跟踪水下机器人的方法

1.一种利用水面机器人跟踪水下机器人的方法，其特征在于，包括以下步骤：
水下机器人启动安装在其背部的水声通信机MA，按周期向安装在水面机器人底部的水声通信机MB发射水下机器人的航行状态信息；
水声通信机MB将所述航行状态信息发送给水面机器人计算机；
水面机器人计算机计算并输出航向角控制指令和速度控制指令，使水面机器人按照该控制指令运动；
所述水面机器人计算机通过自主跟踪算法计算并输出航向角控制指令，具体算法为：
计算水面机器人与水下机器人之间的距离：
2 2 2 1/2
d＝[(x2-x1)+(y2-y1)+(z2-z1)]
其中，水面机器人的位置为：
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，计算γ＝-1
tan [(long2-long1)cos(lati2·λ),(lati2-lati1)]/λ：如果γ＜0，则有α＝γ+360；
如果γ＞360，α＝γ-360；
如果d≥300米，计算shead＝sin(|long2-long1|·λ)sin[(90-lati2)·λ]/sin(d/R)，计算航向角：
-1
i)如果|long2-long1|≥0且|lati2-lati1|≥0，则α＝sin (shead)/λ，转到v)；
否则转到ii)；
-1
ii)如果|long2-long1|＜0且|lati2-lati1|≥0，则α＝-sin (shead)/λ，转到v)；否则转到iii)；
-1
iii)如果|long2-long1|＜0且|lati2-lati1|＜0，则α＝-90-cos (shead)/λ，转到v)；否则转到iv)；
-1
iv)α＝90+cos (shead)/λ，转到v)；
v)如果α＜0，则有α＝α+360；如果α＞360，α＝α-360。
2.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，所述航行状态信息包括水下机器人的航速、航向和地理坐标数据。
3.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，所述水面机器人计算机通过自主跟踪算法计算并输出速度控制指令，具体算法为：
如果水面机器人与水下机器人的距离大于最大通信距离Rmax，当航向角控制指令与水面机器人当前航向角之差β是锐角时，水面机器人加速航行，当航向角控制指令与水面机器人当前航向角之差β不是锐角时，水面机器人减速航行；
如果两者距离小于预先设定的最小距离Rmin，水面机器人减速至零；
否则，水面机器人保持与水下机器人相同的速度航行；
所述航向角控制指令与水面机器人当前航向角之差β是否为锐角的判断方法为：
如果β＞180°，则令θ＝β-360°；
如果β＜180°，则令θ＝β+360°；
如果|θ|＜90°，则β为锐角；否则，β不是锐角。
4.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，所述水面机器人计算机还预测水下机器人下一时刻的位置(long2t,lati2t)为：
式中，
RM＝R[1-2e+3esin(lati2t-1·λ)2]
RN＝R[1+esin(lati2t-1·λ)2]
为当前时刻水下机器人的航向，v为当前时刻水下机器人的速度，(long2t-1，lati2t-1)为当前时刻水下机器人的经纬度位置，T为采样周期， R为地球半径。
5.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，如果所述水声通信机的声通信中断，则水面机器人计算机预测水下机器人下一时刻的位置，计算并输出航向角控制指令，同时保持与上一周期相同的速度航行，当连续中断时间大于预设值，水面机器人减速至零，跟踪结束。
6.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，所述水面机器人计算机预测水下机器人下一时刻的位置在危险区时，航向角转向180度。
7.根据权利要求1所述的一种利用水面机器人跟踪水下机器人的方法，其特征在于，所述水面机器人计算机保存水下机器人的历史航迹。

一种利用水面机器人跟踪水下机器人的方法\n技术领域\n[0001] 本发明涉及水下机器人技术领域，尤其涉及一种利用水面机器人跟踪水下机器人的方法，实现水面机器人自主执行跟踪水下机器人。\n背景技术\n[0002] 水下机器人在水下航行时，陆上操作人员需要一种跟踪水下机器人的装置和方法，实时了解水下机器人的航行状态，确认其工作是否正常。常规的跟踪装置和方法是在有人参与的情况下，用母船执行跟踪任务。该方法利用船载超短基线定位系统或者船载水声通信机得到的位置信息，然后母船上的操作人员据此调整母船航速和航向，使其跟踪水下机器人。由于受水深、航程、设施、人员成本等条件限制，常规跟踪方法往往对船只、人员配备等要求较高且使用复杂，成本非常高昂；由于受母船噪声影响，跟踪性能也较差，因此常规方法不能满足稳定跟踪的需要。\n发明内容\n[0003] 为了克服现有方法的不足，本发明要解决的技术问题是提供一种不需要人参与的自主跟踪水下机器人的方法。该方法不用配备船只和人员，利用水面机器人实现自主跟踪的目的。\n[0004] 本发明为实现上述目的所采用的技术方案是：一种利用水面机器人跟踪水下机器人的方法，包括以下步骤：\n[0005] 水下机器人启动安装在其背部的水声通信机MA，按周期向安装在水面机器人底部的水声通信机MB发射水下机器人的航行状态信息；\n[0006] 水声通信机MB将所述航行状态信息发送给水面机器人计算机；\n[0007] 水面机器人计算机计算并输出航向角控制指令和速度控制指令，使水面机器人按照该控制指令运动。\n[0008] 所述航行状态信息包括水下机器人的航速、航向和地理坐标数据。\n[0009] 所述水面机器人计算机通过自主跟踪算法计算并输出航向角控制指令，具 体算法为：\n[0010] 计算水面机器人与水下机器人之间的距离：\n[0011] d＝[(x2-x1)2+(y2-y1)2+(z2-z1)2]1/2\n[0012] 其中，水面机器人的位置为：\n[0013] x1＝Rsin[(90-lati1)·λ]cos(long1·λ)\n[0014] y1＝Rsin[(90-lati1)·λ]sin(long1·λ)；\n[0015] z1＝Rcos[(90-lati1)·λ]\n[0016] 水下机器人的位置为：\n[0017] x2＝Rsin[(90-lati2)·λ]cos(long2·λ)\n[0018] y2＝Rsin[(90-lati2)·λ]sin(long2·λ)；\n[0019] z2＝Rcos[(90-lati2)·λ] R为地球半径，水面机器人的经纬度为(long1，lati1)，水下机器人的经纬度为(long2，lati2)；\n[0020] 如果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；\n如果γ＞360，α＝γ-360；\n[0021] 如果d≥300米，计算shead＝sin(|long2-long1|·λ)sin[(90-lati2)·λ]/sin(d/R)，计算航向角：\n[0022] i)如果|long2-long1|≥0且|lati2-lati1|≥0，则α＝sin-1(shead)/λ，转到v)；否则转到ii)：\n[0023] ii)如果|long2-long1|＜0且|lati2-lati1|≥0，则α＝-sin-1(shead)/λ，转到v)；否则转到iii)：\n[0024] iii)如果|long2-long1|＜0且|lati2-lati1|＜0，则α＝-90-cos-1(shead)/λ，转到v)；否则转到iv)：\n[0025] iv)α＝90+cos-1(shead)/λ，转到v)：\n[0026] v)如果α＜0，则有α＝α+360；如果α＞360，α＝α-360。\n[0027] 所述水面机器人计算机通过自主跟踪算法计算并输出速度控制指令，具体算法为：\n[0028] 如果水面机器人与水下机器人的距离大于最大通信距离Rmax，当航向角控制指令与水面机器人当前航向角之差β是锐角时，水面机器人加速航行，当航向角控制指令与水面机器人当前航向角之差β不是锐角时，水面机器人减速航行；\n[0029] 如果两者距离小于预先设定的最小距离Rmin，水面机器人减速至零；\n[0030] 否则，水面机器人保持与水下机器人相同的速度航行；\n[0031] 所述航向角控制指令与水面机器人当前航向角之差β是否为锐角的判断方法为：\n[0032] 如果β＞180°，则令θ＝β-360°；\n[0033] 如果β＜180°，则令θ＝β+360°；\n[0034] 如果|θ|＜90°，则β为锐角；否则，β不是锐角。\n[0035] 所述水面机器人计算机还预测水下机器人下一时刻的位置(long2t，lati2t)为：\n[0036] \n[0037] \n[0038] 式中，\n[0039] RM＝R[1-2e+3e sin(lati2t-1·λ)2]\n[0040] RN＝R[1+esin(lati2t-1·λ)2]\n[0041] \n[0042] 为当前时刻水下机器人的航向，v为当前时刻水下机器人的速度，(long2t-1，lati2t-1)为当前时刻水下机器人的经纬度位置，T为采样周期， R为地球半径。\n[0043] 如果所述水声通信机的声通信中断，则水面机器人计算机预测水下机器人下一时刻的位置，计算并输出航向角控制指令，同时保持与上一周期相同的速 度航行，当连续中断时间大于预设值，水面机器人减速至零，跟踪结束。\n[0044] 所述水面机器人计算机预测水下机器人下一时刻的位置在危险区时，航向角转向\n180度。\n[0045] 所述水面机器人计算机保存水下机器人的历史航迹。\n[0046] 本发明具有以下有益效果及优点\n[0047] 1.装置简单，继承性好。本发明装置仅需要一台水面机器人和两台台水声通信机，无需其它辅助装置；水声通信机安装简便，可以安装于水下机器人背部或水面机器人底部的任意位置；自主跟踪算法程序移植方便，可适用于到各种机器人。\n[0048] 2.稳定安全可靠，跟踪性能好。本发明的自主跟踪算法可以根据水下机器人的速度和距离，自动调整水面机器人航向和航速，具有距离远时加速、距离近时减速以及匀速保持功能，还可自动规避已知危险区域；即使在长时间(可达10分钟)声通信中断的情况下，也能依据水下机器人历史航迹，预测其航行信息，输出控制指令进行自动跟踪。\n[0049] 3.自主跟踪算法能够根据水下机器人的航行状态和历史航迹，自动调整航向和速度控制指令，使水面机器人保持稳定跟踪。自主跟踪算法还具有自动规避危险区、追逐防碰、等待保持等功能。\n[0050] 4.成本低，使用方便。本发明装置所使用的水声通信机，采用现有产品。水面机器人被移植本发明的自主跟踪算法后，可自主跟踪水下机器人，不需要动用船只和人员，节省了昂贵的跟踪成本，使用非常方便。\n[0051] 5.应用范围广。本发明不但可以应用于水下机器人，还可以用于其它海洋相关设备，可适用于全海深自主跟踪。\n附图说明\n[0052] 图1是本发明的组成示意图；\n[0053] 图2是本发明的水面机器人自主跟踪算法流程图。\n具体实施方式\n[0054] 下面结合附图及实施例对本发明做进一步的详细说明。\n[0055] 一种用水面机器人跟踪水下机器人的装置组成，如图1所示，由一台水面机器人和两台水声通信机组成。水下机器人上的水声通信机MA按固定周期向水面机器人上的水声通信机MB发送航行状态信息。MB收到后将消息传送给水面机器人计算机，由本发明的自主跟踪算法(见图2)处理。水面机器人计算机利用水下机器人发来的航行状态信息(包含水下机器人当前时刻的速度、航向、位置等信息)、自身的位置信息计算航向角控制指令。\n如果水面机器人与水下机器人的距离大于Rmax，当航向角控制指令与水面机器人当前航向角之差β是锐角时，水面机器人速度增加p米/秒，当航向角控制指令与水面机器人当前航向角之差β不是锐角时，水面机器人速度减小p米/秒(p为设定值)；如果两者距离小于Rmin，水面机器人减速至零；否则，水面机器人保持与水下机器人相同的速度航行。航向角控制指令与水面机器人当前航向角之差β时是否为锐角的判断方法为：\n[0056] 如果β＞180°令则θ＝β-360°；\n[0057] 如果β＜180°令则θ＝β+360°；\n[0058] 如果|β|＜90°，则β为锐角；否则，β不是锐角。\n[0059] 如果声通信中断，水面机器人保持与上一周期相同的速度航行，同时通过预测水下机器人位置，计算水面机器人航向角控制指令，当连续中断时间大于十分钟，则水面机器人减速至零，跟踪结束。\n[0060] 指令航向角计算方法：设水面机器人的经纬度为(long1，lati1)，水下机器人的经纬度为(long2，lati2)，水面机器人计算指令航向角，用α表示。水面机器人按照该航向角航行，用以跟踪水下机器人，再设 R为地球半径，其值为6378137米。指令航向角计算方法分为以下三个步骤：\n[0061] (1)计算水面机器人与水下机器人之间的距离，用d表示，计算完毕之后转到2)。\n[0062] 令\n[0063] x1＝Rsin[(90-lati1)·λ]cos(long1·λ)\n[0064] y1＝Rsin[(90-lati1)·λ]sin(long1·λ)\n[0065] z1＝Rcos[(90-lati1)·λ]\n[0066] x2＝Rsin[(90-lati2)·λ]cos(long2·λ)\n[0067] y2＝Rsin[(90-lati2)·λ]sin(long2·λ)\n[0068] z2＝Rcos[(90-lati2)·λ]\n[0069] 则有\n[0070] d＝[(x2-x1)2+(y2-y1)2+(z2-z1)2]1/2\n[0071] 令\n[0072] β＝sin-1(x/2/R)\n[0073] 于是得\n[0074] d＝2βR\n[0075] (2)如果d＜300米，进行下列计算；否则，转到(3)。\n[0076] i)如果|lati2-lati1|＜0.00001，转到ii)；否则，转到iii)。\n[0077] ii)如果long2-long1＞0，则α＝90度；否则，α＝270度。\n[0078] iii)计算γ＝tan-1[(long2-long1)cos(lati2·λ)，(lati2-lati1)]/λ。如果γ＜0，则有\n[0079] α＝γ+360；如果γ＞360，α＝γ-360。\n[0080] (3)计算shead＝sin (|long2-long1|·λ)sin[(90-lati2)·λ]/sin(d/R)，进行下列计算。\n[0081] i)如果|long2-long1|≥0且|lati2-lati1|≥0，则α＝sin-1(shead)/λ，转到v)；否则转到ii)。\n[0082] ii)如果|long2-long1|＜0且|lati2-lati1|≥0，则α＝-sin-1(shead)/λ，转到v)；否则转到iii)。\n[0083] iii)如果|long2-long1|＜0且|lati2-lati1|＜0，则α＝-90-cos-1(shead)/λ，转到v)； 否则转到iv)。\n[0084] iv)α＝90+cos-1(shead)/λ，转到v)。\n[0085] v)如果α＜0，则有α＝α+360；如果α＞360，α＝α-360。\n[0086] 预测水下机器人位置的方法：设上一时刻水下机器人航向为 速度为v，经纬度位置为(long2t-1，lati2t-1)，周期为T。再 R为地球半径，其值为6378137米。则可预测当前时刻水下机器人的位置(long2t，lati2t)。计算公式如下：\n[0087] \n[0088] \n[0089] 式中\n[0090] RM＝R[1-2e+3esin(lati2t-1·λ)2]\n[0091] RN＝R[1+esin(lati2t-1·λ)2]\n[0092] 。