GPS拒止及复杂电磁条件下多机器人编队控制方法与流程

gps拒止及复杂电磁条件下多机器人编队控制方法
技术领域
1.本发明涉及机器人编队技术领域，具体涉及一种gps拒止及复杂电磁条件下多机器人编队控制方法。


背景技术：

2.equation chapter(next)section 1针对传统编队控制算法的不足，考虑到多机器人系统中机器人个数的增加以及单个机器人数据处理能力的不断提升，分布式多机器人控制算法已经受到研究者的广泛关注。分布式多机器人系统，能更加充分利用机器人的数据处理资源，分担中央处理机的压力，在灵活性、容错性方面具有很大优势，而一致性问题是分布式多机器人控制算法中最基本的问题。在理想环境下的一致性问题已经研究的较为深入，取得了许多研究成果。然而，在实际应用环境中，由于存在通信噪声、通信时延、测量误差等不确定因素，导致一致性问题的分析又有了新的困难。
3.在现有技术中，文献《distributed consensus control for second-order agents with fixed topology and time-delay》研究了二阶多机器人系统通信结构分别为有向图含有生成树或强连通图时，在均匀时延条件下系统达到一致性的条件，但文献未考虑噪声条件，也未考虑多种不同时延条件下的一致性问题。文献《sampled-data based average consensus of second-order integral multi-agent systems:switching topologies and communication noises》针对噪声条件下的二阶多机器人系统一致性问题，提出了一种基于分布式采样数据的控制协议来实现系统一致性，但该算法未将时延条件考虑进去。文献《distributed control for multi-agent consensus motions with nonuniform time delays》采用频域分析法研究了二阶多机器人系统在非均匀多种时延条件下的一致性问题，相较于李雅普诺夫法具有更低的保守性，并将结果推广到高阶多机器人系统，但未考虑噪声条件。


技术实现要素：

