分布式无人机队列与动态避障控制方法
技术领域
本发明涉及一种四旋翼无人机的协同队列控制与动态避障控制方法。针对多无人机集群飞行中的协同巡航任务需求，并考虑巡航过程中的避障问题，设计了一套基于一致性协议的分布式多无人机队列控制与动态避障控制算法。
背景技术
多无人机协同飞行具有探测范围大、负载能力高和环境适应能力强等突出的优点，能够完成复杂的飞行任务和提高整体工作效率，成为近年来无人机领域的研究热点。自主巡航是多无人机执行协同飞行任务的基本方式之一，在军事演练、交通管制、区域侦查和电力巡检等应用场景发挥着重要的作用。因此，研究多无人机的协同巡航控制具有重要的应用价值。
在多无人机执行协同飞行任务的过程中，队形的建立与保持是完成任务的基础，由此产生的协同控制问题既是核心也是难点。目前，多无人机队形控制方法主要有Leader-Follower(领航-跟随)法、基于行为法、虚拟结构法和一致性法等，国内外许多学者对其进行了深入的研究，获得了丰富的研究成果。但在上述方法中，无人机群多采用相对位置矢量或虚拟结构的方法来构成和保持队形，其协同飞行主要通过给定时变的编队中心函数实现，在实际的巡航过程中，多架无人机需要沿相同的期望路径完成协同飞行任务，各无人机间的相对位置和速度矢量时刻变化。同时，考虑巡航任务的实质为路径跟踪问题，即巡航路径为时不变的给定空间路径，其无需表达为具有时变参数的轨迹形式，也不需要无人机在某一时刻到达特定的期望位置，因此直接采用传统的多无人机编队方式设计巡航控制策略具有一定困难。为解决上述问题，一些学者提出了TFL(Transverse FeedbackLinearization,轴切向线性化)方法，将系统状态映射到与期望路径相切和相交的空间上，实现运动系统对固定路径的跟踪。瑞士苏黎世联邦理工学院的研究人员基于TFL方法研究了具有路径限制的多无人机控制问题，实现在同一期望路径上的机间协同，并通过实物实验验证了所设计控制器的有效性(会议：IEEE Conference on Control Technology andApplications；著者：Sun J W,Gill R；出版年月：2019；文章题目：Vehicle platooncontrol with virtual path constraints；页码：456-461)。
在多无人机的协同巡航过程中，不仅要考虑队列内部的相互协同，更需要应对复杂的周围环境和突发状况给任务执行带来的诸多不便，其中外部障碍物的威胁是影响飞行任务顺利执行的重要因素。因此，有效的防撞和避障策略是保障飞行安全的前提和关键。美国达特茅斯学院的一些学者研究了多智能体系统(multi-agent system)的群集行为，设计非线性的一致性协议实现多智能体的群集收敛和避障，并给出了相关的理论证明，通过仿真实验验证了算法的有效性(期刊：IEEE Transactions on Automatic Control；著者：R.Olfati-Saber；出版年月：2006年；文章题目：Flocking for multi-agent dynamicsystems:algorithms and theory；页码：401-420)。日本庆应大学的研究团队研究了基于人工势场法的多无人机编队防撞问题，在一致性控制器中加入避障项，证明了控制算法的稳定性，并通过数值仿真验证了避障效果，但其采取仅在高度上进行机间防撞控制的策略，没有考虑其他方向的动作(会议：American Control Conference；著者：Kuriki Y,Namerikawa T；出版年月：2014；文章题目：Consensus-based cooperative formationcontrol with collision avoidance for a multi-UAV system；页码：2077-2082)。美国斯坦福大学的研究团队采用矢量场驱动的方法实现了多无人机集群飞行过程中对静态障碍物的规避，将多无人机群体视为虚拟刚体结构，通过操纵地面站连接的手柄控制虚拟刚体的平移、旋转和结构变换，同时保证虚拟刚体移动中单架无人机能够避开环境障碍(期刊：IEEE Transactions on Robotics；著者：Zhou DJ，Wang ZJ，Schwager M；出版年月：2018年8月；文章题目：Agile coordination and assistive collision avoidance forquadrotor swarms using virtual structures；页码：916-923)。该文献采用分散式的控制方式，虚拟刚体中的各顶点无人机之间没有相互协同，仅通过与刚体中心保持固定的位置矢量形成编队，并缺乏相关的理论证明。也有一些学者采用数学规划的方法，使无人机在遇到障碍时自主决策轨迹实现躲避障碍。该类方法在一定程度内可以保障存在障碍物时的飞行安全，但对无人机跟踪性能要求较高，且当环境和动力学约束条件复杂时存在求解优化方程计算量较大和实时性难以保证等问题,其稳定性的证明也较为困难。
发明内容
为克服现有技术的不足，本发明旨在提出一种基于滑模与一致性协议的多无人机协同控制与避障算法，实现无人机集群巡航任务与巡航路径上的协同避障。为此，本发明采用的技术方案是，分布式无人机队列与动态避障控制方法，采用Transverse FeedbackLinearization(TFL)方法对四旋翼无人机的动力学模型进行微分同胚映射变换,使其分解为沿期望路径方向的切向子系统和与期望路径相交的横向子系统，然后分别在横向子系统中设计非奇异快速终端滑模控制器使无人机的运动收敛于期望路径，在切向子系统中设计结合一致性与势场法的协同与避障控制算法，进而实现多无人机协同巡航任务以及巡航过程中的动态障碍规避。
具体步骤如下：
定义地面惯性坐标系{I}、队列中的第i架四旋翼无人机的机体坐标系{B(i)}，同时定义第i架四旋翼无人机在惯性坐标系{I}下的位置p(i)＝[x(i),y(i),z(i)]和线速度向量借助上述定义可建立四旋翼无人机的动力学模型如下所示：
上式中m(i)为第i架无人机的质量，g为重力加速度，为总升力大小，ez＝[0,0,1]T为沿Z轴正方向的单位向量，R(i)(t)为机体坐标系{B(i)}到惯性坐标系{I}的旋转矩阵，考虑多无人机协同飞行过程，各无人机的姿态变化平稳，可对中的动力学模型做如下简化，定义虚拟控制输入:
则中的动力学模型简化为
其中状态向量I3表示3×3义的单位矩阵；
在使用TFL方法研究路径跟踪问题时，需要根据期望路径选取合适的映射，将运动系统分解为不同的子系统分别进行控制器设计，首先给定期望巡航路径方程δ如下所示：
上式中确定了三维空间中一条光滑无自交的封闭路径，其中为路径参数，选定四旋翼无人机系统的虚拟输出方程为
设计辅助函数Γ(ξ)和Π(ξ)为
其中为无人机沿期望路径方向经过的圈数，θδ＝2πr为封闭路径的周长，记为的第i个分量，ri为的相对阶，分别对求ri阶Lie导数，得到如下形式：
上式中K(ξ),L(ξ)为TFL输入变换矩阵，当ri满足时L(ξ)可逆，故可设计解耦控制律ν实现解耦控制，根据虚拟输出构造微分同胚映射使动力学模型变换为如下形式：
其中分别为各子系统的系数矩阵，上述为切向子系统，其状态η(i)(t)表示无人机在期望巡航路径上的运动状态；为横向子系统，其状态ζ(i)(t)表示无人机与期望巡航路径的偏差，通过设计控制器使ζ(i)(t)收敛于零即可达到无人机位置收敛于期望路径的控制目标；
对于横向子系统，设计非奇异快速终端滑模控制器，对于队列中的第i架无人机首先定义状态误差向量其中设计滑模面S(i)为
其中α,β为正常数,p,q为正奇数且满足2＞q/p＞1，设计控制律
其中为正常数控制增益，根据李雅普诺夫稳定性分析方法可以得出结论：在控制律的作用下，横向子系统状态误差收敛于零，即各架无人机能够收敛于期望巡航路径；
对于切向子系统，可将其表示为如下形式：
考虑无人机队列巡航过程中的障碍威胁，将障碍物视为圆柱体模型，根据其半径和位置设计如下的排斥场函数：
其中Kob为斥力场的正增益系数，表示场强大小，rsafe为障碍物的安全区域半径，r(i)＝||Γ(pob)-Γ(p(i))||，同时取kδ≡0表示在期望路径方向上障碍物与第i架无人机之间的相对距离，d(i)为第i架无人机与障碍物之间的空间距离，对于队列中的第i架无人机，设计分布式避障控制律ν(i)(t)如下所示:
其中在巡航路径方向的期望速度αob,ρ为正常系数，Φi(·)为奇函数，且满足Φi′(·)≥0，sgn(Φi(x))＝sgn(x)，为多无人机通信拓扑图对应邻接矩阵的元素，表示拓扑结构上的相邻关系，为期望路径上相邻无人机之间的期望距离，为障碍物对在其安全区域范围内的第i架无人机产生的沿期望路径方向的斥力，其符号为正表示指向无人机群行进的正方向。
根据Lyapunov稳定性分析方法和拉塞尔不变性原理证明，在控制律(15)的作用下，当时间趋于无穷时，切向子系统能够收敛于给定的期望距离和期望速度，并且不会发生与障碍物的碰撞。
本发明的特点及有益效果是：
1.本发明基于TFL方法对无人机动力学模型进行变换，通过解耦控制实现了对期望路径的跟踪；
2.本发明将一致性协议与势场法相结合，使无人机群能够在期望巡航路径上达成协同的同时有效规避动态障碍，并通过Lyapunov稳定系分析方法和LaSalle不变性原理证明了编队系统的收敛性和安全性；
3.本发明将算法成功应用于多无人机集群实物平台，并设计和进行了飞行实验，验证了所提出算法的有效性和可靠性。
附图说明
：图1是本发明使用的控制系统结构；
图2是本发明所采用的实验平台；
图3是无人机队列运行的空间位置曲线图；
图4是无人机队列在期望路径上行进的距离曲线图；
图5是无人机队列在期望路径上行进的速度曲线图；
图6是无人机队列协同避障实验效果图；
图7是无人机队列协同避障过程中与障碍物的距离曲线图；
图8是无人机队列协同避障过程中在期望路径上行进的距离曲线图。
具体实施方式
为克服现有技术的不足，本发明旨在设计一种基于滑模与一致性协议的多无人机协同控制与避障算法，实现无人机集群巡航任务与巡航路径上的协同避障。本发明采用的技术方案是，采用Transverse Feedback Linearization(TFL)方法对四旋翼无人机的动力学模型进行微分同胚映射变换,使其分解为沿期望路径方向的切向子系统和与期望路径相交的横向子系统，然后分别在横向子系统中设计非奇异快速终端滑模控制器使无人机的运动收敛于期望路径，在切向子系统中设计结合一致性与势场法的协同与避障控制算法，进而实现了多无人机协同巡航任务以及巡航过程中的动态障碍规避。
具体步骤如下：
定义地面惯性坐标系{I}、队列中的第i架四旋翼无人机的机体坐标系{B(i)}，同时定义第i架四旋翼无人机在惯性坐标系{I}下的位置p(i)＝[x(i),y(i),z(i）]和线速度向量借助上述定义可建立四旋翼无人机的动力学模型如下所示：
上式中m(i)为第i架无人机的质量，g为重力加速度，为总升力大小，ez＝[0,0,1]T为沿Z轴正方向的单位向量，R(i)(t)为机体坐标系{B(i)}到惯性坐标系{I}的旋转矩阵，考虑多无人机协同飞行过程，各无人机的姿态变化平稳，可对中的动力学模型做如下简化，定义虚拟控制输入
则中的动力学模型可简化为
其中状态向量I3表示3×3义的单位矩阵；
在使用TFL方法研究路径跟踪问题时，需要根据期望路径选取合适的映射，将运动系统分解为不同的子系统分别进行控制器设计，首先给定期望巡航路径方程δ如下所示：
上式中确定了三维空间中一条光滑无自交的封闭路径，其中为路径参数，选定四旋翼无人机系统的虚拟输出方程为
设计辅助函数Γ(ξ)和Π(ξ)为
其中为无人机沿期望路径方向经过的圈数，θδ＝2πr为封闭路径的周长，记为的第i个分量，ri为的相对阶，分别对求ri阶Lie导数，得到如下形式：
上式中K(ξ),L(ξ)为TFL输入变换矩阵，当ri满足时L(ξ)可逆，故可设计解耦控制律ν实现解耦控制，根据虚拟输出构造微分同胚映射使动力学模型变换为如下形式：
其中上述为切向子系统，其状态η(i)(t)表示无人机在期望巡航路径上的运动状态；为横向子系统，其状态ζ(i)(t)表示无人机与期望巡航路径的偏差，通过设计控制器使ζ(i)(t)收敛于零即可达到无人机位置收敛于期望路径的控制目标；
对于横向子系统，设计非奇异快速终端滑模控制器，对于队列中的第i架无人机首先定义状态误差向量其中设计滑模面S(i)为
其中α,β为正常数,p,q为正奇数且满足2＞q/p＞1，设计控制律
其中为正常数控制增益，根据Lyapunov稳定性分析方法可以得出结论：在控制律的作用下，横向子系统状态误差收敛于零，即各架无人机能够收敛于期望巡航路径；
对于切向子系统，可将其表示为如下形式：
考虑无人机队列巡航过程中的障碍威胁，将障碍物视为圆柱体模型，根据其半径和位置设计如下的排斥场函数：
其中Kob为斥力场的正增益系数，表示场强大小，rsafe为障碍物的安全区域半径，r(i)＝||Γ(pob)-Γ(p(i))||，同时取kδ≡0表示在期望路径方向上障碍物与第i架无人机之间的相对距离，d(i)为第i架无人机与障碍物之间的空间距离，对于队列中的第i架无人机，设计分布式避障控制律ν(i)(t)如下所示:
其中在巡航路径方向的期望速度αob,ρ为正常系数，Φi(·)为奇函数，且满足Φi′(·)≥0，sgn(Φi(x))＝sgn(x)，为多无人机通信拓扑图对应邻接矩阵的元素，表示拓扑结构上的相邻关系，为期望路径上相邻无人机之间的期望距离，为障碍物对在其安全区域范围内的第i架无人机产生的沿期望路径方向的斥力，其符号为正表示指向无人机群行进的正方向，根据Lyapunov稳定性分析方法和LaSalle不变性原理可以证明，在控制律的作用下，当时间趋于无穷时，切向子系统能够收敛于给定的期望距离和期望速度，并且不会发生与障碍物的碰撞。
下面结合实施例和附图对本发明做出详细说明。
一、多无人机协同飞行实验平台介绍
自主搭建的多无人机协同控制实验平台如图2所示，该实验平台由三架轴距为0.25m的四旋翼无人机构成，每架无人机搭载Pixhawk飞行控制器和ARM计算板,通过动作捕捉系统获得定位信息,在机载ARM计算板上运行所设计的控制方法,通过串口向Pixhawk发送姿态控制指令完成飞行控制并通过WiFi接收地面站指令。在避障实验中，另有一架与上述结构相近的无人机作为动态障碍物，可通过遥控器控制其运动。
二、多无人机协同巡航飞行实验
实验开始时各无人机放置于地面，同时从各自初始位置起飞至一定高度，约4.5秒时通过地面站发送指令，切换至所设计的队列控制算法并持续至150秒左右。根据场地规模，选取期望路径的半径为1.5m，高度1.0m，队列中相邻无人机之间的期望路径距离为3.14m，队列行进速度设定为0.4m/s，控制参数的选取为α＝3.5,β＝1.0,p＝3,q＝5,ρ＝0.1，非线性函数取Φ1(x)＝5.5tanh(1.5x),Φ2(x)＝3tanh(x)，多无人机通信拓扑矩阵为实验结果如图3～5所示。
由图3可以看出，无人机队列能够始终收敛于设定的期望巡航路径，图4所示的无人机沿期望路径方向的距离误差表明各无人机之间能够按照设定的相对路径距离完成协同巡航任务，图5所示的无人机沿期望路径的行进速度表明各无人机能够按照设定的期望速度行进。
三、多无人机动态避障实验
与上组实验相同，首先让无人机以队列的方式运行，待系统状态稳定后，通过遥控器手动控制障碍物无人机，使其多次穿过队列的期望路径，阻碍队列中无人机的行进，实验过程如图6所示，其中圆弧曲线为多无人机的协同巡航期望路径，箭头表示无人机的运动方向。本组实验使用的障碍物无人机可视为圆柱体障碍物类型，根据实际情况，在控制器参数中设定障碍物半径rob＝0.25m,障碍物的安全半径rsafe＝1.0m，排斥场增益系数Kob＝3.5,比例系数αob＝0.05，给定的期望路径和期望相对距离与速度以及其余控制器参数的选取与上组实验相同，实验结果如图6和图7所示。当t＝0秒时队列已达成稳态，即按照给定的状态运行。
图7展示了无人机队列行进过程中与障碍物无人机之间的空间距离，可以看出当相对距离小于安全半径rob时，队列中的无人机能够远离障碍物，整个实验过程各无人机与障碍物之间的距离均大于设定的障碍物半径，没有发生碰撞。图8为无人机队列沿期望路径的行进距离，可以看出，当队列中的某一架无人机收到障碍物阻碍时，其余无人机在一定程度上能够与之协同，保持给定的期望距离。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
