基于双目视觉的机械臂位姿估计及目标自动定位系统

曹阳，丁一岷

（国网嘉兴供电公司，浙江嘉兴 314001）

当前机器人在自动化生产线上得到了广泛的应用，机械臂位姿有效检测是生产线自动化程度和生产效率提高的关键技术之一[1]。若要对工件进行自动化抓取和包装，必须实现机械臂位姿的自动估计。传统使用的关键性技术是基于目标特性约束的激光测距和单目摄像机估计技术，该技术利用机械臂末端的三点式激光测距传感器进行位姿估计，但易受距离影响，估计方法稳定性差；使用基于目标特征约束的单眼照相机位姿估计法，根据提取的对接圆和轮廓特征估计出机械臂的位置和姿态初始值，但易受噪声影响，估计精度低。针对这一问题，设计了基于双目视觉的机械臂位姿估计及目标自动定位系统。

1 系统硬件结构设计

双目视觉采用非经典收敛式双目结构，系统由上、下两个处理单元组成，每个处理单元处理通道的图像数据[2]。下处理器采用DSP 智能摄像机，上处理器采用PC 主机[3]。系统硬件结构如图1 所示。

图1 系统硬件结构

如图1 所示，该摄像机是以DSP 为核心的图像处理平台，可以完成图像的采集和处理[4]。相机采用FPGA 实现CCD 的时序控制和数据采集，同一外部控制信号触发两相机进行同步图像采集。采用以太网技术，实现了摄像机与主机之间的通信，即摄像机之间的图像传输，最后由上位机获得机械臂的姿态数据并控制其移动[5-7]。

1.1 高压柜上按钮

高压柜上的按钮是低压电器，可手动或自动复位[8-9]。按钮通常是在电路中发出一条启动或停止指令，以控制接触器、继电器等线圈的电流，图2 中显示了它的结构[10]。

由图2 可知，按钮会有不同的标志和颜色。一般来说，红色代表停止，绿色代表开启。急停键是红色的蘑菇头，必须配备金属保护环[11]。扣圈应高于按钮盖，以防止误触按钮导致误操作。高压柜上按钮操作过程为：通电后按停止键，松开后再关闭。按下启动键关闭，释放后继续断开[12]。

图2 高压柜上按钮结构

1.2 FPGA

FPGA 是适用于数字集成电路的通信协议芯片，其基本结构如图3 所示。

图3 FPGA芯片基本结构

由图3 可知，FPGA 的内部组件包含了可配置逻辑块（CLB）、寄存器（IOB）、颜色转换模块（CB）、布线资源结构模块（SB）等，这些模块可以组合使用，通过可编程序逻辑资源连接多个模块，形成FPGA 电路，具有高速并行计算速度快、功耗低的优点。

2 系统软件部分设计

2.1 纹理图像和三维点云配准

为了引导机械臂抵达指定目标位置并完成操作任务，需要利用双目视觉将操作目标的空间位置信息传递给机械臂[13-14]。利用双目摄像机进行立体定标，获得系统内外参数。通过摄像机捕获双目图像，根据系统内外参数进行立体校正[15]。经修正的双目图像进行立体匹配，由此获得视差图[16]。由此构建双目视觉模型如图4 所示。

图4 双目视觉模型

由图4 可知，两台相机像平面严格向前方平行线对准，两台物理焦距f一致的相机之间的基线距离为T，像元大小为a。在三维立体空间中的某点A(x,y,z)在两台相机成像平面中的映射点分别为O1和O2，对应双目视觉模型中的坐标分别为(a1,v)和(a2,v)，由此可得到双目视觉差为：

采用二维特征匹配方法，建立纹理图像和校正后的左双目图像的对应关系。具体的匹配过程如下：从图像中提取特征点，通过特征点的匹配来求解单应性矩阵，并将其转化为同一坐标系。

1）特征点匹配

设纹理图像中的某点像素坐标为(x,y)，与其相对应的左目像素坐标为(x′,y′)，由此得到纹理图像与左目像素齐次坐标模式下对应关系：

式（2）中，W表示单应性矩阵；λ表示映射尺度因子，是一个常数。

2）单应性矩阵求解

单应性矩阵的计算公式为：

式（3）中，单应性矩阵W是通过特征点匹配后计算得到的，通过式（2）可得到纹理图像与双目视觉对应的关系。

3）三维坐标与纹理图像匹配

