CN111930132A - 一种用于水下自主航行器的航迹控制方法 - Google Patents

Abstract

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

Description

一种用于水下自主航行器的航迹控制方法

技术领域

本发明涉及水下航行器领域，尤其是用于水下自主航行器(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；

首先计算到达本段路径目标点的航向H,即目标航向H，当前位置A的坐标为(x,y,z),目标点N的坐标为(x₀,y₀,z₀)，目标航向H＝(x，y，z)-(x₀，y₀，z₀)，然后计算航向的偏差量ΔH，当前航向H₁由姿态传感器可得ΔH＝H-H₁；

(4)计算AUV到达目标点的航向控制量ΔH_控制；

H₀表示到达本段路径目标点的航向角，H表示安装在AUV上的航向传感器测量的实时航向角,ΔH表示到达目标点航向角偏差；

采用模糊PID算法计算出到达目标点的航向控制量ΔH_控制，具体算法如下：

将控制器参数引入一个调整量，使原始参数改变为：

式(3)中，K_p为比例系数，K_i为积分系数，K_d为微分系数，ΔK_p为模糊算法得到的比例系数调整量，ΔK_i为模糊算法得到的积分系数调整量，ΔK_d为模糊算法得到的微分系数调整量，K_p1为经过模糊算法得到的比例系数调整量与原PID参数相加后的比例系数，K_i1为经过模糊算法得到的比例系数调整量与原PID参数相加后的积分系数，K_d1为经过模糊算法得到的比例系数调整量与原PID参数相加后的微分系数；

采用PID算法计算出到达目标点的航向控制量ΔH_控制如下：

离散后的PID控制算法为：

式(5)中，ΔH_m为第m个控制周期到达目标点的航向偏差，T为离散的时间间隔；

选定航向误差ΔH及其导数ΔH_c作为模糊控制器的输入量，设计模糊控制规则如表1所示，NB，NM，NS，ZE，PS，PM，PB分别代表负大、负中、负小、零、正小、正中、正大；

表1模糊控制规则

根据制定的模糊控制规则并利用模糊推理的方法进行去模糊化，得到ΔK_p，ΔK_i，ΔK_d的值，再根据式(3)计算得到K_p1，K_i1，K_d1，然后根据式(5)计算得到AUV到达目标点的航向控制量ΔH_控制；

(5)将控制量ΔL_控制引入到航向控制中；

将步骤2计算的控制量ΔL_控制引入到航向控制回路中，ΔL_控制+ΔH_控制表示沿精确航迹航行的航向角总控制量，将航向角总控制量根据各电机的位置进行推力分配，得到各电机的控制量，实现航迹航行的精确控制。

本发明的有益效果在于：

1)本发明采用的水下自主航行器的航迹控制方法可在有海流的情况下实现水下自主航行器的精确航迹跟踪控制。

2)本发明方法采用的模糊PID控制算法对PID控制参数进行优化调整，对航行控制具有较好的自适应能力和较强的鲁棒性。

3)本发明方法简单可行，工作可靠，可有效提高海流条件下水下自主航行器的航迹跟踪精度。

附图说明

图1为根据本发明的水下航行器航行路径示意图.

图2为根据本发明的水下航行器目标定向航向角控制框图。

图3为根据本发明的模糊PID算法控制框图。

图4为根据本发明的水下航行器精确航迹控制框图。

图5为根据本发明的水下航行器精确航迹控制程序流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

计算水下航行器当前位置到规划路径的距离ΔL，将距离ΔL带入PID算法中得到航行路径偏移量的控制量ΔL_控制；计算水下航行器到达目标点的航向偏差ΔH，将航向偏差ΔH带入模糊PID算法中得到水下航行器到达目标点的航向控制量ΔH_控制；将路径偏移控制量ΔL_控制与航向控制量ΔH_控制的和作为总控制量，根据水下航行器的推进器布置情况将控制量分配至各螺旋桨电机，以实现水下航行器的航迹控制。

本发明的实现步骤为：

