基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法与流程

1.本发明涉及一种基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法。


背景技术：

2.基于机载视觉感知的多小型机器人系统能够仅依靠其自身视觉系统感知的局部信息进行队形构建，进而完成给定任务，该系统在通信恶劣环境(例如地下、室内、水下或者gps定位信号较弱的室外环境等)中有着广泛的应用前景。尽管针对多机器人系统的分布式控制方法的研究成果丰富，但现有的基于局部信息的队形收敛方法，主要在控制理论方面对考虑通信反馈延迟、有限时间约束等分布式控制算法做出了贡献，很少有研究者基于机载视觉系统感知的特性，提出适用于基于机载视觉感知系统的多机器人队形收敛方法；而已存在的基于机载视觉感知系统的多机器人队形收敛方法的收敛速度与抗干扰性不能兼顾。
3.由于基于机载单目视觉感知的多小型机器人系统因对通信环境要求比较低、成本低，因此得到了很多研究者的关注。由于机载单目视觉感知较小的范围、深度信息的缺失，机器人单元特征多变且差异性较大，使得机器人单元识别的准确度以及相对位姿的精度受到了各种因素的影响，降低了视觉反馈的准确性。此外，现有技术中很少有队形控制方法考虑机载视觉感知与伺服的特性，进而限制了队形收敛方法的准确性。因此，研究基于机载单目视觉感知的多小型机器人系统队形收敛方法变得至关重要。


技术实现要素：

