CN107300925B - 基于改进鱼群算法的四旋翼无人机姿控参数整定方法 - Google Patents

Abstract

本发明涉及一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，在标准人工鱼群算法的基础上，提出了一种动态调整人工鱼移动步长的策略，引入精英保留、繁殖行为和外部捕鱼行为，克服现有技术中标准人工鱼群算法存在的后期收敛速度严重降低、收敛精度不高和易陷入局部最优的缺陷。该方法可用于解决飞行器设计、飞控参数整定和路径规划等问题，并具有较好的问题求解精度和效率。

Description

基于改进鱼群算法的四旋翼无人机姿控参数整定方法

技术领域

本发明涉及一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法,主要对典型四旋翼无人机自抗扰姿态控制器进行参数整定。

背景技术

四旋翼无人机自抗扰控制器虽然具有良好的抗干扰性能，但控制器参数众多，大量非线性函数的使用，使得控制器参数整定问题由于呈现多变量、非线性、多极值等问题，很难通过解析计算进行解决，需要结合性能指标建立问题描述模型，转化为函数优化问题采用具有强鲁棒性、全局寻优能力的智能优化算法进行求解。我国学者李晓磊等从鱼群仿生机理出发，提出了人工鱼群算法(Atrificial Fish Swarm Algorithm，ASFA)[李晓磊.一种新型的智能优化算法-人工鱼群算法[D].杭州：浙江大学博士学位论文，2003]，通过模拟鱼在水里的游动行为构建一种智能优化算法，用于解决优化问题，是一种鲁棒性强、收敛速度快、全局寻优和自适应能力强的新型群集智能优化算法，该算法一经提出，就以其概念简单、容易实现和需要调整的设计参数较少等优点被应用于解决面向众多对象的控制器参数整定问题。

但标准人工鱼群算法仍存在一些缺陷：前期收敛速度快，但后期收敛速度慢；寻优结果精度不高，易陷入局部最优。当前针对算法缺陷的改进研究也不断开展，通过改变步长和拥挤度因子以提高算法效率；采用自适应步长的方式提高后期寻优结果的精度；借鉴小生境思想改进人工鱼群算法将小生境排挤机制引入人工鱼群算法；将粒子群算法与人工鱼群算法相结合，以提高收敛速度等。以上的改进研究在一定程度上提高了人工鱼群算法的优化性能，但后期收敛速度慢、结果精度低、易陷入局部最优等问题仍然未得到很好地解决，需要进一步的改进完善，以提高控制器参数整定的效率和控制效果的最优精度。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，克服现有参数整定方法对四旋翼无人机自抗扰控制器最优参数求解收敛精度不高、后期收敛速度严重降低和易陷入局部最优的缺陷的问题。该方法在标准鱼群算法的基础上，通过动态调整人工鱼移动步长，引入精英保留、鱼群繁殖和外部捕鱼行对标准鱼群算法进行改进，加快控制器参数整定的收敛速度、提高全局搜索能力和最优参数的求解精度。

技术方案

一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，其特征在于步骤如下：

步骤1、建立典型四旋翼无人机俯仰姿态运动学模型为：

式中，p为无人机的滚动角速度；q为无人机的俯仰角速度；

为无人机的俯仰角加速度；r为无人机的偏航角速度；；I_y为无人机在y轴上的转动惯量；τ_y为无人机在y轴上的力矩；

将运动学模型式变换为积分链式模型形式：

式中，

Δf_θ为模型不确定性项；d_θ为外部干扰项；x₁＝θ为无人机的俯仰角；

由俯仰姿态运动学模型阶数为2，设计由三阶扩展观测器和PID控制构成的自抗扰控制器对四旋翼无人机俯仰姿态进行控制，自抗扰姿态控制器为：

扩展观测器：

PID控制：

自抗扰控制输出：u_ARDC＝u_pid-z₃/b

式中，θ_cmd为俯仰角控制指令；e为俯仰角误差；h为计算步长；z₁、z₂、z₃为扩展张观测器3个状态；β₀、β₁、β₂是扩展观测器观测系数；K_p、K_i、K_d为比例控制增益、积分控制增益和微分控制增益；

整定的自抗扰控制器参数为：X＝[K_p K_i K_d β₀ β₁ β₂]^T，将控制器参数看作优化设计变量；

选择ITAE评价指标为目标函数，

设置控制器仿真步长，计算时间，俯仰角初始状态和俯仰角控制指令；

步骤2，采用动态调整人工鱼移动步长策略，引入精英保留、鱼群繁殖和外部捕鱼行对标准鱼群算法进行改进，获得改进鱼群算法，过程为：

1、采用变步长状态更新：在聚群行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则:

X_c第i条人工鱼探索视觉感知范围内的其他人工鱼所处的中心状态；X_{i_next}为第i条人工鱼朝X_c移动后的状态改为变步长状态更新规则：

Step_i为自适应移动步长；γ为速度因子；

在追尾行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_imax为在第i条人工鱼探索视觉感知范围内食物浓度值最大的人工鱼所处状态。改为变步长状态更新规则：

在觅食行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_rand为第i条人工鱼当前状态X_i的视觉范围内随机产生一个觅食状态。改为变步长状态更新规则：

2、在标准鱼群算法单次迭代流程最后，引入精英保留、繁殖和人工捕鱼行为，提高收敛速度，增加跳出局部最优能力：

所述精英保留行为：将所有人工鱼按食物浓度从大到小进行排序，按照精英保留个数参数N_elite将排序后的前N_elite个人工鱼直接保留到下一次迭代，不参与后续行为；

