CN108958289A - 基于相对速度障碍的集群无人机避碰方法 - Google Patents

Abstract

本发明涉及无人机飞行器避碰技术领域，为实现无人机编队在任务过程中，保证编队内部不发生碰撞，并且在避碰之后能够回到先前的预定航路上。本发明提出的策略，通过建立四旋翼无人机轨迹姿态模型、设计协同控制策略及集群无人机避碰算法，实现集群无人机从初始位置到目标位置的安全飞行。为此，本发明采用的技术方案是，基于相对速度障碍的集群无人机避碰方法，步骤如下：第一部分，基于牛顿–欧拉方程的四旋翼无人机轨迹姿态模型建立；第二部分，分布式串级比例积分PID控制器设计；第三部分，集群无人机避碰算法设计：对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度。本发明主要应用于无人机控制场合。

Description

基于相对速度障碍的集群无人机避碰方法

技术领域

本发明涉及无人机飞行器避碰技术领域，尤其涉及集群无人机避碰方法领域。

背景技术

无人机(Unmanned Aerial Vehicle,UAV)是一种近年来备受研究者关注的无人驾驶飞行器,其在执行搜索救援、监督执法、空中测绘和航空摄影等方面的潜在价值逐渐被挖掘利用。多无人机协同自主编队飞行是近年来的一个前沿领域,可充分利用有限单机资源,共同执行复杂任务。避碰问题是无人机编队(Unmanned Aerial Vehicle Team，UAV Team)执行任务时必须要解决的关键问题之一，是无人机获得航空监管部门授权，飞入通用空域的关键，也是无人机自主化进程中的关键环节。因此，世界各国正在大力推进相关技术的研究，包括美国空军和美国国防研究协会主导以协调无人机系统航空空域飞行为重点的重大项目，英国以航空工业界为主导的“自主系统技术相关空中评估”项目，以及由欧洲防务局组织实施的欧洲研究计划等。这些重大研究计划项目的核心便是无人机的避碰技术。

目前，国内外相关学者提出的集群无人机避碰方法主要有人工势场法、混合整数序列规划法和搜索论法。1)人工势场法：人工势场法的算法理念起源于物理学中的电场力作用原理，即将无人机个体看作存在于电场中的电子，无人机之间存在相互排斥的作用力从而保证期望的机间距离，无人机的运动方向和轨迹就是其所受到合力的方向。人工势场法于1986年一经提出，便得到了广泛的应用与关注。然而，人工势场法易陷入“局部困扰”。所谓“局部困扰”是指：对势场方程的简单选取极易使无人机在面对多架邻机时失去合力作用，进而在缺少驱动作用的情况下，使无人机的运动行为停滞或深陷于局部中无限徘徊。该种情况的发生即意味着无人机避碰失效。2)混合整数序列规划法：混合整数序列规划提前为每架无人机生成最优轨迹，将避碰表示为整数约束。这是一种集中式控制思想，它的局限性是每架无人机都需要知道其它所有无人机的起始位置和目标位置，以此来规划自身轨迹。因此，计算复杂度随无人机数量急剧增加，严重影响了算法的实时性。3)搜索论法：是以搜索论为基础的传统方法，最大化发现概率设计搜索航迹。通常，这样规划的航路是固定的，不具有实时动态规划的能力，一旦突发状况无法及时处理。方法同时存在一定的问题，不能在保证求解问题精度和提高问题的求解速度这两者间做出很好的平衡。

综上所述，使每架无人机在没有全局信息的情况下只依赖邻机信息实现集群无人机间的避碰尤为重要，因为只有这样才能使无人机避碰不受集群规模影响。针对现有避碰策略的不足，本发明首次提出了基于相对速度障碍的集群无人机避碰方法。该方法通过实时计算避碰期望速度，确保集群无人机由指定的起点，能够安全、无碰撞地朝着各自目标点飞行。

发明内容

