悬臂式掘进机视觉位姿检测系统外参标定方法

张旭辉，张楷鑫，张超，杨文娟，杜昱阳，谢楠，周创

（1.西安科技大学机械工程学院，西安710054；2.陕西省矿山机电装备智能监测重点实验室，西安710054）

国际煤炭领域的热门研究，悬臂式掘进机作为广泛使用的采掘装备占有重要地位。掘进机机身位姿检测技术是实现智能化与无人化的关键，位姿测量精度是检测技术的前提[1-5]。

近年来，国内外各研究机构针对悬臂式掘进机机身位姿检测精度评价方法都进行了相应的研究。符世琛等[6-7]提出一种基于超宽带测距技术的机身位姿检测方法，利用3个机身定位点在基站群中的三维坐标来得到机身定位点的位姿，针对超宽带测距技术进行测量误差实验评价，但未对机身定位点相对于机身的外参标定进行研究。朱信平等[8]介绍了一种基于全站仪的机身位姿测量系统，得到棱镜坐标系与全站仪坐标系之间的位姿关系，未研究棱镜相对于机身和全站仪相对于巷道之间的外参标定，其精度误差较大且没有实验数据验证。陶云飞等[9-10]提出基于iGPS的掘进机机身位姿测量方法，利用巷道顶板上的1套单站式测量平台和机身上3个机载接收器进行位姿检测，利用基于激光跟踪仪的位姿检测对其精度进行验证，但未考虑测量平台在巷道坐标系中的外参标定误差，评价方法精度难以保证。陈慎金等[11]在掘进机机身位姿激光测量系统的基础上提出全站仪自主标定策略，对不同距离下的位姿测量误差进行仿真分析，理论上实现了对悬臂式掘进机位姿测量精度的要求，但未解决系统外参标定的问题，导致其误差随着标定距离的增大而增大；Hargrave等[12]搭建了一种基于雷达传感器的井下设备定位系统获得井下设备位姿信息，但并未考虑雷达传感器在巷道中的位姿从而加大了系统误差。到目前为止，国内外针对煤巷悬臂式掘进机位姿检测、自主定位方法展开了许多研究，但由于掘进巷道工况环境复杂、恶劣，易对位姿测量结果产生影响，很少有系统外参标定方法的研究与实验验证。张旭辉团队[13-16]提出一种悬臂式掘进机截割头与机身视觉测量系统，利用Vicon 对截割头进行系统系统标定，精度达到了预期目标。但由于Vicon 系统只适合小范围的系统外参标定，并不能适用于机身外参标定，故机身系统外参标定结果主要靠人工测量与倾角传感器进行获取，降低了视觉位姿检测系统精度。

因此，本文提出一种悬臂式掘进机机身视觉位姿检测系统的外参标定方法。针对视觉测量系统存在的外参标定误差，提出基于全站仪测量系统的外参标定方法，并对全站仪的位姿检测方法进行研究，从而保证该标定方法的可靠性。与现有外参标定方法相比，该方法适用场合广，安装要求低，适宜于掘进机机身位姿检测系统的外参标定。

1 掘进机机身视觉位姿检测系统及误差分析

1.1 机身视觉位姿检测系统

悬臂式掘进机的位姿检测就是获得机身在巷道坐标系中的位置。视觉位姿检测系统由图1所示，通过机身上的防爆相机采集激光指向仪的两个激光光斑，将两个激光光斑、光斑之间的直线与激光线简化为两点三线特征，通过视觉测量方法计算出相机相对于激光指向仪下的位姿关系，然后通过建立的掘进机机身姿态解算模型，解算出悬臂式掘进机在巷道坐标系中的位姿。

图1 视觉位姿检测系统

1.2 误差来源分析

悬臂式掘进机视觉位姿检测方法是根据视觉测量模型与矩阵转换关系得到掘进机机身在巷道中的位姿关系，矩阵转换关系如图2所示。

图2 矩阵转换关系

设巷道坐标系与机身坐标系之间的齐次变换矩阵为haT；巷道坐标系与激光测量坐标系之间的齐次变换矩阵为由两点三线特征来确定激光测量坐标系与相机坐标系之间的齐次变换矩阵dcT。根据空间矩阵变换可得