(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；

首先计算到达本段路径目标点的航向H,即目标航向H，当前位置A的坐标为(x,y,z),目标点N的坐标为(x₀,y₀,z₀)，目标航向H＝(x，y，z)-(x₀，y₀，z₀)，然后计算航向的偏差量ΔH,当前航向H₁由姿态传感器可得ΔH＝H-H₁；

(4)计算AUV到达目标点的航向控制量ΔH_控制；

为了到达目标点，在航行过程中需要不断调整航向角。航向角的控制框图如

图2所示。图2中H₀表示到达本段路径目标点的航向角,H表示安装在AUV上的航向传感器测量的实时航向角,ΔH表示到达目标点航向角偏差。

采用模糊PID算法计算出到达目标点的航向控制量ΔH_控制，模糊PID算法控制框图如图3所示，模糊PID控制器是将模糊算法与PID控制参数的自整定相结合的一种控制算法。具体算法如下：

将控制器参数引入一个调整量，使原始参数改变为：

式(3)中，K_p为比例系数，K_i为积分系数，K_d为微分系数，ΔK_p为模糊算法得到的比例系数调整量，ΔK_i为模糊算法得到的积分系数调整量，ΔK_d为模糊算法得到的微分系数调整量，K_p1为经过模糊算法得到的比例系数调整量与原PID参数相加后的比例系数，K_i1为经过模糊算法得到的比例系数调整量与原PID参数相加后的积分系数，K_d1为经过模糊算法得到的比例系数调整量与原PID参数相加后的微分系数；

采用PID算法计算出到达目标点的航向控制量ΔH_控制如下：

离散后的PID控制算法为：

式(5)中，ΔH_m为第m个控制周期到达目标点的航向偏差，T为离散的时间间隔。

选定航向误差ΔH及其导数ΔH_c作为模糊控制器的输入量，设计模糊控制规则如表1所示，NB，NM，NS，ZE，PS，PM，PB分别代表负大、负中、负小、零、正小、正中、正大。

表1模糊控制规则

根据制定的模糊控制规则并利用模糊推理的方法进行去模糊化，得到ΔK_p，ΔK_i，ΔK_d的值，再根据式(3)计算得到K_p1，K_i1，K_d1，然后根据式(5)计算得到AUV到达目标点的航向控制量ΔH_控制；

(5)将控制量ΔL_控制引入到航向控制中；

将步骤2计算的控制量ΔL_控制引入到航向控制回路中，图4中“ΔL_控制+ΔH_控制”表示沿精确航迹航行的航向角总控制量，然后将航向角总控制量根据各电机的位置进行推力分配，得到各电机的控制量，实现航迹航行的精确控制。

在主控计算机上，用C语言编程实现水下航行器精确航迹控制，其程序流程图如图5所示。

Claims (1)

1.一种用于水下自主航行器的航迹控制方法，其特征在于包括下述步骤：

(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；

首先计算到达本段路径目标点的航向H，即目标航向H，当前位置A的坐标为(x，y，z)，目标点N的坐标为(x₀，y₀，z₀)，目标航向H＝(x，y，z)-(x₀，y₀，z₀)，然后计算航向的偏差量ΔH，当前航向H₁由姿态传感器可得ΔH＝H-H₁；

(4)计算AUV到达目标点的航向控制量ΔH_控制；

H₀表示到达本段路径目标点的航向角，H表示安装在AUV上的航向传感器测量的实时航向角，ΔH表示到达目标点航向角偏差；

采用模糊PID算法计算出到达目标点的航向控制量ΔH_控制，具体算法如下：

将控制器参数引入一个调整量，使原始参数改变为：

式(3)中，K_p为比例系数，K_i为积分系数，K_d为微分系数，ΔK_p为模糊算法得到的比例系数调整量，ΔK_i为模糊算法得到的积分系数调整量，ΔK_d为模糊算法得到的微分系数调整量，K_p1为经过模糊算法得到的比例系数调整量与原PID参数相加后的比例系数，K_i1为经过模糊算法得到的比例系数调整量与原PID参数相加后的积分系数，K_d1为经过模糊算法得到的比例系数调整量与原PID参数相加后的微分系数；

采用PID算法计算出到达目标点的航向控制量ΔH_控制如下：

离散后的PID控制算法为：

式(5)中，ΔH_m为第m个控制周期到达目标点的航向偏差，T为离散的时间间隔；

选定航向误差ΔH及其导数ΔH_c作为模糊控制器的输入量，设计模糊控制规则如表1所示，NB，NM，NS，ZE，PS，PM，PB分别代表负大、负中、负小、零、正小、正中、正大；

表1 模糊控制规则

根据制定的模糊控制规则并利用模糊推理的方法进行去模糊化，得到ΔK_p，ΔK_i，ΔK_d的值，再根据式(3)计算得到K_p1，K_i1，K_d1，然后根据式(5)计算得到AUV到达目标点的航向控制量ΔH_控制；

(5)将控制量ΔL_控制引入到航向控制中；

将步骤2计算的控制量ΔL_控制引入到航向控制回路中，ΔL_控制+ΔH_控制表示沿精确航迹航行的航向角总控制量，将航向角总控制量根据各电机的位置进行推力分配，得到各电机的控制量，实现航迹航行的精确控制。

Images