基于双目立体视觉的目标空间坐标计算及姿态估计

黄青丹， 何彬彬， 宋浩永， 饶 锐， 赵宝玉， 王国库

(1. 南方电网中低压电气设备质量检验测试重点实验室∥广州供电局有限公司电力试验研究院， 广州 510000；2. 河南中分仪器股份有限公司， 郑州 450000)

在实际工程应用中，经常需要通过非接触的方式实现目标空间坐标及姿态的测量，基于双目立体视觉的测量方法具备效率高、系统结构简单、成本低、非接触式等特点，可以实现目标空间三维信息的测量. 20世纪80年代，麻省理工人工智能实验室的MARR提出MARR视觉理论，推动了机器视觉理论的快速发展[1]. 双目立体视觉理论通过视差原理来还原空间物体的深度信息[2]，已被应用于具体应用场景，例如：在无人驾驶的起重机上安装双目立体视觉系统，为起重机吊运目标物体提供了具体的三维空间信息[3]；在月球漫游车模型上搭载双目立体视觉系统[4]，对目标进行切割、抓取等操作；利用双目立体视觉系统测量光学元件的位置姿态，可建立基于双目立体视觉的孔径测量系统[5]. 近些年来双目立体视觉理论及技术得到了长足发展，然而在实际应用中仍然存在鲁棒性差的问题[6]. 需要针对不同的应用背景，设计相应的解决方案.

在电力仪表的全自动化检定领域，需要对仪表进行自动抓取与搬运，其中亟需解决的问题是对仪表空间位置及姿态识别计算的准确性. 虽然基于双目立体视觉与机械臂相配合的系统已在很多场合得到应用，但目前应用于仪表自动识别并抓取的研究少有报道. 北京东方计量研究所目前公布了最新研制的数字式仪表检定平台，通过夹具固定仪表位置，根据仪表型号事先标定相应的空间位置；在变电站巡检系统中，虽然通过模板匹配实现仪表平面内位置的识别，但不能获得其深度信息. 本文提出了一种实用化的目标空间坐标及姿态估计方法，并应用于电力仪表的自动抓取系统，实验结果表明，所提出的方法性能稳定，能满足实际应用需求.

1 研究方法

1.1 双目立体视觉成像模型

双目视觉的基本原理是利用双摄像头模拟人眼的定位. 分析图像中相同目标成像的差异，可以得到目标物的三维位置信息. 图1模型代表了理想状态的双目立体视觉系统，系统成像共面的2个相机(C1、C2)具有一致的焦距且光轴平行[7]. 在左相机坐标系中，空间点P的空间位置坐标为(XC,YC,ZC)，而(uL,vL)、(uR,vR)表示图像像素在左右图像坐标系中的坐标.

图1 双目立体视觉模型

根据几何知识能够推导特征点P和其在左相机图像中投影点的位置关系：

(1)

其中，(u0,v0)是真实坐标原点在像素坐标系中的坐标，f为相机焦距. 定义d=uL-uR作为视差，对于左相机的相机坐标系，特征点P的三维坐标为

(2)

然而，在实际工程的双目视觉系统中，2个成像平面以及焦距难以保持一致，所以，如果知道2个相机的相对位置关系，可实施校正[8]，使校正模型满足理想状态下的成像要求.

1.2 双目相机的标定

基于二维棋盘格图像校准板的相机视觉算法标定方法[9]结合了传统校准算法的优点，具有良好的鲁棒性和自校准算法，可以满足校准对象的依赖性要求[10]. 不需要标定板移动的参数，仅需要标定模板在2个方向上的移动图像即可进行标定. 该方法标定精度高、成本低，已被广泛用于桌面视觉系统.

该标定方法的基本步骤[11]：(1)将棋盘的图像当作标定板，打印并将其附着在刚性平面上；(2)分别从多个角度拍摄标定板；(3)提取每幅图像中的特征点，同时得到特征点在2个关键坐标系中的位置；(4)使用线性模型提取摄像头的内部参数以及每次图像的外部参数；(5)借助畸变模型优化相机内外部参数，得到所有参数.

