一种无线速度测量的四旋翼飞行机器人机动编队控制方法与流程

本发明主要涉及到四旋翼飞行机器人的编队控制领域，特别涉及一种无线速度测量的四旋翼飞行机器人机动编队控制方法。

背景技术：

飞行机器人编队控制由于其在搜救、侦察、监视和中继通信等领域的广泛应用而成为活跃的研究领域。四旋翼飞行机器人由于其结构简单以及具有垂直起降能力而成为一种流行的飞行机器人研究平台。由于四旋翼飞行机器人只有四个独立的输入而有六个自由度，因此四旋翼飞行机器人属于欠驱动系统。四旋翼飞行机器人的欠驱动和非线性耦合特性导致难以控制其运动。在四旋翼飞行机器人编队控制方面已经有一些研究，其编队控制方法可以分为领导者跟随方法、虚拟结构方法和基于行为的方法：在领导者跟随的方法中，跟随者需要跟踪领导者，并相对于领导者保持所需的相对位置；在虚拟结构方法中，整个队形被视为一个刚体，然后将期望的行为分配给虚拟结构，每个成员相对于虚拟结构控制其自己的行为；在基于行为的编队控制中，为每个机器人规定了几种期望的行为，并且每个机器人的控制动作是所有可能行为的控制权重平均值。由于上述所有编队控制器都是基于欧拉角表示的旋转动力学开发的，而欧拉角表示参在固有的几何奇异性。通过使用四元数表示可以避免这种奇异问题，四元数表示可用于描述所有可能的方向。尽管四元数不具有奇异性，它们在表示姿态时存在歧义。因此，无论如何选择参数，所有参数化都无法全局且唯一地表示一组姿态。出于在旋转运动中全局且唯一地表示姿态的愿望驱使，直接用特殊正交群so(3)表示姿态，避免了与euler角(欧拉角)相关的奇异性或四元数的歧义。

另外，当某些状态变量不可用于反馈时，四旋翼飞行机器人的协调控制变得更具挑战性。当前与四旋翼飞行机器人编队控制有关的大多数研究中，鉴于在线速度不可直接测量或难以测量的情况，基本都依赖于全部状态信息可测，但下。

因此，设计一种无线速度测量的四旋翼飞行机器人机动编队控制方法，解决没有线性速度测量的分布编队控制问题，成了本领域人员亟需解决的技术问题。

技术实现要素：

本发明的目的在于避免现有技术中的不足而提供一种无线速度测量的四旋翼飞行机器人机动编队控制方法，基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，以避免任何角度表示的奇异或模糊问题，其中内环控制姿态动力学，外环协调每个四旋翼飞行机器人的平移动力学，以实现编队控制目标，同时能有效提高编队系统的高机动性。

本发明的目的通过以下技术方案实现：提供一种无线速度测量的四旋翼飞行机器人机动编队控制方法，包括以下步骤：

s1、针对可在三维空间机动的多四旋翼飞行机器人系统，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型；

s2、基于辅助滤波信号发生器，引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求；

s3、基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个四旋翼飞行机器人的平移动力学，以实现高机动编队控制目标。

作为进一步的改进，所述步骤s1中针对可在三维空间机动的多四旋翼飞行机器人系统，每个四旋翼飞行机器人的位姿定义在欧氏群se(3)空间，其中，在惯性坐标系下每个四旋翼飞行机器人位置为实数集，是一组所有元素都属于的三元组，每个四旋翼飞行机器人姿态ri∈so(3)，so(3)表示三维旋转群，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型为：

式中，i表示四旋翼飞行机器人的序号，为pi的一阶微分，vi表示在惯性坐标系下每个四旋翼飞行机器人的线速度，为vi的一阶微分，为ri的一阶微分，mi和ji分别表示质量和惯性张量，g为恒定的重力加速度，ωi表示在机体坐标系下的角速度，表示与ωi相关的斜对称矩阵，fi和τi分别是每个四旋翼飞行机器人的净推力和体转矩，矢量e3＝[0,0,1]^t，且为实数集，是一组所有元素都属于的三元组。

