阅读说明：本技术 一种用于uuv回收过程的定位系统及方法 (positioning system and method for UUV recovery process ) 是由 吕小鹏 吴国俊 邵建锋 杨钰城 于 2019-09-30 设计创作，主要内容包括：本发明涉及一种用于UUV回收过程的定位系统及方法,本发明解决了利用声学传感器定位UUV时所带来的定位精度低、换能器体积大、功耗大等问题。该系统包括安装在UUV前端的四元光电探测器、安装在回收站前端的第一LED灯组、安装在回收站后端的第二LED灯组；控制第一LED灯组和第二LED灯组的LED灯控制器以及与进行数据处理计算的上位机。其定位方法的主要步骤为：1、制定编码规则；2、制定解码规则；3、LED灯控制器根据编码规则LED灯发光,LED灯的光信号传输至四元光电探测器；4、四元光电探测器将光信号转换为电压信号发送至上位机；5、上位机利用解码规则、电压信号大小进行数据处理,从而得到UUV和回收站的距离及方位关系。(The invention relates to positioning systems and methods for UUV recovery processes, and solves the problems of low positioning accuracy, large volume of a transducer, large power consumption and the like caused by positioning a UUV by using an acoustic sensor.)

一种用于UUV回收过程的定位系统及方法

技术领域

本发明涉及一种定位系统及方法，具体涉及一种用于UUV回收过程的定位系统及方法。

背景技术

近年来，无人水下航行器(UUV)的回收研究越来越火热，UUV携带有声、压、光等诸多传感器，在回收过程中避免碰撞，保证传感器安全。UUV回收路径的规划及动力控制系统都需要知道UUV相距回收站的距离。再加上回收过程中，复杂多变的海况，导致UUV和回收站的位置关系时刻在变，因此复杂海况下UUV与回收站的距离计算是一个至关重要的研究课题。目前常用的水下测距方法主要依靠声学传感器完成，但是存在定位精度低、换能器体积与功耗大等问题，影响UUV的高效回收，因此研究出具有高精度定位能力的技术手段是UUV回收面临的难题之一。

发明内容

为了解决UUV回收过程中利用声学传感器定位UUV时所带来的定位精度低、换能器体积大、功耗大等问题，本发明提供了一种用于UUV回收过程的定位系统及方法。

为了达到上述目的，本发明采用下述技术方案：

本发明提供了一种用于UUV回收过程的定位系统，包括四元光电探测器、第一LED灯组、第二LED灯组、LED灯控制器以及上位机；

所述四元光电探测器安装在UUV前端；

所述第一LED灯组设置在回收站前端；第一LED灯组包括三个LED灯，三个LED灯位于同一平面，且三个LED灯之间的连线构成一个等边三角形；

所述第二LED灯组设置在回收站尾端；第二LED灯组包括三个LED灯，三个LED灯位于同一平面，且三个LED灯之间的连线构成一个等边三角形；

上位机与所述四元光电探测器连接，所述第一LED灯组、第二LED灯组分别与LED灯控制器连接。

基于上述系统的介绍，现对采用该系统进行UUV回收过程的定位方法进行描述，具体执行步骤如下：

步骤1：制定第一LED灯组和第二LED灯组中共六个LED灯的编码规则，并将该编码规则上传至LED灯控制器；所述编码规则为六个LED灯的发光顺序；

步骤2：制定第一LED灯组和第二LED灯组的解码规则，并将两个解码规则上传至上位机；所述解码规则为上位机对采集的数字信号进行解析的规则；

步骤3：LED灯控制器根据编码规则控制六个LED灯循环闪烁发光，每个LED灯的光信号通过水介质传播到四元光电探测器；

步骤4：当四元光电探测器在第一LED灯组的视场范围内时，利用第一LED灯组的光信号，执行步骤5-7；UUV向回收站方向航行一段距离后，当四元光电探测器脱离第一LED灯组视场范围时，利用第二LED灯组的光信号，执行步骤5-7；

步骤5：四元光电探测器接收第一LED灯组与第二LED灯组中每一个LED灯的光信号，将光信号转换为电压信号，上位机采集电压信号，并转化为数字信号，根据数字信号与第一LED灯组解码规则或第二LED灯组解码规则进行对照，识别出第一LED灯组中三个LED灯发光信息或第二LED灯组中三个LED灯发光信息；

