一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法

(12)发明专利申请

(10)申请公布号 CN 111290429 A(43)申请公布日 2020.06.16

(21)申请号 201911291794.0(22)申请日 2019.12.16

(71)申请人 南京信息工程大学

地址 210044 江苏省南京市江北新区宁六

路219号(72)发明人 刘云平　徐泽　王蔚　杨建康　(74)专利代理机构 南京苏高专利商标事务所

(普通合伙) 32204

代理人 曹坤(51)Int.Cl.

G05D 1/10(2006.01)

权利要求书1页 说明书8页 附图2页

()发明名称

一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法(57)摘要

本发明公开了一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法。本发明涉及一种无人机编队控制方法；本发明在传统人工势场方法地基础上增加了辅助牵引加速度，该加速度可以驱使无人机更快地离开移动障碍物，且避免了算法陷入局部最优化解；本发明通过引入与障碍物移动方向垂直的辅助牵引加速度信息，可消除局部优化解的影响。同时，为了实现无人机机群快速编队和避障，本文将有限时间一致性算法与改进的人工势场方法相结合，提出一种四旋翼无人机有限时间编队及其避障控制方法。该方法避免了基于传统人工势场方法的编队避障算法易陷入局部优化解，进而导致无人机编队避障失败的弊端。

CN 111290429 ACN 111290429 A

权　利　要　求　书

1/1页

1.一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，其操作步骤具体如下：

1.1、判断无人机是否到达目的地，如到达目的地，则无人机编队及其避障控制系统操作结束；

如未到达目的地，则执行步骤1.2；1.2、判断无人机是否为领航者，如是领航者，则执行步骤1.3中的领航者编队控制；如是跟随者，则执行步骤1.4中的跟随者编队控制和步骤1.5中的跟随者避障控制；1.3、领航者编队控制调用有限时间一致性算法得到领航者编队控制输入，随即执行步骤1.9；

1.4、跟随者编队控制首先调用获取其他跟随者的位姿信息，然后再调用有限时间一致性算法产生跟随者编队控制输入，并执行步骤1.9；

1.5、调用改进后的人工势场方法，然后执行步骤1.6和步骤1.7；1.6、跟随者避障控制首先获取其他跟随者的位姿信息，再判断其是否处于其他跟随者势场内，如处于其他跟随者势场内，则产生机间加速度，然后执行步骤1.8；

1.7、跟随者避障控制同时感知无人机周围的障碍物，判断其是否处于障碍物势场内，如处于障碍物势场内，则产生规避加速度及辅助牵引加速度，然后执行步骤1.8；

1.8、步骤1.6中的机间加速度和1.7中的规避加速度及辅助牵引加速度通过叠加产生加速度融合，从而进行跟随者避障控制输入，并执行步骤1.9；

1.9、将步骤1.3，1.4和1.8中的各输入融合产生无人机编队与避障控制协议，后执行步骤1.10；

1.10、更新无人机状态，执行步骤1。

2.根据权利要求1所述的一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，步骤1.1中，在判断无人机是否到达目的地之前，先将其处于初始化无人机状态。

3.根据权利要求1所述的一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，步骤1.3中，在领航者编队控制调用有限时间一致性算法得到领航者编队控制输入之前，先获取目标点的位置信息。

4.根据权利要求1所述的一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，步骤1.4中，跟随者编队控制在调用有限时间一致性算法产生跟随者编队控制输入中，获取的是与之有通信关系的其他跟随者的位姿信息。

5.根据权利要求1所述的一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，步骤1.6中，跟随者避障控制在产生机间加速度中，获取的是与之有通信关系的其他跟随者位姿信息。

2

CN 111290429 A

说　明　书

1/8页

一种基于一致性算法和人工势场方法的无人机编队及其避障

控制方法

技术领域

[0001]本发明涉及一种无人机编队控制方法，尤其涉及一种可以应对快速移动障碍物的四 旋翼无人机编队控制方法。

背景技术

