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

权利要求书

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

包括如下步骤：

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

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

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

所述步骤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。