所述繁殖行为：增加繁殖概率参数P_rep，通过抽取随机数，判断P_rep＜rand()是否成立不定期的触发繁殖行为。根据除精英个体以外的人工鱼的食物浓度计算每个人工鱼的选择概率

随机抽取满足

的两条人工鱼执行繁殖行为

获得一个新的人工鱼状态X_child，并替换当前最差食物浓度处的人工鱼状态，并计算食物浓度；

所述外界捕鱼行为：增加外部捕鱼概率参数P_catch，通过抽取随机数，判断P_catch＜rand()是否成立，不定期的触发外部捕鱼行为；根据捕鱼数量参数N_catch，对食物浓度排序队列中k＝N-1-N_catch,…,N_catch的人工鱼状态重新初始化，并计算各新状态的食物浓度；

步骤3、设置鱼群算法的人工鱼个数N、状态维数n、状态变量范围[Up_k,Down_k],i＝1,2,…,n，感知距离Visual、人工鱼移动的最大步长Step、速度因子γ、人工鱼拥挤度因子δ、人工鱼搜索食物浓度的尝试次数为try_number、最大迭代次数l_max、精英保留策略的鱼群个数为N_elite、繁殖概率为P_rep、精英保留策略的鱼群个数为N_elite、外界捕鱼概率为P_catch、捕鱼个数为N_catch、令食物浓度计算函数为Y＝-J(X)_ITAE；

步骤4、执行改进鱼群算法，进行控制参数整定，直至达到最大迭代次数，输出公告板上的食物浓度Y_board及人工鱼状态X_board；

步骤5、若目标函数值满足需求，则X_board即为最优姿态控制器参数；否则，返回步骤3，重新设置改进鱼群算法参数，执行改进鱼群算法，进行控制参数整定。

有益效果

本发明提出的一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，在标准人工鱼群算法的基础上，提出了一种动态调整人工鱼移动步长的策略，引入精英保留、繁殖行为和外部捕鱼行为，克服现有技术中标准人工鱼群算法存在的后期收敛速度严重降低、收敛精度不高和易陷入局部最优的缺陷。该方法可用于解决飞行器设计、飞控参数整定和路径规划等问题，并具有较好的问题求解精度和效率。

与现有技术相比，本发明的有益效果体现在：

1.变步长思想：算法在搜索过程中，前期人工鱼之间的距离较大，若要快速收敛，移动步长就需要相对大些；后期人工鱼已基本收敛，人工鱼之间距离较小，移动步长就要小些，从而不会使得人工鱼偏离收敛点，以提高其收敛速度。因此本发明采用变步长策略，由相对距离对移动步长进行调整，前期相对距离差距较大，则采用较大的移动步长，提高收敛速度。后期人工鱼逐渐接近最优位置，相对距离不断缩小，则减小移动步长，从而使人工鱼可以不断地逼近最优解，有效地避免某些人工鱼因移动步长较大而偏离最优解的情况，提高了算法的最优控制器参数求解精度。

2.本发明采用了避免陷入局部最优改进策略。原始算法在搜索过程中，只接受最优解，从而可能会陷入局部最优，无法达到全局最优。本发明引入繁殖和外部捕鱼行为，不仅接受最优解，而且还以一定概率分别从人工鱼内部和外部对一部分人工鱼状态进行重构，这样就可以使得算法不定期的跳出局部最优控制器参数，从而达到全局最优的控制器参数，增强了算法的跳出局部最优能力。

3.本发明引入了精英保留策略。在避免陷入局部最优的同时，将现有最优、次优控制器参数直接保留，可以保证前期计算的成果，避免重复搜索，浪费计算时间，加快控制器参数的整定收敛速度。

改进的整定方法具有更快的收敛速度、更好的控制器参数整定效率和控制器参数全局寻优能力。下面结合附图和具体实施方式对本发明作详细说明。

附图说明

图1是本发明方法的流程图。

图2是本发明方法进行四旋翼无人机姿控参数整定结构图。

图3是本发明方法中改进鱼群算法流程图。

图4是本发明方法中改进鱼群算的人工鱼聚群行为的流程图。

图5是本发明方法中改进鱼群算的人工鱼追尾行为的流程图。

图6是本发明方法中改进鱼群算的人工鱼觅食行为的流程图。

图7是本发明方法中改进鱼群算的人工鱼繁殖行为的流程图。

图8是本发明方法中改进鱼群算的人工鱼外部捕鱼行为的流程图。

图9是使用本发明方法对实施例2中自抗扰控制器参数整定过程图。

图10是使用本发明方法对实施例2中自抗扰控制器参数进行整定过程中每代最优控制器参数控制效果图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明解决其技术问题所采用的技术方案：一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，具体步骤如下：

步骤1、建立四旋翼无人机的姿态运动模型，设计自抗扰姿态控制器，确定待整定参数X＝[K_p K_i K_d β₀ β₁ β₂]^T，选择误差性能指标作为优化目标函数J＝f(X)将参数整定问题转化为参数优化问题。

