[发明专利]一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法

权利要求书

1.一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法，其特征在于：包含如下步骤：

步骤(1)：建立AUV六自由度运动学方程，AUV六自由度动力学方程；

步骤(2)：建立海洋环境影响下的欠驱动AUV的相对速度和相对加速度的方程；

步骤(3)：对虚拟锚泊状态下欠驱动水下机器人进行受力分析，得到纵向力，横向力，转艏力矩；

步骤(4)：通过改进的静态悬链线方程表示悬链曲线张力水平分力方程和悬链曲线张力垂直分力方程；

步骤(5)：采用改进牛顿法求解水下机器人所受到的悬链曲线张力水平分力和悬链曲线张力垂直分力；

步骤(6)：对悬链曲线张力水平分力和悬链曲线张力垂直分力进行分解：

步骤(7)：将纵向力、转艏力矩和俯仰力矩分配到AUV的执行器，得出主推进器的推力X等于纵向力、AUV垂直舵产生的转艏力矩等于转艏力矩，AUV垂直舵产生的俯仰力矩等于俯仰力矩；

所述的步骤(3)具体为：

纵向力F_xm＝F_h cos(ψ-Le)，

横向力F_ym＝F_h sin(ψ-Le)，

转艏力矩M_m＝F_hl_m sin(ψ-Le)，

俯仰力矩：N_m＝F_vl_m cos(θ-Ve)；

上式中，F_v为AUV受到锚链作用垂直分力，F_h为AUV受到锚链作用水平分力，ψ为艏向角，θ为纵倾角，Le为第一锚链角，Ve为，l_m为锚泊水平分力到作用点的力臂；

所述的步骤(4)具体为：

改进的静态悬链线方程为：

悬链曲线张力水平分力方程为：

F_h＝q·a；

悬链曲线张力垂直分力方程为：

上式中，a为常数，x是x轴方向的坐标，y是y轴方向的坐标，q为横向角加速度，d是AUV与目标点之间的水平距离；

所述的步骤(5)具体为：

改进牛顿法：

第一步：将AUV与目标点之间的水平距离d和AUV到目标点的垂直距离h代入到悬链线方程中，求解方程解的初值a₀：

将x＝a代入改进的静态悬链线方程，且令则：

代入x＝d，y＝h，h为AUV到目标点的垂直距离，当首次f(a_n)·f(a_n+1)＜0时求出初值a₀；

第二步：将a₀带入改进牛顿法的迭代公式：

初始化令k＝0，每循环一次，令k＝k+1；a_k，a_k+1均为常数；k为非负整数；

第三步：计算误差e＝a_k+1-a_k；

第四步：判断是否|e|＜ε，ε＝0.00001，若是则结束并输出a＝a_k+1，否则转到第二步；

第五步：将a代入悬链曲线张力水平分力方程求得悬链曲线张力水平分力，将a代入悬链曲线张力垂直分力方程求解悬链曲线张力垂直分力；

所述的步骤(6)具体为：

在水平面控制中，将悬链曲线张力水平分力按照AUV的随艇坐标系进行分解，分为纵向力F_xm，横向力F_ym和转艏力矩M_m；对悬链曲线张力垂直分力进行分解，分为俯仰力矩N_m。