为克服现有技术的不足，本发明旨在提出一种基于相对速度障碍的集群无人机避碰方法，无人机编队在任务过程中，要求保证编队内部不发生碰撞，即在无人机执行各自任务时，无论是几架无人机同时相遇，都能够协同一致的相互避开，并且在避碰之后能够回到先前的预定航路上。本发明提出的策略，通过建立四旋翼无人机轨迹姿态模型、设计协同控制策略及集群无人机避碰算法，实现集群无人机从初始位置到目标位置的安全飞行。为此，本发明采用的技术方案是，基于相对速度障碍的集群无人机避碰方法，步骤如下：

第一部分，基于牛顿––欧拉方程的四旋翼无人机轨迹姿态模型建立：四旋翼飞行器是具有四个输入和六个自由度可实现垂直起降的欠驱动强耦合旋翼式飞行器，根据牛顿–欧拉方程对四旋翼飞行器进行了动力学建模；

第二部分，分布式串级比例积分PID控制器设计：设计分布式串级比例积分PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计；

第三部分，集群无人机避碰算法设计：对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度。

具体地：

第一部分，基于牛顿––欧拉方程的四旋翼无人机姿态模型建立，四旋翼飞行器是一种能实现垂直起降的非共轴式多旋翼飞行器，只通过调节蝶形分布的四个旋翼的转速，实现对四旋翼飞行器飞行姿态的控制，其中以第i架无人机为例，建立四旋翼无人机的轨迹姿态数学模型如下，其余无人机控制以此类推：

其中，p_i＝[x_i,y_i,z_i]^T∈R³表示惯性坐标系下三个坐标轴方向的位置，表示惯性坐标系下x,y,z三个方向的线速度，表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率，g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，表示无人机的总升力，表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，旋转矩阵R_i和映射函数f(Ω_i)定义如下

φ_i,θ_i,ψ_i分别表示惯性坐标系下的滚转角、俯仰角及偏航角，控制量与电机转速之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数；

第二部分，分布式串级比例积分PID控制器设计：该部分是设计分布式串级比例积分PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计，进一步地：

A)位置控制器

定义位置跟踪误差

其中，表示第i架无人机期望的位置矢量信息，则期望速度为

其中，为常值向量，表示位置控制器位置环比例增益，随后用集群无人机避碰算法根据第i架无人机的期望速度以及邻机的位置和速度优化产生新的避碰期望速度

定义速度跟踪误差

其中v_i表示第i架无人机的速度矢量信息，

则期望加速度为

其中，为常值向量，分别表示位置控制器速度环比例、积分和微分增益。

外环控制量为

B)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的控制指令转化为期望的飞行姿态，具体解算公式为

r_i1＝r_i2×r_i3 (14)

其中为期望姿态，r_ij,(j＝1,2,3)为期望姿态矩阵的第j列，ψ_d为期望偏航角；

C)姿态控制器

定义姿态跟踪误差

其中f^-1为映射函数f(Ω_i)的逆映射，期望角速度设计为

其中，为常值向量，表示姿态控制器姿态环比例增益，

定义角速度跟踪误差

内环控制量设计为

其中，为常值向量，分别表示姿态控制器姿态角速率环比例、积分和微分增益。

第三部分，集群无人机避碰算法设计：实现对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度，具体地，对于两架无人机i和j，速度障碍是导致i与j在时间τ之前发生碰撞的相对速度集合：

其中，r表示每架四旋翼无人机的安全半径，时间τ越大，无人机对其他无人机的出现反应越迅速，但在自身速度选择上的自由度越小，令D(p,r)表示以p为中心半径为R的圆盘：

D(p,R)＝{q|||q-p||<R} (21)

则

两架无人机i和j的速度障碍为无数个以两架无人机位置中心连线为对称轴的圆盘的交集，并且这些圆有两条过原点的公切线，速度障碍的完整几何形状边界由圆弧和两条公切线共同组成，和是关于原点对称的，类似地，二维平面的速度障碍绕对称轴进行旋转可以得到三维平面的速度障碍；

令v_i和v_j分别是无人机i和j的当前速度，由速度障碍的定义，如果或时，则当i与j保持当前速度继续移动时，i与j会在时间τ之前发生碰撞，反之当时，无人机i和j至少在时间τ内保证不会发生碰撞；