步骤2、动态调整人工鱼移动步长，引入精英保留、鱼群繁殖和外部捕鱼行对标准鱼群算法进行改进，采用改进的鱼群算法对控制器参数进行优化，解决控制器参数整定问题。设置改进鱼群算法参数：人工鱼群F中鱼群个数为N，F＝(X₁,X₂,…,X_i,…,X_N)，1≤i≤N，每条人工鱼个体的状态X即代表一组控制器参数，K_p K_i K_d β₀ β₁ β₂为优化设计变量；X_i表示鱼群中的第i条人工鱼的当前状态，其由K_pi K_ii K_di β_0i β_1i β_2i组成，即为一组控制器参数值；由于人工鱼群算法是求食物浓度最大，而姿控希望误差指标函数最小，则人工鱼的食物浓度计算公式为Y＝-f(X)，其中f(X)为控制器参数整定目标函数值，反映了不同状态下对的控制器参数的姿控效果；设计变量的寻优范围[Down_k,Up_k]，k＝1,2,…6，[Down_k,Up_k]为第k个设计变量的寻优范围，Up_k为范围上界、Down_k为范围下界；人工鱼的视觉感知距离Visual；人工鱼移动的最大步长Step；速度因子γ；拥挤度因子δ。人工鱼搜索食物浓度的尝试次数try_number；最大迭代次数为l_max,l_max＞0；精英保留策略的鱼群个数为N_elite,N_elite＞1；繁殖概率为P_rep,0＜P_rep＜1；外界捕鱼概率为P_catch,0＜P_catch＜1，捕鱼个数为N_catch,0＜N_catch＜N；令人工鱼之间的距离计算公式为d_i,j＝||X_i-X_j||；当前迭代次数l＝1。

步骤3、鱼群初始化：

鱼群中的每条人工鱼均为一个向量，根据鱼群个数和设计变量维数，利用rand()随机函数产生n行，N列个0～1之间的随机数，结合设计变量范围[Up_k,Down_k]初始化N条人工鱼。

x_ki＝Down_k+rand()*(Up-Down_k) (1)

式中，rand()随机函数能生成一个0～1之间均匀分布的随机数。

步骤4、公告板初始化：

通过Y＝f(X)计算每条人工鱼当前状态的食物浓度为Y＝(Y₁,Y₂,…,Y_i,…Y_N)，将最大浓度值Y_board＝max(Y₁,Y₂,…,Y_i,…Y_N)和对应的人工鱼状态赋值X_board＝X_i if Y_i＝Y_board给公告板；

步骤5、执行单次迭代寻优：令当前更新状态人工鱼i＝1；

步骤6、执行聚群行为：

步骤6.1、设第i条人工鱼当前状态为X_i，使用式(2)计算第i条人工鱼X_i与第j条人工鱼之间的距离d_ij，得到第i条人工鱼X_i与鱼群中所有人工鱼之间的距离D_i＝(d_i1,d_i2,…,d_ij,…,d_iN)；

d_ij＝||X_i-X_j||1≤j≤N (2)

步骤6.2、获得位于第i条人工鱼探索视觉感知范围内的其他鱼群数目n_f，即满足d_ij≤Visaul的人工鱼个数以及n_f个人工鱼当前的状态

通过式(3)计算中心状态X_c:

步骤6.3、计算中心状态X_c的食物浓度Y_c＝f(X_c)，判断中心状态X_c的食物浓度Y_c是否大于第i条人工鱼X_i的食物浓度Y_i，且周围鱼群数量不太多满足式(4)，如均满足，则执行步骤6.4，否则执行步骤7；

步骤6.4、利用式(5)计算朝中心状态X_c移动的步长Step_i；

在人工鱼的标准算法聚群行为中,每一条人工鱼采用固定步长Step进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高，但收敛速度太慢。本发明提出变步长改进公式(5)，在同一次迭代中，对离更优值状态距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离更优值状态距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤6.5、利用式(5)计算第i条人工鱼移动后的状态X_{i_next}；

步骤7、执行追尾行为：

设人工鱼当前状态为X_i，探索视觉感知范围内(d_i,j＜Visaul)Y_imax值最大的人工鱼X_imax，如果

表明人工鱼X_j的状态中心位置的食物浓度较大，且周围鱼群数量不太多，则朝人工鱼X_j的方向前进一步；否则进行觅食。

步骤7.1、根据d_ij＜Visaul，得到第i条人工鱼X_i与鱼群中所有人工鱼之间的距离D_i＝(d_i1,d_i2,…,d_ij,…,d_iN)中位于第i条人工鱼探索视觉感知范围内的其他鱼群数目n_f以及n_f个人工鱼当前的状态的食物浓度

找到其中最大食物浓度

及食物浓度最大的人工鱼状态X_imax＝X_u if Y_iu＝Y_imaxwhere u＝1,…,n_f；

步骤7.2、根据d_maxj＝||X_max-X_j||≤Visaul,j＝1,…,N，获得所有距离位于X_imax状态人工鱼探索视觉感知范围内的其他鱼群数目m'；

步骤7.3、判断X_imax人工鱼所处状态的食物浓度Y_c是否大于第i条人工鱼X_i的食物浓度Y_i，且周围鱼群数量不太多，即满足式(7)，如均满足，则执行步骤7.4，否则执行步骤8；

步骤7.4、利用式(8)计算朝X_imax移动的步长Step_i；

在人工鱼的标准算法追尾行为中，每一条人工鱼采用固定步长Step进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进公式(8)，在同一次迭代中，对离当前搜索感知范围内最优值状态距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离当前搜索感知范围内最优值状态距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤7.5、利用式(9)计算第i条人工鱼移动后的状态X_{i_next}；

步骤8、执行觅食行为：

步骤8.1、令觅食尝试次数t＝0；

