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

Abstract

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

Description

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

技术领域

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

背景技术

基于机载视觉感知的多小型机器人系统能够仅依靠其自身视觉系统感知的局部信息进行队形构建，进而完成给定任务，该系统在通信恶劣环境(例如地下、室内、水下或者GPS定位信号较弱的室外环境等)中有着广泛的应用前景。尽管针对多机器人系统的分布式控制方法的研究成果丰富，但现有的基于局部信息的队形收敛方法，主要在控制理论方面对考虑通信反馈延迟、有限时间约束等分布式控制算法做出了贡献，很少有研究者基于机载视觉系统感知的特性，提出适用于基于机载视觉感知系统的多机器人队形收敛方法；而已存在的基于机载视觉感知系统的多机器人队形收敛方法的收敛速度与抗干扰性不能兼顾。

由于基于机载单目视觉感知的多小型机器人系统因对通信环境要求比较低、成本低，因此得到了很多研究者的关注。由于机载单目视觉感知较小的范围、深度信息的缺失，机器人单元特征多变且差异性较大，使得机器人单元识别的准确度以及相对位姿的精度受到了各种因素的影响，降低了视觉反馈的准确性。此外，现有技术中很少有队形控制方法考虑机载视觉感知与伺服的特性，进而限制了队形收敛方法的准确性。因此，研究基于机载单目视觉感知的多小型机器人系统队形收敛方法变得至关重要。

发明内容

本发明的目的在于提出一种基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，该方法通信频次小、计算量小，能够避障，而且通过局部位置信息的时间最优目标分配算法以及异步离散时变加权一致性队形构建算法的权值更新规则，能够提高队形收敛速度。

本发明为了实现上述目的，采用如下技术方案：

基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，包括如下步骤：

步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器从拍摄的单幅图像中完成目标识别，并基于建立的相对距离与相对角度模型从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度；

步骤2.基于局部位置信息的时间最优目标分配算法快速完成期望队形分布，以及机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划；

步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。

本发明具有如下优点：

1.本发明方法，仅依靠单目视觉系统和罗盘感知信息能够快速实现队形的收敛，能够降低对通信模块的要求，并降低了成本，提高了多机器人系统在地下、水下等通信受限区域的应用能力。

2.本发明方法支持机器人数量较多的多机器人系统，提高了应用范围。

3.本发明方法对多机器人系统的机器人单元性能要求较低，仅需要配备摄像机、罗盘、IMU以及简单的通信模块即可。

4.本发明方法提出的针对机载单目视觉感知的基于可在线更新掩模特征库的二维互相关特征滤波器，能够在相对复杂背景下，识别特征多样化的目标物体。

5.本发明方法提出的基于局部位置信息的时间最优目标分配算法，能够优化期望队形的分布以及目标分配，并生成基于梯度下降最大且无交叉的初始轨迹。

6.本发明方法提出的考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，能够基于感知的邻居相对位姿信息，实现避障，更新轨迹，无需通信；而且给出的权值更新规则能够快速实现队形收敛。

附图说明

图1为本发明实施例中基于机载单目视觉感知的多小型机器人混合队形快速收敛方法的流程图；

图2为本发明实施例中目标识别实例示意图；

图3为本发明实施例中相对距离模型、相对角度模型学习过程示意图；

图4为本发明实施例中初始分布构成的连通有向图；

图5为本发明实施例中期望队形分布确定以及目标位置的优化分配示意图；

图6为本发明实施例中基于机载单目视觉感知的多小型机器人混合队形快速收敛方法的仿真结果示意图。

具体实施方式

本实施例提供了一种基于机载单目视觉感知的多小型机器人混合队形快速收敛方法，该方法包含机载单目视觉感知方法、基于局部位置信息的时间最优目标位置分配方法以及考虑避障与收敛速度的异步离散时变加权一致性队形快速构建方法。

