(19)中华人民共和国国家知识产权局 ▣▣ (12)发明专利申请 (10)申请公布号CN105652664A (43)申请公布日2016.06.08 (21)申请号201610113345.7 (22)申请日2016.02.29 (71)申请人北京航空航天大学 地址100191北京市海淀区学院路37号 (72)发明人段海滨陈智隆邓亦敏 (74)专利代理机构北京慧泉知识产权代理有限 公司11232 代理人王顺荣唐爱华 (51)1nt.Cl. G05B13/04(2006.01) 权利要求书5页说明书11页附图5页 (54)发明名称 一种基于鸽群优化的四旋翼无人机显式预测 控制方法 (57)摘要 一种基于启发式仿生鸽群优化的四旋翼无人 机显式预测控制方法，包括如下步骤：1、根据空 2网 气动力学和运动学建立四旋翼无人机模型，并给 定控制量和状态量之间的关系。2、初始化鸽群优 化算法，给定线性化轨迹中要求取样点点的个数 D。3、设计代价函数：4、利用地图罗盘因子进行 寻优：5、利用鸽群优化地标算子进行寻优；6、得 到鸽群优化过的样本目标点，构造线性插值函数； 7、将鸽群优化后的插值函数应用于前面得到的模 型形成：8、进行仿真或实验，得到相关结果并验 证。通过该方法可以降低控制复杂难度，克服了 显式预测控制对轨迹要求高，难以应用的问题，避 免在线优化过程，节约了大量时间，降低了控制成 89249401 石
CN105652664A 权利要求书 1/5页 1.一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特征在于：该控制方法的 步骤如下： 步骤一：根据空气动力学和运动学建立四旋翼无人机模型，并给定控制量和状态量之 间的关系；并给定要求的控制轨迹：对四旋翼无人机的状态量进行微分直至出现控制量的 部分，并根据状态方程中出现控制量的那一阶微分反解控制量，得到相关逆推控制量的模 块：把要求的轨迹的预测微分量和状态量作为反解控制量模块的输入，把反解出的控制量 应用于原模型，建立显示预测四旋翼无人机：形成预测微分量一一反解控制量一一控制量 应用于原模型一一结果的模型： 步骤二：初始化鸽群优化算法，给定线性化轨迹中要求取样点点的个数D,点的个数即 为鸽群优化算法中鸽子位置的维数：给定鸽群数量Np,,最大迭代次数tlmax,以及地图罗盘 算子R; 步骤三：设计代价函数 代价函数的确定是智能优化算法的核心，决定目标检测的准确性：采用误差累计法，即 要求轨迹和误差轨迹的多点之间函数值差值绝对值的求和： 步骤四：利用地图罗盘因子进行寻优 利用初始化的群体位置和速度，根据初始的个体的代价函数值选取全局最优位置Xg:根 据公式(1)中的公式更新每个个体的位置X:,计算新生成鸽子的代价函数值，如果新鸽子的 代价函数值比全局最优位置的代价函数值低，则把新生成的鸽子位置定义为新的全局最优 位置Xg:反复应用地图罗盘算子进行寻优，直到运行代数大于地图罗盘算子最大代数tlmax 时停止： y,(t)=v,(t-1)·et+rand●(xg-x,(t-1) (1) x,(t)=x,(t-1)+v,(t) 其中xg为当前全局最优解，即为当前迭代中代价函数fitness最大或者最小的鸽群位 置，v(t)为第i个鸽子在第t次迭代时的速度，x(t)为第i个鸽子在第t次迭代时的位置， rand为一个0到1之间的随机数： 步骤五：利用鸽群优化地标算子进行寻优 利用地图罗盘算子寻优的结果作为地标算子的初始群体，根据公式(2)~(4)中的公式 更新每个个体的速度V:和位置X,计算新生成鸽子的代价函数值，如果新鸽子的代价函数值 比全局最优位置的代价函数值更低，则把新生成的鸽子位置定义为新的全局最优位置Xg: 根据公式(2)计算新种群的群体数量，根据公式(2)计算的结果舍弃群体中代价函数小的 部分个体，选择当前群体中较优的群体作为保留群体进行下一轮寻优，反复应用地标算子 进行寻优，直到运行代数大于地标算子最大代数t2max时停止； N=心-型 (2) 2 x)卫x⑨me(x) N。·∑fitness(X,(d) (3) Xi(t)=Xi(t-1)+rand.(Xe(t)-Xi(t-1)) (4) 2
CN105652664A 权利要求书 2/5页 在上式中，为鸽群的数目，fitness,是鸽子位置信息的代价函数，为了求得代价函数的 最小值，取fmi作为目标函数，Xc是鸽群的加权位置中心：由于个群数量呈指数型下降而且 每次选取最优的一部分，所以很快的找到最优值： 步骤六：得到鸽群优化过的样本目标点，构造线性插值函数： 步骤七：将鸽群优化后的插值函数应用于前面得到的模型形成： 鸽群优化后的轨迹一一预测轨迹一一逆推控制量一一输入到模型一一结果： 步骤八：进行仿真或实验，得到相关结果并验证。 2.根据权利要求1所述的一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特 征在于：在步骤一中，定义四旋翼无人机机体坐标系xyz与地面坐标系XYZ:欧拉角定义如 下： 偏航角：0x在平面OXY中投影与X轴的夹角： 俯仰角0：0z在平面0XZ中投影与Z轴的夹角： 滚转角中：Oy在平面OYZ中投影与Y轴的夹角： 得到机体坐标系到地面坐标系的转移矩阵： cosw cos cosw sin 0sino cos w sin 0 cos+sinw sind R=R·R,·2= sinw cos0 sinw sin esin sinw sin esino-sin dcosw -sin cos 0sin cos 0 cos (21) Rx,Ry,Rz分别为相对x,y,z的转动变换。 3.根据权利要求1所述的一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特 征在于：简化模型，做如下假设： 1.四旋翼无人机是个均匀对称的刚体： 2.最初的机体坐标的原点在无人机中心： 3.空气阻力不随无人机高度变化： 因此根据牛顿第二定律建立如下方程： dy dl F-m M= dt dt (22) 其中，F为无人机所受力，m为无人机质量，V为无人机速度，t为时间，M为无人机转动惯 量，为无人机所受动量矩； 进一步得到具体量的方程： G=mg D=Ca012=D oT;=pCo/2=Do (23) G为重力，g为重力加速度，D:为每个旋翼的阻力，T:为每个旋翼的升力：D为空气阻力系 数，D为空气升力系数；o为发动机转速； 整理得到位置的状态方程和角度的状态方程： 3
CN105652664A 权利要求书 3/5页 x=(代，-K对/m=依2a((cossin0eos+ins5n-K,刘/m y(FK:m=(ko(sinw sindcos+coswsin/m (24) = 2=(仅，-K,-mg)/m=(k2a((c0)-K,/m-g D M+(I -I2)ar /I M(I -I)rp I M (I -I,)pq I. (25) 其中Ix,Iy,Iz为三个轴的转动惯量，x,y,z为坐标位置，x,y,z为三个方向加速度，p,9,r 为三个方向角速度，Fx,Fy,Fz为x,y,z方向的合外力，kt为升力系数，k1,k2,k3为阻力系数Mx, M,M2由下式得到： M. (E-F)1 M, (F-F)1 U, (26) M. (E+F-E-E)1 U 为了便于控制，将控制量定义如下： , F+F2+F3+F4 k.oi U F-F k,(o02-o2) (27) U: F-F k(@-@) 04 F3+F4-F3-F k,(@2-+o-o) 其中U1为高度通道控制量，U2为滚转通道控制量，U3为俯仰通道控制量，U4为偏航通道控 制量：⊙代表马达转速，F为各个发动机的升力： 在低速条件下，忽略空气阻力，并假设： =p0=qw=r 于是得到无人机的非线性模型以及逆推方式如下： x =(cosy sin0 coso+sinw sin))/m y =(sin w sino cos+cosw sin o/m z=(cos o cos 0)u/m-g =[U2+0w(1,-I)]/1x
CN105652664A 权利要求书 4/5页 0=[l0+中(L2-1)]/1, w=[1U4+0I.-I,)]/I 4=以.-0u,-2 03= 0Ly-p2-1) 1 (28) 04=y1,-81,-1) Ix=Iy=0.8kg.m2Iz=1kg.m21=0.15m m=1kg; 将数据代入得到无人机模型以及反解方程： 中=[0.15*02+0w(0.8-1)]/0.8 0=[0.15*03+y(1-0.8)]/0.8 Ψ=[0.15*∥4+中0.8-0.8)]/1 12=*0.8-00.8-1) 0.15 0*0.8-中w(1-0.8) 3= 0.15 0,=y*1-00.8-0.8) 0,15 由于控制量出现在二阶导数中，所以需要预测二阶导数来反解控制量，这样就按照显 示预测控制方法建立显示预测模型，并在simulink中完成搭建；由于优化后轨迹为线性轨 迹，因此二阶导数输入为0。 4.根据权利要求1所述的一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特 征在于：在步骤二中，设置D为8：Np为100：设定种群的位置上限Pmax=100和位置下限Pmin= 0,给种群中的每个粒子都初始化一个初始的位置X:和初始的速度V1;上限即为函数的区间 上限，下限即为函数的区间下限：其中rand为0到1的一个随机数： V,(t)=rand x,(t)=rand(100-0) 。(29) 5.根据权利要求1所述的一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特 征在于：在步骤三中，将轨迹线性化，在函数段上取D个点，分别在每段上将轨迹线性化： 即给定一个区间[a,b],将选择xi作为样本点，其中xi≠a,b;i=1,2,3,·.N; Vx∈[a,],将选择区间x∈[xi,xi1]或x∈[a,x1]或x∈[xn,b],并定义估计值如下： 5
CN105652664A 权利要求书 5/5页 F)=Fx)+f(m)-f(x2 -x) X+1-X (30) 之后将平局取n个误差误差取样点，误差取样点之间距离d定义如下： d=b-a n (31) 于是每个误差取样点这样得到：XXi=a+id 于是得到累积误差： E- f(xX )-f(XX, (32) n=l00:E为代价函数fitness。 6.据权利要求1所述的一种基于鸽群优化的四旋翼无人机显式预测控制方法，其特征 在于：在步骤四中，R=0.2。 6
CN105652664A 说明书 1/11页 一种基于鸽群优化的四旋翼无人机显式预测控制方法 【技术领域】 [0001] 本发明是一种基于启发式仿生鸽群优化的四旋翼无人机显式预测控制方法，属于 飞行器控制领域。 【背景技术】 [0o02]无人机(Unmanned Aerial Vehicle,UAV)在当今军事行动中扮演越来越重要的角 色，能够有效完成复杂和危险的侦查和作战任务。UAV虽然在军事和民用等领域已有广泛应 用，但其自身的很多关键技术尚有待进一步研究和应用。而四旋翼作为一种更便宜可靠性 更高，可以垂直起降的一种无人机，正被广泛的商业和军事运用，然而由于四旋翼的多维度 性，由四个马达控制起飞悬停转向，因此其对控制率要求比较高。 [0003] 模型预测控制可以实现对被控对象的精确控制，但有些模型预测控制依赖在线的 优化过程，比较耗时。对于有些线性模型预测控制问题来说，可以求取其最优控制的显式 解，但对于非线性问题，求取显式解比较困难。显示非线性模型预测控制(Exp1icit Nonlinear Model Predictive Control,ENMPC)应用泰勒展开原理，可以求取非线性模型 预测控制的显式解。其主要控制思想是要求轨迹一一逆推预测控制量一一输出轨迹。而本 专利申请将通过简化轨迹来简化预测过程，从而大大降低复杂难度。但是其自身有一定的 复杂度，而且对于复杂轨迹和散点轨迹并不能进行微分操作。所以我们可以通过插值方法 求其估计的线性轨迹。 [0004]群体智能是仿生智能的一个重要分支，人们通过对自然界生物群体的观察，受到 自然界中生物群体行为的启发，在此基础上总体提升，将其行为模式用数学的方式描述出 来。在群体智能模型的基础上，人们提出了群体智能优化算法的概念，用生物群体的行为模 式来求解优化问题。启发式仿生鸽群优化算法(Pigeon Inspired Optimization,PI0)是 HaibinDuan和PeixinQiao在2014年提出的一种新型的启发式群智能优化算法，该算法受到 鸽子群体行为的启发，根据鸽子在寻找目标的过程中，先后依据磁场和地标作为指示的行 为特点，建立起地图罗盘和地标两种算法机制。 [0005]鸽群在寻找目的地的过程中，会先参照太阳和磁场进行初步定位，然后依照地标 进行精确定位，根据这一特性，鸽群优化算法提出了两种相对应的算子，分别为地图罗盘算 子和地标机制，来模拟鸽群的这种特性，并将这两种算子结合起来解决优化问题。 [0006]传统的控制方式主要是通过将模型线性化来进行控制，而显式预测控制可以实现 非线性模型的控制，但是复杂度较高，而本方法保持模型原样而将要求轨迹线性化来实现 控制，大大降低控制预测模型控制难度，而又很好的保持了模型的方程。 [0007](1)非线性显式预测控制 [0008] 在非线性显式预测中，是通过将非线性部分泰勒展开，用提前预测要求量来进行 控制，其主要思想如下： [0009]对于一个非线性模型： 7
CN105652664A 说明书 2/11页 x f(x,u) [0010] y=h(x) (1) [0011] 其中，x代表状态量，y代表输出量，f(x,u),h(x)为两个函数。 [0012] 设P:代表经过微分在状态方程中出现第i个控制量的次数，用r代表输入的序号。 对上式每个变量分别进行泰勒展开可以得到 Tr+P y,(t+t)≈y,(t)+ty(t)+… r+p,) ,r+p(t）=TY(t) [0013] (r+p)! (2) Y,(t)=[y,(t)y,(t) y,r+(】 [0014] t是预测控制中的预测时间，)是第1个估计量，t是当前时刻，pi为第i个控制量 出现在状态中需要泰勒展开的阶数，为继续展开的相对阶数。T为泰勒展开的系数矩阵， yi(t)为第i个控制量在t时刻的值，Y(t)为由yi(t)及其各阶导数组成的矩阵。 [0015] 将多个变量和在一个矩阵中，于是上式整理为： 学 yi(t +r) 0 Y (t) Y(t) [0016] y(t+t)= y2(t +T) TY(t) (3) +t) Y,(c) [0017] 其中工为第1个量的泰勒展开系数。 [0018] 将有控制量的和没有控制量的状态变量分离，重新整理矩阵，可以得到下式： 0 i-l, [0019] (4) 0 t)= ,yFy….ryr…产er小 [0020] 其中符号定义如下： [0021] (5) …=12+1 8
CN105652664A 说明书 3/11页 [0022] =1x x%1 -],i=1,2,..n (p,-1) (6) TAti-1 TP,+i-1 T,=diag (A)！ (P+-)！ (Pa+-l)1 0 T [0023] 0 (7) t= [0024] 而其中了，言=1,2，，r+1:在多次微分后出现控制量的部分，可以将其展 开如下： () Lh (x) t) ) Lh(x) A(x)u (8) [0025] (t) Leh (x) [0026] 其中Lh,(x)为对第1个没有输入控制量的pi次微分，A(x)为控制量到状态量 的函数，u为控制输入。i=1,2,.,n; [0027] 将上式对时间进行微分，可以得到如下： [0028] +1(t) y (x) )= +(t) y (x) +A(x)u+p(x,u) (9) +1(t) Lh(x) [0029] 其中p1(x,u)为非线性部分的输入。 [0030] 用该方法反复进行微分如下，直到出现控制量： yin(t) h(x) [0031] y(t) L玲h,(x) +Axn+p,(,u,,.d) (10) (t) Lh(x) [0032] 同样的方法，可以对输出量。(t)进行预测，并将含有控制量量和不含控制量的部 9
CN105652664A 说明书 4/11页 分分离，亚为实际输出量构成的矩阵： @(t+r) [0033] aft +t)= a(t+7) =1,iY) (11) ⊙n(t+t) [0034] 同理，也将控制量u进行泰勒展开，于是可以得到控制量u的预测值ù： [0035] (t+x)≈(t)+四(t)+..+ (t) r (12) [0036] 为了使输出y尽可能地接近w,可以用如下方法估计误差，其中J为误差指标，Q为决 定各个状态量误差权重的矩阵，为估计输出值，w为实际输出值： [0037] J= t+-o+)ygt+）-od+r (13) [0038] 理论上当达到最优时，条件为： (14) [0039] 8J =0 aū [0040] 于是可以得到控制量的预测值着 [0041] ù*=-(x)(M。+M) [0042] 其中K又误差要求矩阵Q决定，M,M由以下定义： [0043] Yi(t) 1(t) Y2(t) W1(t) r.(t) W(t) (15) [0044] -h(x) M L*-h,(x） (ty,i=1,2,.r+1 (16) h(x) 10