步骤8.2、利用rand()函数在第i条人工鱼当前状态X_i的视觉范围内随机产生一个觅食状态X_rand，并计算该状态的食物浓度Y_rand＝f(X_rand)；

步骤8.3、若ΔY＝Y_rand-Y_i＞0，则令移动补偿为

然后执行步骤8.4；否者执行步骤8.5；

在人工鱼的标准算法觅食行为中，每一条人工鱼采用固定步长Step进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进公式

在同一次迭代中，对离觅食状态X_rand距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离觅食状态X_rand距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤8.4、通过式(10)计算第i条人工鱼移动后的状态X_{i_next}，完成觅食动作；

步骤8.5、令t＝t+1；如果满足t＜try_number，执行步骤8.2；否者执行步骤8.6；

步骤8.6、通过式(11)，由随机向某方向移动最大步长，获得第i条人工鱼移动后的状态

步骤8.7、若Y_{i_next}＞Y_board，则将更新后的第i条人工鱼的食物浓度Y_{i_next}和状态X_{i_next}更新到公告板，即Y_board＝Y_{i_next},X_board＝X_{i_next}；

步骤9、选择状态更新人工鱼：

令i＝i+1；如果满足i≤N，则执行步骤4，否则执行步骤7；

步骤10、精英保留：

将所有人工鱼按食物浓度从大到小进行排序，将排序后的前N_elite个人工鱼直接保留到下一次迭代，前N_elite个人工鱼不参与后续行为。

通过增加精英保留策略，便于后续繁殖、捕鱼行为对除当前最优状态的人工鱼进行影响，保证前期计算的成果，避免重复搜索，浪费计算时间。

步骤11、繁殖行为：

步骤11.1、通过rand()函数根据繁殖概率，判断本代是否发生繁殖，如P_rep＜rand()，则执行步骤11.2；否者执行步骤12；

步骤11.2、根据排序的结果，根据除精英个体以外的人工鱼的食物浓度按照式(12)计算每个人工鱼的选择概率；

步骤11.3、通过两次rand()函数按照式(13)，选择出两条人工鱼进行繁殖；

步骤11.4、根据选择出的两条人工鱼当前状态，按照式(14)进行繁殖获得一个新的人工鱼状态X_child，并替换当前最差食物浓度处的人工鱼状态，并计算食物浓度。

在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加生物繁殖行为，以一定概率通过随机抽取当前鱼群内部的两个状态进行组合，对当前最差人工鱼状态进行替换，可以基于鱼群内部状态跳到新的状态进行搜索，防止陷入局部最优，进行全局最优搜索。

步骤12、外部捕鱼行为：

步骤12.1、通过rand()随机函数根据外界捕鱼概率，判断本代是否发生外界捕鱼，如P_catch＜rand()，则执行步骤12.2；否者执行步骤13；

步骤12.2、根据捕鱼数量N_catch，对食物浓度排序队列中k＝N-1-N_catch,…,N_catch的人工鱼状态按照式(1)进行更新，并计算各新状态的食物浓度；

在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加外部捕鱼行为，以一定概率直接在整个寻优空间中随机产生若干个新的状态，对当前若干个较差人工鱼状态进行替换，可以使部分搜索跳到更广阔的外部状态进行，从而跳出局部最优，进行全局最优搜索。

步骤13、若当前迭代次数l≤l_max，则令l＝l+1，并执行步骤5；否则执行步骤14；

步骤14、输出公告板上的食物浓度Y_board及人工鱼状态X_board，X_board即为最优姿态控制器参数。

步骤15、若目标函数值满足需求，则X_board即为最优姿态控制器参数。否则，返回步骤2，重新设置改进鱼群算法参数，执行步骤3～14。

下面结合附图1-10及两个实施例对本发明作进一步的详细描述，此处所描述的实施例仅用于说明和解释本发明，并不用于限定本发明。

实施例1利用测试函数验证改进鱼群算法的求解精度和收敛性

为验证改进算法在求解精度和收敛性方面的改进效果，选择标准测试函数二维Griewank函数、Schaffer函数及Ackley函数对改进前后的算法效果进行对比，基于三种函数计算方法对于就最小值的问题，得到食物浓度计算函数为Y＝f(X)＝-g_h(X)h＝G、S、A，三种函数的数学表达式及全局最优解由表1给出。

假设人工鱼群中有N条鱼，F＝(X₁,X₂,…,X_i,…,X_N)，1≤i≤N，每条人工鱼个体的状态X可用测试函数的参数X＝(x₁,x₂)为优化设计变量；[Down_k,Up_k]为第k个设计变量的寻优范围，Up_k为范围上界、Down_k为范围下界；X_i＝(x_1i,x_2i,…,x_ni)表示鱼群中的第i条人工鱼的当前状态；x_ki(k＝1,…,n,i＝1,…,N)表示第i条人工鱼状态中第k个设计变量的当前值；人工鱼当前位置的食物浓度用Y＝f(X)表示，其中Y为目标函数值，反映了不同状态下所解决问题模型的优劣程度；人工鱼之间的距离为d_i,j＝||X_i-X_j||；Visual表示人工鱼的视觉感知距离；Step表示人工鱼移动的最大步长；γ为速度因子；δ为拥挤度因子。人工鱼搜索食物浓度的尝试次数为try_number；

表1实施例测试函数具体信息表

本发明的改进鱼群方法与标准鱼群算法主要改进体现在：

(1)采用变步长状态更新，在聚群行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则:

