一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法技术

本发明专利技术针对自主水下航行器的航向角控制问题，利用改进的萤火虫算法，提出一种基于改进萤火虫PID方法的自主水下航行器姿态控制方法，通过将PID控制参数用作萤火虫解空间的值，将错误性能用作改进萤火虫算法的评估函数，实现对自主水下航行器航向角PID控制器参数的自适应调整，仿真结果表明，改进后的算法解决了由于控制器参数选择不当造成的过冲和响应时间过长的问题，改进的萤火虫算法可用于自适应地调制PID控制参数。

【技术实现步骤摘要】
一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法
本专利技术涉及自主水下航行器控制
，具体为一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法。
技术介绍
随着海洋开发的不断深入，智能水下航行器(AUV)越来越以其体积小，无需电缆，运动灵活，潜水深度深，可进入复杂环境，并且不需要巨大的水面支撑，能够自主作业的优点而备受青睐，其已经成为海洋开发的重要工具。自治水下机器人(AUV)是一个复杂而精密的高技术集成体，涉及到多方面的科学技术；AUV作为控制对象具有非线性，各自由度之间相互耦合及时变的特点，被控对象的数学模型及其工作环境难以准确的描述，甚至有时给定的任务要求AUV负载可变，这就决定了一般的基于模型的经典控制方法应用于AUV控制时很难取得理想的控制效果。
技术实现思路
由于萤火虫PID控制不依赖于被控对象的精确数学模型，易于对不确定系统进行控制；同时相比于BP、RBF等神经网络，萤火虫PID控制实时性强，不需要离线训练参数，工程应用可操作性高；相比于不具有动态调整能力的模糊控制，具有很强的自适应性，同时又可以快速消除稳态误差。因此本专利技术基于改进萤火虫PID方法，提出了一种响应速度快，不依赖精确数学模型，能够自适应调整控制参数的自主水下航行器的航向角控制方法。本专利技术的基本原理是：首先将PID控制参数作为萤火虫在空间中的位置坐标，一个萤火虫即为萤火虫种群中的一个个体，然后对种群初始化，包括位置，吸引度与萤火虫个数等参数，再将萤火虫种群的每一只萤火虫个体代入PID控制器中，给定航向角期望值进行控制，控制过程结束后，计算出该个体对应的积分性能指标并作为适应度函数值，将种群中每一只萤火虫的适应度函数值对比，适应度函数值最小的作为最优解，判断种群迭代次数是否达到预设值，未达到预设值则完成萤火虫的位置更新后，重复上述过程，达到预设值则将适应度函数值最小的萤火虫位置作为控制器的自适应PID控制参数，用以自主水下航行器的航向角控制。本专利技术的技术方案为：所述一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法，以自主水下航行器当前航向角与目标航向角偏差和偏差变化率作为PID控制器输入，以自主水下航行器的航向执行器控制量为PID控制器输出，其特征在于：所述PID控制器的控制参数Kp、Ki、Kd通过以下步骤得到：步骤1：设置萤火虫初始种群，种群内萤火虫个数为N，并初始化单个萤火虫个体j在解空间中的位置步骤2：取萤火虫当前种群中的某个萤火虫个体的位置作为PID控制器的控制参数Kp、Ki、Kd，以自主水下航行器当前航向角与目标航向角偏差和偏差变化率作为PID控制器输入，利用PID控制器解算得到自主水下航行器的航向执行器控制量δ步骤3：将航向执行器控制量δ输入AUV状态模型，得到输出航向角，并以输出航向角与目标航向角的偏差作为当前时刻t的系统误差输出e(t)；步骤4：在一个设定的控制时长T内重复步骤2和步骤3，控制步长为dt，根据公式计算得到当前萤火虫个体的适应度J；步骤5：重复步骤2到步骤4，计算得到萤火虫当前种群中每个萤火虫个体的适应度，然后进行收敛性判断，若满足下列条件之一，则可停止迭代，得到萤火虫个体位置最优解，并以位置最优解作为PID控制器的控制参数Kp、Ki、Kd，否则进入步骤6；条件1：当前种群中个体适应度最小值小于设定值，则停止迭代，取当前种群中适应度值最小的个体作为最优解；条件2：种群迭代次数达到设定的最大迭代次数，则取当前种群中适应度值最小的个体作为最优解；步骤6：进行萤火虫种群更新，然后返回步骤2。进一步的，步骤6中进行萤火虫种群更新的过程为：对种群中的萤火虫个体按适应度进行排序；然后除了适应度最小的萤火虫个体，其余萤火虫个体按照以下过程进行位置更新：对于某一萤火虫个体j，按照公式计算该萤火虫个体被某一比其适应度低的萤火虫个体i吸引后的更新位置其中k为种群迭代次数，βij(rij)为萤火虫i对萤火虫j的吸引力，rij为萤火虫i到萤火虫j的笛卡尔距离，α为处于[0，1]范围内的设定值，为随机数向量；通过和得到萤火虫j被萤火虫i所吸引的位置矢量，相应的能够得到萤火虫j被所有适应度比其低的萤火虫个体所吸引的位置矢量，对这些位置矢量计算矢量合，得到矢量合的终点位置作为萤火虫个体j的更新位置进一步的，步骤6中进行萤火虫种群更新时，对于适应度最小的萤火虫个体则施加一个随机位置小扰动，得到更新位置。进一步的，对于更新位置公式中的设定值α，通过公式α(k+1)＝α(k)exp(-(1/4)(k/K))进行更新，其中K为设定的最大迭代次数。进一步的，萤火虫i对萤火虫j的吸引力βij(rij)根据公式计算得到，其中β0为最大吸引度，γ为光强吸引系数。有益效果本专利技术针对自主水下航行器的航向角控制问题，利用改进的萤火虫算法，提出一种基于改进萤火虫PID方法的自主水下航行器姿态控制方法，通过将PID控制参数用作萤火虫解空间的值，将错误性能用作改进萤火虫算法的评估函数，实现对自主水下航行器航向角PID控制器参数的自适应调整，仿真结果表明，改进后的算法解决了由于控制器参数选择不当造成的过冲和响应时间过长的问题，改进的萤火虫算法可用于自适应地调制PID控制参数。本专利技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本专利技术的实践了解到。附图说明本专利技术的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：图1：AUV坐标系示意图；图2：控制系统结构图；图3：流程图；图4:航向跟踪仿真图。具体实施方式下面详细描述本专利技术的实施例，所述实施例是示例性的，旨在用于解释本专利技术，而不能理解为对本专利技术的限制。本专利技术主要针对自主水下航行器的航向角控制问题，利用改进的萤火虫算法，提出一种基于改进萤火虫PID方法的自主水下航行器姿态控制方法，通过将PID控制参数用作萤火虫解空间的值，将错误性能用作改进萤火虫算法的评估函数，实现对自主水下航行器航向角PID控制器参数的自适应调整，仿真结果表明，改进后的算法解决了由于控制器参数选择不当造成的过冲和响应时间过长的问题，改进的萤火虫算法可用于自适应地调制PID控制参数。对于自主水下航行器的控制问题，首先还是介绍AUV数学模型的建立。建立如图1所示的坐标系，(N,E,D)是以东北为标准的固定坐标系，而(x,y,z)是以AUV重心为原点的运动坐标系。参照水下机器人六自由度运动学的数学模型，可以得到水下机器人水平部分的运动模型。AUV水平运动方程可描述为：在公式中，X,Y是分力，N是外部扭矩，v和u是分速度，r是机器人回转的角速度。AUV水平动力学方程可描述为：...

