一种基于模型预测控制框架的集群无人机编队方法与流程

本发明涉及旋翼无人机的技术领域，更具体地，涉及一种基于模型预测控制框架的集群无人机编队方法。

背景技术：

科学技术的发展，机器人技术逐渐和人类的生产工作结合起来，在军事、工业与服务业都得到了广泛的应用。随着机器人应用领域的不断扩展，从当前的机器人发展的技术水平来说，单机器人在信息的获取处理及运动的控制能力等方面都是有限的。于是人们考虑研制多机器人组成的群体系统通过协调合作来完成单机器人无法或难以完成的工作。群体机器人系统具有丰富广泛的环境感知能力，并行执行的高工作效率，优秀稳定的鲁棒性和卓越出众的容错能力等众多单机器人难以拥有的优势。多机器人协调合作可以完成单个机器人无法完成的复杂任务，并且大大提高工作的效率，所以群体机器人系统是机器人研究领域的一个重要问题。

目前，无人机正在慢慢渗入人民生活的方方面面，在未来的发展中将会将会进入史无前例的良好发展趋势。目前市场上的大多数无人机的应用是单机执行的，无法完成高难度的任务，集群无人机的协同编队控制能更好地完成复杂的系统任务。集群无人机的协同编队控制问题主要分为四个技术模块：编队生成、编队保持、编队切换和编队避障。编队生成是多无人机系统根据环境的限制、任务要求和无人机数量设计稳定的编队队形；编队保持是多无人机系统在整体运动中保持编队不变，或者在允许队形少量变化的条件下，保持整个编队同意前进；编队避障是多无人机利用环境信息，避免与环境中的障碍物发生碰撞的同时，又保证不会与其他无人机发生碰撞。

目前集群无人机协同编队的主要研究，主要是解决集群无人机的协同编队控制的编队保持问题。编队保持算法主要有领航跟随法(leader-follower,lf)、基于行为的方法、虚拟结构法。当前国内外的研究中，已经有人通过对编队保持的问题进行运动建模分析，得到了编队保持的运动控制律，但是当机器人遇到障碍物时，控制律将难以权衡编队保持和编队避障问题。编队避障算法主要是解决躲避环境中障碍物的问题，同时保障编队内部的无人机不会发生碰撞，所以避障问题可以归纳为避障和避碰两部分。在目前的研究当中，主要针对的是环境中的静态障碍物，在环境为静态、平稳环境情况下能取得较好的效果，但当环境是动态的不可知环境时往往编队的效果就会下降。

技术实现要素：

本发明为克服上述现有技术所述的至少一种缺陷，提供一种基于模型预测控制框架的集群无人机编队方法，结合了图论的方法指定了集群无人机的预设编队队形，同时在编队飞行时通过向mpc的代价函数中添加避障的势场函数，解决了规避动态障碍物的问题。

为解决上述技术问题，本发明采用的技术方案是：一种基于模型预测控制框架的集群无人机编队方法，本方案是在领航跟随法的编队保持方法的基础下，结合了图论的方法指定了集群无人机的预设编队队形，同时在编队飞行时通过向mpc的代价函数中添加避障的势场函数，求解模型预测控制问题得到最优控制输入量，作为无人机的控制输入，完成编队的编队保持和编队避障问题。本方法很好地兼顾了编队保持和编队避障两方面的问题，同时每一次求解mpc时都将最新的障碍物信息输入到算法中，解决了规避动态障碍物的问题。

集群无人机协同编队需要无人机之间相互交换信息，图论(graphtheory)的方法很好地描述这种通信网络。使用有向图(directedgraphs)来表示集群无人机的拓扑网络，有向图用来表示，其中是顶点(vertices)集合，是有向边(directededge)的集合。

对于一幅有n个顶点的有向图，可以用n×n的邻接矩阵(adjacencymatrix)来表示这幅图的网络拓扑关系，当(vi,vj)∈ε时，矩阵的第i行第j列的元素值为1，否则为0。加权图是一种为每一条边关联一个权重或者成本的图模型。而一幅有n个顶点的加权有向图，当(vi,vj)∈ε时，矩阵的第i行第j列的元素值为对应于这条边的权重，否则为0。