令表示集合X和Y的闵可夫斯基和：

则对于任意集合V_j，如果v_j∈V_j并且则i和j以当前速度至少在时间τ内保证不会碰撞，指定j从集合V_j选择自身速度，引出i的避碰速度集合定义：

当并且时，称i和j的一对速度集合V_i和V_j为相互避碰的，当并且时，称V_i和V_j互为最大相互避碰速度集合，

基于上述定义，为i和j选取允许的速度集合V_i和V_j使得并且即V_i和V_j是相互避碰的且互为最大相互避碰速度集合，保证i和j至少在时间τ内不会碰撞。有无数对满足这些要求的集合V_i和V_j，在它们当中选择靠近i和j的当前速度v_i和v_j的允许速度最多的一对。将i的最优相互避碰速度集合表示为j的最优相互避碰速度集合为并且正式定义如下：

定义和使得它们是相互避碰的且互为最大相互避碰速度集合，即且并且对所有其它的相互避碰速度集合V_i和V_j，以及所有半径R>0,使得

其中|V|表示集合V的测度，即R²中的面积，这表示和分别包含更多靠近v_i和v_j的速度，另外，允许速度的分布是平均的，即靠近i和j当前速度的速度数量是相等的，

假设i和j处于产生碰撞的轨迹，即令u为从v_i-v_j指向速度障碍边界上最近一点的向量，

并且令n为在(v_i-v_j)+u点指向边界外的法向量，则u为i和j在时间τ内避免碰撞所需的相对速度的最小改变量，为了以一个平均的方式在无人机之间分担避碰责任，无人机i至少改变自身速度并且假设无人机j承担另一半。因此i的允许速度集合为在起始点指向n方向的半平面：

对于j对称的定义集合当i和j当前速度没有处于产生碰撞的轨迹，即时，以上等式同样适用，在这种情况下，无人机各自承担一半保持在无碰撞轨迹的责任；

只要无人机能够观测互相的位置，安全半径和当前速度，i和j能够在不互相通信的情况下分别计算和

接下来应用最优相互避碰定义在多架无人机之间进行避碰：

每架无人机i以时间步长△t进行感知和运动的连续循环，在每个循环中，无人机获得其他无人机和自身的当前位置和当前速度，根据这个信息，无人机关于其他每架无人机j计算允许速度的半平面i关于所有无人机允许速度的集合为关于其他每架无人机允许速度的半平面的交集，把这个集合表示为

这个定义也包括了无人机i的最大速度约束，

最后，无人机在允许速度区域内的所有速度中选择最靠进自身期望速度的避碰期望速度使无人机解决避碰问题时，偏离预定路线尽可能小：

使用线性规划单纯形法求解上述问题。

本发明的特点及有益效果是：

本发明以理论推导和虚拟仿真技术相结合为主要研究手段，提出一种基于相对速度障碍的集群无人机避碰方法，该方法能够确保集群无人机执行各自任务时，无论是几架无人机同时相遇，都能够协同一致的相互避开，并且在避碰之后能够回到先前的预定航路上。

附图说明：

附图1 12架无人机避碰飞行示意图。

附图2集群无人机避碰方法结构框图。

附图3两架无人机位置。

附图4速度障碍对称轴。

附图5速度障碍完整几何形状。

附图6最优相互避碰半平面。

具体实施方式

本发明涉及四旋翼无人机飞行避碰技术领域。具体来说，首先提出了不同于以往传统集群无人机避碰的基于相对速度障碍的集群无人机避碰方法，该方法通过实时计算避碰速度，只依赖邻机的位置和速度信息，可有效避免传统集群无人机为每架无人机规划轨迹的不足，集群无人机的避碰通过无人机期望速度的实时优化得到。随后通过设置飞行场景，对本发明提出策略的有效性进行验证。

