锚杆支护自动作业二次定位修正方法

李航,李瑞,李响

(1.北京信息科技大学 仪器科学与光电工程学院,北京 100192;2.北京唯实深蓝科技有限公司 北京 110105)

0 引言

在煤矿掘进领域中,巷道的安全和畅通是保证快速掘进、高效开采的必然前提。目前,针对巷道安全的众多保护措施中,锚杆支护技术是应用最广泛,安全效果最佳的防护方式之一[1]。但目前的锚杆支护安装和定位任务基本都是靠人工操作来完成的[1]。这种方式不仅对操作员的要求高,而且效率低,对操作员存在许多危险[2]。因此,锚杆支护的自动作业技术成为了煤矿领域研究的热点。

锚杆支护的自动作业,核心是锚孔的自动识别和机械臂的定位。锚杆支护的自动作业是否成功完成主要在于机械臂定位钻孔的位置是否存在偏差。在实际的工况要求中,机械臂对锚钻孔的定位偏离误差通常要求小于5 mm。但是由于煤矿井下环境恶劣,锚杆机长期进行大重量操作,会使机械系统零部件的间隙逐渐增大,导致定位精度下降,无法保证机械臂达到钻孔位置的精度和重复性要求[3-4]。在锚钻孔定位中,基于视觉测量的辅助定位技术能够实时检测钻臂与锚钻孔的空间位置关系,进而可减小机械系统自身误差累积造成的定位误差[5]。目前,机器臂的视觉技术可分为两类:二维视觉技术和三维视觉技术[6]。相较于二维视觉技术,三维视觉技术在获得平面信息的同时,可以获得目标点的深度信息,进而更能准确地描述目标点的空间位置,所以被广泛应用到机械臂定位中[7]。

目前市场上用于三维视觉定位的相机包括:结构光相机、双目相机和TOF相机。而结构光相机既不需要进行大量的双目匹配计算,又因其高精度、小体积、低成本、低功耗等优势成为了主流的三维视觉相机,并在巷道、隧道等领域逐渐兴起[4]。但市场上的工业级结构光相机由于加工工艺和装配精度等原因,会使相机镜头存在畸变,严重影响定位测量精度[8]。为了消除畸变对测量误差的影响,陈文艺等[9]提出基于深度学习的单张图像畸变校正方法。丁先之等[10]对面向矿石破碎无人化系统的手眼标定及误差补偿研究来提升定位精度。但这两种方法计算量大,操作复杂,在实际的煤矿环境中实施困难。因此,本文提出了一种在不进行畸变矫正的情况下,通过二次定位修正补偿的方法,可以满足在0.5～1.5 m工作范围内,锚钻孔定位误差小于5 mm的工作要求。

1 锚孔定位误差理论建模

1.1 锚孔定位系统建模

整个锚钻孔定位系统模型含有相机坐标系、机械臂末端坐标系和机械臂基坐标系。通过“手眼”标定的方法,可以将相机中的锚钻孔的三维坐标转到机械臂基坐标系下坐标,进而让机械臂进行坐标定位。而“手眼”系统又可以根据相机与机械臂的位置关系分为“眼在手上”和“眼在手外”[11]。

本文使用的是“眼在手上”的“手眼”系统。Tsai的两步标定法可以通过一个三变量的优化搜索求解,来消除“手眼”标定中相机畸变对标定精度的影响[12]。因此,本文选择Tsai两步法进行“手眼”标定工作。

图1 锚钻机械臂

将图1简化成几何结构,如图2所示,其中L1为机械臂旋转轴与俯仰轴的水平距离;L2为机械臂旋转轴与俯仰轴的垂直距离;L3为机械臂末端与机械臂俯仰轴的距离;L4为机械臂末端与机械臂俯仰轴的垂直距离。机械臂结构由铁架组成,L1、L2、L3、L4数值由卷尺测量得到。

图2 机械臂几何抽象图

通过机械臂的运动学模型的测量结果,求解得到:

(1)

(2)

然后利用式(1)和(2),就可以将相机坐标系下锚钻孔的三维坐标转换到机械臂基座标系下的三维坐标:

(3)

式中:xbase、ybase、zbase为机械臂基座坐标系下的坐标;Xc、Yc、Zc为相机坐标系下的坐标。

旋转机械臂将锚杆插入到锚钻孔中,得到插入该位置的锚钻孔所需的旋转角度θ1和θ2,并且此时锚钻孔在机械臂末端坐标系下的坐标为O(x′,0,0),通过机械臂的逆运动学模型[13]求坐标x′的值,计算式如式(4)所示:

(4)

式中:T为机械臂基坐标系到机械臂末端坐标系的转换矩阵。当机械臂两个旋转轴的运动角度都小于5°时,可将式(4)化简为

(5)

1.2 误差模型理论推导

结构光视觉测量系统含有像素坐标系、图像坐标系、相机坐标系以及世界坐标系,通过相机的标定可以得到各个坐标系之间的转换关系[13]。根据结构光相机的透视投影模型和相似三角形计算关系,可以得到图像坐标系与相机坐标系之间的转换关系[14]:

