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

权利要求书

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_控制表示沿精确航迹航行的航向角总控制量，将航向角总控制量根据各电机的位置进行推力分配，得到各电机的控制量，实现航迹航行的精确控制。