作为进一步的改进，所述无线速度测量的四旋翼飞行机器人机动编队控制方法基于动力学模型的控制目标是针对每个四旋翼飞行机器人设计净推力fi和体转矩τi，满足以下条件：

式中，j表示四旋翼飞行机器人的序号且i≠j，δij表示第i个和第j个四旋翼飞行机器人之间期望的相对位置，δij用于确定期望的编队，vd是编队队形的参考线速度。

作为进一步的改进，所述步骤s2中引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求，具体步骤如下：

s21、将以下辅助变量的二阶信号关联到每个四旋翼飞行机器人：

其中，kpi、kdi、lpi和ldi均是正增益，和均为辅助变量，为αi的一阶微分，为βi的一阶微分，为αi的二阶微分，为βi的二阶微分，和是待设计的附加输入矢量，函数tanh(·)按元素定义；

s22、定义ξi如下：

ξi＝pi-αi-βi(5)

s23、定义输入的和φi分别为：

其中，ξij＝ξi-ξj-δij，ξj＝pj-αj-βj，aij表示第i个和第j个四旋翼飞行机器人之间通信网络的权值，aji表示第j个和第i个四旋翼飞行机器人之间通信网络的权值，aij＝aji≥0，kψi,kvi为正增益，ψi表示由公式(8)产生的信号，是ψi的一阶微分；

s24、使用辅助变量αi，定义：

式中，为vd的一阶微分。

作为进一步的改进，所述步骤s3中基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个飞行机器人的平移动力学，以实现高机动编队控制目标，具体步骤如下：

s31、设计中间控制信号fi，令每个四旋翼飞行机器人的净推力输入如下：

fi＝-mifi^trie3(11)

式中，t表示矩阵转置；

s32、定义b3id的表达式如下：

其中，||fi||≠0；

s33、获得的期望姿态rid为：

rid＝[b2id×b3id,b2id,b3id](13)

其中，b2id、b3id分别表示期望姿态rid的两个分量，且b2id表示为b2id＝(b3id×b1id)/||b3id×b1id||，b1id表示为期望姿态rid的第一个分量，b1id不平行于b3id；

s34、在特殊正交组so(3)上定义结构误差函数为i表示三维单位矩阵，tr(·)表示矩阵的迹，并且so(3)的切线空间上的姿态动力学和的误差函数为：

其中，ωid是由给定的期望角速度；

s35、设计每个四旋翼飞行机器人的体转矩τi为：

其中，和为正控制增益，是ωid的一阶微分。

本发明相比现有技术，首先对可在三维空间机动的多四旋翼飞行机器人系统，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型；其次，基于辅助滤波信号发生器，引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求；最后，基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个四旋翼飞行机器人的平移动力学，以实现高机动编队控制目标。故此，本发明所涉及的控制律(净推力fi和体转矩τi)均不取决于线速度矢量，即本发明所提出的四旋翼飞行机器人机动编队控制方法是实现无线速度的，无需线速度测量。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1是本发明一种无线速度测量的四旋翼飞行机器人机动编队控制方法的流程图。

图2是本发明一种无线速度测量的四旋翼飞行机器人机动编队控制方法的原理图。

图3是本发明辅助变量在辅助滤波信号发生器的传播示意图。

图4为本发明实施例的多四旋翼飞行机器人系统在仿真中的编队过程轨迹示意图，其中uav1、uav2、uav3、uav4分别表示第1号、第2号、第3号和第4号四旋翼飞行机器人。

图5为本发明实施例的多四旋翼飞行机器人系统在仿真中的编队误差收敛示意图。

图6为本发明实施例的第一个四旋翼飞行机器人系统的线速度矢量误差示意图。

图7为本发明实施例的第二个四旋翼飞行机器人系统的线速度矢量误差示意图。

图8为本发明实施例的第三个四旋翼飞行机器人系统的线速度矢量误差示意图。