三维立体空间中的某点A(x,y,z)坐标与纹理图像中二维坐标(x,y)匹配关系为：

式（4）中，Q-1表示双目视觉重建的投影逆矩阵；WD表示单应性矩阵中的逆投影结构，计算公式为：

式（5）中，采用左双目匹配的桥形图，可以得到匹配纹理图像的二维坐标和双目三维坐标点云模型的关系。通过对匹配关系的确定，将单视纹理图像映射到双目视觉模型，得到目标中心的姿态信息。

2.2 目标旋转中心和方向向量确定

由于机器人本身位置定位存在一定误差，变电站所内光照情况也会发生变化，因此不同操作任务下拍摄的图像会存在较大差别。为了排除光线变化、位置变动以及柜体表面相似外观测点的干扰，采用基于模板匹配的粗定位在纹理图中定位目标的大致位置，再进一步根据目标模板图像，识别物体位姿。

2.2.1 目标模板图像形成

构建目标模板图像库模块，针对目标的三维模型，利用球形摄像机对目标视场进行多角度、多距离的投影。以目标三维模型的重心为基准，设定坐标系的原点，该点记为P。转化空间坐标，将基准坐标系转化为以观测点为P′原点的坐标系，两个坐标系原点之间的距离为Z轴。之后从以P′点为原点的坐标系中获取三维模型投影目标模板图像，记录该目标的三维位置P′点到目标三维模型之间的距离，由此生成目标模板图像数据集。

2.2.2 机械臂位姿识别

将目标模板图像放入存储中，然后，对经过预处理的二维目标图像进行目标检测和识别，并采用相似性度量方法将模板图像与源图像进行比较，由此计算机械臂位姿坐标：

式（6）中，ori(O,r)表示目标在模板图像上r横坐标梯度；ori(I,c+r)表示目标在二维图像上c+r横坐标梯度；c表示目标模板图像中心点。在堆叠相同对象的二维图像时，根据最终计算结果的值来选择得分最高的对应位置。

2.3 剔除噪点区域

利用3×3 邻域高斯模板对图像数据进行平滑输入，对二维高斯函数进行采样得到高斯模版，在图像处理中得到广泛应用。给出图像数据g(i,j)的特定表达式如下：

经过滤波平滑后的图像数据g(i,j)对图像中噪声的侵袭具有一定的鲁棒性，利用该方法提取方向纹理特征，机械臂位姿估计及定位结果更加精准。

3 实 验

3.1 实验装置

实验装置是基于数字处理器智能相机的双目视觉平台，使用UR5 机械臂，如图5 所示。

图5 机器人机械臂实物图

使用数字处理器智能相机采集相关图像内容，利用标定参数校正，将校正后的图像发送到相机中，由此完成双目图像视差估计，并将结果传回到PC 主机。利用数据统计结果，实现目标位姿估计。

3.2 实验数据分析

使用机械臂精确控制目标的位移和旋转运动，得到姿态刚体变换参数信息，即位姿。实验选用焦距为8 mm 的镜头，工作距离为450 mm，检测的物体为35 mm×20 mm×20 mm 矩形块，一共获取60 张图。实验测试集如表1 所示。

表1 实验测试集

3.3 实验结果与分析

分别使用激光测距和单目摄像机估计技术Q1、单眼照相机位姿估计法Q2 和基于双目视觉机械臂位姿估计及目标自动定位系统Q3 对机械臂运动坐标展开对比分析，结果如图6 所示。

图6 机械臂运动坐标估计精度对比分析

由图6 可知，使用激光测距和单目摄像机估计技术在从1 处到2 处的运动轨迹是曲线型的，变化幅度较大，尤其是在坐标点为（40.5,-17,26.5）时，差别最大；使用单眼照相机位姿估计法在从1 处到2 处的运动轨迹是曲线型的，变化幅度比第一种方法要小，但与预期轨迹差别较大，尤其在坐标点为（41.2,-16,28）时，差别最大；使用基于双目视觉机械臂位姿估计及目标自动定位系统从1 处到2 处的运动轨迹与预期轨迹基本一致，说明使用该系统机械臂运动坐标估计结果较为精准。

4 结束语

文中基于双目视觉的机械臂位姿估计及目标自动定位系统，利用双目视觉引导机械臂完成操作任务，采用基于模板匹配的粗定位在纹理图中定位目标的大致位置，根据目标模板图像，识别物体位姿，解决了传统定位系统定位精准度低的问题。