本发明公开了一种水面无人船导引缆回收自主水下航行器的规划方法，所述回收的逻辑流程如下：首先，当AUV结束任务后开始回收过程；其次，判断数据是否存在异常；再次，判断AUV的状态情况，若存在异常，则对其进行状态异常处理，否则将发出指令，执行下一步动作；最后，以是否回收成功作为逻辑流程终止判断条件。本发明的有益效果是：本发明针对水面无人船自主回收水下航行器的作业过程，提出了一种自主水下航行器与水面无人船协同对接回收的规划方法，包括回收任务启动/终止、对接协调控制以及对接失败后的重规划等各阶段的控制策略，通过全过程的规划调整来保证整个回收过程的实效性，大幅提高水下航行器由水面无人船自主回收的成功率。
1.一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述回收的逻辑流程如下：首先，当AUV结束任务后开始回收过程，AUV、USV所搭载的各传感器同时工作，检测当前状态数据并实时传输至控制系统；
其次，通过对状态数据的滤波处理以及各定位系统的信息进行校核，判断数据是否存在异常，若发现异常则进行数据异常处理，重新获取数据，否则进行状态危险度判断；
再次，通过在控制系统内部嵌入AUV动力学模型，并将融合处理后的数据与AUV动力学模型航位推算得到的信息进行比较，从而判断AUV的状态情况，若存在异常，则对其进行状态异常处理，否则将发出指令，执行下一步动作；
最后，以是否回收成功作为逻辑流程终止判断条件，若满足则结束，若不满足则进行各系统调整，重新工作；
所述回收自主水下航行器的具体规划方法包括以下步骤：
步骤一：AUV任务结束后，与USV进行通讯并向USV发送回收请求；
步骤二：USV确认回收指令，启动回收程序，回收过程开始；
步骤四：USV根据风向传感器调整航向，实现顶风并按设定的航速前行；
步骤五：AUV进行空间轨迹规划驶向USV尾部后下方，USV保持原运动；
步骤六：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断AUV是否进入回收范围，如果是，转向步骤七，否则转向步骤五；
步骤七：AUV对USV的航行轨迹进行估计，以航向及横向位移偏差为约束条件进行路径规划，选择最优路径航行至与USV同一直线，按设定航速同向前行，USV保持原运动；
步骤八：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断是否满足能够对接距离，如果是，转向步骤九，否则转向步骤七；
步骤九：AUV、USV减速，保持直线航线进行对接，过程中AUV利用视觉定位，通过前视摄像头观测USV的导引缆是否出现，及其与系缆钳的相对位置，实时监控对接目标状态，保证导引缆在系缆钳张开的范围内；
步骤十：通过AUV艏部观测得到的导引缆与系缆钳动作情况或AUV艏部力传感器的脉冲信号，判断AUV是否对接成功，如果是转向步骤十二，否则转向步骤十一；
步骤十二：AUV关闭推进器，向USV发送对接成功信号，USV收到信号后收缆，完成整个回收任务；
在水下对接的过程中，从AUV捕获装置闭合且AUV与导引缆上的同步信号不为零开始表示AUV此次对接过程失败，所述步骤十一中AUV启动自我调整模式的具体实施步骤如下：由USBL超短基线定位系统测得数据获得AUV在USV当前艏向方向的位移偏差Δξ，作为判断依据；
若Δξ＞0，则表示AUV位置仍处于USV后方，此时AUV采取对对接距离条件重新判断，尝试第二次对接；
若Δξ＜0，则表示AUV错过对接目标，此时AUV采取保持原运动航行一段后重新进行空间轨迹规划进行调头，回到修正后的对接起点，开始尝试第二次的对接，此过程USV保持减速慢行；
假设要求USV在逆风条件下直线航行所对应的实际轨迹为横向波动于设定直线的曲线，一定的横向误差能够由捕获装置开张尺度弥补，因此，由USV在对接阶段的始末两点推延得到USV后续运动的拟合线性轨迹aUSVx+bUSVy+cUSV＝0；其中，aUSV、bUSV和cUSV为USV水平面运动轨迹的线性参数，根据运动轨迹拟合得到；
同理，得到AUV前次对接阶段的线性轨迹aAUVx+bAUVy+cAUV＝0；其中，aAUV、bAUV和cAUV为AUV水平面运动轨迹的线性参数，根据运动轨迹拟合得到；
因此得到，修正后航向角为Ψcor＝Ψori+Δα，修正后的起始点横向坐标为ycor＝yori+Δy；
其中Ψori为原对接航向，能够通过惯性导航系统/电罗经测量得到；Δα为修正角度，取yori为AUV原对接横向坐标，能够通过USBL超短基线定位系统测量得到，Δy为横向修正值，即横向位移偏差，取
2.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述步骤三中AUV调整至预定深度的判断条件以测深仪输出的信息为依据，满足hAUV-cmd＝hps；
其中，hAUV-cmd是AUV深度调整的指令，hps是预设的AUV深度，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角，考虑环境条件，hps取值为5m。
3.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述步骤四中USV调整航向按设定航速前行实现的航向指令和航速指令分别为：ψUSV-cmd＝ψwind+π和vUSV-cmd＝vUSV-ps1；
其中，ψUSV-cmd为USV航向调整的指令，ψwind为风向角，由USV所携带的风向传感器获得；
vUSV-cmd为USV航速指令，为保证低速航行，取值2～3节；vUSV-ps1为USV预设速度，由GPS得到。
4.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述步骤五中，AUV驶向USV实现的航速指令为：UAUV-cmd＝VR+Uc；
其中，UAUV-cmd为AUV驶向USV实现的航速指令；VR为预想航速，取vAUV-ps2为AUV预设的航速，取值3～5节，ψcmd为预想航向，取 ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得；Uc为在AUV运动坐标系下的海流速度，取为在大地坐标系下的海流速度，由ADCP声学多普勒流速剖面仪获得，S为旋转变换阵，取
5.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特 征 在于 ：所 述 步 骤六 中 ，判 断 A UV 是 否 进 入 回收 范 围 的 条 件 是 ：其中，ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得，R为回收范围的半径，取值范围为10～
30m，R＝r+Δr，其中r为USV与AUV的相对距离，能够通过超短基线定位系统得到，Δr是由导引缆绳偏离铅垂向带来的增量，考虑USV低速航行，导引缆绳的偏角在10°～20°以内，在水深5m处产生的增量为1m。
6.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述步骤七中，AUV航行至与USV同一直线且同向实现的命令为：vAUV-cmd＝vAUV-ps2；
其中，同一直线的实现由以USV上一轨迹点为目标点，USV航向为目标航向进行规划实现，vAUV-cmd为AUV的航速指令，vAUV-ps2为AUV预设的航速，取值3～5节。
7.根据权利要求4所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特 征在 于 ：所 述步 骤 八 中 ，判断 是 否 满 足能 够 对 接 距 离的 条 件 是 ：其中，ycable-ps为导引缆绳上预定深度对应的点的横向坐标，取
xcable-ps为缆绳上预定深度对应点的纵向坐标，取xcable-ps＝xUSV，yUSV、xUSV分别为USV的横向和纵向坐标，由USV所携带的GPS获得；yAUV、xAUV分别为AUV的横向和纵向坐标，取xAUV＝xUSV+ΔxUSV-AUV(t)，yAUV＝yUSV+ΔyUSV-AUV(t)；D为设定的对接距离值，考虑到在对接范围内采用视觉传感器进行引导对接能提高对接识别精度，D取视觉传感器高效作用范围2～3m；hps是预设的AUV深度，考虑环境条件，hps取值为5m，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角。
8.根据权利要求1所述的一种水面无人船导引缆回收自主水下航行器的规划方法，其特征在于：所述步骤十中，判断AUV是否对接成功的条件为：观测图像中导引缆绳是否在系缆钳中或当AUV用于捕获对接目标的装置闭合后AUV艏部的力传感器数值是否明显增大，或AUV的速度是否受限。
一种水面无人船导引缆回收自主水下航行器的规划方法
技术领域
[0001] 本发明涉及一种水面无人船导引缆回收自主水下航行器的规划方法，属于海上无人技术领域。
背景技术
[0002] 水面无人船(Unmanned Surface Vehicle，简称USV)和自主水下航行器(Autonomous Underwater Vehicle，简称AUV)作为海上无人系统的主要组成部分，近年来受到国内外越来越多的关注。作为研究热点，其在军事和民用方面都有广阔的应用前景。随着各国对海洋战略的重视及海洋开发力度的加大，USV和AUV都将扮演越来越重要的角色，对人类水上工程应用、海洋军事活动及海洋开发利用等产生深远影响。
[0003] 作为无人平台，USV和AUV各有所长，也各有所短，无法相互替代。AUV受到水下通讯、导航和续航能力等技术瓶颈制约，很难独立的很好解决，严重影响了其工程应用前景。而USV的工作范围位于水上，深水活动能力和水下探测能力又受到一定的限制。将USV和AUV两种平台进行系统融合，来执行一系列不能由其中单个系统独立实现的任务，特别是需要建立水下的AUV和水面USV之间联系的某些场合，在海洋领域具有光明的前景，也可进一步发展成由USV和AUV组成的海上无人编队系统，充分发挥集群作用。其中，如何自主回收AUV是USV和AUV协同作业的重要关键技术。
[0004] 事实上，AUV由于其功能多样且具备隐蔽性高、机动性好等优点而广泛运用于海洋军事、海洋调查、海底资源勘探、管线巡检等各个领域，但由于水下数据的实时传输仍存在较大的难度，利用AUV执行任务时通常是在回收后才进行数据的下载；同时，AUV自带电池，其续航力受到限制，必须在回收后进行电力补给，因而AUV的成功回收显得格外重要。
[0005] 但由于海洋环境复杂，受海面的风、浪、流影响，自主水下航行器的回收问题一直是世界性的难题，成为限制水下航行器发展的技术瓶颈。尤其是有人辅助回收，需要人员乘坐机动艇靠近水下航行器进行吊点对接，当海况恶劣时母船升沉幅度较大，极易损害设备并严重威胁人员安全。因此，在水面以无人的方式自主引导对接并回收AUV技术成为近年来研究的热点。过去十多年内，以美国为首的西方国家先后提出多种无人布放和回收AUV的方式，但依然存在水动力影响严重、动态对接难度较大等缺陷，回收成功率较低。
[0006] 在水面无人船与自主水下航行器进行对接的关键过程中，AUV会受到复杂海洋环境扰动的影响，USV的运动状态也会因海面风、浪、流的作用而与期望状态发生偏离，再加上现有的通讯、定位等技术在充满不定因素的水下环境缺乏稳健性，这些都会极大的影响回收成功率。因此，在回收过程AUV如何进行规划调整以保证回收实效性是重要的技术问题。
发明内容
[0007] 针对现有技术上存在的不足，本发明的目的在于通过全过程的规划调整来保证整个回收过程的实效性，从而大幅提高水下航行器由水面无人船自主回收的成功率。
[0008] 为了实现上述目的，本发明是通过如下的技术方案来实现：
[0009] 一种水面无人船导引缆回收自主水下航行器的规划方法，所述回收的逻辑流程如下：
[0010] 首先，当AUV结束任务后开始回收过程，AUV、USV所搭载的各传感器同时工作，检测当前状态数据并实时传输至控制系统；
[0011] 其次，通过对状态数据的滤波处理以及各定位系统的信息进行校核，判断数据是否存在异常，若发现异常则进行数据异常处理，重新获取数据，否则进行状态危险度判断；
[0012] 再次，通过在控制系统内部嵌入AUV动力学模型，并将融合处理后的数据与AUV动力学模型航位推算得到的信息进行比较，从而判断AUV的状态情况，若存在异常，则对其进行状态异常处理，否则将发出指令，执行下一步动作；
[0013] 最后，以是否回收成功作为逻辑流程终止判断条件，若满足则结束，若不满足则进行各系统调整，重新工作。
[0014] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述回收自主水下航行器的具体规划方法包括以下步骤：
[0015] 步骤一：AUV任务结束后，与USV进行通讯并向USV发送回收请求；
[0016] 步骤二：USV确认回收指令，启动回收程序，回收过程开始；
[0017] 步骤三：AUV上浮调整至预定深度；
[0018] 步骤四：USV根据风向传感器调整航向，实现顶风并按设定的航速前行；
[0019] 步骤五：AUV进行空间轨迹规划驶向USV尾部后下方，USV保持原运动；
[0020] 步骤六：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断AUV是否进入回收范围，如果是，转向步骤七，否则转向步骤五；
[0021] 步骤七：AUV对USV的航行轨迹进行估计，以航向及横向位移偏差为约束条件进行路径规划，选择最优路径航行至与USV同一直线，按设定航速同向前行，USV保持原运动；
[0022] 步骤八：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断是否满足能够对接距离，如果是，转向步骤九，否则转向步骤七；
[0023] 步骤九：AUV、USV减速，保持直线航线进行对接，过程中AUV利用视觉定位(通过前视摄像头观测USV的导引缆是否出现，及其与系缆钳的相对位置)实时监控对接目标状态，保证导引缆在系缆钳张开的范围内；
[0024] 步骤十：通过AUV艏部观测得到的导引缆与系缆钳动作情况或AUV艏部力传感器的脉冲信号，判断AUV是否对接成功，如果是转向步骤十二，否则转向步骤十一；
[0025] 步骤十一：启动AUV自我调整模式，进行重回收；
[0026] 步骤十二：AUV关闭推进器，向USV发送对接成功信号，USV收到信号后收缆，完成整个回收任务。
[0027] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，在水下对接的过程中，从AUV捕获装置闭合且AUV与导引缆上的同步信号不为零开始表示AUV此次对接过程失败，所述步骤十一中AUV启动自我调整模式的具体实施步骤如下：
[0028] 由USBL超短基线定位系统测得数据获得AUV在USV当前艏向方向的位移偏差Δξ，作为判断依据；
[0029] 若Δξ＞0，则表示AUV位置仍处于USV后方，此时AUV采取对对接距离条件重新判断，尝试第二次对接；
[0030] 若Δξ＜0，则表示AUV错过对接目标，此时AUV采取保持原运动航行一段后重新进行空间轨迹规划进行调头，回到修正后的对接起点，开始尝试第二次的对接，此过程USV保持减速慢行；
[0031] 假设要求USV在逆风条件下直线航行所对应的实际轨迹为横向波动于设定直线的曲线，一定的横向误差能够由捕获装置开张尺度弥补，因此，由USV在对接阶段的始末两点推延得到USV后续运动的拟合线性轨迹aUSVx+bUSVy+cUSV＝0；其中，aUSV、bUSV和cUSV为USV水平面运动轨迹的线性参数，根据运动轨迹拟合得到。
[0032] 同理，得到AUV前次对接阶段的线性轨迹aAUVx+bAUVy+cAUV＝0；其中，aAUV、bAUV和cAUV为AUV水平面运动轨迹的线性参数，根据运动轨迹拟合得到。
[0033] 因此得到，修正后航向角为Ψcor＝Ψori+Δα，修正后的起始点横向坐标为ycor＝yori+Δy；
[0034] 其中Ψori为原对接航向，能够通过惯性导航系统/电罗经测量得到；Δα为修正角度，取 yori为AUV原对接横向坐标，能够通过USBL超短基线定位系统测量得到，Δy为横向修正值，即横向位移偏差，取
[0035] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤三中AUV调整至预定深度的判断条件以测深仪输出的信息为主，满足hAUV-cmd＝hps；
[0036] 其中，hAUV-cmd是AUV深度调整的指令，hps是预设的AUV深度，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角，考虑环境条件，hps优先取为5m。
[0037] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤四中USV调整航向按设定航速前行实现的航向指令和航速指令分别为：ψUSV-cmd＝ψwind+π和vUSV-cmd＝vUSV-ps1；
[0038] 其中，ψUSV-cmd为USV航向调整的指令，ψwind为风向角，由USV所携带的风向传感器获得；vUSV-cmd为USV航速指令，为保证低速航行，取值2～3节；vUSV-ps1为USV预设速度，由GPS得到。
[0039] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤五中，AUV驶向USV实现的航速指令为：UAUV-cmd＝VR+Uc；
[0040] 其中，UAUV-cmd为AUV驶向USV实现的航速指令；VR为预想航速，取vAUV-ps2为AUV预设的航速，取值3～5节，ψcmd为预想航向，取
ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)
为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得；Uc为在AUV运动坐标系下的海流速度，取 为在大地坐标系下的海流速度，由ADCP声
学多普勒流速剖面仪获得，S为旋转变换阵，取
姿态角ψ，θ，
分别为艏向角、俯仰角和滚转角，都由罗经获得。
[0041] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤六中，判断AUV是否进入回收范围的条件是：
[0042] 其中，ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得，R为回收范围的半径，优选范围为10～30m，R＝r+Δr，其中r为USV与AUV的相对距离，能够通过超短基线定位系统得到，Δr是由导引缆绳偏离铅垂向带来的增量，考虑USV低速航行，导引缆绳的偏角在10°～20°以内，在水深5m处产生的增量约为1m。
[0043] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤七中，AUV航行至与USV同一直线且同向实现的命令为：vAUV-cmd＝vAUV-ps2；
[0044] 其中，同一直线的实现由以USV上一轨迹点为目标点，USV航向为目标航向进行规划实现，vAUV-cmd为AUV的航速指令，vAUV-ps2为AUV预设的航速，取值3～5节。
[0045] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤八中，判断是否满足能够对接距离的条件是：
[0046] 其中，ycable-ps为导引缆绳上预定深度对应的点的横向坐标，取xcable-ps为缆绳上预定深度对应点的纵向坐标，取xcable-ps＝xUSV，yUSV、
xUSV分别为USV的横向和纵向坐标，由USV所携带的GPS获得；yAUV、xAUV分别为AUV的横向和纵向坐标，取xAUV＝xUSV+ΔxUSV-AUV(t)，yAUV＝yUSV+ΔyUSV-AUV(t)；D为设定的对接距离值，考虑到在对接范围内采用视觉传感器进行引导对接能提高对接识别精度，D取视觉传感器高效作用范围2～3m；hps是预设的AUV深度，考虑环境条件，hps优先取为5m，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角。
[0047] 上述的一种水面无人船导引缆回收自主水下航行器的规划方法，所述步骤十中，判断AUV是否对接成功的条件为：观测图像中导引缆绳是否在系缆钳中或当AUV用于捕获对接目标的装置闭合后AUV艏部的力传感器数值是否明显增大，或AUV的速度是否受限。
[0048] 本发明的有益效果为：
[0049] (1)本发明针对水面无人船自主回收水下航行器的作业过程，提出了一种自主水下航行器与水面无人船协同对接回收的规划方法，包括回收任务启动/终止、对接协调控制以及对接失败后的重规划等各阶段的控制策略，通过全过程的规划调整来保证整个回收过程的实效性，从而大幅提高水下航行器由水面无人船自主回收的成功率；
[0050] (2)无人干预下安全可靠的回收：本发明以回收、对接阶段中USV、AUV的位置及运动状态信息作为判断依据，调整AUV下一步的动作，通过两者的交互反馈，在无人干预下实现安全可靠的回收过程；
[0051] (3)综合考虑多种可能性以提高回收的成功率：本发明在回收过程中考虑了各阶段AUV实时的状态，对其各阶段中不同的情况进行不同方式的调整，特别在对接环节失败后，控制系统将根据反馈进行数据修正，重新作出判断，进入再次回收，以“总结失败经验”达到提高回收成功率的目的；
[0052] (4)趋利避害以减少可能的误差：本发明在要求USV在水面航行以逆风的方式增加其运动的稳定，减少海情对其姿态的影响；考虑水下AUV在回收的范围内受到海流的影响，增大回收过程运动控制的精度，并且以调整AUV的运动为主来实现回收，避免了难度较大的两个复杂移动物对接的状况，降低了对控制精度的要求；
[0053] (5)计算简易，便于实现：本发明步骤中的判断条件所需的计算以及物理量的测量均较为简易，一般利用简单的程序语句和传感器均可实现。
附图说明
[0054] 下面结合附图和具体实施方式来详细说明本发明：
[0055] 图1为USV回收AUV的整体概况图；
[0056] 图2为AUV数学模型整体和局部坐标系；
[0057] 图3为回收过程逻辑流程图；
[0058] 图4为AUV回收过程中AUV自我调整的方法流程图；
[0059] 图5为空间轨迹规划流程图；
[0060] 图6为动目标跟踪控制原理图；
[0061] 图7为坐标转换图示；
[0062] 图8为USV引导处理流程图；
[0063] 图9为USV引导回收AUV操作示意图；
[0064] 图10为控制系统原理结构图。
具体实施方式
[0065] 为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。
[0066] 本发明由但不限于以下装置系统方案实现：整个系统由水面无人船USV、自主水下航行器AUV以及所搭载的设备构成；具体USV和AUV用于回收引导和通讯交互的设备如下：
[0067] 除了保证USV正常运行的设备外，本发明还要求USV搭载有：GPS定位系统，无线通讯天线，USBL超短基线定位系统的主基阵，风向传感器；
[0068] 除了保证AUV正常工作的设备外，本发明还要求AUV搭载有：惯性导航系统，ADCP声学多普勒流速剖面仪，USBL超短基线定位系统的应答器，测深仪，摄像头，电罗经。
[0069] 如图1至图10所示，本发明的水面无人船导引缆回收自主水下航行器的规划方法，回收的逻辑流程如下：
[0070] 首先，当AUV结束任务后开始回收过程，AUV、USV所搭载的各传感器同时工作，检测当前状态数据并实时传输至控制系统；
[0071] 其次，通过对状态数据的滤波处理以及各定位系统的信息进行校核，判断数据是否存在异常，若发现异常则进行数据异常处理，重新获取数据，否则进行状态危险度判断；
[0072] 再次，通过在控制系统内部嵌入AUV动力学模型，并将融合处理后的数据与AUV动力学模型航位推算得到的信息进行比较，从而判断AUV的状态情况，若存在异常，则对其进行状态异常处理，否则将发出指令，执行下一步动作；
[0073] 最后，以是否回收成功作为逻辑流程终止判断条件，若满足则结束，若不满足则进行各系统调整，重新工作。
[0074] 回收自主水下航行器的具体规划方法包括以下步骤：
[0075] 步骤一：AUV任务结束后，与USV进行通讯并向USV发送回收请求；
[0076] 步骤二：USV确认回收指令，启动回收程序，回收过程开始；
[0077] 步骤三：AUV上浮调整至预定深度；
[0078] 步骤四：USV根据风向传感器调整航向，实现顶风并按设定的航速前行；
[0079] 步骤五：AUV进行空间轨迹规划驶向USV尾部后下方，USV保持原运动；
[0080] 步骤六：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断AUV是否进入回收范围，如果是，转向步骤七，否则转向步骤五；
[0081] 步骤七：AUV对USV的航行轨迹进行估计，以航向及横向位移偏差为约束条件进行路径规划，选择最优路径航行至与USV同一直线，按设定航速同向前行，USV保持原运动；
[0082] 步骤八：通过超短基线定位系统得到AUV与USV水平面上的距离，从而判断是否满足能够对接距离，如果是，转向步骤九，否则转向步骤七；
[0083] 步骤九：AUV、USV减速，保持直线航线进行对接，过程中AUV利用视觉定位(通过前视摄像头观测USV的导引缆是否出现，及其与系缆钳的相对位置)实时监控对接目标状态，保证导引缆在系缆钳张开的范围内；
[0084] 步骤十：通过AUV艏部观测得到的导引缆与系缆钳动作情况或AUV艏部力传感器的脉冲信号，判断AUV是否对接成功，如果是转向步骤十二，否则转向步骤十一；
[0085] 步骤十一：启动AUV自我调整模式，进行重回收；
[0086] 步骤十二：AUV关闭推进器，向USV发送对接成功信号，USV收到信号后收缆，完成整个回收任务。
[0087] 在水下对接的过程中，从AUV捕获装置闭合且AUV与导引缆上的同步信号不为零开始表示AUV此次对接过程失败，步骤十一中AUV启动自我调整模式的具体实施步骤如下：
[0088] 由USBL超短基线定位系统测得数据获得AUV在USV当前艏向方向的位移偏差Δξ，作为判断依据；
[0089] 若Δξ＞0，则表示AUV位置仍处于USV后方，此时AUV采取对对接距离条件重新判断，尝试第二次对接；
[0090] 若Δξ＜0，则表示AUV错过对接目标，此时AUV采取保持原运动航行一段后重新进行空间轨迹规划进行调头，回到修正后的对接起点，开始尝试第二次的对接，此过程USV保持减速慢行；
[0091] 假设要求USV在逆风条件下直线航行所对应的实际轨迹为横向波动于设定直线的曲线，一定的横向误差能够由捕获装置开张尺度弥补，因此，由USV在对接阶段的始末两点推延得到USV后续运动的拟合线性轨迹aUSVx+bUSVy+cUSV＝0；其中，aUSV、bUSV和cUSV为USV水平面运动轨迹的线性参数，根据运动轨迹拟合得到。
[0092] 同理，得到AUV前次对接阶段的线性轨迹aAUVx+bAUVy+cAUV＝0；其中，aAUV、bAUV和cAUV为AUV水平面运动轨迹的线性参数，根据运动轨迹拟合得到。
[0093] 因此得到，修正后航向角为Ψcor＝Ψori+Δα，修正后的起始点横向坐标为ycor＝yori+Δy；
[0094] 其中Ψori为原对接航向，能够通过惯性导航系统/电罗经测量得到；Δα为修正角度，取 yori为AUV原对接横向坐标，能够通过USBL超短基线定位系统测量得到，Δy为横向修正值，即横向位移偏差，取
[0095] 步骤三中AUV调整至预定深度的判断条件以测深仪输出的信息为主，满足hAUV-cmd＝hps；
[0096] 其中，hAUV-cmd是AUV深度调整的指令，hps是预设的AUV深度，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角，考虑环境条件，hps优先取为5m。
[0097] 步骤四中USV调整航向按设定航速前行实现的航向指令和航速指令分别为：ψUSV-cmd＝ψwind+π和vUSV-cmd＝vUSV-ps1；
[0098] 其中，ψUSV-cmd为USV航向调整的指令，ψwind为风向角，由USV所携带的风向传感器获得；vUSV-cmd为USV航速指令，为保证低速航行，取值2～3节；vUSV-ps1为USV预设速度，由GPS得到。
[0099] 步骤五中，AUV驶向USV实现的航速指令为：UAUV-cmd＝VR+Uc；
[0100] 其中，UAUV-cmd为AUV驶向USV实现的航速指令；VR为预想航速，取vAUV-ps2为AUV预设的航速，取值3～5节，ψcmd为预想航向，取
ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)
为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得；Uc为在AUV运动坐标系下的海流速度，取 为在大地坐标系下的海流速度，由ADCP声
学多普勒流速剖面仪获得，S为旋转变换阵，取
姿态角ψ，θ，
分别为艏向角、俯仰角和滚转角，都由罗经获得，由于海流具有很大的不确定性，在时空上存在一定的差别，全面的考虑一个海流模型并不现实也不必要，因此仅考虑海流流速和方向两方面的影响，并且在回收范围相对较小的航行水域，航行时间有限，假设流速与方向不变。
[0101] 步骤六中，判断AUV是否进入回收范围的条件是：
[0102]
[0103] 其中，ΔxUSV-AUV(t)为该时刻USV与AUV纵向位置的偏差，ΔyUSV-AUV(t)为该时刻USV与AUV横向位置的偏差，两者均由超短基线定位系统获得，R为回收范围的半径，优选范围为10～30m，R＝r+Δr，其中r为USV与AUV的相对距离，能够通过超短基线定位系统得到，Δr是由导引缆绳偏离铅垂向带来的增量，考虑USV低速航行，导引缆绳的偏角在10°～20°以内，在水深5m处产生的增量约为1m。
[0104] 步骤七中，AUV航行至与USV同一直线且同向实现的命令为：vAUV-cmd＝vAUV-ps2；
[0105] 其中，同一直线的实现由以USV上一轨迹点为目标点，USV航向为目标航向进行规划实现，vAUV-cmd为AUV的航速指令，vAUV-ps2为AUV预设的航速，取值3～5节。
[0106] 步骤八中，判断是否满足能够对接距离的条件是：
[0107]
[0108] 其中，ycable-ps为导引缆绳上预定深度对应的点的横向坐标，取
[0109] xcable-ps为缆绳上预定深度对应点的纵向坐标，取xcable-ps＝xUSV，yUSV、xUSV分别为USV的横向和纵向坐标，由USV所携带的GPS获得；yAUV、xAUV分别为AUV的横向和纵向坐标，取xAUV＝xUSV+ΔxUSV-AUV(t)，yAUV＝yUSV+ΔyUSV-AUV(t)；D为设定的对接距离值，考虑到在对接范围内采用视觉传感器进行引导对接能提高对接识别精度，D取视觉传感器高效作用范围2～3m；hps是预设的AUV深度，考虑环境条件，hps优先取为5m，满足hps∈(zUSV,zUSV-l·sinσ)，zUSV为USV垂向位置坐标，由USV所携带的GPS获得，l为对接缆绳释放的长度，σ为缆绳在水下的偏角。
[0110] 步骤十中，判断AUV是否对接成功的条件为：观测图像中导引缆绳是否在系缆钳中或当AUV用于捕获对接目标的装置闭合后AUV艏部的力传感器数值是否明显增大，或AUV的速度是否受限。
[0111] 本发明的路径规划与目标跟踪的实现：
[0112] 由于AUV在回收过程中根据与对接目标的距离关系采取的定位系统以及侧重点均不相同，当距离较远时，AUV的运动规划侧重于安全快速地进入无人船预定的回收范围内，而在近距离的对接范围内则更加侧重对于对接目标的精准跟踪和一步步地接近。因此，在本发明中，于对接范围之外对AUV进行空间轨迹规划，于对接范围内对其进行动目标状态估计和跟踪，并且加上USV的引导操作来提高对接的成功率，具体实现可以但不限于如下方式：
[0113] (1)对接范围之外的AUV轨迹规划
[0114] 在回收过程开始或重回收过程开始时，通过各传感器系统获取AUV初始时刻的运动状态并且进行目标位置和约束条件初始化，将以上信息导入简化的模型中，对轨迹参数进行表示，利用A*算法进行节点搜索，对满足能量和时间约束条件的路径进行适航性判断，以及航路代价的计算，从而形成轨迹规划参数方程，并根据所定判别标准，例如航行轨迹最短等进行最优化问题求解，最后对所得航路进行平滑处理，得到期望的运动状态。在这个过程中需要实时更新当前流场信息以及障碍物情况，保证AUV航行的安全。
[0115] (2)对接范围内近距离时的动目标状态估计和跟踪
[0116] 由于在近距离对接阶段对于AUV的状态精度要求较高，从调整航向角尽快趋近视线角，控制跟踪速度稳定收敛于目标速度两方面进行自我调整。通过前视摄像头观测对接目标轨迹以及障碍物情况，将观测值导入简化模型中利用滤波算法进行运动参数估计，之后利用估计值进行航向和航速的解算，然后输出至非奇异终端滑模控制器对AUV进行航向和航速的控制，同时在这个过程由导航系统实时反馈相对运动参数进行实时监控调整，保持在定位系统视场范围内，一旦AUV接近视场边缘时，控制器将速度提升至限制范围内的最大进行追踪；一旦进入障碍物危险圆以内时，设计在最大转艏角速度的限制下，控制AUV以最大线速度尽快转向远离障碍物的视线方向。
[0117] (3)USV的引导处理
[0118] 首先建立USV水动力模型与运动方程，由该动态模型可输出大地参考坐标系下的USV位置(ξ，η)E、大地测量航向角ψE和随体坐标系下的运动速度UE。为得到更为精确的状态指令，对导航测量传感器和目标定位系统进行仿真，建立这些内部和外部传感器的模型，并为其增加预期的噪声干扰。如GPS和罗经所测量的位置信息 可通过方程在位置状态上增加传感器随机噪声得到；其中，nGPS/INS为GPS的随机
噪声，ncompass为罗经的随机噪声，在位置状态上增加传感器随机噪声得到。
[0119] 但由于传感器探测装置和目标位置分属不同坐标系，因此需要进行坐标转换。根据图7确定以目标航点为中心的位置偏差 将与目标中心的偏差进行归一化，作为模糊逻辑评价函数的输入量，并将增量航向指令输出结果与理想航向角相加得到实际的大地测量航向角的期望值以及速度期望值，再形成目标指令输出至底层控制器；其中，ψG为USV航向角，xG、yG分别为其在大地坐标系下的纵向和横向坐标，Δx、Δy为与目标航点在x、y方向的绝对坐标偏差，RE为USV从当前位置到目标航点的直线距离，xE、yE为USV目标航点的随体坐标系下纵横向偏差，α、为USV的当前艏向角，γ为USV的艏向调整角度。
[0120] 图8中，ψd、Ud分别为USV的期望艏向角和期望速度，ξE,ηE,ψE,UE分别为USV的纵横向大地坐标、艏向角和速度，ψLF是控制器输出的艏向调整角。
[0121] 图9中，ξR、ηR为AUV的纵横向大地坐标，ψR、UR为AUV的艏向角和航速，θR为AUV与USV之间的视线角。
[0122] 本发明回收过程的控制系统具体工作的实现可以但不限如下方式：
[0123] AUV的整个系统结构主要可以分为任务层、规划层、控制层以及执行层。在回收的过程中，执行层的传感器采集数据进行数据预处理，之后根据系统特征得到相邻时刻的状态转移规律，将预处理后的数据作为某个状态分量依据观测方程与所有维度上的状态分量建立联系，在统计滤波理论的指导下，实现实时的状态更新，即得到最小方差准侧下的滤波结果。通过观测值的累积，不断优化随机信号和噪声的统计特性，改善滤波精度最终趋于真实值后输入控制层。由于AUV具有耦合性及非线性较强，单纯运用传统PID控制方法效果并不那么理想，因此本发明采用基于模糊理论的PID控制方法，结合两种方式的优点，使控制器超调小，鲁棒性好。具体工作是输入滤波后的数据与目标姿态的偏差，模糊控制器根据误差选择论域进行运算，并将运算结果反馈给PDI控制器进行比例系数kp、积分时间常数ki和微分时间常数kd三个参数的调整，然后通过传统PID控制原理对模型进行计算，得到推力分配方案，由执行层推进器执行动作，同时在这个过程中传感器也实时采集当前的状态信息反馈于控制器。
[0124] USV在对接过程的导引操作控制具体为：主要以来自于USBL超短基线定位系统的数据(包括平面二维坐标和航向角)作为交会动态对接控制器的输入量，以视觉定位系统的数据对输入量进行校核监控，保证数据的可靠性，并在导引过程中保持每隔数秒进行一次数据更新；交会动态对接控制器从航向和航速两方面进行调整，得到具体执行方案，引导USV逐渐减少交叉跟踪目标偏差。
[0125] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