图9为本发明实施例的第四个四旋翼飞行机器人系统的线速度矢量误差示意图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

如图1至图3所示，本发明实施例提供的一种无线速度测量的四旋翼飞行机器人机动编队控制方法，包括以下步骤：

s1、针对可在三维空间机动的多四旋翼飞行机器人系统，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型；优选地，步骤s1中针对可在三维空间机动的多四旋翼飞行机器人系统，每个四旋翼飞行机器人的位姿定义在欧氏群se(3)空间，其中，在惯性坐标系下每个四旋翼飞行机器人位置为实数集，是一组所有元素都属于的三元组，每个四旋翼飞行机器人姿态ri∈so(3)，so(3)表示三维旋转群，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型为：

式中，i表示四旋翼飞行机器人的序号，为pi的一阶微分，vi表示在惯性坐标系下每个四旋翼飞行机器人的线速度，为vi的一阶微分，为ri的一阶微分，mi和ji分别表示质量和惯性张量，g为恒定的重力加速度，ωi表示在机体坐标系下的角速度，表示与ωi相关的斜对称矩阵，fi和τi分别是每个四旋翼飞行机器人的净推力和体转矩，矢量e3＝[0,0,1]^t，且为实数集，是一组所有元素都属于的三元组；

多四旋翼飞行机器人系统间的通信拓扑图由无向图g＝(v,e)表示，且假定图g是连通的，对于所有i,j(j表示四旋翼飞行机器人的序号且i≠j，i、j均为正整数)而言，通过在第i个和第j个四旋翼飞行机器人之间期望的相对位置确定期望的编队；具体而言，此处的控制目标是针对每个四旋翼飞行机器人设计净推力fi和体转矩τi，满足以下条件：

式中，vd是编队队形的参考线速度。

s2、基于辅助滤波信号发生器，引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求；具体地，步骤s2中引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求，可分解为以下过程：

s21、将以下辅助变量的二阶信号关联到每个四旋翼飞行机器人：

其中，kpi、kdi、lpi和ldi均是正增益，和均为辅助变量，为αi的一阶微分，为βi的一阶微分，为αi的二阶微分，为βi的二阶微分，和是待设计的附加输入矢量，函数tanh(·)按元素定义；需要说明的是，由于函数tanh(·)按元素定义，假设函数tanh括号内元素为x,且则

s22、定义ξi如下：

ξi＝pi-αi-βi(5)

s23、定义输入的和φi分别为：

其中，ξij＝ξi-ξj-δij，ξj＝pj-αj-βj，aij表示第i个和第j个四旋翼飞行机器人之间通信网络的权值，aji表示第j个和第i个四旋翼飞行机器人之间通信网络的权值，aij＝aji≥0，kψi,kvi为正增益，ψi表示由公式(8)产生的信号，且ψi可以任意初始化，是ψi的一阶微分；所有辅助变量之间的关系如图3所示，该图说明了使用状态变量pi生成辅助变量αi和βi；

s24、使用辅助变量αi，定义：

式中，为vd的一阶微分。

s3、基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个四旋翼飞行机器人的平移动力学，以实现高机动编队控制目标。

进一步地，步骤s3中基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个飞行机器人的平移动力学，以实现高机动编队控制目标，具体步骤如下：

s31、设计中间控制信号fi，令每个四旋翼飞行机器人的净推力输入如下：

fi＝-mifi^trie3(11)

式中，t表示矩阵转置；

s32、定义b3id的表达式如下：

其中，||fi||≠0；

s33、获得的期望姿态rid为：

rid＝[b2id×b3id,b2id,b3id](13)

其中，b2id、b3id分别表示期望姿态rid的两个分量，且b2id表示为b2id＝(b3id×b1id)/||b3id×b1id||，b1id表示为期望姿态rid的第一个分量，b1id不平行于b3id；

s34、在特殊正交组so(3)上定义结构误差函数为i表示三维单位矩阵，tr(·)表示矩阵的迹，并且so(3)的切线空间上的姿态动力学和的误差函数为：

