CN112013773B - 水下无人航行器回收系统及回收方法 - Google Patents

Abstract

本申请公开了一种水下无人航行器回收系统及回收方法，其中，方法包括设置于所述水下无人航行器头部的检测单元，获取回收站上设置的至少一组灯组中的多个灯按照预定规则发射的光信号；根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于检测单元的距离、偏移方位及偏角；根据各灯相对于所述检测单元的距离、偏移方位及偏角，确定所述水下无人航行器相对于所述回收站的位姿；至少根据所述位姿对所述水下无人航行器进行航进控制，以使所述水下无人航行器与所述回收站对接。上述方案，可以保证回收的准确性、可靠性及安全性。

Description

水下无人航行器回收系统及回收方法

技术领域

本发明一般涉及水下无人航行器技术领域，具体涉及一种水下无人航行器回收系统及回收方法。

背景技术

近年来，随着水下无人航行器(AutonomousUnderwaterVehicle； AUV)自主对接回收的深入研究，如何准确的确定水下无人航行器位姿的需求日益迫切，一方面是安全需要，AUV携带有声、压、光等诸多传感器，在回收过程中避免碰撞，保证传感器安全；另一方面是AUV 自主对接回收路径规划及动力控制系统的需要，不同的位姿需要相应的路径规划和动力控制算法；再加上回收过程中，复杂多变的海况，导致AUV和回收站之间的位姿时刻在变，对如何准确的确定水下无人航行器位姿提出了更高的要求。

发明内容

本申请期望提供一种水下无人航行器回收系统及回收方法，以提高对水下无人航行器位姿确定的准确性，保证回收的可靠性及安全性。

第一方面，本发明提供一种水下无人航行器回收方法，包括：

设置于所述水下无人航行器头部的检测单元，获取回收站上设置的至少一组灯组中的多个灯按照预定规则发射的光信号；

根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于检测单元的距离、偏移方位及偏角；

根据各灯相对于所述检测单元的距离、偏移方位及偏角，确定所述水下无人航行器相对于所述回收站的位姿；

至少根据所述位姿对所述水下无人航行器进行航进控制，以使所述水下无人航行器与所述回收站对接。

作为可实现方式，所述回收站自前至后至少设置两个所述灯组，以最靠近回收站前部的灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，并在水下无人航行器与回收站之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，依次循环直至所述水下无人航行器回收完成。

作为可实现方式，所述预定规则包括各灯的点亮顺序、点亮时长以及相邻点亮的两灯的间隔时间。

作为可实现方式，检测单元具有多个感光区域，根据各所述感光区域接收的其中一灯组中各所述灯的所述光信号，确定当前灯组中各所述灯相对于所述检测单元的所述距离、所述偏移方位及所述偏角。

作为可实现方式，所述检测单元包括四元光电探测器，所述偏角包括水平偏角和竖直偏角；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

作为可实现方式，所述检测单元包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧， Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0 表示对应的灯位于所述四元光电探测器中心点的上侧。

作为可实现方式，所述回收站至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位 Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。

作为可实现方式，所述回收站设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、 r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

作为可实现方式，同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角依次表示为：

其中，A、B、C分别为一灯组中三个所述灯中心位置，O为四元光电探测器的中心位置；

若α为上述三个夹角的第一值，β为第二值，γ为第三值；α≥β，α≥γ，β≥γ，且α、β、γ均小于60°；

在α、β、γ三者不相等时，α与β之间的公共边长度为mid， mid的最小值为low，最大值为upp，令low＝r，

mid＝0.5(low+upp)，根据以下关系式确定mid、r₁、r₂：

若，|r₃-r₁|<ε,ε为距离精度，则将计算获得的mid、r₁、r₂作为所述OD计算的输入值；否则，

若r_3>r₁+ε将当前计算获得的mid作为upp，再次根据上述关系式 (1)-(3)进行计算，直至满足|r₃-r₁|<ε；

若r₃<r₁-ε将当前计算获得的mid作为low，再次根据上述关系式(1)-(3)进行计算，直至满足|r₃-r₁|<ε。

作为可实现方式，各所述灯与所述四元光电探测器中心之间的距离通过以下方式获得：

获取各所述灯照射在所述四元光电探测器上产生的，分别经多级放大检波电路处理后的输出量；

自最后一级逐级向前，直至判断出当前一级所述放大检波电路的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与所述四元光电探测器中心之间的距离。