[0002]目前，无人机机群编队方法主要有跟随领航者法、虚拟结构法、基于行为法以及一 致性算法等。一致性算法采用分布式网络进行信息交互，具有灵活性高和鲁棒性强等特 点。因此，被广泛应用于无人机机群编队控制。而无人机机群避障方法主要有人工势场 方法、模型预测法及最优控制法等。人工势场方法对计算能力要求较低且实时性强，被 广泛应用于无人机机群避障。然而，无人机机群对障碍物的感知范围有限，面对鸟类等 快速移动的障碍物时，无人机机群需要具有快速避障的能力。此外，当障碍物始终在无 人机与目标点所在的直线方向移动，且移动速度大于无人机最大速度时，基于传统人工 势场方法的编队避障算法易陷入局部优化解，进而导致无人机机群避障失败。[0003]针对无人机机群编队避障问题，众多国内外学者研究并提出了多种算法。Ruchti J 等人在《Unmanned Aerial System Collision Avoidance Using Artificial Potential Fields》中提出通过在传统人工势场方法上引入优先级模型和方向变化权重，以改善无 人机之间的死锁情况，可实现快速避障。Ferreiravazquez E D等人在《Distance-based Formation Control Using Angular Information Between Robots》中提出通过将角度 信息融入传统人工势场方法，并将其与一致性法融合，以减少局部最优化解对算法的影 响，实现了机器人编队成功避障。段海滨等人在《基于捕食逃逸鸽群优化的无人机紧密 编队协同控制》中提出通过将人工势场方法和基于捕食逃逸的鸽群优化方法结合，改进 了这两种方法易陷入局部最优化解的问题，实现了无人机机群的紧密编队。朱旭等人在 《基于改进人工势场的无人机编队防碰撞控制方法》中提出通过在传统人工势场方法的 基础上引入无人机与障碍物之间的相对速度，补偿障碍物无法主动规避无人机的缺点， 实现无人机机群快速避障。张佳龙等人在《Fixed-Wing UAV Formation Control Design With Collision Avoidance Based on an Improved Artificial Potential Field》中 提出将平行于x-y平面和y-z平面的两个势场复合成具有三维空间旋转矢量的人工势场 方法，解决了无人机编队避障过程中易陷入局部最优化解的问题。这些学者的研究丰富 了无人机机群编队避障理论，但都存在改进的空间。诸如前两篇文献存在算法可能陷入 局部最优化解的问题，且所有文献都没有考虑无人机机群应对快速移动障碍物(障碍物 移动的速度高于无人机最大飞行速度)时的避障问题。

[0004]本文研究四旋翼无人机机群编队针对鸟类等快速移动障碍物的避障问题，考虑障碍 物始终在无人机与目标点所在的直线方向移动且移动速度大于无人机最大速度时的情 况。通过在传统人工势场方法上引入辅助牵引加速度信息，得到一种改进的人工势场避 障方法，实现无人机机群快速避障，并克服局部最优对避障过程的影响。为提高无人机 机群

3

CN 111290429 A

说　明　书

2/8页

编队过程的响应速度，本文将有限时间一致性算法与改进的人工势场避障方法相结 合，提出一种四旋翼无人机机群编队和避障方法，实现无人机机群在飞行过程中快速编 队与避障。

发明内容

[0005]本发明的目的是提供一种四旋翼无人机机群编队及其避障控制方法，该方法可以 避免算法陷入局部最优化解，并可以使四旋翼无人机机群应对快速移动障碍物(障碍 物移动的速度高于无人机最大飞行速度)。[0006]本发明的技术方案是：一种基于一致性算法和人工势场方法的无人机编队及其避障 控制方法，其操作步骤具体如下：[0007]1.1、判断无人机是否到达目的地，如到达目的地，则无人机编队及其避障控制系 统操作结束；

[0008]如未到达目的地，则执行步骤1.2；[0009]1.2、判断无人机是否为领航者，如是领航者，则执行步骤1.3中的领航者编队控制；

[0010]如是跟随者，则执行步骤1.4中的跟随者编队控制和步骤1.5中的跟随者避障控制；