X_c第i条人工鱼探索视觉感知范围内的其他人工鱼所处的中心状态；X_{i_next}为第i条人工鱼朝X_c移动后的状态改为变步长状态更新规则：

Step_i为自适应移动步长；γ为速度因子。

在追尾行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_imax为在第i条人工鱼探索视觉感知范围内食物浓度值最大的人工鱼所处状态。改为变步长状态更新规则：

在觅食行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_rand为第i条人工鱼当前状态X_i的视觉范围内随机产生一个觅食状态。改为变步长状态更新规则：

在标准鱼群算法的聚群、追尾、觅食行为中，每一条人工鱼采用固定步长进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进方法，在同一次迭代中，对离目标状态距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离目标状态距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

(2)在标准鱼群算法单次迭代流程最后，引入精英保留、繁殖和人工捕鱼行为，提高收敛速度，增加跳出局部最优能力。

精英保留行为将所有人工鱼按食物浓度从大到小进行排序，按照精英保留个数参数N_elite将排序后的前N_elite个人工鱼直接保留到下一次迭代，不参与后续行为。通过增加精英保留策略，便于后续繁殖、捕鱼行为对除当前最优状态的人工鱼进行影响，保证前期计算的成果，避免重复搜索，浪费计算时间。

繁殖行为通过增加繁殖概率参数P_rep，通过抽取随机数，判断P_rep＜rand()是否成立不定期的触发繁殖行为。根据除精英个体以外的人工鱼的食物浓度计算每个人工鱼的选择概率

随机抽取满足

的两条人工鱼执行繁殖行为

获得一个新的人工鱼状态X_child，并替换当前最差食物浓度处的人工鱼状态，并计算食物浓度。在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加生物繁殖行为，以一定概率通过随机抽取当前鱼群内部的两个状态进行组合，对当前最差人工鱼状态进行替换，可以基于鱼群内部状态跳到新的状态进行搜索，防止陷入局部最优，进行全局最优搜索。

外界捕鱼行为通过增加外部捕鱼概率参数P_catch，通过抽取随机数，判断P_catch＜rand()是否成立，不定期的触发外部捕鱼行为。根据捕鱼数量参数N_catch，对食物浓度排序队列中k＝N-1-N_catch,…,N_catch的人工鱼状态重新初始化，并计算各新状态的食物浓度；在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加外部捕鱼行为，以一定概率直接在整个寻优空间中随机产生若干个新的状态，对当前若干个较差人工鱼状态进行替换，可以使部分搜索跳到更广阔的外部状态进行，从而跳出局部最优，进行全局最优搜索。

图3给出了改进后鱼群算法的流程图，图4给出了改进后的人工鱼聚群行为的流程图。图5给出了改进后的人工鱼追尾行为的流程图。图6给出了改进后的人工鱼觅食行为的流程图。图7给出了改进后的人工鱼繁殖行为的流程图。图8给出了改进后的人工鱼外部捕鱼行为的流程图。

实施例1将本发明中改进鱼群算法与标准鱼群算法进行对比，结果可参照表2。其中标准鱼群算法的人工鱼个数N＝50、状态维数n＝2、状态变量范围[Up₁,Down₁]＝[-20,20],[Up₂,Down₂]＝[-20,20]、感知距离Visual＝5、人工鱼移动的最大步长Step＝0.1、人工鱼拥挤度因子δ＝0.6、人工鱼搜索食物浓度的尝试次数为try_number＝5、最大迭代次数l_max＝100。改进标准鱼群算法中相同的参数设置与标准鱼群算法一致，新增的算法参数设置为速度因子γ＝0.5，精英保留策略的鱼群个数为N_elite＝2,N_elite＞1；繁殖概率为P_rep＝0.2,0＜P_rep＜1；精英保留策略的鱼群个数为N_elite＝2,N_elite＞1；外界捕鱼概率为P_catch＝0.1,0＜P_catch＜1，捕鱼个数为N_catch＝10,0＜N_catch＜N。

实施例1中，利用每种算法对各测试函数重复运行30次得到对优化结果进行分析，记录其收敛次数和收敛时的迭代次数，得到它们的平均迭代次数及收敛成功率,收敛成功率＝收敛成功次数/30，表3给出了收敛性结果对比。

表3实施例的不同测试函数收敛性测试结果对比表

通过实施例1的收敛性测试结果可以看出，相对于标准鱼群算法，本发明中的改进鱼群算法在收敛性方面有很大提升。这也证明了本发明对标准鱼群算法改进的正确性。

在实施例1中，利用每种算法对各测试函数重复运行30次，记录其最优解及对应的人工鱼状态，并计算平均优化结果，表4给出了最优值优化求解结果对比。

表4实施例的不同测试函数最优值求解结果对比表

通过实施例1的收敛性测试结果可以看出，相对于标准鱼群算法，本发明中改进鱼群算法在最优值求解结果精度上有近2个数量级的提高，在最优解人工鱼状态求解精度上有近1个数量级的提高，在优化求解方面有很大提升。这也证明了本发明中对标准鱼群算法改进的正确性。

实施例2改进鱼群算法四旋翼无人机参数整定可行性验证

参照附图1～8，本发明方法的具体实施步骤如下：

步骤1、建立典型四旋翼无人机俯仰姿态运动学模型为：

式中，p为无人机的滚动角速度；q为无人机的俯仰角速度；

为无人机的俯仰角加速度；r为无人机的偏航角速度；；I_y为无人机在y轴上的转动惯量；τ_y为无人机在y轴上的力矩；