第二方面，本发明提供一种水下无人航行器回收系统，包括：

回收站，所述回收站上设置的至少一组灯组，每组灯组包括多个灯；

检测单元，设置于所述水下无人航行器头部，用于获取各灯按照预定规则发射的光信号；

位姿确定单元，用于根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于检测单元的距离、偏移方位及偏角；并根据各灯相对于所述检测单元的距离、偏移方位及偏角，确定所述水下无人航行器相对于所述回收站的位姿；

航行控制单元，至少根据所述位姿对所述水下无人航行器进行航进控制，以使所述水下无人航行器与所述回收站对接。

作为可实现方式，所述回收站自前至后至少设置两个所述灯组；

所述位姿确定单元，用于以最靠近回收站前部的灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，并在水下无人航行器与回收站之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，依次循环直至所述水下无人航行器回收完成。

作为可实现方式，所述检测单元包括四元光电探测器，所述偏角包括水平偏角和竖直偏角；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

作为可实现方式，所述检测单元包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧， Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0 表示对应的灯位于所述四元光电探测器中心点的上侧。

作为可实现方式，所述回收站至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，所述检测单元包括四元光电探测器，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。

作为可实现方式，所述回收站设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、 r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

作为可实现方式，所述位姿确定单元包括信号处理模块及距离确定模块；

所述信号处理模块包括相互连接的多级放大检波电路，每一级所述放大检波电路包括依次连接的放大模块和检波模块；前一级所述放大检波电路的放大模块的输出端，与后一级所述放大检波电路的放大模块的输入端连接；

所述四元光电探测器的输出端与第一级所述放大检波电路的放大模块的输入端连接；

所述距离确定模块，用于自最后一级逐级向前，直至判断出当前一级所述放大检波电路的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与所述四元光电探测器中心之间的距离。

作为可实现方式，最后一级所述放大检波电路的放大模块的输出端连接有比较器，所述比较器用于在最后一级所述放大检波电路的放大模块的输出量大于比较阈值时，输出距离确定启动信号；

所述距离确定模块，响应于所述距离确定启动信号，接收各级所述放大检波电路的输出量以进行距离确定。

上述方案，首先确定各灯的ID，由于回收站的各灯位置是相对固定的，在确定各灯的ID后，可以以各灯为参考来通过对应的光信号确定各灯相对于检测单元的距离、偏移方位及偏角，并据此得出水下无人航行器相对于回收站的位姿。由于各灯位置是相对固定，因此，以其作为参考来确定水下无人航行器相对于回收站的位姿，准确性及精度高，然后根据当前的位姿及距离，控制无人航行器向回收站航进，由于位姿及距离的精度高，则可以保证回收的准确性、可靠性及安全性。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1为本发明实施例提供的水下无人航行器回收方法的流程图；

图2为本发明实施例提供的水下无人航行器回收系统的原理图；

图3为本发明对各灯点亮的预定规则的时序图；

图4为任意两个LED灯在水平及竖直方向夹角的示意图；

图5为每相邻两LED灯到检测单元中心夹角与边长的对应关系图；

图6为检测单元与位姿确定单元连接的原理图；

图7为另一实现方式检测单元与位姿确定单元连接的原理图；

图8为再一实现方式检测单元与位姿确定单元连接的原理图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

如图1所示，本发明实施例示出的一种水下无人航行器回收方法，包括：

S1：设置于所述水下无人航行器1头部的检测单元11，获取回收站2上设置的至少一组灯组中的多个灯按照预定规则发射的光信号；

S2：根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID(Identitydocument；身份证标识)，以及各ID的灯相对于检测单元11的距离、偏移方位及偏角；

S3：根据各灯相对于所述检测单元11的距离、偏移方位及偏角，确定所述水下无人航行器1相对于所述回收站2的位姿；

S4：至少根据所述位姿对所述水下无人航行器1进行航进控制，以使所述水下无人航行器1与所述回收站2对接。

水下无人航行器1在水下行进时，由于复杂多变的海况，其存在一定量的俯仰、侧倾、翻转等姿态。而回收站2的位置相对固定。回收站2上设置有多个灯，例如但不限于为LED灯。为了以各灯为参考，就需要知道接收的当前光信号是哪个灯发出的，在获知是哪个灯发出的之后，再根据光信号的强弱以及在检测单元11中的位置等，确定发出当前光信号的灯相对于检测单元11的距离、偏移方位及偏角。