本发明以理论推导和虚拟仿真技术相结合为主要研究手段，提出一种基于相对速度障碍的集群无人机避碰方法，该方法能够确保集群无人机执行各自任务时，无论是几架无人机同时相遇，都能够协同一致的相互避开，并且在避碰之后能够回到先前的预定航路上。

本发明提出的基于相对速度障碍的集群无人机避碰方法，主要包括以下三部分：

第一部分，基于牛顿––欧拉方程的四旋翼无人机轨迹姿态模型建立：四旋翼飞行器是一种具有四个输入和六个自由度可实现垂直起降的欠驱动强耦合旋翼式飞行器。为了解决四旋翼飞行器的飞行控制问题，根据牛顿–欧拉方程对四旋翼飞行器进行了动力学建模。

第二部分，分布式串级PID控制器设计：该部分主要目的是设计分布式串级PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计。

第三部分，集群无人机避碰算法设计：该部分主要对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度。

最后，为了验证本发明提出的基于相对速度障碍的集群无人机避碰方法的有效性，在MATLAB\Simulink仿真平台上搭建集群无人机避碰系统，通过设置飞行场景，对本发明提出策略的有效性进行验证。

本发明提出的基于相对速度障碍的集群无人机避碰方法，在MATLAB/Simulink环境下完成了集成设计及验证，具体过程如下：

(1)参数设置

1)四旋翼无人机物理参数：质量m＝1.1kg，惯性参数I_x＝1.431×10^-2kgm²，I_y＝1.431×10^-3kgm²，I_z＝2.721×10^-3kgm²，旋翼转动中心到无人机中心的距离d＝0.225m，旋翼的升力系数k_F＝9.832×10^-6Nms²/rad²,旋翼的扭矩系数k_M＝1.32×10^-7Ns²/rad²。

2)控制器参数：位置控制器位置环比例增益速度环比例增益积分增益微分增益姿态解算算法期望偏航角ψ_d＝0。姿态控制器姿态环比例增益姿态角速率环比例增益积分增益微分增益

3)其它参数设置：每架四旋翼无人机的安全半径r＝0.65m，无人机i的最大速度仿真测试验证过程中，采样时间设置为定步长20毫秒。

(2)仿真结果及性能分析

以12架四旋翼无人机为例，对上述提出的分布式串级PID控制器及集群无人机避碰算法的有效性进行数值仿真验证。基于本发明提出的策略，通过设定正方形编队的12架无人机垂直起飞后向各自关于正方形编队中心对称的位置飞行的场景，在MATLAB环境中，利用本发明提出的算法对飞行场景中的集群无人机避碰问题进行求解，能够很好的满足无人机避碰实时性要求。用于12架四旋翼无人机避碰飞行的示意图如图1所示，从中可以看出基于本发明给出的策略，能实现多无人机的自主避碰飞行。进一步，表1中的数表示其行列指数对应的两架无人机在整个飞行过程中的最小距离，从中可以看出集群无人机飞行过程中任意两架无人机之间的最小距离为1.04m，依然大于安全距离1m，因此可以实现集群无人机从初始位置到目标位置的安全飞行。

表1任意两架无人机之间最小距离

以下结合附图对本发明作进一步详述。

基于相对速度障碍的集群无人机避碰方法总体结构图如图2所示。具体实现

步骤如下：

第一步，基于牛顿––欧拉方程的四旋翼无人机姿态模型建立。四旋翼飞行器是一种能实现垂直起降的非共轴式多旋翼飞行器，可以只通过调节蝶形分布的四个旋翼的转速，实现对四旋翼飞行器飞行姿态的控制。为了解决四旋翼飞行器的飞行控制问题，以第i架无人机为例，建立四旋翼无人机的轨迹姿态数学模型如下

其中，p_i＝[x_i,y_i,z_i]^T∈R³表示惯性坐标系下三个坐标轴方向的位置，表示惯性坐标系下x,y,z三个方向的线速度，表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率。g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，表示无人机的总升力，表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，旋转矩阵R_i和映射函数f(Ω_i)定义如下

φ_i,θ_i,ψ_i分别表示惯性坐标系下的滚转角、俯仰角及偏航角。控制量与电机转速之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数。