某型四旋翼飞行器的转动惯量为：I_x＝0.039kgm²、I_y＝0.039kgm²、I_z＝0.078kgm²Δf_θ＝0.01cos(0.8t)θ，，p＝0.05sin(0.5t)rad/s，r＝0.02cos(0.5t)rad/s，d_θ＝0.01rad/s

将式(1)变换为式(2)的积分链式模型形式：

式中，

Δf_θ为模型不确定性项；d_θ为外部干扰项。x₁＝θ为无 人机的俯仰角。

由俯仰姿态运动学模型阶数为2，设计由三阶扩展观测器和PID控制构成的自抗扰控制器对四旋翼无人机俯仰姿态进行控制，自抗扰姿态控制器为：

扩展观测器：

PID控制：

自抗扰控制输出：u_ARDC＝u_pid-z₃/b

式中，θ_cmd为俯仰角控制指令；e为俯仰角误差；h为计算步长；z₁、z₂、z₃为扩展张观测器3个状态；β₀、β₁、β₂是扩展观测器观测系数；K_p、K_i、K_d为比例控制增益、积分控制增益和微分控制增益。

需整定的自抗扰控制器参数为：X＝[K_p K_i K_d β₀ β₁ β₂]^T，将控制器参数看作优化设计变量。

选择ITAE评价指标为目标函数，

设置控制器仿真步长h＝0.01，设置计算时间为[010]秒，无人机俯仰角初始值为5°，θ_cmd＝0°。

步骤2、设置鱼群算法的人工鱼个数N＝50、状态维数n＝6、状态变量范围[Up₁,Down₁]＝[10,50]、[Up₂,Down₂]＝[0,10]、[Up₃,Down₃]＝[5,50]、[Up₄,Down₄]＝[500,2000]、[Up₅,Down₅]＝[3000,8000]、[Up₆,Down₆]＝[13000,16000]，感知距离Visual＝5、人工鱼移动的最大步长Step＝1、速度因子γ＝0.5、人工鱼拥挤度因子δ＝0.6、人工鱼搜索食物浓度的尝试次数为try_number＝5、最大迭代次数l_max＝20、精英保留策略的鱼群个数为N_elite＝2、繁殖概率为P_rep＝0.2、精英保留策略的鱼群个数为N_elite＝2、外界捕鱼概率为P_catch＝0.1、捕鱼个数为N_catch＝10、令食物浓度计算函数为Y＝-J(X)_ITAE。

步骤3、鱼群初始化：

结合实施例函数表达式可知鱼群中的每条人工鱼的状态均为一个6维向量，确定鱼群个数N＝50和设计变量维数n＝6，利用rand()函数产生n＝6行，N＝50列个0～1之间的随机数，结合设计变量范围[Up_k,Down_k],k＝1,…,n，初始化获得N＝50条人工鱼。

x_ki＝Down_k+rand()*(Up_k-Down_k)，k＝1,…,n,i＝1,…,N (1)

步骤4、公告板初始化：

通过Y＝f(X)计算每条人工鱼当前状态的食物浓度为Y＝(Y₁,Y₂,…,Y_i,…Y_N)，将最大浓度值Y_board＝max(Y₁,Y₂,…,Y_i,…Y_N)和对应的人工鱼状态赋值X_board＝X_i if Y_i＝Y_board给公告板；

步骤5、执行单次迭代寻优：令当前更新状态人工鱼i＝0；

步骤6、执行聚群行为：

步骤6.1、设第i条人工鱼当前状态为X_i，使用式(2)计算第i条人工鱼X_i与第j条人工鱼之间的距离d_ij，得到第i条人工鱼X_i与鱼群中所有人工鱼之间的距离D_i＝(d_i1,d_i2,…,d_ij,…,d_iN)；

d_ij＝||X_i-X_j||1≤j≤N (2)

步骤6.2、获得位于第i条人工鱼探索视觉感知范围内的其他鱼群数目n_f，即满足d_ij≤Visaul的人工鱼个数以及n_f个人工鱼当前的状态

通过式(3)计算中心状态X_c:

步骤6.3、计算中心状态X_c的食物浓度Y_c＝f(X_c)，判断中心状态X_c的食物浓度Y_c是否大于第i条人工鱼X_i的食物浓度Y_i，且周围鱼群数量不太多满足式(4)，如均满足，则执行步骤6.4，否则执行步骤7追尾行为；

步骤6.4、利用式(5)计算朝中心状态X_c移动的步长Step_i；

在人工鱼的标准算法聚群行为中,每一条人工鱼采用固定步长进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进公式(5)，在同一次迭代中，对离更优值状态距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离更优值状态距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤6.5、利用式(5)计算第i条人工鱼移动后的状态X_{i_next}；

步骤7、执行追尾行为：

设人工鱼当前状态为X_i，探索视觉感知范围内(d_i,j＜Visaul)Y_j值最大的人工鱼X_j，如果

表明人工鱼X_j的状态中心位置的食物浓度较大，且周围鱼群数量不太多，则朝人工鱼X_j的方向前进一步；否则进行觅食。

步骤7.1、根据d_ij＜Visaul，得到第i条人工鱼X_i与鱼群中所有人工鱼之间的距离D_i＝(d_i1,d_i2,…,d_ij,…,d_iN)中位于第i条人工鱼探索视觉感知范围内的其他鱼群数目n_z以及n_z个人工鱼当前的状态的食物浓度

找到其中最大食物浓度

