CN113805606B - 一种基于超短基线声学定位的水下航行器自适应导引对接回收方法 - Google Patents

Abstract

本发明提出一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，AUV以航路点导引方式依次完成航路点，并到达位于回收站正前方的最后一个航路点，根据参考航向角以及水下航行器速度，采用模糊控制直线跟踪方法继续行驶，进行水下航行器自适应导引对接回收；并在接近回收站的过程中，接收USBL定位测量结果，结合航行器自身的经纬度，对回收站的经纬度进行实时更新；当水下航行器与回收站距离小于设定值后，根据水下航行器速度判断水下航行器是否实现回坞。本发明能够实现AUV的水下回收，降低AUV能源消耗，同时避免发生不必要的碰撞，提高对接成功率。

Description

一种基于超短基线声学定位的水下航行器自适应导引对接回 收方法

技术领域

本发明涉及的是一种自主水下航行器与回收站的对接方法，更具体地说是基于超短基线声学定位的自主水下航行器在水下与回收站进行回收对接的控制方法。

背景技术

21世纪以来，人类社会的发展速度超过了以往几千年的历史时期，自然资源的消耗也达到了空前规模，随之人类开始将目光转向蕴含着丰富资源的海洋空间。海洋资源的开发利用需要先进的设备和技术，而自主水下航行器(Autonomous Underwater Vehicles，简称AUV)作为我们认识和探索海洋领域必不可缺的工具，也逐渐被广泛应用。

AUV在水下活动的时间主要受自身携带能源的限制，为了提高AUV的工作效率，节省回收布放所耗时间，水下的对接回收成为实现AUV连续工作的关键技术。此外，水下对接因其隐蔽性强的特点，可以在不被海面以上发现的情况下完成回收任务，在军事领域上应用更为广泛，但是在水下对接回收方面存在很多技术难题，比如AUV航行路径的规划、AUV自身经纬度的测量。以往的研究提出过多种无人水下航行器的回收对接方式，但是仍存在着对接难度大、精度不高等缺陷，回收成功率较低。可以说AUV的水下自主回收技术优势与难点并存，迫切需要相关知识理论研究。

发明内容

本发明的目的在于提出一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，旨在保证AUV的水下回收，降低AUV能源消耗，同时避免发生不必要的碰撞，提高对接成功率。

本发明的技术方案为：

所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，包括以下步骤：

步骤1：按照任务需求设定航路点，并根据回收站位置及回收方向，将最后一个航路点设置在回收站正前方；

步骤2：水下航行器以航路点导引方式依次完成航路点，并到达位于回收站正前方的最后一个航路点；

步骤3：根据参考航向角ψ_ref以及水下航行器速度，采用模糊控制直线跟踪方法继续行驶，进行水下航行器自适应导引对接回收；并在接近回收站的过程中，接收超短基线声学定位测量的回收站距离和方位，根据测量结果，结合航行器自身的经纬度，对回收站的经纬度进行实时更新；

步骤4：当水下航行器与回收站距离小于设定值后，根据水下航行器速度判断水下航行器是否实现回坞；如果水下航行器的的位置超过回收站后方设定值后，判定对接失败，此时根据设定的回收尝试次数判断是否进行下一次回收机动，若小于回收尝试次数，则控制水下航行器返回到最后一个航路点，并返回步骤3，否则结束任务浮出水面等待打捞。

进一步的，步骤3中参考航向角ψ_ref通过以下过程得到：

参考航向角ψ_ref的计算方式为：

ψ_ref＝ψ_cr+ψ_init

其中ψ_cr为直线跟踪角，ψ_init为任务初始设定的参考航向角；

ψ_cr＝-arctan(k*L)

k为根据水下航行器与回收站距离而确定的直线跟踪参数；L为水下航行器与回收站之间的横向距离。

进一步的，在水下航行器距离回收站200米以外时，选择直线跟踪参数k＝0.05，在水下航行器距离回收站200米及200米以内时，选择直线跟踪参数k＝0.1。

