CN113211439A - 一种基于bm匹配机械臂控制系统和方法 - Google Patents

Abstract

本发明公开了一种基于BM匹配机械臂控制系统，包括用于发送启动指令的上位机、用于处理并输出控制参数的嵌入式计算机、用于目标物拍摄识别的单目相机、双目相机和机械臂；单目相机安装在机械臂和目标物的正上方，双目相机安装在机械臂附近；单目相机、双目相机和机械臂与嵌入式计算机和上位机信号连接。本发明还公开了基于BM匹配机械臂控制方法，单目视觉通过基于颜色特征检测完成对目标的初步定位，并将信息传输至上位机；利用极线校正算法校正双目视觉摄取的左右图像，使用改进的BM立体匹配算法，完成目标的三维重建；采用五次多项式插值对机械臂进行轨迹规划，完成对目标的抓取。本发明提高机械臂对目标物的抓取速度，降低机械臂的磨损。

Description

一种基于BM匹配机械臂控制系统和方法

技术领域

本发明涉及图像识别、机械臂和信息传输领域，具体涉及一种基于BM匹配机械臂控制系统和方法。

背景技术

随着工业生产逐步向自动化、智能化方向发展，工业机器人开始得到普遍应用，以机器人代替人工生产，不仅可以提高生产效率，减少失误，还能有效降低工人的工作强度。机械臂作为机器人的分支之一，在工业生产中可以负责搬运、分拣等工作，对于工业生产而言，物料搬运作为工业生产中的重要环节之一，搬运效率及准确率会对生产产生直接影响。

目前，机械臂在准备抓取目标时存在由于图像处理时间长而导致抓取的准备时间长以及抓取的精度不高问题，还有机械臂在抓取过程中由于加速度过大，增加了对机械臂支架和电机的磨损。

现有改进的BM匹配算法BMHS算法，利用与模式串末位对应的后一位文本串字符T[m+1]，若T[m+1]不在模式串中，则可将模式串右移m+1位，若在模式串中，则利用坏字符启发函数进行右移。与BMH算法相比，BMHS算法提高了模式串的最大右移量，可达到m+1位。

在专利文献CN107767423A中公开了一种基于双目视觉的机械臂目标定位抓取方法，仅通过双目来实现对目标的定位，对于目标物的识别只识别面向摄像头的一个面的三维信息，容易造成造成目标物信息的丢失，导致抓取失败。

现有技术中存在的缺点有：在进行目标定位时，时间较长，导致从运行到结束的时间长；机械臂在抓取目标时，角速度在单位时间内变化大，导致角加速度大，造成舵机齿轮的磨损，影响机械臂的精度。

发明内容

发明目的：本发明的目的是提供一种基于BM匹配机械臂控制系统，提高机械臂对目标物的抓取速度，降低机械臂的磨损；本发明的另一个目的是提供一种基于BM匹配机械臂控制方法，精准高效地实现机械臂对目标物的抓取。

技术方案：一种基于BM匹配机械臂控制系统，包括用于发送启动指令的上位机、用于处理并输出控制参数的嵌入式计算机、用于目标物拍摄识别的单目相机、双目相机和机械臂；单目相机安装在机械臂和目标物的正上方，双目相机安装在机械臂附近；单目相机、双目相机和机械臂与嵌入式计算机和上位机信号连接。

进一步地，嵌入式计算机和上位机处于同一局域网下。

进一步地，双目相机的光心连线与水平线平行。减少校正的压力，同时也减少深度信息的计算误差。

进一步地，单目相机、双目相机、机械臂通过USB接口连接嵌入式计算机，嵌入式计算机至少拥有三个USB接口。

一种适用上述的基于BM匹配机械臂的控制方法，包括如下步骤：

步骤1：上位机发送开启指令，系统初始化；

步骤2：单目相机获取环境信息，建立全局空间定位模型，实现障碍物的识别和基于颜色特征检测对目标的粗定位，同时将环境信息上传至上位机；

步骤3：双目摄像头进行图像采集，采集后的图像经过极线校正操作得到在同一水平线上的左右视图，降低立体匹配的复杂度，进行BM立体匹配，经过三维重建得到目标的三维坐标；