本文利用MATLAB对双目相机进行标定. 从多个角度用2个相机获取20幅平面标定板的双图像组(图2展示20幅左相机图像)，对获得的图像通过MATLAB的Stereo Calibration工具完成标定.

图2 标定使用的左相机图像

标定得到平移向量T为[-71.409 21，1.071 99，-0.194 66]，2个相机的间距为72 mm，平移参数经过标定取得，结果为72.526 17 mm，大体上持平，同时，剩余维度的偏移很小. 所以，双目相机标定的结果可作为校正参数.

1.3 仪表图像的立体匹配

仪表图像的立体匹配主要采用半全局立体匹配算法，通过在不同方向扫描线上一维的局部寻优过程来逼近二维图像的全局寻优过程. 主要步骤为计算单个像素点的匹配代价. 通过线上的平滑约束和多个方向上匹配成本的聚合阶段，构建逼近的全局能量函数. 使用优化方法最小化该能量函数来估计视差[12]. 半全局匹配方法在确保良好精度的前提下，具有相对低的时间复杂度，同时，并行处理单元可使算法的计算速度得到提高. 由于函数对相机采样效果不敏感[13]，本文使用BT函数计算像素点的匹配成本. 实验中以仪表图像为目标，算法的结果视差图通过实验得到(图3).

图3 立体匹配效果

在定义半全局匹配方法的能量函数时，增加额外的视差平滑约束：

(3)

式中，第一项为数据项，是基于视差的像索点匹配成本累计和；第二、三项为平滑项，P1、P2分别代表NP邻域中视差变化的匹配点P的惩罚系数(满足P1>P2). 通常，为了避免引起边界模糊匹配，需要依据P′2=P2/(Irp-Irq)像素灰度的变化自适应调节P2[14].

针对二维图像上的全局最小NP完全问题求解能量函数，需要使用多方向的一维平滑来逼近二维情况的平滑约束. 半全局匹配算法吸收了扫描线最优算法的思想[15]：沿着8条不同的扫描线分别计算，然后聚合沿扫描线存储的扫描成本. 以迭代的方式得到视差计算：采用右视差图像纠正右图像，进而完成匹配并生成新的视差图像. 还可以使用分层计算的方法：首先在半分辨率的情况下计算视差图像作为初始值，然后使用所获得的视差图像执行递归计算[16]. 以左图像为基准完成相应的左图像和右图像的匹配，若2次匹配视差结果不相同则视为无效匹配. 与此同时，利用视觉系统的左相机收集图像并进行处理. 采用背景差分的方法在预先存储了背景图像的前提下分割采集到的图像，可获取仪表的轮廓. 图4为仪表轮廓背景差分方法的流程图.

图4 背景差分法提取仪表轮廓的流程图

Figure 4 The flow chart of instrument outline extraction with the background difference method

采用上述流程对采集的图像进行处理，去除处于图像边缘区域的轮廓，保留连通域中最大部分作为进行仪表定位和抓取的工作区域，可得到仪表的轮廓信息，具体包含：仪表的轮廓大小、中心点坐标和倾斜角度[7]. 然后，在原始图像中显示仪表的轮廓信息(图5).

图5 仪表轮廓的提取结果

结合视差可计算仪表中心点的三维坐标. 仅获得仪表中心点的三维坐标仍然不能描述仪表在空间中的姿态，本文通过增加仪表在水平面的倾斜角度，结合仪表的中心点三维坐标，可以完整地描述仪表处于水平面时的空间姿态.

1.4 实验设置

本文将所设计的双目立体视觉系统应用于电力仪表的自动抓取，实现仪表空间坐标计算及姿态估计，系统实物如图6所示，机械臂末端安装夹爪及双目立体视觉系统，通过双目立体视觉系统识别仪表空间坐标及姿态，进而实现对仪表的自动抓取.

图6 电力仪表抓取系统的实物图