进一步的，水下航行器与回收站之间的横向距离L＝dN₁*sin(ψ)+dE₁*cos(ψ)，其中北向距离dN₁和东向距离dE₁为

dN₁＝(Dlat-Glat)*M₁

dE₁＝(Dlon-Glon)*M₂

M₁＝R_e

M₁是地球子午圈的半径R_e；M₂是地球平行圈的半径；Glon和Glat为水下航行器的经度和纬度；ψ为水下航行器的航向角，Dlon和Dlat为回收站的经度和纬度。

进一步的，步骤3中，对回收站的经纬度进行实时更新的过程为：

根据超短基线USBL测量得到的水下航行器相对于回收站之间的斜距R、水下航行器相对于回收站的方位角θ_yz、水下航行器相对于回收站的俯仰角θ_xz，并结合水下航行器自身的经纬度Glon和Glat，对回收站经纬度Dlon和Dlat进行更新：

回收站距离航行器的东向距离dE和北向距离dN可以通过以下公式求得：

dE＝(-1.0)*Rcos(θ_xz)sin(ψ-θ_yz)

dN＝Rcos(θ_xz)cos(ψ-θ_yz)

并根据纬度更新率ρ₁和经度更新率ρ₂，计算出更新后的回收站的经纬度：

Dlon＝Glon+dE*ρ₂

Dlat＝Glat+dN*ρ₁。

进一步的，纬度更新率ρ₁和经度更新率ρ₂根据公式

其中e²为地球的第一偏心率。

进一步的，步骤4中，当水下航行器与回收站距离小于20米后，根据水下航行器速度判断水下航行器是否实现回坞：

当水下航行器在载体坐标系下对地的前向速度V_x≤0.2m/s，且保持连续时间在20s以上，则认为水下航行器已经成功回坞，否则认为水下航行器回坞失败。

进一步的，当水下航行器回坞失败，则航行器继续行驶，并判断航行器位置是否超过回收站，当航行器的位置超过回收站后方80米时，则判断对接失败。

有益效果

本发明所构思的技术方案与现有技术相比，主要具备以下的技术优点如下：

1、本发明将回收过程包括两部分，一是采用直线跟踪算法对AUV从最后一个设定航路点到回收站之间的路径进行跟踪控制，二是在跟踪控制过程中，利用USBL对回收站位置进行实时更新，提高控制精度，保证AUV能安全可靠地进入回收站；

2、本发明通过惯性导航系统和USBL系统反馈的信息计算得到AUV与回收站的动态信息，以此作为依据来判断回收成功，无需人为干预即可完成回收过程；

3、由于一定存在不易预估且不可避免的外界因素，面对AUV与回收站对接失误的情况，本发明给出了失误时的处理方式，在实际应用中能更方便安全地完成任务；

4、本发明中所需的各项数据测量容易，计算量小，便于实现。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1水下回收系统流程图；

图2水声定位系统示意图；

图3对接回收阶段示意图；

图4对接回收轨迹曲线图；

图5航行器线速度和角速度变化图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。

首先，按照期望的路径设定航路点，航路点的设置应保证避开障碍物，而为了实现水下航行器自适应导引对接回收，设置的离回收站最近的最后一个航路点在回收站正前方，本实施例中处于回收站正前方400米处。

在航路点导引航行器的过程中，航行器通过自身携带的惯性导航系统设备输出解算的自身经度Glon、纬度Glat(本实施例中经纬度均指大地坐标系下的经纬度)和航向角ψ(本实施例中航向角以北偏西为正)，回收站的经度Dlon和纬度Dlat由GPS测得，以此来完成对航路点的跟踪。

AUV以航路点导引方式依次完成航路点，并到达回收站正前方的航路点之后，根据参考航向角ψ_ref以及AUV在大地坐标系下的速度，采用模糊控制直线跟踪方法继续行驶，进行AUV自适应导引对接回收，其中AUV在大地坐标系下的速度通过惯性导航系统设备输出，参考航向角ψ_ref通过以下过程得到：