为了便于获知或确定各灯身份，可以采用按照预定规则点亮各灯的方式来确定各灯的ID，也即各灯的身份。例如但不限于，如图2所示，回收站2上设置了两个灯组，一个灯组位于回收站2的头部，一个灯组位于回收站2的尾部，各灯依次标记为A、B、C、D、E、F，各灯组的中心表示为D、H。当然还可以根据需要设置其他数量的灯组，每个灯组具有三个灯，当然也可以设置其他数量的灯，一个位于回收站2的底部，另两个分置在回收站2的左右两侧，根据预定规则点亮各灯时，通过与预定规则对应的解码规则就可以获知哪次是底部的灯点亮，哪次是左侧的灯点亮，哪次是右侧的灯点亮。各灯的ID可以用字母表示，如A表示左侧的灯，B表示右侧的灯，C表示底部的灯，还可以用数字，如1表示左侧的灯，2表示右侧的灯，3表示底部的灯等。

本文所指的位姿可以理解为水下无人航行器1相对于回收站2的距离、位置及姿态等。如，水下无人航行器1相对于回收站2有多远，水下无人航行器1是位于回收站2之上还是在之下，是在回收站2的左侧还是右侧，水下无人航行器1的头部是指向回收站2还是远离回收站2等。

在确定水下无人航行器1相对于所述回收站2的位姿后，通过航行器进行航进控制，以使水下无人航行器1朝着回收站2航进。在向回收站2航进时，可以根据其与回收站2的距离采用不同的速度，例如距离回收站2远时速度可以高于距离回收站2近时的速度，当然，还可以匀速的回收。

上述方案，首先确定各灯的ID，由于回收站2的各灯位置是相对固定的，在确定各灯的ID后，可以以各灯为参考来通过对应的光信号确定各灯相对于检测单元11的距离、偏移方位及偏角，并据此得出水下无人航行器1相对于回收站2的位姿。由于各灯位置是相对固定，因此，以其作为参考来确定水下无人航行器1相对于回收站2的位姿，然后根据当前的位姿及距离，控制无人航行器向回收站2航进，由于位姿及距离的精度高，则可以保证回收的准确性、可靠性及安全性。

作为可实现方式，所述回收站2自前至后至少设置两个所述灯组，以最靠近回收站2前部的灯组中各灯发射的光信号确定所述水下无人航行器1的当前位姿，并在水下无人航行器1与回收站2之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器1的当前位姿，依次循环直至所述水下无人航行器1回收完成。

在水下无人航行器1距离回收站2较远时，以回收站2头部的灯组作为光信号源来确定水下无人航行器1相对于回收站2的位姿，在水下无人航行器1向着回收站2航进的过程中，随着水下无人航行器 1相对与回收站2距离的降低，位于回收站2头部的灯组可能会超出检测单元11的接收视角，为了能够继续准确的获得水下无人航行器1 当前的位姿，保证回收过程的连续性、可靠性及安全性，就需要切换到靠后的灯组来作为光信号源，以确定当前的位姿，依次方式循环，直至水下无人航行器1回收完成。

作为可实现方式，所述预定规则包括各灯的点亮顺序、点亮时长以及相邻点亮的两灯的间隔时间。其中，点亮时长以及相邻点亮的两灯的间隔时间可以相同，也可以不同，具体可以根据实际需要确定。

例如，如图3所示，该示例中以设置两个灯组，每个灯组中具有三个LED灯，每个LED灯的点亮时间相等，且相邻点亮的两灯的间隔时间也相同，并与灯的点亮时间相等为例进行说明。

两个灯组顺次完成一次点亮为一个大周期，每个大周期平分为9 个小周期，依次为t1-t9。作为其中一个示例，小周期t1期间，1号LED 灯亮；小周期t2期间，所有LED灯全灭；小周期t3期间，2号LED 灯亮；小周期t4期间，3号LED灯亮；小周期t5期间，所有LED灯全灭；小周期t6期间，4号LED灯亮；小周期t7期间，5号LED灯亮；小周期t8期间，6号LED灯亮；小周期t9期间，所有LED灯全灭。那么用数字可以表示为101101110，其中，“1”代表灯亮、“0”代表灯灭、“X”代表1或0。也就是用1011表示是第一灯组中的三个灯及其点亮顺序，1110表示是第二灯组中的三个灯及其点亮顺序。

