[发明专利]一种水面无人船导引缆回收自主水下航行器的规划方法

权利要求书

1.一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述回收的逻辑流程如下：

首先，当AUV结束任务后开始回收过程，AUV、USV所搭载的各传感器同时工作，检测当前状态数据并实时传输至控制系统；

其次，通过对状态数据的滤波处理以及各定位系统的信息进行校核，判断数据是否存在异常，若发现异常则进行数据异常处理，重新获取数据，否则进行状态危险度判断；

再次，通过在控制系统内部嵌入AUV动力学模型，并将融合处理后的数据与AUV动力学模型航位推算得到的信息进行比较，从而判断AUV的状态情况，若存在异常，则对其进行状态异常处理，否则将发出指令，执行下一步动作；

最后，以是否回收成功作为逻辑流程终止判断条件，若满足则结束，若不满足则进行各系统调整，重新工作；

所述回收自主水下航行器的具体规划方法包括以下步骤：

步骤一：AUV任务结束后，与USV进行通讯并向USV发送回收请求；

步骤二：USV确认回收指令，启动回收程序，回收过程开始；

步骤三：AUV上浮调整至预定深度；

步骤四：USV根据风向传感器调整航向，实现顶风并按设定的航速前行；

步骤五：AUV进行空间轨迹规划驶向USV尾部后下方，USV保持原运动；

步骤六：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断AUV是否进入回收范围，如果是，转向步骤七，否则转向步骤五；

步骤七：AUV对USV的航行轨迹进行估计，以航向及横向位移偏差为约束条件进行路径规划，选择最优路径航行至与USV同一直线，按设定航速同向前行，USV保持原运动；

步骤八：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断是否满足能够对接距离，如果是，转向步骤九，否则转向步骤七；

步骤九：AUV、USV减速，保持直线航线进行对接，过程中AUV利用视觉定位，通过前视摄像头观测USV的导引缆是否出现，及其与系缆钳的相对位置，实时监控对接目标状态，保证导引缆在系缆钳张开的范围内；

步骤十：通过AUV艏部观测得到的导引缆与系缆钳动作情况或AUV艏部力传感器的脉冲信号，判断AUV是否对接成功，如果是转向步骤十二，否则转向步骤十一；

步骤十一：启动AUV自我调整模式，进行重回收；

步骤十二：AUV关闭推进器，向USV发送对接成功信号，USV收到信号后收缆，完成整个回收任务；

在水下对接的过程中，从AUV捕获装置闭合且AUV与导引缆上的同步信号不为零开始表示AUV此次对接过程失败，所述步骤十一中AUV启动自我调整模式的具体实施步骤如下：

由USBL超短基线定位系统测得数据获得AUV在USV当前艏向方向的位移偏差Δξ，作为判断依据；

若Δξ＞0，则表示AUV位置仍处于USV后方，此时AUV采取对对接距离条件重新判断，尝试第二次对接；

若Δξ＜0，则表示AUV错过对接目标，此时AUV采取保持原运动航行一段后重新进行空间轨迹规划进行调头，回到修正后的对接起点，开始尝试第二次的对接，此过程USV保持减速慢行；

假设要求USV在逆风条件下直线航行所对应的实际轨迹为横向波动于设定直线的曲线，一定的横向误差能够由捕获装置开张尺度弥补，因此，由USV在对接阶段的始末两点推延得到USV后续运动的拟合线性轨迹a_USVx+b_USVy+c_USV＝0；其中，a_USV、b_USV和c_USV为USV水平面运动轨迹的线性参数，根据运动轨迹拟合得到；

同理，得到AUV前次对接阶段的线性轨迹a_AUVx+b_AUVy+c_AUV＝0；其中，a_AUV、b_AUV和c_AUV为AUV水平面运动轨迹的线性参数，根据运动轨迹拟合得到；

因此得到，修正后航向角为Ψ_cor＝Ψ_ori+Δα，修正后的起始点横向坐标为y_cor＝y_ori+Δy；

其中Ψ_ori为原对接航向，能够通过惯性导航系统/电罗经测量得到；Δα为修正角度，取y_ori为AUV原对接横向坐标，能够通过USBL超短基线定位系统测量得到，Δy为横向修正值，即横向位移偏差，取