一种面向复杂环境的多异构机器人视觉伺服队形变换方法

1.本发明属于移动机器技术领域，更为具体地讲，涉及一种面向复杂环境的多异构机器人视觉伺服队形变换技术。


背景技术：

2.基于视觉传感器的定位技术，主要通过摄像头获取视野内目标物体的深度信息，从而实现目标位置的测量。以apriltag标签为辅助标记的目标检测方法具备较高精度，且实现简单。测量三维位姿的同时，能通过解码获取标签信息，实现身份识别。相较于传统而二维码，具有更高的定位精度，鲁棒性更高，运行速率较快。
3.移动多机器人协同系统目前主要应用同构机器人搭建物理平台。同构机器人虽然能携带不同类型传感，但是在运动能力上不能相互补充，不能克服同类机器人的弊端和缺陷。异构机器人系统可以充分发挥各自的优势，扬长避短，弥补不足，提高系统性能同时增强鲁棒性。针对模型相异的空-地多机器人实现协同控制，存在一定的挑战和难度。
4.在复杂地面环境下，多移动机器人为保证运行安全或高效完成任务目标，需要进行多机器人编队队形调整。根据机器人工作场景和目标要求构建基础队形库和设计编队变换评价函数，用于实时编队变换策略。结合实际环境约束，进行适当的队形变换操作，保证机器人能够以最大限度完成预期任务。


技术实现要素：

