基于零空间方法的四旋翼无人机避障与协同编队控制

杨钟煜，余自权，程月华，徐贵力

（南京航空航天大学自动化学院，江苏南京 210016）

0 引言

无人机在民用和军事[1-3]领域都发挥着越来越重要的作用，如电力线路检查、水土保持、森林防火[4-5]等。搜索并跟踪移动地面目标也是无人机预测目标意图并采取主动措施所需的主要能力，具体应用包括边境巡逻[6-7]、空中监视[8-9]和警察执法等。四旋翼无人机为最常见的一类无人机，它具有可垂直起降、控制灵活和适应性强等优点，在民用和军用领域都有广泛应用。

四旋翼无人机在编队执行任务中可能遭遇外界障碍物、群内可能发生机间碰撞，造成巨大损失，影响无人机飞行安全。目前针对避障问题应用较多的方法有人工势场法[10-12]、动态窗口法[13-14]、基于行为法[15-17]等。但在无人机编队执行任务时，需要其进行编队飞行、目标趋近和避障避撞等几项行为，而这些行为之间常存在冲突，基于行为的方法能很好地调节系统行为，解决冲突问题。整个行为调节的效果主要取决于行为选择机制。目前，行为选择机制主要有行为抑制法、加权平均法和模糊逻辑法[18]等。文献[19]提出的零空间行为法（NSB）即为模糊逻辑法的1种。在保持首要任务完成的前提下，可以充分利用零空间完成其他次要任务，具有实时性强、各子任务间零冲突的优点。文献[20]针对自主水下机器人在复杂海底环境驶向目标点的过程中可能遇到动、静态障碍物的问题，设计了基于零空间行为法的避障策略。文献[21]提出了1 种基于零空间和滑模的移动机器人控制方法，实现了队形保持和队形内避障的协调控制。文献[22]采用基于零空间的行为控制对各无人机的行为进行融合，解决了行为间的冲突问题。但现有文献中研究对象大多为智能体，并未考虑对应实际模型，且大多仅研究了路径规划上的避障策略，并未研究相应具体的控制方法。

考虑到这一研究现状，本文基于零空间方法，针对四旋翼无人机在编队执行任务时可能遭遇障碍物问题，提出了1 种四旋翼无人机避障与协同编队控制算法。首先，建立四旋翼无人机动力学模型，并建立虚拟控制量简化控制模型；然后，基于零空间方法进行避障与协同编队控制算法研究，将无人机任务执行分解为目标趋向任务、避障避撞任务和协同编队任务，并根据优先级进行任务融合得到期望速度；最后，基于PID方法设计控制律。

1 问题描述与预备知识

1.1 四旋翼无人机动力学模型

本文采用四旋翼无人机，其动力学模型表示为[23]：

式（1）中：xi、yi、zi为无人机的位置分量；ϕi、θi、ψi为无人机的姿态角，分别为滚转角、俯仰角和偏航角；J为旋翼转动惯量；Ix Iy Iz为3个轴的转动惯量；m为机身的质量；g为重力加速度；Ωri=ω1i-ω2i+ω3i-ω4i为4 个旋翼转速的残差；ωji(j=1,2,3,4)为4 个旋翼的转速；U1i为无人机的总升力；U2i、U3i、U4i分别为横滚力矩、俯仰力矩和偏航力矩。

四旋翼无人机是1个含有4个系统输入、6个系统输出的典型欠驱动系统。因此，根据四旋翼无人机动力学模型和坐标系转换矩阵，定义虚拟控制分量，使四旋翼无人机系统成为1 个6 个控制输入和6 个控制输出的全驱动系统。定义虚拟控制量：

进一步推导可得：

式（3）～（8）中，ϕd,i、θd,i、ψd,i表示姿态角的期望值。

1.2 基于零空间的行为控制方法

式（9）两边对pi求导得：

式（9）中，Ji(pi)为δi的雅可比矩阵。则可得到速度的最小二乘解为：

式（11）得到了单个任务的速度输出。如果存在多个任务，采用NSB的方法将次要任务的速度输出投影到优先任务的零空间上。第q个任务向较高一级任务零空间的投影为可表示为：

式（12）中，I为单位矩阵。以3个任务为例，得到速度输出为：

式（13）中，下标表示任务的优先级。

2 基于零空间方法的避障与协同编队控制算法研究

四旋翼无人机编队飞行时，可将任务执行分解为3种任务：避障避撞任务、协同编队任务和目标趋向任务。

2.1 避障避撞任务

在无人机飞行过程中，避障避撞任务能保证无人机的安全和完整，为后续任务的实现提供基本的保障，故其拥有最高的优先级。假设障碍物在前进方向上，目的是保持无人机与障碍物和相邻无人机之间的安全距离，为方便分析与算法设计，将相邻无人机同样 视 为 障 碍 物 ，定 义 障 碍 物 坐 标 为po,m=(xo,m,yo,m,zo,m)T，m=1,2,…,M。无人机可控的任务变量为：