[0011]1.3、领航者编队控制调用有限时间一致性算法得到领航者编队控制输入，随即执 行步骤1.9；[0012]1.4、跟随者编队控制首先调用获取其他跟随者的位姿信息，然后再调用有限时间 一致性算法产生跟随者编队控制输入，并执行步骤1.9；[0013]1.5、调用改进后的人工势场方法，然后执行步骤1.6和步骤1.7；[0014]1.6、跟随者避障控制首先获取其他跟随者的位姿信息，再判断其是否处于其他跟 随者势场内，如处于其他跟随者势场内，则产生机间加速度，然后执行步骤1.8；[0015]1.7、跟随者避障控制同时感知无人机周围的障碍物，判断其是否处于障碍物势场 内，如处于障碍物势场内，则产生规避加速度及辅助牵引加速度，然后执行步骤1.8；[0016]1.8、步骤1.6中的机间加速度和1.7中的规避加速度及辅助牵引加速度通过叠加 产生加速度融合，从而进行跟随者避障控制输入，并执行步骤1.9；[0017]1.9、将步骤1.3，1.4和1.8中的各输入融合产生无人机编队与避障控制协议，后 执行步骤1.10；[0018]1.10、更新无人机状态，执行步骤1。[0019]进一步的，步骤1.1中，在判断无人机是否到达目的地之前，先将其处于初始化无 人机状态。

[0020]进一步的，步骤1.3中，在领航者编队控制调用有限时间一致性算法得到领航者编 队控制输入之前，先获取目标点的位置信息。[0021]进一步的，步骤1.4中，跟随者编队控制在调用有限时间一致性算法产生跟随者编 队控制输入中，获取的是与之有通信关系的其他跟随者的位姿信息。[0022]进一步的，步骤1.6中，跟随者避障控制在产生机间加速度中，获取的是与之有通 信关系的其他跟随者位姿信息。

4

CN 111290429 A[0023]

说　明　书

3/8页

本发明所述的控制方法为：四旋翼无人机机群中的虚拟领航者实时追踪目标点，

当 虚拟领航者到达目标点后，保持与目标点的速度一致；各跟随者在有限时间内保持和虚 拟领航者的飞行状态一致；该机群能够在有限时间内形成指定编队队形，且能够持久的 维持队形(各无人机保持相对位姿不变)；在机群飞行过程中，无人机之间以及无人机 与障碍物之间均不发生碰撞，且无人机机群在躲避障碍物后能够迅速恢复队形。[0024]本发明所述改进后的人工势场方法为：在传统人工势场方法地基础上增加了辅助牵 引加速度，该加速度可以驱使无人机更快地离开移动障碍物，且避免了算法陷入局部最 优化解。

[0025]本文将有限时间一致性算法和改进后的人工势场方法结合，提出一种四旋翼无人机 机群编队和避障控制方法，实现了无人机机群在飞行过程中快速编队与避障。[0026]本发明具有的有益效果：通过引入与障碍物移动方向垂直的辅助牵引加速度信息， 可消除局部优化解的影响；同时，为了实现无人机机群快速编队和避障，本文将有限时 间一致性算法与改进的人工势场方法相结合，提出一种四旋翼无人机机群有限时间编队 和避障方法；该方法避免了基于传统人工势场方法的编队避障算法易陷入局部优化解， 进而导致无人机机群避障失败的弊端。

附图说明

[0027]图1为本发明中无人机编队及其避障控制方法的流程图；

[0028]图2为本发明中人为构建的障碍物和无人机的势场及其影响范围示意图；[0029]图3为本发明中辅助牵引速度示意图；

[0030]图4为本发明中无人机机群的编队网络拓扑示意图。

具体实施方式

[0031]下面结合实例和说明书附图1-4对发明的技术方案进行详细说明：

[0032]一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其操作步骤 具体如下：[0033]1.1、判断无人机是否到达目的地，如到达目的地，则无人机编队及其避障控制系 统操作结束；

[0034]如未到达目的地，则执行步骤1.2；[0035]1.2、判断无人机是否为领航者，如是领航者，则执行步骤1.3中的领航者编队控制；