步骤4：目标位置转化为世界坐标，将坐标转化为机械臂控制参数，驱动机械臂抓取目标。

进一步地，步骤1包括：

上位机启动，指令通过5G信号下的局域网实时传送到嵌入式计算机，嵌入式计算机收到指令，将单目和双目相机初始化，输出控制参数至机械臂，使机械臂位姿处于初始状态。

进一步地，步骤2包括：

单目相机启动，基于颜色特征进行目标检测，将RGB色彩模型转换为HSV色彩模型，对单目相机摄取的图像进行HSV图像阈值分割，将可能的目标与背景分开，对分割后的图像进行腐蚀操作和膨胀操作去噪点，通过目标物的外形区别目标物的轮廓和其他物体；单目相机摄取的周围环境信息通过5G信号下的局域网上传至上位机；其中，目标物的外形包括形状和大小。通过目标物的外形区别目标物的轮廓和其他物体，是将所求得的所有轮廓半径、长、宽与目标物的半径、长、宽进行对比。

进一步地，步骤3包括：

步骤3.1：双目相机获取左右图像：采用bouguet极线校正算法对摄取的左右图像进行校正，该算法是将OPenCV求解出来的旋转和平移矩阵分解成左右相机各旋转一半的旋转和平移矩阵R1,T1与R2,T2；

步骤3.2：BM立体匹配：图像中的每个像素点用像素值来表示，在匹配时，左视图中选中目标并作为模式窗口P，选取右视图中同一行所有像素即待匹配像素组T进行匹配；将二维像素组按顺序排列为一维；其中，P共有m个像素，位置记为P[1]，P[2]，…，P[m]，将T与P进行左对齐，然后进行从右向左比较；

步骤3.3：目标的三维重建，已知相机焦距f，视差d，光心距离B可求得深度信息Z，深度信息Z的计算公式为：

设在左视图中的目标点为(x,y)，视差为d，在以左相机坐标系下的目标点世界坐标为(X,Y,Z)，用Q来表示目标点的物理坐标与世界坐标间的关系，通过下式将图像坐标映射到相机三维空间中；

Q*[x,y,d,1]^T＝[X,Y,Z,W]^T

其中，Q为深度视差映射矩阵；x，y分别为目标点在物理坐标中的横坐标和纵坐标；W为转换因子；X/W，Y/W，Z/W分别为目标点转换后的世界坐标的横坐标，纵坐标和竖坐标；

其中，d＝X_L-X_R，X_L为目标上一点在左相机中成像的图像坐标，X_R为同一点在右相机中成像的图像坐标。

进一步地，步骤3.2包括：

在匹配过程中需要储存两个右移位置值d₁和d₂，d₁为当像素不匹配时，计算模式窗口P最右端像素对应的像素T[i]的下一次右移位置，d₂为计算T[i+1]确定的下一次右移位置，d₁和d₂的计算规则如下：

步骤3.2.1：考察模式窗口P的末像素P[m]与对应的待匹配像素组T[m]是否匹配，若不匹配，则考虑T[m]的下一像素T[m+i]，且利用坏像素规则，求得d₁的值；

步骤3.2.2：考察像素T[i+m+d₁-1]，计算d₂的值；

若像素T[i+m+d₁-1]不在模式窗口P中，则考察其下一像素T[i+m+d₁]是否与P[1]相同，若相同，则将模式窗口P右移m+d₁位，否则移动m+d₁+1位；

若像素T[i+m+d₁-1]在模式窗口P中，则计算d₂的值是否为1：

当d₂≠1时，将模式窗口P右移d₁+d₂-1位；

当d₂＝1时，考察字符T[i+m+d₁-1]是否与P[1]相同，若不同，则将模式窗口P右移，使得T[i+m+d₁-1]与T[i+m+d₁-1]像素的前一位对齐，否则按d₂时右移。

进一步地，步骤4包括：

嵌入式计算机根据世界坐标输出机械臂的控制参数，驱动机械臂抓取目标；

上述机械臂运动采用五次多项式插值进行机械臂运动轨迹规划，五次多项式插值的角位移、角速度和角加速度的函数表达式为：