第二步，分布式串级PID控制器设计：该部分主要目的是设计分布式串级PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计。

D)位置控制器

定义位置跟踪误差

其中表示第i架无人机期望的位置矢量信息，则期望速度为

其中，为常值向量，表示位置控制器位置环比例增益。随后用集群无人机避碰算法根据第i架无人机的期望速度以及邻机的位置和速度优化产生新的避碰期望速度具体步骤将在下一部分详细说明。

定义速度跟踪误差

其中v_i表示第i架无人机的速度矢量信息。

则期望加速度为

其中，为常值向量，分别表示位置控制器速度环比例、积分和微分增益。

外环控制量为

E)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的控制指令转化为期望的飞行姿态，具体解算公式为

r_i1＝r_i2×r_i3 (14)

其中为期望姿态，r_ij,(j＝1,2,3)为期望姿态矩阵的第j列，ψ_d为期望偏航角。

F)姿态控制器

定义姿态跟踪误差

其中f^-1为映射函数f(Ω_i)的逆映射。期望角速度设计为

其中，为常值向量，表示姿态控制器姿态环比例增益。

定义角速度跟踪误差

内环控制量设计为

其中，为常值向量，分别表示姿态控制器姿态角速率环比例、积分和微分增益。

第三步，集群无人机避碰算法设计：该部分主要对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度。

对于两架无人机i和j，速度障碍是导致i与j在时间τ之前发生碰撞的相对速度集合。

其中，r表示每架四旋翼无人机的安全半径，时间τ越大，无人机对其他无人机的出现反应越迅速，但在自身速度选择上的自由度越小。令D(p,r)表示以p为中心半径为R的圆盘：

D(p,R)＝{q|||q-p||<R} (21)

则

以二维平面为例，当两架无人机i和j的位置如图3所示，速度障碍为无数个以两架无人机位置中心连线为对称轴的圆盘的交集，并且这些圆有两条过原点的公切线，如图4所示，速度障碍的完整几何形状如图5中灰色区域向无穷远平面无限延伸，其边界由圆弧和两条公切线共同组成。注意和是关于原点对称的。类似地，二维平面的速度障碍绕对称轴进行旋转可以得到三维平面的速度障碍。

令v_i和v_j分别是无人机i和j的当前速度，由速度障碍的定义，如果或时，则当i与j保持当前速度继续移动时，i与j会在时间τ之前发生碰撞。反之当时，无人机i和j至少在时间τ内保证不会发生碰撞。

令表示集合X和Y的闵可夫斯基和：

则对于任意集合Vj，如果vj∈Vj并且则i和j以当前速度至少在时间τ内保证不会碰撞。指定j从集合Vj选择自身速度，引出i的避碰速度集合定义：

当并且时，称i和j的一对速度集合V_i和V_j为相互避碰的。当并且时，称V_i和V_j互为最大相互避碰速度集合。

基于上述定义，为i和j选取允许的速度集合V_i和V_j使得并且即V_i和V_j是相互避碰的且互为最大相互避碰速度集合，保证i和j至少在时间τ内不会碰撞。有无数对满足这些要求的集合V_i和V_j，在它们当中选择靠近i和j的当前速度v_i和v_j的允许速度最多的一对。将i的最优相互避碰速度集合表示为j的最优相互避碰速度集合为并且正式定义如下。

定义和使得它们是相互避碰的且互为最大相互避碰速度集合，即且并且对所有其它的相互避碰速度集合V_i和V_j，以及所有半径R>0,使得

其中|V|表示集合V的测度(即R²中的面积)。这表示和分别包含更多靠近v_i和v_j的速度。另外，允许速度的分布是平均的，即靠近i和j当前速度的速度数量是相等的。

假设i和j处于产生碰撞的轨迹，即令u为从v_i-v_j指向速度障碍边界上最近一点的向量，如图6所示

并且令n为在(v_i-v_j)+u点指向边界外的法向量。则u为i和j在时间τ内避免碰撞所需的相对速度的最小改变量。为了以一个平均的方式在无人机之间分担避碰责任，无人机i至少改变自身速度并且假设无人机j承担另一半。因此i的