[0036]如是跟随者，则执行步骤1.4中的跟随者编队控制和步骤1.5中的跟随者避障控制；

[0037]1.3、领航者编队控制调用有限时间一致性算法得到领航者编队控制输入，随即执 行步骤1.9；[0038]1.4、跟随者编队控制首先调用获取其他跟随者的位姿信息，然后再调用有限时间 一致性算法产生跟随者编队控制输入，并执行步骤1.9；[0039]1.5、调用改进后的人工势场方法，然后执行步骤1.6和步骤1.7；[0040]1.6、跟随者避障控制首先获取其他跟随者的位姿信息，再判断其是否处于其他跟 

5

CN 111290429 A

说　明　书

4/8页

随者势场内，如处于其他跟随者势场内，则产生机间加速度，然后执行步骤1.8；[0041]1.7、跟随者避障控制同时感知无人机周围的障碍物，判断其是否处于障碍物势场 内，如处于障碍物势场内，则产生规避加速度及辅助牵引加速度，然后执行步骤1.8；[0042]1.8、步骤1.6中的机间加速度和1.7中的规避加速度及辅助牵引加速度通过叠加 产生加速度融合，从而进行跟随者避障控制输入，并执行步骤1.9；[0043]1.9、将步骤1.3，1.4和1.8中的各输入融合产生无人机编队与避障控制协议，后 执行步骤1.10；[0044]1.10、更新无人机状态，执行步骤1。[0045]以下对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的 多种不同方式实施。

[0046]无人机机群协同编队飞行技术具有可执行各类军事或民用领域的各类复杂任务； 然而无人机机群在编队飞行过程中常遇到鸟类等移动障碍物，若机群与之发生碰撞，会 产生严重的后果；针对这一现状，提出一种无人机机群编队及其避障控制方法，实现了 无人机机群在飞行过程中的快速编队与避障。

[0047]本发明所述无人机机群编队及其避障控制方法将有限时间一致性算法和改进后的 人工市场方法结合，可使得无人机机群在有限时间内形成指定队形；若遇到障碍物时， 无人机机群快速避开移动障碍物并重新快速形成编队。[0048]本发明不考虑无人机在垂直方向上的运动，单架无人机(包含虚拟领航者和跟随者) 的动力学模型为二阶系统，其数学描述为：

[0049][0050]

其中，Xr(t)＝[xr,yr]T∈R2表示无人机r在t时刻的位姿(xr,yr)；

表 示无人机r在t时刻的速度

ur(t)表示无人机r在t时刻的

输入。

如图1所示为无人机编队及其避障控制方法流程图，包括：[0052]步骤1.1、判断无人机是否到达目的地，如到达目的地，则无人机编队及其避障控 制系统操作结束；

[0053]如未到达目的地，则执行步骤1.2；[00]步骤1.2、判断经过初始化无人机状态是否为领航者，如是领航者，则执行步骤1.3 中的领航者编队控制；[0055]如是跟随者，则执行步骤1.4中的跟随者编队控制和步骤1.5中的跟随者避障控制；

[0056]步骤1.3、领航者编队控制调用有限时间一致性算法得到领航者编队控制输入，随 即执行步骤1.9；

[0057]本发明定义a0(t)、V0(t)以及X0(t)＝[x0，y0]T表示领航者的加速度、速度以及位姿，定义 ag(t)、Vg(t)以及Xg(t)＝[xg，yg]T表示目标点g的加速度、速度以及位姿；本发明的领航者控 制输入设计为：

[0058]

6

[0051]

CN 111290429 A[0059]

说　明　书

5/8页

其中，sig(x)为符号函数；c1、c2、p1和p2为调节参数，其可调节 算

法的收敛速度。

[0060]步骤1.4、跟随者编队控制首先调用获取其他跟随者的位姿信息，然后再调用有限 时间一致性算法产生跟随者编队控制输入，并执行步骤1.9；[0061]本发明定义Vi(t)、Vj(t)、V0(t)以及Xi(t)、Xj(t)、X0(t)分别表示跟随者i、j和虚拟领航者的 速度以及位姿，定义