相应地，可以在接收到的光信号中判断四个小周期中出现1011，则说明其第一灯组，四个小周期中出现1110则说明其第一灯组，根据以下情况确定灯的ID：

对于第一灯组具有以下六种情况：

情况1：数字信号为10110XXXX表示1号LED灯在小周期t1时亮，2号LED灯在小周期t3内亮，3号LED灯在小周期t4内亮；

情况2：数字信号为1010110XX表示1号LED灯在小周期t3时亮，2号LED灯在小周期t5内亮，3号LED灯在小周期t6内亮；

情况3：数字信号为1X01011XX表示1号LED灯在小周期t4时亮，2号LED灯在小周期t6内亮，3号LED灯在小周期t7内亮；

情况4：数字信号为1XX010110表示1号LED灯在小周期t5时亮，2号LED灯在小周期t7内亮，3号LED灯在小周期t8内亮；

情况5：数字信号为10XXX0101表示1号LED灯在小周期t7时亮，2号LED灯在小周期t9内亮，3号LED灯在小周期t1内亮；

情况6：数字信号为110XXX010表示1号LED灯在小周期t8时亮，2号LED灯在小周期t1内亮，3号LED灯在小周期t2内亮；

对于第二灯组具有以下六种情况：

情况1：数字信号为1110X0XX0表示4号LED灯在小周期t1时亮，5号LED灯在小周期t2内亮，6号LED灯在小周期t3内亮；

情况2：数字信号为101110XXX表示4号LED灯在小周期t3时亮，5号LED灯在小周期t4内亮，6号LED灯在小周期t5内亮；

情况3：数字信号为1X01110X0表示4号LED灯在小周期t4时亮，5号LED灯在小周期t5内亮，6号LED灯在小周期t6内亮；

情况4：数字信号为10XX01110表示4号LED灯在小周期t6时亮，5号LED灯在小周期t7内亮，6号LED灯在小周期t8内亮；

情况5：数字信号为10X0XX011表示4号LED灯在小周期t8时亮，5号LED灯在小周期t9内亮，6号LED灯在小周期t1内亮；

情况6：数字信号为110X0XX01表示4号LED灯在小周期t9时亮，5号LED灯在小周期t1内亮，6号LED灯在小周期t2内亮。

作为可实现方式，检测单元11具有多个感光区域，根据各所述感光区域接收的各所述灯的所述光信号，确定各所述灯相对于所述检测单元11的所述距离、所述偏移方位及所述偏角。

作为可实现方式，至少参见图4所示，所述检测单元11包括四元光电探测器，XOZ为四元光电探测器的感光面，所述偏角包括水平偏角，可以理解为相对于Y轴的夹角和竖直偏角可以理解为相对于水平面XOY的夹角，图中示出了两个灯，依次标记为LED及LED’；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

作为可实现方式，所述检测单元11包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧， Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0 表示对应的灯位于所述四元光电探测器中心点的上侧。

作为可实现方式，所述回收站2至少设置两个灯组，例如是两个灯组，其中一个设置在回收站2的头部，另外一个设置在回收站2的尾部，每个所述灯组包括三个呈等边三角形排布的所述灯，两个灯组所处的平面相互平行，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。一般地，四元光电探测器可以位于水下无人航行器1头部的正中，根据四元光电探测器中心位置相对于灯组中心位置，来控制水下无人航行器1的姿态，例如，四元光电探测器中心位置相对于灯组中心位置是向上偏，则控制水下无人航行器1上头部向下调整航行，直至四元光电探测器中心位置与灯组中心位置重合。

作为可实现方式，所述回收站2至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、 r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

作为可实现方式，任意两所述灯与所述四元光电探测器中心的夹角依次表示为：

其中，A、B、C分别为一灯组中三个所述灯中心位置，O为四元光电探测器的中心位置；

若α为上述三个夹角的第一值，β为第二值，γ为第三值；α≥β，α≥γ，β≥γ，且α、β、γ均小于60°；

如图5所示，在α、β、γ三者不相等时，α与β之间的公共边长度为mid，mid的最小值为low，最大值为upp，令low＝r，

mid＝0.5(low+upp)，根据以下关系式确定mid、r₁、r₂：