【技术保护点】
1.一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法，以自主水下航行器当前航向角与目标航向角偏差和偏差变化率作为PID控制器输入，以自主水下航行器的航向执行器控制量为PID控制器输出，其特征在于：所述PID控制器的控制参数K

【技术特征摘要】
1.一种基于改进萤火虫PID方法的自主水下航行器航向角控制方法，以自主水下航行器当前航向角与目标航向角偏差和偏差变化率作为PID控制器输入，以自主水下航行器的航向执行器控制量为PID控制器输出，其特征在于：所述PID控制器的控制参数Kp、Ki、Kd通过以下步骤得到：
步骤1：设置萤火虫初始种群，种群内萤火虫个数为N，并初始化单个萤火虫个体j在解空间中的位置
步骤2：取萤火虫当前种群中的某个萤火虫个体的位置作为PID控制器的控制参数Kp、Ki、Kd，以自主水下航行器当前航向角与目标航向角偏差和偏差变化率作为PID控制器输入，利用PID控制器解算得到自主水下航行器的航向执行器控制量δ
步骤3：将航向执行器控制量δ输入AUV状态模型，得到输出航向角，并以输出航向角与目标航向角的偏差作为当前时刻t的系统误差输出e(t)；
步骤4：在一个设定的控制时长T内重复步骤2和步骤3，控制步长为dt，根据公式



计算得到当前萤火虫个体的适应度J；
步骤5：重复步骤2到步骤4，计算得到萤火虫当前种群中每个萤火虫个体的适应度，然后进行收敛性判断，若满足下列条件之一，则可停止迭代，得到萤火虫个体位置最优解，并以位置最优解作为PID控制器的控制参数Kp、Ki、Kd，否则进入步骤6；
条件1：当前种群中个体适应度最小值小于设定值，则停止迭代，取当前种群中适应度值最小的个体作为最优解；
条件2：种群迭代次数达到设定的最大迭代次数，则取当前种群中适应度值最小的个体作为最优解；
步骤6：进行萤火虫种群更新，然后返回步骤2。


2.根据权利要求1所述一种...

【专利技术属性】
技术研发人员：刘明雍，游粮根，焦振宏，牛云，向举苗，赵格睿，汪培新，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61