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

摘要

本发明提供的是一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法。步骤1.初始化；步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差；步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律；步骤4.在运动学等价控制律的基础上，采用迭代，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律；步骤5.计算当前AUV位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离若小于设定的航迹切换半径R，则表示完成当前指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。本发明能够提高AUV的路径跟踪精度。

主权项

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。