(6)

式中:x、y为图像坐标系下的坐标;f为相机的焦距。化简后得到:

(7)

联立式(3)和(7)得到锚钻孔中心点的图像坐标与机械臂基座标的转换关系:

(8)

将式(5)中的xbase、ybase、zbase用式(8)算出的结果代替,最终得到了在结构光相机镜头理想情况下机械臂定位到锚钻孔中心点所需的角度与其图像坐标之间关系:

(9)

但由于镜头畸变的存在,导致图像边缘出现拉伸或收缩,如图3所示。在结构光视觉测量中,如果使用了带有畸变的镜头进行视觉测量,会大大影响视觉测量的精度,并且,越是靠近图像边缘位置的点,其测量误差越大,图像四个角的点误差达到最大值[15]。

图3 畸变图像示意图

在所有畸变参数中造成影响较大的是径向畸变,其数学模型为:

xu=x(1+k1r2+k2r4+k3r6+…)

(10)

yu=y(1+k1r2+k2r4+k3r6+…)

(11)

因此,当结构光相机镜头存在径向畸变时,取径向畸变系数为k1,将式(9)、(10)、(11)联立,得到锚钻孔中心点的图像坐标受相机镜头径向畸变影响的真实定位模型为

(12)

将式(9)与式(12)做差,即得到了由于相机镜头径向畸变对锚钻孔定位造成的误差模型:

(13)

当锚钻孔与相机所在平面平行时,令Zc=1 000 mm,f=1 000,k1=0.01,观察机械臂定位图像上不同点位置所需的旋转角度θ1与θ2的关系,如图4所示。

图6(a)为当图像坐标x为0时,y从-40 mm到40 mm取值,分别用不带相机镜头畸变的模型与相机带径向畸变的模型计算出的定位角的差值。图6(b)为当图像坐标y为0时,x分别从-40 mm到40 mm取值,分别用不带相机镜头畸变的模型与相机带径向畸变的模型计算出的定位角的差值。从图6可以看出,当图像坐标x、y越趋于0时,机械臂定位的误差角度Δθ1和Δθ2越小,定位的精度越高;反之,当图像坐标x、y的绝对值越大时,机械臂定位的误差角度Δθ1和Δθ2越大,定位的精度越低。

图4 无畸变时图像x、y坐标与定位角θ1、θ2的关系

图5 有畸变时图像x、y坐标与定位角的关系

图6 图像坐标与误差角度的关系

根据弧长公式可以计算出机械臂在运动到不同角度时,其在锚钻孔平面所行走的距离:

(14)

式中:n为机械臂旋转的角度;r为锚钻孔所在平面到机械臂旋转轴之间的距离;L为机械臂转动的距离。当工作距离为1 m时,若要定位误差小于5 mm的精度要求,用式(14)计算出最大误差角度为0.133°。从图6可以看出,要想使定位的误差角度小于0.133°,图像坐标x的取值范围为:-22 mm

2 二次定位修正方法

针对上述分析结论,本文提出了使用二次定位修正的方法来减小第一次定位时的误差,其流程如图7所示。首先,从结构光深度相机中获取锚钻孔的三维坐标;然后通过“手眼”标定结果和机械臂的运动学模型,将相机坐标系下锚钻孔的三维坐标转到机械臂基坐标系下坐标;最后通过机械臂运动学模型的反解,计算得到机械臂的定位角度θ1和θ2。

图7 二次修正补偿流程

3 实验验证

为了验证本文提出的二次定位修正方法,搭建了图8所示的实验系统。该系统中在锚杆钻臂的一侧安装深度相机。锚杆钻臂距离锚孔所在平面距离为1 m,与1.2节中的仿真条件相同。以墙上随机分布的十字交叉点作为目标锚孔位置,分别标记第一次定位后锚杆达到的位置和二次修正后的位置。用刻度尺分别测量第一次定位的位置与目标点的距离和二次修正后的位置与目标点的距离,即为第一次定位误差和二次修正定位后的误差。实验结果记录数据见表1。

图8 锚杆机实物图

将表1中的定位误差数据进行整理后如图9所示,第一次定位误差以“+”表示,二次修正定位后的误差以“*”表示。从表1可以看出,第一次定位的误差均大于5 mm。在二次修正后,12组定位角度均小于0.133°的阈值,且二次修正后的定位误差均小于5 mm,平均为2.1 mm,与第一次定位误差结果相比,定位误差最大减小了83.7%,平均减小了74.7%。

表1 实验数据

4 结束语

本文通过对3D结构光深度相机的镜头存在畸变导致测量误差的问题进行分析,得出了锚钻孔定位误差与锚钻孔所在图像的位置的关系。当锚钻孔距离图像中心点越远时,锚钻孔的定位误差越大,反之越小。根据此关系,用“手眼”标定模型以及径向畸变矫正模型,建立了锚杆钻孔的二次定位修正模型。经过多次实验验证,该二次定位修正方法只需要进行一次修正就能满足5 mm误差的精度要求,实际锚钻孔的定位误差平均为2.1 mm。