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

技术特征：
1.基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，其特征在于，包括如下步骤：步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器，从拍摄的单幅图像中完成目标识别；并基于建立的相对距离与相对角度模型，从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度；步骤2.基于局部位置信息的时间最优目标分配算法快速确定期望队形分布，以及完成机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划；步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。2.根据权利要求1所述的多小型机器人系统混合队形快速收敛方法，其特征在于，所述步骤1具体为：步骤1.1.基于机载单目视觉感知的多小型机器人系统的工作环境为通信受限环境，感知传感器为机载单目视觉系统和罗盘；多个小型机器人单元无序状态下进入通信受限环境，每个机器人单元原地绕自身旋转一周感知周边环境，步幅小于单目摄像机的水平视场范围；各个机器人单元分别利用机载单目视觉系统识别邻居，即邻近机器人单元，估计与邻居在其摄像机坐标系o
ic
x
ic
y
ic
下的相对距离d
ij
与相对角度结合罗盘测量的全局方位θ
ij
，确定局部机器人坐标系o
li
x
li
y
li
下邻居的相对位姿p
ij
＝(x
ij
,y
ij
)，其中，θ
ij
指机器人-i在感知到机器人-j时的全局方位，具体步骤如下：i.目标识别：首先在线下通过学习目标在一固定距离d
mask
不同姿态下的颜色、形状、结构特征，建立初始目标识别掩模特征库，然后通过给出的二维互相关特征滤波器，实现目标识别，二维互相关特征滤波器如公式(1)所示；其中，f[m
f
,n
f
]是掩模特征库中的一个掩模，m
f
、n
f
分别表示掩模f[m
f
,n
f
]的第m
f
行、第n
f
列像素；g[x,y]是从当前感知图像中提取的一个特征区域，w和h分别表示特征区域g[x,y]的高和宽，x、y分别表示特征区域g[x,y]的第1行、第1列像素在当前感知图像中的第x行、第y列；表示当掩模f[m
f
,n
f
]与特征区域g[x,y]尺寸不一致时，通过放大或缩小调整f[m
f
,n
f
]的尺寸后与特征区域g[x,y]尺寸一致后的掩模，并得到尺度调整比例因子r
g
；(f*g)[x,y]的值越大，表示掩模f[m
f
,n
f
]与特征区域g[x,y]的相似性越大；基于此，定义一个可信度因子判断特征区域g[x,y]是不是目标物体；其中，a＝(f*g)[x,y]，b＝(f*f)[x,y]，b表示掩模跟掩模自身求相关程度；当特征区域g[x,y]与掩模特征库中所有掩模对比完成后，若可信度因子的最大值λ
max
≥0.6，则特征区域g[x,y]为目标物体，否则，特征区域g[x,y]不是目标物体；当从当前感知图像上提取的全部特征区域都经过二维互相关特征滤波器筛选后，确定目标物体的数量以及在当前感知图像中的位置，至此目标识别完成；给定阈值ε
mask
，当λ
max
≤ε
mask
时，将当前目标的特征区域g[x,y]制作成一个掩模，更新掩模特征库；
ii.相对位姿确定：为了确定机器人单元在局部坐标系o
li
x
li
y
li
的位姿，首先建立机器人局部坐标系o
li
x
li
y
li
、摄像机坐标系o
ic
x
ic
y
ic
以及图像坐标系o
i
u
i
v
i
；图像坐标系的原点o
i
建立在图像的第1行、第1列像素上，o
i
u
i
轴沿图像的行方向，o
i
v
i
轴沿图像的列方向；摄像机坐标系o
ic
x
ic
y
ic
的原点与机载摄像机的光心重合，光轴方向为摄像机坐标系o
ic
x
ic
y
ic
的y轴，摄像机坐标系o
ic
x
ic
y
ic
的x轴平行于图像坐标系o
i
u
i
v
i
的o
i
u
i
轴；机器人局部坐标系o
li
x
li
y
li
的原点与摄像机坐标系o
ic
x
ic
y
ic
的原点重合，罗盘测量的正北方向为机器人局部坐标系o
li
x
li
y
li
的y轴，按右手准则确定机器人局部坐标系o
li
x
li
y
li
的x轴；所有机器人局部坐标系o
li
x
li
y
li
相互平行；其次，在摄像机坐标系o
ic
x
ic
y
ic
下，通过学习不同距离下提取的特征尺度以及学习不同角度下提取的特征位置，建立目标相对距离d
ij
与特征尺度比例因子关系，即相对距离模型，以及目标相对角度与特征位置关系，即相对角度模型，如公式(2)所示；其中，r
g
为掩模f[m
f
,n
f
]的尺度调整比例因子，即特征尺度比例因子，u表示目标在当前感知图像的图像坐标系o
i
u
i
v
i
中的横坐标，即特征位置；a2、b1、a2、b2为待学习的参数，其中，a1代表生成掩模库时的d
mask
，a1的值围绕d
mask
上下波动，b1为不确定因子，a2、b2为比例系数；当将识别得到的目标对应的特征尺度比例因子r
g
和特征位置u代入公式(2)后，得到目标的相对距离d
ij
与相对角度然后结合感知到机器人-j时罗盘测量得到的机器人-i的全局方位θ
ij
，从当前感知图像中实现机器人-j在机器人局部坐标系o
li
x
li
y
li
下相对位姿p
ij
的确定，如公式(3)所示；步骤1.2.每个机器人单元在完成周边环境感知后，多小型机器人系统通过唯一的一次通信共享感知到的局部信息，包括自身id、感知到的邻居id、邻居数量n
i
，以及所有感知到的邻居在其坐标系o
li
x
li
y
li
下的相对位姿使得每个机器人单元都拥有所有机器人单元感知的局部信息；然后，每个机器人单元各自从共享的这些相同信息中选择邻居数量n
k
最多的机器人-k的局部坐标系o
lk
x
lk
y
lk
，作为共同的公共坐标系o
c
x
c
y
c
，n
k
＝max{n1,...,n
n
}，n表示机器人-n，n1表示机器人-1感知的邻居数量，n
n
表示机器人-n感知的邻居数量；步骤1.3.在确定初始分布过程中，每个机器人单元需要至少发现一个邻居，而且每个机器人对邻居的感知是一个有向通道，并且需要每个机器人感知邻居的子集s
i
＝{r
m
,...r
q
}的并集包含所有机器人单元构成的集合s＝{r1,...,r
n
}，即：构成一个连通有向图；其中，r
m
、r
q
、r1、r
n
分别代表机器人-m，机器人-q，机器人-1，和机器人-n；若不满足上述条件，则每个机器人单元随机微调位姿，重新回到步骤1.1，直至满足该
条件；随后，以广度优先顺序遍历共享的邻居信息，逐层计算所有机器人单元在该公共坐标系下的坐标，得到多小型机器人系统的初始分布，如公式(4)所示；其中，用于存放除机器人-j之外的其他机器人单元感知到机器人-j的m
j
次公共坐标系下的坐标；p
ci
最初为选作公共坐标系的机器人-k局部坐标系o
lk
x
lk
y
lk
下的邻居机器人-i的坐标p
ki
，即p
ki
＝p
ci
，p
ij
则为机器人-i感知的邻居机器人-j坐标；随后，当机器人-j确定了其在公共坐标系下的一次坐标后，其感知到的邻居坐标也会被转移到公共坐标系下，直到所有的机器人都被遍历一遍；多次被感知到的机器人单元坐标取平均值作为最终的坐标值p
cj
。3.根据权利要求2所述的多小型机器人系统混合队形快速收敛方法，其特征在于，所述步骤2具体为：步骤2.1.通过主分量分析法找到多小型机器人系统的初始分布的主轴方向，同时将该主轴方向作为期望队形的主轴，并作为期望队形坐标系o
p
x
p
y
p
的y轴，以多小型机器人系统的初始分布的平均位置为期望队形坐标系o
p
x
p
y
p
的原点，按右手准则确定期望队形坐标系o
p
x
p
y
p
的x轴；期望队形的主轴与公共坐标系o
c
x
c
y
c
的x轴o
c
x
c
正方向的夹角用β表示；随后确定公共坐标系o
c
x
c
y
c
与期望队形坐标系o
p
x
p
y
p
之间的转换关系p
ci
＝rp
pi
t，其中，p
ci
、p
pi
分别表示机器人-i在公共坐标系o
c
x
c
y
c
、期望队形坐标系o
p
x
p
y
p
下的坐标，将多小型机器人系统的初始分布由公共坐标系o
c
x
c
y
c
转换到期望队形坐标系o
p
x
p
y
p
下；其中，是公共坐标系o
c
x
c
y
c
与期望队形坐标系o
p
x
p
y
p
之间的旋转矩阵，是公共坐标系o
c
x
c
y
c
与期望队形坐标系o
p
x
p
y
p
之间的平移向量；然后，选择期望队形坐标系o
p
x
p
y
p
下最大y值坐标y
pmax
对应的y轴位置(0,y
pmax
)作为期望队形中领导者的位置，期望队形中的其他位置按照机器人单元之间的期望相对距离d
e
与相对角度依次确定；步骤2.2.在分配每个机器人单元的目标位置过程中，为避免目标运动过程中互相发生碰撞，以所有机器人单元当前位置与期望位置的距离和最小作为损失函数，完成目标位置的分配，如公式(5)所示；其中，w(g)表示损失函数，g表示多小型机器人构成的拓扑结构，为机器人-i在期望队形坐标系o
p
x
p
y
p
下的初始位置坐标，与机器人-i在期望队形坐标系o
p
x
p
y
p
下的目标位置坐标，表示初始位置坐标与目标位置坐标之间的相对距离；
为降低计算量，引入分域划界的理念，通过几何与代数相结合的方式，优化目标位置的分配，以满足上述公式(5)；优化目标位置分配的具体过程如下：首先将期望队形坐标系o
p
x
p
y
p
下的多小型机器人系统的初始分布与期望队形分布分别按x坐标值划分成左右两个子集；然后每个子集内按x、y坐标值不断划分子区域，直到子区域内最多包含两对初始位置与目标位置，记为一个联合子组；最后在联合子组内通过线性回归的方式迭代计算，使得分配的目标位置满足上述公式(5)，而生成的连接每个机器人单元初始位置坐标与目标位置坐标的无交叉直线段记为初始轨迹。4.根据权利要求3所述的多小型机器人系统混合队形快速收敛方法，其特征在于，所述步骤3具体为：机器人单元在运动过程中遇到障碍后，其运动状态被截断，不能仅凭自身的机载感知传感器确定该机器人单元在公共坐标系中的位置，每个机器人单元需要依靠视觉和罗盘感知的局部坐标系下的邻居信息，独立快速更新轨迹，低功耗完成队形构建；提出一种异步离散时变加权一致性队形构建算法，如公式(6)所示，通过视觉感知、距离误差矢量计算以及运动控制循环，不断更新轨迹，直到到达相对目标位置；e
i
[(k 1)
i
]＝w
i
[k
i
]c
i1
[k
i
]e
id
[k
i
] w
io
[k
i
]c
i2
[k
i
]e
io
[k
i
]
ꢀꢀꢀꢀꢀ
(6)其中，(k 1)
i
、k
i
分别是机器人-i的第k 1个循环和k个循环，w
ij
[k
i
]、n
i
[k
i
]分别表示机器人-i在第k
i
个循环感知到的一个邻居机器人-j的权值、邻居数量；表示机器人-i在第k
i
个循环感知到的所有邻居的权值构成的权值向量；w
io
[k
i
]、n
io
[k
i
]分别表示机器人-i在第k
i
个循环感知到的障碍物-o的权值、障碍物数量；表示机器人-i在第k
i
个循环感知到所有障碍物的权值构成的权值向量；是感知到所有邻居在机器人-i坐标系下的距离矢量d
ij
[k
i
]与期望距离矢量之差构成的矩阵，之差构成的矩阵，和分别是为避免过度轨迹规划从贝尔曼方程引入的惩罚因子矩阵，β
ij
[k
i
]、α
ij
[k
i
]分别为感知到的机器人-j、障碍物-j对避免机器人-i过度轨迹规划与避免碰撞贡献的惩罚因子，是感知到所有障碍物在机器人-i坐标系下的距离矢量d
io
[k
i
]构成的矩阵；]构成的矩阵；]构成的矩阵；d
ij
[k
i
]、θ
ij
[k
i
]分别表示机器人-i在第k
i
次循环中感知到机器人-j时两者之间的相对距离、摄像机坐标系o
ic
x
ic
y
ic
下的相对角度以及罗盘感知到此时机器人-i的全局方位；
与分别表示机器人-i与机器人-j人之间的期望距离与期望角度；d
io
[k
i
]、θ
io
[k
i
]分别表示机器人-i在第k
i
次循环中感知到障碍物-o时两者之间的相对距离、摄像机坐标系o
ic
x
ic
y
ic
下的相对角度以及罗盘感知到此时机器人-i的全局方位；多小型机器人系统的收敛条件是w
i
[k
i
]c
i1
[k
i
]的谱半径ρ
i
＜1；其中，为避免过度规划轨迹，c
i1
[k
i
]的每个元素β
ij
[k
i
]∈[0,0.5]，如公式(7)所示，其谱半径小于1，所以核心是确定w
i
[k
i
]，因为谱半径ρ(a)不大于矩阵任意范数||a||，即ρ(a)≤||a||，而基于视觉感知的队形构建能够看作离散马尔科夫链，其转移矩阵的范数满足||p
i
[k
i
]||＜1，因此，w
i
[k
i
]从转移矩阵中得到，而且矩阵谱半径ρ越小收敛越快；首先每个机器人单元基于视觉感知到的邻居建立当前子拓扑结构为强连通子图g
i
[k
i
]，通过设计感知到的邻居的可信度确定邻接矩阵其中，a
ij
[k
i
]表示感知到的邻居机器人-j在k
i
循环中的可信度，得到拉普拉斯矩阵其中，是强连通子图g
i
[k
i
]的度量矩阵，进而得到转移矩阵t∈z且t＞0，z表示自然数；因为两个矩阵乘积的范数小于或等于两个矩阵范数的乘积，即||ab||≤||a||||b||，加上转移矩阵所以进而得到对应的范数最小，也就是对应的谱半径最小；因此，由计算权值向量，使得每个循环收敛速度最快，以提高多小型机器人系统的收敛速度；两个惩罚因子矩阵中β
ij
[k
i
]和α
ij
[k
i
]的计算，分别如公式(7)和公式(8)所示：其中，β
ij
[k
i
]因子调节机器人-i在邻居机器人-j方向上的分量；其中，α
ij
[k
i
]∈[-1,0]，r是机器人的长度；α
ij
[k
i
]因子调节机器人-i与障碍物之间的距离。

技术总结
本发明公开了一种基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，其包括如下步骤：首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器，从拍摄的单幅图像中完成目标识别；基于建立的相对距离与相对角度模型从单幅图像中获得三维信息；基于局部位置信息的时间最优目标分配算法快速确定期望队形分布，以及完成机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划；当遇到障碍时自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿独立更新轨迹，直到队形收敛。本发明通信频次小、计算量小，能够避障，且提高了队形收敛速度。提高了队形收敛速度。提高了队形收敛速度。


技术研发人员：王晓敏 孙振东 郑舟 孔旭
受保护的技术使用者：山东科技大学
技术研发日：2021.10.27
技术公布日：2022/2/18