若，|r₃-r₁|<ε,ε为距离精度，则将计算获得的mid、r₁、r₂作为所述OD计算的输入值；否则，

若r_3>r₁+ε将当前计算获得的mid作为upp，再次根据上述关系式 (1)-(3)进行计算，直至满足|r₃-r₁|<ε；

若r₃<r₁-ε将当前计算获得的mid作为low，再次根据上述关系式 (1)-(3)进行计算，直至满足|r₃-r₁|<ε。

其中，各所述灯与所述四元光电探测器中心之间的距离，也即上述的mid、r1、r2，可以通过以下方式获得：

获取各所述灯照射在所述四元光电探测器上产生的，分别经多级放大检波电路处理后的输出量；

自最后一级逐级向前，直至判断出当前一级所述放大检波电路的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与所述四元光电探测器中心之间的距离。

第二方面，另至少参见图2，本发明提供一种水下无人航行器回收系统，包括：回收站2，所述回收站2上设置的至少一组灯组，每组灯组包括多个灯；例如但不限于，可以采用笼式回收站2。在该示例中，回收站2的前后各设置了一组灯组。

检测单元11，设置于所述水下无人航行器1头部，用于获取各灯按照预定规则发射的光信号；

位姿确定单元，用于根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于检测单元11的距离、偏移方位及偏角；并根据各灯相对于所述检测单元11的距离、偏移方位及偏角，确定所述水下无人航行器1相对于所述回收站2的位姿；

航行控制单元，至少根据所述位姿对所述水下无人航行器1进行航进控制，以使所述水下无人航行器1与所述回收站2对接。

作为可实现方式，所述回收站2自前至后至少设置两个所述灯组；

所述位姿确定单元，用于以最靠近回收站2前部的灯组中各灯发射的光信号确定所述水下无人航行器1的当前位姿，并在水下无人航行器1与回收站2之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器 1的当前位姿，依次循环直至所述水下无人航行器1回收完成。

作为可实现方式，所述检测单元11包括四元光电探测器，所述偏角包括水平偏角和竖直偏角；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

作为可实现方式，所述检测单元11包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧， Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0 表示对应的灯位于所述四元光电探测器中心点的上侧。

作为可实现方式，所述回收站2至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位 Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。

作为可实现方式，所述回收站2设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、 r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

作为可实现方式，另参见图6，所述位姿确定单元包括信号处理模块21及距离确定模块3；当然，所述位姿确定单元还包括用于根据上述关系式确定偏移方位及偏角的模块，图中未予以示出。

所述信号处理模块21包括相互连接的多级放大检波电路4，每一级所述放大检波电路包括依次连接的放大模块5和检波模块6；前一级所述放大检波电路的放大模块5的输出端，与后一级所述放大检波电路6的放大模块5的输入端连接；

检测单元11的输出端与第一级所述放大检波电路4的放大模块的输入端连接。

所述距离确定模块3，用于自最后一级逐级向前，直至判断出当前一级所述放大检波电路4的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与检测单元11中的四元光电探测器中心之间的距离。

作为可实现方式，如图7所示，最后一级所述放大检波电路4的放大模块5的输出端连接有比较器7，所述比较器7用于在最后一级所述放大检波电路4的放大模块5的输出量大于比较阈值时，输出距离确定启动信号；

所述距离确定模块3，响应于所述距离确定启动信号，接收各级所述放大检波电路4的输出量以进行距离确定。

这里所说的多级放大检波电路，是由多个放大检波电路4相互连接构成。例如，放大检波电路4包括依次连接的放大模块5和检波模块6。第一级放大检波电路4的放大模块5的输出端，与第二级放大检波电路4的放大模块5的输入端连接，第二级放大检波电路4的放大模块5的输出端，与第三级放大检波电路4的放大模块5的输入端连接，依此规律，直至连接至最后一级放大检波电路4，以构成多级放大检波电路4。这样，检测单元11产生的电压信号，经第一级放大检波电路4的放大模块5的放大后，输入至第二级放大检波电路4，并经第二级放大检波电路4的放大模块5的放大后，输入至第三级放大检波电路4，依此规律，逐级放大至最后一级放大检波电路4。每一级放大检波电路4对应的电压信号的大小，都对应一特定范围的距离。并且第一级放大检波电路4对应的距离最小，最后一级放大检波电路 4对应的距离最大。采用多级放大检波电路4可以增加测距的动态范围，例如，每一级放大检波电路4的动态范围在10db以上，那么对应 n级放大检波电路4来说，其动态范围在10ndb以上，其中，n为自然数。