防爆相机外参不能直接测量得到，通过调试保证机身、激光指向仪和巷道平行，人工多次测量得到机身与激光指向仪在巷道中的位置平均值，并结合视觉测量结果dcT 得到相机外参矩阵acT。根据已经得到的相机外参与矩阵转换关系实时得到掘进机机身位姿。

从测量系统结构与位姿解算方法分析可知，视觉位姿检测系统误差主要来源于视觉测量误差与系统外参标定误差，其中视觉测量误差由特征提取方法决定，系统外参标定误差由相机外参标定、机身与激光指向仪相对于巷道的测量误差导致。

1.3 误差处理方法

针对系统外参标定误差对视觉测量系统精度的影响，利用全站仪测量结果与坐标系转换矩阵对视觉位姿检测系统进行外参标定。坐标系转换原理图如图3所示。

图3 坐标系转换原理

设巷道坐标系与全站仪坐标系之间的其次变换矩阵为hoT，全站仪坐标系与机身坐标系之间的齐次变换矩阵为oaT，全站仪坐标系与激光指向仪坐标系之间的齐次变换矩阵为odT；根据空间矩阵变换得：

全站仪坐标系与巷道坐标系重合，故其之间的齐次变换矩阵hoT为1，全站仪坐标系与机身坐标系、激光指向仪坐标系之间变换矩阵由全站仪位姿检测方法得到。故全站仪坐标系与机身坐标系、激光指向仪坐标系之间的变换矩阵即为巷道坐标系与机身坐标系、激光指向仪坐标系之间的变换矩阵。并根据式（1）视觉位姿检测系统中的空间矩阵变换关系得到相机外参矩阵。

该误差处理方法主要是利用全站仪对视觉位姿检测系统的系统外参进行高精度标定，避免了人工测量误差与相机外参标定误差对视觉检测系统精度的影响。因此建立基于全站仪的掘进机机身位姿检测系统成为提高视觉精度的关键。

2 掘进机机身视觉位姿检测系统外参标定方案

悬臂式掘进机机身视觉位姿检测系统外参标定方案如图4所示，包括悬臂式掘进机模型、防爆工业相机、全站仪、3个全方位棱镜和两个激光指向仪。将防爆相机固定于掘进机机身平面且方向与巷道掘进方向相反，使用锚杆将两个激光指向仪固定于煤层顶部，并通过特定的加持装置保证其平行，在掘进机工况过程中保证相机实时可以拍到激光光斑与激光光束。全站仪位于机身后侧，确保其安全性。将棱镜A 安装在机身中心（回转台中心）,棱镜B与棱镜C分别安装在掘进机机身中轴线两侧，保证和棱镜A 在机身同一个水平面，以棱镜A为圆点Oa建 立机身坐标系OaXaYaZa。设巷道坐标系OhXhYhZh与 全站仪坐标系OoXoYoZo重合且为大地绝对坐标系；

图4 视觉位姿检测系统外参标定方案

该外参标定方案主要由视觉测量系统与全站仪测量系统组成，首先通过全站仪测量原理得到掘进机机身3个360°棱镜的三维坐标；然后利用全站仪机身位姿解算方法得到全站仪与机身、激光指向仪之间的空间变换矩阵，实现视觉位姿检测系统的外参标定；最后分别将全站仪与人工测量得到的系统外参标定结果代入视觉位姿检测系统，将得到的掘进机机身在巷道中的位姿进行对比，验证基于全站仪系统外参标定方法的准确性与可靠性。其中保证基于全站仪外参标定方法的精度是视觉位姿检测系统的前提。

3 全站仪外参标定方法与精度分析

研究全站仪位姿检测方法，并对其进行精度分析，保证系统外参标定方法的精度。本文采用全站仪外参标定方法对机身与巷道、激光指向仪与巷道之间的外参进行标定。通过全站仪测量激光指向仪两个光斑与其不共线外一点的三维信息，通过位姿解算模型得到全站仪坐标系与激光指向仪坐标系之间的位姿关系。以机身为例，分析位姿解算模型精度，最后验证全站仪外参标定方法的可靠性。

3.1 机身位姿测量

