基于改进ICP算法和CAD模型库的单目视觉位姿测量

季 涛,李福东,杨月全,张天平

(扬州大学信息工程学院,江苏 扬州 225009)

基于视觉的位姿测量在工业自动化中应用广泛,由于人们对机器人系统的智能化需求不断提高,而位姿测量是实现机器人灵活有效运动的前提,故其精确度和实时性成为研究热点．目前,基于单目视觉的位姿测量方法主要分为基于物体表面特征和基于模板匹配两类．基于物体表面特征的方法通常使用点、线、圆等几何特征进行匹配,有时须人为设定特征点．Loncomilla 等[1]通过识别对象局部不变特征的方法总结出机器人视觉中用于对象识别的系统．Liu等[2]对工件上的几个特征点进行定向测量,利用微分变换估计初始迭代值,再采用不确定度分析法优化摄像机姿态．基于模板匹配的方法则根据对象的CAD模型在各种姿态下的成像离线建立模板库,然后对待测图像进行处理并与模板库里的图像匹配来确定物体当前位姿．宋薇等[3]采用倾角分层方法将距离映射图按边缘倾角分层,结合模板探索点的倾角进行匹配．刘凌云等[4]首先选出模板库里的候选图,然后确定最佳匹配模板以加快匹配速度．Bratanic等[5]基于模板匹配的方法,改进相似性度量并引入光度立体视觉获得表面法线实现位姿估计．Markus[6]结合尺度空间思想和方位图的相似性实现三维物体的快速识别．由于基于物体表面特征的方法具有局限性,故本文拟采用基于模板匹配的方法,首先利用OpenGL绘制出物体的CAD模型,设置合适的采样间距适当减少匹配库容量,然后采用改进的Hausdorff距离匹配算法得到与待测图像位姿最接近的一幅模板,匹配得到粗略位姿,最后使用改进的ICP算法对待测图像的边缘点集和物体三维边缘点集进行迭代优化得到精确位姿．

1 模板库的建立

首先利用OpenGL绘制出物体的三维模型,以物体几何中心为原点建立世界坐标系(Ow,Xw,Yw,Zw),再以其原点为球心建立地理坐标系,虚拟相机在球面上不同的观察点处获得对应的投影图,如图1所示．以相机的光轴为Zc轴建立相机坐标系(Oc,Xc,Yc,Zc),其光轴始终指向球心．虚拟相机在球面移动时,世界坐标系和相机坐标系的位姿关系可用参数经度φ、纬度δ和半径r表示出来．

图1 虚拟相机投影示意图Fig.1 Virtual camera projection

根据虚拟相机绕自身Zc轴转动的角度θ及其在地理坐标系中的坐标,可得到世界坐标系和相机坐标系之间的转换矩阵

(1)

其中R()表示旋转量,其下标代表旋转所围绕的轴,T()表示平移量．根据实际相机的观测范围设置参数φ、δ和r的范围和步长建立模板库．若步长设置适中,则可较大程度地减少模板库的容量,提高后续匹配速度．若步长设置得不够小,匹配得到的只是粗略位姿,于是须采用ICP算法对粗略位姿进行迭代优化得到精确位姿．根据式(1),欲得到世界坐标系与相机坐标系的相对位姿,只须确定参数φ、δ、r和θ的值．待测图像与模板库里的图像配准后可得参数φ、δ、和r的值,参数θ的值则由图像配准后的仿射变换求得．

2 改进的Hausdorff距离阈值法匹配

Hausdorff距离是描述两组点集间相似程度的一种度量,2组点集间的Hausdorff距离越小说明两者相似程度越高．对于2组有限点集A,B,定义Hausdorff距离[4]

H(A,B)=max{h(A,B),h(B,A)},

(2)

其中h(A,B)=maxa∈A(minb∈B(‖a-b‖)),h(B,A)=maxb∈B(mina∈A(‖a-b‖))．

由于待处理图像中存在噪声,所以提取的边缘点中含出格点,即图像噪声导致的伪边缘点．这里对Hausdorff距离进行改进,设置阈值β去除出格点,定义点集A到点集B的新有向距离

(3)

Hn(A,B)=max{hn(A,B),hn(B,A)}．

(4)

3 基于三维模型与改进ICP算法的位姿测量方案

