[发明专利]一种用于水下自主航行器的航迹控制方法

说明书

本发明提供了一种用于水下自主航行器的航迹控制方法，计算当前位置到规划路径直线的垂直距离，得到航行路径偏移量的控制量，计算水下航行器到达目标点的航向偏差和AUV到达目标点的航向控制量，将控制量引入到航向控制中，通过航向角总控制量根据各电机的位置进行推力分配，得到各电机的控制量，实现航迹航行的精确控制。本发明采用的水下自主航行器的航迹控制方法可在有海流的情况下实现水下自主航行器的精确航迹跟踪控制，采用的模糊PID控制算法对PID控制参数进行优化调整，对航行控制具有较好的自适应能力和较强的鲁棒性，本发明方法简单可行，工作可靠，可有效提高海流条件下水下自主航行器的航迹跟踪精度。

技术领域

本发明涉及水下航行器领域，尤其是用于水下自主航行器(AUV,AutonomousUnderwater Vehicle)的航行控制方法。

背景技术

认识、开发和利用海洋是建设海洋强国的基础，在各种海洋技术中，水下自主航行器能在一般潜航器不能达到的水域自主进行工作，被广泛应用在科研考察、水下作业和军事活动中。

传统的水下自主航行器通常采用目标点闭环的方式来按规划的航路航行，即目标航向角始终指向航行目标点，如图1所示，图中MN表示规划的路径，N为航行路径目标点，L₁表示在静水水域航行时的航迹，L₂表示在有海流情况下,传统的目标点闭环控制方法所实现的航迹，如果按照该航迹航行，水下航行器就会在海流的作用下偏离规划的路径，无法完成航迹精确跟踪任务。例如，在狭窄水域航行或海底地形地貌的全覆盖探测时,就要求水下自主航行器严格按照规划的航路航行。

PID控制算法在各种控制方法中发展最为成熟，控制器设计可以不基于系统模型，被广泛应用在工业界各种控制系统中，目前水下航行器普遍采用该算法进行航行控制，但该控制器参数一旦确定将不会跟随系统状态进行改变，不具有自调整特性，因而很多时候不能满足控制需求。

发明内容

为了克服现有技术中水下自主航行器的航迹精确跟踪控制方面的不足，本发明提供一种用于水下自主航行器的航迹控制方法。

本发明解决其技术问题所采用的技术方案是包括以下步骤：

(1)计算当前位置到规划路径直线的垂直距离ΔL,即目前沿规划航路航行的偏差ΔL；计算方法为：

当前位置A的坐标为(x,y,z),规划路径直线的方向向量为M，规划路径上的一个点B的坐标为(x₁,y₁,z₁)，向量AB＝(x,y,z)-(x₁,y₁,z₁)，则ΔL＝|AB+M|；

(2)计算出航行路径偏移量的控制量ΔL_控制；

采用PID算法计算出航行路径偏移量的控制量ΔL_控制如下：

式(1)中，K_p为比例系数，K_i为积分系数，K_d为微分系数，ΔL为当前位置到规划路径的距离偏差，t为积分时间，dt为微分时间；

计算机控制系统对上式进行离散化处理，离散后的控制量ΔL_控制为：

式(2)中，ΔL_m为第m个控制周期当前位置到规划路径的距离偏差；

(3)计算水下航行器到达目标点的航向偏差ΔH；