其中，机载单目视觉感知方法解决了每个机器人单元的目标识别与空间定位问题。在识别目标时，通过建立掩模特征库，以及基于特征库的二维互相关特征滤波器，识别特征丰富且变化较大的目标，而且根据识别得到的目标特征，按照一定规则更新掩模特征库。为了获取目标三维信息，基于摄像机模型，构建了局部坐标系下的相对距离与相对角度模型，从单幅图像中确定感知到的邻近机器人及障碍物在其局部坐标系下的相对位姿。

基于局部位置信息的时间最优目标位置分配方法，解决了期望队形的分布与机器人单元在期望队形中的目标位置优化分配问题。在通过唯一一次通信，共享感知的机器人单元数量与相对位姿，确定多小型机器人系统初始分布后，基于局部位置信息的时间最优目标位置分配方法基于主分量分析法(PCA)优化期望队形分布使其趋近于初始分布，基于二分图原理设计目标位置分配规则，计算期望位置关系(Position Relationship,PR)，规划避碰的初始无交叉轨迹。

考虑避障与收敛速度的异步离散时变加权一致性队形快速构建方法，解决了障碍环境下轨迹更新问题。当某机器人单元沿初始轨迹向目标位置运动过程中遇到动/静态障碍，不能明确自身在公共坐标系下的位姿时，基于提出的考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，该机器人单元通过“感知、计算、运动”的循环，不断感知当前邻居与障碍信息，并异步独立更新轨迹直到收敛到期望队形。在轨迹更新过程中，每个机器人单元的运动控制由PID控制算法实现。

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示，基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，包括如下步骤：

步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器从拍摄的单幅图像中完成目标识别，并基于建立的相对距离与相对角度模型从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度。

步骤1.1.基于机载单目视觉感知的多小型机器人系统的工作环境为通信受限环境，例如密闭室内、地下，以及水下环境等，感知传感器为机载单目视觉系统和罗盘，无全局传感器。

多个小型机器人单元无序状态下进入通信受限环境，每个机器人单元原地绕自身旋转一周感知周边环境，步幅小于单目摄像机的水平视场范围，即r_step≤γ_horizon。其中，r_step表示步幅，γ_horizon表示机载单目摄像机的水平视场范围。各个机器人单元分别利用机载单目视觉系统识别邻居，即邻近机器人单元，估计与邻居在摄像机坐标系o_icx_icy_ic下的相对距离d_ij与相对角度

结合罗盘测量的全局方位θ_ij，确定局部机器人坐标系o_lix_liy_li下邻居的相对位姿p_ij＝(x_ij,y_ij)，θ_ij指机器人-i在感知到机器人-j时的全局方位，具体步骤如下：

I.目标识别：通过学习目标在一固定距离d_mask不同姿态下的颜色、形状、结构特征，建立初始掩模特征库，并通过给出的二维互相关特征滤波器，如公式(1)所示，实现目标识别，并按照一定规则更新掩模库。图2给出了两种不同目标的识别实例，其中，图2中(a)图表示特征简单物体的识别过程，(b)图表示特征复杂物体的部分掩模，(c)图表示增加的部分掩模。

其中，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分别表示特征区域的第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])；当g[x,y]与掩模特征库中所有掩模对比完成后，若可信度因子的最大值λ_max≥0.6，则g[x,y]为目标物体，否则不是目标物体。

当从当前感知图像上提取的全部特征区域都经过二维互相关特征滤波器筛选后，可以确定目标物体的数量，以及在当前感知图像中的位置，至此目标识别完成。而且给定阈值ε_mask，当λ_max≤ε_mask时，将当前目标的特征区域g[x,y]制作成一个掩模，更新掩模特征库。