受虚拟观察点采样间距的限制,匹配得到的位姿精度偏低,故笔者引入ICP算法进行精匹配[7]．由于ICP算法要求2个待匹配点集间的初始位姿不可相差太大[8-10],故选择Hausdorff距离粗匹配法使得待匹配的2个点集满足该条件．

设置目标函数

(5)

1) 对于点集Bk中的每个点,在点集Ak中找到其最近点．为了剔除伪点对,以改进的Hn(A,B)为阈值,将距离大于该阈值的点对视为伪点对剔除．剔除伪点对后,点集Bk中剩余的点组成点集pk,在点集Ak中找到最近的点组成点集Qk;

2) 对点集Pk,Qk采用单位四元数法求得(H-1)k;

6) 如果|dk-dk+1|<ε(ε为设定阈值),则迭代停止;否则,转步骤7);

7) 迭代更新:pk=pk+1;Qk=Qk+1,转入步骤2)．

图2 基于三维模型和ICP算法的单目视觉位姿测量流程图Fig.2 Overall block diagram of monocular vision pose measurement

基于三维模型和改进ICP算法的单目位姿测量步骤如下: 利用OpenGL绘制目标物体的三维模型,以经纬度和半径为参数设置一系列虚拟观察点,生成虚拟相机投影图,提取图像边缘构成模板库;对待测图像进行去噪处理后提取边缘,以边缘点集作为特征点集;使用改进的Hausdorff距离阈值法在模板库里找出与待测图像最近似的一幅图像,根据式(1)计算粗略位姿;再利用相机内参和外参矩阵乘积的广义逆矩阵,将待测图像二维点集转换为三维点集,结合物体边缘在世界坐标系下的三维点集采用ICP算法迭代优化得到精确的位姿参数．其流程如图2所示．

4 仿真分析

实验采集数据所用相机型号为AVT Manta G125B,采集的图片分辨率为1 292×964像素(其中焦距f=8.26 mm)．先采集多张标定板图片,对相机标定得到相机内参矩阵的值[11]．将如图3(a)所示的三维物体摆放在固定位姿,提取当前位姿下所采集图像的边缘得到边缘图像,与模板库里的图像进行匹配得到粗略位姿,再采用ICP算法迭代得到精确位姿．

图3 三维物体作平移和旋转处理后的位姿图Fig.3 A sketch of translational and rotational processing of three-dimensional objects

以初始位姿为基准,且其取值为[4.034 12.642 36.820 -2.144 0.440 0.464],前3个参数分别表示世界坐标系相对于相机坐标系沿X、Y、Z轴平移的距离,单位为cm;后3个参数分别表示世界坐标系相对于相机坐标系绕三轴旋转的角度,单位为rad．在此基础上对三维物体作图3(b)～(e)所示的平移和旋转处理,物体移动后的位姿可依据其转换关系计算出期望的位姿,对比实际位姿与期望位姿的误差验证算法的有效性．

根据三维物体与相机的实际位姿关系离线建立模板库,设置虚拟观察点时的参数取值φ∈[50,110],δ∈[20,70],r∈[20,30],设定经纬度的步长为2°,半径的步长为2 cm,得到一系列不同位姿图,模板库里的图片以参数φ,δ,r的值(r_φ_δ.bmp)命名,部分样本如图4所示．

图4 模板库部分样本图Fig.4 Sample diagram of partial template library

待检测图像与模板库中图像在提取边缘后经Hausdorff距离匹配算法得到匹配图像,据式(1)计算出待测图像中物体的粗略位姿,再采用ICP算法迭代优化,计算所得实际位姿与期望位姿的误差,结果如表1～2所示．由表1～2可知,平移处理和旋转处理后位姿参数的最大误差分别为0.293 cm,0.029 rad,均可满足一般机械手抓取任务的要求[3]．

表1 平移处理后的位姿误差Tab.1 Posture error after translational processing cm

表2 旋转处理后的位姿误差Tab.2 Posture error after rotation processing rad

5 结论

本文针对单目视觉位姿问题,提出了一种基于改进ICP算法和CAD模型库的单目视觉位姿测量方法．在设置虚拟观察点建立模板库时,通过选择合理的采样间距减小模板库容量,加快了后续匹配速度;应用改进的Hausdorff距离阈值法剔除图像噪声所致的出格边缘点;设置阈值剔除伪点对,采用ICP算法迭代优化得到精确的位姿参数．结果表明本文方案是有效的且具有良好的实际应用价值．