增加对起止点角速度的约束(t₀＝0)，得到以下约束条件：

通过解方程得到a₀，a₂，a₃，a₄，a₅的值。

有益效果：与现有技术相比，本发明具有如下显著的优点：

(1)改进的BM匹配算法经过对模式窗口P移动规则的调整，加快匹配速度，减少目标定位的时间；

(2)BM立体匹配时间更短，降低机械臂的操作误差，效率更高，机械臂抓取速度更快；

(3)降低舵机齿轮旋转时角速度在单位时间内的波动，使其变化平稳，降低机械臂的磨损。

附图说明

图1为本发明的方法流程图；

图2为本发明的基于颜色特征识别流程图；

图3为本发明的系统框架示意图；

图4为本发明的系统结构示意图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

如图1所示，一种基于BM立体匹配机械臂控制方法，包括以下步骤：

S1：上位机2发送开启指令，系统初始化；

S2：单目相机3获取环境信息，建立全局空间定位模型，实现障碍物的识别和基于颜色特征检测对目标的粗定位，同时将环境信息上传至上位机2；

S3：双目摄像头进行图像采集，采集后的图像经过极线校正操作得到在同一水平线上的左右视图，降低立体匹配的复杂度，接着进行BM立体匹配，经过三维重建得到目标的三维坐标；

S4：目标位置转化为世界坐标，将坐标转化为机械臂5控制参数，驱动机械臂5抓取目标。

针对目标定位时间长的问题，使用基于BM匹配算法的改进BM匹配算法，在不影响误匹配率的基础上减少匹配次数，缩短匹配时间，有效的加快了目标定位时间；针对机械臂5的磨损问题，采用5次多项式插值进行运动规划，通过降低角加速度的值，使角速度变化平稳，降低驱动机械臂5舵机齿轮的磨损。

其中，S1的具体步骤为：

在上位机2上点击启动按钮，指令通过5G信号下的局域网实时传送到嵌入式计算机1，嵌入式计算机1收到指令，将单目和双目相机4初始化，输出控制参数至机械臂5，使机械臂5位姿处于初始状态；

如图2所示，S2的具体步骤为：

单目相机3启动，基于颜色特征进行目标检测，将RGB色彩模型转换为HSV色彩模型，对单目相机3摄取的图像进行HSV图像阈值分割，将可能的目标与背景分开，对分割后的图像进行腐蚀操作和膨胀操作去噪点，通过目标物的外形区别目标物的轮廓和其他物体；

同时，将单目相机3摄取的周围环境信息通过5G信号下的局域网上传至上位机2；目标物的外形包括形状和大小；通过目标物的外形区别目标物的轮廓和其他物体，是将所求得的所有轮廓半径、长、宽与目标物的半径、长、宽进行对比；

S3的具体步骤为：

S3.1：双目相机4获取左右图像；

首先，采用bouguet极线校正算法对摄取的左右图像进行校正，该算法是将OPenCV求解出来的旋转和平移矩阵分解成左右相机各旋转一半的旋转和平移矩阵R1,T1与R2,T2。分解的原则是使得，左右图像重投影造成的畸变最小，左右视图的共同面积最大。

S3.2：进行BM立体匹配；

图像中的每个像素点可以用像素值来表示，在匹配时，在左视图中选中目标并作为模式窗口P，选取右视图中同一行所在所有像素即待匹配像素组T进行匹配。将二维像素组按顺序排列为一维；其中，P共有m个像素，位置记为P[1]，P[2]，…，P[m]，先将T与P进行左对齐，然后进行从右向左比较，具体匹配步骤如下：

在匹配过程中需要储存两个右移位置值d₁和d₂，d₁为当像素不匹配时，首先计算模式串最右端像素对应的像素T[i]的下一次右移位置，d₂为计算T[i+1]确定的下一次右移位置。d₁和d₂的计算规则如下：

(1)首先，考察模式窗口P的末像素P[m]与对应的待匹配像素组T[m]是否匹配。若不匹配，则考虑T[m]的下一像素T[m+i]，且利用坏像素规则，求得d₁的值；