允许速度集合为在起始点指向n方向的半平面。

对于j对称的定义集合如图6所示。当i和j当前速度没有处于产生碰撞的轨迹，即时，以上等式同样适用。在这种情况下，无人机各自承担一半保持在无碰撞轨迹的责任。

可以看出如上构造的和实际上是最优的。只要无人机能够观测互相的位置，安全半径和当前速度，i和j能够在不互相通信的情况下分别计算和

接下来应用最优相互避碰定义在多架无人机之间进行避碰。

每架无人机i以时间步长△t进行感知和运动的连续循环。在每个循环中，无人机获得其他无人机和自身的当前位置和当前速度。根据这个信息，无人机关于其他每架无人机j计算允许速度的半平面i关于所有无人机允许速度的集合为关于其他每架无人机允许速度的半平面的交集。把这个集合表示为

注意这个定义也包括了无人机i的最大速度约束。

最后，无人机在允许速度区域内的所有速度中选择最靠进自身期望速度的避碰期望速度使无人机解决避碰问题时，偏离预定路线尽可能小：

使用线性规划单纯形法求解上述问题。

基于以上三步，完成了基于相对速度障碍的集群无人机避碰方法的全部设计流程。

Claims (2)

1.一种基于相对速度障碍的集群无人机避碰方法，其特征是，步骤如下：

第一部分，基于牛顿––欧拉方程的四旋翼无人机轨迹姿态模型建立：四旋翼飞行器是具有四个输入和六个自由度可实现垂直起降的欠驱动强耦合旋翼式飞行器，根据牛顿–欧拉方程对四旋翼飞行器进行了动力学建模；

第二部分，分布式串级比例积分PID控制器设计：设计分布式串级比例积分PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计；

第三部分，集群无人机避碰算法设计：对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度。

2.如权利要求1所述的基于相对速度障碍的集群无人机避碰方法，其特征是，具体地：

第一部分，基于牛顿––欧拉方程的四旋翼无人机姿态模型建立，四旋翼飞行器是一种能实现垂直起降的非共轴式多旋翼飞行器，只通过调节蝶形分布的四个旋翼的转速，实现对四旋翼飞行器飞行姿态的控制，其中以第i架无人机为例，建立四旋翼无人机的轨迹姿态数学模型如下，其余无人机控制以此类推：

其中，p_i＝[x_i,y_i,z_i]^T∈R³表示惯性坐标系下三个坐标轴方向的位置，表示惯性坐标系下x,y,z三个方向的线速度，表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率，g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，表示无人机的总升力，表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，旋转矩阵R_i和映射函数f(Ω_i)定义如下

φ_i,θ_i,ψ_i分别表示惯性坐标系下的滚转角、俯仰角及偏航角，控制量与电机转速之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数；

第二部分，分布式串级比例积分PID控制器设计：该部分是设计分布式串级比例积分PID控制器，相关技术包括外环位置控制器、姿态解算及内环的姿态控制器设计，进一步地：

A)位置控制器

定义位置跟踪误差

其中，表示第i架无人机期望的位置矢量信息，则期望速度为

其中，为常值向量，表示位置控制器位置环比例增益，随后用集群无人机避碰算法根据第i架无人机的期望速度以及邻机的位置和速度优化产生新的避碰期望速度

定义速度跟踪误差

其中v_i表示第i架无人机的速度矢量信息，

则期望加速度为

其中，为常值向量，分别表示位置控制器速度环比例、积分和微分增益。

外环控制量为

B)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的控制指令转化为期望的飞行姿态，具体解算公式为

r_i1＝r_i2×r_i3 (14)

其中为期望姿态，r_ij,(j＝1,2,3)为期望姿态矩阵的第j列，ψ_d为期望偏航角；

C)姿态控制器

定义姿态跟踪误差

其中f^-1为映射函数f(Ω_i)的逆映射，期望角速度设计为

其中，为常值向量，表示姿态控制器姿态环比例增益，

