一种基于回转圆的移动式对接轨迹规划方法技术

本发明专利技术公开了一种基于回转圆的移动式对接轨迹规划方法，本发明专利技术利用欠驱动AUV与对接装置的相对位置关系，基于回转圆半径与周期，估计对接所需时间并提前进行轨迹规划，本发明专利技术结合纯跟踪导引率和视线法导引率，完成回转运动使欠驱动AUV位于对接装置之后且运动同方向，使欠驱动AUV能快速地完成移动式对接回收任务。本发明专利技术计算简单，易于实现，且不依赖于精确的AUV模型，在不确定性外界环境干扰下具有较强的自适应性。

【技术实现步骤摘要】
一种基于回转圆的移动式对接轨迹规划方法
本专利技术涉及一种基于回转圆的移动式对接轨迹规划方法，属于欠驱动水下机器人轨迹规划领域，水下潜器、水下潜艇、无人水面艇和水面船舶等水中航行装备都在本专利技术的应用范围内。
技术介绍
AUV作为一种应用范围广泛、实用性高、前景广阔的海洋无人航行设备，在海洋开发、海洋科学、水下搜救、军事活动甚至是大众娱乐等方面都扮演着越来越重要的角色。AUV原则上可以在没有“母舰”的情况下进行远距离探测，其执行任务的时间受其自身条件的限制，包括能源存储容量、推进系统效率、艇体的流体动力特性、定位和导航精度等。而其中能源存储容量即电池容量是决定任务持续时间的关键因素，单纯增加电池数量或使用新类型电池会使成本急剧增加。除了能源消耗外，对于长航时任务，AUV仍需要定期上传在搜查作业中收集到的大量数据，并下载更新任务。在没有与母船连接的情况下，数据通信只能在水下使用低带宽声波通信，或者浮出水面使用高带宽电磁波通信。在涉及大量数据的情况下，这两种选择都不理想。另一种选择是AUV回到它的母舰上，通过有线连接恢复数据。考虑到能量存储和数据通信的限制，水下机器人需要由“母舰”定期回收和充电，这一过程需要人工操作且对它们的效率造成了很大的限制。因为AUV水下对接技术在AUV水下能量补充、数据交换、水下回收等方面具有重要的研究意义和实用价值，所以AUV水下对接技术为解决上述问题提供了可能，大量的水下对接理论研究和实验正在进行。按照对接装置的运动形式可分为固定式和移动式对接两类。固定式对接装置的位置不能改变，但为了减小海流对回收对接任务的影响，可能会为适应海流而在水平面内进行旋转，再协同靠近的AUV完成对接。其中，固定漏斗型对接站是最常用的对接站，它的位置不随海流移动，AUV可以更准确地测量其与固定对接站之间的距离；移动式对接装置一般是搭载在潜艇或牵连在水面船等处于运动状态的大型航行器上，会随母艇一起运动，相对于固定式对接来说移动式对接装置的位置需要由AUV实时测量得到并估计其运动信息，所以移动式对接对AUV的规划避障和跟踪控制能力提出了更高的要求。综上，现有的自主水下对接技术主要是以固定式对接为主，存在灵活性低、成本过高的缺陷。《High-LevelFuzzyLogicGuidanceSystemforanUnmannedSurfaceVehicle(USV)taskedtoperformAutonomousLaunchandRecovery(ALR)ofanAutonomousUnderwaterVehicle(AUV)》采用模糊逻辑控制器来控制AUV的艏向，对静态对接回收任务效果较好，但对于AUV与移动对接站相向运动的移动式对接任务存在控制效果滞后的情况；《TimeandEnergyEfficientTrajectoryGeneratorforAutonomousUnderwaterVehicleDockingOperations》利用虚拟域逆动力学方法将对接任务的轨迹规划问题看成是非线性规划问题，需要依赖非线性规划求解器，求解时间较长，与求解器的选择、非线性约束条件的多少和初始值的选取都有关系，无法做到快速求解。为了实现低成本、高灵活性的移动式水下对接，一种针对欠驱动水下机器人的移动式对接回收的轨迹规划方法亟待提出。
技术实现思路
针对上述现有技术，本专利技术要解决的技术问题是提供一种基于回转圆的移动式对接轨迹规划方法，利用回转圆的信息，结合纯跟踪导引率和视线法导引率，有效的解决了欠驱动AUV与移动式对接装置相向运动靠近时的轨迹规划问题。为解决上述技术问题，本专利技术的一种基于回转圆的移动式对接轨迹规划方法，包括以下步骤：步骤1：设AUV巡航速度为uu，对接装置随载体以速度Ud向前作匀速直线运动，在水平面上将对接装置周围分为3个区域：对接装置正后方长为d2，宽为d3的长方形为区域1；以对接装置为中心，边长为d1的方形以外为区域3；剩余部分为区域2；设置距离d1，d2和d3及控制参数k1，k2，k3，k4和k5，b为基于回转圆的轨迹规划成功标记，若为0，代表未成功；若不为0，则代表成功且可采用规划好的轨迹制导，其初始值设为0；步骤2：建立以对接装置为原点O，以其前进方向为X轴，左侧为Y轴，上侧为Z轴的坐标系OXYZ，并得到在此坐标系三个坐标轴上AUV和对接装置之间的相对位置差xe，ye和ze，期望俯仰角始终采用视线法导引率进行计算θd＝arctan(-ze/k1)，判断AUV所在区域：如果在区域3，则采用纯跟踪导引率并转到步骤8：ud＝uu其中，ψd为期望艏向角，ud为期望速度；如果AUV在区域1中，则采用视线法导引率并转到步骤8：ψd＝arctan(-ye/k2)如果AUV在区域2中，则先判断纵向位置误差xe是否小于等于0，如果是，采用纯跟踪导引率并转到步骤8；否则继续判断标记b是否为0，如果b等于0，则转到步骤3进行基于回转圆的轨迹规划；如果b不等于0，则转到步骤5根据规划好的轨迹和视线法进行控制；步骤3：当欠驱动AUV与对接装置运动方向相反时，提前进行轨迹规划并完成回转运动使欠驱动AUV位于对接装置之后且运动同方向，选择AUV以速度uu和设定的舵角做回转运动得到的回转圆半径为R，周期为T，角速度为ω＝2*π/T，假设超前时间为t1，即AUV由当前位置运动t1秒可到达轨迹规划的终点位置，对接装置运动t1秒后的位置为A，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在位置A的正后方d2米处B，此时AUV所在位置E与对接装置所在位置O的连线在水平面上与X轴的夹角为a1，计算对接装置当前所在位置和规划的终点位置之间的距离为d4＝Ud*t1-d2，计算此时对接装置与AUV在水平面上的距离为dis0，按三种情况进行规划轨迹：第一种情况：如果满足条件(π-a1)/ω≥(dis0*cos(a1)+R*sin(a1)+d2)/Ud，规划的轨迹为一段回转圆圆弧，作半径为R的回转圆O1与连接AUV所在位置E和对接装置所在位置O的直线OE相切于E点，且圆心O1和规划终点位置B所连直线O1B与X轴垂直，圆弧的起点为E，终点为回转圆O1与直线O1B的交点C，这种情况下AUV由当前位置立刻进行回转运动，因为AUV沿这段回转圆圆弧由E运动到C所花的时间t2大于对接装置由O运动到A的时间t1，此时应以改变AUV运动方向为主，若AUV沿规划的轨迹运动到X轴上时满足条件xe＜-d2，即此时AUV与对接装置的距离大于d2，不会发生碰撞，认为轨迹规划成功，为b赋值不为0的数，并把当前的坐标系XOY记作X'O'Y'，记录AUV与对接装置所在直线OE在水平面上与X轴的夹角a＝a1和AUV此时位置与圆弧起点间的距离dis2＝0，记录此时时间t4＝t和AUV沿圆弧运动的时间t3＝t2，因当AUV的位置关于X轴对称时规划的轨迹也关于X轴对称的特点只需计算半个平面，设sig为AUV此时在Y轴正负半轴的标志，若此时ye≥0，sig＝-1；若ye＜0时，sig＝1，转到步骤5，本文档来自技高网...

