[发明专利]一种基于航向平滑技术的自主水下机器人路径跟踪方法

说明书

一种基于航向平滑技术的自主水下机器人路径跟踪方法，本发明涉及自主水下机器人路径跟踪方法。本发明的目的是为了解决现有AUV路径跟踪控制中艏向穿越‑π/π不连续点的情况时，路径跟踪准确率低的问题。过程为：一、测量初始时刻AUV的状态测量值，设置AUV的期望路径；二、得到AUV的路径跟踪误差；三、得到优化后的路径跟踪误差；四、使优化后的路径跟踪误差收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行二到五，直至AUV走完跟踪路径。本发明用于自主水下机器人路径跟踪领域。

技术领域

本发明涉及自主水下机器人路径跟踪方法。

背景技术

近几十年来，随着我国的经济与科技实力的飞速发展，海洋在国家战略上的地位也显著提升。自主水下机器人(Autonomous underwater vehicle,AUV)因为其能够显著的减少水下作业的危险性和作业时间的特点，受到了世界各国的关注。目前已经有许多AUV成功的应用于海底石油管道检测、海底水文检测等任务中去，诸如此类的应用促使有关AUV的研究成为系统科学与机器人学中的一个活跃的领域^[1](Cabezas J M,Pastor E,Camacho LM,et al.I-AUV Mechatronics Integration for the TRIDENT FP7Project[J].IEEE/ASME Transactions on Mechatronics,2015,20(5):2583-2592.)。

在AUV实际执行任务时，路径跟踪能力是衡量AUV智能化程度的一个重要指标。由于AUV复杂的系统动力学特性和不可预测的复杂水下环境，难以应用高精度的基于模型的控制方法。且AUV大多采用欠驱动推进方案来提高续航能力，即采取舵桨作为控制输入，因此三自由度的路径控制则更为复杂。因此实际工程中的AUV路径跟踪控制中一般采用视线引导法将原本的路径跟踪问题转化为参考航向跟踪问题。AUV的航向角的定义域一般为(-π,π]，当参考航向与AUV当前航向之间的差值穿过了-π/π的不连续点时，则会导致参考航向与AUV航向动态不一致的现象，尤其是进行路径跟踪任务时，AUV可能会多次圆周航行，则一定会出现穿越-π/π的交界点的时刻，这时候就会出现一些不良的跟踪效果。例如，AUV沿着圆周进行运动，从圆周顶点开始运动，艏向向西，此时AUV的艏向为π，与此同时，随着AUV前进，参考点期望角会从π跳跃到-π，从而导致AUV的突然转向。

综上，导致AUV路径跟踪控制中艏向穿越-π/π(-180°～180°)不连续点的情况时，路径跟踪准确率低。

发明内容

本发明的目的是为了解决现有AUV路径跟踪控制中艏向穿越-π/π不连续点的情况时，路径跟踪准确率低的问题，而提出一种基于航向平滑技术的自主水下机器人路径跟踪方法。

一种基于航向平滑技术的自主水下机器人路径跟踪方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

步骤三、对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)；