通过自最后一级逐级向前，直至判断出当前一级所述放大检波电路4的输出量小于当前一级电压饱和阈值的方式，进行逐级的筛选，若最后一级放大检波电路4的输出量的电压信号高于最后一级电压饱和阈值，说明目标光源的距离小于最后一级放大检波电路4对应的距离，因此再去判断倒数第二级放大检波电路4的输出量的电压信号是否高于倒数第二级电压饱和阈值，若是，则说明目标光源的距离小于倒数第二级放大检波电路4对应的距离，继续向下一级进行判断，直至找到当前一级放大检波电路4的输出量的电压信号小于当前一级电压饱和阈值，说明目标光源的距离在该当前一级放大检波电路4对应的距离范围内，然后根据电压与距离的映射关系，确定目标光源的距离。

其中，电压与距离的映射关系可以预先进行标定。即预先标定目标光源距离检测单元11不同距离时的电压大小。标定环境参照使用环境确定。如该测距系统应用于水下，则在一定水下环境中进行标定，以保障最后测量的准确性。

一般地，距离确定模块3可以包括模数转换模块和计算模块，模数转换模块用于将级放大检波电路4传送进距离确定模块3的模拟量转化为数字量，以供计算模块进行计算处理获得所测量的距离。

上述方案，通过光电的方式进行测距，与声学的测距方式相比，具有体积小、实时性高、能耗低的优点。此外，通过多级放大检波电路4对信号进行处理后，自最后一级逐级向前，直至判断出当前一级放大检波电路4的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级放大检波电路4的输出量所对应的距离，此种方式具有动态范围广，精度高的优点。

作为可实现方式，如图7所示，最后一级放大检波电路4的放大模块5的输出端连接有比较器7，比较器7用于在最后一级放大检波电路4的放大模块5的输出量大于比较阈值时，输出距离确定启动信号；距离确定模块3，响应于距离确定启动信号，接收各级放大检波电路4的输出量以进行距离确定。

为了降低功耗、提高智能化及防止测距系统误动作，在最后一级放大检波电路4的放大模块5的输出端连接有比较器7，只有在最后一级放大检波电路4的放大模块5的输出量足够大时，比较器7才向距离确定模块3发出距离确定启动信号，该距离确定启动信号例如但不限于为电平信号。距离确定模块3只有在接收到距离确定启动信号后，响应于该信号才开始进行测距流程。一般地，比较阈值应该大于系统噪声等级，防止系统噪声误触发。

另参见图8，检测单元11可以由四象限光电探测器8、放大单元 9，和信号单元10等构成；放大单元9，用于对四象限光电探测器8 各象限的输出量进行初步的放大，当然根据实际情况还可进行滤波，以提高灵敏度和精度；和信号单元10，用于将放大滤波后的所述四象限光电探测器8各象限的输出量合并输出。

四象限光电探测器8是光电转换元件，其受光面分为四个象限，每一象限具有一个电压输出，放大单元9分别对各路电压进行滤波后输入到和信号单元10，和信号单元10将该四路信号合并为一路信号然后输入至第一级放大检波电路4。

作为可实现方式，各级放大检波电路4中，后一级放大检波电路 4的电压饱和阈值大于前一级放大检波电路4的电压饱和阈值。

作为可实现方式，放大模块5包括第一放大器U1，第一放大器 U1的正向输入端连接接地的第一电阻R1；第一放大器U1的负向输入端通过第二电阻R3与第一放大器U1的输出端连接。第一放大器U1 的负向输入端通过第七电阻R2与检测单元11的信号输出端连接。

作为可实现方式，检波模块6包括第二放大器U2，第二放大器 U2的正向输入端连接接地的第三电阻R5；第二放大器U2的负向输入端通过第一二极管T1与第二放大器U2的输出端连接；第一二极管T1的输出端连接有第二二极管T2，第二放大器U2的负向输入端通过第四电阻R6与第二二极管T2的输出端连接。第二放大器U2的负向输入端还分别顺次连接的第二电容C1、第八电阻R4与第一放大器U1 的输出端连接。

作为可实现方式，第二二极管T2的输出端连接有滤波模块。