II.相对位姿确定：为了确定机器人单元在局部坐标系o_lix_liy_li的位姿，首先建立机器人局部坐标系o_lix_liy_li、摄像机坐标系o_icx_icy_ic，以及图像坐标系o_iu_iv_i；其中，图像坐标系的原点o_i建立在图像的第1行、第1列像素上，o_iu_i轴沿图像的行方向，o_iv_i轴沿图像的列方向；摄像机坐标系o_icx_icy_ic的原点与机载摄像机的光心重合，光轴方向为摄像机坐标系o_icx_icy_ic的y轴，摄像机坐标系o_icx_icy_ic的x轴平行于图像坐标系o_iu_iv_i的o_iu_i轴；机器人局部坐标系o_lix_liy_li的原点与摄像机坐标系o_icx_icy_ic的原点重合，罗盘测量的正北方向为机器人局部坐标系o_lix_liy_li的y轴，按右手准则确定机器人局部坐标系o_lix_liy_li的x轴。所有机器人局部坐标系o_lix_liy_li相互平行，降低了空间转换难度。其次，在摄像机坐标系o_icx_icy_ic下，通过学习不同距离下提取的特征尺度以及学习不同角度下提取的特征位置，如图3所示，建立目标距离d_ij与特征尺度比例因子关系，以及目标角度

与特征位置关系，如公式(2)所示；其中，在图3中，对应的学习范围如图浅灰色区域所示。

其中，r_g为掩模f[m_f,n_f]的尺度调整比例因子，即特征尺度比例因子，u为目标在当前感知图像的图像坐标系o_iu_iv_i中的横坐标，即特征位置。a₂、b₁、a₂、b₂为待学习的参数，其中a₁代表生成掩模库时的d_mask，即构建初始掩模特征库时与目标固定的相对距离，如上述步骤1.1所示，a₁的值围绕d_mask上下波动，b₁为不确定因子，a₂、b₂为比例系数。

当将识别得到的目标对应的特征尺度比例因子r_g和特征位置u代入公式(2)后，得到目标的相对距离d_ij与相对角度

结合感知到机器人-j时罗盘测量得到的机器人-i的全局方位θ_ij，从当前感知图像中实现机器人-j在o_lix_liy_li下相对位姿p_ij的确定，如公式(3)所示。

步骤1.2.每个机器人单元在完成周边环境感知后，多小型机器人系统通过唯一的一次通信共享感知到的局部信息，包括自身ID、感知到的邻居ID、邻居数量N_i，以及所有邻居在其坐标系o_lix_liy_li下相对位姿

使得每个机器人单元都拥有所有机器人单元感知的局部信息。然后，每个机器人单元各自从共享的这些相同信息中选择邻居数量N_k最多的机器人-k的局部坐标系o_lkx_lky_lk，作为共同的公共坐标系o_cx_cy_c。

步骤1.3.在确定初始分布过程中，每个机器人单元需要至少发现一个邻居，而且每个机器人对邻居的感知是一个有向通道，即机器人A感知到B，并不等同于机器人B同时感知到A。并且需要每个机器人感知邻居的子集S_i＝{R_m,...R_q}的并集包含所有机器人单元构成的集合S＝{R₁,...,R_n}，即：

构成一个连通有向图，如图4所示。

其中，R_m、R_q、R₁、R_n分别代表机器人-m，机器人-q，机器人-1，和机器人-n。

若不满足上述条件，则每个机器人单元随机微调位姿，重新回到步骤1.1，直至满足该条件；随后，以广度优先顺序遍历共享的邻居信息，逐层计算所有机器人单元在该公共坐标系下的坐标，得到多小型机器人系统的初始分布，如公式(4)所示。

其中，

存放除机器人-j之外的其他机器人单元感知到机器人-j的m_j次公共坐标系下的坐标；p_ci最初为选作公共坐标系的机器人-k局部坐标系o_lkx_lky_lk下的邻居机器人-i的坐标p_ki，即p_ki＝p_ci，p_ij则为机器人-i感知的邻居机器人-j坐标；随后，当机器人-j确定了其在公共坐标系下的一次坐标

后，其感知到的邻居坐标也会被转移到公共坐标系下，直到所有的机器人都被遍历一遍；多次被感知到的机器人单元坐标取平均值作为最终的坐标值p_cj。

