本发明涉及一种基于回转圆的移动式对接轨迹规划方法，属于欠驱动水下机器人轨迹规划领域，水下潜器、水下潜艇、无人水面艇和水面船舶等水中航行装备都在本发明的应用范围内。
背景技术：
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，否则认为轨迹规划失败，转到步骤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轴相切于点b，再作回转圆o1与回转圆o2相切于点c，回转圆o1和oe所在直线相切于点d，回转圆o1与回转圆o2的回转半径均为r，回转周期为t，计算auv沿第一段圆弧由d运动到c点的时间为t2，计算auv沿两段圆弧由d运动到b点的时间为t3，计算第一段圆弧起点与对接装置的距离为dis1，转到步骤4。
如果不满足条件a1＜arctan(r/d4)*2则转到第三种情况；
第三种情况：如果满足条件a1≥arctan(r/d4)*2，规划的轨迹为一段回转圆圆弧与一条直线的拼接；auv所在位置为e，对接装置所在位置为o，对接装置在t1秒后的位置为a，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在对接装置运动t1秒后的正后方d2米处b，作回转圆o1与auv和对接装置所在直线oe相切于点d，且圆心与规划的终点位置b的连线o1b垂直于x轴，作直线bc与回转圆o1相切于c点，回转圆o1的回转半径为r，回转周期为t，计算auv沿圆弧由d运动到c点的时间为t2，计算auv沿圆弧和直线由d运动到b点的时间为t3，计算圆弧起点与对接装置的距离为dis1，转到步骤4；
如果不满足条件a1＜arctan(r/d4)*2则转到步骤4；
步骤4：对超前时间t1进行调整：
如果dis0≤dis1&&t3≥t1，则计算auv当前位置e与第一段圆弧起点d的距离为dis2＝dis1-dis0，将两段拼接曲线沿auv与对接装置的连线由d平移至auv当前位置e，平移后的轨迹即为规划的轨迹，若auv沿规划的轨迹运动到x轴上时xe≤-d2，即此时auv与对接装置的距离大于d2，不会发生碰撞，认为轨迹规划成功，为b赋值不为0的数，并把当前的坐标系xoy记作x'o'y'，记录auv与对接装置所在直线此时在水平面上与x轴的夹角a＝a1，记录此时时间t4＝t和auv沿规划轨迹运动到第二段圆弧或直线的理论时间为t5＝t4 t2，结合当auv的位置关于x轴对称时规划的轨迹也关于x轴对称的特点只需计算半个平面，设sig为auv此时在y轴正负半轴的标志，若此时ye≥0，sig＝-1；若ye＜0时，sig＝1，转到步骤5，否则认为轨迹规划失败，此时对t1重新赋值t1＝t1-dis2*cos(a1)/ud，转到步骤7；
如果dis0≤dis1&&t3＜t1或者dis0＞dis1&&t3≥t1，则认为轨迹规划失败，此时对t1重新赋值t1＝t3，转到步骤7；
如果dis0＞dis1&&t3＜t1，则认为轨迹规划失败，此时t1不变，转到步骤7；
步骤5：求出回转圆o1和o2在x'o'y'坐标系下的圆心坐标为
期望直线段轨迹坐标方程
其中，当auv沿回转圆逆时针转动时，角速度ω'＝ω；当auv沿回转圆顺时针转动时，角速度ω'＝-ω；
对期望圆轨迹坐标方程求导可得到目标点处的艏向角
将t5带入上面两个方程可求得直线段轨迹上的艏向角。
建立期望轨迹坐标系并进行坐标转换
再结合视线法导引率得到期望速度和期望艏向角
ud＝uu-s/k4
ψd＝ψ′d-arctan(e/k5)
步骤6：若auv在规划轨迹上运动的时间大于规划总时间t3，即t-t4＞t3或当auv运动在直线段时满足条件xe＜-d2，则令b＝0，并转到步骤8；
步骤7：轨迹规划失败则采用(2)中的纯跟踪导引率并转到步骤8；
步骤8：控制器根据期望速度ud，期望艏向角ψd和期望俯仰角θd对auv进行控制；
步骤9：判断对接任务是否完成，若未完成，转到步骤2进行下一个时间步的规划，若完成，输出对接成功，auv断电。
本发明的有益效果：
欠驱动自主水下机器人(autonomousunderwatervehicle,auv)无法原地回转，且动态对接装置只能实现单向对接，所以当欠驱动auv与对接装置运动方向相反时，需要提前进行轨迹规划并完成回转运动使欠驱动auv位于对接装置之后且运动同方向，本发明提出的基于回转圆的移动式对接轨迹规划方法，利用auv与对接装置的相对位置关系，结合回转圆半径与周期，估计对接所需时间并计算期望轨迹用于auv控制，使其完成移动式对接回收任务。本发明计算简单，易于实现，且不依赖于精确的auv模型，在不确定性外界环境干扰下具有较强的自适应性。
本发明利用欠驱动auv与对接装置的相对位置关系，基于回转圆半径与周期，估计对接所需时间并提前进行轨迹规划，本发明结合纯跟踪导引率和视线法导引率，完成回转运动使欠驱动auv位于对接装置之后且运动同方向，使欠驱动auv能快速地完成移动式对接回收任务。
本发明计算简单，在实际应用中易于实现，参数数量较少，各参数物理意义清晰，在实际控制系统中参数易于调节。
附图说明
图1是auv完成移动式对接流程示意图；
图2是水平面上对接装置周围的分区示意图；
图3是基于回转圆的移动式对接轨迹规划方法流程结构图；
图4是本轨迹规划方法的第一种情况示意图；
图5是本轨迹规划方法的第二种情况示意图；
图6是本轨迹规划方法的第三种情况示意图。
具体实施方式
下面结合附图对本发明具体实施方式做进一步说明。
结合图1说明欠驱动auv完成移动式对接任务的流程。移动式对接任务较为复杂，通常令对接装置随载体做相对简单的匀速直线运动，速度为ud，auv的巡航速度为uu，携带超短基线声学定位系统(ultra-shortbaselineacousticpositionsystem，usbl)和摄像头，通过声学定位和光视觉定位可得到auv和对接装置之间的相对位置关系，若相对位置关系满足对接成功条件，auv被固定在对接装置中随载体一起运动，那么此时auv可断电停止；若未完成对接任务，则可结合船位推算单元得到对接装置的运动方向，建立以对接装置为原点o，以其前进方向为x轴，左侧为y轴，上侧为z轴的坐标系oxyz，并得到在此坐标系三个坐标轴上auv和对接装置之间的相对位置差xe，ye和ze；然后进行轨迹规划得到期望速度ud，期望艏向角ψd和期望俯仰角θd；最后通过控制器和操纵执行机构完成对auv的自动控制。本发明主要用于欠驱动auv进行移动式对接任务，由于欠驱动auv无法原地回转，所以当欠驱动auv与对接装置运动方向相反时，需要提前进行轨迹规划并完成回转运动使欠驱动auv位于对接装置之后且运动同方向。
结合图2说明水平面上对接装置周围的不同分区。对接装置随载体以速度ud向前作匀速直线运动，对接装置后方长为d2，宽为d3的长方形为区域1，以对接装置为中心，边长为d1的方形外为区域3，剩余部分为区域2。
结合图3-6说明移动式回收制导流程，制导方法采用基于回转圆的移动式对接轨迹规划方法，纯跟踪导引率和视线法导引率切换的形式。b为基于回转圆的轨迹规划成功标记，若为0，代表失败；若不为0，则代表成功且可采用规划好的轨迹制导，其初始值设为0。
(1)设auv巡航速度为uu，对接装置随载体以速度ud向前作匀速直线运动，结合图2在水平面上将对接装置周围分为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，则采用纯跟踪导引率并转到(13)：
ud＝uu
(3)如果auv在区域1中，则采用视线法导引率并转到(13)：
ψd＝arctan(-ye/k2)
(4)如果在区域2中，则先判断纵向位置误差xe是否小于等于0，如果是，采用纯跟踪导引率并转到(13)；否则继续判断标记b是否为0，如果等于0，则转到(5)进行基于回转圆的轨迹规划；如果不等于0，则转到(10)根据规划好的轨迹和视线法进行控制；
(5)由于欠驱动auv无法原地回转，所以当欠驱动auv与对接装置运动方向相反时，需要提前进行轨迹规划并完成回转运动使欠驱动auv位于对接装置之后且运动同方向，选择auv以速度uu和某一舵角(与海流大小有关，海流越大，舵角选择越小)做回转运动得到的回转圆半径为r，周期为t，角速度为ω＝2*π/t，假设超前时间为t1(其初始值为一个大于t的数)，即auv由当前位置经过t1秒可到达轨迹规划的终点位置，对接装置运动t1秒后的位置为a，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在位置a的正后方d2米处的点b，此时auv所在位置e与对接装置所在位置o的连线在水平面上与x轴的夹角为a1，计算对接装置当前所在位置和规划的终点位置之间的距离为d4＝ud*t1-d2，计算对接装置与auv在水平面上的距离为dis0，按照图4-6的三种情况进行规划轨迹；
(6)第一种情况如果满足条件(π-a1)/ω≥(dis0*cos(a1) r*sin(a1) d2)/ud，规划的轨迹如图4所示为一段回转圆圆弧，作半径为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，转到(10)，否则认为轨迹规划失败，转到(12)。
如果不满足条件(π-a1)/ω≥(dis0*cos(a1) r*sin(a1) d2)/ud则转到(7)。
(7)第二种情况如果满足条件a1＜arctan(r/d4)*2，规划的轨迹如图5所示为两段回转圆圆弧的拼接；auv所在位置为e，对接装置所在位置为o，对接装置运动t1秒后的位置为a，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在对接装置运动t1秒后的正后方d2米处b，作回转圆o2与x轴相切于点b，再作回转圆o1与回转圆o2相切于点c，回转圆o1和oe所在直线相切于点d，回转圆o1与回转圆o2的回转半径均为r，回转周期为t，计算auv沿第一段圆弧由d运动到c点的时间为t2，计算auv沿两段圆弧由d运动到b点的时间为t3，计算第一段圆弧起点与对接装置的距离为dis1，转到(9)。
如果不满足条件a1＜arctan(r/d4)*2则转到(8)。
(8)第三种情况如果满足条件a1≥arctan(r/d4)*2，规划的轨迹如图6所示为一段回转圆圆弧与一条直线的拼接；auv所在位置为e，对接装置所在位置为o，对接装置在t1秒后的位置为a，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在对接装置运动t1秒后的正后方d2米处b，作回转圆o1与auv和对接装置所在直线oe相切于点d，且圆心与期望位置b的连线o1b垂直于x轴，作直线bc与回转圆o1相切于c点，回转圆o1的回转半径为r，回转周期为t，计算auv沿圆弧由d运动到c点的时间为t2，计算auv沿圆弧和直线由d运动到b点的时间为t3，计算圆弧起点与对接装置的距离为dis1，转到(9)。
如果不满足条件a1＜arctan(r/d4)*2则转到(9)。
(9)对超前时间t1进行调整；
如果dis0≤dis1&&t3≥t1，则计算auv当前位置e与第一段圆弧起点d的距离为dis2＝dis1-dis0，将图中实线沿auv与对接装置的连线由d平移至auv当前位置e，平移后的点虚线即为规划的轨迹，若auv沿规划的轨迹运动到x轴上时xe≤-d2，即此时auv与对接装置的距离大于d2，不会发生碰撞，认为轨迹规划成功，为b赋值不为0的数，并把当前的坐标系xoy记作x'o'y'，记录auv与对接装置所在直线此时在水平面上与x轴的夹角a＝a1，记录此时时间t4＝t和auv沿规划轨迹运动到第二段圆弧或直线的理论时间为t5＝t4 t2，结合auv的位置关于x轴对称时规划的轨迹也关于x轴对称的特点只需计算半个平面，设sig为auv此时在y轴正负半轴的标志，若此时ye≥0，sig＝-1；若ye＜0时，sig＝1，转到(10)，否则认为轨迹规划失败，此时对t1重新赋值t1＝t1-dis2*cos(a1)/ud，转到(12)。
如果dis0≤dis1&&t3＜t1或者dis0＞dis1&&t3≥t1，则认为轨迹规划失败，此时对t1重新赋值t1＝t3，转到(12)。
如果dis0＞dis1&&t3＜t1，则认为轨迹规划失败，此时t1不变，转到(12)。
(10)求出回转圆o1和o2在x'o'y'坐标系下的圆心坐标为
期望直线轨迹坐标方程
其中，当auv沿回转圆逆时针转动时，角速度ω'＝ω；当auv沿回转圆顺时针转动时，角速度ω'＝-ω。
对期望圆轨迹坐标方程求导可得到目标点处的艏向角
将t5带入上面两个方程可求得图5中的直线轨迹上的艏向角。
建立期望轨迹坐标系并进行坐标转换
再结合视线法导引率得到期望速度和期望艏向角
ud＝uu-s/k4
ψd＝ψ′d-arctan(e/k5)
(11)若auv在规划轨迹上运动的时间大于规划总时间t3，即t-t4＞t3或当auv运动在图5中的直线段时满足条件xe＜-d2，则令b＝0，并转到(13)；
(12)轨迹规划失败则采用(2)中的纯跟踪导引率；
(13)控制器根据期望速度ud，期望艏向角ψd和期望俯仰角θd对auv进行控制；
(14)判断对接任务是否完成，若未完成，转到(2)进行下一个时间步的规划，若完成，输出对接成功，auv断电。
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轴相切于点b，再作回转圆o1与回转圆o2相切于点c，回转圆o1和oe所在直线相切于点d，回转圆o1与回转圆o2的回转半径均为r，回转周期为t，计算auv沿第一段圆弧由d运动到c点的时间为t2，计算auv沿两段圆弧由d运动到b点的时间为t3，计算第一段圆弧起点与对接装置的距离为dis1，转到步骤4。
如果不满足条件a1＜arctan(r/d4)*2则转到第三种情况；
第三种情况：如果满足条件a1≥arctan(r/d4)*2，规划的轨迹为一段回转圆圆弧与一条直线的拼接；auv所在位置为e，对接装置所在位置为o，对接装置在t1秒后的位置为a，为了保证对接安全，基于回转圆的轨迹规划的终点位置选在对接装置运动t1秒后的正后方d2米处b，作回转圆o1与auv和对接装置所在直线oe相切于点d，且圆心与规划的终点位置b的连线o1b垂直于x轴，作直线bc与回转圆o1相切于c点，回转圆o1的回转半径为r，回转周期为t，计算auv沿圆弧由d运动到c点的时间为t2，计算auv沿圆弧和直线由d运动到b点的时间为t3，计算圆弧起点与对接装置的距离为dis1，转到步骤4；
如果不满足条件a1＜arctan(r/d4)*2则转到步骤4；
步骤4：对超前时间t1进行调整：
如果dis0≤dis1&&t3≥t1，则计算auv当前位置e与第一段圆弧起点d的距离为dis2＝dis1-dis0，将两段拼接曲线沿auv与对接装置的连线由d平移至auv当前位置e，平移后的轨迹即为规划的轨迹，若auv沿规划的轨迹运动到x轴上时xe≤-d2，即此时auv与对接装置的距离大于d2，不会发生碰撞，认为轨迹规划成功，为b赋值不为0的数，并把当前的坐标系xoy记作x'o'y'，记录auv与对接装置所在直线此时在水平面上与x轴的夹角a＝a1，记录此时时间t4＝t和auv沿规划轨迹运动到第二段圆弧或直线的理论时间为t5＝t4 t2，结合当auv的位置关于x轴对称时规划的轨迹也关于x轴对称的特点只需计算半个平面，设sig为auv此时在y轴正负半轴的标志，若此时ye≥0，sig＝-1；若ye＜0时，sig＝1，转到步骤5，否则认为轨迹规划失败，此时对t1重新赋值t1＝t1-dis2*cos(a1)/ud，转到步骤7；
如果dis0≤dis1&&t3＜t1或者dis0＞dis1&&t3≥t1，则认为轨迹规划失败，此时对t1重新赋值t1＝t3，转到步骤7；
如果dis0＞dis1&&t3＜t1，则认为轨迹规划失败，此时t1不变，转到步骤7；
步骤5：求出回转圆o1和o2在x'o'y'坐标系下的圆心坐标为
期望直线段轨迹坐标方程
其中，当auv沿回转圆逆时针转动时，角速度ω'＝ω；当auv沿回转圆顺时针转动时，角速度ω'＝-ω；
对期望圆轨迹坐标方程求导可得到目标点处的艏向角
将t5带入上面两个方程可求得直线段轨迹上的艏向角。
建立期望轨迹坐标系并进行坐标转换
再结合视线法导引率得到期望速度和期望艏向角
ud＝uu-s/k4
ψd＝ψ′d-arctan(e/k5)
步骤6：若auv在规划轨迹上运动的时间大于规划总时间t3，即t-t4＞t3或当auv运动在直线段时满足条件xe＜-d2，则令b＝0，并转到步骤8；
步骤7：轨迹规划失败则采用(2)中的纯跟踪导引率并转到步骤8；
步骤8：控制器根据期望速度ud，期望艏向角ψd和期望俯仰角θd对auv进行控制；
步骤9：判断对接任务是否完成，若未完成，转到步骤2进行下一个时间步的规划，若完成，输出对接成功，auv断电。
技术总结