一种欠驱动AUV移动对接动态路径规划方法及装置

一种欠驱动auv移动对接动态路径规划方法及装置
技术领域
1.本技术涉及水下自主航行器的回收技术领域，尤其涉及一种欠驱动auv移动对接动态路径规划方法及装置。


背景技术：

2.海洋强国战略是全面建设社会主义现代化强国的重要组成部分。水下自主航行器(autonomous underwater vehicle,auv)是公认的探索海洋的重要工具，在海洋资源探测、水文环境监测、水下情报侦察等商业和国防领域具有重要的应用价值。auv自身携带能源有限，因此在进行一段时间的作业后必须进行回收以获得能源补充。回收技术是将在海洋环境中作业的auv导引进入对接装置的关键技术，它可以保障auv与位于对接装置内的充电装置建立物理连接以便实现后续的充电和数据交互。目前关于水下对接系统的开发主要集中在对于静态对接系统的开发，国内外已有了较多成功的尝试和探索。但静态回收对接的方式极大的限制了auv的航行范围，不利于auv的远程作业。因此，探索和搭载于具有运动能力的船舶、大型水下机器人或移动载体上的对接装置的移动对接系统是水下机器人回收对接技术发展的大势所趋。
3.目前移动回收对接相关技术的探索还处于起步阶段且主要研究集中在环境感知和机器人控制领域，对于规划方法的研究较少。现有回收对接系统中实际使用的方法主要基于点对点制导定律实现的相关方法。该类方法利用几何关系通过auv和目标之间的相对位姿关系实时计算得到下一时刻的期望航向。常用的基于点对点制导定律的规划方法有比例导引法、纯追踪导引法、视线法。基于制导定律的规划算法计算量小，能够实时对环境做出响应，但该类方法没有考虑auv的运动执行能力，在存在较大环境干扰条件下也难以满足较为严格的对接装置进入条件。其他公开研究主要基于图形搜索、随机搜索树采样、曲线插值和智能优化等方法发展而来，但这些方法规划耗时较长，难以实时实现。基于视觉定位的末端路径规划通常采用视觉伺服的方式实现，通过视觉定位解算位置偏差后直接交由控制器驱动相应推进器实现偏差修正；该方法依赖于全驱动机器人的侧向推进器以实现快速偏差修正，不适用于欠驱动机器人。
4.在实现本发明的过程中，发明人发现现有技术中至少存在以下问题：1、支持实时实现的规划方法没有考虑欠驱动auv的实际执行能力，可能给出不合理的规划结果造成误差无法收敛或是控制超调。2、路径规划忽略auv环境感知的需求，可能造成航行过程中视觉定位数据丢失的问题从而进一步导致对接失败的问题。3、针对移动回收对接任务的规划方法中没有充分利用移动对接相对速度可调的优势。


技术实现要素：

