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

本发明专利技术公开了一种欠驱动AUV移动对接动态路径规划方法及装置，涉及水下自主航行器回收技术领域。该方法针对侧向驱动能力缺乏或不足的欠驱动水下自主航行器与单向进入的移动对接装置之间的回收对接问题，提出了一种实时、鲁棒的路径规划方法。远端归航阶段采用纯追踪方法跟踪航路点，具有计算简单、易于实现、适合单向对接的优点。末端对接阶段基于模型预测控制的方法，进行约束条件下的局部规划，求解约束条件下的优化问题，得到规划的速度和航向。所提出的方法可以避免由于不合理规划造成的环境感知信息无法获取或是控制严重超调的问题，能够有效地提升水下自主航行器在非理想环境中回收对接的鲁棒性和安全性。境中回收对接的鲁棒性和安全性。境中回收对接的鲁棒性和安全性。

A dynamic path planning method and device for mobile docking of underactuated AUV

【技术实现步骤摘要】
一种欠驱动AUV移动对接动态路径规划方法及装置


[0001]本申请涉及水下自主航行器的回收
，尤其涉及一种欠驱动AUV移动对接动态路径规划方法及装置。

技术介绍

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

技术实现思路

[0005]本申请实施例的目的是提供一种欠驱动AUV移动对接动态路径规划方法及装置、电子设备，以解决移动回收对接问题中实时规划方法不考虑AUV实际执行能力，不考虑对接过程中环境感知需求以及没有充分利用对移动对接优势的技术问题，有效提升了移动回收
对接规划方法的鲁棒性。
[0006]根据本申请实施例的第一方面，提供一种欠驱动AUV移动对接动态路径规划方法，包括：
[0007]S101：AUV自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；
[0008]S102：AUV和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；
[0009]S103：AUV根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；
[0010]S104：当AUV上的摄像头捕获到所述移动对接装置时，AUV根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；
[0011]S105：S104步骤执行结束后判断AUV是否满足所述移动对接装置的进入条件，若满足所述进入条件且AUV成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从AUV当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明AUV当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则重新回到S104步骤继续进行对接。
[0012]进一步地，在步骤S105之前还包括：
[0013]S106：若步骤S104过程中所述移动对接装置从AUV视野中丢失，则AUV在视野丢失时立即按照最大减加速度减速至低于移动对接装置所允许的最低航行速度并以纯追踪方法朝向预测的移动对接装置位置航行，直至所述移动对接装置回到AUV视野中，返回步骤S104。
[0014]进一步地，所述预测的移动对接装置位置的获取过程包括：
[0015]根据视觉定位信息存在时最后一次解算得到目标位姿预测AUV视野丢失期间移动对接装置所在位置：
[0016][0017]其中，Dock
x，Last
，Dock
y，Last
，Dock
ψ，Last
是在视觉定位信息存在时最后一次解算得到的移动对接装置的位置和航向信息，Dock
u
表示移动对接装置在对接期间的固定航行速度，Δ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的移动对接装置的位置和航向信息，Dock
u
表示移动对接装置在对接期间的固定航行速度，Δt表示采样间隔时间。
[0030]进一步地，所述目标函数为：
[0031本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种欠驱动AUV移动对接动态路径规划方法，其特征在于，包括：S101：AUV自主航行到对接水域，进入所述对接水域后从当前位置开始绕圈航行，通过声学定位数据获取移动对接装置的位置；S102：AUV和所述移动对接装置通过声学信号建立连接，以使得搭载有所述移动对接装置的移动载体在所述对接水域内按预定对接深度开启定速定向航行；S103：AUV根据融合导航数据信息，以纯追踪导引的方式追逐航路点以接近所述移动对接装置，其中所述融合导航数据信息由惯性导航系统和声学传感器提供的信息融合得到；S104：当AUV上的摄像头捕获到所述移动对接装置时，AUV根据视觉定位得到的定位数据，基于模型预测控制的框架进行约束条件下的局部规划，求解约束条件下的优化问题，得到下一时刻的航行速度和方向；S105：S104步骤执行结束后判断AUV是否满足所述移动对接装置的进入条件，若满足所述进入条件且AUV成功进入所述移动对接装置，则向决策模块发送成功对接信号；否则检查从AUV当前位置指向目标装置位置的向量方向，若相邻采样间隔间该向量方向发生反转，则说明AUV当前位置已超过移动对接装置，判定为对接失败，发送对接失败信号；否则重新回到S104步骤继续进行对接。2.根据权利要求1所述的方法，其特征在于，在步骤S105之前还包括：S106：若步骤S104过程中所述移动对接装置从AUV视野中丢失，则AUV在视野丢失时立即按照最大减加速度减速至低于移动对接装置所允许的最低航行速度并以纯追踪方法朝向预测的移动对接装置位置航行，直至所述移动对接装置回到AUV视野中，返回步骤S104。3.根据权利要求2所述的方法，其特征在于，所述预测的移动对接装置位置的获取过程包括：根据视觉定位信息存在时最后一次解算得到目标位姿预测AUV视野丢失期间移动对接装置所在位置：其中，Dock
x，Last
，Dock
y，Last
，Dock
ψ，Last
是在视觉定位信息存在时最后一次解算得到的移动对接装置的位置和航向信息，Dock
u
表示移动对接装置在对接期间的固定航行速度，Δt
loss
是从视觉信息丢失时刻开始到当前规划时刻的时间差值。4.根据权利要求1所述的方法，其特征在于，步骤S104包括：建立洋流影响环境下AUV的运动学模型和动力学模型；将所述运动学模型和动力学模型离散化，根据离散化的模型预测未来时刻AUV的状态；根据所述摄像头采集的图像进行所述移动对接装置的位姿解算；根据对图像进行位姿解算后获得的移动对接装置当前位姿信息，结合所述移动对接装置定速定向航行的前提条件，预测所述移动对接装置在未来时刻的位置；根据由离散化模型预测得到的未来时刻的状态和预测得到的移动对接装置未来时刻的位姿建立AUV与所述移动对接装置在位置收敛、航向收敛和速度收敛条件下的目标函数；建立AUV运动能力约束；建立视野持续跟踪约束并引入控制冗余度考虑；
对AUV运动能力约束、持续视野跟踪约束下的目标函数最小化优化问题进行求解，得到下一时刻的航行速度和方向。5.根据权利要求4所述的方法，其特征在于，根据对图像...

【专利技术属性】
技术研发人员：徐元欣，李曾妮，曾庆锋，刘诚，陈首旭，张彩宝，李心慧，陶子寅，王鹏，
申请(专利权)人：浙江大学，
类型：发明
国别省市：