基于二维码的相机位姿求解

吴 飞， 介远航， 余神志

(武汉理工大学 机电工程学院， 湖北 武汉 430070)

二维码因其信息容量大，编码范围广，已被广泛应用于手机支付和信息查阅，极大方便了人们的生活.在定位技术领域，二维码也得到了广泛应用[1].在定位技术中，二维码可以很好地存储绝对位置信息，且信息获取简单易用，为机器人里程计提供的后验信息可用于修正机器人里程计数据.现已广泛应用于智能物流、机器人巡检等场景，是重要的定位方式之一[2].目前研究人员针对机器人定位问题提出诸多解决方案，例如，利用惯性传感器构建电子地图完成定位[3]，利用二维码实现机器人之间的相对定位[4].但惯性传感器累计误差较大，而传统二维码定位过程中，机器人相机识别二维码的位姿通常是未知的，传统二维码定位技术为机器人提供的定位信息不能实现机器人的精准定位.因此，在二维码为机器人提供位置信息的基础上，如果通过特征点匹配反解出相机位姿，那就可以为机器人提供更为精准、全面的位置信息，包括机器人的平移量和旋转量，进而实现机器人的精准定位.机器人位姿求解示意图如图1所示.

图1 机器人位姿求解示意图

机器人在全局坐标系下的旋转矩阵R和平移向量t定义如下：

R=R1×R2,

t=t1+t2，

式中：R1和t1分别为相机坐标系相对于二维码坐标系的旋转矩阵和平移向量；R2和t2分别为机器人坐标系相对于相机坐标系的旋转矩阵和平移向量.因此，利用二维码进行相机位姿求解的方法，不仅对动态相机识别二维码有重要的指导意义[5]，还可以为定位问题提供一定的技术支持.

1 标定二维码

笔者选取戴尔720p HD摄像头，对地面粘贴的二维码进行拍摄.为了保证计算效率，定义图像分辨率为480×640像素，同时选取二维码标准位置进行标定，以便下一步进行特征点匹配和位姿求解[6].标准位置定义如下：二维码中心像素位于图像中央，二维码边长为100像素，如图2所示.此时相机位姿定义为标准位姿，所对应的旋转矩阵为Rs，即三阶单位矩阵E.用激光测距仪测量相机光心至二维码中心距离为Zs=0.58 m，故定义平移向量ts=(0,0,0.58)，此位姿下的4个特征点定义为标准位姿特征点集Ps.

图2 二维码标准位置

2 标定相机

无论是在三维重建还是在机器视觉应用中，相机参数的标定都极为重要，标定结果的精度将直接影响获得图像数据的准确性，所以做好相机标定是进行图像处理的基础[7].相机标定的本质是获得世界坐标系下三维坐标到图像中像素点二维坐标的转化矩阵K，也称为相机内参矩阵.标定相机就是求解相机内参矩阵K中的未知量.K是一个三阶矩阵，未知量有4个.K的定义如下：

式中：fx为像素在x方向的缩放量；fy为像素在y方向的缩放量；cx为像素在x方向的平移量；cy为像素在y方向的平移量.世界坐标系到像素坐标系的坐标变换关系为

(1)

式中：Q为世界坐标系到相机坐标系的变换矩阵；u、v为像素在像素坐标系中的x、y坐标值；x0、y0和z0为像素在世界坐标系中的x、y、z坐标值.

笔者使用棋盘格和OpenCV内置API实现相机标定，棋盘格采用6个×8个角点，棋盘格边长为24 mm，通过相机标定可以得出内参矩阵K.

3 图像预处理

为了获得特征点，需要对获得的源图像进行预处理，笔者首先利用OpenCV库中的API算法接口对图像进行预处理，最终获得二维码边缘轮廓，以便下一步对边缘轮廓进行特征点筛选.

3.1 初步预处理

初步预处理包括对图像采用灰度转化、图像平滑和增强对比度.其目的如下：降低计算量[8]和颜色干扰；在保持良好边缘的情况下，对噪声进行消除和抑制；提高图像对比度，使目标区域更容易区分.初步预处理后的图像如图3所示.

图3 初步预处理后的图像

3.2 分组阈值分割

通过阈值分割可以使图像只保留二维码像素点.传统阈值分割是反复尝试，直至得到合适的阈值.但不同的拍摄角度、光照条件使得每个图像的最佳阈值不同，此时若仍采用传统阈值分割，需进行大量的尝试.由于本研究中的研究对象均为单色背景的二维码图像，随着阈值增大，二维码像素将逐渐显现，随着阈值的继续增大，最终趋于稳定.故可将阈值进行分组，选择方差最小的组为最佳组，再从最佳组中选择最接近平均数的阈值为最佳阈值.算法步骤如下：

1) 将[0,255]阈值分为16组，每组16个阈值，i为组数，可取[0,15],j为序数，可取[0,15]，由此可知第i组的第j个阈值为15×i+j.