步骤6：根据四元光电探测器中四个象元电压信号的大小，计算四元光电探测器与第一LED灯组或第二LED灯组中每一个LED灯之间水平、垂直方向夹角以及偏移方位；

步骤7：利用四元光电探测器与第一LED灯组或第二LED灯组之间的几何关系，计算四元光电探测器与第一LED灯组或第二LED灯组构成的等边三角形中心之间的距离以及方位关系，即就得到UUV和回收站的距离及方位关系。

进一步地，上述编码规则具体是：

设定时间T为一个大周期，将时间T分隔成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灯全灭；

其中，1号LED灯、2号LED灯、3号LED灯构成第一LED灯组；4号LED灯、5号LED灯、6号构成第二LED灯组。

进一步地，上述解码规则具体是：

第一LED灯组的解码规则具有以下六种情况：

情况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内亮；

对于第二LED灯组解码规则以下六种情况：

情况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内亮；

其中，“1”代表灯亮、“0”代表灯灭、“X”代表1或0。

进一步地，上述步骤6中四元光电探测器与第一LED灯组或第二LED灯组中每一个LED灯之间水平方向夹角φ、垂直方向夹角θ的具体计算公式为：

其中，Kx、Ky、mx、my为四元光电探测器标定值且为常数，U₁、U₂、U₃、U₄为四元光电探测器四个象元电压信号值。

进一步地，上述步骤6中四元光电探测器与第一LED灯组或第二LED灯组中每一个LED灯之间的偏移方位Dirx、Diry具体计算公式为：

其中，Dirx＝1表示LED灯在四元光电探测器中心点的右侧；Diry＝1表示LED灯在四元光电探测器中心点的下侧。

进一步地，上述步骤7的具体实现步骤如下：

步骤7.1：计算第一LED灯组或第二LED灯组中每两个LED灯与四元光电探测器之间的夹角∠AOB、∠BOC、∠COA；

其中，A、B、C分别代表第一LED灯组或第二LED灯组中三个LED灯的中心位置，O代表四元光电探测器的中心位置；

φ_A、φ_B、φ_C分别为四元光电探测器与第一LED灯组或第二LED灯组中每个LED灯水平夹角的余角；

θ_A、θ_B、θ_C分别为四元光电探测器与第一LED灯组或第二LED灯组中每个LED灯垂直夹角的余角；

步骤7.2：判断∠AOB、∠BOC、∠COA三个角度大小关系，计算OA、OB、OC的值；

令∠AOB、∠BOC、∠COA三个角度中角度最大值为α，角度中间值为β，角度最小值为γ：且α、β、γ均小于60°；

情况1：若，α＝β＝γ则

其中，r为第一LED灯组或第二LED灯组构成的等边三角形的边长；

情况2：α＝β≠γ或α≠β＝γ或α＝γ≠β或α≠γ≠β时，

则设定：α与β之间的公共边长度为mid，mid最小值为low，最大值为upp；

且第一次令，low＝r，

mid＝0.5*(low+upp)，之后开始按照以下公式(1)-(3)计算：

若|r₃-r₁|，ε为距离精度，则输出mid，r₁，r₂，跳转至步骤7.3；

若r₃＞r₁+ε，令upp为上一次的mid值，并重新计算mid的值，并将该值代入公式(1)-(3)继续进行计算，直到满足条件|r₃-r₁|＜ε，输出mid，r₁，r₂，跳转至步骤7.3；

若r₃＜r₁-ε，则令low为上一次的mid值，并重新计算mid的值，将该值代入公式(1)-(3)继续进行计算，直到满足条件|r₃-r₁|＜ε，输出mid，r₁，r₂，跳转至步骤7.3；

步骤7.3：计算四元光电探测器中心位置与第一LED灯组或第二LED灯组构成的等边三角形中心点D或第二LED灯组构成的等边三角形中心点H之间的距离OD；

步骤7.4：计算四元光电探测器中心O点与第一LED灯组中心D或第二LED灯组中心H在水平方向夹角φ_d、垂直方向夹角θ_d；

根据步骤7.2中∠AOB、∠BOC、∠COA与α、β、γ之间的对应关系，α与β之间为mid，β与γ为r₂，γ与α之间为r₁，将OA、OB、OC与mid、r₁、r₂一一对应；

步骤7.5：计算四元光电探测器中心位置与第一LED灯组或第二LED灯组构成的等边三角形中心点D之间的方位关系Dirox、Diroy，

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与第一LED灯组或第二LED灯组中三个LED灯在X轴水平方向的方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与第一LED灯组或第二LED灯组中三个LED灯在Z轴垂直方向的方位。