【技术保护点】
1.一种基于回转圆的移动式对接轨迹规划方法，其特征在于，包括以下步骤：/n步骤1：设AUV巡航速度为uu，对接装置随载体以速度U

【技术特征摘要】
1.一种基于回转圆的移动式对接轨迹规划方法，其特征在于，包括以下步骤：
步骤1：设AUV巡航速度为uu，对接装置随载体以速度Ud向前作匀速直线运动，在水平面上将对接装置周围分为3个区域：对接装置正后方长为d2，宽为d3的长方形为区域1；以对接装置为中心，边长为d1的方形以外为区域3；剩余部分为区域2；设置距离d1，d2和d3及控制参数k1，k2，k3，k4和k5，b为基于回转圆的轨迹规划成功标记，若为0，代表未成功；若不为0，则代表成功且可采用规划好的轨迹制导，其初始值设为0；
步骤2：建立以对接装置为原点O，以其前进方向为X轴，左侧为Y轴，上侧为Z轴的坐标系OXYZ，并得到在此坐标系三个坐标轴上AUV和对接装置之间的相对位置差xe，ye和ze，期望俯仰角始终采用视线法导引率进行计算θd＝arctan(-ze/k1)，判断AUV所在区域：
如果在区域3，则采用纯跟踪导引率并转到步骤8：



