一种基于鸽群优化的四旋翼无人机显式预测控制方法与流程

本发明是一种基于启发式仿生鸽群优化的四旋翼无人机显式预测控制方法，属于飞行器控制领域。

背景技术：

无人机(Unmanned Aerial Vehicle,UAV)在当今军事行动中扮演越来越重要的角色，能够有效完成复杂和危险的侦查和作战任务。UAV虽然在军事和民用等领域已有广泛应用，但其自身的很多关键技术尚有待进一步研究和应用。而四旋翼作为一种更便宜可靠性更高，可以垂直起降的一种无人机，正被广泛的商业和军事运用，然而由于四旋翼的多维度性，由四个马达控制起飞悬停转向，因此其对控制率要求比较高。

模型预测控制可以实现对被控对象的精确控制，但有些模型预测控制依赖在线的优化过程，比较耗时。对于有些线性模型预测控制问题来说，可以求取其最优控制的显式解，但对于非线性问题，求取显式解比较困难。显示非线性模型预测控制(Explicit Nonlinear Model Predictive Control，ENMPC)应用泰勒展开原理，可以求取非线性模型预测控制的显式解。其主要控制思想是要求轨迹——逆推预测控制量——输出轨迹。而本专利申请将通过简化轨迹来简化预测过程，从而大大降低复杂难度。但是其自身有一定的复杂度，而且对于复杂轨迹和散点轨迹并不能进行微分操作。所以我们可以通过插值方法求其估计的线性轨迹。

群体智能是仿生智能的一个重要分支，人们通过对自然界生物群体的观察，受到自然界中生物群体行为的启发，在此基础上总体提升，将其行为模式用数学的方式描述出来。在群体智能模型的基础上，人们提出了群体智能优化算法的概念，用生物群体的行为模式来求解优化问题。启发式仿生鸽群优化算法(Pigeon Inspired Optimization，PIO)是HaibinDuan和PeixinQiao在2014年提出的一种新型的启发式群智能优化算法，该算法受到鸽子群体行为的启发，根据鸽子在寻找目标的过程中，先后依据磁场和地标作为指示的行为特点，建立起地图罗盘和地标两种算法机制。

鸽群在寻找目的地的过程中，会先参照太阳和磁场进行初步定位，然后依照地标进行精确定位，根据这一特性，鸽群优化算法提出了两种相对应的算子，分别为地图罗盘算子和地标机制，来模拟鸽群的这种特性，并将这两种算子结合起来解决优化问题。

传统的控制方式主要是通过将模型线性化来进行控制，而显式预测控制可以实现非线性模型的控制，但是复杂度较高，而本方法保持模型原样而将要求轨迹线性化来实现控制，大大降低控制预测模型控制难度，而又很好的保持了模型的方程。

(1)非线性显式预测控制

在非线性显式预测中，是通过将非线性部分泰勒展开，用提前预测要求量来进行控制，其主要思想如下：

对于一个非线性模型：

其中，x代表状态量,y代表输出量，f(x,u),h(x)为两个函数。

设ρ_i代表经过微分在状态方程中出现第i个控制量的次数，用r代表输入的序号。对上式每个变量分别进行泰勒展开可以得到

τ是预测控制中的预测时间,是第i个估计量，t是当前时刻，ρi为第i个控制量出现在状态中需要泰勒展开的阶数，r为继续展开的相对阶数。为泰勒展开的系数矩阵，y_i(t)为第i个控制量在t时刻的值，Y_i(t)为由y_i(t)及其各阶导数组成的矩阵。

将多个变量和在一个矩阵中，于是上式整理为：

其中为第i个量的泰勒展开系数。

将有控制量的和没有控制量的状态变量分离，重新整理矩阵，可以得到下式：

其中符号定义如下：

而其中在多次微分后出现控制量的部分，可以将其展开如下：

其中为对第i个没有输入控制量的ρi次微分，A(x)为控制量到状态量的函数，u为控制输入。i＝1,2,...,n；

将上式对时间进行微分，可以得到如下：

其中p₁(x,u)为非线性部分的输入。

用该方法反复进行微分如下，直到出现控制量：

同样的方法，可以对输出量ω(t)进行预测，并将含有控制量量和不含控制量的部分分离，为实际输出量构成的矩阵：

同理，也将控制量u进行泰勒展开，于是可以得到控制量u的预测值

为了使输出y尽可能地接近w，可以用如下方法估计误差，其中J为误差指标，Q为决定各个状态量误差权重的矩阵，为估计输出值，w为实际输出值：

