一种基于虚拟领航-领航-跟随的多机器人编队控制方法与流程

1.本发明属于机器人编队控制技术领域，具体涉及一种基于虚拟领航-领航-跟随的多机器人编队控制方法。


背景技术：

2.近年来，消防机器人在实战中也得到越来越多的应用。但是从应用效果来看，目前的消防机器人绝大多数需要依赖后台消防人员的远程操作，并且存在侦察视野窄、人机交互差、火源定位受限等技术挑战，从而无法适应更加复杂的灾情。面向强干扰、高动态等复杂火灾环境下的消防需求，为实现机器人系统的任务能力扩展及整体消防效能提升，采取多种智能消防机器人协同作业的模式，实现能力互补和行动协调，已成为未来消防应用的主要发展方向。智能群组消防机器人在执行编队追踪任务时，由于行驶道路上存在各种未知障碍物以及受道路两侧边缘约束，机器人既要保证行驶在安全区域内，又要在行驶过程中快速避开前方障碍物，同时当通道宽度发生变化时，编队机器人能进行动态调整队形形状以最优构型通过通道。因此需要一种高效、安全、可靠的编队避障算法，以提高编队避障的灵活性和编队结构的稳定性。


技术实现要素：

3.鉴于上述的分析，本发明旨在公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法，用于解决多机器人的编队控制问题。
4.本发明公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法，包括以下步骤：
5.建立虚拟领航-领航-跟随的运动结构；指定编队中一台机器人为领航机器人，其余为跟随机器人，并设定一台虚拟领航机器人在编队参考轨迹点上随队行进；
6.领航机器人实时跟踪虚拟领航机器人的位置，根据检测和提取的包括道路边界及障碍物在内的环境信息，进行队形变换动态优化，得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境；
7.通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制；使每个机器人对虚拟领航机器人的实时位置进行跟踪；通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零；使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。
8.进一步地，跟随机器人与虚拟领航机器人之间的期望距离和期望方位角的获取方法，包括：
9.机器人编队以期望的初始队列构型，跟随虚拟领航机器人行进；
10.领航机器人利用自身携带的环境感知传感器检测和提取出道路边界信息以及障碍物信息，并计算出队形的伸缩因子；
11.根据队形的伸缩因子确定出队形变换指令分发到跟随机器人，使跟随机器人得到
与虚拟领航机器人之间的期望距离和期望方位角。
12.进一步地，所述领航机器人实时跟踪虚拟领航机器人的位置，并且通过自身携带的多线激光雷达实时感知环境信息，提取道路的边界、计算两侧道路宽度，检测和识别障碍物信息；并将检测到的部分障碍物边界点作为候选边界点，对道路边界点进行缺失修补，以得到最终的边界点信息。
13.进一步地，通过对道路边界和障碍物的轮廓进行膨胀处理，设置膨胀层，使编队中机器人与道路边界和障碍物之间的距离大于膨胀层宽度。
14.进一步地，领航机器人根据测得的最小通行宽度和初始队列构型的期望队形宽度之间的比值，作为队形的伸缩因子。
15.进一步地，根据队形的伸缩因子确定出队形变换指令包括：
16.当伸缩因子大于1时，输出队形零变换指令，使编队保持原队形继续前进；
17.当伸缩因子处于第一阈值和1的取值区间内时；输出队形同构变换指令，使编队进行队形收缩后通过前方的通道；
18.当伸缩因子处于第二阈值和第一阈值的取值区间内时；输出队形异构变换指令，使编队变换成新的队形通过前方通道；
19.当伸缩因子不大于第二阈值时，输出队形异常状态指令，编队无法通过前方通道，等待下一步指令。
20.进一步地，所述分布式协同耦合的编队控制器的编队控制方法包括：
21.根据虚拟领航机器人和编队机器人的一阶积分器模型，确定出编队机器人与虚拟领航机器人之间的相对位移和相对速度；
22.建立圆形运动控制律，引导每个编队机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心、以与虚拟领航机器人之间期望距离为半径的圆上；
23.在圆形运动控制律的基础上耦合一个方位角定位的控制律；根据耦合了方位角定位的圆形运动控制律和避障控制律，对每个编队机器人进行具有避障功能的编队控制。
24.进一步地，所述避障控制律通过设置斥力势场产生排斥力进行避障。
25.进一步地，在设置斥力势场中，通过建立道路边界作用的斥力速度，以限制机器人的行驶区域；将障碍物作用的斥力速度和道路边界作用的斥力速度进行合成得到斥力合速度；通过调整斥力合速度的方向输出最终的避障控制律，以解决局部极小值和目标不可达问题，使编队机器人在道路的安全区域内进行避障。
26.进一步地，建立动态权重因子，对耦合了方位角定位的圆形运动控制律和避障控制律进行比值调整，得到分布式协同耦合的编队控制器最终输出的控制律。
27.本发明至少可实现以下有益效果之一：
28.本发明的基于虚拟领航-领航-跟随的多机器人编队控制方法，解决多机器人的编队控制问题。
29.本发明采用虚拟领航-领航-跟随模式，领航机器人既负责整个系统规划和协调，但又不影响到其他机器人的运动状态。解决了传统的领航者-跟随者模式的链式结构中跟随机器对领航机器的过度依赖，容易造成跟踪误差累积以及领航机器出现故障会导致整个系统出现崩溃等问题。
30.本发明建立了队形的伸缩因子，在常规的队形变换状态基础上还考虑了异常状态
情况，即当前方通道宽度不满足编队个体通过时的情况，需等待下一步指令；提出了衡量编队变换策略优劣的性能综合评价指标函数，该指标能够不依赖于系统的动力学模型，其表达形式简单明了，不仅能够适用于多消防机器人的时变编队变换而且能适用于其他多智能体系统。
31.本发明从实际应用出发，考虑了道路边界和停靠在路边的障碍物会与机器人发生碰撞，通过对道路边界和障碍物的轮廓添加了膨胀处理，使得机器人尽可能与道路边界和障碍物之间保持一个安全距离。
32.本发明建立了一种分布式协同耦合的编队控制器，在圆形运动控制律的基础上建立增加方位角定位的控制律，与避障控制律结合，能同时实现按编队队形行进的过程中有效地避开障碍物。
33.本发明设置斥力势场产生排斥力进行避障，能够不依赖于系统的精确数学模型，以非常简单的算法实现不确定性高的非线性耦合动态系统的避障控制，以及实现高效、安全、可靠的编队避障。并且把道路边界作用效果也考虑进去，使得机器人能够在道路的安全区域内有效避障，同时有效地解决传统人工势场法存在的局部最优和目标不可达的问题。
附图说明
34.附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
35.图1为本发明实施例中的基于虚拟领航-领航-跟随的多机器人编队控制方法流程图；
36.图2为本发明实施例中的坐标系系统图；
37.图3为本发明实施例中的多机器人编队几何结构示意图；
38.图4为本发明实施例中的队形形状示例图；
39.图5为本发明实施例中的多机器人编队行走过程中遇到的通道示意图；
40.图6为本发明实施例中的队形变换动态优化流程图；
41.图7为本发明实施例中的传统人工势场法存在局部最优和目标不可达的问题示意图；
42.图8为本发明实施例中的改进的人工势场法实现效果图；
43.图9为本发明实施例中的对数障碍函数图；
44.图10为本发明实施例中的分布式协同耦合的编队控制器结构框图。
具体实施方式
45.下面结合附图来具体描述本发明的优选实施例，其中，附图构成本技术一部分，并与本发明的实施例一起用于阐释本发明的原理。
46.本发明的一个实施例公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法，如图1所示，包括以下步骤：
47.步骤s101、建立虚拟领航-领航-跟随的运动结构；指定编队中一台机器人为领航机器人，其余为跟随机器人，并设定一台虚拟领航机器人在编队参考轨迹点上随队行进；
48.步骤s102、领航机器人实时跟踪虚拟领航机器人的位置，根据检测和提取的包括
道路边界及障碍物在内的环境信息，进行队形变换动态优化，得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境；
49.步骤s103、通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制；使每个机器人对虚拟领航机器人的实时位置进行跟踪；通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零；使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。
50.考虑一个由n(n≥2)台消防机器人组成的多机器人编队。由于传统的领航者-跟随者模式中跟随者对领航者的过度依赖，这种链式结构容易造成跟踪误差累积以及领航者出现故障会导致整个系统出现崩溃等问题，本实施例中建立虚拟领航-领航-跟随运动结构。
51.具体的，在本实施例中以多消防机器人编队的编队控制为例。消防机器人在室外场景下使用内置双差分gnss 4g模块辅助的epsilon-d4g模组(rtk)，可通过千寻服务实现稳定的厘米级定位。epsilon-d4g模块提供的定位信息有经度、纬度、高度和航向角等信息。为方便机器人的位姿表示，需要将rtk测得的经纬度和高度数据转换到惯性坐标系下机器人的位置，航向角转换到导航坐标系下机器人的偏航角。
52.为了更加准确和方便地描述消防机器人的运动状态，需定义合适的坐标系系统以及各坐标系之间的转换关系，定义的坐标系系统的如图2所示。其中，∑{oe,xe,ye,ze}为地心地固坐标系(ecef)，坐标原点oe与地球中心重合，xe和ye位于赤道平面，但是分别指向初始子午线和东经90度的子午线，ze与地球自转轴重合；∑{on,xn,yn,zn}为惯性坐标系与地面固联，按照东北天(enu)坐标系建立，即三轴xn、yn、zn分别指向地理正东、正北、正上方。π{or,xr,yr,zr}为消防机器人的本体坐标系，其中or与机器人的几何中心固联，其三轴xr，yr，zr分别按机器人运动正前、正左、正上方向选取。
53.首先需要通过椭球参数变换将经纬度和高度数据转换到ecef笛卡尔位置，转换公式为：
[0054][0055][0056]
其中，λb、λ
l
、λh分别表示测得的维度、经度和高度，n为卯酉圈曲率半径，l
а
和e分别表示地球椭圆的长半轴和第一偏心率。其中，l
а
＝6378137
±
2m，e2＝0.0066943799013m。
[0057]
接着，将ecef坐标系转换到惯性坐标系下，转换关系为：
[0058][0059]
其中，为基准点在ecef系下的位置。
[0060]
通过上述三式可以得到消防机器人以基准点作为原点在惯性坐标系下的位置。同时，为了获得机器人在惯性坐标系下的偏航角θ∈[-π,π)，需要将指北的航向角ψ∈[0,2π)转换到绝对偏航角，转换关系为：
[0061][0062]
具体的，在步骤s101中虚拟领航-领航-跟随运动结构的建立包括：
[0063]
1)建立多机器人编队几何结构；
[0064]
如图3所示；其中，虚拟领航机器人r0在编队参考轨迹点上随队行进，虚拟领航机器人r0的运动轨迹由一系列连续可导的轨迹点组成；指定其中一台机器人为领航机器人r1，实时跟踪虚拟领航机器人的位置，即满足领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零，即另外，其他机器人为跟随机器人ri(i≥2)，将各跟随机器人与领航机器人r1之间保持期望的距离和方位角转换到与虚拟领航机器人r0之间保持相应的期望值；这种编队结构设计的好处在于领航机器人r1既负责整个系统规划和协调，但又不影响到其他机器人的运动状态。
[0065]
其中，机器人i与虚拟领航者之间的距离为li≥0，方位角为其计算公式为：
[0066][0067][0068]
其中，
[0069][0070][0071]
x0,y0为虚拟领航机器人在惯性坐标系下的位置坐标，xi,yi为机器人i在惯性坐标系下的位置坐标；
[0072]
根据机器人i与虚拟领航机器人的相对位置，利用光滑的反正切函数atan2(y,x)得到范围为[-π,π)的唯一值，再利用g(atan2(y,x))函数将atan2(y,x)范围转换到[0,2π)。
[0073]
将每个机器人建模成一阶积分器模型：
[0074][0075]
其中，机器人i在惯性坐标系下的位置为机器人i的控制输入，其中u
ix
和u
iy
为它在惯性坐标系下xn方向和yn方向的控制输入分量。
[0076]
将虚拟领航机器人也建模成一阶积分器模型：
[0077][0078]
其中，为虚拟领航机器人在惯性坐标系下的位置和速度。
[0079]
定义机器人i与虚拟领航者之间的相对位移为：
[0080][0081]
定义机器人i与虚拟领航者之间的相对速度为：
[0082][0083]
定义机器人i与虚拟领航者之间的距离为方位角为定义机器人i与虚拟领航者之间的期望距离为期望方位角为将机器人i与虚拟领航者之间期望距离和方向角写成向量形式为：
[0084][0085][0086]
如果满足则设计的编队结构为
[0087]
2)建立多机器人编队队形结构知识库
[0088]
建立对多消防机器人系统的队形结构是研究多机器人编队变换的基础，通过对一些典型队形结构建立空间位置上的数学关系，可以大大简化多机器人编队变换过程中的数学计算。在构建队形形状主要考虑有一字型(line)、锲形(wedge)、柱状型(columnar)、三角形(triangle)、菱形(diamond)、圆形(circular)等，如图4所示，图中o为编队目标点，例如火源。
[0089]
首先，假设有n台消防机器人和规划的一条参考轨迹(虚拟领航机器人)，我们指定队形中某一机器人为队形的领航机器人，其余机器人为跟随机器人。采用图论中的有向无环图来描述一个可伸缩的队形，每个机器人看作成一个顶点，两个机器人之间的关系看作是边。每个机器人都有唯一的id号，这里将虚拟领航机器人设为r0，领航者设为r1，其余跟随机器人依次设为r2，r3，
…
，rn。为了表示机器人之间的相互关系及队形的形状参数，定义队形参数矩阵的通式为：
[0090][0091]fid
＝[f
i1
,f
i2
,f
i3
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0092]
其中，fd为某个队形形状的参数信息矩阵，为机器人的id号。每个机器人用3组参数表示，f
i1
为机器人i的编号，f
i2
为机器人i与虚拟领航机器人之间需要保持的期望距离f
i3
为机器人i与虚拟领航机器人之间的期望方位角则消防机器人编队的队形形状可以描述为：
[0093][0094]
针对上述几种典型的编队队形形状，通过不同的队形参数矩阵建立队形知识库，其期望队形参数矩阵表示如下：
[0095]
(1)一字队形：(2)柱状队形：(2)三角队形：(4)锲形队形：(5)菱形队形：(6)圆形队形：
[0096]
上述建立的期望队形参数矩阵并不能表示这类队形所有的结构，只是这类形状的一个特例，比如一字队形，通过调整机器人的顺序或者各机器人与虚拟领航机器人之间的距离可以得到新的一字队形，但是所有的队形都可以通过调整和得到。
[0097]
3)选择编队队形，从多机器人编队队形结构知识库中获取队形参数，进行编队控制实现多机器人系统以指定队形运动；
[0098]
若要实现多消防机器人系统以指定队形运动，并且能根据感知到的环境变化进行有效的队形变换，根据具有反馈控制的编队控制器，使得领航机器人能够跟踪提前规划好
的一系列轨迹点，各机器人与虚拟领航机器人之间保持期望的距离和方位角，即满足的一系列轨迹点，各机器人与虚拟领航机器人之间保持期望的距离和方位角，即满足
[0099]
在本实施例的一个具体方案中，多机器人编队系统协同作业是针对室外复杂多变环境，例如石化厂，气田，石油灌溉区，机场地铁，仓库，林道和其他高风险，高温，有毒，强辐射，易燃，易爆的地方以及容易倒塌的消防车和人员无法接近时，需要派遣消防灭火机器人执行其职责，并进入危险区域进行消防和救援，调查和救援，对化学污染场所进行冷却和消毒。机器人编队行走的区域多为窄过道、回廊或者有障碍物的通道。以三台机器人为例，分析多机器人编队行走过程中遇到的通道示意图，如图5所示。
[0100]
在本实施例中的编队中的领航机器人和跟随机器人均携带的多线激光雷达实时感知环境信息；其中，
[0101]
领航机器人实时跟踪虚拟领航机器人的位置，并且通过自身携带的多线激光雷达实时感知环境信息，提取结构化道路的边界以及检测和识别障碍物。
[0102]
跟随机器人与领航机器人组成特定队形执行编队任务，同时也利用自身携带的多线激光雷达检测和识别障碍物。
[0103]
为实现根据环境约束来避开障碍物同时能动态调节队形宽度以适应不同道路宽度，使得多消防机器人系统队形变换适应当前环境的综合性能达到最优。
[0104]
在步骤s102中，领航机器人实时跟踪虚拟领航机器人的位置，根据检测和提取的包括道路边界及障碍物在内的环境信息，进行队形变换动态优化，得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境；
[0105]
具体包括：
[0106]
1)机器人编队以期望的初始队列构型，跟随虚拟领航机器人行进；
[0107]
具体的，机器人群组通过编队控制器快速形成期望的初始构型，并一起跟随参考轨迹行进，执行编队跟随任务；
[0108]
2)领航机器人利用自身携带的环境感知传感器检测和提取出道路边界信息以及障碍物信息，并计算出队形的伸缩因子；
[0109]
具体的，所述领航机器人实时跟踪虚拟领航机器人的位置，并且通过自身携带的多线激光雷达实时感知环境信息，提取道路的边界并计算两侧道路宽度、检测和识别障碍物信息；并将检测到的部分障碍物边界点作为候选边界点，作进一步判别，修补缺失的道路边界点，以得到边界点信息。
[0110]
并且通过对道路边界和障碍物的轮廓进行膨胀处理，设置膨胀层，使编队中机器人与道路边界和障碍物之间的距离大于膨胀层宽度。优选的，膨胀层的宽度为b/2；b为机器人的宽度。
[0111]
在实际道路场景中，由于车辆等其他障碍物停靠在道路边界位置，从而对激光雷达发射的部分射线产生遮挡而无法获得完整的道路边界点，因此可将检测到的该部分障碍物边界点作为候选边界点，并作进一步判别，“修补”缺失的道路边界点，如图5所示。由图5可得，领航机器人利用激光雷达检测到的道路边界点位置为：
[0112]
[0113]
其中，和分别为第束有效激光检测道路边界得到的距离和角度。
[0114]
考虑到激光雷达可能会受安装位置的限制，以及编队行进过程中只需要对前方道路宽度和边界点提取，而不需要再对后方进行数据处理。因此，只需取激光雷达特定角度范围内α∈[α
min
,α
max
]的有效数据，这样既可以排除一些异常数据和减小计算量，又可以余留充足的时间进行队形调节。
[0115]
定义最近边界点的距离为ρ所对应的激光雷达扫描的角度为α，则有β＝α-π/2 θi，进而得到到领航机器人前进速度v1的垂直距离为：
[0116]
l＝ρ|sinβ|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0117]
定义d
min
＝2l为机器人的最小通行宽度。
[0118]
领航机器人根据测得的最小通行宽度d
min
和初始队列构型的期望队形宽度d0之间的比值，作为队形的伸缩因子ξ；
[0119][0120]
其中，式中，和分别为初始时刻t0时跟随机器人1与虚拟领航机器人之间的期望距离和期望方位角。
[0121]
3)根据队形的伸缩因子确定出队形变换指令分发到跟随机器人，使跟随机器人得到与虚拟领航机器人之间的期望距离和期望方位角。
[0122]
根据队形的伸缩因子ξ确定出队形变换指令σ包括：
[0123][0124]
其中，第一阈值第二阈值σ∈{0,1,2,3}为队形变换模式指令，
[0125]
领航机器人根据计算得到的ξ发布以下四种指令来动态调节队形以适应不同通道宽度的变化：
[0126]
(1)当伸缩因子大于1时，输出队形零变换指令σ＝0，表明前方通道足够宽，使编队保持原队形继续前进；此时，队形宽度设置为d＝d0。
[0127]
(2)当伸缩因子处于第一阈值和1的取值区间内时；输出队形同构变换指令σ＝1，表明机器人编队如果按照原队形继续前进则可能和道路边界发生碰撞，使编队进行队形收缩后通过前方的通道；此时，队形宽度设置为d＝d0ξ-b。
[0128]
(3)当伸缩因子处于第二阈值和第一阈值的取值区间内时；输出队形异构变换指令σ＝2，使编队变换成新的队形通过前方通道；
[0129]
表明前方道路宽度只允许单个机器人通过，必须变换程柱状队形才能够通过。此
时，队形宽度设置为d＝b。
[0130]
(4)当伸缩因子不大于第二阈值时，输出队形异常状态指令σ＝3，编队无法通过前方通道，等待下一步指令。即等待总控平台发布是否结束本次编队任务或者执行新的编队路径指令。
[0131]
可由此得出，领航机器人与虚拟领航机器人间的期望距离、期望方位角如下式：
[0132][0133][0134]
其中，表示异常值，领航机器人退出当前编队任务，等待下一步指令。
[0135]
各跟随机器人和虚拟领航机器人间的期望距离、期望方位角如下式：
[0136][0137][0138]
其中，
[0139][0140][0141][0142]
其中，i∈{2,3}；表示异常值，跟随机器人退出当前编队任务，等待下一步指令。g(
·
)函数表示将定义域[-π,π)转换为[0,2π)。
[0143]
具体的队形变换动态优化流程如图6所示。
[0144]
为进一步评价时变编队变换方法的性能，本实施例中建立了新的评价指标函数，包括能量损耗率、队形调节时间比、队形失真度在内的队形变换性能评价函数。具体的，评价指标函数设计如下：
[0145]
(1)能量损耗函数
[0146]
目前，机器人大多采用机载电池供电，电能的消耗将对机器人执行任务时长起到决定性作用。而在编队变换过程中较小的能耗可以为机器人实现更长时间的连续工作提供一定的保障，这对实际多机器人编队任务的执行有着重要的现实意义。机器人i的能量函数
可以建模为：
[0147][0148]
其中，ei(t)为机器人i在t时刻的能量；zi：是一个非线性映射。
[0149]
由于能量损失本质上可以由行驶路径长度体现，假设机器人静止时候能耗为零，机器人初始时刻的能量为δi是每行驶单位长度的标准能量消耗率。能量函数可以描述为：
[0150][0151]
式中，vi为机器人i的速度。
[0152]
则，机器人i在时间[t0,t]内总的能耗为：
[0153][0154]
则，编队在时间[t0,t]内队形变换总的能量损耗率函数设置为：
[0155][0156]
式中，δer(t)和δe
t
(t)分别为编队在时间[t0,t]内的实际能耗和理论能耗。
[0157]
由此可知，fe(t)值越小表明能耗越少，机器人连续工作的时间越长。
[0158]
(2)队形调节时间函数
[0159]
当编队接受到队形变换指令时从当前队形变换到目标队形需要一定的调节时间，为了使调节时间尽可能短，因此，队形调节时间效率也是影响编队整体执行任务效率的影响因素之一。将队形调节时间比作为评价队形变换的一个重要指标，一方面可以用来描述队形结构的分散程度；另一方面，反映编队变换的响应速度，当编队通过不同宽度的通道时需要多次变换队形，而队形调节总时间与行驶总时间的比值从侧面反映出编队从当前队形变换到目标队形的收敛速度。则，编队在时间[t0,t]内队形调节时间比函数设置为：
[0160][0161]
式中，为变换次数；为机器人i在时间[t0,t]内总的变换次数；t
j,s
和t
j,e
分别为机器人在第j变换时从当前队形变换到目标队形的起始时刻和结束时刻。
[0162]
由此可知，fr(t)值越小表现编队变换所花费的时间越小，变换效率越高。
[0163]
(3)队形结构失真函数
[0164]
多机器人系统在行进过程中由于通道宽度的变化，这时需要变换合适的队形以适应环境变换，为最大的优化队形，其队形变形量应尽可能小。将队形失真度作为评价队形变换性能的函数之一，来描述编队当前队形与期望队形的畸变程度。队形结构失真函数为：
[0165][0166]
由此可知，fs(t)值越小表明队形失真度越小，与期望队形越接近。
[0167]
由于不同的多机器人系统对各性能函数的要求不一样，比如，对于多消防机器人系统，能量损耗和队形调节时间的权重相比队形失真度会更高，所以我们对各性能函数设置了权重因子，以区分各性能指标的重视程度。则，多机器人系统的队形变换的性能综合评价指标函数为：
[0168]
f(t)＝λefe(t) λafa(t) λ
sfs
(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0169]
其中，λe＞0、λr＞0、λs＞0分别为能量损耗函数权重因子、队形调节时间函数权重因子、队形稳定性函数权重因子，并且满足λe λr λs＝1。
[0170]
环境适应度函数f(t)可以作为衡量编队变换策略优劣的综合效能评价指标。其值越小表明编队变换策略越优。
[0171]
具体的，步骤s103中通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制方法包括：
[0172]
1)根据虚拟领航机器人和编队机器人的一阶积分器模型，确定出编队机器人与虚拟领航机器人之间的相对位移和相对速度；
[0173]
根据虚拟领航机器人和编队机器人的一阶积分器模型，确定出机器人i与虚拟领航者之间的相对位移为：机器人i与虚拟领航者之间的相对速度为：
[0174]
2)建立圆形运动控制律，引导每个编队机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心、以与虚拟领航机器人之间期望距离为半径的圆上；
[0175]
具体的，所述圆形运动控制律，引导每个机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心所形成的圆上，而不考虑圆上具体位置。该控制是通过旋转一个固定的参考矢量来实现以目标点p0为中心逆时针旋转，可以得到一个旋转变化的圆形轨迹。然后，将这个旋转变化轨迹作为参考轨迹，就能得到圆形参考轨迹，机器人跟踪这条轨迹做圆周运动。控制目标可描述如下：
[0176][0177]
其中，是旋转角为φi(t)∈[0,2π)的旋转矩阵，为初始时刻t0由目标指向机器人i的单位向量，定义：
[0178][0179][0180][0181]
则，圆形运动控制器设计为：
[0182][0183]
式中，k
p
＞0为位置调节参数，用于调整机器人圆形轨迹跟踪的收敛速度。
[0184]
在本实施例中对控制律(31)的控制目标的实现进行了证明；
[0185]
定理1：设φi(t)为一阶连续可导函数，在控制律(31)作用下，机器人i最终能跟踪上半径为距离虚拟领航机器人的期望相对距离方向为的圆形轨迹，并围绕目标环绕运动，即t
→
∞时，并且有
[0186]
证明：定义位置跟踪误差为：
[0187][0188]
如果式(32收敛到零，则误差的差分方程可以写为：
[0189][0190]
则可以继续展开为要使得误差收敛到零，误差动态方程可写成
[0191][0192]
其中，
[0193][0194]
由误差的一阶微分方程可得ei(t)＝exp(-k
p
t)ei(t0)，则当t
→
∞，误差ei(t)收敛到0，有控制目标即特别地，当时，机器人可匀速环绕目标运动。
[0195]
3)在圆形运动控制律的基础上耦合一个方位角定位的控制律；根据耦合了方位角定位的圆形运动控制律和避障控制律，对每个编队机器人进行具有避障功能的编队控制。
[0196]
为了确保机器人能够运动到圆上指定位置，可以通过改变转向角来实现期望队形的角间距自定义分配。根据式(28)，我们可以得到则，控制律(31)可以改写为：
[0197][0198]
其中，
[0199][0200]
考虑到实际应用，有时有必要确定机器人在自身的圆上的具体位置。例如，当一个多机器人系统监控感兴趣的区域，需要预先规划每个机器人的准确位置监视特定区域。因此，有必要设计一种方位角间距分配控制律，使机器人能够到达圆上指定的位置。我们在控制器(36)中引入一个非线性函数：
[0201][0202]
式中，是一个方位角调节常数，用于调整机器人方位角角间距的收敛速度。
[0203]
定理2：在圆形运动控制律(36)下，所有机器人的方位角定位控制律均满足(38)。
圆上机器人的方位角角都可以收敛到所需期望的方位角值，即
[0204]
证明：选择一个正定的李亚普洛夫函数：
[0205][0206]
式(39)两端对时间求导，得到差分方程为：
[0207][0208]
对于式(40)，保证是负定的。设有相对平衡点这表明每个机器人的方位角收敛是由控制律(38)保证的。
[0209]
综上所述，控制律和证明了圆形运动控制器是稳定的，可以将机器人i与虚拟领航机器人之间的距离和方位角收敛到期望相对距离和期望方位角
[0210]
考虑到编队成员运动过程或队形变换中将不可避免地与环境中障碍物和其他成员发生碰撞，有必要设计一个有效的避障控制律，不仅使机器人能够有效避开障碍物，同时也保证队形的稳定性。
[0211]
具体的，所述避障控制律通过设置斥力势场产生排斥力进行避障。
[0212]
人工势场法作为一种经典的避障控制方法，广泛应用于机器人的运动控制中。在实施例中没有必要设置额外的引力场函数产生吸引力，因为每个机器人有自己的轨迹需要跟踪，这种由目标位置牵引的作用表现可以看作是引力势场产生“引力速度”，所以仅通过设置斥力场势场产生排斥力来避开障碍物，传统的斥力场函数定义为：
[0213][0214]
其中ko是一个排斥增益系数。ρ0＞0是避障响应距离。是避障区域ρ0内的障碍物数量。定义其中其中是障碍物k在导航坐标系下的位置。
[0215]
然后，由于机器人接收到的斥力沿斥力场函数的负梯度，因此避障控制律表示如下：
[0216][0217]
由于传统的人工势场法存在局部极小值和目标不可达的问题。如图7所示
[0218]
本实施例中通过改进的人工势场法设置斥力势场进行多机器人编队的避障控制，输出避障控制律；
[0219]
在设置斥力势场中，通过建立道路边界作用的斥力速度，以限制机器人的行驶区域；将障碍物作用的斥力速度和道路边界作用的斥力速度进行合成得到斥力合速度；通过调整斥力合速度的方向输出最终的避障控制律，以解决局部极小值和目标不可达问题，使编队机器人在道路的安全区域内进行避障。
[0220]
其实现效果如图8所示。
[0221]
这里假设建立的惯性坐标系位于道路中间位置，即道路两端关于xn对称。
[0222]
设置道路边界作用的“斥力速度”为：
[0223][0224][0225]
其中，ke＞0是道路边界作用的斥力增益系数，w为道路宽度，b为机器人宽度(假设两驱动轮轴心距等于机器人车体宽度)。定义两驱动轮轴心距等于机器人车体宽度)。定义是机器人i的偏置点pi距离道路边界上最近点的位置。ξi为对数障碍函数，其函数图像如图9所示。可以看出，当机器人靠近道路势场区域时，受到来自道路边界的法向方向的“斥力速度”，并随着机器人离道路边界越近，其“斥力速度”也越大，最后趋于无穷大；当机器人处于安全区域内，其“斥力速度”为0。
[0226]
则，机器人i受到的“斥力合速度”为：
[0227][0228]
从图9可以看出“斥力速度”产生了2种作用，一是使机器人前往目标方向加减速，二是使得机器人产生转弯的切向力绕开障碍物，因此存在机器人在目标方向上合速度为0的情况，这时机器人容易陷入局部最优和目标不可达的问题。本实施例中通过调整“斥力合速度”的方向，使其变为与目标作用的“引力速度”方向垂直且远离障碍物的方向，使得斥力只改变运动方向，不改变前往目标的速度。
[0229]
则，设置新的“斥力合速度”为：
[0230][0231]
其中，表示的原“斥力合速度”与“引力速度”之间夹角表示的原“斥力合速度”与“引力速度”之间夹角
旋转矩阵或
[0232]
最终，建立动态权重因子，对耦合了方位角定位的圆形运动控制律和避障控制律进行比值调整，得到分布式协同耦合的编队控制器对机器人i输出的最终输出的控制律ui；
[0233][0234]
其中，是动态权重因子，用于调整机器人控制输入的内部比值。
[0235]
如图10所示，为分布式协同耦合的编队控制器的控制框图，图中的控制器为a、b、c三部分耦合而成，a部分主要用于执行圆形运动控制律，b部分主要用于执行方位角定位控制律，c部分主要用于执行避障控制律。
[0236]
综上所述，本发明的基于虚拟领航-领航-跟随的多机器人编队控制方法，解决多机器人的编队控制问题。
[0237]
本发明采用虚拟领航-领航-跟随模式，领航机器人既负责整个系统规划和协调，但又不影响到其他机器人的运动状态。解决了传统的领航者-跟随者模式的链式结构中跟随机器对领航机器的过度依赖，容易造成跟踪误差累积以及领航机器出现故障会导致整个系统出现崩溃等问题。
[0238]
本发明建立了队形的伸缩因子，在常规的队形变换状态基础上还考虑了异常状态情况，即当前方通道宽度不满足编队个体通过时的情况，需等待下一步指令；提出了衡量编队变换策略优劣的性能综合评价指标函数，该指标能够不依赖于系统的动力学模型，其表达形式简单明了，不仅能够适用于多消防机器人的时变编队变换而且能适用于其他多智能体系统。
[0239]
本发明从实际应用出发，考虑了道路边界和停靠在路边的障碍物会与机器人发生碰撞，通过对道路边界和障碍物的轮廓添加了膨胀处理，使得机器人尽可能与道路边界和障碍物之间保持一个安全距离。
[0240]
本发明建立了分布式协同耦合的编队控制器，在圆形运动控制律的基础上建立增加方位角定位的控制律，与避障控制律结合，能同时实现按编队队形行进的过程中有效地避开障碍物。
[0241]
本发明设置斥力势场产生排斥力进行避障，能够不依赖于系统的精确数学模型，以非常简单的算法实现不确定性高的非线性耦合动态系统的避障控制，以及实现高效、安全、可靠的编队避障。并且把道路边界作用效果也考虑进去，使得机器人能够在道路的安全区域内有效避障，同时有效地解决传统人工势场法存在的局部最优和目标不可达的问题。
[0242]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。