参考航向角ψ_ref的计算方式为：

ψ_ref＝ψ_cr+ψ_init (1)

其中ψ_cr为直线跟踪角，ψ_init为任务初始设定的参考航向角。

ψ_cr＝-arctan(k*L) (2)

k为根据AUV与回收站距离而确定的直线跟踪参数；本实施例中计算直线跟踪角ψ_cr时，在距离回收站200米以外时，选择直线跟踪参数k＝0.05，使得航行器直线跟踪运动轨迹较为平滑，超调较少；在距离回收站200米以内时，选择直线跟踪参数k＝0.1，使得航行器在跟踪水声定位设备更新的回收站位置时，航行器可以快速且有效地做出相应位姿调整，保证进入回收站的航向和位置，即

L为航行器与回收站之间的横向距离：

L＝dN₁*sin(ψ)+dE₁*cos(ψ) (4)

北向距离dN₁和东向距离dE₁为

dN₁＝(Dlat-Glat)*M₁ (5)

dE₁＝(Dlon-Glon)*M₂ (6)

M₁＝R_e (7)

M₁是地球子午圈的半径R_e；M₂是地球平行圈的半径，两者均为近似值，但都满足导航精度。

在AUV接近回收站的过程中，超短基线USBL测量出的信息有：航行器相对于回收站之间的斜距R，航行器相对于回收站的方位角θ_yz(右正左负)，航行器相对于回收站的俯仰角θ_xz(上正下负)。由于在水下环境中，回收站依靠GPS定位的经纬度精度不能满足回收需要，因此本发明根据超短基线USBL测量结果，结合航行器自身的经纬度Glon和Glat，对回收站经纬度Dlon和Dlat进行更新，更新方法如下：

回收站距离航行器的东向距离dE和北向距离dN可以通过以下公式求得：

dE＝(-1.0)*Rcos(θ_xz)sin(ψ-θ_yz) (9)

dN＝Rcos(θ_xz)cos(ψ-θ_yz) (10)

已知地球的第一偏心率e²＝0.0066943799013，地球半径R为6378137.0米，则可以计算航行器所在子午圈曲率半径以及卯酉圈曲率半径，进而计算出纬度更新率ρ₁和经度更新率ρ₂：

由此，计算出更新后的回收站的经纬度：

Dlon＝Glon+dE*ρ₂ (13)

Dlat＝Glat+dN*ρ₁ (14)

接下来计算更新后的回收站与航行器之间的距离y_e：

当y_e小于20米后，开始通过航行器速度的变化，判断航行器是否实现回坞。本实施例中的判别条件为：当航行器在载体坐标系下对地的前向速度V_x≤0.2m/s，且保持连续时间在20s以上，则认为航行器已经成功回坞，否则认为航行器回坞失败，航行器继续行驶，并判断航行器位置是否超过回收站，当航行器的位置超过回收站后方80米时，则判断对接失败。此时根据设定的尝试次数判断是否进行下一次回收机动，若小于回收尝试次数，则回到最后一个航路点，重新回收，若达到尝试次数，则结束任务浮出水面等待打捞。

如图4所示，为实验结果的轨迹曲线图，圆圈和正方块分别标注出轨迹的起点和终点，曲线代表自主水下航行器实际的轨迹，起点在(-2,1,2),终点在(300,0,0)，即为回收站的位置。图5是对接回收实验中的航行器线速度和角速度变化图，上方短虚线、长虚线和实线，分别表示X轴、Y轴、Z轴的线速度变化，下方短虚线、长虚线和实线，分别表示X轴、Y轴、Z轴的角速度变化。

从实验结果看出，采用本发明提出的方法能够实现AUV的水下回收，降低AUV能源消耗，同时避免发生不必要的碰撞，提高对接成功率。

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