在刚体上建立刚体坐标系OaXaYaZa， 另外在刚体之外建立一个全站仪坐标系OoXoYoZo，如图5所示。将机身上的3个棱镜A、B、C确定为机身平面，用于反射全站仪发射的电磁波，构成全站仪测量系统的3个特征点[17-18]。通过全站仪采集棱镜A的三维坐标作为掘进机机身位置信息；机身的姿态测量采用平面与向量之间的角度关系进行解算，通过计算机身平面下的机体坐标系与全站仪坐标系的旋转角度，来确定掘进机在实际工况中的姿态，包括掘进机机身在全站仪坐标系下的偏航角α、俯仰角 β和横滚角γ。

图5 机身姿态检测模型

根据棱镜B与C在全站仪坐标系下的三维坐标B(xB,yB,zB)、C(xC,yC,zC)，计算出棱镜B与C在空间坐标系下的中点 Oa为：

按照机体坐标系的方向，中点坐标(xa,ya,za)指向棱镜A(xA,yA,zA) 形成向量 OaYa(xaA,yaA,zaA)，中点坐标(xa,ya,za)指 向棱镜B(xB,yB,zB)形成向量OaXa(xaB,yaB,zaB)。设垂直于XOY 面的向量OaZa为( x1,y2,z3)，由于坐标系特性，向量OaZa与 OaYa、OaXa两两正交，故可以列出：

其中向量OaYa、OaXa的坐标已知，令x1=0，则YOZ 平面上的向量即为法向量OaZa。用向量OaXa、OaYa、OaZa代表全站仪坐标系下机体XYZ 坐标轴，用向量OoXo、OoYo、OoZo代表全站仪坐标系的3个坐标轴，机身姿态测量方法如下：

机体坐标系纵轴在全站仪坐标系纵轴和横轴平面内的投影与全站仪坐标系纵轴夹角为偏航角α（如图6a）所示），机体坐标系竖轴在全站仪坐标系纵轴和竖轴平面内的投影与全站仪坐标系竖轴夹角为俯仰角β（如图6b）所示），机体坐标系竖轴在全站仪坐标系横轴和竖轴平面内的投影与全站仪坐标系竖轴的夹角为横滚角 γ（如图6c）所示）。机身姿态解算方法为：

图6 机身姿态测量方法

3.2 精度分析

利用全站仪对棱镜的三维坐标进行测量时，由于全站仪本身存在测量误差I，导致该位姿检测方法的精度也有所下降，因此对外参标定方法中全站仪位姿检测方法进行精度分析[19-20]。本文以掘进机机身偏航角为例建立如图7所示全站仪位姿检测方法的误差模型。

图7航向角误差模型

图7 分析了偏航角误差模型，棱镜理论值建立的平面坐标系为OaXaYa，全站仪实际测量得到的棱镜坐标误差为I ，实际偏航角的坐标轴为O′aYa′。因此所建立的机身偏航角存在角度误差 Δα，且满足：

式中： D 为全站仪与棱镜群的距离，mm； L为棱镜B和C中点到棱镜A的距离，mm。

4 实验结果与分析

4.1 全站仪位姿检测方法评价

本系统对悬臂式掘进机进行位置测量中，直接采用机身中心棱镜A的三维坐标。为了评价全站仪位姿解算方法下航向角、俯仰角与横滚角的精确性。搭建了悬臂式掘进机全站仪姿态评价实验平台，如图8所示。平台由全站仪（索佳iX-1000）、3个全方位棱镜、三轴转台和计算机组成。采取三轴转台对航向角进行测量，并与基于全站仪的定位方法进行姿态评价。实验具体步骤如下：

图8 全站仪位姿检测评价平台

1）将全站仪与三脚架调平，3个棱镜按照搭建系统要求放置于三轴转台，保证移动过程中位置不会发生偏移。

2）通过调节按钮将三轴转台分别在航向角、俯仰角与横滚角3个方向进行旋转，三轴转台在每个方向一共设置11个观测点，且每次转动2°。

3）记录每个位置下3个棱镜的三维坐标，最后通过全站仪姿态解算方法得出每个位置下机身的姿态。

4）对比相邻位置间转台角度与全站仪解算的变化角度。

根据以上实验步骤进行实验，将全站仪位姿检测评价平台中测量所得到的L 、 D代入式（7），得到该全站仪检测方法理论误差 Δα为0.0381°。得到实验结果和误差如下所示。