(2)其次，考察像素T[i+m+d₁-1]，计算d₂的值。

若像素T[i+m+d₁-1]不在模式串中，则考察其下一像素T[i+m+d₁]是否与P[1]相同。若相同，则将模式窗口P右移m+d₁位，否则移动m+d₁+1位。

若像素T[i+m+d₁-1]在模式串中，则计算d₂的值是否为1。

i)当d₂≠1时，将模式串右移d₁+d₂-1位；

ii)当d₂＝1时，考察字符T[i+m+d₁-1]是否与P[1]相同。若不同，则将模式右移，使得T[i+m+d₁-1]与T[i+m+d₁-1]像素的前一位对齐，否则按d₂时右移。

S3.3：进行目标的三维重建；

根据双目测距原理，由已知的相机焦距f，视差d，光心距离B可求得深度信息Z。还需获得目标物的横纵坐标X、Y，则需要知道c_x和c_y。设在左视图中的目标点为(x,y)，视差为d，在以左相机坐标系下的目标点世界坐标为(X,Y,Z)，则用Q来表示目标点的物理坐标与世界坐标间的关系，通过下式将图像坐标映射到相机三维空间中；

Q*[x,y,d,1]^T＝[X,Y,Z,W]^T

深度信息Z的计算公式为：

其中，d＝X_L-X_R，X_L为目标上一点在左相机中成像的图像坐标，X_R为同一点在右相机中成像的图像坐标。

S4的具体步骤为：

嵌入式计算机1根据世界坐标输出机械臂5的控制参数，驱动机械臂5抓取目标；

上述机械臂5运动采用五次多项式插值进行机械臂5运动轨迹规划，五次多项式插值的角位移、角速度和角加速度的函数表达式为：

增加对起止点角速度的约束(t₀＝0)，得到以下约束条件：

通过解方程得到a₀，a₂，a₃，a₄，a₅的值；

如图3所示，一种基于BM立体匹配机械臂5控制系统包括上位机2、嵌入式计算机1、单目摄像头模块、双目摄像头模块和机械臂5。

如图4所示，一种基于BM立体匹配机械臂5控制系统主控采用嵌入式计算机1，单目相机3、双目相机4与机械臂5通过USB2.0接口连接嵌入式计算机1；单目相机3安装在机械臂5与目标的正上方；双目相机4安装在机械臂5附近；嵌入式计算机1至少拥有三个USB接口，且拥有无线传输模块，其传输速度满足5G网络速度；使用5G信号可为5G手机开启的热点，5G覆盖范围为50m；嵌入式计算机1和上位机2所在的计算机连接上述热点，处于同一局域网下。

使用的嵌入式计算机1为飞凌RK3399，处理器为瑞芯微公司的RK3399，具备2个ARMCotex-A72内核，主频1.8GHz；4个ARM Cotex-A53内核，主频1.4GHz；GPU采用Mali-T864，支持OpenGL ES1.1/2.0/3.0/3.1，OpenVG1.1，OpenCL，DX11；板载2GB LPDDR3RAM，16GB eMMCROM；嵌入式计算机1拥有电源输入接口，输入电压为12V，单目相机3模块、双目相机4模块和机械臂5均通过各自与嵌入式计算机1连接的USB接口供电。嵌入式计算机1使用linux系统，并在该系统上安装OpenCV平台。

使用的单目相机3为全瑞视讯生产的USB1080P01，最大分辨率为1920*1080，镜头采用无畸变处理，支持USB2.0快速传输，支持多系统无需驱动。

使用的双目相机4为金乾象公司生产型号为KS861，最大分辨率为2569*960，采用USB2.0连接方式，支持linux系统，无需驱动。

双目相机4安装时，光心连线尽可能的与水平线平行，减少校正的压力，同时也减少深度信息的计算误差；机械臂5拥有控制核心，控制核心根据参数分别控制每一个舵机的运动；机械臂5拥有6个舵机，每一个舵机拥有固定的ID号，具有四种舵机模式和四种马达模式，舵机模式为180°/270°顺逆转动四种，马达模式为定圈/定时顺逆转动四种。