2) 计算每个阈值所对应的二维码像素个数，设第i组的第j个阈值的二维码像素个数为Nij.

3) 设第i组的像素平均值为ni，方差为σi，计算公式为

(2)

(3)

4) 设最佳组的组数为m，得到

(4)

5)m组中距离像素平均值最小的阈值为最佳阈值.设最佳阈值为e,可得

(5)

综上步骤可求出该图像的最佳阈值为15m+e.分组阈值分割后得到的二值化图像如图4所示.

图4 分组阈值分割后的二值化图像

3.3 边缘求解

在进行图像边缘提取之前，采用形态学闭运算，对二维码内部进行黑色填充，以免在边缘检测时得到较多无用的轮廓信息，增加算法的计算量.图像边缘就是灰度值发生剧烈变化的地方，采用Canny算子可以较好地得到二维码图像的边缘轮廓[9].边缘求解后的图像如图5所示.

图5 边缘求解后的图像

4 特征点集求解方法

图像预处理得到了二维码边缘轮廓.边缘轮廓构成的点集称为边缘点集合.可从边缘点集合中通过循环遍历等算法提取二维码特征点.设4个二维码特征点构成的集合为待求位姿下特征点集T，用于与标准位姿下的特征点集Ps进行匹配，以求解位姿.

4.1 循环遍历算法

传统提取二维码特征点的方法是采用霍夫直线检测算法[10]，该算法的基本原理是首先检测出二维码的4条边界直线，然后求解出这些直线的交点，即特征点集T.但是由于霍夫直线检测算法需要对图像进行离散化处理，离散化程度较大，会造成图像失真、边界点的坐标误差变大，进而导致所匹配的直线较多，不能精确地获得4条边界直线；若离散化程度较小，会得到较为密集的离散点，降低算法的效率.同时，由于霍夫直线的筛选是采用投票制，阈值过大或过小依然会导致所筛选出的直线偏少或偏多，如图6所示.最佳投票阈值的参数设置需要大量试验，故采用霍夫直线检测算法去提取二维码特征点的鲁棒性较差.

图6 霍夫直线筛选情况

可针对二维码特征点提出循环遍历算法，步骤如下：通过对像素点从上到下逐行遍历，从左到右逐列遍历，总能找到第1个特征点，定义为T1；同理，对像素点从下到上逐行遍历，从右到左逐列遍历，能找到第2个特征点，定义为T2. 同时，正方形的二维码边缘经过透视变换后得到的图形是凸四边形.由几何关系可知，通过循环遍历算法识别出的2个特征点正好是原二维码对角方向的一对特征点，如图7所示.

图7 特征点T1和T2位置示意图

4.2 最大距离算法

通过循环遍历算法，可以得到一对在对角方向的特征点，其坐标分别为T1(x1,y1)和T2(x2,y2)，而剩余2个特征点坐标为T3(x3,y3)和T4(x4,y4)，它们为距离T1和T2两点间直线L的最远点.根据已知2个特征点的坐标分别为T1(x1,y1)和T2(x2,y2)，采用式(6)可得出通过这2个特征点的直线L的方程，即

(y2-y1)x-(x2-x1)y+x2y1-x1y2=0.

(6)

然后，对图像边缘点进行遍历，设第i个边缘点坐标为Pi(Xi,Yi).遍历到第i个边缘点Pi时，点Pi到直线L的距离di定义如下：

(7)

若点Pi在L上方，则di>0；若点Pi在L下方，则di<0；若点Pi在L上，则di=0.

di=(y2-y1)Xi-(x2-x1)Yi+x2y1-x1y2.

(8)

由于特征点T3和T4距离直线L最远，所以di的最大值dmax和最小值dmin所对应的特征点即为T3和T4.至此，可得到二维码的特征点T3和T4.特征点集T示意图如图8所示.

图8 特征点集T示意图

4.3 边缘失真优化算法

二维码边缘倾斜程度较大时，总能筛选出正确的特征点集T.但如果二维码边缘与图像边缘平行,且出现失真现象时，就会导致误匹配，如图9所示.失真现象是由于像素过低，导致二维码边界直线轮廓出现偏移.

图9 失真现象示意图

误匹配现象如图10所示.由于失真现象，通过循环遍历算法会首先遍历到T1的误匹配位置，造成误匹配.

图10 误匹配现象示意图