4.本发明为克服机器人系统时延且噪声环境下的一致性问题，提出一种gps拒止及复杂电磁条件下多机器人编队控制方法。
5.本发明旨在提供一种gps拒止及复杂电磁条件下多机器人编队控制方法，其特征在于，所述gps拒止及复杂电磁条件下多机器人编队控制方法包括以下步骤；
6.步骤1，建立一般多机器人系统模型；
7.步骤2，在真实环境中对多机器人编队系统状态向量模型进行调整，生成含多种时延多机器人系统；
8.步骤3，确定机器人之间通信的最大时延及所处噪声环境的最大幅值，并再以此设计位置比例系数和速度比例系数。
9.更进一步地，在步骤1中，所述一般多机器人系统模型为：
10.11.其中，表示一般多机器人系统模型，ψ为系数矩阵，表示一般多机器人系统模型，ψ为系数矩阵，表示一般多机器人系统模型，ψ为系数矩阵，为克罗内克积，k1为位置比例系数，k2为速度比例系数，i表示单位阵，l表示拉普拉斯矩阵；s(t)为多机器人系统状态向量：
12.s(t)＝[δ1(t)，δ2(t)，δ3(t)，...，δi(t)]
[0013]
其中，δi(t)为第i个机器人在时间t的状态向量：
[0014]
δi(t)＝[xi(t)，vi(t)]
t
[0015]
其中，xi(t)为第i个机器人在时间t的位置，vi(t)为第i个机器人在时间t的速度；
[0016][0017]
其中，ui(t)为第i个机器人在时间t的输入控制量。
[0018]
更进一步地，在步骤2中，所述含多种时延多机器人系统为：
[0019][0020]
其中，ζ(t)为机器人之间的通信噪声或测量噪声，τd为传输时延，ld为当时延为τd时机器人节点组成子拓扑图所对应的拉普拉斯矩阵，
[0021]
更进一步地，在步骤3中，所述含多种时延多机器人系统为无向图时，所述含多种时延多机器人系统的系统时延和噪声与位置比例系数和速度比例系数的关系为：
[0022][0023]
其中，τ
max
表示传输时延τ的最大值，w
max
表示角频率w的最大值，λ
max
表示拉普拉斯矩阵l的最大特征根。
[0024]
更进一步地，在步骤3中，所述含多种时延多机器人系统为为有向图且存在生成树时，所述含多种时延多机器人系统的系统时延和噪声与位置比例系数和速度比例系数的关系为：
[0025]
[0026]
其中，wi表示第i个机器人频域分析时的角频率w，λi表示拉普拉斯矩阵l的非零特征根。
[0027]
本发明的有益效果包括：
[0028]
本发明解决了在多种时延和噪声条件下二阶多机器人系统的一致性问题，通过频域分析法将系统特征方程转化为纯虚数特征根的二次多项式，然后再求解，最终得到其临界稳定状态。
[0029]
本发明提出的控制方法针对全向移动机器人，该全向移动机器人水平和竖直方向可以分开控制，构建成两个一维二阶的多机器人系统，该一维二阶的多机器人系统现实性了在延时和噪声条件下的系统一致性。
附图说明
[0030]
图1是本发明实施例提供的一种gps拒止及复杂电磁条件下多机器人编队控制方法的结构示意图；
[0031]
图2是本发明实施例一提供的系统通信拓扑结构图；
[0032]
图3是本发明实施例一提供的白噪声幅值图；
[0033]
图4是本发明实施例一提供的matlab程序结构图；
[0034]
图5是本发明实施例一中实验1提供的x随时间变化轨迹的仿真图；
[0035]
图6是本发明实施例一中实验1提供的v随时间变化轨迹的仿真图；
[0036]
图7是本发明实施例一中实验2提供的x随时间变化轨迹的仿真图；
[0037]
图8是本发明实施例一中实验2提供的v随时间变化轨迹的仿真图；
[0038]
图9是本发明实施例二提供的系统通信拓扑结构图；
[0039]
图10是本发明实施例二提供的matlab程序结构图；
[0040]
图11是本发明实施例二中实验1提供的x随时间变化轨迹的仿真图；
[0041]
图12是本发明实施例二中实验1提供的v随时间变化轨迹的仿真图；
[0042]
图13是本发明实施例二中实验2提供的x随时间变化轨迹的仿真图；
[0043]
图14是本发明实施例二中实验2提供的v随时间变化轨迹的仿真图；
[0044]
图15是本发明实施例三提供的uwb定位系统实测示意图。
具体实施方式
[0045]
下面结合附图对本发明的技术方案进行更详细的说明，本发明包括但不仅限于下述实施例。
[0046]
如附图1所示，本发明提供了一种gps拒止及复杂电磁条件下多机器人编队控制方法，包括以下步骤：
[0047]
步骤1，建立一般多机器人系统模型。
[0048]
假设系统由n个全向机器人组成，第i个全向机器人x方向上的动态特性为：
[0049][0050]
其中，xi(t)为第i个机器人在时间t的位置，vi(t)为第i个机器人在时间t的速度，
ui(t)为第i个机器人在时间t的输入控制量。若多机器人系统中任意第i个机器人和第q个机器人实现了：
[0051][0052][0053]
则称在控制量ui(t)下多机器人系统的动态特性实现了一致性。
[0054]
设第i个机器人的状态向量为：
[0055]
δi(t)＝[xi(t)，vi(t)]
t
ꢀꢀ
(3)
[0056]
其中，δi(t)为第i个机器人在时间t的状态向量；则多机器人系统状态向量为：
[0057]
s(t)＝[δ1(t)，δ2(t)，δ3(t)，...，δn(t)]
ꢀꢀ
(4)
[0058]
其中，s(t)为多机器人系统状态向量。
[0059]
设定一般多机器人系统为：
[0060][0061]
其中，表示一般多机器人系统模型，ψ为系数矩阵，表示一般多机器人系统模型，ψ为系数矩阵，表示一般多机器人系统模型，ψ为系数矩阵，为克罗内克积。
[0062]
当在无噪声和时延的理想情况下，控制量如下：
[0063][0064]
式中，a
iq
＞0为机器人i与机器人q通信拓扑权重，k1为需要设计的位置比例系数，k2为需要设计的速度比例系数，ni为与第i个机器人具有通信拓扑的机器人集合。
[0065]
当多机器人系统的通信拓扑为连通拓扑图时，系数矩阵ψ有1个双重零根，且其他特征根为负实数。
[0066]
当多机器人系统的通信拓扑为有向图且含有生成树时，当多机器人系统的通信拓扑为有向图且含有生成树时，λi为多机器人系统拉普拉斯矩阵l的特征值，系数矩阵ψ有1个双重零根，且其他特征根实部为负。
[0067]
步骤2，在真实环境中对多机器人编队系统状态向量模型进行调整，生成含多种时延多机器人系统。
[0068]
真实环境中，由于存在噪声干扰，且不同机器人硬件之间通信也存在差异，所以需要对上述控制量进行改进，假设系统中共存在d种不同的时延，含多种时延多机器人系统为：
[0069][0070]
其中，ζ(t)为机器人之间的通信噪声或测量噪声，τd为传输时延，ld为当时延为τd时机器人节点组成子拓扑图所对应的拉普拉斯矩阵，
[0071]
步骤3，确定机器人之间通信的最大时延及所处噪声环境的最大幅值，并再以此设
计位置比例系数和速度比例系数。
[0072]
步骤31，当含多种时延多机器人系统为无向图时，采用频域分析法进行分析，对含多种时延多机器人系统进行拉普拉斯变换，可得：
[0073][0074]
其中，i
2n
为2n阶单位矩阵，in为n阶单位矩阵。
[0075]
令所以行列式|g
τ
(s)|的特征根即为系统的特征根。
[0076]
含多种时延多机器人系统相对于一般多机器人系统系统的一致性条件是在时延τ和噪声ζ(t)的干扰下，如何保持系统特征根在负半平面。因为在真实环境中测量噪声和通信噪声ζ(t)为不确定量，无法进行准确的定量分析，因此只需要考虑在噪声ζ(t)的作用下系统时延τ增大到何值时，使得系统的非零特征根第一次出现在虚轴上，此时的时延τ则为系统保持稳定的临界值。
[0077]
假设系统特征根在虚轴上，所以令s＝jw为其特征根，j表示虚部符号，w表示角频率；为特征根对应的特征向量，且假设为特征根对应的特征向量，且假设为n阶对称阵，则有下式：
[0078][0079]
系统的虚数特征根以共轭形式成对出现，此处仅分析当w＞0的情况。对式(9)左乘αh得下式：
[0080][0081]
其中，αh为α的共轭矩阵。
[0082]
因为式(9)中等号左侧矩阵每一行都为0，所以jwα1＝α2，将其代入式(10)中可得：
[0083][0084]
其中，将其中w改为-w可得下式：
[0085][0086]
对式(12)等号两侧取模得：
[0087][0088]
其中，λ
max
为最大特征根。
[0089]
令即可得，当w≤w
max
，上
式成立。
[0090]
由式(12)可得：
[0091][0092]
其中，θ(w)表示复变函数辐角主值，θ(w)∈[0，2π)。
[0093]
令
[0094]
对τ(w)求导可得：
[0095][0096]
对m2(w)求导可得：
[0097][0098]
所以m2(w)是递减的，所以当w＞0时，m2(w)＜m2(0)＝0，所以m1(w)＜0，即τ(w)也是递减的。所以τ(w)≥τ(w
max
)＝τ
max
。当τd＜τ
max
时，可得：
[0099][0100]
即与τ(w)≥τ(w
max
)相矛盾，因此，当τd＜τ
max
即可使系统特征根保持在左半平面内，从而使得含多种时延多机器人系统实现一致性。
[0101]
当系统时延τd都小于在噪声ζ(t)作用下的τ
max
时，则在控制量ui(t)产生的控制律作用下，系统可实现一致性。其中，系统时延和噪声与位置比例系数和速度比例系数的关系为：
[0102][0103]
步骤32，当含多种时延多机器人系统为有向图且存在生成树时，步骤32，当含多种时延多机器人系统为有向图且存在生成树时，系数矩阵ψ有1个双重零根，且其他特征根实部为负。同样采用频域分析法进行分析，只需要考虑在噪声ζ(t)的作用下系统时延τ增大到何值时，使得含多种时延多机器人系统的非零特征根第一次出现在虚轴上，此时的时延τ则为系统保持稳定的临界值。对式(12)等号两侧分别求模，即：
[0104][0105]
令w为||f(w)||的函数，则式(19)可化简为：
[0106][0107]
由式(20)可得w为关于||f(w)||的递增函数。由式(12)可得：
[0108][0109]
其中，τm表示τ中的最大值。所以：
[0110][0111]
因为所以λi为拉普拉斯矩阵l的非零特征根。所以||f(w)||≤||ζ(t)λi||。因为w为关于||f(w)||的递增函数，所以：
[0112][0113]
当τd＜τ
max
时，可得：
[0114][0115]
即与式(22)相矛盾，因此，当τd＜τ
max
即可使系统特征根保持在左半平面内，使得含多种时延多机器人系统实现一致性。
[0116]
当系统时延τd都小于在噪声ζ(t)作用下的τ
max
时，且则在控制量ui(t)产生的控制律作用下，系统可实现一致性。其中，系统时延和噪声与位置比例系数和速度比例系数的关系为：
[0117][0118]
式中，
[0119]
实施例一
[0120]
如附图2所示，含多种时延多机器人系统由4个机器人组成，1号机器人和2号机器
人与3号机器人与4号机器人之间的时延都为τ1，1号机器人和4号机器人与2号机器人与3号机器人之间的时延都为τ2。设邻接通信权重a
ij
＝1，则拉普拉斯矩阵l为：
[0121][0122]
则λ
max
＝4。
[0123]
如附图3所示，假设通信噪声或测量噪声为最大幅值为2的白噪声。
[0124]
设计k1＝1.2，k2＝1，则根据定理1可得τ
max
＝0.226。
[0125]
如附图4所示，通过matlab程序进行仿真实验。
[0126]
如附图5-6所示，第1组实验中设定τ1＝0.21，τ2＝0.22，初始位姿假设为(1，0)，(2，0)，(3，0)，(4，0)。
[0127]
如附图7-8所示，第2组实验在同样条件下设定τ1＝0.23，τ2＝0.24，其余初始位置和初始速度仍保持不变。
[0128]
将第1组试验与第2组试验对比可知，当所有τd都小于τ
max
时含多种时延多机器人系统能够达到一致；当所有τd都大于τ
max
时含多种时延多机器人系统发散，无法达到一致。
[0129]
实施例二
[0130]
如附图9所示，含多种时延多机器人系统由4个机器人组成，1号机器人到2号机器人τ1，2号机器人到3号机器人为τ2，3号机器人到4号机器人时延为τ3，4号机器人到1号机器人时延为τ4。设邻接通信权重a
ij
＝1，则拉普拉斯矩阵l为：
[0131][0132]
则所以real(λi)表示λi的实部，imag(λi)表示λi的虚部。与实验1类似，假设通信噪声或测量噪声为最大幅值为2的白噪声，设计k1＝1，k2＝1，则根据定理2可得τ
max
。
[0133]
如附图10所示，通过matlab程序进行仿真实验。
[0134]
如附图11-12所示，第1组实验中设定τ1＝0.13，τ2＝0.12，τ3＝0.11，τ4＝0.1，初始位姿假设为(1，0)，(2，0)，(3，0)，(4，0)。
[0135]
如附图13-14所示，第2组实验在同样条件下设定τ1＝0.14，τ2＝0.141，τ3＝0.142，τ4＝0.143。
[0136]
将第1组试验与第2组试验对比可知，当所有τd都小于τ
max
时含多种时延多机器人系统能够达到一致；当所有τd都大于τ
max
时含多种时延多机器人系统发散，无法达到一致。
[0137]
实施例三
[0138]
在本实施例中，因为全向机器人为全驱动机器人，水平和竖直方向可以分开控制，能够构造成两个一维二阶多机器人系统，因此用全向机器人来验证所提算法。需要指出的是在将所提算法应用于实际多机器人系统时，需要首先确定机器人之间通信的最大时延及
所处噪声环境的最大幅值，然后再以此设计位置比例系数和速度比例系数。同时需要注意的是，在上述所提算法中，并没有考虑多机器人系统在达到一致性过程中的避碰行为，只考虑了系统是否能在控制律作用下达到一致，因此在实验过程中，利用了平台机器人底层控制程序中的避碰算法程序，当机器人即将出现碰撞时，中断编队算法程序运行，执行避碰行为，当机器人之间达到安全距离后，继续执行编队算法程序。
[0139]
本实施例中，邻接通信权重a
ij
＝1，则中央处理机通过ssh远程登录每个机器人，利用ping命令获取通讯权重a
ij
不为0的两个机器人之间的通信延迟，实际通信环境中机器人之间的时延是时变的，取其最大时延，即τ
a1
＝0.056s，τ
a2
＝0.043s，τ
a3
＝0.047s，τ
a4
＝0.061s，测得每个机器人接收数据并处理所用时间分别为τ
b1
＝0.021s，τ
b2
＝0.02s，τ
b3
＝0.021s，τ
b4
＝0.021s，则机器人之间时延为τ1＝0.077s，τ2＝0.063s，τ3＝0.068s，τ4＝0.082s。由于在本实施例中实验室环境下实验，所以假设通信噪声为最大幅值为2的白噪声。依据式(25)及全向机器人移动速度，设计k1＝1，k2＝1.4。实验中，将4辆全向机器人置于任意位姿，分别为(0.83，2.20，0)，(0.35，1.74，0)，(0.67，0.88，0)，(0.56，0.48，0)。在本市实施例中实验的机器人无法汇聚到一点，因此将式(2)改为：
[0140][0141]
其中，f
p
为队形参数，p＝1，2，...，n。由于系统只安装了uwb测距传感器来给机器人提供定位，因此在实验开始之前机器人会进行位姿判断，并将机器人的机体坐标系与全局坐标系保持一致。
[0142]
如附图15所示，由实施例中实验可得，在多种时延和噪声环境下，多机器人系统最终能够达到一致并形成编队，验证了所提算法的有效性。
[0143]
本发明不仅局限于上述具体实施方式，本领域一般技术人员根据实施例和附图公开内容，可以采用其它多种具体实施方式实施本发明，因此，凡是采用本发明的设计结构和思路，做一些简单的变换或更改的设计，都落入本发明保护的范围。