作为可实现方式，滤波模块包括并联接地的第一电容C2和第五电阻R8。在实际使用中，可以尽量减小第一电容和第五电阻的乘积值参数，以缩短充电时间，延长放电时间，降低检波电压文波，进一步提高测距精度及线性度。

作为可实现方式，第二二极管T2的输出端连接有隔离模块。用于将数字端与模拟端进行隔离，防止相互干扰，提高测距精度。

作为可实现方式，隔离模块包括第三放大器U3，第三放大器U3 的正向输入端通过第六电阻R7与第二二极管T2的输出端连接，第三放大器U3的负向输入端与第三放大器U3的输出端连接，第三放大器 U3的输出端与所述距离确定模块3连接。

上述方案的水下无人航行器回收系统，可以实现了水下0～100米范围内精确导引，其中，角度测量精度为0.1°，距离测量精度为10mm。

需要理解的是，上文如有涉及术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (16)

1.一种水下无人航行器回收方法，其特征在于，包括：

设置于所述水下无人航行器头部的检测单元，获取回收站上设置的至少一组灯组中的多个灯按照预定规则发射的光信号；

根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于所述检测单元的距离、偏移方位及偏角；

根据各灯相对于所述检测单元的距离、偏移方位及偏角，确定所述水下无人航行器相对于所述回收站的位姿；

至少根据所述位姿对所述水下无人航行器进行航进控制，以使所述水下无人航行器与所述回收站对接；

所述回收站自前至后至少设置两个所述灯组，以最靠近回收站前部的灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，并在水下无人航行器与回收站之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，依次循环直至所述水下无人航行器回收完成。

2.根据权利要求1所述的水下无人航行器回收方法，其特征在于，所述预定规则包括各灯的点亮顺序、点亮时长以及相邻点亮的两灯的间隔时间。

3.根据权利要求1或2所述的水下无人航行器回收方法，其特征在于，

检测单元具有多个感光区域，根据各所述感光区域接收的其中一灯组中各所述灯的所述光信号，确定当前灯组中各所述灯相对于所述检测单元的所述距离、所述偏移方位及所述偏角。

4.根据权利要求1或2所述的水下无人航行器回收方法，其特征在于，所述检测单元包括四元光电探测器，所述偏角包括水平偏角和竖直偏角；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

5.根据权利要求1所述的水下无人航行器回收方法，其特征在于，所述检测单元包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧，Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0表示对应的灯位于所述四元光电探测器中心点的上侧。

6.根据权利要求5所述的水下无人航行器回收方法，其特征在于，所述回收站至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。

7.根据权利要求4所述的水下无人航行器回收方法，其特征在于，所述回收站设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

8.根据权利要求7所述的水下无人航行器回收方法，其特征在于，同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角依次表示为：

其中，A、B、C分别为一灯组中三个所述灯中心位置，O为四元光电探测器的中心位置；

若α为上述三个夹角的第一值，β为第二值，γ为第三值；α≥β，α≥γ，β≥γ，且α、β、γ均小于60°；

在α、β、γ三者不相等时，α与β之间的公共边长度为mid，mid的最小值为low，最大值为upp，令low＝r，

mid＝0.5(low+upp)，根据以下关系式确定mid、r₁、r₂：

若，|r₃-r₁|<ε,ε为距离精度，则将计算获得的mid、r₁、r₂作为所述OD计算的输入值；否则，

若r_3>r₁+ε将当前计算获得的mid作为upp，再次根据上述关系式(1)-(3)进行计算，直至满足|r₃-r₁|<ε；

若r₃<r₁-ε将当前计算获得的mid作为low，再次根据上述关系式(1)-(3)进行计算，直至满足|r₃-r₁|<ε。

9.根据权利要求7所述的水下无人航行器回收方法，其特征在于，各所述灯与所述四元光电探测器中心之间的距离通过以下方式获得：

获取各所述灯照射在所述四元光电探测器上产生的，分别经多级放大检波电路处理后的输出量；

自最后一级逐级向前，直至判断出当前一级所述放大检波电路的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与所述四元光电探测器中心之间的距离。

10.一种水下无人航行器回收系统，其特征在于，包括：

回收站，所述回收站上设置的至少一组灯组，每组灯组包括多个灯；

检测单元，设置于所述水下无人航行器头部，用于获取各灯按照预定规则发射的光信号；