本发明的优点在于：

1、本发明利用四元光电探测器和LED光源，实现了UUV回收过程中UUV与回收站之间距离以及方位关系的精准测量，系统结构简单、测量方法简单实用，极大的提升了UUV的回收成功率，克服了传统以声波为载体测距方法的不足。

2、本发明通过对回收站上第一LED灯组和第二LED灯组制定闪烁规则(即编码规则)，同时制定对应的解码规则，实现了UUV在水下姿态实时变化的情况，能够准确的获取第一LED灯组和第二LED灯组中每一个LED灯相对于四元光电探测器的位置，从而可准确的获取UUV相对于回收站的方位，使得UUV回收工作更加高效和可靠。

3、本发明在回收站的前后安装两个LED灯组，并通过两个LED灯组的切换功能，增大了光电探测器的视场范围。

4、本发明采用定位系统不仅可应用于UUV航行器的自主回收工作，还可以用于其他测距精度要求较高的水下设备中。

附图说明

图1为采用本发明的定位系统进行UUV回收的结构示意图；

图2为编码规则示意图。

图3为四元光电探测器与第一LED灯组或第一LED灯组中任意一个LED灯之间水平、垂直方向夹角角度示意图。

图4为角度与边长对应关系图。

具体实施方式

为使本领域技术人员更加清晰、准确的理解本发明的技术方案，下面结合附图和具体实施例对本发明做进一步说明。

本发明的优选实施方式结合附图论述如下：

参见图1，一种用于UUV回收过程的定位系统，包括四元光电探测器、六个LED灯、LED灯控制器以及上位机；

四元光电探测器安装在UUV前端；其中，1号LED灯、2号LED灯、3号LED灯为第一LED灯组，第一LED灯组安装回收站前端，三个LED灯构成等边三角形，边长为d1；4号LED灯、5号LED灯、6号LED灯为第二LED灯组，第二LED灯组安装回收站尾端，三个LED灯构成等边三角形，边长为d2；

上位机与所述四元光电探测器连接，所述第一LED灯组、第二LED灯组分别与LED灯控制器连接。

该定位系统的基本工作原理如下：

LED灯控制器控制第一LED灯组和第二LED灯组闪烁发光，四元光电探测器接收到第一LED灯组和第二LED灯组的光信号，识别出第一LED灯组和第二LED灯组每一个LED灯发出的光信息，然后根据四元光电探测器原理以及四元光电探测器与第一LED灯组和第二LED灯组中每一个LED灯之间的位置信息，进行计算，最终实时得到UUV和回收站的距离及方位关系。

基于上述系统结构与工作原理的描述，现对采用定位系统进行UUV回收过程中的定位方法进行详细的描述：

步骤1：制定第一LED灯组和第二LED灯组中共六个LED灯的编码规则，并将该编码规则上传至LED灯控制器；所述编码规则为六个LED灯的发光顺序，如图2所示，

设定时间T为一个大周期，将时间T分隔成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灯全灭；

其中，1号LED灯、2号LED灯、3号LED灯构成第一LED灯组；4号LED灯、5号LED灯、6号构成第二LED灯组。

步骤2：制定第一LED灯组和第二LED灯组的解码规则，并将两个解码规则上传至上位机；所述解码规则为上位机对采集的数字信号进行解析的规则；

第一LED灯组的解码规则具有以下六种情况：

情况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内亮；

对于第二LED灯组解码规则以下六种情况：

情况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内亮；

其中，“1”代表灯亮、“0”代表灯灭、“X”代表1或0。

具体解码规则见表1和表2

表1(第一LED灯组解码表)

表2(第二LED灯组解码表)

步骤3：LED灯控制器根据编码规则控制六个LED灯循环闪烁发光，每个LED灯的光信号通过水传播到四元光电探测器；

步骤4：当四元光电探测器在第一LED灯组的视场范围内时，利用第一LED灯组的光信号，执行步骤5-7；向回收站方向航行一段距离后，当四元光电探测器脱离第一LED灯组视场范围时，利用第二LED灯组的光信号，执行步骤5-7；

此处以UUV与回收站距离较远，且四元光电探测器在第一LED灯组的视场范围内的情况为例进行以下说明：

