[发明专利]一种基于水下动态回收平台的AUV回收方法

权利要求书

1.一种基于水下动态回收平台的AUV回收方法，其特征在于，包括以下步骤：

S1、回收平台进行三维悬停定位，并根据海流数据调整艏向角，具体的：

基于位置阶，进行三维悬停定位，基于姿态阶，通过海流数据调整艏向角，其位姿制导律为：

其中，下标p代指平台，下标d代指期望值，(x_intial,y_intial,z_intial)为预设的期望位置；为平台的期望姿态，为保证回收的平稳性，平台的期望纵倾和横倾均为0，u_r和v_r分别为平台随体坐标系下沿x轴和y轴的水流速度，确保AUV在回收装置附近不受侧向流影响，Ψ是平台当前艏向角；

S2、AUV通过USBL获取回收平台位姿，具体的：

AUV通过USBL获取回收平台的位姿，AUV的制导律为：

其中，下标A代指AUV，(x_usbl,y_usbl,z_usbl)为AUV通过USBL获取的期望位姿信号，AUV通过水声通信设备获取ψ_Pd并计算自身的ψ_Ad，u₁为一定常量，u_Ad为AUV的期望速度，AUV根据USBL信号接近回收平台；

S3、AUV判断回收平台是否感知自身，若是，则AUV保持直线稳定性，回收平台通过视觉信息调整位姿，具体的：

S31、基于水声通信设备，AUV获知动态平台发现自身后，保持直线稳定性，回收平台利用视觉信息调整自身位姿；其中，对于AUV有：

其中，u₂为预定航速，且被发现后，AUV航速不变；

S32、回收平台需要根据视觉信息主动调整自身位姿，完成对接任务，基于单目摄像机，有：

其中，下标un表示基于的单目摄像机无标定视觉伺服算法所解算出的期望位姿；在期望速度项解算中，k_y,k_z,k_ψ为正增益；

S33、基于PartitionedBroyden算法估计雅克比矩阵以及目标动态信息，在算法的第k步，有：

其中，为协方差矩阵，为回收平台三自由度步长和采样时间构成的一维向量，κ∈(0,1]为遗忘因子，为雅克比矩阵的估计值与目标运动速度估计值的复合矩阵，有为像平面特征点在第k步时的误差向量；

S34、回收平台计算下一步的期望输出，为：

其中，ζ_k+1＝[v_dk+1,w_dk+1,r_dk+1]^T，即在第k+1步时平台期望横移速度、升沉速度以及回转速度，λ为增益矩阵，为矩阵的伪逆，h_t为采样时间，为目标运动速度估计值；

S4、判断回收平台与AUV的距离是否处于预设范围内，若是，则判定AUV到达期望位置，完成对接。