其中，ωid是由给定的期望角速度；

s35、设计每个四旋翼飞行机器人的体转矩τi为：

其中，和为正控制增益，是ωid的一阶微分。

所设计的控制律由式(11)定义的推力和式(16)定义的转矩组成。可以验证所提出的控制算法是实现无线速度。

需要说明的是，通过式(11)和式(16)可知每个四旋翼飞行机器人的净推力fi、体转矩τi(推力fi和转矩τi即为本发明的控制律)都不取决于线速度矢量。故此可以得出本发明所提出的编队控制方法是无需线速度的。

在此，为了更好说明本发明方法对四旋翼飞行机器人机动编队的有效控制，结合图4-图9在此进行仿真说明：为方便说明，以下出现的数据全部默认使用国际单位。在仿真中，设定一组四个四旋翼飞行机器人组成的系统，它们之间的通信关系构成一个环形拓扑图1-2-3-4。其中：每个四旋翼飞行机器人模型的质量和惯性参数取为：mi＝1，ji＝diag(0.1,0.1,0.1)，i＝{1,2,3,4}；每个飞行机器人的初始位置设为：p1＝[2,3,1]^t,p2＝[6,2,1]^t,p3＝[2,-2,0]^t,p2＝[6,-2,0]^t；初始线速度和角速度均为零值；系统的期望队形由四旋翼飞行机器人之间相对位移δ12＝[4,0,0]^t,δ23＝[0,4,0]^t,δ34＝[-4,0,0]^t,δ41＝[0,-4,0]^t确定，队形的参考线速度为：vd＝[sin(0.1t),0.5cos(0.1t),0.5]^t；所有的辅助变量的初始值均取为零；其他相关参数取为kpi＝1,kdi＝1,lpi＝1,ldi＝1,kvi＝10,kψi＝5,kri＝10，kωi＝10。在这些参数配置下相应的仿真结果如图4-9所示。图4给出了多四旋翼飞行机器人系统在三维空间的编队过程轨迹。图5显示了多四旋翼飞行机器人系统在仿真中的编队误差示意图，可以看到编队误差会收敛于零。图6-9分别给出了四个四旋翼飞行机器人的线速度矢量误差||vi-vid||示意图，其中：图6中v1x-v1xd、v1y-v1yd、v1z-v1zd分别表示第1号四旋翼飞行机器人沿x轴、y轴和z轴的线速度分量误差示意图，图7中v2x-v2xd、v2y-v2yd、v2z-v2zd分别表示第2号四旋翼飞行机器人沿x轴、y轴和z轴的线速度分量误差示意图，图8中v3x-v3xd、v3y-v3yd、v3z-v3zd分别表示第3号四旋翼飞行机器人沿x轴、y轴和z轴的线速度分量误差示意图，图9中v4x-v4xd、v4y-v4yd、v4z-v4zd分别表示第4号四旋翼飞行机器人沿x轴、y轴和z轴的线速度分量误差示意图。仿真结果表明，本发明的编队控制方法具有良好的控制性能。

本发明基于一种无线速度测量的四旋翼飞行机器人机动编队控制方法，首针对可在三维空间机动的多四旋翼飞行机器人系统，利用牛顿-欧拉方程建立每个四旋翼飞行机器人的动力学模型；其次，基于辅助滤波信号发生器，引入若干辅助变量的二阶信号以消除在多四旋翼飞行机器人编队控制中对线速度测量的需求；最后，基于几何控制的方法，在欧氏群se(3)上设计包括内外环级联结构的非线性控制律，其中内环控制姿态动力学，外环协调每个四旋翼飞行机器人的平移动力学，以实现高机动编队控制目标；与现有技术相比，本发明所涉及的控制律(净推力fi和体转矩τi)均不取决于线速度矢量，即本发明所提出的四旋翼飞行机器人机动编队控制方法是实现无线速度的，无需线速度测量。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明的范围，否则都应该包括在本发明的保护范围内。