在嵌入式计算机1中设有目标检测模块、极线校正模块、立体匹配模块和轨迹规划模块：目标检测模块中设置有基于颜色特征的目标检测算法；极线校正模块中设置有bouguet极线校正算法；立体匹配模块中设置有改进的BM立体匹配算法；轨迹规划模块中设置有5次多项式插值算法。

其中，目标检测模块输入端与单目相机3输出端通过USB连接；极线校正模块和立体匹配模块的输入端与双目相机4的输出端通过USB连接；轨迹规划模块输出端与机械臂5控制核心电性连接。

机械臂5抓取目标的具体过程为：上位机2发送启动指令，系统启动，完成初始化，单目相机3获取环境信息，在目标检测模块中检测并识别目标，同时将环境信息上传至上位机2；然后双目相机4摄取图像，在极线校正模块将左右图像处于同意平面且在同一水平线上，接着在立体匹配模块进行图像匹配，在左视图中选取目标窗口，在右视图中进行匹配，接着进行三维重建；最后输出控制参数至机械臂5，驱动机械臂5抓取目标。

实验采用Windows10操作系统，AMD2600 3.4GHz，16GB内存的台式电脑，在VisualStudio2013编程平台和OpenCV3.0图像处理库协同编程实现改进BM匹配算法。实验使用Middlebury公开数据集中的Cones、Teddy、Tsukuba、Venus四组图片，同时与BM算法、SGBM算法比较算法的的运行时间。实验结果如下表1所示。

表1立体匹配时间表

从实验数据可以看出，BM算法和改进的BM算法运行时间皆低于SGBM算法，原因是SGBM算法的复杂度比BM算法的高。从表中还可以看出，改进的BM匹配算法经过对模式窗口P移动规则的调整，确实能加快匹配速度。

在相同的打光、相机曝光情况下，抓取小木块60次，记录每次立体匹配时间和抓取时间，最终取平均值，结果如表2所示。

表2实验结果

在60次的抓取实验中，均能成功抓取，其抓取的平均时间为12.358s，平均立体匹配时间为5.167s。满足系统需求。由于标定的方法、机械臂5装配以及电机精度等原因，机械臂5产生的误差如表3所示。

表3机械臂5误差

Claims (10)

1.一种基于BM匹配机械臂控制系统，其特征在于，包括用于发送启动指令的上位机、用于处理并输出控制参数的嵌入式计算机、用于目标物拍摄识别的单目相机、双目相机和机械臂；所述单目相机安装在机械臂和目标物的正上方，所述双目相机安装在机械臂附近；所述单目相机、双目相机和机械臂与嵌入式计算机和上位机信号连接。

2.根据权利要求1所述的基于BM匹配机械臂控制系统，其特征在于，所述嵌入式计算机和上位机处于同一局域网下。

3.根据权利要求1所述的基于BM匹配机械臂控制系统，其特征在于，所述双目相机的光心连线与水平线平行。

4.根据权利要求1所述的基于BM匹配机械臂控制系统，其特征在于，所述单目相机、双目相机、机械臂通过USB接口连接嵌入式计算机，所述嵌入式计算机至少拥有三个USB接口。

5.根据权利要求1所述的基于BM匹配机械臂控制方法，其特征在于，包括如下步骤：

步骤1：上位机发送开启指令，系统初始化；

步骤2：单目相机获取环境信息，建立全局空间定位模型，实现障碍物的识别和基于颜色特征检测对目标的粗定位，同时将环境信息上传至上位机；

步骤3：双目摄像头进行图像采集，采集后的图像经过极线校正操作得到在同一水平线上的左右视图，使用改进的BM立体匹配，经过三维重建得到目标的三维坐标；

步骤4：目标位置转化为世界坐标，将坐标转化为机械臂控制参数，驱动机械臂抓取目标。

6.根据权利要求5所述的基于BM匹配机械臂控制方法，其特征在于，所述步骤1包括：

上位机启动，指令通过5G信号下的局域网实时传送到嵌入式计算机，嵌入式计算机收到指令，将单目和双目相机初始化，输出控制参数至机械臂，使机械臂位姿处于初始状态。