理论上当达到最优时，条件为：

于是可以得到控制量的预测值

其中K又误差要求矩阵Q决定，M_ρ,M_i由以下定义：

其中为估计输出量构成的矩阵。

(2)启发式仿生鸽群优化算法

第一步：通过地图罗盘算子寻优

给定鸽群数量Np,，最大迭代次数t1max,以及地图罗盘算子R，进化方法可以由如下给出：

其中x_g为当前全局最优解，即为当前迭代中代价函数fitness最大或者最小的鸽群位置，v_i(t)为第i个鸽子在第t次迭代时的速度，x_i(t)为第i个鸽子在第t次迭代时的位置，rand为一个0到1之间的随机数。

用此种方法迭代，得到迭代t1imax步之后的鸽群的速度，以及局部和全局和局部最优解。

第二步：通过地标算子寻优

由于鸽子在寻找目的地的后期，主要依靠的是地标来进行目标的导引，为此根据其行为特性提出地标算子。所以将上部经过地图罗盘算子寻优的结果继续通过改方法迭代。该算子规定，每一代的鸽群数目减半，为了更快的到达目的地，剩下的鸽子直接飞向目的地。具体的更新准则如下式所示：

X_i(t)＝X_i(t-1)+rand·(X_c(t)-X_i(t-1)) (20)

在上式中，N_p为鸽群的数目，fitness是鸽子位置信息的代价函数，为了求得代价函数的最小值，可以取f_min作为目标函数，X_c是鸽群的加权位置中心。通过该种方法，由于个群数量呈指数型下降而且每次选取最优的一部分，所以可以很快的找到最优值。

鸽群优化算法示意图如图3所示，下次迭代的速度由当前速度和目标速度矢量合成。

技术实现要素：

1、发明目的：

本发明提出了一种基于启发式仿生鸽群优化的四旋翼无人机显式预测控制方法，其目的是提供一种具有较高可靠性四旋翼无人机控制方法。

该方法采用鸽群算法对轨迹进行插值，并应用于显式非线性模型预测控制，可以克服非线性模型预测控制对轨迹要求高，复杂轨迹难以应用的问题，避免了非线性模型预测控制中求多次导数的问题，节省了控制成本。

2、技术方案：

基于启发式仿生鸽群优化的四旋翼无人机显式预测控制方法的步骤如下：

步骤一：根据空气动力学和运动学建立四旋翼无人机模型,并给定控制量和状态量之间的关系。并给定要求的控制轨迹。对控制模型(四旋翼无人机)的状态量进行微分直至出现控制量的部分，并根据状态方程中出现控制量的那一阶微分反解控制量，得到相关逆推控制量的模块。把要求的轨迹的预测微分量和状态量作为反解控制量模块的输入，把反解出的控制量应用于原模型，建立显示预测控制模型。形成

预测微分量——反解控制量——控制量应用于原模型——结果的模型。

步骤二：始化鸽群优化算法，给定线性化轨迹中要求取样点点的个数D，点的个数即为鸽群优化算法中鸽子位置的维数。

定鸽群数量Np,，最大迭代次数t1max,以及地图罗盘算子R。

步骤三：设计代价函数

代价函数的确定是智能优化算法的核心，决定目标检测的准确性。本方法中，采用误差累计法，即要求轨迹和误差轨迹的多点之间函数值差值绝对值的求和。

步骤四：利用地图罗盘因子进行寻优

利用初始化的群体位置和速度，根据初始的个体的代价函数值选取全局最优位置X_g。根据公式(17)中的公式更新每个个体的位置X_i，计算新生成鸽子的代价函数值，如果新鸽子的代价函数值比全局最优位置的代价函数值更低，则把新生成的鸽子位置定义为新的全局最优位置X_g。反复应用地图罗盘算子进行寻优，直到运行代数大于地图罗盘算子最大代数t1max时停止。

步骤五：利用鸽群优化地标算子进行寻优

利用地图罗盘算子寻优的结果作为地标算子的初始群体，根据公式(18)～(20)中的公式更新每个个体的速度V_i和位置X_i，计算新生成鸽子的代价函数值，如果新鸽子的代价函数值比全局最优位置的代价函数值更低，则把新生成的鸽子位置定义为新的全局最优位置X_g。根据公式(18)计算新种群的群体数量，根据公式(18)计算的结果舍弃群体中代价函数较小的一部分个体，选择当前群体中较优的群体作为保留群体进行下一轮寻优，反复应用地标算子进行寻优，直到运行代数大于地标算子最大代数t2max时停止。