在集群无人机协同编队任务中，编队保持要求每一个无人机要与其邻居保持确切的期望的距离。对于有n个无人机的编队拓扑中，用顶点表示一个无人机，用有向边表示无人机之间的信息流动方向，集群无人机协同编队的网络拓扑图可以表示为其中是顶点的集合，ε是有向边的集合，表示无人机之间的期望距离，表示无人机的期望的相对方位角。用顶点表示一个无人机，顶点的下标表示每个无人机的唯一的身份识别号，v1表示领航者，其余的顶点表示跟随者。

把旋翼无人机看成完整性约束的机器人，用机器人在世界坐标系的三维坐标p＝[x,y,z]^t以及无人机的偏航角θ作为系统方程的状态向量，系统方程的输入是机器人在机体坐标系下机体线速度v＝[vx,vy,vz]^t以及角速度ω。可以得到旋翼无人机的单积分非线性运动学模型为

在本发明中采用模型预测控制框架，在代价函数中加入避障惩罚项和位置约束。

模型预测控制(mpc)是一类特殊的控制。它的当前控制动作是在每一个采样瞬间通过求解一个有限时域开环最优控制问题而获得。过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用。这是它与那些使用预先计算控制律的算法的最大不同。在该方案中，领航者和跟随者分别执行不同的任务，对于领航者而言，主要任务是根据给定的路径点为整个编队系统导航出一条可行的飞行路径，而跟随者的主要任务是维持整体的编队队形、避开环境中的障碍物以及保证无人机之间的不会碰撞，所以对于领航者于跟随者在本发明中设计了不同的代价函数。

对于领航者，其代价函数为：

x(0)＝x(t0)

其中q,r和qn是有适当维度的半正定的对称矩阵，是之前所述的运动学模型，t是预测时域。

xref是根据无人机当前位置和路径点计算得到的无人机预测域内的期望状态，所以第一个代价惩罚项的作用是驱使领航者尽快接近目标的路径点，当领航者已经处于目标路径点的某一个球域附近时，我们认为领航者已经到达了当前的目标路径点，此时删除当前的期望路径点，将下一个路径点作为新的期望路径点，不断重复以上步骤，直到领航者到达最终的路径点。

当我们得到了领航者的期望的位置状态后，为了使期望路径更加平滑，有必要计算领航者的期望输入uref，所以第二个代价惩罚项的作用是使得领航者的飞行轨迹变得平滑稳定。

避障代价函数jo(t)定义为：

其中0＜τ＜＜1是可以调节的参数，po,i(t)和ro分别是机器人检测得到的障碍物的位置和半径，rs是安全半径。为了能够保证这个代价惩罚项能够在机器人碰上障碍物之前起作用，安全半径rs要大于机器人的半径r，即rs＞r。由公式可以看出，当机器人与障碍物的距离‖p(t)-po,i(t)‖小于rs+ro时，也就是说当机器人与障碍物的空隙间距小于rs-r时，避障代价惩罚项jo(t)会出现非常大的增率，从而驱使机器人与障碍物保持一定的距离，达到避开障碍物的目的。只要无人机能实时检测到环境里的静态或者动态障碍物，就能实现规避静动态障碍物的功能。

领航者的最后一项代价惩罚项为终端代价惩罚项这与第一个代价惩罚项的形式上是一致的，但有两处不同点：第一个不同点是状态向量的时刻不同，前面的代价项是将状态向量从0到t时刻的积分，而终端代价项只是简单叠加了t时刻的状态向量；第二个不同点是对应的二次型矩阵不同，前面的是矩阵q，后面的是矩阵qn，为了能让实际终端状态更接近于期望的终端状态，一般令终端状态矩阵qn对应的权重大于q的大小。

对于其余的跟随者，其代价函数为：

x(0)＝x(t0)

其中，q，r和qn是有适当维度的半正定的对称矩阵，是之前所述的运动学模型，t是预测时域。

这里的xref是根据领航者的位置计算得到的跟随者的期望状态，在大规模的集群无人机编队中，由于无法计算估计得到任意一个跟随者的具体的期望的三维坐标，但是希望跟随者的偏航角保持跟领航者的偏航角一致，故可以令r＝diag(0,0,0,ωθ)。除此之外，还希望跟随者能够以尽量小的能量代价完成编队任务，所以设置了第二项代价惩罚项以实现这一要求。

跟随者的代价函数的第三项是距离同步项，对于某个跟随者i，根据提前预设的集群无人机协同编队的网络拓扑图，容易得到该无人机的邻居集和与其邻居对应的期望编队距离sij，期望在编队任务中，每一个跟随者能够与其所有邻居保持期望的编队距离，所以第三个代价惩罚项的作用是驱使跟随者能够与其所有邻居保持期望的编队距离，其中ωij是编队距离保持项的代价权重，权重越大说明编队拓扑图中对应的边(ei,ej)越重要。