本发明步骤1中提出了一种针对目标特征丰富且多变的可在线更新的掩模特征库，结合二维互相关特征滤波器，解决了现有方法目标识别准确度与速度不能兼容的问题，能够在相对复杂的背景环境中，正确且快速识别目标并计算相对位姿，满足编队对目标感知的精度以及速度要求，保证编队的成功率。

步骤2.基于局部位置信息的时间最优目标分配算法快速完成期望队形分布，以及机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划。

步骤2.1.在确定期望队形的分布过程中，需要期望队形分布尽可能趋近于初始分布，运动控制的能量消耗最小化。我们采用提出的基于局部位置信息的时间最优目标分配算法，通过主分量分析法(PCA)找到多小型机器人系统的初始分布的主轴方向，同时将该主轴方向作为期望队形的主轴，并作为期望队形坐标系o_px_py_p的y轴，以多小型机器人系统的初始分布的平均位置

为期望队形坐标系o_px_py_p的原点，按右手准则确定期望队形坐标系o_px_py_p的x轴。由期望队形的主轴与公共坐标系o_cx_cy_c的x轴o_cx_c正方向的夹角用β表示。

随后确定公共坐标系o_cx_cy_c与期望队形坐标系o_px_py_p之间的转换关系p_ci＝Rp_pi+T，其中，p_ci，p_pi分别表示机器人-i在公共坐标系、期望队形坐标系下的坐标，将多小型机器人系统的初始分布由公共坐标系o_cx_cy_c转换到期望队形坐标系o_px_py_p下。

其中

是o_cx_cy_c与o_px_py_p两个坐标系之间的旋转矩阵，

是o_cx_cy_c与o_px_py_p两个坐标系之间的平移向量。然后，选择期望队形坐标系o_px_py_p下最大y值坐标y_pmax对应的y轴位置(0,y_pmax)作为期望队形中领导者的位置，期望队形中的其他位置按照机器人单元之间的期望相对距离与相对角度依次确定，例如：本发明构建的队形结构为一平面金字塔队形结构，如图5中(a)图所示的期望队形的确定，是一个主从式结构，除领导者外，每个机器人单元，又称为子机器人单元，都跟随其运动方向正前方的一个机器人单元，又称为父机器人单元，其位置的确定如公式(5)所示；

其中，d_e、

分别是平面金字塔队形中父机器人与子机器人之间的期望相对距离与期望相对角度，

od_pa、od_ch分别是父机器人单元与子机器人单元在期望队形中的序号，ly_ch为子机器人在期望队形中的层号，(x_pa,y_pa)为期望队形中已确定位置的父机器人单元的坐标，(x_ch,y_ch)为坐标为(x_pa,y_pa)的父机器人单元的子机器人单元的坐标。

步骤2.2.在分配每个机器人的目标位置过程中，为避免目标运动过程中互相发生碰撞，基于图论中“两对固定顶点的无交叉连线之和距离最小”这一引理，如图5中(b)图所示，该图为目标位置分配引理示意图。以所有机器人单元当前位置与期望位置的距离和最小作为损失函数，完成目标位置的分配，如公式(6)所示。

其中，w(G)表示损失函数，G表示多小型机器人构成的拓扑结构，

为机器人-i在期望队形坐标系o_px_py_p下的初始位置的坐标，

与机器人-i在期望队形坐标系o_px_py_p下的目标位置的坐标，

表示

与

之间的相对距离。

为降低计算量，引入分域划界的理念，通过几何与代数相结合的方式，优化目标位置的分配，以满足上述公式(6)；优化目标位置分配的具体过程如下：

首先将o_px_py_p坐标系下的多小型机器人系统的初始分布与期望队形分布分别按x坐标值划分成左右两个子集；然后每个子集内同样地按x、y坐标值不断划分子区域，直到子区域内最多包含两对初始位置与目标位置，记为一个联合子组；最后在联合子组内通过线性回归的方式迭代计算，使得分配的目标位置满足上述公式(6)，而生成的连接每个机器人单元初始位置

与目标位置

的无交叉直线段记为初始轨迹，如图5中(c)图所示，该图示出了目标位置的优化。