步骤六：得到鸽群优化过的样本目标点，构造线性插值函数。

步骤七：将鸽群优化后的插值函数应用于前面得到的模型形成：

鸽群优化后的轨迹——预测轨迹——逆推控制量——输入到模型——结果

这样的结构模型

步骤八：进行仿真或实验，得到相关结果并验证

3、优点及效果：

本发明提出了一种基于启发式仿生鸽群优化的四旋翼无人机显式预测控制方法。通过启发式仿生鸽群优化算法将要求经行线性化可以降低控制复杂难度，克服了显式预测控制对轨迹要求高，复杂轨迹难以应用的问题，避免在线优化过程，节约了大量时间，降低了控制成本，可以很好的应用于四旋翼或多旋翼控制领域。

【附图说明】

图1为四旋翼无人机示意图。

图2为显式预测控制示意图。

图3为鸽群优化算法示意图。

图4为实验四旋翼显示与预测控制模拟示意图。

图5为鸽群优化拟合曲线。

图6为常规等距插值方法拟合曲线。

图7为误差收敛曲线。

图8为本发明流程图。

图9为最终实际输出轨迹与鸽群优化后轨迹对比。

图10为最终实际输出轨迹与最初要求轨迹对比。

图中标号及符号说明如下：

F—升力；φ,θ,Ψ—无人机角速度；M—显式预测控制中的反馈矩阵；

K—显式预测控制中比例系数，由误差要求决定；U—控制量；

-A(x)^-1—通过要求量逆推控制量的矩阵；

【具体实施方式】

下面通过一个具体的无人机目标检测实例来验证本发明所提出的设计方法的有效性。本实验计算机配置为i7-4710MQ处理器，2.50Ghz主频，4G内存，软件为MATLAB 2014a版本。

本实例的具体实现步骤如下：

步骤一：建立四旋翼无人机模型和控制量，给定要求轨迹，并建立显示预测控制模型。四旋翼无人机简化图如附图1，图中定义机体坐标系xyz与地面坐标系XYZ，欧拉角定义如附图2。

其中欧拉角定义如下：

偏航角Ψ：Ox在平面OXY中投影与X轴的夹角

俯仰角θ：Oz在平面OXZ中投影与Z轴的夹角

滚转角φ：Oy在平面OYZ中投影与Y轴的夹角

由此可以得到机体坐标系到地面坐标系的转移矩阵：

R_x，R_y，R_z分别为相对x，y,z的转动变换。

为简化模型，做如下假设：

1.四旋翼无人机是个均匀对称的刚体。

2.最初的机体坐标的原点在无人机中心。

3.空气阻力不随无人机高度变化。

因此可以根据牛顿第二定律建立如下方程：

其中F为无人机所受力，m为无人机质量，V为无人机速度，t为时间，M为无人机转动惯量，H为无人机所受动量矩。

可以进一步得到具体量的方程：

G为重力，g为重力加速度，D_i为每个旋翼的阻力，T_i为每个旋翼的升力。D_d为空气阻力系数，D_t为空气升力系数。ω_i为发动机转速。

将上述所有式子整理，可以得到位置的状态方程和角度的状态方程：

其中I_x,I_y,I_z为三个轴的转动惯量，x,y,z为坐标位置，为三个方向加速度，p,q,r为三个方向角速度，F_x,F_y,F_z为x,y,z方向的合外力，k_t为升力系数，k₁,k₂,k₃为阻力系数M_x,M_y,M_z可由下式可得：

为了便于控制，将控制量定义如下：

其中U₁为高度通道控制量，U₂为滚转通道控制量，U₃为俯仰通道控制量，U₄为偏航通道控制量。ω_i代表马达转速，F_i为各个发动机的升力。

在低速条件下，可以忽略空气阻力，并假设：

于是可以得到无人机的非线性模型以及逆推方式如下：

本例中I_x＝I_y＝0.8kg.m2I_z＝1kg.m² l＝0.15mm＝1kg.

将数据代入得到无人机模型以及反解方程：

由于控制量出现在二阶导数中，所以需要预测二阶导数来反解控制量，这样就可以按照显示预测控制方法建立显示预测模型，并在simulink中完成搭建。而由于的方法中由于优化后轨迹为线性轨迹，因此二阶导数输入为0。整体模型附图，图2为显式预测控制示意图，图4为整体实现图。本例中以只控制角度为例。

步骤二：初始化鸽群优化算法，给定线性化要求点的个数，点的个数即为鸽群优化算法中鸽子位置的维数。