5.本发明的目的在于克服现有技术的不足，提供一种面向复杂环境的多异构机器人视觉伺服队形变换方法，具体实现为基于视觉信息的多异构机器人控制系统——对四旋翼无人机和地面移动机器人设计跟踪和编队控制器，确保四旋翼持续地获取地面机器人相对位姿，并引导地面编队的生成。其次根据实时环境约束，设计一种编队变换优化策略，提高机器人在非结构化环境中的运动能力。
6.为实现上述发明目的，本发明一种面向复杂环境的多异构机器人视觉伺服队形变换方法，该方法包括：
7.(1)、构建基本队形库，完成各基本编队队形基本信息配置；基本队形库构建规则如下：
8.l＝{γ1,γ2,
…
,γi,
…
,γn}
9.其中，l表示基本队形库；γi表示第i个基本队形信息，具体内容如下：
10.γi＝{d
ix
,d
iy
,d
safe
,d
width
,d
length
,ni}
11.其中，d
ix
表示第i个基本队形的x轴期望距离矩阵，d
iy
表示第i个基本队形的y轴期望距离矩阵；d
safe
表示机器人间的安全距离，d
wid
表示第i个基本队形的宽度，d
len
表示第i个基本队形的长度，ni表示第i个基本队形包含的机器人数量；
12.(2)、指定初始编队队形，给定任务目标；
13.(2.1)、设计编队控制律f1和跟踪控制律f2，完成无人机的视觉伺服跟踪；通过搭载
在无人机的视觉相机获取地面编队的中心位姿信息，传回上位机，基于伺服跟踪控制算法计算无人机期望速度信号；再传回四旋翼板载控制器，将期望速度信号转换为飞控接收信号，控制无人机运动到编队中心的上空；
14.(2.2)、通过基于辅助标签的视觉算法解算地面机器人的相对位姿，传回上位机并计算各地面移动机器人的期望运动速度，通过蓝牙传送到各地面移动机器人终端，实现期望编队形状；
15.(3)、异构多机器人系统感知周围环境，检测机器人集群路径内环境约束；
16.(4)、计算编队变换系数，如下式：
[0017][0018]
其中，γ表示编队的变换系数；d
form
表示当前多机器人编队的宽度；dc表示复杂地形可通过的最大安全宽度；根据变换系数大小选择对应操作：
[0019][0020]
根据变换系数判断是否不需变换即可通过当前狭窄地形，如果γ小于等于1，则表明地形安全宽度大于当前编队宽度，不需要任何队形变换操作即可通过；如果γ大于1，则表明地形安全宽度小于当前编队宽度，即继续保持原队形无法通过当前环境，转到步骤(5)；
[0021]
(5)、编队变换优化策略；
[0022]
(5.1)、首先判断当前编队的基本队形能否穿越地形，如果基本队形满足安全约束，转到步骤(5.2)；若不能满足安全约束，则只能进行编队切换；编队切换评价函数f如下：
[0023]
f＝k
tft
+kff
df
+k
sfs
[0024]
其中，k
t
、kf、e表示的权重系数；f表示编队队形变换策略评价函数；f
t
表示编队变换所需要的时间；f
df
表示当前队形与期望队形的差异；fs表示安全系数，用以衡量期望队形的安全程度；f
t
的计算方式如下：
[0025][0026]
其中，ta表示编队变换的开始时刻；tb表示编队变换的结束时刻；实际中可通过测量的方式获取不同队形间切换所需时间；f
df
的计算方式如下：
[0027][0028]
其中，d
dx
表示期望编队队形的x轴信息；d
dy
表示期望编队队形的y轴信息；d
cx
表示当前编队队形的x轴信息；d
cy
表示当前编队队形的y轴信息；fs的计算方式如下：
[0029][0030]
其中，α表示编队宽度的权重系数；β表示编队长度的权重系数；f
x
表示编队宽度；fy表示编队长度；在所有其他编队中选取评价函数最小的作为被切换编队队形，转到步骤(5.3)；
[0031]
(5.2)、若当前编队的最小队形可安全通过当前区域，那么对编队收缩操作，根据经验选取最佳收缩变换系数γ
*
，当步骤(4)中的编队变换系数满足γ≥γ
*
时，直接进行切换操作，计算步骤如(5.1)所示，得出最佳切换编队队形，并转到步骤(5.3)；
[0032]
当γ满足1＜γ＜γ
*
时，考虑编队队形的收缩操作；选取1/γ为队形收缩系数，确定收缩后的编队形状；为弥补人为设置最佳收缩变换系数带来的不足，也即确保真正实现更优的编队变换操作，计算收缩评价函数，并在可收缩情况下计算选取编队切换操作的评价函数；通过比较收缩和切换评价函数的值，最终确定出优化过的期望编队队形，转到步骤(5.3)；
[0033]
(5.3)、将期望编队队形设置为步骤(5.1)或(5.2)中通过编队变换策略选取的优化队形，然后根据给定的控制方法，确保多机器人系统完成编队变换操作；
[0034]
(6)、保持切换后编队，通过当前狭窄区域；然后转到步骤(2)，检测机器人周围环境约束，再重复步骤(3)、(4)、(5)。
[0035]
本发明面向复杂环境的多异构机器人视觉伺服队形变换方法，首先借助视觉传感器获取地面机器人位姿，对无人机和地面机器人分别设计伺服控制算法和编队算法，完成无人机跟踪和地面多移动机器人编队；其次通过视觉传感器对异构多机器人系统运行环境进行建模；针对复杂环境中的避障，实现狭窄区域下的编队队形变换策略；构建基础编队队形库，基于编队变换时间、队形差异以及安全系数设计编队变换评价函数；针对实际情形，从无变换操作、编队收缩和编队切换中选取最符合当前环境约束与预期任务要求的编队队形，并应用伺服和编队算法实现相关操作，完成预期任务并安全通过障碍地形。
[0036]
(1)、本发明使用的视觉定位系统无需基站，只需将定位标签绑定机器人即可；在传统的无线电定位系统中，需在场地内提前布置基站；不需额外设备即可实现未知环境下多机器人系统的搜索；且在室内环境下，视觉定位系统精度更高；此外，相较于无线电定位系统，视觉传感器可获取未知环境信息；针对采集到的图像信息，用于定位系统的同时，也可用于目标识别和机器人建图，增加系统的智能；
[0037]
(2)、采用定位与控制算法分离的系统框架；整个视觉定位模块位于四旋翼板载计算机内，采集到图像直接在树莓派上解算出相对位姿信息，通过ros节点和局域网，将其传送到上位机，有效分担上位机的计算负荷；同时降低移动终端和上位机间的数据交换量，降低通信要求和带宽；此外，地面机器人仅接收来自上位机的速度控制指令，不增加任何处理模块即可组件基于视觉信息的编队系统，降低成本；
[0038]
(3)、不完全的分布式算法；本发明中，视觉定位系统置于四旋翼板载树莓派，而跟踪控制器和编队控制器则位于上位机；通信分别采用wi-fi和蓝牙串口；相较于传统集中式控制方案，视觉定位系统下放缓解上位机的计算压力；定位与控制相互独立，提高系统运行效率，节省响应时间；相较于传统分布式控制方案，个机器人之间没有大量的数据传输，降低通信带宽需求；其次高性能上位机弥补地面移动机器人运算不足的缺点，各移动机器人仅需接收控制指令，控制自身运动即可完成期望的编队行为；
[0039]
(4)、借助无人机的视觉系统实现伺服跟踪以及地面机器人编队控制，并结合编队变换优化策略实现环境障碍下的安全避撞；构建基础队形库，设计编队变换评价函数，根据狭窄地形选择编队收缩或切换操作，提高异构多机器人系统在复杂环境的适应能力。
附图说明
[0040]
图1是本发明面向复杂环境的多异构机器人视觉伺服队形变换方法流程图；
[0041]
图2是机器人编队与环境约束示意图；
[0042]
图3是编队变换策略的切换和收缩动作示意图；
[0043]
图4是编队变换策略基础队形库示意图；
[0044]
图5是地面机器人集群编队收缩轨迹示意图；
[0045]
图6是地面机器人集群编队切换轨迹示意图。
具体实施方式
[0046]
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
[0047]
实施例
[0048]
图1是本发明面向复杂环境的多异构机器人视觉伺服队形变换方法。
[0049]
在本实施例中，如图1所示，本发明是一种面向复杂环境的多异构机器人视觉伺服队形变换方法，包括以下步骤：
[0050]
s1、算法初始化；
[0051]
准备阶段，构建基础编队队形库，确定机器人安全间隔、确定最佳变换系数γ
*
。在本实施例中，如图4所示，构建包含横直线、竖直线、三角形、矩形、圆形的基础队形库。那么x轴y轴对应的编队矩阵如下：
[0052][0053]
其中，分别和表示x和y轴上两机器人间相对距离关系。
[0054]
s2、执行位姿检测和控制算法；
[0055]
通过无人机，引导地面机器人形成期望编队队形，确保无人机位于地面编队中心上方。在本实施例中，通过视觉+辅助标签定位方案获取地面移动机器人相对位姿，传回上位机后根据设计的编队控制律f1和无人机伺服跟踪控制律f2计算全体机器人的移动速度。
[0056]
s3、更新机器人位置，计算变换系数；
[0057]
上位机通过wifi将无人机期望速度下传至板载控制器，通过mavros节点和mavlink通信协议，将其转换为飞控接收信号，控制无人机运动到编队中心上空。同时，上位机通过蓝牙串口将地面机器人期望速度传至各机器人控制器，形成期望编队形状。
[0058]
基于异构多机器人系统中视觉传感器对运行环境进行建模，获取狭窄地形环境约束，变换系数计算规则如下：
[0059][0060]
其中，γ表示编队的变换系数；d
form
表示当前多机器人编队的宽度；dc表示复杂地
形可通过的最大安全宽度。图2是机器人编队与环境约束示意图，各变量物理意义已在图2中标注。
[0061]
s4、是否进行编队变换；
[0062]
根据变换系数γ的大小选择对应操作：
[0063][0064]
根据变换系数判断是否不需变换即可通过当前狭窄地形，两种编队变换动作示意图见图3。收缩操作只需要对当前编队形状缩小，而切换则需要变换当前编队形状，两者都是为满足集群机器人运行环境的约束变化。
[0065]
如果γ小于等于1，则表明地形安全宽度大于当前编队宽度，不需要任何队形变换操作即可通过，准到s5；如果γ大于1，则表明地形安全宽度小于当前编队宽度，即继续保持原队形无法通过当前环境，需要执行编队变换策略，转到s6；
[0066]
s5、不进行队形变换；
[0067]
当前地形安全宽度大于当前编队宽度时，不需要进行编队变换即可通过，保持当前编队构型继续运动即可，转到s2。
[0068]
s6、执行编队变换策略；
[0069]
当前地形安全宽度小于当前编队宽度，需要执行编队变换策略。一般情况下，收缩比切换所需时间更少，在策略上优先考虑收缩操作。首先检验当前编队在队形库中的基本(最小)队形能否通过狭窄地形，若基本队形能够通过，则存在直接收缩变换的可能，转到s7；若基本队形不能通过，直接进行编队切换，转到s8。
[0070]
s7、衡量收缩标准；
[0071]
根据经验设置最佳收缩阈值γ
*
，当满足1＜γ＜γ
*
时，考虑编队队形的收缩操作；选取1/γ为队形收缩系数，确定收缩后的编队形状，转到s9；否则，直接进行切换操作，转到s8。
[0072]
s8、执行编队切换；
[0073]
编队切换评价函数f如下：
[0074]
f＝k
tft
+kff
df
+k
sfs
[0075]
其中，k
t
、kf、e表示的权重系数；f表示编队队形变换策略评价函数；f
t
表示编队变换所需要的时间；f
df
表示当前队形与期望队形的差异；fs表示安全系数，用以衡量期望队形的安全程度；f
t
的计算方式如下：
[0076][0077]
其中，ta表示编队变换的开始时刻；tb表示编队变换的结束时刻。实际中可通过测量的方式获取不同队形间切换所需时间；f
df
的计算方式如下：
[0078][0079]
其中，d
dx
表示期望编队队形的x轴信息；d
dy
表示期望编队队形的y轴信息；d
cx
表示当前编队队形的x轴信息；d
cy
表示当前编队队形的y轴信息；fs的计算方式如下：
[0080][0081]
其中，α表示编队宽度的权重系数；β表示编队长度的权重系数；f
x
表示编队宽度；fy表示编队长度。在所有其他编队中选取评价函数最小的作为被切换编队队形，确定期望编队信息，转到s2；
[0082]
s9、执行编队收缩优化；
[0083]
s9.1、计算收缩评价函数；
[0084]
为尽量弥补经验的不准确，针对收缩操作进行优化。当执行编队收缩策略时，按照编队切换评价函数规则计算收缩评价函数f1，计算规则如下：
[0085]
f1＝k
tfst
+kff
df
+k
sfs
[0086]
其中，f
st
表示编队收缩所需要的时间，其它参数同编队切换评价函数计算规则。
[0087]
s9.2、计算切换评价函数；
[0088]
执行切换策略，计算切换评价函数f2，计算规则见s8。
[0089]
s9.3、选择最佳队形；
[0090]
根据f1和f2的值，确定最佳期望变换队形。若f1≤f2，说明在当前的评价标准下，选择收缩操作更优，根据收缩系数计算期望收缩编队信息，转到s2；若f1＞f2，表明此时选择切换操作更优，根据决策结果，确定变换结果和队形信息，转到s2。
[0091]
实例
[0092]
为方便理解，以四个地面机器人和四旋翼组成的异构多机器人系统为例。那么四地面移动机器人id记为r0、r1、r2、r3，设定其初始位置分别为p0＝(0,0)、p1＝(1,1)、p2＝(1,0)、p3＝(0,1)。构建包含横直线、竖直线、三角形、矩形、圆形的基础队形库，见图4。图4是本实例中基础队形库示意图，箭头表明机器人数量的增加方向。表1是五种基础队形的参数表。用户可跟据实际情况设计基础队形库及相关参数信息。
[0093][0094]
表1
[0095]
五种基本编队的期望位姿矩阵为：
[0096]d1x
＝[0,-1,-2,-3；1,0,-1,-2；2,1,0,-1；3,2,1,0]，d
1y
＝[0]4×4；
[0097]d2x
＝[0]4×4，d
2y
＝[0,1,2,3；-1,0,1,2；-2,-1,0,1；-3,-2,-1,0]；
[0098]d3x
＝[0,0,1,0；0,0,1,0；-1,-1,0,-1；0,0,1,0]，d
3y
＝[0,2,2,1；-2,0,0,-1；-2,0,0,-1；-1,1,1,0]；
[0099]d4x
＝[0,0,-1,-1；0,0,-1,-1；1,1,0,0；1,1,0,0]，d
4y
＝[0,-1,-1,0；1,0,0,1；1,0,0,1；0,-1,-1,0]；
[0100]d5x
＝[0,0.7,0,-0.7；-0.7,0,-0.7,-1.4；0,0.7,0,-0.7；0.7,1.4,0.7,0]，
[0101]d5y
＝[0,0.7,1.4,0.7；-0.7,0,0.7,0；-1.4,-0.7,0,-0.7；-0.7,0,0.7,0]。
[0102]
四旋翼无人机视觉伺服跟踪算法如下：
[0103][0104]
其中，kd表示控制器速度项增益矩阵；k
p
表示位置项增益矩阵；e(t)表示误差；表示误差变化率。
[0105]
地面移动机器人编队控制算法为：
[0106][0107]
其中，k1和k2分别表示位置项和速度项的增益系数；a
ij
是邻接矩阵元素，表示通信情况；ξi(t)＝[xi(t),yi(t),yawi(t)]
t
表示第i个地面机器人的状态；d
ij
表示第i和第j个地面机器人之间期望位置关系；表示第i个地面机器人的期望速度向量；f
′
表示避撞项排斥力。用户可根据实际情况设计f1和f2。
[0108]
首先是编队收缩实例，机器人期望矩阵为横直线编队，见图5。编队变换策略中各参数分别设置为：k
t
＝0.7，kf＝0.3，ks＝1.0，α＝0.7，β＝0.3，γ
*
＝1.6，机器人集群运动速度为vd＝[0,-0.015]
t
m/s。
[0109]
进入狭窄区域之前，编队宽为6.0m，隧道宽为6.0m，不满足安全条件，无法直接通过，需要进行编队变换。确定收缩系数为0.9，收缩后编队宽度为5.4m。而后隧道宽度变为4.5m，经编队变换策略优化后，收缩系数为0.72，即编队收缩后宽度为3.9m。在整个变换过程中，机器人之间没有碰撞，与隧道两侧保持安全距离，且最终通过隧道区域。图5是地面机器人集群编队收缩轨迹示意图，
[0110]
在编队切换实例，机器人期望矩阵为菱形编队，见图6。编队变换策略中各参数分别设置为：k
t
＝0.3，kf＝0.3，ks＝0.4，α＝0.7，β＝0.3，γ
*
＝1.6，机器人集群运动速度为vd＝[0,-0.015]
t
m/s。
[0111]
在控制律f1和f2作用下，引导生成期望菱形编队。当前需要穿过隧道的宽度为4.0m，当前编队宽度为7.0m，编队变换策略选择切换到横直线编队队形才能继续安全执行任务。切换后期望横直线编队宽度为3.4m。当编队前进到y轴10m处，隧道宽度进一步缩小为3.0m。而横直线编队最小宽度为3.0m，还好预留两侧安全距离，因此需要再次进行切换操作。经计算，需切换到竖直线编队，切换后编队继续前进，直到任务结束。