将测量得到的由于不能保证三轴转台与全站仪在不同水平面完全平行，故拟采取11个观测点之间的变换量评价全站仪方法的准确性。图9a）、图9b）、图9c）分别表示偏航角、俯仰角和横滚角基于全站仪和三轴转台下11 个数据点之间的10 个变化量。由图9d）可知本文使用的全站仪姿态解算方法在偏航角、俯仰角、横滚角方向与三轴转台的最大误差分别在0.06°、0.06°、0.08°之内，平均误差分别为0.0426°、0.0335°、0.0516°。根据上述分析：本文使用的全站仪姿态解算方法在3 个角度下的平均误差为0.0425°，与理论误差相符，可以作为视觉评价系统的基准。

图9 全站仪姿态测量结果误差

4.2 系统外参标定方法精度评价

为评价悬臂式掘进机视觉测量系统外参标定方法精度，建立基于全站仪测量方法的评价平台，如图10所示。平台由履带机器人、全站仪（索佳 iX-1000）、3个全方位棱镜、YHJ-800激光指向仪、防爆工业相机（MV_EM510C）、烟雾机和计算机组成。

图10 系统外参标定下视觉位姿测量精度实验

将全站仪坐标系确定为巷道坐标系，使用烟雾机制造烟雾来模拟掘进巷道环境。且该系统环境下，通过式（7）和式（8）可知全站仪姿态测量精度理论最大值为0.07142°，也适用于此实验的系统外参标定。首先通过全站仪位姿检测方法得到机身、激光指向仪在全站仪坐标系下的位姿，将全站仪系统外参标定结果与人工测量系统外参标定结果分别代入视觉位姿检测系统，并将履带机器人进行移动，两侧履带差速运动将掘进机进行位置变化，侧帮辅助履带转动进行姿态变化。为了模拟掘进机实际工况，控制机器人朝巷道掘进方向运动20 m，一共采集20组数据进行处理，为了保证其系统的可靠性，在运动过程中随机控制机器人进行运动，使用防爆计算机采集3个棱镜的三维坐标进行位姿计算。最后对比两次外参标定下机身在巷道中的位姿，从而验证基于全站仪系统外参标定方法的精确性。

表1中列举了履带机器人在运动过程中5个观测点下的实验数据，由全站仪系统外参标定和人工测量系统外参标定下全站仪测量得到的机身位姿信息组成。由实际工况了解到：机器人在偏航角、与俯仰角方向变化较大，故实验中偏航角 α、俯仰角β摆动角度较大。所有实验数据如图11所示。

表1 系统外参标定方法的机身位姿部分实验结果

实验结果表明本文所述基于全站仪系统外参标定方法可以提高悬臂式掘进机视觉位姿测量精度。图11a）为两种系统外参标定结果下全站仪测量得到的掘进机位置误差，其中掘进机X 轴方向平均误差提高了13.073 mm；Y 轴方向平均误差提高了21.511 mm；Z 轴方向平均误差提高了18.159 mm。图11b）为两种系统外参标定结果下全站仪测量得到的掘进机姿态角角度误差，其中机身偏航角角度平均误差提高了0.225°；俯仰角角度平均误差提高了0.246°；横滚角角度平均误差提高了0.246°。

图11 机身位姿测量结果误差

5 结论

论文分析了悬臂式掘进机位姿视觉测量系统误差来源，并提出一种基于全站仪的系统外参标定方法，根据全站仪机身位姿解算模型与系统坐标系转换关系，评价基于全站仪的系统外参标定方法对机身视觉位姿检测系统的影响。研究获得以下结论：

1）掘进机视觉测量方法的误差主要来源于视觉测量误差与系统外参标定误差。

2）基于全站仪位姿测量的掘进机机身静态定位方法在精度方面满足验证需求，通过三轴转台进行精度对比分析表明，其中位置误差在±3 mm 内，姿态角角度误差在0.08°内。

3）搭建了全站仪与视觉组成的机身测量系统，并建立该系统下各坐标系之间的转换关系，利用全站仪位姿检测方法评价视觉位姿测量系统中外参标定对其精度的影响息。结果表明：该外参标定方法可以提高视觉位姿检测系统的精度，其中X、Y 和Z 方向的位置平均误差分别提高了13.073 mm、21.511 mm 和18.159 mm，偏航角、俯仰角和姿态角的角度平均误差分别提高了0.225°、0.246°和0.246°。