陈慎金，成 龙，王鹏江，李 瑞，刘 丹，吴 淼
(中国矿业大学(北京) 机电与信息工程学院，北京 100083)
摘 要:围绕在狭长综掘巷道中掘进机位姿激光测量系统位姿参数高精度自动测量的问题，首先系统介绍了位姿测量系统，研究了位姿测量系统测站移站后高精度自动全站仪自主标定原理。依据激光测量原理及掘进机工作特点，构建了全站仪移站后全站仪设站点坐标与定位棱镜坐标测量误差数学平差模型，并通过Matlab进行了不同标定距离、不同测量距离对悬臂式掘进机位姿参数测量误差影响规律的仿真分析。分析结果表明:掘进方向的位姿误差比较大，掘进机姿态角误差均随测量距离和标定距离的增加而增加，且测量距离对姿态角误差影响更明显;姿态角的最大测量误差为航向角，测量误差为0.015 7°。从而验证了激光导向系统自主标定的可行性，并由此提出了一种相对最优的自主标定策略。完全满足目前煤矿综掘工作面对悬臂式掘进机位姿测量精度的要求。
关键词:掘进机;位姿测量系统;自主标定;Matlab
中图分类号:TD421.5
文献标志码:A
文章编号:0253-9993(2018)09-2647-06
移动阅读
陈慎金，成龙，王鹏江，等.基于掘进机位姿测量系统的自主标定方法误差分析[J].煤炭学报,2018,43(9):2647-2652.doi:10.13225/j.cnki.jccs.2017.1726
CHEN Shenjin,CHENG Long,WANG Pengjiang,et al.Error analysis of autonomous calibration strategy based on roadheader attitude measurement system[J].Journal of China Coal Society,2018,43(9):2647-2652.doi:10.13225/j.cnki.jccs.2017.1726
收稿日期:2017-12-07
修回日期:2018-04-10
责任编辑：许书阁
基金项目:国家重点基础研究发展计划(973)资助项目(2014CB046306)
作者简介:陈慎金(1993—)，男，湖南娄底人，硕士研究生。Tel:010-62339113，E-mail:18301691272@163.com
CHEN Shenjin,CHENG Long,WANG Pengjiang,LI Rui,LIU Dan，WU Miao
(School of Resource and Safety Engineering,China University of Mining and Technology(Beijing),Beijing 100083,China)
Abstract:In terms of the problems associated with the high precision automatic measurement of attitude and posture parameters of roadheader in narrow heading tunnel,the pose measurement system was firstly introduced systematically,and then the automatic calibration of high precision automatic total station was studied.According to the principle of laser measurement and the characteristics of boring machine,a mathematical adjustment model of total station coordinate and positioning prism coordinate measurement error was developed.The influence of different calibration distances and different measurement distances on the measurement error of pose parameters of cantilever roadheader was simulated by Matlab.The analysis results show that the error of heading direction is relatively large,and the error of attitude angle increases with the increase of measuring distance and calibration distance,and the influence of measuring distance on attitude angle error is more obvious;the maximum measurement error of the attitude angles of boom-type roadheader is the value of course angle,the measurement error is 0.015 7°.The feasibility of autonomous calibration of laser guided system was verified,and a relatively optimal autonomic calibration strategy was proposed,which fully satisfies the requirement accuracy of the attitude and position of roadheader in the current production of coal mine.
Key words:roadheader;position and attitude measurement;autonomous calibration;Matlab
随着我国煤炭持续不断的开采，煤炭开采的深度和难度也将会越来越大，使得煤炭生产安全形势严峻，矿工生命安全受到极大的威胁，这就对掘进机提出了高效、安全、智能等更高的技术要求，因此研究深部危险煤层无人采掘装备、实现掘进机智能机器人化显得尤为重要。实现悬臂式掘进机位姿的自主测量是综掘工作面无人化技术的关键[1-2]，是实现矿山物联网的最前端，即感知阶段，掘进质量的好坏，直接决定着煤炭安全及生产效率。
近年来，一些高等院校针对掘进机位姿测量方法进行了一定的研究。中国矿业大学(北京)吴淼教授团队[3]进行了利用激光指向器引导掘进机定向掘进的研究，但此方法仅能实现一定的定向功能，无法准确的实现掘进机的精准定位;文献[4]提出了一种基于超宽带技术的掘进机自主定位导向方法，但该方法在Z轴的误差无法满足煤矿井巷工程质量验收规范。文献[5]介绍了一种基于全站仪的掘进机机身位姿参数测量系统，利用全站仪能准确测量出掘进机的姿态角(偏向角、俯仰角和横滚角)，但没有进行深入研究。文献[6]提出了基于iGPS的掘进机位姿检测方法并对其可行性进行验证研究，但是最大测量距离只有40 m，无法进行远距离测量。到目前为止，国内外针对煤巷掘进机位姿检测、自主定位方法展开了相关研究，但是由于综掘巷道工况条件复杂、恶劣，易对自动测量结果产生影响，导致测量精度低，这些技术在综掘巷道工作面的实际应用效果尚未达到无人化采掘的技术水平。
位姿激光测量系统是一种利用高精度全站仪对掘进机上的待测棱镜进行测量，再经由位姿解算计算机解算掘进机的位姿[5]，而本文在悬臂式掘进机激光导向系统位姿参数自动测量的基础下研究全站仪测站移站后自主标定，即当掘进机上待测棱镜超出自动全站仪的测量范围时，必须进行全站仪快速移站操作，对全站仪进行自主标定，是实现掘进机位姿自主检测的基础。本文主要基于该技术在局域范围内的高精度测量功能，针对综掘工作面条件，提出了一种基于掘进机位姿测量系统的自主标定方法及系统，从测量系统的组成，自主标定原理，自主标定数学模型，系统测量精度分析4个方面对掘进机位姿自主测量的实现进行了研究。
掘进机位姿测量系统主要用于井下隧道开挖、巷道掘进的一种自动导向系统，能实现掘进机自主定位和位姿检测及全站仪测站的自主标定功能[5]。掘进机位姿测量系统位姿自动测量方法的组成及布置方式如图1所示。
图1 掘进机位姿激光自动测量方法布置
Fig.1 Arrangement diagram of laser automatic measurement method
1,2，3—机载待测棱镜;4—后视棱镜;5—自动全站仪;6—自整平式仪器平台;7—掘进机;8—掘进工作面
此系统是由高精度自动全站仪、自整平式仪器平台、机载测量棱镜、后视棱镜、测站定位棱镜以及位姿解算计算机等组成的，通过上述各个设备之间的相互配合以实现掘进机位姿自动测量等功能。
自动全站仪对机载测量棱镜进行坐标测量时，自动全站仪在工业计算机远程控制下，对机载测量棱镜进行自动目标搜索、自动照准及自动测量，P1点(x1，y1)测量原理如图2所示。
(1)
式中，Si为自动全站仪到机载测量棱镜Pi的斜距;φi为自动全站仪到机载测量棱镜Pi之间的水平角;φi为自动全站仪到机载测量棱镜Pi之间的垂直角;TR为测量坐标系与巷道坐标系的转换矩阵。
图2 自动全站仪对测量点测量原理
Fig.2 Schematic diagram of measuring the measuring point of the automatic total station
在掘进机激光测量系统中，自动全站仪的自主标定是掘进机真正实现无人化的关键。目前，在掘进机自主定位方面的研究比较少，也没有真正实现完全自主的无人化掘进。随掘进机不断的向前掘进，机载测量棱镜和掘进机之间的距离不断加大，在煤矿开采的复杂工况下，测量误差持续加大，最后导致导向系统失效，而自动全站仪的标定必须由人工前去完成，不仅安装、整平、标定复杂，而且所需时间较长，不利于煤矿安全生产。因此，提出一种自动全站仪移站后自主标定方法，可以实现掘进机的位姿参数的自动测量，使得测量距离大大提高，减少综掘工作面员工的工作时间，保证了煤矿安全生产。自动移站及定位原理如图3所示，当机载待测棱镜超出高精度自动全站仪的测量范围时，则必须进行快速移站。在进行快速移站过程中，通过自整平式仪器平台的快速悬挂功能，先在距自动全站仪一定距离的前方巷道顶板任意位置处安装3个自整平式仪器平台，为便于说明，初步选定为A,B，C三点。自整平式仪器平台B,C两点分别安装后视棱镜、测站定位棱镜并进行自整平操作，待自整平完成后通过自动全站仪准确测得测量点的位置坐标;再将自动全站仪转移至A点自整平式仪器平台上并进行自整平操作，待自整平完成后自动全站仪再通过自由设站的方法确定自动全站仪移站后的位置坐标，从而实现快速移站及移站后测站的自主标定。
图3 自动移站及定位原理
Fig.3 Automatic shift station and positioning principle
1,2，3—机载测量棱镜;4—后视棱镜;5—自动全站仪;6—掘进机;7—掘进工作面;8—工业计算机;9—测站定位棱镜
实现测站移站后自主标定，本质是通过全站仪采用自由设站的技术完成对2个及以上已知点的联测，快速确定设站点坐标，从而在此基础上，以设站点为已知点，对机载待测棱镜进行测量，得出掘进机的位姿，此过程全是通过计算机控制全站仪完成的。自由设站的简单示意如图4所示，自由设站法由于精度高、速度快、布点灵活而被经常采用。在设站点P点放置全站仪，测出设站点到已知点的距离和角度，根据方向观测值和边长观测值建立误差方程式，然后按照最小二乘原理解算设站点的坐标。
图4 自由设站原理
Fig.4 Schematic diagram of free set station
由上文测站移站后自主标定原理可知标定棱镜间距S0、标定距离L都会对设站点的坐标产生影响，从而影响掘进机位姿测量精度。因此，先构建设站点P点的误差数学模型，并进行精度分析，然后再通过自主标定后的全站仪测量掘进机的位姿参数，得出掘进机位姿精度变化规律。
由于在P点反觇了A,B点的高程，且A点和B点的计算方法一样，利用三角高程测量原理[8-9]计算A点得出:量取仪器高i 和棱镜高ν，测得P点和A点两点间斜距S与竖角α 以计算两点间的高差hPA为
hPA=Ssin α+i-ν
(1)
若精度满足要求，表明各个观测元素是正确的，可通过取平均值得到两点间的高程值。
如图4所示，在未知点P点设站，至少需要观测两个已知点才能求出P点的坐标。利用全站仪测量两个已知点A(x1，y1)和B(x2，y2)得到直线PA和PB的方位角δpa 和δpb，PA和PB的长度Spa和Spb以及两已知点A,B之间的距离S0，由于存在多余观测量，需利用间接平差计算得出设站点的坐标P(x0，y0)。
采用间接平差计算[8-11]，有2个角度观测量，2个边长观测量，而必要观测数为2。现取待测点P点坐标平差值为参数，即
利用测边后方交会公式计算待定点P的近似坐标
,
，即
(2)
其中，
，
由P点的近似坐标可以得出PA和PB的近似方位角
和
，PA和PB的近似边长
和
则P点的坐标为
(3)
式中，δxp 和δyp 为P点的坐标改正数。
由于存在两类观测值需要确定两类观测值的权。设测角中误差为σβ，测距中误差为σS，这两个量都是由全站仪的测角和测距精度决定的，本文所用的全站仪是徕卡TM30全站仪，测角精度为0.5″，测距精度取仪器固定误差为1 mm。设方向权为单位权1，则距离权为:
，可以得到权阵P为
采用间接平差，并顾及全式的单位得出误差方程式为
ν
(4)
式中，
，其中，ρ″为弧度化秒因子，且ρ″=206 265。
按最小二乘原理可以得出设站点P的坐标:
(5)
根据煤矿井巷工程质量验收规范，理论巷道边界与实际巷道边界的最大误差范围不超过10 cm[20]。实际巷道成型过程中，巷道边界误差受到掘进机系统控制误差、截割误差、行走误差、机身定位误差等的影响，其中机身定位精度是保证其他系统精度的基础。由于该系统的定位参数是由全站仪测量计算得出的，全站仪的设站精度是位姿测量系统精度的基础。因此本文采用单位权中误差、点位中误差来进行掘进机位姿测量系统自主标定精度分析，首先求出单位权中误差:
(6)
式中，n为测量值数;t为必要观测值数。
由间接平差计算结果的协因数阵，可得设站点P的近似坐标改正数的协因数阵
，即
(7)
于是有设站点P的点位中误差:
(8)
式中，
，
为探究此系统的定位误差分布规律，利用Matlab对该方法定位过程进行仿真模拟。根据上文的分析研究，掘进机自主标定数学模型的初始测量参数有全站仪所测的δpa和δpb,Spa和Spb四个量，选取标定棱镜间距和标定距离两个因素对全站仪设站误差分布规律以及选取标定距离和测量距离对掘进机姿态角误差分布规律展开研究。拟定掘进机的掘进航向为X轴正向，在X轴负方向20～60 m 内每隔1 m 的设站点进行仿真模拟计算，得到在相应影响因素下设站点定位误差仿真计算结果和在X轴正方向20～60 m 内每隔1 m 的测量点进行仿真模拟计算，得到在相应影响因素下掘进机位姿误差的仿真计算结果，仿真结果如图5，6所示，得到各设站点的仿真均值及误差。
图5 X,Y轴仿真误差值随标定距离分布曲线
Fig.5 Calibration distance distribution of simulation error for the X-axis,Y-axis
图6 设站点误差值随标定距离分布曲线
Fig.6 Calibration distance distribution of station setting error
仿真表明，通过此自主标定方法能够检测出不同测量位置处全站仪设站点的位置坐标，但均存在着一定的测量误差。其中X轴向的误差值要比Y轴向的误差值要大，这说明掘进方向的误差比较大，精度较低，且各轴向的误差值均随标定距离的增大而增大;全站仪设站误差随标定棱镜间距的增大而减小，随标定距离的增大而增大，当标定距离超出35 m的范围时，标定棱镜间距对设站点误差影响不大;同时，通过全站仪设站误差可得知，当标定距离为60 m时，设站点误差最大，最大值为0.19 mm。
通过自主标定后，自动全站仪对掘进机进行测量计算得出的掘进机姿态角误差仿真结果如图7所示。
图7 掘进机姿态角误差随标定距离分布
Fig.7 Calibration distance distribution of boring machine attitude angle error
仿真表明，随测量距离和标定距离的增加，航向角的误差相较于俯仰角、横滚角误差要大，但都满足煤矿井巷工程质量验收规范，航向角最大值在标定距离和测量距离都为60 m的情况下取得，最大值为0.015 7°;姿态角误差均随测量距离和标定距离增加而增加，测量距离对姿态角误差影响更明显，所以提出一种相对最优的自主标定策略，标定距离和测量距离都取60 m。
当全站仪进行多次自主标定时，由设站点坐标的解算过程和图8可知，设站点1的坐标由标定点1和2标定得出，可以测量得出标定点3和4的坐标，然后将全站仪移站到设站点2，由于是通过观测设站点2到标定点3和4的距离和角度标定出设站点2的坐标，因此当标定点3和4的坐标为真值时，设站点2的坐标误差变化规律和设站点1的坐标误差变化规律是一样的，以此类推，当每次标定点坐标为真值时，设站点的定位精度变化规律和第1次标定的定位精度变化是一致的。
图8 全站仪多次标定示意
Fig.8 Multiple calibration diagram of total station
(1)提出了一种基于掘进机位姿测量系统的自主标定方法，可代替传统人工对全站仪进行整平和设站，提高测量效率，为远程精准控制奠定基础。
(2)构建了自主标定数学计算模型，并进行了系统测量精度分析。仿真表明，掘进方向的误差比较大，各轴向的误差值均随标定距离的增大而增大。
(3)仿真表明，姿态角误差均随测量距离和标定距离的增加而增加，且当测量距离和标定距离都为60 m时，航向角的最大测量误差为0.015 7°，相较于其他定位定向技术而言，本方法的测量精度是最高的，可有效扩大测量范围，减少井下人员工作时间，是实现无人化综掘面的关键。
参考文献(References) :
[1] 王虹.我国综合机械化掘进技术发展40 a[J].煤炭学报,2010,35(11):1815-1820.
WANG Hong.The 40 years developmental review of the fully mechanizedmine roadway heading technology in China[J].Journal of China Coal Society,2010,35(11):1815-1820.
[2] 葛世荣.智能化采煤装备的关键技术[J].煤炭科学技术,2014,42(9):7-11.
GE Shirong.Key technology of intelligent coal mining equipment[J].Coal Science and Technology,2014,42(9):7-11.
[3] 李睿.悬臂掘进机机身位姿参数检测系统研究[D].北京:中国矿业大学(北京),2012:41-48.
LI Rui.Research on an automatic detection system for the position and orientation parameters of boom-type roadheader body[D].Beijing:China University of Mining and Technology(Beijing),2012:41-48.
[4] 符世琛,李一鸣,杨健健,等.基于超宽带技术的掘进机自主定位定向方法研究[J].煤炭学报,2015,40(11):2603-2610.
FU Shichen,LI Yiming，YANG Jianjian,et al.Research on autonomous positioning and orientation method of roadheader based on Ultra Wide-Band technology[J].Journal of China Coal Society,2015,40(11):2603-2610.
[5] 朱信平,李睿,高娟,等.基于全站仪的掘进机机身位姿参数测量方法[J].煤炭工程,2011(6):113-115.
ZHU Xingpeng,LI Rui,GAO Juan,et al.Boring machine pose measurement method based on total station[J].Coal Engineering,2011(6):113-115.
[6] 陶云飞,宗凯,张敏骏,等.基于iGPS的掘进机单站多点分时机身位姿测量方法[J].煤炭学报,2015,40(11):2611-2616.
TAO Yunfei,ZONG Kai,ZHANG Minjun,et al.Aposition and orientation measurement method of singlestation,multipoint and time-sharing for roadheaderbody based on iGPS[J].Journal of China Coal Society,2015,40(11):2611-2616.
[7] 吴淼,贾文浩,华伟,等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[J].煤炭学报,2015,40(11):2596-2602.
WU Miao,JIA Wenhao,HUA Wei,et al.Autonomous measurement of position and attitude of boom-type roadheader based on space intersection measurement[J].Journal of China Coal Society,2015,40(11):2596-2602.
[8] 武汉测绘科技大学《测量学》编写组.测量学(3版)[M].北京:测绘出版社,1991:199-234.
[9] 赵建三,贺跃光.测量学(2版)[M].北京:中国电力出版社,2013:124-129.
[10] 葛永慧.测量平差基础[M].北京:煤炭工业出版社,2007:100-170.
[11] 武汉测绘科技大学测量平差教研室.测量平差基础(3版)[M].北京:测绘出版社,1996:83-113.
[12] 周西振,尹任祥.测边后方交会精度研究及其应用[J].西南交通大学学报,2006,41(3):344-348.
ZHOU Xizhen,YIN Renxiang.Accuracy analysis of linear-resection and its application in control measurement of Sutong bridge[J].Journal of Southwest Jiaotong University,2006,41(3):344-348.
[13] 何燕佑,许多文.关于全站仪自由设站原理及精度的探究[J].科技情报开发与经济,2009,19(29):151-153.
HE Yanyou,XU Duowen.The Discussion on the Principles and Precision Analysis of Free Stationing with Total Station[J].Sci-Tech Information Development & Economy,2009,19(29):151-153.
[14] 胡菊英,朱良文,郭楠.全站仪自由设站法及其精度分析[J].测绘与空间地理信息,2014,37(11):179-181.
HU Juying,ZHU Liangwen,GUO Nan.The method of total station free-stationing and its accuracy analysis[J].Geomatics & Spatial Information Technology,2014,37(11):179-181.
[15] 杜雨馨,刘停,童敏明,等.基于机器视觉的悬臂式掘进机机身位姿检测系统[J].煤炭学报,2016,41(11):2897-2906.
DU Yuxin,LIU Ting,TONG Minming,et al.Pose measurement system of boom-type roadheader based on machine vision[J].Journal of China Coal Society,2016,41(11):2897-2906.
[16] 李军利,廉自生,李元宗.机器人化掘进机的运动分析及车体定位[J].煤炭学报,2008,33(5):583-587.
LI Junli,LIAN Zisheng,LI Yuanzong.Kinematics analysis and carriage positioning of roboticized roadheader[J].Journal of China Coal Society,2008,33(5):583-587.
[17] 周玲玲,董海波,杜雨馨.基于双激光标靶图像识别的掘进机位姿检测方法[J].激光与光电子学进展,2017,54(4):186-192.
ZHOU Lingling,DONG Haibo,DU Yuxin.Method of roadheader position detection based on image recognition of double laser targets[J].Laser Optoelectronics Progress,2017,54(4):186-192.
[18] 杨文平,胡鹏,樊纲.掘进机自动定向技术探究[J].煤矿机械,2016,37(8):46-48.
YANG Wenping,HU Peng,FAN Gang.Research on automatic orientation technology of excavator[J].Coal Mine Machinery,2016,37(8):46-48.
[19] 何斐彦,林嘉睿,高扬,等.单目视觉与倾角仪组合优化的位姿测量系统[J].光学学报,2016,36(12):190-197.
HE Feiyan,LIN Jiarui,GAO Yang,et al.Optimized pose measurement system combining monocular vision with inclinometer sensors[J].Acta Optica Sinica,2016,36(12):190-197.
[20] GB 50213—2010,煤矿井巷工程质量验收规范[S].