表示跟随者i和j相对于虚拟领航者的期望相对位姿；

本发明 跟随者的编队控制输入设计为：

[0062]

[0063]其中，i,j∈{1,2,…,n}，p1,p2,b1,b2,b3,b4∈R为调节参数，其可

调节算法 的收敛速度。[00]步骤1.5、调用改进后的人工势场方法，然后执行步骤1.6和步骤1.7；[0065]本发明所述的改进后的人工势场方法首先需要构建人工势场，人为构建的障碍物和 无人机的势场及其影响范围如下图2所示；本发明对障碍物仅构建斥力场，对无人机同 时构建斥力场和引力场：第一，本发明构建一个以障碍物s为势场中心，以ro为半径的 圆形障碍物斥力场；若无人机i进入障碍物的势场影响范围内，即无人机与障碍物的距 离小于最小安全距离，则无人机i受障碍物s的斥力场作用而产生规避加速度auo(i，s，t)， 该加速度可驱使无人机i远离障碍物s；为应对快速移动的障碍物，本发明在传统人工势 场方法上引入一个垂直于障碍物s移动方向(障碍物速度vo(t)的方向)的辅助牵引加速 度avuo(i，s，t)，利用该加速度辅助无人机i快速远离障碍物s。[0066]第二，本发明构建一个以无人机j为势场中心，以为半径的圆形无人机斥力场； 同时，本发明在斥力场上叠加一个引力场，该势场的影响范围为以无人机j为势场中心， 以为半径的圆外；若无人机i进入无人机j的斥力场影响范围内，即无人机i与无人机 j之间的距离小于

则无人机i受无人机j的斥力场作用而产生加速度auu(i，j，t)，该加 速度迫

 力场作用而使无人机i远离无人机j；若上述两机的距离大于则无人机i受无人机j的引产生加速度auu(i，j，t)，该加速度迫使无人机i接近无人机j。[0067]步骤1.6、跟随者避障控制首先获取其他跟随者的位姿信息，再判断其是否处于其 他跟随者势场内，如处于其他跟随者势场内，则产生机间加速度，然后执行步骤1.8；[0068]本发明跟随者的无人机间加速度设计为：

[0069]

[0070]

其中，为跟随者j的斥力场最大作用半径，为引力场最小作用半径， duu

(i，j)表示跟随者i和跟随者j之间的相对距离，和为加速度调节参数。

7

CN 111290429 A[0071]

说　明　书

6/8页

步骤1.7、跟随者避障控制同时感知无人机周围的障碍物，判断其是否处于障碍物 

势场，如处于障碍物势场内，则产生规避加速度及辅助牵引加速度，然后执行步骤1.8；[0072]本发明定义Xi(t)＝[xi，yi]T以及Xs(t)＝[xs，ys]T表示跟随者i和障碍物s的位姿，跟随者的规 避加速度设计为：

[0073]

[0074]其中，表示跟随者i与障碍物s之间的相对距离，ro(s)表示

障 碍物s的势场范围，表示无人机i的探测范围；和为规避加速度可调参数。[0075]本发明定义vi(t)表示跟随者i得速度，vs(t)表示障碍物s的速度，l1表示跟随者i与障 碍物s之间的连线，l2表示障碍物s为端点沿其速度方向的射线，θ表示将l2逆时针旋转 至与l1重合的角度，α表示跟随者i和障碍物s两者移动方向之间的相对角度；跟随者的 辅助牵引加速度设计为：

[0076]

[0077]其中，θ∈[0，2π]，α∈[0，2π]，旋转角α和θ均为逆时针旋转。为辅助牵引加速度调

节 参数；R(φ)为旋转矢量矩阵，

辅助牵引加速度得方向与θ有关；如图3所示，当0≤θ＜180°时，辅助牵引加速度的 