4.本发明的目的在于提出一种基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，该方法通信频次小、计算量小，能够避障，而且通过局部位置信息的时间最优目标分配算法以及异步离散时变加权一致性队形构建算法的权值更新规则，能够提高队形收敛速度。
5.本发明为了实现上述目的，采用如下技术方案：
6.基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，包括如下步骤：
7.步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器从拍摄的单幅图像中完成目标识别，并基于建立的相对距离与相对角度模型从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度；
8.步骤2.基于局部位置信息的时间最优目标分配算法快速完成期望队形分布，以及机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划；
9.步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。
10.本发明具有如下优点：
11.1.本发明方法，仅依靠单目视觉系统和罗盘感知信息能够快速实现队形的收敛，能够降低对通信模块的要求，并降低了成本，提高了多机器人系统在地下、水下等通信受限区域的应用能力。
12.2.本发明方法支持机器人数量较多的多机器人系统，提高了应用范围。
13.3.本发明方法对多机器人系统的机器人单元性能要求较低，仅需要配备摄像机、罗盘、imu以及简单的通信模块即可。
14.4.本发明方法提出的针对机载单目视觉感知的基于可在线更新掩模特征库的二维互相关特征滤波器，能够在相对复杂背景下，识别特征多样化的目标物体。
15.5.本发明方法提出的基于局部位置信息的时间最优目标分配算法，能够优化期望队形的分布以及目标分配，并生成基于梯度下降最大且无交叉的初始轨迹。
16.6.本发明方法提出的考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，能够基于感知的邻居相对位姿信息，实现避障，更新轨迹，无需通信；而且给出的权值更新规则能够快速实现队形收敛。
附图说明
17.图1为本发明实施例中基于机载单目视觉感知的多小型机器人混合队形快速收敛方法的流程图；
18.图2为本发明实施例中目标识别实例示意图；
19.图3为本发明实施例中相对距离模型、相对角度模型学习过程示意图；
20.图4为本发明实施例中初始分布构成的连通有向图；
21.图5为本发明实施例中期望队形分布确定以及目标位置的优化分配示意图；
22.图6为本发明实施例中基于机载单目视觉感知的多小型机器人混合队形快速收敛方法的仿真结果示意图。
具体实施方式
23.本实施例提供了一种基于机载单目视觉感知的多小型机器人混合队形快速收敛方法，该方法包含机载单目视觉感知方法、基于局部位置信息的时间最优目标位置分配方法以及考虑避障与收敛速度的异步离散时变加权一致性队形快速构建方法。
24.其中，机载单目视觉感知方法解决了每个机器人单元的目标识别与空间定位问题。在识别目标时，通过建立掩模特征库，以及基于特征库的二维互相关特征滤波器，识别特征丰富且变化较大的目标，而且根据识别得到的目标特征，按照一定规则更新掩模特征库。为了获取目标三维信息，基于摄像机模型，构建了局部坐标系下的相对距离与相对角度模型，从单幅图像中确定感知到的邻近机器人及障碍物在其局部坐标系下的相对位姿。
25.基于局部位置信息的时间最优目标位置分配方法，解决了期望队形的分布与机器人单元在期望队形中的目标位置优化分配问题。在通过唯一一次通信，共享感知的机器人单元数量与相对位姿，确定多小型机器人系统初始分布后，基于局部位置信息的时间最优目标位置分配方法基于主分量分析法(pca)优化期望队形分布使其趋近于初始分布，基于二分图原理设计目标位置分配规则，计算期望位置关系(position relationship,pr)，规
划避碰的初始无交叉轨迹。
26.考虑避障与收敛速度的异步离散时变加权一致性队形快速构建方法，解决了障碍环境下轨迹更新问题。当某机器人单元沿初始轨迹向目标位置运动过程中遇到动/静态障碍，不能明确自身在公共坐标系下的位姿时，基于提出的考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，该机器人单元通过“感知、计算、运动”的循环，不断感知当前邻居与障碍信息，并异步独立更新轨迹直到收敛到期望队形。在轨迹更新过程中，每个机器人单元的运动控制由pid控制算法实现。
27.下面结合附图以及具体实施方式对本发明作进一步详细说明：
28.如图1所示，基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，包括如下步骤：
29.步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器从拍摄的单幅图像中完成目标识别，并基于建立的相对距离与相对角度模型从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度。
30.步骤1.1.基于机载单目视觉感知的多小型机器人系统的工作环境为通信受限环境，例如密闭室内、地下，以及水下环境等，感知传感器为机载单目视觉系统和罗盘，无全局传感器。
31.多个小型机器人单元无序状态下进入通信受限环境，每个机器人单元原地绕自身旋转一周感知周边环境，步幅小于单目摄像机的水平视场范围，即r
step
≤γ
horizon
。其中，r
step
表示步幅，γ
horizon
表示机载单目摄像机的水平视场范围。各个机器人单元分别利用机载单目视觉系统识别邻居，即邻近机器人单元，估计与邻居在摄像机坐标系o
ic
x
icyic
下的相对距离d
ij
与相对角度结合罗盘测量的全局方位θ
ij
，确定局部机器人坐标系o
li
x
liyli
下邻居的相对位姿p
ij
＝(x
ij
,y
ij
)，θ
ij
指机器人-i在感知到机器人-j时的全局方位，具体步骤如下：
32.i.目标识别：通过学习目标在一固定距离d
mask
不同姿态下的颜色、形状、结构特征，建立初始掩模特征库，并通过给出的二维互相关特征滤波器，如公式(1)所示，实现目标识别，并按照一定规则更新掩模库。图2给出了两种不同目标的识别实例，其中，图2中(a)图表示特征简单物体的识别过程，(b)图表示特征复杂物体的部分掩模，(c)图表示增加的部分掩模。
[0033][0034]
其中，f[mf,nf]是掩模特征库中的一个掩模，mf、nf分别表示掩模f[mf,nf]的第mf行、第nf列像素；g[x,y]是从当前感知图像中提取的一个特征区域，w和h分别表示特征区域g[x,y]的高和宽，x、y分别表示特征区域的第1行、第1列像素在当前感知图像中的第x行、第y列；表示当f[mf,nf]与特征区域g[x,y]尺寸不一致时，通过放大或缩小调整f[mf,nf]的尺寸后，与特征区域g[x,y]尺寸一致后的掩模，并得到调整的尺度调整比例因子rg。(f*g)[x,y]的值越大，f[mf,nf]与特征区域g[x,y]的相似性越大。
[0035]
基于此，定义一个可信度因子判断特征区域g[x,y]是不是目标物
体；其中，a＝(f*g)[x,y]，b＝(f*f)[x,y])；当g[x,y]与掩模特征库中所有掩模对比完成后，若可信度因子的最大值λ
max
≥0.6，则g[x,y]为目标物体，否则不是目标物体。
[0036]
当从当前感知图像上提取的全部特征区域都经过二维互相关特征滤波器筛选后，可以确定目标物体的数量，以及在当前感知图像中的位置，至此目标识别完成。而且给定阈值ε
mask
，当λ
max
≤ε
mask
时，将当前目标的特征区域g[x,y]制作成一个掩模，更新掩模特征库。
[0037]
ii.相对位姿确定：为了确定机器人单元在局部坐标系o
li
x
liyli
的位姿，首先建立机器人局部坐标系o
li
x
liyli
、摄像机坐标系o
ic
x
icyic
，以及图像坐标系oiuivi；其中，图像坐标系的原点oi建立在图像的第1行、第1列像素上，oiui轴沿图像的行方向，oivi轴沿图像的列方向；摄像机坐标系o
ic
x
icyic
的原点与机载摄像机的光心重合，光轴方向为摄像机坐标系o
ic
x
icyic
的y轴，摄像机坐标系o
ic
x
icyic
的x轴平行于图像坐标系oiuivi的oiui轴；机器人局部坐标系o
li
x
liyli
的原点与摄像机坐标系o
ic
x
icyic
的原点重合，罗盘测量的正北方向为机器人局部坐标系o
li
x
liyli
的y轴，按右手准则确定机器人局部坐标系o
li
x
liyli
的x轴。所有机器人局部坐标系o
li
x
liyli
相互平行，降低了空间转换难度。其次，在摄像机坐标系o
ic
x
icyic
下，通过学习不同距离下提取的特征尺度以及学习不同角度下提取的特征位置，如图3所示，建立目标距离d
ij
与特征尺度比例因子关系，以及目标角度与特征位置关系，如公式(2)所示；其中，在图3中，对应的学习范围如图浅灰色区域所示。
[0038][0039]
其中，rg为掩模f[mf,nf]的尺度调整比例因子，即特征尺度比例因子，u为目标在当前感知图像的图像坐标系oiuivi中的横坐标，即特征位置。a2、b1、a2、b2为待学习的参数，其中a1代表生成掩模库时的d
mask
，即构建初始掩模特征库时与目标固定的相对距离，如上述步骤1.1所示，a1的值围绕d
mask
上下波动，b1为不确定因子，a2、b2为比例系数。
[0040]
当将识别得到的目标对应的特征尺度比例因子rg和特征位置u代入公式(2)后，得到目标的相对距离d
ij
与相对角度结合感知到机器人-j时罗盘测量得到的机器人-i的全局方位θ
ij
，从当前感知图像中实现机器人-j在o
li
x
liyli
下相对位姿p
ij
的确定，如公式(3)所示。
[0041][0042]
步骤1.2.每个机器人单元在完成周边环境感知后，多小型机器人系统通过唯一的一次通信共享感知到的局部信息，包括自身id、感知到的邻居id、邻居数量ni，以及所有邻居在其坐标系o
li
x
liyli
下相对位姿使得每个机器人单元都拥有所有机器人单元感知的局部信息。然后，每个机器人单元各自从共享的这些相同信息中选择邻居数量nk最多的机器人-k的局部坐标系o
lk
x
lkylk
，作为共同的公共坐标系ocxcyc。
[0043]
步骤1.3.在确定初始分布过程中，每个机器人单元需要至少发现一个邻居，而且每个机器人对邻居的感知是一个有向通道，即机器人a感知到b，并不等同于机器人b同时感知到a。并且需要每个机器人感知邻居的子集si＝{rm,...rq}的并集包含所有机器人单元构
成的集合s＝{r1,...,rn}，即：构成一个连通有向图，如图4所示。
[0044]
其中，rm、rq、r1、rn分别代表机器人-m，机器人-q，机器人-1，和机器人-n。
[0045]
若不满足上述条件，则每个机器人单元随机微调位姿，重新回到步骤1.1，直至满足该条件；随后，以广度优先顺序遍历共享的邻居信息，逐层计算所有机器人单元在该公共坐标系下的坐标，得到多小型机器人系统的初始分布，如公式(4)所示。
[0046][0047]
其中，存放除机器人-j之外的其他机器人单元感知到机器人-j的mj次公共坐标系下的坐标；p
ci
最初为选作公共坐标系的机器人-k局部坐标系o
lk
x
lkylk
下的邻居机器人-i的坐标p
ki
，即p
ki
＝p
ci
，p
ij
则为机器人-i感知的邻居机器人-j坐标；随后，当机器人-j确定了其在公共坐标系下的一次坐标后，其感知到的邻居坐标也会被转移到公共坐标系下，直到所有的机器人都被遍历一遍；多次被感知到的机器人单元坐标取平均值作为最终的坐标值p
cj
。
[0048]
本发明步骤1中提出了一种针对目标特征丰富且多变的可在线更新的掩模特征库，结合二维互相关特征滤波器，解决了现有方法目标识别准确度与速度不能兼容的问题，能够在相对复杂的背景环境中，正确且快速识别目标并计算相对位姿，满足编队对目标感知的精度以及速度要求，保证编队的成功率。
[0049]
步骤2.基于局部位置信息的时间最优目标分配算法快速完成期望队形分布，以及机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划。
[0050]
步骤2.1.在确定期望队形的分布过程中，需要期望队形分布尽可能趋近于初始分布，运动控制的能量消耗最小化。我们采用提出的基于局部位置信息的时间最优目标分配算法，通过主分量分析法(pca)找到多小型机器人系统的初始分布的主轴方向，同时将该主轴方向作为期望队形的主轴，并作为期望队形坐标系o
p
x
pyp
的y轴，以多小型机器人系统的初始分布的平均位置为期望队形坐标系o
p
x
pyp
的原点，按右手准则确定期望队形坐标系o
p
x
pyp
的x轴。由期望队形的主轴与公共坐标系ocxcyc的x轴ocxc正方向的夹角用β表示。
[0051]
随后确定公共坐标系ocxcyc与期望队形坐标系o
p
x
pyp
之间的转换关系p
ci
＝rp
pi
t，其中，p
ci
，p
pi
分别表示机器人-i在公共坐标系、期望队形坐标系下的坐标，将多小型机器人系统的初始分布由公共坐标系ocxcyc转换到期望队形坐标系o
p
x
pyp
下。
[0052]
其中是ocxcyc与o
p
x
pyp
两个坐标系之间的旋转矩阵，是ocxcyc与o
p
x
pyp
两个坐标系之间的平移向量。然后，选择期望队形坐标系o
p
x
pyp
下最大y值坐标y
pmax
对应的y轴位置(0,y
pmax
)作为期望队形中领导者的位置，期望队形中的其他位置按照机器人单元之间的期望相对距离与相对角度依次确定，例如：本发明构建的队形结构为
一平面金字塔队形结构，如图5中(a)图所示的期望队形的确定，是一个主从式结构，除领导者外，每个机器人单元，又称为子机器人单元，都跟随其运动方向正前方的一个机器人单元，又称为父机器人单元，其位置的确定如公式(5)所示；
[0053][0054]
其中，de、分别是平面金字塔队形中父机器人与子机器人之间的期望相对距离与期望相对角度，od
pa
、od
ch
分别是父机器人单元与子机器人单元在期望队形中的序号，ly
ch
为子机器人在期望队形中的层号，(x
pa
,y
pa
)为期望队形中已确定位置的父机器人单元的坐标，(x
ch
,y
ch
)为坐标为(x
pa
,y
pa
)的父机器人单元的子机器人单元的坐标。
[0055]
步骤2.2.在分配每个机器人的目标位置过程中，为避免目标运动过程中互相发生碰撞，基于图论中“两对固定顶点的无交叉连线之和距离最小”这一引理，如图5中(b)图所示，该图为目标位置分配引理示意图。以所有机器人单元当前位置与期望位置的距离和最小作为损失函数，完成目标位置的分配，如公式(6)所示。
[0056][0057]
其中，w(g)表示损失函数，g表示多小型机器人构成的拓扑结构，为机器人-i在期望队形坐标系o
p
x
pyp
下的初始位置的坐标，与机器人-i在期望队形坐标系o
p
x
pyp
下的目标位置的坐标，表示与之间的相对距离。
[0058]
为降低计算量，引入分域划界的理念，通过几何与代数相结合的方式，优化目标位置的分配，以满足上述公式(6)；优化目标位置分配的具体过程如下：
[0059]
首先将o
p
x
pyp
坐标系下的多小型机器人系统的初始分布与期望队形分布分别按x坐标值划分成左右两个子集；然后每个子集内同样地按x、y坐标值不断划分子区域，直到子区域内最多包含两对初始位置与目标位置，记为一个联合子组；最后在联合子组内通过线性回归的方式迭代计算，使得分配的目标位置满足上述公式(6)，而生成的连接每个机器人单元初始位置与目标位置的无交叉直线段记为初始轨迹，如图5中(c)图所示，该图示出了目标位置的优化。
[0060]
本发明提出的步骤2能够在降低通信频次与通信量的基础上，通过提出的基于局部位置信息的时间最优目标位置分配方法确定期望队形的分布，以及优化目标位置的分配，并形成初始的无交叉运动轨迹，以降低每个机器人的运动距离。
[0061]
步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。
[0062]
机器人单元在运动过程中遇到障碍后，其运动状态被截断，不能仅凭自身的机载感知传感器确定该机器人单元在公共坐标系中的位置。每个机器人单元需要依靠视觉和罗盘感知的局部坐标系下的邻居信息，独立快速更新轨迹，低功耗完成队形构建；
[0063]
本发明提出一种异步离散时变加权一致性队形构建算法，如公式(7)所示，通过视
觉感知、误差计算以及运动控制循环，不断更新轨迹，直到到达相对目标位置。
[0064]ei
[(k 1)i]＝wi[ki]c
i1
[ki]e
id
[ki] w
io
[ki]c
i2
[ki]e
io
[ki]
ꢀꢀꢀꢀꢀ
(7)
[0065]
其中，(k 1)i、ki分别是机器人-i的第k 1个循环和k个循环，w
ij
[ki]、ni[ki]分别表示机器人-i在第ki个循环感知到的一个邻居机器人-j的权值、邻居数量；表示机器人-i在第ki个循环感知到的所有邻居的权值构成的权值向量。
[0066]wio
[ki]、n
io
[ki]分别表示机器人-i在第ki个循环感知到的障碍物-o的权值、障碍物数量；表示机器人-i在第ki个循环感知到所有障碍物的权值构成的权值向量；
[0067]
是感知到所有邻居在机器人-i坐标系下的距离矢量d
ij
[ki]与期望距离矢量之差构成的矩阵，
[0068]
和分别是为避免过度轨迹规划从贝尔曼方程引入的惩罚因子矩阵，β
ij
[ki]，α
ij
[ki]分别为感知到的机器人-j、障碍物-j对避免机器人-i过度轨迹规划与避免碰撞贡献的惩罚因子，是感知到所有障碍物在机器人-i坐标系下的距离矢量d
io
[ki]构成的矩阵。
[0069][0070][0071][0072]dij
[ki]、θ
ij
[ki]分别表示机器人-i在第ki次循环中感知到机器人-j时两者之间的相对距离、摄像机坐标系o
ic
x
icyic
下的相对角度以及罗盘感知到此时机器人-i的全局方位。
[0073]
分别表示机器人-i与机器人-j之间的期望距离与期望角度。
[0074]dio
[ki]、θ
io
[ki]分别表示机器人-i在第ki次循环中感知到障碍物-o时两者之间的相对距离、摄像机坐标系o
ic
x
icyic
下的相对角度以及罗盘感知到此时机器人-i的全局方位。
[0075]
多小型机器人系统的收敛条件是wi[ki]c
i1
[ki]的谱半径ρi＜1；其中，为避免过度规划轨迹，c
i1
[ki]的每个元素β
ij
[ki]∈[0,0.5]，如公式(8)所示，其谱半径小于1，所以核心是确定wi[ki]，因为谱半径ρ(a)不大于矩阵任意范数||a||，即ρ(a)≤||a||，而基于视觉感知的队形构建能够看作离散马尔科夫链，其转移矩阵的范数满足||pi[ki]||＜1，因此，wi[ki]从转移矩阵中得到，而且矩阵谱半径ρ越小收敛越快。
[0076]
首先每个机器人单元基于视觉感知到的邻居建立当前子拓扑结构为强连通子图gi[ki]，通过设计感知到的邻居的可信度确定邻接矩阵其中，a
ij
[ki]表示感知到的邻居机器人-j在ki循环中的可信度，得到拉普拉斯矩阵其中，是强连通子图gi[ki]的度量矩阵，进而得到转移矩阵t∈z且t＞0，z表示自然数；因为两个矩阵乘积的范数小于或等于两个矩阵范数的乘积，即||ab||≤||a||||b||，加上转移矩阵所以进而得到对应的范数最小，也就是对应的谱半径最小；因此，由计算权值向量，使得每个循环收敛速度最快，提高多小型机器人系统的收敛速度。
[0077]
两个惩罚因子矩阵中β
ij
[ki]和α
ij
[ki]的计算，分别如公式(8)和公式(9)所示：
[0078][0079]
其中，
[0080]
β
ij
[ki]因子调节机器人-i在邻居机器人-j方向上的分量，当分量较小时，能够增大分量，加快收敛速度；当分量较大时，能够减小分量，避免机器人之间发生碰撞。
[0081][0082]
其中，α
ij
[ki]∈[-1,0]，r是机器人的长度；
[0083]
α
ij
[ki]因子调节机器人-i与障碍物之间的距离，避免与障碍物发生碰撞。
[0084]
所有到达目标位置的机器人，通过感知位于其运动方向前方的机器人的位置，微调自身位姿，降低机器人运动控制误差带来的影响。
[0085]
本发明步骤3在遇到障碍后，每个机器人单元通过机载单目视觉感知环境，通过提出的异步离散时变加权一致性队形收敛算法，独立更新运动轨迹，并通过设计的权值向量与惩罚因子实现多机器人系统在未知环境中的快速收敛、避免轨迹过度规划，且降低了成本。
[0086]
此外，本发明还给出了基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法的仿真结果，如图6所示，图6中(a)图表示blender三维仿真环境，(b)图表示机器人的运动轨迹示意图。由图6能够看出，当机器人-1和机器人-5出现意外情况或遇到障碍，丢失自身在公共坐标系下的位姿时，由考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法基于邻居信息到达目标位置，其他机器人单元沿规划的初始轨迹达到目标位姿。
[0087]
综上，本发明针对通信受限密闭室内地面环境或者水下同一深度环境中的多小型机器人系统(二维环境)，结合成本低、硬件设备好实现、应用范围最广的单目视觉感知系统，提出了一种基于机载单目视觉感知、通信频次较低的混合队形快速收敛方法，该方法仅依靠单目视觉系统和罗盘感知信息能够快速实现队形的收敛，降低对通信模块的要求，并降低了成本，提高了多机器人系统在地下、水下等通信受限区域的应用能力。
[0088]
当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。