本发明提出的步骤2能够在降低通信频次与通信量的基础上，通过提出的基于局部位置信息的时间最优目标位置分配方法确定期望队形的分布，以及优化目标位置的分配，并形成初始的无交叉运动轨迹，以降低每个机器人的运动距离。

步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。

机器人单元在运动过程中遇到障碍后，其运动状态被截断，不能仅凭自身的机载感知传感器确定该机器人单元在公共坐标系中的位置。每个机器人单元需要依靠视觉和罗盘感知的局部坐标系下的邻居信息，独立快速更新轨迹，低功耗完成队形构建；

本发明提出一种异步离散时变加权一致性队形构建算法，如公式(7)所示，通过视觉感知、误差计算以及运动控制循环，不断更新轨迹，直到到达相对目标位置。

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] (7)

其中，(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_icx_icy_ic下的相对角度以及罗盘感知到此时机器人-i的全局方位。

分别表示机器人-i与机器人-j之间的期望距离与期望角度。

d_io[k_i]、

θ_io[k_i]分别表示机器人-i在第k_i次循环中感知到障碍物-o时两者之间的相对距离、摄像机坐标系o_icx_icy_ic下的相对角度以及罗盘感知到此时机器人-i的全局方位。

多小型机器人系统的收敛条件是W_i[k_i]C_i1[k_i]的谱半径ρ_i＜1；其中，为避免过度规划轨迹，C_i1[k_i]的每个元素β_ij[k_i]∈[0,0.5]，如公式(8)所示，其谱半径小于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]的计算，分别如公式(8)和公式(9)所示：

其中，

β_ij[k_i]因子调节机器人-i在邻居机器人-j方向上的分量，当分量较小时，能够增大分量，加快收敛速度；当分量较大时，能够减小分量，避免机器人之间发生碰撞。

其中，α_ij[k_i]∈[-1,0]，r是机器人的长度；

α_ij[k_i]因子调节机器人-i与障碍物之间的距离，避免与障碍物发生碰撞。

所有到达目标位置的机器人，通过感知位于其运动方向前方的机器人的位置，微调自身位姿，降低机器人运动控制误差带来的影响。

本发明步骤3在遇到障碍后，每个机器人单元通过机载单目视觉感知环境，通过提出的异步离散时变加权一致性队形收敛算法，独立更新运动轨迹，并通过设计的权值向量与惩罚因子实现多机器人系统在未知环境中的快速收敛、避免轨迹过度规划，且降低了成本。

此外，本发明还给出了基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法的仿真结果，如图6所示，图6中(a)图表示Blender三维仿真环境，(b)图表示机器人的运动轨迹示意图。由图6能够看出，当机器人-1和机器人-5出现意外情况或遇到障碍，丢失自身在公共坐标系下的位姿时，由考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法基于邻居信息到达目标位置，其他机器人单元沿规划的初始轨迹达到目标位姿。

综上，本发明针对通信受限密闭室内地面环境或者水下同一深度环境中的多小型机器人系统(二维环境)，结合成本低、硬件设备好实现、应用范围最广的单目视觉感知系统，提出了一种基于机载单目视觉感知、通信频次较低的混合队形快速收敛方法，该方法仅依靠单目视觉系统和罗盘感知信息能够快速实现队形的收敛，降低对通信模块的要求，并降低了成本，提高了多机器人系统在地下、水下等通信受限区域的应用能力。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (4)

1.基于机载单目视觉感知的多小型机器人系统混合队形快速收敛方法，其特征在于，

包括如下步骤：

步骤1.首先通过机载单目视觉感知算法建立的基于可在线更新掩模特征库的二维互相关特征滤波器，从拍摄的单幅图像中完成目标识别；并基于建立的相对距离与相对角度模型，从当前包含目标的单幅图像中获得目标的三维信息，即相对距离与相对角度；

步骤2.基于局部位置信息的时间最优目标分配算法快速确定期望队形分布，以及完成机器人单元目标位置的优化匹配，实现基于梯度下降速度最快且无交叉初始轨迹的规划；