对于边缘失真问题，提出“像素突变”算法进行优化，算法的基本原理是对于失真图像进行逐行遍历，得到每行的首个边缘点，定义为行首边缘点.图11为行首边缘点集和局部视图.行首边缘点构成的集合定义为F，从上到下的第i个行首边缘点定义为Fi，通过“像素突变”算法，首先得到5个行首边缘点F1、F2、F3、F4和F5，如图11a所示.为方便观察，每个行首边缘点均配有局部视图，如图11b-f所示.经过预处理后的图像是二值化图像，即只有白色像素和黑色像素，边缘轮廓为白色像素，像素值为255，设(rowi,coli)为Fi在像素坐标系下的坐标，M[x][y]表示在像素坐标系下坐标为(x,y)的像素值.因此，Fi的坐标表示为(rowi,coli)，M[rowi][coli]表示Fi的像素值.通过局部视图可以看出，行首边缘点Fi的像素值M[rowi][coli]=255(白色).求解特征点的方法如下：判断下一个点像素值M[rowi][coli+1]是否突变为0(黑色)，若突变为0，特征点即为上一个行首边缘点，若不突变为0，则继续遍历行首边缘点，直至突变情况的出现.如图11所示，F2、F3、F4和F5的下一个像素点均没有发生突变，行首边缘点F1的下一个像素突变为黑色，故可得特征点为点F2.

图11 行首边缘点集和局部视图

通过“像素突变”算法的优化，利用循环遍历算法与最大距离算法，均能够正确找到特征点，以证明算法的准确性和可行性.图12为特征点求解试验中特征点提取前后对比.图12a是8次随机拍摄的图像进行的特征点提取，试验结果图12b所示，均能正确筛选出特征点.

图12 特征点集求解试验

5 位姿求解与修正

通过以上算法可以得到二维码的4个特征点T1(x1,y1)、T2(x2,y2)、T3(x3,y3)和T4(x4,y4).由于特征点均位于同一个平面上，利用特征点的单应性[11]，对4个特征点进行位姿求解.平面方程定义如下：

(x,y,z)nT+d=0.

式中：n为平面方程的法向量；d为平面方程的常数项.

根据式(1),进行坐标变换得到

T=K(RPs+t).

代入平面方程与特征点，可得

(9)

式中：R为标准位姿到当前位姿的旋转矩阵；t为标准位姿到当前位姿的平移向量.

T=HPs.

(10)

代入求得的4个特征点，与标准位姿特征点集Ps进行匹配[12]，可求解出当前相机位姿相对于标准位姿的旋转矩阵R和平移向量t.求解位姿后，通过透视变换，可将非标准位姿二维码进行矫正.与标准位姿二维码对比，若定位标志位置相同，则不必对旋转矩阵R进行修正.若不同，则需对旋转矩阵R进行修正.修正步骤如下：

1) 图13为区域定义示意图.图13中，标准位姿二维码的定位标志在区域1、2、3处.设i区域存在的黑色像素个数为bi，对于标准位姿二维码，min(bi)=b4.

图13 区域定义示意图

2) 对于矫正后的二维码：

若min(bi)=b1，则矫正角angle=180°；

若min(bi)=b2，则矫正角angle=90°；

若min(bi)=b3，则矫正角angle=270°；

若min(bi)=b4，则矫正角angle=0°.

修正矩阵Rf定义如下：

Rf=RangleR，

(11)

式中：Rangle为旋转角度为angle时的旋转矩阵.

修正平移向量tf定义如下：

tf=t+ts.

(12)

通过修正计算可以得出修正矩阵Rf和修正平移向量tf，通过Rf求解旋转轴A和旋转角度θ，其计算公式如下：

A=RfA,

(13)

(14)

在旋转上，当前相机位姿相对于二维码中心的旋转轴为A，旋转角度为θ；在平移上，当前相机位姿相对于二维码中心的平移向量为tf.

6 误差分析

为证明算法的准确、有效，拍摄50张Rf与tf已知的图像进行位姿求解，现对求解结果的误差进行分析.误差在x、y和z方向均有分布，定义总误差为在x、y和z方向上误差的内积，总误差分布曲线如图14所示.

图14 总误差分布曲线

由图14可知：旋转轴总误差约为±0.18 m；旋转角误差约为±2°；平移量总误差约为±0.02 m.故该算法能够为机器人提供较准确的位姿信息，包括机器人相对于世界坐标系的旋转轴、旋转角和平移量，可以帮助其实现精准定位.

7 结 论

1) 提出一种针对二维码的分组阈值分割算法，与传统阈值分割算法相比，该算法能自动求解适合提取二维码区域的阈值，针对不同光照、不同角度拍摄得到的二维码图像，均能有效地完成二维码区域的二值化.

2) 提出运用循环遍历算法和最大距离算法求解特征点集，并采用像素突变优化出现的失真现象，通过多次试验验证了特征点集求解方法的可行性和有效性.

3) 提出基于二维码的相机位姿求解算法，通过对相机和二维码进行标定、图像预处理、求解特征点集、位姿求解与修正，完成相机位姿的求解.测试数据集中的误差表明，相较于传统的定位算法，提出的基于二维码的相机位姿求解算法可以为机器人提供准确的定位信息，包括机器人的旋转量和位移量，实现对机器人的精准定位.