5.本技术实施例的目的是提供一种欠驱动auv移动对接动态路径规划方法及装置、电子设备，以解决移动回收对接问题中实时规划方法不考虑auv实际执行能力，不考虑对接过程中环境感知需求以及没有充分利用对移动对接优势的技术问题，有效提升了移动回收
对接规划方法的鲁棒性。
6.根据本技术实施例的第一方面，提供一种欠驱动auv移动对接动态路径规划方法，包括：
7.s101：auv自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
8.s102：auv和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
9.s103：auv根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；
10.s104：当auv上的摄像头捕获到所述移动对接装置时，auv根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；
11.s105：s104步骤执行结束后判断auv是否满足所述移动对接装置的进入条件，若满足所述进入条件且auv成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从auv当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明auv当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则重新回到s104步骤继续进行对接。
12.进一步地，在步骤s105之前还包括：
13.s106：若步骤s104过程中所述移动对接装置从auv视野中丢失，则auv在视野丢失时立即按照最大减加速度减速至低于移动对接装置所允许的最低航行速度并以纯追踪方法朝向预测的移动对接装置位置航行，直至所述移动对接装置回到auv视野中，返回步骤s104。
14.进一步地，所述预测的移动对接装置位置的获取过程包括：
15.根据视觉定位信息存在时最后一次解算得到目标位姿预测auv视野丢失期间移动对接装置所在位置：
[0016][0017]
其中，dock
x，last
，dock
y，last
，dock
ψ，last
是在视觉定位信息存在时最后一次解算得到的移动对接装置的位置和航向信息，docku表示移动对接装置在对接期间的固定航行速度，δt
loss
是从视觉信息丢失时刻开始到当前规划时刻的时间差值。
[0018]
进一步地，步骤s104包括：
[0019]
建立洋流影响环境下auv的运动学模型和动力学模型；
[0020]
将所述运动学模型和动力学模型离散化，根据离散化的模型预测未来时刻auv的状态；
[0021]
根据所述摄像头采集的图像进行所述移动对接装置的位姿解算；
[0022]
根据对图像进行位姿解算后获得的移动对接装置当前位姿信息，结合所述移动对接装置定速定向航行的前提条件，预测所述移动对接装置在未来时刻的位置；
[0023]
根据由离散化模型预测得到的未来时刻的状态和预测得到的移动对接装置未来时刻的位姿建立auv与所述移动对接装置在位置收敛、航向收敛和速度收敛条件下的目标函数；
[0024]
建立auv运动能力约束；
[0025]
建立视野持续跟踪约束并引入控制冗余度考虑；
[0026]
对auv运动能力约束、持续视野跟踪约束下的目标函数最小化优化问题进行求解，得到下一时刻的航行速度和方向。
[0027]
进一步地，根据对图像进行位姿解算后获得的移动对接装置当前位姿信息，结合移动对接装置定速定向航行的前提条件，预测所述移动对接装置在未来时刻的位置，包括：
[0028][0029]
其中，dock
x，i
,dock
y，i
,dock
ψ，i
,分别表示时刻i的移动对接装置的位置和航向信息，docku表示移动对接装置在对接期间的固定航行速度，δt表示采样间隔时间。
[0030]
进一步地，所述目标函数为：
[0031][0032]
其中，j1＝δp
′
q1δp,j2＝δψ
′
q2δψ,j3＝δu
′
q3δu分别表示位置收敛、航向收敛、速度收敛相应要求，w1，w2，w3为各项指标的加权系数，其中δp,δψ,δu分别表示auv与移动对接装置之间的位置偏差、航向偏差和速度偏差，q1，q2，q3分别对应各项优化指标的权重系数矩阵。
[0033]
进一步地，根据所述摄像头采集的图像进行所述移动对接装置的位姿解算，以对所述移动对接装置进行持续视野跟踪，包括：
[0034]
获取摄像头采集的所述移动对接装置上光源的图像，其中所述移动对接装置的导引罩的外侧和内侧安装有两层定位灯阵，外侧的定位灯阵用于进行远距离的视觉导引，内侧的定位灯阵用于进行近距离的视觉导引；
[0035]
根据所述图像中的光源，进行所述移动对接装置的位姿解算。
[0036]
根据本技术实施例的第二方面，提供一种欠驱动auv移动对接动态路径规划装置，包括：
[0037]
绕圈航行模块，用于auv自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
[0038]
建立连接模块，用于auv和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
[0039]
追踪接近模块，用于auv根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；
[0040]
局部规划模块，用于当auv上的摄像头捕获到所述移动对接装置时，auv根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；
[0041]
发送信号模块，用于局部规划模块执行结束后判断auv是否满足所述移动对接装置的进入条件，若满足所述进入条件且auv成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从auv当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明auv当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则继续进行对接。
[0042]
根据本技术实施例的第三方面，提供一种电子设备，包括：
[0043]
一个或多个处理器；
[0044]
存储器，用于存储一个或多个程序；
[0045]
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。
[0046]
根据本技术实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如第一方面所述方法的步骤。
[0047]
本技术的实施例提供的技术方案可以包括以下有益效果：
[0048]
由上述实施例可知，本技术根据任务阶段不同需求使用不同的导引方式。远端归航阶段采用纯追踪方法追踪多个航路点实现了auv快速接近移动对接装置并实现航向大致对齐的需求，计算量小、能耗低且有利于末端捕获视觉定位数据。末端对接阶段采用模型预测控制的方法，将环境感知要求、auv运动执行能力和环境干扰直接加入规划问题。通过求解局部规划问题保障了对接阶段视觉定位模块的正常工作，也使得欠驱动auv能够及时响应环境变化以抵抗环境干扰并给出符合auv实际运动能力的规划结果。
[0049]
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本技术。
附图说明
[0050]
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本技术的实施例，并与说明书一起用于解释本技术的原理。
[0051]
图1是根据一示例性实施例示出的一种欠驱动auv移动对接动态路径规划方法的流程图。
[0052]
图2是根据一示例性实施例示出的一种搭载于运动船舶下方的漏斗型移动对接装置。
[0053]
图3是根据一示例性实施例示出的一种搭载于大型水下机器人的背驼式移动对接装置。
[0054]
图4是根据一示例性实施例示出的仿真1无海流环境下归航阶段航路点追踪轨迹示意图。
[0055]
图5是根据一示例性实施例示出的仿真2侧流环境下归航阶段航路点追踪轨迹示意图。
[0056]
图6是根据一示例性实施例示出的步骤s104的流程图。
[0057]
图7是根据一示例性实施例示出的一种采用双层导引灯的漏斗形对接装置示意图。
[0058]
图8是根据一示例性实施例示出的仿真3侧流环境下末端回收对接轨迹示意图。
[0059]
图9是根据一示例性实施例示出的仿真3中auv前向速度变化示意图。
[0060]
图10是根据一示例性实施例示出的仿真3中auv航向变化示意图。
[0061]
图11是根据一示例性实施例示出的仿真3中auv侧向速度变化示意图。
[0062]
图12是根据一示例性实施例示出的一种欠驱动auv移动对接动态路径规划方法的流程图。
[0063]
图13是根据一示例性实施例示出的一种欠驱动auv移动对接动态路径规划装置的框图。
具体实施方式
[0064]
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本技术相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本技术的一些方面相一致的装置和方法的例子。
[0065]
在本技术使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本技术。在本技术和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
[0066]
应当理解，尽管在本技术可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本技术范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在
……
时”或“当
……
时”或“响应于确定”。
[0067]
名词解释：欠驱动auv：auv的驱动器数量少于其自由度。在水平面运动中，欠驱动机器人由于不具有侧向推进器，无法通过直接控制推力实现auv位置的左右移动。
[0068]
移动对接装置：搭载于具有运动能力的船舶、大型水下机器人或移动载体上的对接装置。
[0069]
图1是根据一示例性实施例示出的一种欠驱动auv移动对接路径规划方法的流程图，如图1所示，该方法应用于欠驱动auv，可以包括以下步骤：
[0070]
步骤s101：auv自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
[0071]
步骤s102：auv和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
[0072]
步骤s103：auv根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；该阶段导引方法在无海流情况下和存在侧流干扰情况下的仿真示例如图4和图5所示。
[0073]
步骤s104：当auv上的摄像头捕获到所述移动对接装置时，auv根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向。
[0074]
步骤s105：s104步骤执行结束后判断auv是否满足所述移动对接装置的进入条件，若满足所述进入条件且auv成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从auv当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明auv当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则重新回到s104步骤继续进行对接。
[0075]
由上述实施例可知，本技术根据任务阶段不同需求使用不同的导引方式。远端归航阶段采用纯追踪方法追踪多个航路点实现了auv快速接近移动对接装置并实现航向大致对齐的需求，计算量小、能耗低且有利于末端捕获视觉定位数据。末端对接阶段采用模型预测控制的方法，将环境感知要求、auv运动执行能力和环境干扰直接加入规划问题，通过求解局部规划问题保障了对接阶段视觉定位模块的正常工作，也使得欠驱动auv能够及时响应环境变化以抵抗环境干扰并给出符合auv实际运动能力的规划结果。
[0076]
本发明中所述移动对接装置是指对接装置搭载于船舶、大型水下机器人或移动载体等移动航行器上，随之一起移动。该方法所适用的移动对接装置为只支持单向进入的对接装置，如漏斗型移动对接装置和背驼式移动对接装置，分别如图2、图3所示。移动对接装置中的移动是指移动对接装置还包括用于提供定位数据的声学传感器装置和用于视觉定位的导引光源。导引光源的一种布置方式是在对接入口处按正方形分布布放四个定位光源用于视觉定位。其中声学定位数据由超短基线传感器提供。
[0077]
在步骤s101的具体实施中，auv自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
[0078]
具体地，绕圈航行是由于欠驱动auv具有最小转弯半径的限制，无法实现原地旋转。绕圈航行是指auv按固定航行速度以恒定角速度从当前位置开始转向行驶。通过旋转操作使得auv可以在360度范围内搜索声学定位数据，获得最佳定位结果。
[0079]
其中，若该阶段持续时间超过允许的最大定位时间限制后，判定为对接失败，向决策中心上传对接失败信号，终止本次对接尝试。决策中心是auv的任务分配中心，在多任务系统中起到任务调度的作用。
[0080]
其中，声学定位数据通常由超短基线传感器数据或长基线传感器数据提供。
[0081]
在步骤s102的具体实施中，auv和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
[0082]
具体地，搭载有移动对接装置的目标航行器最初通过动力定位悬停在人为划定的安全对接水域之内。由于欠驱动航行器对侧流干扰抵抗力较弱，定向航行的初始航行方向设为迎流航行，以最大程度减少近端末端对接过程中侧向海流的不利干扰。对接装置定速定向的移动方式有利于auv在对接过程中定位数据的获取，有助于降低对接难度。
[0083]
在步骤s103的具体实施中，auv根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；
[0084]
具体地，该步骤的作用阶段为远端归航阶段。远端归航阶段是指auv从进入回收水域后直到在接近目标对接装置过程中获取到视觉定位数据之前的过程。此时auv距离移动对接装置较远，允许偏差范围较大，因此选用计算量小，简单易用的纯追踪算法执行规划任
务即可。纯追踪方法直接指向移动对接装置航行，还可以保证最好的声学定位效果。
[0085]
进一步说明，该阶段航向规划采用纯追踪方法，也就是朝向预定目标点的视线角方向航行。纯追踪方法具有从尾部接近的特性，可以从移动对接装置的尾部实现接近，符合对接装置只能从尾部进入的单向对接要求。单向对接是指auv仅能从单一入口方向进入移动对接装置，主要针对漏斗型对接装置和背驼式对接装置。单向对接装置主要区别于另一种常用的对接装置形式：叉-杆式对接装置。叉-杆式对接装置可以支持auv从任何方向接近对接装置完成对接。该阶段目标是尽快从尾部接近移动对接装置，因此按约定深度固定航行即可。综上所述，该阶段规划得到的下一时刻期望航行、期望速度、期望深度指令如下式所示。
[0086][0087]
其中ψ
next
，u
next
，z
next
分别表示下一时刻的期望航向、期望速度、期望深度。(x，y)表示auv的当前位置，(p
x
，py)表示目标航路点的位置。由于目标对接装置深度已知，该阶段期望深度直接按固定深度z
fixed
定深航行。为节省能量，该阶段直接按固定速度u
fixed
航行即可。当auv到达预设目标点一定范围内即认为到达目标点，目标点切换至下一个目标点。
[0088]
其中，为了使得auv在接近过程中的航向尽可能与对接装置移动方向对齐以确保auv在接近过程中能够探测到视觉定位数据，设置移动对接装置移动方向正后方增设两个航路点以实现航向对齐。
[0089]
在一实施例中，航路点依次设计为移动对接装置运动方向正后方100米，移动对接装置正后方50米，移动对接装置处，共三个航路点。通过追踪过程中航路点的不断切换，auv在接近的过程和目标对接装置间的航向角偏差会逐步减小，有利于视觉后续视觉定位数据的获取。在上述航路点设置下，该方法在无海流环境中和存在侧流干扰环境中的仿真结果分别如附图4和附图5所示。附图4中auv初始位置设定为(0,0),初始航向朝向正东方向，初始速度2m/s。移动对接装置初始位置位于(200,30)，初始朝向也沿正东方向，按1m/s速度定向航行。通过s103步骤的具体实施中所述规划方法，auv在不存在海流干扰的环境中成功接近移动对接装置并完全实现了航向对齐。附图5中初始参数设置同附图4，与附图4的区别在于此时对接水域中存在沿正南方向0.3m/s的洋流干扰。从附图5中可以看到在海流存在的环境中，尽管航行轨迹在发生航路点切换时不够平滑，但是仍能在所提出方法的导引下接近对接装置并捕获视觉定位信息，证明了该方法在存在洋流干扰环境中的鲁棒性。从附图5中可以看到，归航阶段结束时auv和移动对接装置位置间还存在一定距离，剩余航向和距离偏差将在末端对接阶段进行处理。
[0090]
在s104步骤的具体实施中，当auv上的摄像头捕获到所述移动对接装置时，auv根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；
[0091]
具体地，移动对接装置采用具有喇叭口导引的漏斗型对接装置。漏斗型对接装置具有无需对auv进行改装、不影响航行器流体力学特性、可以容许一定的轨迹误差的优点。但该对接装置只能允许单向对接，对末端接触时的位置和航向误差都有一定的要求。声学
定位数据更新频率低、惯导数据累积误差较大，因此视觉定位数据丢失可能会导致对接过程的失败。其中，该步骤作用范围为auv从摄像头捕获视觉定位数据开始直至成功进入对接装置的过程。如图6所示，此步骤可以包括以下子步骤：
[0092]
步骤s201：建立洋流影响环境下auv的运动学模型和动力学模型；
[0093]
其中，对接过程中的环境干扰是指海流干扰和突发未知扰动。尽管对接初期目标对接装置迎流航行，但对接过程中由于航行距离、洋流时变性、尾流干扰等因素都可能造成末端对接时仍存在较小的侧流干扰。因此首先在洋流影响环境下进行auv水平面运动学和动力学模型建立，以便对海流环境干扰下的欠驱动auv运动进行描述。其中海流环境干扰下的运动学模型和动力学模型如下式所示：
[0094][0095][0096]
其中，v
x
，vy分别表示世界坐标系下的洋流在x方向和y方向的分量。为简化问题表示，此处假定海流为定常无旋流。因此该分量为标量。由于每个采样间隔时间都需要重新进行预测规划，因此在单一时刻内对于定常流的假设是合理的。其中ur，vr分别表示auv相对于海流的前向速度和侧向速度，ψ表示auv的当前朝向，r为auv的加速度。τu，τr分别表示auv在前向运动和转向运动上的推进力，从动力学模型中可以直观看到缺乏侧向速度的直接驱动力。均表示由于流体力学特性引入的力。表示对应参量的微分约束。
[0097]
步骤s202：将所述运动学模型和动力学模型离散化，根据离散化的模型预测未来时刻auv的状态；
[0098]
具体地，实际的系统控制指令按照离散的方式发送给执行器，因此运动方程所表示约束进行离散化用于预测未来时刻auv的状态。得到预测方程表示如下所示。为了减少规划过程中的计算量对动力学方程进行了简化，并忽略了角速度对应的动力学方程。
[0099][0100]
其中x(k)，y(k)，ψ(k)分别表示k时刻的auv的状态量，分别代表导航坐标系下沿x轴的位置、导航坐标系下沿y轴的位置、航向。u(k)，v(k)表示载体坐标系下auv的前向速度、侧向速度。a(k)，r(k)是k时刻的auv控制参量，表示载体坐标系下auv的前向加速度和偏航
角角速度。其中δt表示采样间隔时间。x(k+1)，y(k+1)，ψ(k+1)表示k+1时刻的auv相应状态量。
[0101]
步骤s203：根据所述摄像头采集的图像进行所述移动对接装置的位姿解算。
[0102]
在一实施例中，根据摄像头采集的图像进行位姿解算即视觉定位的部分是通过epnp算法(kirk j t o.light and photosynthesis in aquatic ecosystems[m].cambridge university press,1994.)解算由auv携带的摄像头捕获到的图像数据获得。在具体实施中，图像位姿解算还可以通过linemode算法(hoda
ň
t,zabulis x,lourakis m,et al.detection and fine 3d pose estimation of texture-less objects in rgb-d images[c]//2015ieee/rsj international conference on intelligent robots and systems(iros).ieee,2015:4421-4428.)、pvnet算法(peng s,zhou x,liu y,et al.pvnet:pixel-wise voting network for 6dof object pose estimation[j].ieee transactions on pattern analysis and machine intelligence,2020.)实现。
[0103]
步骤s204：根据对图像进行位姿解算后获得的移动对接装置当前位姿信息，结合移动对接装置定速定向直线航行的前提条件，推测所述移动对接装置在未来时刻的位置；
[0104]
具体地，本步骤通过下式进行推测：
[0105][0106]
其中，dock
x，i
,dock
y，i
,dock
ψ，i
,分别表示时刻i的移动对接装置的位置和航向信息，docku表示移动对接装置在对接期间的固定航行速度。δt表示采样间隔时间。
[0107]
步骤s205：根据由离散化模型预测得到的未来时刻的状态和预测得到的移动对接装置未来时刻的位姿建立auv与所述移动对接装置在位置收敛、航向收敛和速度收敛条件下的目标函数；
[0108]
具体地，所述目标函数如下式所示：
[0109][0110]
j1＝δp’q1δp
[0111]
j2＝δψ’q2δψ
[0112]
j3＝δu’q3δu
[0113]
其中，j1＝δp
′
q1δp,j2＝δψ
′
q2δψ,j3＝δu
′
q3δu分别表示位置收敛、航向收敛、速度收敛相应要求。w1，w2，w3为各项指标的加权系数，其中δp,δψ,δu分别表示auv与移动对接装置之间的位置偏差、航向偏差和速度偏差。q1，q2，q3分别对应各项优化指标的权重系数矩阵，
[0114]
目标函数通过线性加权的方式将多目标优化问题转为单目标优化问题。由于各项指标之间会存在较大尺度差异，因此加权系数需要进行缩放到相同尺度，以免单一目标占优使得其他子目标失效。欠驱动航行器在海流干扰下位置收敛、速度收敛、航向收敛是互相冲突的目标参数，在局部优化的前提下很难同时达到最小值，因此根据漏斗型对接装置容许度的要求，适当减弱对于航向和速度的约束系数，在满足约束的前提下优先保障位置收敛。q1，q2，q3分别对应各项优化指标的权重系数，可以根据任务需求分配相应权重。
[0115]
其中，时刻i下的各项偏差具体计算如下式所示。其中xi，yi，ψi，ui分别表示时刻i时auv的当前状态信息，dock
x，i
,dock
y，i
,dock
ψ，i
,dock
u，i
分别表示时刻i的移动对接装置的状态信息。
[0116][0117]
进一步说明，位置收敛是指auv在对接末端需要到达移动对接装置入口处。航向收敛是指漏斗型对接装置只能容许单向进入，因此对于末端对接时的航向和对接装置的入口朝向偏差不能超过一定范围。速度收敛是指接近时auv和移动目标之间的相对速度不能过大，以免接触时发生强烈的碰撞。漏斗型对接装置所定义的成功对接条件如下式所示。
[0118]
|p
auv-p
dock
|≤ε
p
[0119]
|ψ
auv-ψ
dock
|≤ε
ψ
[0120]
|u
auv-u
dock
|≤εu
[0121]
其中，ε
p
，ε
ψ
，εu分别表示对接装置对于位置、航向和速度偏差的冗余度，这些参数由对接装置的设计参数决定。
[0122]
步骤s206：建立auv运动能力约束；
[0123]
具体地，auv运动能力约束是指，执行机构通过改变驱动力间接改变控制量和状态量的变化，因此控制量在无外界干扰的情况下不能发生突变。也就是要求规划出的轨迹应符合角速度、加速度、速度相应限制。末端对接距离较近，生成的轨迹需要充分考虑auv的运动能力，避免auv无法跟踪目标轨迹导致的超调；这可能会导致短时间内无法修正的偏差。
[0124]
进一步说明，欠驱动auv由于驱动力有限，因此只能通过前进和转向来控制auv的位置变化。因此，在存在环境干扰的海洋环境中工作时，auv通常会有一个最低速度的限制以使得auv具有抵抗海流干扰的能力。此外，侧向速度是由于运动耦合导致的，不直接受控于驱动力，因此不对其进行显示约束。上述auv运动能力约束表示如下公式所示。
[0125][0126]
步骤s207：建立视野持续跟踪约束并加入控制冗余度考虑。
[0127]
具体地，建立视野持续跟踪约束是通过根据所述摄像头采集的图像进行所述移动对接装置的位姿解算实现的，具体可以包括以下步骤：
[0128]
步骤s301：获取摄像头采集的所述移动对接装置上光源的图像，其中所述移动对接装置的导引罩的外侧和内侧安装有两层定位灯阵，外侧的定位灯阵用于进行远距离的视觉导引，内侧的定位灯阵用于进行近距离的视觉导引；
[0129]
具体地，图7给出一种在锥形对接装置上安装双层定位灯阵的示意图。现有对接装置定位灯阵仅安装于对接装置外侧，也就是图中所对应的外层灯阵。但由于近距离视野受限，auv距离对接装置入口非常近距离时不可能捕获外层灯阵的图像进行定位信息解算，此时只能依靠声学定位数据继续向前导引。在进入过程中可能会因为定位误差导致auv偏离
入口进一步造成无法进入对接装置的问题。因此为了保证视觉定位在整个末端对接中作用的连续性并且不影响现有外层灯阵的作用距离，在对接装置内增设一层内层导引光源。使得在近距离无法捕获外层光源时可以通过捕获内层光源的定位图像完成视觉定位，并可以通过视觉定位信息将auv直接导引进入对接装置储存舱体内，保证对接的成功。通过提出双层灯光导引方案，通过在移动对接装置内增加一层定位灯光，解决了由于auv近距离视野受限造成的视觉定位数据丢失的问题，进而能够实现在视觉导引作用下将auv完全导引进入移动对接装置的技术效果。
[0130]
步骤s302：根据所述图像中的光源，进行所述移动对接装置的位姿解算
[0131]
视觉定位通过s1043所描述的方法对安装于auv艏部的摄像头捕获锥形对接装置上的多个光源进行位姿数据解算。声学定位数据更新周期较长，而在移动对接中，由于收发装置间的复杂相对运动关系会声学定位数据的精度和误码率都会受到一定程度的影响。因此在需要精准位姿信息的对接末端采用具有精度高、数据更新快、准确度高优点的视觉定位方式作为主要定位信息。
[0132]
其中，持续视觉跟踪是指，为了保证能够持续获取视觉定位数据，auv在接近目标的过程中需要时刻具有视觉目标感知的能力，也就是对接装置上安装的用于定位的所有光源需位于auv摄像头视野范围之内。
[0133]
进一步说明，持续的视觉定位数据获取要求auv在行驶的过程中，从当前位置指向灯阵上所有光源的视线角和auv当前航向的夹角小于摄像头开角的一半，如下式所示。
[0134]
abs(atan((p
light_y，i-y)/(p
light_x，i-x))-ψ)≤visionangle
[0135]
其中表示n个光源的位置坐标，visionangle是艏部搭载的摄像头视线开角的一半。(x，y，ψ)分别表示auv当前的位置和航向。
[0136]
当auv在接近对接装置的过程中保持持续视觉跟踪时可以同时保障末端对接过程中的安全问题。末端对接安全性的保障也是通过持续的视野跟踪实现，避免由于定位错误导致的auv从非入口方向和对接装置发生碰撞的安全性问题。由于回收对接水域是人为选定的安全对接水域，因此并不存在障碍物干扰。该阶段的安全性问题是由于auv和移动对接装置的不合理碰撞引入的。持续的视野跟踪是末端对接中对接安全的有利保障，可以绝对保障auv不与目标对接装置非导引口部分发生碰撞，以免造成auv和对接装置的损坏。
[0137]
具体地，由于海洋环境中auv运动的复杂性，auv在真实海洋环境中的运动难以用预测模型精确表达。通信时延、控制死区电压和控制误差等因素都可能造成auv的实际运动和预测之间存在一定的偏差。也就是存在模型失配的问题。为了保障在存在一定程度模型失配的情况下auv也能够成功的完成对接，此处将持续视野跟踪约束进行0.7倍的缩放，使得auv在实际任务执行过程中即使存在一定偏差也能满足持续视野跟踪的要求。引入控制冗余度的相应变化如下式所示。
[0138]
abs(atan((p
light_y，i-y)/(p
light_x，i-x))-ψ)≤visionangle
·
0.7
[0139]
步骤s208：对auv运动能力约束、持续视野跟踪约束下的目标函数最小化优化问题进行求解，得到下一时刻的航行速度和方向。
[0140]
经过上述步骤，建立了一个完整的约束非线性优化问题。采用大型非线性优化求解器ipopt在每个规划时刻对优化问题进行求解，得到当前时刻规划的速度和航向指令，并将规划信息发送给航行器执行。
[0141]
其中，ipopt是一个基于内点算法的大型非线性优化开源软件。内点算法是一种经典的非线性问题求解方法，通过将非线性优化问题中的不等式约束体条件转化为惩罚项加入优化目标函数中，将原始非线性规划问题转化为只包含等式约束的新问题，这将使得问题求解变得更加容易。
[0142]
其中，通过在末端对接中每个规划时刻求解当前采样时刻的速度和航向来完成当前位置的局部规划，可以实现在非理想环境下的末端对接问题。由于局部规划在每一次迭代后都会采用最新获得的位姿信息进行重新规划，因此该方法对环境变化和自身位姿变化能够及时进行响应。同时，在规划的过程中会对未来一段时间内的行为进行预测，可以及时的进行减速操作保障留有足够的相对距离进行位姿修正。
[0143]
在侧流存在情况下的末端对接仿真轨迹图如附图8所示。附图8中，对接装置沿正东方向以1.5m/s的速度匀速航行，初始位置位于(20,10)。auv初始位置位于(0,0),初始朝向正东方向，初始速度为2m/s。本次仿真中，采样间隔时间δt设置为0.1秒。通过s104所述的末端对接规划方法，auv在存在沿正南方向0.3m/s的海流环境下成功进入移动对接装置。本次规划过程中规划出的速度值如附图9所示，航向如附图10所示，侧向速度如附图11所示。由附图9可知，本次对接总共花费的对接时间为24.1秒。航行过程中设置的auv最大允许速度为2.5m/s,最低航行速度设定为1m/s。从附图9中显示，auv在对接过程中大部分时候按照所允许的最大速度航行以实现快速对接。图中速度共存在两次下降，第一次为初始阶段为了调整航向并保持持续的视野跟踪进行了主动降速。第二次降速是为了实现速度收敛的优化目标。由于欠驱动航行器的耦合性质，由于侧向洋流干扰的存在，降速后不利于auv实现航向调整。auv又通过提高航行速度完成了满足了航向收敛的指标。附图10是整个对接过程中auv的航向变化，由规划结果可知，整体对接过程中航向变化较为平缓，可以较好的保持平稳的视野跟踪。在对接末端为了满足航向收敛的对接要求末端在接近对接装置时航向发生较大偏转，并基本实现和对接装置的航向对齐。成功对接时auv航向和目标对接装置之间航向差约为1度。附图11是末端对接过程中auv侧向速度的变化情况，侧向速度不直接受驱动器控制，由运动耦合引起。从图中可以看到尽在发生明显转向的阶段存在较大侧向速度，其余时刻侧向速度均较小，对auv航行的影响几乎可以忽略不计。
[0144]
在步骤s105的具体实施中，s104步骤执行结束后判断auv是否满足所述移动对接装置的进入条件，若满足所述进入条件且auv成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从auv当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明auv当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则重新回到s104步骤继续进行对接。
[0145]
其中，决策模块是auv的任务分配中心，在多任务系统中起到任务调度的作用。
[0146]
具体地，如图12所示，在步骤s105之前还可以包括：
[0147]
步骤s106：若步骤s104过程中所述移动对接装置从auv视野中丢失，则auv在视野丢失时立即按照最大减加速度减速至低于移动对接装置所允许的最低航行速度并以纯追踪方法朝向预测的移动对接装置位置航行，直至所述移动对接装置回到auv视野中，返回步骤s104。
[0148]
具体地，所述预测的移动对接装置位置的获取过程包括：
[0149]
根据视觉定位信息存在时最后一次解算得到目标位姿预测auv视野丢失期间移动
对接装置所在位置：
[0150][0151]
其中，dock
x，last
，dock
y，last
，dock
ψ，last
是在视觉定位信息存在时最后一次解算得到的移动对接装置的位置和航向信息，docku表示移动对接装置在对接期间的固定航行速度，δt
loss
是从视觉信息丢失时刻开始到当前规划时刻的时间差值。
[0152]
其中，若末端对接过程中由于突发干扰或控制误差导致目标从视野中丢失时上述步骤s104中的局部优化问题中的视野跟踪约束难以满足。而非线性优化问题在无法获得可行解时的求解时间可能会超出实时的限制且此时不能保证获得局部最优解的正确性。为了解决这个问题，并且在视野丢失情况下保证对接安全性，auv在视野丢失时立即按照最大减加速度减速至低于移动对接装置的所允许的最低航行速度。也就是说，auv通过主动降速增加和移动对接装置的相对距离以实现扩大视野范围的目的。此外，由于降速后auv的航行速度低于目标对接装置，在进行视野恢复调整的过程中，不会出现auv和目标对接装置的碰撞问题，保障了对接的安全性。根据历史数据推测移动目标对接装置位置变化，将auv的当前航向朝向预测后的目标位置以及时恢复视野探测，预测方式同上述s106中公式所示，航向确定如s103中航向计算公式所示，将航路点替换为预测后目标位置即可。一旦视觉信息恢复，回到步骤s104重新进行末端规划。在出现视野丢失问题时，通过使auv以纯追踪方式朝向预测的移动对接装置位置航行并辅以主动降速的操作以扩大视野范围，解决了视觉定位数据丢失的恢复问题。该技术手段充分利用移动对接中相对速度可调节的特点，使得auv在对接过程中具有视野恢复和近距离误差修正的能力，能够在存在环境干扰的非理想对接水域中提升对接成功率。
[0153]
图13是根据一示例性实施例示出的一种欠驱动auv移动对接动态路径规划装置的框图。参照图13，该装置应用于欠驱动auv，可以包括：
[0154]
绕圈航行模块21，用于auv自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
[0155]
建立连接模块22，用于auv和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
[0156]
追踪接近模块23，用于auv根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；
[0157]
局部规划模块24，用于当auv上的摄像头捕获到所述移动对接装置时，auv根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；
[0158]
发送信号模块25，用于局部规划模块24执行结束后判断auv是否满足所述移动对接装置的进入条件，若满足所述进入条件且auv成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从auv当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明auv当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则继续进行对接。
[0159]
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
[0160]
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本技术方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0161]
相应的，本技术还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的一种欠驱动auv移动对接动态路径规划方法。
[0162]
相应的，本技术还提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如上述的一种欠驱动auv移动对接动态路径规划方法。
[0163]
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本技术的其它实施方案。本技术旨在涵盖本技术的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本技术的一般性原理并包括本技术未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本技术的真正范围和精神由下面的权利要求指出。
[0164]
应当理解的是，本技术并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本技术的范围仅由所附的权利要求来限制。