避障代价函数jo(t)和终端代价惩罚项与领航者的代价函数一致，这里不再详细阐述。

在本发明中，集群无人机分布式通信系统是使用了机器人操作系统(ros)的multi-master模块功能实现的。

进一步的，基于模型预测控制框架的集群无人机编队方法，包括以下步骤：

s1.通过人机交互界面向集群无人机分布式通信系统发送起飞命令，控制所有无人机平稳起飞，所有无人机进入就绪工作状态；

s2.通过分布式通信系统，每一架无人机能得到其邻居无人机的位置和速度的状态信息，同时预测无人机在未来几个时刻的期望状态；

s3.无人机通过机载传感器实时检测感知其周围环境的静态和动态障碍物以及非邻居无人机的状态信息；

s4.将步骤s2和s3得到的信息作为分布式mpc算法的输入，并且在线求解得到无人机的最优控制输入，实现编队保持和规避动态障碍物；

s5.将步骤s4得到的最优控制输入作用到无人机上，驱使无人机到达期望的目标点；

s6.判断无人机是否到达了目标点，如果尚未到达目标点，返回步骤s2，如果已经到达目标点，则完成飞行任务，平稳降落所有无人机。

与现有技术相比，有益效果是：本发明提供的一种基于模型预测控制框架的集群无人机编队方法，用于多无人机在存在多种障碍物的环境下的实现编队保持的同时规避障碍物的方法，即该方法实现了多无人机系统在障碍物环境中的协调导航。该方法将集群无人机在有障碍物环境下保持编队和躲避障碍物的问题转化成分布式的模型预测控制问题，并且集群中的每架无人机能在线求解模型预测控制的最优解，提高了无人机集群编队的可拓展性和灵活性。同时该方法是在领航跟随法的编队方法的基础下，结合了图论的方法指定了集群无人机的预设编队队形，在编队飞行时通过求解模型预测控制问题得到最优控制输入量，从而快速形成预设队形。所提出的方法需要无人机快速得到其邻居无人机的位置和速度，可以通过集群无人机的分布式快速通信系统得以实现。

附图说明

图1是本发明方法流程图。

图2是本发明无人机和树莓派连接的实物图。

图3是本发明集群无人机分布式通信系统的multi-masterros系统架构图。

图4是本发明集群无人机分布式通信系统的multi-masterros系统架构图。

图5是本发明实施案例的无人机的飞行轨迹图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1所示，在本实施例中使用到的无人机是由法国parrot公司生产的bebop2无人机，为了实现集群无人机的分布式控制，为每一个无人机分配了一个树莓派作为无人机的控制系统，图2展示了bebop2和树莓派连接的实物图。

为了能准确获得每个无人机的位姿信息，可以通过室内定位运动捕捉系统实时获取，图3是集群无人机分布式通信系统的multi-masterros系统架构图，pc端通过运动捕捉系统optitrack获取得到每一台无人机的位姿信息后，通过wifi将无人机的位姿信息共享到无线局域网中，然后每一台无人机上的树莓派通过无线wifi连接到无线局域网，计算得到该无人机的位姿信息后，将自身的位姿信息共享发布到multi-master上的ros话题中，同时该无人机从multi-masterros系统上获取得到其邻居的其他无人机的位姿信息。每一台无人机将得到的位姿信息预测该无人机及其邻居未来时域的状态信息，并将这些信息作为模型预测控制的输入，通过在线求解得到无人机的最优控制输入后，将该输入作用到无人机上。

如图4所示是本发明实施方案的三个无人机的集群无人机协同编队的网络拓扑图，其中1号作为领航者，2号和3号作为跟随者，每一个无人机表示有向图的顶点，图中的黑色箭头表示有向图的有向边，由有向图可以得到该图的邻接矩阵为

通过邻接矩阵可以容易得到每台无人机的邻居集。

图5是实施案例的无人机的飞行轨迹图，我们给定初始的编队队形为等边三角形，令所有的期望的编队距离为1米，让领航者跟踪“8”字形的轨迹，整个编队无人机在执行编队任务中，会在“8”字形的两端遇到两种不同的障碍物，可以明显看到无人机能顺利地规避障碍物，并且尽量保持了原有的等边三角形编队队形。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。