[发明专利]基于迭代的自主水下航行器三维曲线路径跟踪控制方法

权利要求书

1.一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是：

步骤1.初始化，给定三维空间期望跟踪路径参数化方程描述，给定AUV初始位置和姿态信息，给定期望跟踪路径参数的初始值，给定期望路径上“虚拟向导”初始位置和初始移动速度信息；

步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差；

步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律，包括纵向移动速度、AUV的转艏角速度和纵倾角速度虚拟控制律；

步骤4.在运动学等价控制律的基础上，采用迭代，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律，即根据AUV具体水动力数学模型解算出最终的执行指令信号，包括推进器推力、纵倾控制力矩和转艏控制力矩；

步骤5.计算当前AUV位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离若小于设定的航迹切换半径R，则表示完成当前指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。

2.根据权利要求1所述的基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是：给定期望跟踪路径Ω上的虚拟向导P在固定坐标系的坐标表示为某一标量参数s∈R的函数为

ηdn(s)=[xd(s),yd(s),zd(s)]T]]>

为了保证被跟踪路径的光滑性，要求x_d(s)，y_d(s)，z_d(s)二阶偏导数存在；

定义虚拟向导点P的速度u_p方向为沿曲线路径的切线方向与固定坐标系水平轴的夹角ψ_d为

ψ_d＝arctan(y′_d/x′_d)

速度向量u_p与固定坐标系垂直轴的夹角θ_d定义为

θd=tan-1(-zd′(xd′)2+(yd′)2)]]>

其中因此虚拟向导点P沿曲线路径的旋转角速度分别表示为

qd=θ·d]]>

rd=ψ·d]]>

然后给定AUV在固定坐标系下的初始位置为ηⁿ＝[x,y,z]^T，AUV的初始艏向角和纵倾角分别为ψ和θ，AUV纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q。