(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