式（14）中，Do,i为无人机与障碍物之间的距离。定义安全距离为do，对可控变量δo,i求雅克比矩阵，得到：

则避障避撞任务的输出为：

式（16）中，λo,i为避障避撞增益系数。

在机间避撞时，提出1种优先级机间避撞策略，即在需要考虑将相邻无人机视为障碍物时，采取按优先级选择障碍物进行避障的方式：将各无人机进行编号，在避撞时会选择在避障范围内比自身序号小的无人机进行优先避障，如2 号无人机只对1 号无人机进行避撞，3 号无人机只对1 号和2 号无人机进行避撞，以此类推。此种机间避撞策略可以有效避免重复的双向选择，避免执行避撞行为的2 架无人机产生越飞越远的现象。

2.2 协同编队任务

多无人机编队任务涉及各个无人机之间的相互协作。为实现无人机编队，以各无人机与编队队形中心的位置偏差为输入建立任务函数。定义多无人机编队队形中心位置坐标为pc=(xc,yc,zc)T，取无人机可控的任务变量为：

式（17）中，Df,i为第i架无人机与编队队形中心点之间的坐标偏差。定义第i架无人机当前位置点与编队队形中心点的期望坐标偏差为df,i，对可控变量δf,i求雅克比矩阵，得到：

则协同编队任务的输出为：

式（19）中，λf,i为协同编队增益系数。

2.3 目标趋向任务

目标趋向任务控制无人机向目标点飞行。定义目标点位置为pd,i=(xd,i,yd,i,zd,i)T，可控的任务变量为δd,i=pi。引入四旋翼无人机最大飞行速度、实时位置坐标和飞行目标位置坐标，对增益系数进行重新定义，保证规划得到的最终速度始终在无人机飞行能力范围内。对可控变量δd,i求雅克比矩阵，得到：

则目标趋向任务的输出为：

式（21）～（23）中：λd,i为目标趋向增益系数；vmax为无人机最大飞行速度；变量rd为趋向目标任务的调节因子。

2.4 基于零空间方法的任务融合

得到避障避撞任务、协同编队任务和目标趋向任务的输出后，考虑避障避撞任务优先级最高，其次为协同编队任务优于目标趋向任务，采取NSB的方法将其综合，可以得到最终的速度输出为：

2.5 无人机控制算法设计

采用PID 控制方法作为无人机位置和姿态控制方法，首先定义如下速度跟踪误差与姿态跟踪误差：

式（27）（28）中：Θi=(ϕi,θi,ψi)T；Θd,i=(ϕd,i,θd,i,ψd,i)T。

则可得到控制律如下：

式 （29） （30） 中 ：Uv,i=(Uxi,Uyi,Uzi)T；Ua,i=(Uϕi,Uθi,Uψi)T；KP、KI、KD、kP、kI、kD为待设计的参数对角矩阵，且其元素均为正常数。最后由式（3）～（6），即可计算得到控制输入U1i、U2i、U3i、U4i。

3 仿真结果及分析

本文通过仿真对提出的基于零空间方法的四旋翼无人机避障与协同编队控制算法进行有效性验证。考虑编队中共有4架四旋翼无人机（见表1）。彼此之间均可进行信息交互，即每架无人机均可获得其他无人机的位置信息。

表1 无人机相关参数表Tab.1 Related parameters of UAV

图1 展示了各无人机的飞行轨迹。可以看出，各无人机在遭遇障碍物时，可以及时进行避障，保证自身安全。1号无人机在避障后靠近4号无人机，4号无人机同时远离1号无人机保持飞行距离以避免相互碰撞。4架无人机在避障前后均可以维持设定的队形进行飞行，并向各自目标点趋近。

图1 各无人机的飞行轨迹Fig.1 Flight path of UAVs

各无人机在x、y、z方向上的线速度见图2；图3为各无人机的控制信号U1i，由图可知各无人机可以在遭遇障碍物时及时作出机动反应；图4 为各无人机的滚转角。

图2 各无人机的线速度分量Fig.2 Linear velocity components of UAVs

图3 各无人机的控制信号U1iFig.3 Control signal U1i of UAVs

图4 各无人机的滚转角Fig.4 Roll angle of UAVs

从以上仿真结果和分析中可知，本文提出的方案可以使四旋翼无人机在编队飞行时及时进行避障以免相互避撞，保证了各无人机在遭遇障碍物时的飞行安全。

4 结论

本文针对四旋翼无人机在编队飞行执行任务时可能遭遇障碍物问题，考虑多无人机避障及机间避撞的需求，提出了1 种基于零空间方法的四旋翼无人机避障与协同编队控制算法。首先，建立了四旋翼无人机动力学模型，并建立虚拟控制量简化控制模型；然后，基于零空间方法进行了避障与协同编队控制算法研究，将无人机任务执行分解为目标趋向任务、避障避撞任务和协同编队任务3 种任务，根据优先级进行任务融合得到期望速度，并基于PID 方法设计了控制律；最后，通过仿真验证了所提控制算法的有效性。所提方法可保证四旋翼无人机在编队飞行中遭遇障碍物时的飞行安全。