ud＝uu
其中，ψd为期望艏向角，ud为期望速度；
如果AUV在区域1中，则采用视线法导引率并转到步骤8：
ψd＝arctan(-ye/k2)



如果AUV在区域2中，则先判断纵向位置误差xe是否小于等于0，如果是，采用纯跟踪导引率并转到步骤8；否则继续判断标记b是否为0，如果b等于0，则转到步骤3进行基于回转圆的轨迹规划；如果b不等于0，则转到步骤5根据规划好的轨迹和视线法进行控制；
步骤3：当欠驱动AUV与对接装置运动方向相反时，提前进行轨迹规划并完成回转运动使欠驱动AUV位于对接装置之后且运动同方向，选择AUV以速度uu和设定的舵角做回转运动得到的回转圆半径为R，周期为T，角速度为ω＝2*π/T，假设超前时间为t1，即AUV由当前位置运动t1秒可到达轨迹规划的终点位置，对接装置运动t1秒后的位置为A，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在位置A的正后方d2米处B，此时AUV所在位置E与对接装置所在位置O的连线在水平面上与X轴的夹角为a1，计算对接装置当前所在位置和规划的终点位置之间的距离为d4＝Ud*t1-d2，计算此时对接装置与AUV在水平面上的距离为dis0，按三种情况进行规划轨迹：
第一种情况：如果满足条件(π-a1)/ω≥(dis0*cos(a1)+R*sin(a1)+d2)/Ud，规划的轨迹为一段回转圆圆弧，作半径为R的回转圆O1与连接AUV所在位置E和对接装置所在位置O的直线OE相切于E点，且圆心O1和规划终点位置B所连直线O1B与X轴垂直，圆弧的起点为E，终点为回转圆O1与直线O1B的交点C，这种情况下AUV由当前位置立刻进行回转运动，因为AUV沿这段回转圆圆弧由E运动到C所花的时间t2大于对接装置由O运动到A的时间t1，此时应以改变AUV运动方向为主，若AUV沿规划的轨迹运动到X轴上时满足条件xe＜-d2，即此时AUV与对接装置的距离大于d2，不会发生碰撞，认为轨迹规划成功，为b赋值不为0的数，并把当前的坐标系XOY记作X'O'Y'，记录AUV与对接装置所在直线OE在水平面上与X轴的夹角a＝a1和AUV此时位置与圆弧起点间的距离dis2＝0，记录此时时间t4＝t和AUV沿圆弧运动的时间t3＝t2，因当AUV的位置关于X轴对称时规划的轨迹也关于X轴对称的特点只需计算半个平面，设sig为AUV此时在Y轴正负半轴的标志，若此时ye≥0，sig＝-1；若ye＜0时，sig＝1，转到步骤5，否则认为轨迹规划失败，转到步骤7。
如果不满足条件(π-a1)/ω≥(dis0*cos(a1)+R*sin(a1)+d2)/Ud则转到第二种情况；
第二种情况：如果满足条件a1＜arctan(R/d4)*2，规划的轨迹为两段回转圆圆弧的拼接；AUV所在位置为E，对接装置所在位置为O，对接装置运动t1秒后的位置为A，基于回转圆的轨迹规划的终点位置选在位置A的正后方d2米处B，作回转圆O2与X轴相切于点...

【专利技术属性】
技术研发人员：谢天奇，李晔，姜言清，庞硕，武皓微，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23