基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质

1.本发明涉及多移动机器人控制领域，特别涉及一种基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质。


背景技术：

2.随着智能制造业的快速发展，多移动机器人的应用需求在逐步提升。受自然界群体行为启发，在实际的工程应用中，人们也将多个无人系统通过通信连接实现群体行为，比如多无人机、多水下航行器、多自主移动机器人等的协同作业。
3.对多个无人机来说，通过协同编队执行侦察打击任务，不仅可以增加侦查或者攻击的范围，同时也提升了整体系统的鲁棒性，不会因为单个系统故障或失败而影响整个任务；在太空中，多个小卫星协同编队可以获得更好的观测效果，并且降低了对单个卫星观测能力的要求；在水下，多个无人潜航器可以通过协同来提高探测精度或者扩大整体搜寻范围，不仅提升了整个系统的信息搜集能力，同时也提高了搜寻效率；在陆地上，多个自主移动机器人可以通过协同来对危险环境进行监测，甚至在战场上完成伤员的搜救工作，每个移动机器人仅仅需要负责部分区域，然后通过整体的编队协同高效完成整个区域的监测或者搜救任务。
4.实际的多移动机器人编队系统中存在诸多不确定因素，如地形因素、地面摩擦力、移动机器人的检测噪声等，另外各移动机器人通过无线网络进行系统信息的参数与共享，相比于有限通信网络，无线通信网络的带宽资源有限，信息的传输跟任意受到干扰。因此，在多移动机器人协同编队控制器设计中需要重点基于机器人间的通信干扰问题。现有的编队控制研究绝大部分针对的编队队形不变的情况，但在现实情况中跟随机器人和领航机器人的相对位置是需要根据具体的任务而改变的。


技术实现要素：