双目视觉系统首先采集仪表图像，然后提取特征点，进而实现立体匹配. 仪表特征点集中在仪表表盘目标区域，且仪表外壳区域特征点的分布没有统计意义. 所以，仪表中心点的三维坐标能够通过表盘区域中匹配点的视差得到. 通过查阅仪表规格知道表盘区域较外壳突出16 mm，那么计算所有匹配点的视差得到坐标Z最大的点，则判断为表盘区域的点. 为了得到仪表中心点坐标，可以通过这个点的视差得到，对Z进行补偿能够得到中心点的三维坐标. 使用精度为0.01 mm的手动三维坐标测量仪测量坐标，将其作为标定的世界坐标，视为坐标真值，与双目视觉系统匹配得到的三维坐标对比，进行坐标误差计算.

2 结果与讨论

2.1 匹配误差分析

系统与目标仪表检测区域的相对距离为400 mm，考虑不同仪表的不同姿态，进行10次实验，实验所得仪表中心点的三维坐标、坐标真值以及两者间的误差如表1所示.

表1 匹配结果及误差

当双目立体视觉系统与仪表的距离分别为380、400、420、440、460 mm时，计算仪表中心点三维坐标中Z的绝对误差如图7所示，其平均相对误差不超过1.84%. 根据趋势，图中Z的绝对误差随着仪表深度距离的增加而增大，当双目立体视觉系统与仪表的距离增加到460 mm时，误差达到最大(13.96 mm)，此时已不满足系统的误差要求. 而在实际操作情况中，在保证检测速度的前提下，通过转动仪表检测平台，设置实验将双目立体视觉系统的相对距离设置为 400 mm，即相机与不同仪表的距离为400 mm，对仪表中心点三维坐标进行立体匹配. 经过大量的测试实验，结果表明：本文设计的双目立体视觉系统运行稳定、鲁棒性好、系统误差满足要求，可实现对仪表空间坐标的计算及姿态估计，进而实现对仪表的准确抓取.

图7 不同距离下三维坐标Z的误差

Figure 7 The errors inZof three-dimensional coordinates at different distances

2.2 与其他方法的对比及分析

目标空间位置识别的准确性主要取决于采用的立体匹配算法，将本文提出的方法与基于尺度不变特征变换(SIFT)特征算子的立体匹配方法进行比较. 基于SIFT特征算子的立体匹配是典型的基于特征的匹配算法，仅需要对图像中的一部分像素点计算视差，因此其计算量较小. 双目立体视觉系统与仪表的深度距离为400 mm时采集10个不同姿态的仪表图像，不同方法的平均坐标误差及计算时间如表2所示. 虽然所提出的方法耗时比SIFT特征匹配方法的秏时略长，但准确性显著优于SIFT特征匹配方法，这是因为基于SIFT特征的匹配方法属于稀疏匹配，匹配到的特征点位置具有随机性，但是对仪表中心点坐标计算的精度不如本文所提出的方法.

表2 本文方法与SIFT特征匹配方法对比

Table 2 the comparison of the proposed method and the SIFT method

匹配方法平均坐标绝对误差/mm平均计算时间/s本文方法6.1568.101SIFT特征匹配方法8.4128.056

3 结论

提出了基于双目立体视觉的目标空间坐标计算及姿态估计方法，并应用于电力仪表抓取系统. 首先在完成双目系统标定后，通过对左右相机采集的图像经过半全局立体匹配算法计算获得目标视差图；然后通过背景差分法获得仪表轮廓信息，包括仪表轮廓大小、轮廓中心点坐标以及相对垂直方向的倾斜角；最后通过仪表中心点的像素坐标和仪表的视差图计算出仪表中心点在空间中的三维坐标，将该坐标与仪表的倾斜角度结合起来，可以描述仪表在空间中的任意水平姿态. 实际测试结果表明：所设计的双目视觉系统性能稳定，可以实现仪表空间坐标的计算及姿态估计，为机械臂抓取仪表奠定了基础，具有重要的应用价值.