步骤3.当遇到障碍时，无需通信，自主切换至考虑避障与收敛速度的异步离散时变加权一致性队形快速构建算法，基于感知到的邻居位姿，独立更新轨迹，直到队形收敛。

2.根据权利要求1所述的多小型机器人系统混合队形快速收敛方法，其特征在于，

所述步骤1具体为：

步骤1.1.基于机载单目视觉感知的多小型机器人系统的工作环境为通信受限环境，感知传感器为机载单目视觉系统和罗盘；多个小型机器人单元无序状态下进入通信受限环境，每个机器人单元原地绕自身旋转一周感知周边环境，步幅小于单目摄像机的水平视场范围；

各个机器人单元分别利用机载单目视觉系统识别邻居，即邻近机器人单元，估计与邻居在其摄像机坐标系o_icx_icy_ic下的相对距离d_ij与相对角度

结合罗盘测量的全局方位θ_ij，确定局部机器人坐标系o_lix_liy_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_lix_liy_li的位姿，首先建立机器人局部坐标系o_lix_liy_li、摄像机坐标系o_icx_icy_ic以及图像坐标系o_iu_iv_i；图像坐标系的原点o_i建立在图像的第1行、第1列像素上，o_iu_i轴沿图像的行方向，o_iv_i轴沿图像的列方向；摄像机坐标系o_icx_icy_ic的原点与机载摄像机的光心重合，光轴方向为摄像机坐标系o_icx_icy_ic的y轴，摄像机坐标系o_icx_icy_ic的x轴平行于图像坐标系o_iu_iv_i的o_iu_i轴；机器人局部坐标系o_lix_liy_li的原点与摄像机坐标系o_icx_icy_ic的原点重合，罗盘测量的正北方向为机器人局部坐标系o_lix_liy_li的y轴，按右手准则确定机器人局部坐标系o_lix_liy_li的x轴；所有机器人局部坐标系o_lix_liy_li相互平行；

其次，在摄像机坐标系o_icx_icy_ic下，通过学习不同距离下提取的特征尺度以及学习不同角度下提取的特征位置，建立目标相对距离d_ij与特征尺度比例因子关系，即相对距离模型，以及目标相对角度

与特征位置关系，即相对角度模型，如公式(2)所示；

其中，r_g为掩模f[m_f,n_f]的尺度调整比例因子，即特征尺度比例因子，u表示目标在当前感知图像的图像坐标系o_iu_iv_i中的横坐标，即特征位置；a₂、b₁、a₂、b₂为待学习的参数，其中，a₁代表生成掩模库时的d_mask，a₁的值围绕d_mask上下波动，b₁为不确定因子，a₂、b₂为比例系数；当将识别得到的目标对应的特征尺度比例因子r_g和特征位置u代入公式(2)后，得到目标的相对距离d_ij与相对角度

然后结合感知到机器人-j时罗盘测量得到的机器人-i的全局方位θ_ij，从当前感知图像中实现机器人-j在机器人局部坐标系o_lix_liy_li下相对位姿p_ij的确定，如公式(3)所示；

步骤1.2.每个机器人单元在完成周边环境感知后，多小型机器人系统通过唯一的一次通信共享感知到的局部信息，包括自身ID、感知到的邻居ID、邻居数量N_i，以及所有感知到的邻居在其坐标系o_lix_liy_li下的相对位姿

使得每个机器人单元都拥有所有机器人单元感知的局部信息；

然后，每个机器人单元各自从共享的这些相同信息中选择邻居数量N_k最多的机器人-k的局部坐标系o_lkx_lky_lk，作为共同的公共坐标系o_cx_cy_c，N_k＝max{N₁,...,N_n}，n表示机器人-n，N₁表示机器人-1感知的邻居数量，N_n表示机器人-n感知的邻居数量；

