·1602 工程科学学报，第41卷，第12期 能耗最小的运动.一般情况下，头雁会在期望的高 2)对于无人机i,在其机体坐标系Oxyz中，计 度下按照期望速度匀速朝目标飞去 算其他无人机在该系下的位置=（戈号），选 13加速模式 取无人机集合U:={i≠i,x>0: 加速模式是鸿雁群中两只鸿雁交换位置的方 3)如果U分=0，则表示无人机离目标最近，称 法，一般是一对领导者和其跟随者交换.当实际中 为长机，记为无人机L。此时无人机L以恒定的速度 出现鸿雁群中两只鸿雁交换位置的必要时，跟随 =o,其中vo表示不随时间变化的恒定速度，表 者会产生一个在运动方向上的加速度，使其超越 示计算集合中元素的个数； 其领导者，假设跟随者的领导者为j,即领导者j与 4)如果U=1,则表示无人机前方只有长机， 跟随者的几何距离在坐标系Oxy中x轴的分量 此时无人机将跟随长机L,且规定当>O时，即 ,>0,或领导者j与跟随者的几何距离在坐标系 长机在无人机的右前方，因此无人机跟随在长机 Ox沙中轴的分量x,<0.当跟随者飞行到其领 的左后方，反之无人机跟随在长机的右后方； 导者前方时，即，<0或x,>0,此时鸿雁将跟随 5)如果0=2，则表示无人机前方有两架无 鸿雁i,完成位置交换 人机，并且希望无人机集群呈“V”字队形，即长机 2仿鸿雁无人机编队控制 左右两侧无人机数量尽量保持一致.因此无人机 将跟随长机的另一侧； 2.1算法流程 假设无人机在起飞后，将根据接收到的所有 6)如果U>2,此时无人机按公式(1)和公式 无人机信息中的位置信息，找出在任意一架无人 (2)确定领导者k:.且当无人机k跟随在其领导者 机的机体坐标系O下，所有无人机中位置矢 的左侧时，无人机也跟随在无人机k的左侧，同样 量P的x轴分量数值最大的无人机作为长机，并按 当无人机k跟随在其领导者的右侧时，无人机也 照公式(1)计算接收到的所有无人机与自己的距 跟随在无人机k的右侧： 离R.根据公式(2)找出无人机的领导者.并根据 7)如果U1>2,且无人机的领导者k就是长机 上述的鸿雁群编队的两条规则确定其跟随在领导 L,则表示长机左右两侧都有无人机跟随的情况 者的左侧还是右侧，最后按照参数设定的编队中 下，无人机的领导者是长机，此时规定当y>0时， 领导者与跟随者之间的固定儿何距离矢量 即长机在无人机的右前方，因此无人机向长机的 d=（xye)计算无人机的期望位置，并将领导 左后方移动，反之无人机向长机的右后方移动： 者的速度作为无人机的期望速度输入飞控板，其 8)如果无人机进入加速模式后，其期望速度 流程如图3所示 :='a=k,v表示集群中无人机在加速模式下的 仿鸿雁群的无人机集群算法流程具体描述如下： 速度，B>1,%表示无人机的领导者无人机k的速 1)假设有N架无人机，在初始状态下所有无人 度.当无人机的领导者发生改变，即结束加速模 机的位置p和速度随机分布，其中i=1,2,…,N; 式，跟随新的领导者 串口状态 线程3 读取 线程2 MavLink UDP 集群 鸿雁群 下达 申请状态 换模 数据 状态 编队控 控制 信息 广播 汇总 制算法 指令 局域网状 线程1 鸿雁群编队控制算法 态接收 地 寻找长 确定 选择跟 计算期 面站 领导 随左侧 望位置 或右侧 和速度 图3无人机集群算法流程 Fig.3 Algorithm process of UAV formation能耗最小的运动. 一般情况下，头雁会在期望的高 度下按照期望速度匀速朝目标飞去. 1.3    加速模式 i j j i Oixiyizi x x i i j > 0 j i Ojxjyjzj x x j i j < 0 i j x i i j < 0 x j i j > 0 j i 加速模式是鸿雁群中两只鸿雁交换位置的方 法，一般是一对领导者和其跟随者交换. 当实际中 出现鸿雁群中两只鸿雁交换位置的必要时，跟随 者会产生一个在运动方向上的加速度，使其超越 其领导者，假设跟随者 的领导者为 ，即领导者 与 跟随者 的几何距离在坐标系 中 轴的分量 ，或领导者 与跟随者 的几何距离在坐标系 中 轴的分量 . 当跟随者 飞行到其领 导者 前方时，即 或 ，此时鸿雁 将跟随 鸿雁 ，完成位置交换. 2    仿鸿雁无人机编队控制 2.1    算法流程 i i Oixiyizi P i x R i i j i de = ( xe ye )T i i 假设无人机 在起飞后，将根据接收到的所有 无人机信息中的位置信息，找出在任意一架无人 机 的机体坐标系 下，所有无人机中位置矢 量 的 轴分量数值最大的无人机作为长机，并按 照公式（1）计算接收到的所有无人机与自己的距 离 . 根据公式（2）找出无人机 的领导者. 并根据 上述的鸿雁群编队的两条规则确定其跟随在领导 者的左侧还是右侧，最后按照参数设定的编队中 领 导 者 与 跟 随 者 之 间 的 固 定 几 何 距 离 矢 量 计算无人机 的期望位置，并将领导 者的速度作为无人机 的期望速度输入飞控板，其 流程如图 3 所示. 仿鸿雁群的无人机集群算法流程具体描述如下： N pi vi i = 1,2,··· ,N 1）假设有 架无人机，在初始状态下所有无人 机的位置 和速度 随机分布，其中 ； i Oixiyizi p i j = ( x i j y i j )T Ui = { j| j , i, x i j > 0 } 2）对于无人机 ，在其机体坐标系 中，计 算其他无人机在该系下的位置 ，选 取无人机集合 ； |Ui | = 0 i L L v c L =v0 v0 |·| 3）如果 ，则表示无人机 离目标最近，称 为长机，记为无人机 . 此时无人机 以恒定的速度 ，其中 表示不随时间变化的恒定速度， 表 示计算集合中元素的个数； |Ui | = 1 i i y i L > 0 i i i 4）如果 ，则表示无人机 前方只有长机， 此时无人机 将跟随长机 L，且规定当 时，即 长机在无人机 的右前方，因此无人机 跟随在长机 的左后方，反之无人机 跟随在长机的右后方； |Ui | = 2 i i 5）如果 ，则表示无人机 前方有两架无 人机，并且希望无人机集群呈“V”字队形，即长机 左右两侧无人机数量尽量保持一致. 因此无人机 将跟随长机的另一侧； |Ui | > 2 i ki ki i ki ki i ki 6）如果 ，此时无人机 按公式（1）和公式 （2）确定领导者 . 且当无人机 跟随在其领导者 的左侧时，无人机 也跟随在无人机 的左侧，同样 当无人机 跟随在其领导者的右侧时，无人机 也 跟随在无人机 的右侧； |Ui | > 2 i ki L i y i L > 0 i i i 7）如果 ，且无人机 的领导者 就是长机 ，则表示长机左右两侧都有无人机跟随的情况 下，无人机 的领导者是长机，此时规定当 时， 即长机在无人机 的右前方，因此无人机 向长机的 左后方移动，反之无人机 向长机的右后方移动； i vi = va = βvki va β > 1 vki i ki i 8）如果无人机 进入加速模式后，其期望速度 ， 表示集群中无人机在加速模式下的 速度， ， 表示无人机 的领导者无人机 的速 度. 当无人机 的领导者发生改变，即结束加速模 式，跟随新的领导者. 串口状态 读取 局域网状 态接收 MavLink 申请状态 信息 UDP 数据 广播 集群 状态 汇总 鸿雁群 编队控 制算法 下达 控制 指令 确定 领导 者 选择跟 随左侧 或右侧 鸿雁群编队控制算法 计算期 望位置 和速度 线程3 线程1 线程2 开始 解锁地面站 起飞 切换模式 降落 结束寻找长机 图 3    无人机集群算法流程 Fig.3    Algorithm process of UAV formation · 1602 · 工程科学学报，第 41 卷，第 12 期