Claims (8)

1.一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：包括以下步骤：

步骤1：按照任务需求设定航路点，并根据回收站位置及回收方向，将最后一个航路点设置在回收站正前方；

步骤2：水下航行器以航路点导引方式依次完成航路点，并到达位于回收站正前方的最后一个航路点；

步骤3：根据参考航向角ψ_ref以及水下航行器速度，采用模糊控制直线跟踪方法继续行驶，进行水下航行器自适应导引对接回收；并在接近回收站的过程中，接收超短基线声学定位测量的回收站距离和方位，根据测量结果，结合航行器自身的经纬度，对回收站的经纬度进行实时更新；

步骤4：当水下航行器与回收站距离小于设定值后，根据水下航行器速度判断水下航行器是否实现回坞；如果水下航行器的的位置超过回收站后方设定值后，判定对接失败，此时根据设定的回收尝试次数判断是否进行下一次回收机动，若小于回收尝试次数，则控制水下航行器返回到最后一个航路点，并返回步骤3，否则结束任务浮出水面等待打捞。

2.根据权利要求1所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：步骤3中参考航向角ψ_ref通过以下过程得到：

参考航向角ψ_ref的计算方式为：

ψ_ref＝ψ_cr+ψ_init

其中ψ_cr为直线跟踪角，ψ_init为任务初始设定的参考航向角；

ψ_cr＝-arctan(k*L)

k为根据水下航行器与回收站距离而确定的直线跟踪参数；L为水下航行器与回收站之间的横向距离。

3.根据权利要求2所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：在水下航行器距离回收站200米以外时，选择直线跟踪参数k＝0.05，在水下航行器距离回收站200米及200米以内时，选择直线跟踪参数k＝0.1。

4.根据权利要求2所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：水下航行器与回收站之间的横向距离L＝dN₁*sin(ψ)+dE₁*cos(ψ)，其中北向距离dN₁和东向距离dE₁为

dN₁＝(Dlat-Glat)*M₁

dE₁＝(Dlon-Glon)*M₂

M₁＝R_e

M₁是地球子午圈的半径R_e；M₂是地球平行圈的半径；Glon和Glat为水下航行器的经度和纬度；ψ为水下航行器的航向角，Dlon和Dlat为回收站的经度和纬度。

5.根据权利要求1所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：步骤3中，对回收站的经纬度进行实时更新的过程为：

根据超短基线USBL测量得到的水下航行器相对于回收站之间的斜距R、水下航行器相对于回收站的方位角θ_yz、水下航行器相对于回收站的俯仰角θ_xz，并结合水下航行器自身的经纬度Glon和Glat，对回收站经纬度Dlon和Dlat进行更新：

回收站距离航行器的东向距离dE和北向距离dN可以通过以下公式求得：

dE＝(-1.0)*Rcos(θ_xz)sin(ψ-θ_yz)

dN＝Rcos(θ_xz)cos(ψ-θ_yz)

并根据纬度更新率ρ₁和经度更新率ρ₂，计算出更新后的回收站的经纬度：

Dlon＝Glon+dE*ρ₂

Dlat＝Glat+dN*ρ₁。

6.根据权利要求5所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：纬度更新率ρ₁和经度更新率ρ2根据公式

其中e²为地球的第一偏心率。

7.根据权利要求1所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：步骤4中，当水下航行器与回收站距离小于20米后，根据水下航行器速度判断水下航行器是否实现回坞：

当水下航行器在载体坐标系下对地的前向速度V_x≤0.2m/s，且保持连续时间在20s以上，则认为水下航行器已经成功回坞，否则认为水下航行器回坞失败。

8.根据权利要求1所述一种基于超短基线声学定位的水下航行器自适应导引对接回收方法，其特征在于：当水下航行器回坞失败，则航行器继续行驶，并判断航行器位置是否超过回收站，当航行器的位置超过回收站后方80米时，则判断对接失败。