专利名称：基于迭代的自主水下航行器三维曲线路径跟踪控制方法
技术领域：
本发明涉及欠驱动自主水下航行器的三维空间运动控制技术领域。
背景技术：
海底地形的勘探和测绘对深海资源的开发有着重要的意义，欠驱动自主水下航行器AUV (Autonomous Underwater Vehicle)由于具有良好的机动性和续航能力，在海洋勘探开发中扮演着重要的角色，随着AUV在海洋工程领域应用的不断深入，使得对AUV在水下三维空间的运动控制技术的研究提出了新的挑战，考虑到受到航行经济性或负载能力制约，通常执行机构配置为纵向尾部推进器、水平方向舵和垂直升降舵，AUV大多未配备横向和垂向辅助推进器，使得控制输入的维数远小于运动自由度数，为欠驱动系统，无法设计光滑时不变控制律实现反馈控制，同时由于受到复杂多变的海洋环境作用，使得AUV动力学 模型具有较高的非线性、不确定性和模型自身存在的耦合性，这也成为欠驱动AUV三维空间跟踪控制器设计的难点。目前，国内外针对欠驱动AUV的三维空间运动控制的研究较少，研究大多针对解耦的水平面运动子系统和垂直面深度控制子系统分别设计控制器，进而实现对欠驱动自主水下航行器在水下三维空间的运动控制，由于忽略了模型的耦合作用，设计的控制器无法实现欠驱动AUV对空间任意平滑曲线的跟踪控制。这里讨论的跟踪控制问题具体为水下三维空间的路径跟踪控制问题，水下三维空间路径的描述通过参数化方程进行描述，不同于三维轨迹跟踪控制问题中的轨迹方程以时间作为参数，克服了传统轨迹跟踪控制问题中由于引入具有同构动力学模型的“虚拟AUV”受到环境干扰作用引起闭环跟踪系统具有不稳定动态，本发明中通过设计期望路径上“虚拟向导”的移动速度作为跟踪系统额外的控制输入，由于“虚拟向导”只具有运动学特性而无具体动力学模型，因此状态不受外界扰动的影响，能够保证跟S示系统的稳定性和动态性能。P. Encarnacao 等在论文《3D Path Following for Autonomous UnderwaterVehicle)) (Proceedings of the 39th IEEE Conference on Decision and Control, IEEEPress, 2000, Sydney.)利用正交投影的思想建立AUV在期望路径坐标系(Serret-Frenet)下的三维路径跟踪误差模型，由于存在奇异值点，对AUV的初始位置有约束，无法实现AUV跟踪的全局收敛，而本专利建立的AUV载体坐标系下的三维跟踪误差模型不存在奇异值问题，因此能够保证AUV跟踪误差的全局收敛性；《基于自适应Backst印ping的欠驱动AUV三维航迹跟踪控制》(控制与决策，2012,第38卷第2期)依据视线法(line-of-sight, L0S)计算期望跟踪视线角，基于自适应反步法设计跟踪控制器，针对离散的航迹点的跟踪控制，并未给出三维航迹跟踪的误差方程，无法实现对三维空间光滑曲线的跟踪；且跟踪导引策略为视线法(Line of Sight, L0S)而本专利采用的为虚拟向导策略(Virtual Guidance),通过跟踪期望路径上的“虚拟向导”点实现AUV收敛于期望路径；文献《基于离散滑模预测的欠驱动AUV三维航迹跟踪控制》(控制与决策，2011，第26卷第10期)中给出了在期望路径上虚拟向导坐标系下的AUV三维路径跟踪误差方程的形式，将AUV假定为虚拟质点，假设运动方向与合速度矢量方向一致，获得的三维路径跟踪误差方程需要AUV运动的侧漂角和攻角精确可量测，这在实际应用中是存在困难，只能够通过对横向和垂向运动速度的测量，进而计算出侧漂角和攻角，而由于沿三轴线速度的测量较困难，导致最终的控制器解算存在潜在的中断可能。《基于非线性迭代滑模的欠驱动UUV三维航迹跟踪控制》(自动化学报，2012，第38卷第2期)基于工程控制器解耦的思想设计非线性迭代滑模航迹跟踪控制器，由于被控对象模型为六自由度耦合运动模型，因此针对纵向速度、艏向控制和纵倾控制分别设计的解耦控制器只能通过鲁棒项抑制模型中的耦合作用，当模型各自由度之间的耦合作用较明显时，控制器只能通过输出较高的控制器增益为代价消除耦合作用，引起控制器输出饱和信号，解耦的控制器只能够保证三个独立控制子系统的渐近稳定性，而无法保证整个控制系统的渐近稳定性，而本专利提出的三维航迹跟踪控制器能够保证整个系统全局渐近稳定性；《自主式水下航行器三维路径跟踪的神经网络H00鲁棒自适应控制方法》(控制理论与应用，2012，第29卷第3期)基于正交投影Serret-Frenet坐标系建立AUV三维路径跟踪误差方程，运用H00鲁棒控制思想设计控制器，同时引入神经网络补偿模型不确定性，但由于基于正交投影Serret-Frenet坐标系建立AUV三维路径跟踪误差模型存在奇异值点，使得对AUV的初始条件有约束，即AUV初始位置必须位于跟踪曲线最小曲率半径内， 因此无法实现AUV跟踪的全局收敛性，而本专利建立在AUV载体坐标系下表示的三维跟踪误差模型不存在奇异值问题，因此能够保证AUV跟踪误差的全局收敛性，此外本专利采用迭代方法设计控制器不同于H00鲁棒控制设计思想。以上文献中涉及的方法均针对建立在Serret-Frenet坐标系下误差方程进行控制器设计，由于误差模型中的状态变量无法直接量测，导致控制系统对初始值精确性较为依赖，而本专利针对AUV载体坐标系建立三维路径跟踪误差方程，基于迭代方法设计三维路径跟踪控制器避免了采用传统反步法设计控制器时存在奇异值点，保证系统的全局收敛性。
发明内容
本发明的目的在于提供一种能提高路径跟踪精度的基于迭代的自主水下航行器三维路径跟踪控制方法。本发明的目的是这样实现的步骤I.初始化，给定三维空间期望跟踪路径参数化方程描述，给定AUV初始位置和姿态信息，给定期望跟踪路径参数的初始值，给定期望路径上“虚拟向导”初始位置和初始移动速度信息；步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差；步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律(如纵向移动速度、AUV的转艏角速度和纵倾角速度虚拟控制律)； 步骤4.在运动学等价控制律的基础上，采用迭代设计思想，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律，即根据AUV具体水动力数学模型解算出最终的执行指令信号(如推进器推力、纵倾控制力矩和转艏控制力矩)；步骤5.计算当前AUV位置n11 = (x, y, z)与标定的转向点WPk= (xk, yk, zk)之间的距离J = ^{x-xkf+(y-ykf+(z- J若小于设定的航迹切换半径R，则表示完成当前
指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。本发明相对现有技术具有如下的优点及效果I.基于AUV载体坐标系下建立三维路径跟踪误差方程，结合AUV的运动特性，避免了在期望路径上虚拟向导坐标系下的AUV三维路径跟踪误差方程时，将AUV假定为虚拟质点，假设运动方向与合速度矢量方向一致，获得的三维路径跟踪误差方程需要AUV运动的侧漂角和攻角精确可量测，这在实际应用中是存在困难，只能够通过对横向和垂向运动速度的测量，进而计算AUV的侧漂角和攻角，而由于沿三轴线速度的测量较困难，导致最终的控制器解算存在潜在的中断可能的不足。2.引入期望路径上“虚拟向导”点的移动速度作为额外的控制输入，保证实际应用 中在具有较大跟踪误差时，跟踪系统具有良好的动态性能，避免控制器输出较高的增益信号和推力饱和现象；采用迭代设计思想，将AUV三维路径跟踪控制系统，分为运动学和动力学两部分设计等价控制器，基于李雅普诺夫稳定性理论保证三维路径跟踪误差闭环系统的稳定性，且控制器对海洋环境作用引起的模型参数不确定性具有一定鲁棒性。
图I是本发明基于虚拟向导的AUV三维路径跟踪示意图。图2是本发明AUV三维路径跟踪控制器解算流程图。图3是本发明AUV三维路径跟踪非线性控制器框图。图4 10为本发明设计AUV三维曲线路径跟踪控制仿真对比曲线。从图4中可以看出本发明设计控制方法在AUV与期望跟踪三维路径初始距离较大时仍能实现精确跟踪控制，图5和图6分别为AUV三维跟踪轨迹在水平面和垂直面得投影曲线，可以看出跟踪三个方向上的跟踪误差逐渐减小，在图7中的AUV三维跟踪误差曲线最终收敛到零，验证了本发明设计控制方法的有效性；图8 9为AUV状态变量的变化曲线；图10为AUV控制输入响应曲线。
具体实施例方式下面举例对本发明做更详细的描述对于步骤I中给定期望跟踪路径Q上的虚拟向导P在固定坐标系的坐标可以表示为某一标量参数SGR的函数为⑷=[- ⑷，{s)J(I)为了保证被跟踪路径的光滑性，要求Xd(S)，yd(s)，Zd(S) 二阶偏导数存在。定义虚拟向导点P的速度Up方向为沿曲线路径的切线方向与固定坐标系水平轴的夹角Vd为￥d = arctan(y/ d/x/ d) (2)速度向量up与固定坐标系垂直轴的夹角0d定义为
权利要求
1.一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是 步骤I.初始化，给定三维空间期望跟踪路径参数化方程描述，给定AUV初始位置和姿态信息，给定期望跟踪路径参数的初始值，给定期望路径上“虚拟向导”初始位置和初始移动速度信息； 步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差； 步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律，包括纵向移动速度、AUV的转艏角速度和纵倾角速度虚拟控制律； 步骤4.在运动学等价控制律的基础上，采用迭代，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律，即根据AUV具体水动力数学模型解算出最终的执行指令信号，包括推进器推力、纵倾控制力矩和转艏控制力矩； 步骤5.计算当前AUV位置nn= (x, y, z)与标定的转向点WPk= (xk, yk, zk)之间的距离d =^{x-xk)2 + (y-yk f+(z-zk f ,若小于设定的航迹切换半径R，则表示完成当前指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。
2.根据权利要求I所述的基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是给定期望跟踪路径Q上的虚拟向导P在固定坐标系的坐标表示为某一标量参数sGR的函数为
3.根据权利要求I所述的基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是根据下式计算三维路径跟踪误差
4.根据权利要求I所述的基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是根据下式分别计算运动学虚拟控制器 (1)期望路径上虚拟向导点P的期望移动速度计算
5.根据权利要求I所述的基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是根据AUV具体水动力数学模型解算出最终的执行指令信号的具体过程为 根据AUV实测水动力系数，忽略横摇运动对模型的影响，得到AUV自由度数学模型 如下
全文摘要
本发明提供的是一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法。步骤1.初始化；步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差；步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律；步骤4.在运动学等价控制律的基础上，采用迭代，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律；步骤5.计算当前AUV位置ηn＝(x,y,z)与标定的转向点WPk＝(xk,yk,zk)之间的距离若小于设定的航迹切换半径R，则表示完成当前指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。本发明能够提高AUV的路径跟踪精度。
文档编号G05D1/10GK102768539SQ201210211449
公开日2012年11月7日 申请日期2012年6月26日 优先权日2012年6月26日
发明者于乐, 吕洪莉, 王宏健, 陈兴华, 陈子印 申请人:哈尔滨工程大学