7.根据权利要求5所述的基于BM匹配机械臂控制方法，其特征在于，所述步骤2包括：

单目相机启动，基于颜色特征进行目标检测，将RGB色彩模型转换为HSV色彩模型，对单目相机摄取的图像进行HSV图像阈值分割，将可能的目标与背景分开，对分割后的图像进行腐蚀操作和膨胀操作去噪点，通过目标物的外形区别目标物的轮廓和其他物体；单目相机摄取的周围环境信息通过5G信号下的局域网上传至上位机；其中，目标物的外形包括形状和大小。

8.根据权利要求5所述的基于BM匹配机械臂控制方法，其特征在于，所述步骤3包括：

步骤3.1：双目相机获取左右图像：采用bouguet极线校正算法对摄取的左右图像进行校正，该算法是将OPenCV求解出来的旋转和平移矩阵分解成左右相机各旋转一半的旋转和平移矩阵R1,T1与R2,T2；

步骤3.2：BM立体匹配：图像中的每个像素点用像素值来表示，在匹配时，左视图中选中目标并作为模式窗口P，选取右视图中同一行所有像素即待匹配像素组T进行匹配；将二维像素组按顺序排列为一维；其中，P共有m个像素，位置记为P[1]，P[2]，…，P[m]，将T与P进行左对齐，然后进行从右向左比较；

步骤3.3：目标的三维重建，已知相机焦距f，视差d，光心距离B可求得深度信息Z，深度信息Z的计算公式为：

设在左视图中的目标点为(x,y)，视差为d，在以左相机坐标系下的目标点世界坐标为(X,Y,Z)，用Q来表示目标点的物理坐标与世界坐标间的关系，通过下式将图像坐标映射到相机三维空间中；

Q*[x,y,d,1]^T＝[X,Y,Z,W]^T

其中，Q为深度视差映射矩阵；x，y分别为目标点在物理坐标中的横坐标和纵坐标；W为转换因子；X/W，Y/W，Z/W分别为目标点转换后的世界坐标的横坐标，纵坐标和竖坐标；

其中，d＝X_L-X_R，X_L为目标上一点在左相机中成像的图像坐标，X_R为同一点在右相机中成像的图像坐标。

9.根据权利要求8所述的基于BM匹配机械臂控制方法，其特征在于，所述步骤3.2包括：

在匹配过程中需要储存两个右移位置值d₁和d₂，d₁为当像素不匹配时，计算模式窗口P最右端像素对应的像素T[i]的下一次右移位置，d₂为计算T[i+1]确定的下一次右移位置，d₁和d₂的计算规则如下：

步骤3.2.1：考察模式窗口P的末像素P[m]与对应的待匹配像素组T[m]是否匹配，若不匹配，则考虑T[m]的下一像素T[m+i]，且利用坏像素规则，求得d₁的值；

步骤3.2.2：考察像素T[i+m+d₁-1]，计算d₂的值；

若像素T[i+m+d₁-1]不在模式窗口P中，则考察其下一像素T[i+m+d₁]是否与P[1]相同，若相同，则将模式窗口P右移m+d₁位，否则移动m+d₁+1位；

若像素T[i+m+d₁-1]在模式窗口P中，则计算d₂的值是否为1：

当d₂≠1时，将模式窗口P右移d₁+d₂-1位；

当d₂＝1时，考察字符T[i+m+d₁-1]是否与P[1]相同，若不同，则将模式窗口P右移，使得T[i+m+d₁-1]与T[i+m+d₁-1]像素的前一位对齐，否则按d₂时右移。

10.根据权利要求5所述的基于BM匹配机械臂控制方法，其特征在于，所述步骤4包括：

嵌入式计算机根据世界坐标输出机械臂的控制参数，驱动机械臂抓取目标；

上述机械臂运动采用五次多项式插值进行机械臂运动轨迹规划，五次多项式插值的角位移、角速度和角加速度的函数表达式为：

增加对起止点角速度的约束(t₀＝0)，得到以下约束条件：

通过解方程得到a₀，a₂，a₃，a₄，a₅的值。

Images