定义角速度跟踪误差

内环控制量设计为

其中，为常值向量，分别表示姿态控制器姿态角速率环比例、积分和微分增益。

第三部分，集群无人机避碰算法设计：实现对控制器产生的期望速度进行优化，根据邻机的位置和速度产生新的避碰期望速度，具体地，对于两架无人机i和j，速度障碍是导致i与j在时间τ之前发生碰撞的相对速度集合：

其中，r表示每架四旋翼无人机的安全半径，时间τ越大，无人机对其他无人机的出现反应越迅速，但在自身速度选择上的自由度越小，令D(p,r)表示以p为中心半径为R的圆盘：

D(p,R)＝{q|||q-p||<R} (21)

则

两架无人机i和j的速度障碍为无数个以两架无人机位置中心连线为对称轴的圆盘的交集，并且这些圆有两条过原点的公切线，速度障碍的完整几何形状边界由圆弧和两条公切线共同组成，和是关于原点对称的，类似地，二维平面的速度障碍绕对称轴进行旋转可以得到三维平面的速度障碍；

令v_i和v_j分别是无人机i和j的当前速度，由速度障碍的定义，如果或时，则当i与j保持当前速度继续移动时，i与j会在时间τ之前发生碰撞，反之当时，无人机i和j至少在时间τ内保证不会发生碰撞；

令表示集合X和Y的闵可夫斯基和：

则对于任意集合V_j，如果v_j∈V_j并且则i和j以当前速度至少在时间τ内保证不会碰撞，指定j从集合V_j选择自身速度，引出i的避碰速度集合定义：

当并且时，称i和j的一对速度集合V_i和V_j为相互避碰的，当并且时，称V_i和V_j互为最大相互避碰速度集合，

基于上述定义，为i和j选取允许的速度集合V_i和V_j使得并且即V_i和V_j是相互避碰的且互为最大相互避碰速度集合，保证i和j至少在时间τ内不会碰撞。有无数对满足这些要求的集合V_i和V_j，在它们当中选择靠近i和j的当前速度v_i和v_j的允许速度最多的一对。将i的最优相互避碰速度集合表示为j的最优相互避碰速度集合为并且正式定义如下：

定义和使得它们是相互避碰的且互为最大相互避碰速度集合，即且并且对所有其它的相互避碰速度集合V_i和V_j，以及所有半径R>0,使得

其中|V|表示集合V的测度，即R²中的面积，这表示和分别包含更多靠近v_i和v_j的速度，另外，允许速度的分布是平均的，即靠近i和j当前速度的速度数量是相等的，

假设i和j处于产生碰撞的轨迹，即令u为从v_i-v_j指向速度障碍边界上最近一点的向量，

并且令n为在(v_i-v_j)+u点指向边界外的法向量，则u为i和j在时间τ内避免碰撞所需的相对速度的最小改变量，为了以一个平均的方式在无人机之间分担避碰责任，无人机i至少改变自身速度并且假设无人机j承担另一半。因此i的允许速度集合为在起始点指向n方向的半平面：

对于j对称的定义集合当i和j当前速度没有处于产生碰撞的轨迹，即时，以上等式同样适用，在这种情况下，无人机各自承担一半保持在无碰撞轨迹的责任；

只要无人机能够观测互相的位置，安全半径和当前速度，i和j能够在不互相通信的情况下分别计算和

接下来应用最优相互避碰定义在多架无人机之间进行避碰：

每架无人机i以时间步长△t进行感知和运动的连续循环，在每个循环中，无人机获得其他无人机和自身的当前位置和当前速度，根据这个信息，无人机关于其他每架无人机j计算允许速度的半平面i关于所有无人机允许速度的集合为关于其他每架无人机允许速度的半平面的交集，把这个集合表示为

这个定义也包括了无人机i的最大速度约束，

最后，无人机在允许速度区域内的所有速度中选择最靠进自身期望速度的避碰期望速度使无人机解决避碰问题时，偏离预定路线尽可能小：

使用线性规划单纯形法求解上述问题。