及食物浓度最大的人工鱼状态X_imax＝X_u if Y_iu＝Y_imaxwhere u＝1,…,n_z；

步骤7.2、根据

获得所有距离位于X_imax状态人工鱼探索视觉感知范围内的其他鱼群数目m'；

步骤7.3、判断X_imax人工鱼所处状态的食物浓度Y_c是否大于第i条人工鱼X_i的食物浓度Y_i，且周围鱼群数量不太多，即满足式(7)，如均满足，则执行步骤7.4，否则执行步骤8觅食行为；

步骤7.4、利用式(8)计算朝X_imax移动的步长Step_i；

在人工鱼的标准算法追尾行为中，每一条人工鱼采用固定步长Step进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进公式(8)，在同一次迭代中，对离当前搜索感知范围内最优值状态距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离当前搜索感知范围内最优值状态距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤7.5、利用式(9)计算第i条人工鱼移动后的状态X_{i_next}；

步骤8、执行觅食行为：

步骤8.1、令觅食尝试次数t＝0；

步骤8.2、利用rand()函数在第i条人工鱼当前状态X_i的视觉范围内随机产生一个觅食状态X_rand，并计算该状态的食物浓度Y_rand＝f(X_rand)；

步骤8.3、若ΔY＝Y_rand-Y_i＞0，则令移动补偿为

然后执行步骤8.4；否者执行步骤8.5；

在人工鱼的标准算法觅食行为中，每一条人工鱼采用固定步长Step进行移动，若固定步长较大，则可能导致后期人工鱼跳离最优解，出现收敛速度慢、精度低问题。若固定步长较小，结果精度较高,但收敛速度太慢。本发明提出变步长改进公式

在同一次迭代中，对离觅食状态X_rand距离较远的个体使用较大的移动步长，加快其收敛速度，相反，对离觅食状态X_rand距离较近的个体使用较小的移动步长，避免产生振荡现象，提高寻优结果精度，通过速度因子γ可以控制速度随距离变化的快慢，这种变步长选择方式，既考虑到前期算法收敛速度，又提高了后期寻优精度。

步骤8.4、通过式(10)计算第i条人工鱼移动后的状态X_{i_next}，完成觅食动作；

步骤8.5、令t＝t+1；如果满足t＜try_number，执行步骤8.2；否者执行步骤8.6；

步骤8.6、通过式(11)，由随机向某方向移动最大步长，获得第i条人工鱼移动后的状态

步骤8.7、若Y_{i_next}＞Y_board，则将更新后的第i条人工鱼的食物浓度Y_{i_next}和状态X_{i_next}更新到公告板，即Y_board＝Y_{i_next},X_board＝X_{i_next}；

步骤9、选择状态更新人工鱼：

令i＝i+1；如果满足i＜N，则执行步骤4，否则执行步骤7；

步骤10、精英保留：

将所有人工鱼按食物浓度从大到小进行排序，将排序后的前N_elite＝2个人工鱼直接保留到下一次迭代，前N_elite＝2个人工鱼不参与后续行为。

通过增加精英保留策略，便于后续繁殖、捕鱼行为对除当前最优状态的人工鱼进行影响，保证前期计算的成果，避免重复搜索，浪费计算时间。

步骤11、繁殖行为：

步骤11.1、通过rand()函数根据繁殖概率，判断本代是否发生繁殖，如P_rep＜rand()，则执行步骤11.2；否者执行步骤12；

步骤11.2、由食物浓度排序的结果，根据除精英个体以外的人工鱼的食物浓度按照式(12)计算每个人工鱼的选择概率；

步骤11.3、通过两次rand函数按照式(13)，选择出两条人工鱼进行繁殖；

步骤11.4、根据选择出的两条人工鱼当前状态，按照式(14)进行繁殖获得一个新的人工鱼状态X_child，并替换当前最差食物浓度处的人工鱼状态，并计算食物浓度。

在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加生物繁殖行为，以一定概率通过随机抽取当前鱼群内部的两个状态进行组合，对当前最差人工鱼状态进行替换，可以基于鱼群内部状态跳到新的状态进行搜索，防止陷入局部最优，进行全局最优搜索。

步骤12、外界捕鱼行为：

步骤12.1、通过rand函数根据外界捕鱼概率，判断本代是否发生外界捕鱼，如P_catch＜rand()，则执行步骤12.2；否者执行步骤13；

步骤12.2、根据捕鱼数量N_catch，对食物浓度排序队列中k＝N-1-N_catch,…,N_catch的人工鱼状态按照式(1)进行更新，并计算各新状态的食物浓度；

在标准鱼群算法有收敛速度慢、陷入局部极值的缺陷，故在标准算法进入下一次迭代之前，增加外部捕鱼行为，以一定概率直接在整个寻优空间中随机产生若干个新的状态，对当前若干个较差人工鱼状态进行替换，可以使部分搜索跳到更广阔的外部状态进行，从而跳出局部最优，进行全局最优搜索。

步骤13、若当前迭代次数l＜l_max，则令l＝l+1，并执行步骤5；否则执行步骤14；

步骤14、输出公告板上的食物浓度Y_board及人工鱼状态X_board；

步骤15、若目标函数值满足需求，则X_board即为最优姿态控制器参数。否则，返回步骤2，重新设置改进鱼群算法参数，执行步骤3～14。

附图1给出了本发明方法进行控制器参数整定的流程图，附图2给出了应用本方法对四旋翼无人机自抗扰姿态控制器进行整定的结构图。附图3～8附图1中改进鱼群算法的执行流程图。附图9给出了实施例2的整定过程图，附图10给出了整定过程中每次当前最优控制器参数作用下的俯仰姿态控制曲线。