位姿确定单元，用于根据其中一所述灯组中各所述灯的所述光信号，确定该灯组中各灯的ID，以及各ID的灯相对于检测单元的距离、偏移方位及偏角；并根据各灯相对于所述检测单元的距离、偏移方位及偏角，确定所述水下无人航行器相对于所述回收站的位姿；

航行控制单元，至少根据所述位姿对所述水下无人航行器进行航进控制，以使所述水下无人航行器与所述回收站对接；

所述回收站自前至后至少设置两个所述灯组；

所述位姿确定单元，用于以最靠近回收站前部的灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，并在水下无人航行器与回收站之间的距离小于等于当前灯组的切换阈值时，以当前灯组相邻的后一灯组中各灯发射的光信号确定所述水下无人航行器的当前位姿，依次循环直至所述水下无人航行器回收完成。

11.根据权利要求10所述的水下无人航行器回收系统，其特征在于，所述检测单元包括四元光电探测器，所述偏角包括水平偏角和竖直偏角；根据以下关系式确定所述水平偏角和所述竖直偏角：

其中，φ为所述四元光电探测器与各所述灯的水平偏角，θ为所述四元光电探测器与各所述灯的竖直偏角，Kx、Ky、mx和my为所述四元光电探测器的标定值且为常数，U₁、U₂、U₃和U₄分别为所述四元光电探测器感光区域根据所述光信号产生的电压信号。

12.根据权利要求11所述的水下无人航行器回收系统，其特征在于，所述检测单元包括四元光电探测器，所述偏移方位包括第一水平偏移方位和第一竖直偏移方位；根据以下关系式确定所述第一水平偏移方位Dirx和所述第一竖直偏移方位Diry：

其中，Dirx＝1表示对应的灯位于所述四元光电探测器中心点的右侧，Dirx＝0表示对应的灯位于所述四元光电探测器中心点的左侧，Diry＝1表示对应的灯位于所述四元光电探测器中心点的下侧，Diry＝0表示对应的灯位于所述四元光电探测器中心点的上侧。

13.根据权利要求12所述的水下无人航行器回收系统，其特征在于，所述回收站至少设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，根据以下关系式确定所述四元光电探测器中心位置相对于所述灯组中心位置的第二水平偏移方位Dir0x和第二竖直偏移方位Dir0y：

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一水平偏移方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与其中一个灯组中三个所述灯的所述第一竖直偏移方位。

14.根据权利要求10所述的水下无人航行器回收系统，其特征在于，所述回收站设置两个灯组，每个所述灯组包括三个呈等边三角形排布的所述灯，所述检测单元包括四元光电探测器，根据以下关系式确定所述四元光电探测器中心与灯组中心的距离OD：

若同一所述灯组中任意两所述灯与所述四元光电探测器中心的夹角均相等，各所述灯与四元光电探测器中心之间的距离相等且为

则

若灯组中任意两所述灯与所述四元光电探测器中心的夹角中至少其中两个不相等，则

其中，α为任意两所述灯与所述四元光电探测器中心的夹角，mid、r1、r2分别为所述四元光电探测器中心到各灯的距离，r为所述等边三角形的边长。

15.根据权利要求14所述的水下无人航行器回收系统，其特征在于，所述位姿确定单元包括信号处理模块及距离确定模块；

所述信号处理模块包括相互连接的多级放大检波电路，每一级所述放大检波电路包括依次连接的放大模块和检波模块；前一级所述放大检波电路的放大模块的输出端，与后一级所述放大检波电路的放大模块的输入端连接；

所述四元光电探测器的输出端与第一级所述放大检波电路的放大模块的输入端连接；

所述距离确定模块，用于自最后一级逐级向前，直至判断出当前一级所述放大检波电路的输出量小于当前一级电压饱和阈值，并根据电压与距离的映射关系，确定当前一级所述放大检波电路的输出量所对应的距离，该输出量所对应的距离为当前灯与所述四元光电探测器中心之间的距离。

16.根据权利要求15所述的水下无人航行器回收系统，其特征在于，最后一级所述放大检波电路的放大模块的输出端连接有比较器，所述比较器用于在最后一级所述放大检波电路的放大模块的输出量大于比较阈值时，输出距离确定启动信号；

所述距离确定模块，响应于所述距离确定启动信号，接收各级所述放大检波电路的输出量以进行距离确定。

Images