5.针对多移动机器人的编队控制和队形变换控制问题，本发明提供一种基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质，采用基于领航-跟随的多移动机器人编队控制方法，以解决上述问题。
6.本发明是通过以下技术方案实现的：
7.第一方面，本发明提供了一种基于通信干扰的多移动机器人协同编队控制方法，包括：通过坐标变换建立轮式移动机器人的运动学模型、编队控制的运动学模型；
8.对于领航机器人，通过建立机器人的运动学模型，将当前实际位姿与期望位姿通过分析得到位姿误差并输入到编队控制器中，计算得到控制机器人跟踪期望轨迹的线速度和角速度；
9.对于跟随机器人，通过建立机器人的运动学模型，根据领航机器人当前实际位姿和相对位置计算得到跟随机器人的期望位姿，再通过分析得到位姿误差；
10.采用滑模变结构控制，利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，实现跟踪误差的快速收敛。
11.进一步地，所述轮式移动机器人的运动学模型的建立步骤如下：
12.在由n个轮式移动机器人组成的领航-跟随型编队系统中，用i＝{1,2,...,n}表示第i个移动机器人，其中第一个为领航机器人。假设移动机器人的速度较低，忽略车轮的横向滑动和行驶过程中所受的横向力，建立以地面为参考的全局坐标系xoy和以移动机器人为参考的机器人坐标系xoy，其中θi为移动机器人移动方向和水平方向的夹角；
13.移动机器人实际位姿坐标为(xi,yi,θi)，向前移动时线速度和角速度分别为vi、ωi，机器人运动学模型为：
[0014][0015]
其中表示机器人的运动学模型；
[0016]
在xoy坐标下给定的领航机器人期望轨迹位姿坐标为(xr,yr,θr)，实际位姿坐标为(x1,y1,θ1)，期望轨迹的线速度和角速度分别为vr和ωr，实际轨迹的线速度角速度分别为ν1和ω1，利用平面坐标变换，领航机器人的位姿误差方程为：
[0017][0018]
对式(2)进行微分，结合式(1)整理可以得到领航机器人位姿误差微分方程：
[0019][0020]
进一步地，所述编队控制的运动学模型建立过程如下：
[0021]
r1为领航机器人，ri(i＝{2,3,...,n})为跟随机器人，r
vi
为ri在期望队形中的映射机器人，线速度和角速度分别为v
vi
和ω
vi
，在运动过程中，r1和r
vi
之间的相对位置用δi＝[δ
ix
,δ
iy
]∈r2描述，其中δ
ix
为跟随机器人的期望位姿与领航机器人当前实际位姿在x方向上的偏差，δ
iy
为跟随机器人的期望位姿与领航机器人位姿在y方向上的偏差；利用平面坐标变换，得到跟随机器人的期望位姿为：
[0022][0023]
(x1,y1,θ1)
t
为领航机器人的位姿，(x
vi
,y
vi
,θ
vi
)
t
为跟随机器人ri(i＝{2,3,...,n})的期望位姿，则跟随机器人的位姿误差方程为：
[0024][0025]
对式(5)进行微分并将式(1)代入继续整理得到可以得到跟随机器人位姿误差微分方程：
[0026][0027]
进一步地，所述利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，具体步骤如下：
[0028]
多移动机器人编队任务描述：由n个轮式移动机器人组成的领航-跟随型编队系统中，编队控制的目的在于调整各个机器人的位置以特定的几何图形移动，即跟随机器人和领航机器人的线速度和角速度保持一致，即v
vi
＝v1，ω
vi
＝ω1，并且相对位置和方向满足期望的拓扑和物理约束：
[0029][0030][0031][0032]
为实现多移动机器人的编队控制和队形变换控制，提出了基于有限时间趋近律的编队控制策略，包括：
[0033]
对于领航机器人，首先根据式(1)建立机器人的运动学模型，将当前实际位姿与期望位姿根据式(2)分析得到位姿误差并输入到编队控制器中，计算得到控制领航机器人跟踪期望轨迹的线速度和角速度；
[0034]
对于跟随机器人，同样根据式(1)建立机器人的运动学模型，然后根据领航机器人当前实际位姿和相对位置δi＝[δ
ix
,δ
iy
]∈r2计算得到跟随机器人的期望位姿，再根据式(5)分析得到位姿误差；
[0035]
设计滑模切换函数：
[0036][0037]
通过设计移动机器人编队控制器，使得s1和s2趋于0，即可使收敛到0，并且收敛到从而保证和实现移动机器人的轨迹跟踪和编队控制；
[0038]
设计有限时间趋近律：
[0039][0040]
其中k1，k2，ε1，ε2，η1，η2均为可调参数，δ1和δ2分别为fal(s1,η1,δ1)、fal(s2,η2,δ2)在原点附近正负对称线性段的区间长度，并且fal(s1,η1,δ1)和fal(s2,η2,δ2)均为非连续函数，
[0041][0042][0043]
令将式(12)求导并结合式(5)和式(13)得：
[0044][0045]
整理上式可以得到控制律：
[0046][0047]
其中特别的，当i＝1时，以上各式中v
v1
＝vr，ω
v1
＝ωr；
[0048]
当时，选择李雅普诺夫函数：
[0049][0050]
令对上式求导可得：
[0051]
[0052]
当且仅当时等号成立，则：
[0053][0054]
保证收敛到并且收敛到0，则收敛到0。
[0055]
第二方面，本发明提供一种基于通信干扰的多移动机器人协同编队控制装置，其特征在于，所述装置包括：
[0056]
获取模块，用于通过坐标变换建立轮式移动机器人的运动学模型、编队控制的运动学模型；
[0057]
第一分析模块，对于领航机器人，通过建立机器人的运动学模型，将当前实际位姿与期望位姿通过分析得到位姿误差并输入到编队控制器中，计算得到控制机器人跟踪期望轨迹的线速度和角速度；
[0058]
第二分析模块，对于跟随机器人，通过建立机器人的运动学模型，根据领航机器人当前实际位姿和相对位置计算得到跟随机器人的期望位姿，再通过分析得到位姿误差；
[0059]
控制模块，采用滑模变结构控制，利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，实现跟踪误差的快速收敛。
[0060]
第三方面，本发明提供一种电子设备，其特征在于，包括：处理器和存储器，所述处理器用于执行所述存储器中存储的计算机程序，以使所述电子设备执行上述任一项所述的机器人协同编队控制方法。
[0061]
第四方面，本发明提供一种计算机可读存储介质，其特征在于，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现上述任一项所述的机器人协同编队控制方法。
[0062]
本发明的优点是：
[0063]
(1)本发明所设计的多移动机器人协同编队控制方法，可实现编队误差的有限时间收敛，并且削弱了系统的抖振现象，从而加快协同编队系统的相应速度；
[0064]
(2)本发明所设计的多移动机器人协同编队控制方法基于了多移动机器人间的通信干扰，实验结果表明该多移动机器人编队控制方法在队形变换控制时同样具有较好的抗干扰性能；
[0065]
(3)本发明所设计的多移动机器人协同编队控制方法可实现多移动机器人的编队控制和队形变换控制，能满足实际应用场景。
附图说明
[0066]
图1是本发明实施例移动机器人间通信拓扑结构；
[0067]
图2是本发明实施例移动机器人位姿模型；
[0068]
图3是本发明实施例移动机器人编队控制运动学模型；
[0069]
图4是本发明实施例移动机器人编队控制结构；
[0070]
图5是本发明实施例期望编队队形；
[0071]
图6是本发明实施例编队控制仿真中移动机器人编队运动轨迹1；
[0072]
图7是本发明实施例编队控制仿真中跟随机器人航向角误差1；
[0073]
图8是本发明实施例编队控制仿真中跟随机器人与领航机器人x轴方向距离1；
[0074]
图9是本发明实施例编队控制仿真中跟随机器人与领航机器人y轴方向距离1；
[0075]
图10是本发明实施例编队控制仿真中干扰下移动机器人编队运动轨迹1；
[0076]
图11是本发明实施例编队控制仿真中干扰下跟随机器人航向角误差1；
[0077]
图12是本发明实施例编队控制仿真中干扰下跟随机器人与领航机器人x轴方向距离1；
[0078]
图13是本发明实施例编队控制仿真中干扰下跟随机器人与领航机器人y轴方向距离1；
[0079]
图14是本发明实施例队形变换控制仿真中移动机器人编队运动轨迹2；
[0080]
图15是本发明实施例队形变换控制仿真中跟随机器人航向角误差2；
[0081]
图16是本发明实施例队形变换控制仿真中跟随机器人与领航机器人x轴方向距离2；
[0082]
图17是本发明实施例队形变换控制仿真中跟随机器人与领航机器人y轴方向距离2；
[0083]
图18是本发明实施例队形变换控制仿真中干扰下移动机器人编队运动轨迹2；
[0084]
图19是本发明实施例队形变换控制仿真中干扰下跟随机器人航向角误差2；
[0085]
图20是本发明实施例队形变换控制仿真中干扰下跟随机器人与领航机器人x轴方向距离2；
[0086]
图21是本发明实施例队形变换控制仿真中干扰下跟随机器人与领航机器人y轴方向距离2。
具体实施方式
[0087]
下面结合附图，对本发明的实施例作详细说明，下述的实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
[0088]
实施例：
[0089]
参见附图1-21，针对多移动机器人的编队控制和队形变换控制问题，本发明实施例提供了一种基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质，所述方法采用基于领航-跟随的多移动机器人编队控制方法。具体技术方案如下：
[0090]
第一方面，本发明提供了一种基于通信干扰的多移动机器人协同编队控制方法，包括：通过坐标变换建立轮式移动机器人的运动学模型、编队控制的运动学模型；
[0091]
对于领航机器人，通过建立机器人的运动学模型，将当前实际位姿与期望位姿通过分析得到位姿误差并输入到编队控制器中，计算得到控制机器人跟踪期望轨迹的线速度和角速度；
[0092]
对于跟随机器人，通过建立机器人的运动学模型，根据领航机器人当前实际位姿和相对位置计算得到跟随机器人的期望位姿，再通过分析得到位姿误差；
[0093]
采用滑模变结构控制，利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，实现跟踪误差的快速收敛。
[0094]
进一步地，所述轮式移动机器人的运动学模型的建立步骤如下：
[0095]
在由n个轮式移动机器人组成的领航-跟随型编队系统中，用i＝{1,2,...,n}表示
第i个移动机器人，其中第一个为领航机器人。假设移动机器人的速度较低，忽略车轮的横向滑动和行驶过程中所受的横向力，建立以地面为参考的全局坐标系xoy和以移动机器人为参考的机器人坐标系xoy，其中θi为移动机器人移动方向和水平方向的夹角；
[0096]
移动机器人实际位姿坐标为(xi,yi,θi)，向前移动时线速度和角速度分别为vi、ωi，机器人运动学模型为：
[0097][0098]
其中表示机器人的运动学模型；
[0099]
在xoy坐标下给定的领航机器人期望轨迹位姿坐标为(xr,yr,θr)，实际位姿坐标为(x1,y1,θ1)，期望轨迹的线速度和角速度分别为vr和ωr，实际轨迹的线速度角速度分别为ν1和ω1，利用平面坐标变换，领航机器人的位姿误差方程为：
[0100][0101]
对式(2)进行微分，结合式(1)整理可以得到领航机器人位姿误差微分方程：
[0102][0103]
进一步地，所述编队控制的运动学模型建立过程如下：
[0104]
r1为领航机器人，ri(i＝{2,3,...,n})为跟随机器人，r
vi
为ri在期望队形中的映射机器人，线速度和角速度分别为v
vi
和ω
vi
，在运动过程中，r1和r
vi
之间的相对位置用δi＝[δ
ix
,δ
iy
]∈r2描述，其中δ
ix
为跟随机器人的期望位姿与领航机器人位姿在x方向上的偏差，δ
iy
为跟随机器人的期望位姿与领航机器人位姿在y方向上的偏差；利用平面坐标变换，得到跟随机器人的期望位姿为：
[0105][0106]
(x1,y1,θ1)
t
为领航机器人的位姿，(x
vi
,y
vi
,θ
vi
)
t
为跟随机器人ri(i＝{2,3,...,n})的期望位姿，则跟随机器人的位姿误差方程为：
[0107][0108]
对式(5)进行微分并将式(1)代入继续整理得到可以得到跟随机器人位姿误差微分方程：
[0109][0110]
进一步地，所述利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，具体步骤如下：
[0111]
多移动机器人编队任务描述：由n个轮式移动机器人组成的领航-跟随型编队系统中，编队控制的目的在于调整各个机器人的位置以特定的几何图形移动，即跟随机器人和领航机器人的线速度和角速度保持一致，即v
vi
＝v1，ω
vi
＝ω1，并且相对位置和方向满足期望的拓扑和物理约束：
[0112][0113][0114][0115]
为实现多移动机器人的编队控制和队形变换控制，提出了基于有限时间趋近律的编队控制策略，包括：
[0116]
对于领航机器人，首先根据式(1)建立机器人的运动学模型，将当前实际位姿与期望位姿根据式(2)分析得到位姿误差并输入到编队控制器中，计算得到控制领航机器人跟踪期望轨迹的线速度和角速度；
[0117]
对于跟随机器人，同样根据式(1)建立机器人的运动学模型，然后根据领航机器人当前实际位姿和相对位置δi＝[δ
ix
,δ
iy
]∈r2计算得到跟随机器人的期望位姿，再根据式(5)分析得到位姿误差；
[0118]
设计滑模切换函数：
[0119][0120]
通过设计移动机器人编队控制器，使得s1和s2趋于0，即可使收敛到0，并且收敛到从而保证和实现移动机器人的轨迹跟踪和编队控制；
[0121]
设计有限时间趋近律：
[0122][0123]
其中k1，k2，ε1，ε2，η1，η2均为可调参数，δ1和δ2分别为fal(s1,η1,δ1)、fal(s2,η2,δ2)在原点附近正负对称线性段的区间长度，并且fal(s1,η1,δ1)和fal(s2,η2,δ2)均为非连续函数，
[0124]
[0125][0126]
令将式(12)求导并结合式(5)和式(13)得：
[0127][0128]
整理上式可以得到控制律：
[0129][0130]
其中特别的，当i＝1时，以上各式中v
v1
＝vr，ω
v1
＝ωr；
[0131]
当时，选择李雅普诺夫函数：
[0132][0133]
令对上式求导可得：
[0134][0135]
当且仅当时等号成立，则：
[0136][0137]
保证收敛到并且收敛到0，则收敛到0。
[0138]
第二方面，本发明提供一种基于通信干扰的多移动机器人协同编队控制装置，其特征在于，所述装置包括：
[0139]
获取模块，用于通过坐标变换建立轮式移动机器人的运动学模型、编队控制的运动学模型；
[0140]
第一分析模块，对于领航机器人，通过建立机器人的运动学模型，将当前实际位姿与期望位姿通过分析得到位姿误差并输入到编队控制器中，计算得到控制机器人跟踪期望轨迹的线速度和角速度；
[0141]
第二分析模块，对于跟随机器人，通过建立机器人的运动学模型，根据领航机器人当前实际位姿和相对位置计算得到跟随机器人的期望位姿，再通过分析得到位姿误差；
[0142]
控制模块，采用滑模变结构控制，利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，实现跟踪误差的快速收敛。
[0143]
第三方面，本发明提供一种电子设备，其特征在于，包括：处理器和存储器，所述处理器用于执行所述存储器中存储的计算机程序，以使所述电子设备执行上述任一项所述的机器人协同编队控制方法。
[0144]
第四方面，本发明提供一种计算机可读存储介质，其特征在于，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现上述任一项所述的机器人协同编队控制方法。
[0145]
为了验证本发明所提出的领航跟随型编队控制方法的有效性，分别对由五个移动机器人组成的编队进行编队队形控制和队形变换数值仿真实验，期望的编队队形共有三种，如图8所示。式(13)中有限时间趋近律滑模控制参数设置为k1＝k2＝3，δ1＝δ2＝0.2，η1＝η2＝0.5，ε1＝ε2＝0.01。
[0146]
图5所示期望编队队形中，r1和r
vi
之间的相对位置矩阵为
[0147][0148]
进一步地，多移动机器人编队控制仿真步骤如下：
[0149]
首先采用4个跟随机器人和1个领航机器人组成的多移动机器人系统在matlab/simulink下进行编队控制数值仿真。
[0150]
移动机器人的初始条件如表1所示
[0151]
表1移动机器人初始条件
[0152][0153]
领航-跟随型编队控制任务分为三个阶段：i)0至9.4s为第一个阶段，期望编队队形为图5的(1)，领航者的期望轨迹为直线，期望线速度为vr＝4.8m/s，期望角速度为ωr＝0rad/s；ii)9.4s至25.1s为第二个阶段，期望编队队形不变，领航者的期望轨迹为半径为24米的圆弧，期望线速度为vr＝4.8m/s，期望角速度为ωr＝0.2rad/s；iii)25.1s至34.6s为第三个阶段，期望编队队形不变，领航者的期望轨迹为直线，期望线速度为vr＝4.8m/s，期望
角速度为ωr＝0rad/s。仿真结果如图6至图9所示。
[0154]
图7至图9为跟随机器人与领航机器人航向角、x轴方向和y轴方向误差，由于开始时机器人处于静止状态，并且每个机器人位置和航向角与期望值均存在一定误差，因此收敛至期望队形用时较长，为2.61s。
[0155]
为验证本发明所提出的多移动机器人编队控制方法的抗干扰性能，基于到移动机器人在实际的运动过程中任意受到地形和电子信号等因素对机器人将通信的影响，假设跟随机器人和领航机器人在12.5s至13.5s之间存在短时的通信干扰，在状态变量x和y中加入时变函数和来模拟外部干扰。仿真结果如图10至图13所示。
[0156]
进一步地，多移动机器人队形变换控制仿真步骤如下：
[0157]
同样采用4个跟随机器人和1个领航机器人组成的多移动机器人系统在matlab/simulink下进行编队控制和队形变换控制数值仿真。
[0158]
移动机器人的初始条件如表2所示
[0159]
表2移动机器人初始条件
[0160][0161]
领航-跟随型编队任务分为三个阶段：
[0162]
i)0至15.7s为第一阶段，期望编队队形为图5的(1)，领航者的期望轨迹为半径为24米的圆弧：
[0163][0164]
期望线速度为vr＝4.8m/s，角速度为ωr＝0.2rad/s；
[0165]
ii)15.7s至30s为第二个阶段，期望编队队形为图5的(2)，领航者的期望轨迹为半径为32米的圆弧：
[0166][0167]
期望线速度和角速度分别为vr＝4.8m/s和ωr＝0.15rad/s；
[0168]
iii)30s至50s为第三个阶段，期望编队队形为图5的(3)，领航者的期望轨迹不变。
[0169]
仿真结果如图14至图17所示。
[0170]
图14为5个移动机器人系统在各个运动时刻的队形，每个机器人用不同颜色的圆
圈表示，圆圈中的箭头表示各移动机器人的运动方向。在0s、15.7s和30s开始队形变换，5个机器人在三个阶段均可收敛到期望队形并沿着期望轨迹运动。
[0171]
图15至图17为跟随机器人与领航机器人航向角、x轴方向和y轴方向误差，由于开始机器人处于静止状态，并且每个机器人位置和航向角与期望值偏差较大，第一阶段收敛至期望队形δⅰ用时较长，为2.00s。第二阶段编队队形调整为δⅱ，编队队形仍为正方形，只是跟随机器人和领航机器人在x轴和y轴方向的距离增大，因此调整用时较短，为1.79s。第三阶段编队队形调整为δⅲ，编队队形由正方形变换为菱形，由于队形变化较大，在队形调整过程中跟随机器人1和4与领航机器人航向角误差偏大，调整时间边长，为1.82s。
[0172]
同样的，为验证本文所提出的多移动机器人编队控制方法在队形变换控制时的抗干扰性能，基于到移动机器人在实际的运动过程中任意受到地形和电子信号等影响，假设跟随机器人和领航机器人在20s至21s之间存在短时的通讯干扰，在状态变量x和y中加入时变函数和来模拟外部干扰。仿真结果如图18至21所示。
[0173]
由图18至图21可知，20s至21s加入扰动后各跟随机器人的位置和方向同样都受到了影响，航向角误差最大值为0.3259rad，与领航机器人x轴和y轴方向距离误差为1.4525m和0.5829m。扰动结束后，跟随机器人用时1.59s便恢复了期望队形，可见该多移动机器人编队控制方法在队形变换控制时同样具有较好的抗干扰性能。
[0174]
由图6至图21的实验结果可知，本文设计的编队控制策略能够有效应用于多移动机器人的编队控制和队形变换控制中。
[0175]
本发明上述实施例提供的基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质，所述控制方法是一种领航-跟随型多移动机器人编队控制方法，重点是通过坐标变换定义非完整约束轮式移动机器人的运动学模型，采用滑模变结构控制，利用每个小车的跟踪误差设计编队控制器以实时调整机器人线速度和角速度，实现跟踪误差的快速收敛。所发明的编队控制方法能有效应用于多移动机器人的编队控制和队形变换控制中，并且具有较好的抗干扰性能，使多移动机器人系统能保持期望队形并按照期望轨迹运动，有助于解决多移动机器人系统编队、协作搬运等复杂作业任务。
[0176]
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的试验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。