参照附图9可知目标函数值随迭代次数的增加不断减小，最终稳定在0.8附近。附图10中给出了每代当前最优控制器参数下的控制效果图，可知随着迭代次数的增加，最优控制参数不断更新，姿态控制效果不断提高。经过整定后获得最优控制器参数为：

K_p＝11、K_i＝1.2、K_d＝18.2、β₀＝1005、β₁＝5106、β₂＝15011

最优ITAE指标为：0.81，可见仅经过20次迭代即可完成无人机自抗扰控制器参数整定。本发明方法在标准鱼群算法的基础上，通过动态调整人工鱼移动步长，引入精英保留、鱼群繁殖和外部捕鱼行对标准鱼群算法进行改进，加快控制器参数整定的收敛速度、提高全局搜索能力和最优参数的求解精度。

Claims (1)

1.一种基于改进鱼群算法的四旋翼无人机姿控参数整定方法，其特征在于步骤如下：

步骤1、建立典型四旋翼无人机俯仰姿态运动学模型为：

式中，p为无人机的滚动角速度；q为无人机的俯仰角速度；

为无人机的俯仰角加速度；r为无人机的偏航角速度；I_y为无人机在y轴上的转动惯量；τ_y为无人机在y轴上的力矩；

将运动学模型式变换为积分链式模型形式：

式中，

Δf_θ为模型不确定性项；d_θ为外部干扰项；x₁＝θ为无人机的俯仰角；

由俯仰姿态运动学模型阶数为2，设计由三阶扩展观测器和PID控制构成的自抗扰控制器对四旋翼无人机俯仰姿态进行控制，自抗扰姿态控制器为：

扩展观测器：

PID控制：

自抗扰控制输出：u_ARDC＝u_pid-z₃/b

式中，θ_cmd为俯仰角控制指令；e为俯仰角误差；h为计算步长；z₁、z₂、z₃为扩展张观测器3个状态；β₀、β₁、β₂是扩展观测器观测系数；K_p、K_i、K_d为比例控制增益、积分控制增益和微分控制增益；

整定的自抗扰控制器参数为：X＝[K_p K_i K_d β₀ β₁ β₂]^T，将控制器参数看作优化设计变量；

选择ITAE评价指标为目标函数，

设置控制器仿真步长，计算时间，俯仰角初始状态和俯仰角控制指令；

步骤2，采用动态调整人工鱼移动步长策略，引入精英保留、鱼群繁殖和外部捕鱼行对标准鱼群算法进行改进，获得改进鱼群算法，过程为：

(1)采用变步长状态更新：在聚群行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则:

X_c为第i条人工鱼探索视觉感知范围内的其他人工鱼所处的中心状态；X_{i_next}为第i条人工鱼朝X_c移动后的状态改为变步长状态更新规则：

Step_i为自适应移动步长；γ为速度因子；

在追尾行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_imax为在第i条人工鱼探索视觉感知范围内食物浓度值最大的人工鱼所处状态；改为变步长状态更新规则：

在觅食行为中，将标准鱼群算法中采用固定步长Step进行状态更新的规则：

X_rand为第i条人工鱼当前状态X_i的视觉范围内随机产生一个觅食状态；改为变步长状态更新规则：

(2)在标准鱼群算法单次迭代流程最后，引入精英保留、繁殖和人工捕鱼行为，提高收敛速度，增加跳出局部最优能力：

所述精英保留行为：将所有人工鱼按食物浓度从大到小进行排序，按照精英保留个数参数N_elite将排序后的前N_elite个人工鱼直接保留到下一次迭代，不参与后续行为；

所述繁殖行为：增加繁殖概率参数P_rep，通过抽取随机数，判断P_rep＜rand()是否成立不定期的触发繁殖行为；根据除精英个体以外的人工鱼的食物浓度计算每个人工鱼的选择概率

随机抽取满足i_presel＝w,

的两条人工鱼执行繁殖行为

获得一个新的人工鱼状态X_child，并替换当前最差食物浓度处的人工鱼状态，并计算食物浓度；

所述人工捕鱼行为：增加外部捕鱼概率参数P_catch，通过抽取随机数，判断P_catch＜rand()是否成立，不定期的触发外部捕鱼行为；根据捕鱼数量参数N_catch，对食物浓度排序队列中k＝N-1-N_catch,…,N_catch的人工鱼状态重新初始化，并计算各新状态的食物浓度；

步骤3、设置鱼群算法的人工鱼个数N、状态维数n、状态变量范围[Up_k,Down_k],i＝1,2,…,n，感知距离Visual、人工鱼移动的最大步长Step、速度因子γ、人工鱼拥挤度因子δ、人工鱼搜索食物浓度的尝试次数为try_number、最大迭代次数l_max、精英保留策略的鱼群个数为N_elite、繁殖概率为P_rep、外界捕鱼概率为P_catch、捕鱼个数为N_catch、令食物浓度计算函数为Y＝-J(X)_ITAE；

步骤4、执行改进鱼群算法，进行控制参数整定，直至达到最大迭代次数，输出公告板上的食物浓度Y_board及人工鱼状态X_board；

步骤5、若目标函数值满足需求，则X_board即为最优姿态控制器参数；否则，返回步骤3，重新设置改进鱼群算法参数，执行改进鱼群算法，进行控制参数整定。

Images