步骤1.3.在确定初始分布过程中，每个机器人单元需要至少发现一个邻居，而且每个机器人对邻居的感知是一个有向通道，并且需要每个机器人感知邻居的子集S_i＝{R_m,...R_q}的并集包含所有机器人单元构成的集合S＝{R₁,...,R_n}，即：

构成一个连通有向图；

其中，R_m、R_q、R₁、R_n分别代表机器人-m，机器人-q，机器人-1，和机器人-n；

若不满足上述条件，则每个机器人单元随机微调位姿，重新回到步骤1.1，直至满足该条件；随后，以广度优先顺序遍历共享的邻居信息，逐层计算所有机器人单元在该公共坐标系下的坐标，得到多小型机器人系统的初始分布，如公式(4)所示；

其中，

用于存放除机器人-j之外的其他机器人单元感知到机器人-j的m_j次公共坐标系下的坐标；p_ci最初为选作公共坐标系的机器人-k局部坐标系o_lkx_lky_lk下的邻居机器人-i的坐标p_ki，即p_ki＝p_ci，p_ij则为机器人-i感知的邻居机器人-j坐标；随后，当机器人-j确定了其在公共坐标系下的一次坐标

后，其感知到的邻居坐标也会被转移到公共坐标系下，直到所有的机器人都被遍历一遍；多次被感知到的机器人单元坐标取平均值作为最终的坐标值p_cj。

3.根据权利要求2所述的多小型机器人系统混合队形快速收敛方法，其特征在于，

所述步骤2具体为：

步骤2.1.通过主分量分析法找到多小型机器人系统的初始分布的主轴方向，同时将该主轴方向作为期望队形的主轴，并作为期望队形坐标系o_px_py_p的y轴，以多小型机器人系统的初始分布的平均位置

为期望队形坐标系o_px_py_p的原点，按右手准则确定期望队形坐标系o_px_py_p的x轴；

期望队形的主轴与公共坐标系o_cx_cy_c的x轴o_cx_c正方向的夹角用β表示；

随后确定公共坐标系o_cx_cy_c与期望队形坐标系o_px_py_p之间的转换关系p_ci＝Rp_pi+T，其中，p_ci、p_pi分别表示机器人-i在公共坐标系o_cx_cy_c、期望队形坐标系o_px_py_p下的坐标，将多小型机器人系统的初始分布由公共坐标系o_cx_cy_c转换到期望队形坐标系o_px_py_p下；

其中，

是公共坐标系o_cx_cy_c与期望队形坐标系o_px_py_p之间的旋转矩阵，

是公共坐标系o_cx_cy_c与期望队形坐标系o_px_py_p之间的平移向量；

然后，选择期望队形坐标系o_px_py_p下最大y值坐标y_pmax对应的y轴位置(0,y_pmax)作为期望队形中领导者的位置，期望队形中的其他位置按照机器人单元之间的期望相对距离d_e与相对角度

依次确定；

步骤2.2.在分配每个机器人单元的目标位置过程中，为避免目标运动过程中互相发生碰撞，以所有机器人单元当前位置与期望位置的距离和最小作为损失函数，完成目标位置的分配，如公式(5)所示；

其中，w(G)表示损失函数，G表示多小型机器人构成的拓扑结构，

为机器人-i在期望队形坐标系o_px_py_p下的初始位置坐标，

与机器人-i在期望队形坐标系o_px_py_p下的目标位置坐标，

表示初始位置坐标

与目标位置坐标

之间的相对距离；

为降低计算量，引入分域划界的理念，通过几何与代数相结合的方式，优化目标位置的分配，以满足上述公式(5)；优化目标位置分配的具体过程如下：

首先将期望队形坐标系o_px_py_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_icx_icy_ic下的相对角度以及罗盘感知到此时机器人-i的全局方位；

与

分别表示机器人-i与机器人-j人之间的期望距离与期望角度；

d_io[k_i]、

θ_io[k_i]分别表示机器人-i在第k_i次循环中感知到障碍物-o时两者之间的相对距离、摄像机坐标系o_icx_icy_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与障碍物之间的距离。

Images