步骤5：四元光电探测器接收到第一LED灯组与第二LED灯组中每一个LED灯的光信号，将光信号转换为电压信号，上位机再将电压信号转化为数字信号，根据数字信号与第一LED灯组解码规则进行对照识别出第一LED灯组中三个LED灯发光信息；

步骤6：根据四元光电探测器中四个象元电压信号的大小，计算四元光电探测器与第一LED灯组中每一个LED灯之间水平、垂直方向夹角以及偏移方位，如图3所示，图中，XZ平面为探测器表面，Y轴方向为探测器轴向，LED’为LED在XY平面的投影。

四元光电探测器与第一LED灯组中任意一个LED灯之间水平、垂直方向夹角具体计算公式为：

其中，Kx、、Ky、mx、my为四元光电探测器标定值且为常数，U₁、U₂、U₃、U₄为四元光电探测器四个象元电压信号值。

四元光电探测器与第一LED灯组中任意一个LED灯之间的偏移方位Dirx、Diry具体计算公式为：

其中，Dirx＝1表示LED灯在四元光电探测器中心点的右侧；Diry＝1表示LED灯在四元光电探测器中心点的下侧。

步骤7：计算四元光电探测器与第一LED灯组构成的等边三角形中心之间的距离以及方位关系，即就得到UUV和回收站的距离及方位关系。

步骤7.1：计算第一LED灯组中每两个LED灯与四元光电探测器之间的夹角∠AOB、∠BOC、∠COA；

其中，A、B、C分别代表第一LED灯组中三个LED灯的中心位置，O代表四元光电探测器的中心位置；

φ_A、φ_B、φ_C分别为四元光电探测器与第一LED灯组中每个LED灯水平夹角的余角；

θ_A、θ_B、θ_C分别为四元光电探测器与第一LED灯组中每个LED灯垂直夹角的余角；

步骤7.2：判断∠AOB、∠BOC、∠COA三个角度大小关系，计算OA、OB、OC的值；

令∠AOB、∠BOC、∠COA三个角度中角度最大值为α，角度中间值为β，角度最小值为γ：且α、β、γ均小于60°；

情况1：若，α＝β＝γ则

其中，d1为第一LED灯组或第二LED灯组构成的等边三角形的边长；

情况2：α＝β≠γ或α≠β＝γ或α＝γ≠β或α≠γ≠β时，

则设定：如图4所示，α与β之间的公共边长度为mid，mid最小值为low，最大值为upp；

且第一次令，low＝d1，mid＝0.5*(low+upp，之后开始按照以下公式(1)-(3)计算：

若|r₃-r₁|，ε为距离精度，则输出mid，r₁，r₂，跳转至步骤7.3；

若r₃＞r₁+ε，令upp为上一次的mid值，并重新计算mid的值，并将该值代入公式(1)-(3)继续进行计算，直到满足条件|r₃-r₁|＜ε，输出mid，r₁，r₂，跳转至步骤7.3；

若r₃＜r₁-ε，则令low为上一次的mid值，并重新计算mid的值，将该值代入公式(1)-(3)继续进行计算，直到满足条件|r₃-r₁|＜ε，输出mid，r₁，r₂，跳转至步骤7.3；

步骤7.3：计算四元光电探测器中心位置与第一LED灯组构成的等边三角形中心点D之间的距离OD；

步骤7.4：计算四元光电探测器中心O点与第一LED灯组中心D或第二LED灯组中心H在水平方向夹角φ_d、垂直方向夹角θ_d；

如图4所示，根据步骤7.2中∠AOB、∠BOC、∠COA与α、β、γ之间的对应关系，α与β之间为mid，β与γ为r₂，γ与α之间为r₁，将OA、OB、OC与mid、r₁、r₂一一对应；

步骤7.5：计算四元光电探测器中心位置与第一LED灯组构成的等边三角形中心点D之间的方位关系Dirox、Diroy，

其中，Dir1x、Dir2x、Dir3x分别为四元光电探测器与第一LED灯组或第二LED灯组中三个LED灯在X轴水平方向的方位，Dir1y、Dir2y、Dir3y分别为四元光电探测器与第一LED灯组或第二LED灯组中三个LED灯在Z轴垂直方向的方位。

在UUV和回收站较近距离，且四元光探测器脱离第一LED灯组视场范围时，切换为第二LED灯组，利用第二LED灯组的光信息以及第二LED灯组的解码规则，从而执行上述步骤6和7，计算UUV和回收站之间的距离、以及方位信息。