方向为沿障碍物s的速度方向逆时针旋转π/2；否则，辅助牵引加速度的方向为沿障碍 物s的速度方向顺时针旋转π/2。[0079]步骤1.8、步骤1.6中的机间加速度和1.7中的规避加速度及辅助牵引加速度通过 叠加产生加速度融合，从而进行跟随者避障控制输入，并执行步骤1.9；

[0080]本发明定义单无人机在空域中受障碍物势场(斥力场)以及各无人机势场(包括引 力场和斥力场)影响而产生多个加速度，将各加速度叠加后即为无人机所受的合加速度； 本发明所述跟随者的避障控制输入设计为：

[0081][0082][0078]

其中，auo(i，s，t)为跟随者i在障碍物s斥力场影响下的规避加速度；avuo(i，s，t)为

跟随者i 在障碍物s势场(斥力场)影响下的辅助牵引加速度，auu(i，j，t)为跟随者i在跟随者j势场 (包括斥力场和引力场)影响下的加速度。

8

CN 111290429 A[0083]

说　明　书

7/8页

步骤1.9、将步骤1.3，1.4和1.8中的各输入融合产生无人机编队与避障控制协议， 

后执行步骤1.10；

[0084]

[0085]

其中，u0(t)为虚拟领航者的控制输入，ui(t)为跟随者的控制输入。b1、b2、b3、b4、 

以及为各项加速度调节参数。

[0087]步骤1.10、更新各无人机状态，执行步骤1。[0088]进一步的，步骤1.3中，在领航者编队控制调用有限时间一致性算法得到领航者编 队控制输入之前，先获取目标点的位置和速度信息，领航者可利用该信息追踪目标点； 该信息的获取方式包括但不限于由地面站发送取得的、利用无人机携带的传感器检测取 得的以及依靠目标点自身发送取得的。[00]进一步的，步骤1.4中，跟随者编队控制在调用有限时间一致性算法产生跟随者编 队控制输入中，获取的是与之有通信关系的其他跟随者的位姿信息；所有的跟随者均只 与编队中的部分跟随者进行信息交互，而不获取全局编队信息，此举有利于减轻通信网 络的带宽消耗。

[0090]进一步的，步骤1.6中，跟随者避障控制在产生机间加速度中，获取的是与之有通 信关系的其他跟随者位姿信息；利用该信息可以在不增加额外通信网络的带宽消耗的同 时，提高无人机之间规避的速率。[0091]作为一种优选，本发明考虑四旋翼无人机机动性强这一特性，使用平衡区域代替传 统人工势场方法中的平衡点，该平衡区域的大小与无人机速度相关；当无人机处于斥力 场最大作用半径和引力场最小作用半径之间时，无人机i不受无人机j势场的作用；采用 平衡区域可减小人工势场方法对有限时间一致性算法收敛时间的影响；无人机势场中斥 力场最大作用半径和引力场最小作用半径定义为：

[0092][0093]

[0086]

其中，和为势场范围调节参数。[0095]作为一种优选，本发明通过调节和的大小以改变无人机斥力场和引力场影响范 围的边界；当且时，引力场和斥力场的边界相交，与传统人工势场方法中的 平衡点一致；当或时，引力场和斥力场的边界由无人机速度决定，其由传统 人工势场方法的平衡点转变为平衡区域。[0096]作为一种优选，本发明可通过调节以及的值以应对不同移动速度的障碍物； 当无人机群感知到障碍物的移动速度高于无人机的速度且无人机机群在障碍物斥力场 内时，本文令大于和使辅助牵引加速度的值大于其他加速度的值，从而迫使无 人

[0094]

9

CN 111290429 A

说　明　书

8/8页

机快速远离障碍物。

[0097]作为一种优选，本发明定义无人机机群中至少存在一个跟随者可获取虚拟领航者信 息，且对于任意一个跟随者，至少存在一个其他跟随者可与之进行双向通信，所有无人 机之间的通信均为同步的；其通信拓扑如图4所示。

10

CN 111290429 A

说　明　书　附　图

1/2页

图1

11

CN 111290429 A

说　明　书　附　图

2/2页

图2

图3

图4

12