(1)初始化优化参数维数D

本例中将会寻找八个最优点(非端点)经行线性插值，所以D为8。

(2)初始化种群数量Np

群智能优化算法的种群数量Np对优化效果影响很大，选择一个适当的种群数量，保证算法的准确性和快速性。本方法去Np为100。

(4)初始化种群位置和速度

在搜索空间没设定群体的位置上限P_max＝100和位置下限P_min＝0，给种群中的每个粒子都初始化一个初始的位置X_i和初始的速度V_i。本例中上限即为函数的区间上限，下限即为函数的区间下限。其中rand为0到1的一个随机数。

V_i(t)＝rand

x_i(t)＝rand(100-0) (29)

(5)设置算法代数

鸽群优化算法有两个算子，分别是地图罗盘算子和地标算子，算法运算前需要分别设定两个算法运行的最大代数NC₁＝50和NC₂＝8。

步骤三：设计代价函数

代价函数的确定是智能优化算法的核心，决定目标检测的准确性。本方法中，采用误差累计法，即要求轨迹和误差轨迹的多点之间函数值差值绝对值的求和。

因为显式预测控制对模型函数要求比较高，因此考虑将轨迹线性化，在函数段上取D个点，分别在每段上将轨迹线性化。

即给定一个区间[a,b]，将选择x_i作为样本点，其中x_i≠a,b。(i＝1,2,3,..N)

将选择区间x∈[x_i,x_i+1]或x∈[a,x₁]或x∈[x_n,b]，并定义估计值如下：

之后将平局取n个误差误差取样点，误差取样点之间距离d定义如下：

于是每个误差取样点可这样得到：XX_i＝a+id

于是可以得到累积误差：

本例中误差取样点数为100，所以n＝100。

而累计误差E即为代价函数fitness。

步骤四：利用地图罗盘因子进行寻优

利用初始化的群体位置和速度，根据初始的个体的代价函数值选取全局最优位置X_g。根据公式(17)中的公式更新每个个体的位置X_i，计算新生成鸽子的代价函数值，如果新鸽子的代价函数值比全局最优位置的代价函数值更低，则把新生成的鸽子位置定义为新的全局最优位置X_g。反复应用地图罗盘算子进行寻优，直到运行代数大于地图罗盘算子最大代数t1max时停止。

迭代公式如下：本例中R＝0.2，带入如下

v_i(t)＝v_i(t-1)·e^-Rt+rand·(x_g-x_i(t-1)) (30)

x_i(t)＝x_i(t-1)+v_i(t)

本例中R＝0.2，带入如下

V_i(t)＝V_i(t-1)·e^-0.2*t+rand·(x_g-x_i(t-1)) (31)

x_i(t)＝x_i(t-1)+V_i(t)

步骤五：利用鸽群地标算子进行寻优

利用地图罗盘算子寻优的结果作为地标算子的初始群体，根据公式(18)～(20)中的公式更新每个个体的速度V_i和位置X_i，计算新生成鸽子的代价函数值，如果新鸽子的代价函数值比全局最优位置的代价函数值更低，则把新生成的鸽子位置定义为新的全局最优位置X_g。根据公式(18)计算新种群的群体数量，根据公式(18)计算的结果舍弃群体中代价函数较小的一部分个体，选择当前群体中较优的群体作为保留群体进行下一轮寻优，反复应用地标算子进行寻优，直到运行代数大于地标算子最大代数t2max时停止。

步骤六：得到D个最佳插值取样点，并根据这些点的函数值建立线性插值，这个插值即为应用于显式预测控制的轨迹。

步骤七：将鸽群优化过的线性轨迹用于显示预测控制,构造

鸽群优化优化结果——预测轨迹——逆推控制量——结果

的模型，如附图4.

步骤八：得到最终结果，进行验证，最终结果如附图10所示

通过附图5和附图6的对比，我们可以看出，与常用的等距插值相比，通过鸽群优化进行的非线性插值可以更好的复原原函数，而且具有更小的累积误差。而从附图7可看出鸽群优化具有良好的收敛特性，可以很快的求出最优值并使误差很小。

由附图10可知，通过该方法，我们最后的输出轨迹可以很好的跟踪原始复杂轨迹，而且本方法通过简化轨迹避免了轨迹起伏严重，并降低了控制成本。通过鸽群优化进行线性插值的方法也可以很好的应用于散点轨迹，从而实现对离散点轨迹要求的控制，可以用于避障，路径规划，避开雷达监测等方面的应用。