Advanced Search

ISSN1001-3806 CN51-1125/TN Map

Volume 41 Issue 1
Jan.  2017
Article Contents
Turn off MathJax

Citation:

Study on measurement method of realtime position and attitude of roadheader based on vision/inertial navigation system

  • Corresponding author: YANG Linghui, icelinker@tju.edu.cn
  • Received Date: 2016-01-14
    Accepted Date: 2016-02-24
  • In order to measure the position and attitude of roadheader dynamically and automatically, by analyzing and comparing the features of several existing measurement methods. a position and attitude multiple measurement method for roadheader based on vision/inertial navigation system (INS) was proposed. By getting the attitude of roadheader and the position of monocular vision measurement roadheader with strapdown inertial navigation system (SINS), the dynamic five degree-of-freedom (DOF) measurement was realized. This method was verified with the fast driving equipment in Daliuta mine. The experiment results show that the accuracy of attitude measurement is within 0.1°, the standard deviation of static drift is within 0.25° and the accuracy of position measurement is within 1cm. This method can be used for position and attitude dynamic measurement for roadheader, and possesses very broad prospect in engineering practice.
  • 加载中
  • [1]

    WANG J G, REN J G. Application of efficient fast driving auxiliary equipment[J]. Coal Mine Machinery, 2014, 30(9):149-151(in Chinese).
    [2]

    HUANG Ch R. Error analysis and application in the downhole measurement of laser pointer[J]. Beijing Surveying and Mapping, 2009, 16(4):72-74(in Chinese).
    [3]

    LI R. Research on an automatic detection system for the position and orientation parameters of boom-type roadheader body[D]. Beijing: China University of Mining & Technology, 2012: 23-28(in Chinese).
    [4]

    TIAN Y. Discus sion of automatic guidance and location technology for cantilever excavator[J]. Industry and Mine Automation, 2010, 36(8):26-29(in Chinese).
    [5]

    YANG H, LI W, LUO Ch M, et al.Experimental study on position and attitude technique for shearer using SINS measurement[J]. Journal of China Coal Society, 2014, 39(12):2550-2556(in Chinese).
    [6]

    FAN QI G, LI W, WANG Y Q, et al. A shearer dynamic positioning method using strap down inertial navigation[J]. Journal of China Coal Society, 2011, 36(10):1758-1761(in Chinese).
    [7]

    QIN Y Y. Inertial navigation system[M]. Beijing:Science Press, 2006:287-288(in Chinese).
    [8]

    HUANG G P, LI G Y, WANG B F, et al. Evolution for monocular vision measurement[J]. Acta Metrologica Sinica, 2004, 25(4):314-317(in Chinese).
    [9]

    ZHU J G, YU Zh J. The principle of vision metrology[M]. Beijing:China Machine Press, 2011:16-17(in Chinese).
    [10]

    CAO Y, FENG Y, ZHAO L Sh, et al. Effect of attitude installation error of camera on precision of monocular visual odometry[J]. Transducer and Microsysterm Technologies, 2012, 31(12):23-26(in Chinese).
    [11]

    LOBO J, DIAS J. Relative pose calibration between visual and inertial sensors[J]. The International Journal of Robotics Research, 2007, 26(6):561-575. doi: 10.1177/0278364907079276
    [12]

    MIRZAEI F M, ROUMELIOTIS S I. A Kalman filter-based algorithm for IMU-camera calibration:Observability analysis and performance evaluation[J].IEEE Transactions on Robotics, 2008, 24(5):1143-1156. doi: 10.1109/TRO.2008.2004486
    [13]

    ZHANG G J. Vison measurement[M]. Beijing:Science Press, 2008:66-67, 90-92(in Chinese).
    [14]

    WANG H T, LUO Ch Zh, WANG Y, et al. An improved centroid algorithm for star point[J]. Opto-Electronic Engineering, 2009, 36(7):55-59(in Chinese).
    [15]

    GAO W Sh, ZHANG X G, YANG L, et al. An improved Sobel edge detection[C]//3rd IEEE International Conference on Computer Science and Information Technology (ICCSIT). New York, USA: IEEE, 2010, 5: 67-71.
  • 加载中
通讯作者: 陈斌, bchen63@163.com
  • 1. 

    沈阳化工大学材料科学与工程学院 沈阳 110142

  1. 本站搜索
  2. 百度学术搜索
  3. 万方数据库搜索
  4. CNKI搜索

Figures(6) / Tables(3)

Article views(478) PDF downloads(293) Cited by()

Proportional views

Study on measurement method of realtime position and attitude of roadheader based on vision/inertial navigation system

    Corresponding author: YANG Linghui, icelinker@tju.edu.cn
  • 1. State Key Laboratory of Precision Measuring Technology and Instruments, Tianjin University, Tianjin 300072, China
  • 2. Shenhua Shendong Coal Group Co. Ltd., Shenmu 719315, China
  • 3. Shanghai Raising Measurement Technical Co. Ltd., Shanghai 201203, China

Abstract: In order to measure the position and attitude of roadheader dynamically and automatically, by analyzing and comparing the features of several existing measurement methods. a position and attitude multiple measurement method for roadheader based on vision/inertial navigation system (INS) was proposed. By getting the attitude of roadheader and the position of monocular vision measurement roadheader with strapdown inertial navigation system (SINS), the dynamic five degree-of-freedom (DOF) measurement was realized. This method was verified with the fast driving equipment in Daliuta mine. The experiment results show that the accuracy of attitude measurement is within 0.1°, the standard deviation of static drift is within 0.25° and the accuracy of position measurement is within 1cm. This method can be used for position and attitude dynamic measurement for roadheader, and possesses very broad prospect in engineering practice.

引言
  • 掘进机是一种大型巷道挖掘专用设备,广泛应用于煤矿巷道掘进工作, 更是井下巷道综掘施工的核心装备[1],在我国煤炭工业生产中占有极重要地位。近年来,神华神东煤炭集团联合北方重工等制造业厂家研制了世界首套全断面高效快速掘进系统,实现了巷道断面一次成型、掘进、运输、支护连续作业,月进尺可以达到4000余米。随着快掘系统深入应用,对矿井下巷道综掘施工的自动化水平提出了更高要求,掘进机的实时位姿自动检测技术是其中最为关键的研究内容之一。

    在传统方法中,煤矿巷道的中心轴线设计主要靠精确调整的激光指向仪保证[2],掘进机司机通过目视断面上的激光光斑控制掘进机截割头, 并在断面上截割运行。掘进工程质量很大程度上取决于司机的经验和熟练程度。针对掘进机的自动位姿测量技术,目前方法主要有基于全站仪的导向和定位、惯性导航、电子罗盘和视觉测量等[3-4]。基于全站仪的导向技术多年前就被用于盾构掘进机进行隧道、地铁掘进时的导向。虽然全站仪精度高,但其使用时对环境要求较高,在煤矿等恶劣环境下难以长时间正常使用。惯性导航则是通过陀螺仪和加速度计对载体进行导向和定位,目前已有研究将捷联式惯性导航系统(strapdown inertial navigation system,SINS)应用于掘进机位姿测量中的案例[5-6]。惯性导航的优点是独立自主,不需要任何外界信息就能得到导航参量;但其缺点也十分明显,由于导航信息经过时间积分而产生,定位误差随时间而增大,不能长程连续地提供位置参量。罗盘类传感器通过比较掘进机机身轴线地磁场北极的夹角获得掘进机轴线的航向角,易受外界电磁干扰(如电机内变化电流产生的电磁干扰)而失去精度。视觉测量是根据摄像机获得的图像视觉信息对目标的位置和姿态进行测量的技术,其具有实时、非接触等优点,但难点在于如何克服井下恶劣工作环境以及相机拍摄姿态的影响。

    以上几种测量方法均有各自的优点和不足之处,所以本文中将视觉测量和惯性导航结合起来,取长补短、互相弥补,提出了一种基于视觉/惯导的掘进机的实时位姿的组合测量方法:惯性导航系统给出姿态信息,视觉测量给出两个方向上的位置信息,从而实现掘进机实时位姿的5自由度测量。首先对惯性导航和单目视觉测量方法进行简要介绍,然后给出了具体的测量方案,以及针对井下恶劣环境给出了光斑中心提取算法。在此基础上,作者依托神华神东煤炭集团下属大柳塔煤矿的快掘设备对本方法进行了实验验证,实验结果显示,姿态测量精度为0.1°,静态漂移的标准差不大于0.25°,定位精度优于1cm。本文中方法具有很强的应用价值,可以应用于掘进机的实时位姿测量。

1.   测量原理
  • 惯性导航[7]是利用载体上安装的陀螺和加速度计测量到的载体相对惯性空间的角速度信息和比力信息,通过解算常微分方程组,得到载体在导航坐标系中的位置、速度、姿态等信息。根据惯性导航系统(inertial navigation system, INS)是否具有实体的物理平台来划分,惯性导航可以分为以下几类:(1)平台式惯性导航系统(gimballed inertial navigation system,GINS)具有实体的机电平台系统,陀螺、加速度计均安装在该机电台体上; (2)捷联式惯性导航系统没有实体的机电平台系统,只有虚拟的数学平台,这样省略了复杂的物理实体平台,结构简单、体积小、重量轻、可靠性高,还可以通过余度技术提高系统的容错能力。在越来越多的场合,SINS已经逐渐取代了GINS。图 1为SINS的原理简图,图中v为速度,p为位置。

    Figure 1.  Schematic diagram of SINS

    图 1所示,陀螺仪用来测量运载体的角运动信息, 加速度计用来测量线运动信息,然后机载计算机根据这些测量信息计算出运载体的航向、姿态、速度及位置。假设姿态精度为0.25°,在补偿重力加速度g时,水平加速度误差可高达sin0.25°=4.4μg,也就是约为4cm/s2。要想在1min内保持1cm的位置测量精度,即使在加速度计完全理想的情况下,也要求陀螺漂移小于0.003°/h,一般陀螺精度根本无法达到要求。可见仅凭捷联惯导单独对位姿进行高精度测量是无法实现的。

  • 视觉测量是以计算机视觉为基础的新型测量技术,按视觉传感器数量可以分为单目视觉测量、双目视觉(立体视觉)测量和多目视觉测量等。单目视觉测量即指只使用一台相机或摄像机拍摄一张像片来完成测量工作[8]

    根据相机的透视成像模型[9],设世界坐标系中一点为Q(Xw, Yw, Zw),建立其与像点q(u, v)像素坐标的关系如下:

    式中,axayu, v轴上的尺度因子,也称为u, v轴上的等效(归一化)焦距;u0, v0为图像的中心点像素坐标;R是相机坐标系与世界坐标系之间的旋转矩阵(姿态矩阵); T是坐标系原点之间的平移矩阵。

    当相机安装在固定位置时,相机的外参量矩阵是不变的;但当相机安装在运动载体上时,相机相对于载体坐标系是不变的,而相对于世界坐标系却是变化的。

    由此,将相机安装在掘进机上进行测量时[10],需要知道相机的实时姿态(即姿态矩阵R)。虽然捷联惯导不能单独进行位姿测量,但3自由度的姿态测量却是十分精确。由此本文中提出的组合位姿测量方案即利用捷联惯导测量掘进机姿态角,并将该姿态角传递给视觉测量的外参量矩阵,再由视觉测量给出掘进机的位置偏差。

  • 在进行测量前,首先需要建立掘进机的姿态模型:在不考虑路面颠簸的情况下,掘进机的姿态可以用刚体的姿态角模型表示。刚体的姿态矩阵可用绕x, y, z3个坐标轴的旋转角ω, φ, β来表示。旋转角沿着坐标轴的方向望去,旋转方向逆时针为正,旋转矩阵R为:

    而捷联惯导姿态更新采用四元数微分方程,矩阵形式如下:

    式中,ωnbb为载体坐标系相对于导航坐标系的旋转角速度,即为载体坐标系b相对导航坐标系n的旋转角速度矢量在载体坐标系b中的表示。ωnbb的获取按下式进行:

    式中,ωibb是捷联陀螺的输出, 它是载体坐标系相对于惯性坐标系的旋转角速度,又称载体的绝对角速度;(ωien+ωenn)合称牵连角速度,是由地球自转和位置运动引起的有害角速度,ωienωenn分别是位置速率和地球自转速率;i表示地心惯性坐标系、e表示地球坐标系、g表示地理坐标系。

    四元数可确定出b系(载体坐标系)到n系(导航坐标系)的坐标变化矩阵:

    记,由于旋转过程中坐标系始终保持直角坐标系(正交坐标系),所以:

    设掘进机的航向角为Ψ(习惯上以北偏东为正),俯仰角为θ,横滚角为γ,并规定x, y, z的指向依次为东、北、天,则比较(2)式和(7)式可得:

    位置测量示意图 2所示。图中1号光斑为矿用激光指向仪光斑,2号光斑和3号光斑为引入的一组参考光斑,参考光斑由两束与相机光轴(Yc轴)平行的点激光器(与激光指向仪的绿光波长相同)提供。在测量景深(3.0±0.2)m内,通过机械上的调整,两光斑在Xc轴方向距离可以保证为(100.0±2.0)mm,以此作为测量的参考基准。CCD相机和两个点激光器一起固定在掘进机侧壁上,而捷联惯导系统一般固定在掘进机侧下方。

    Figure 2.  Instrumentation plan

    本文中定义如下正交坐标系,相机坐标系为Oc-XcYcZc(Yc即为相机光轴方向),捷联惯导坐标系为Os-XsYsZs,虚拟世界坐标系Ow-XwYwZw,并且Ow-XwYw面与水平面平行,OwZw轴与重力方向相反。

    设相机坐标系到捷联惯导坐标系的旋转矩阵为Rcs,捷联惯导坐标系到世界坐标系的旋转矩阵为Rsw,则相机坐标系和世界坐标系之间的关系可以表示为:

    式中,Rcs可以利用参考文献[11]和参考文献[12]中的方法标定得到,Rsw则由捷联惯导系统通过姿态更新得到。

    为方便起见,一般假设Ow-XwYwZwOc-XcYcZc的坐标原点重合,即(2)式中T=000]TR=Rwc

    实际拍摄到的光斑如图 3所示,由于巷道一般曲率很小,可以保证1号光斑(激光指向仪)始终在最左方,以此区分3个光斑。根据参考光斑2和参考光斑3在Xc轴方向上距离不变,利用比例关系即可求得垂直偏移和水平偏移。

    Figure 3.  Facula on image plane

  • 对于井下恶劣环境,视觉测量不免受到杂散光源、粉尘,甚至水雾的影响。本文中提出的光斑中心提取算法,主要分为以下4步。

  • 矿工头灯、矿用探照灯等光线是杂散光源的主要来源,图 4a为滤除杂散光源前的拍摄图,可见出现大面积干扰光源。通过杂散光波的波长与激光指向仪发出的绿光(535nm)的波长不同,在相机镜头前安装窄带滤光片可以滤除这些杂散光源,滤光后效果如图 4b所示。

    Figure 4.  Effect of the optical filter

  • 对于杂散光斑,主要是由煤壁上反射光照射到空气中粉尘产生。在掘进机掘进过程中会产生大量粉尘,必须将这些杂散光斑与主光斑分离开来。首先对图像进行均值滤波预处理,规避光斑边界处不规则,光斑距离较近的影响;再依据杂散光斑和主光斑面积大小的不同,根据ZHANG等人提出的一种四连通序贯算法[13-15],采用连通域标记的方法可以将杂散光斑和主光斑分离。若不进行均值滤波预处理,四连通序贯算法容易将主光斑和杂散光斑识别为一体。图 5为分离杂散光斑前后对比图。

    Figure 5.  Distinguish of the stray facula

  • 采用质心法提取激光光斑中心首先要确定光斑边界。根据连通域标记可知主光斑的大致位置,以此作为Sobel算子提取边界的大致区域;取Sobel算子确定的光斑左右、上下边界组成的矩形作为质心法的计算区域。

  • 首先设置背景阈值,对边界确定区域内图像做阈值化处理,低于背景阈值的点灰度值均为0;再采用平方加权重心法突出离中心较近的较大灰度值像素点对中心位置的影响。通过重复性实验对此光斑提取算法进行评价,光斑中心提取的重复精度在0.11pixel内。

2.   实验验证
  • 实验中选用激光捷联惯导系统,测量精度0.1°;相机选用Toshiba Teli公司的CS8600标准模拟黑白数字照相机;中心波长535nm(绿光)的点激光器及相应滤光片;作为长度参考的光电测距仪测量精度为±2mm。

  • 对于激光捷联惯导系统,姿态测量精度虽为0.1°,但在井下环境随时间漂移大小未知。在掘进机保持静态时,每隔30min记录激光捷联惯导的欧拉角输出,如表 1所示。

    No. yaw/(°) pitch/(°) roll/(°)
    1 -40.8119 0.5180 1.6826
    2 -40.7728 0.5064 1.7511
    3 -40.7830 0.4416 1.8270
    4 -40.7391 0.3238 1.9184
    5 -40.7362 0.2488 1.9446
    6 -40.7212 0.1463 1.9197
    7 -40.7352 0.0450 1.8611
    8 -40.7747 -0.0735 1.4261
    9 -40.7643 -0.0846 1.4956
    10 -40.8075 -0.0963 1.5789
    11 -40.8003 -0.0850 1.7174
    12 -40.7572 -0.0262 1.8116

    Table 1.  Euler angles

    表 1中各姿态角基本恒定,进一步分析欧拉角统计特征值,如表 2所示,其标准差不大于0.25°,能够满足实际应用中的精度需求。

    angle maximum/(°) minimum/(°) average/(°) standard deviation
    yaw -40.7212 -40.8119 -40.767 0.029
    pitch 0.5180 -0.0963 0.155 0.233
    roll 1.9446 1.4261 1.745 0.171

    Table 2.  Statistical characteristics of Euler angles

  • 在神华神东煤炭集团下属大柳塔煤矿的快掘设备上,对本测量方案进行了位置测量精度实验,测试现场如图 6所示,测试数据如表 3所示。使用光电测距仪测得数据作为参考值进行比对,可以看到单目视觉定位精度最大测量误差在1cm以内,满足了设计需求。

    Figure 6.  Calibration schematic of experimental set-cup

    No. horizontal measured value/mm horizontal reference value/mm deviation/mm vertical measured value/mm vertical reference value/mm deviation/mm
    1 360.5 356.4 3.9 450.4 446.2 4.2
    2 330.9 334.0 -3.1 448.1 444.9 3.2
    3 280.6 289.7 -9.1 410.1 412.3 -2.2
    4 290.2 295.7 -5.5 415.7 420.0 -4.3
    5 313.4 310.0 3.4 420.6 422.7 -2.1
    6 320.0 315.5 4.5 435.5 434.9 0.6
    7 336.2 340.8 -4.6 440.2 441.9 -1.7
    8 305.9 311.7 -5.8 432.8 437.0 -4.8

    Table 3.  Position precision

3.   结论
  • 本文中基于煤矿生产中对掘进机实时位姿自动检测技术的迫切需求,通过分析比较了现有几种位姿测量方法的优缺点,提出了一种基于视觉/惯导的掘进机位姿组合测量方法,依托大柳塔煤矿的快掘设备,对这种方法进行了实验验证。实验结果显示,姿态测量精度为0.1°,静态漂移的标准差不大于0.25°, 位置定位精度在1cm以内,满足了现阶段掘进机实时位姿的测量精度需求。

    在今后的工作中,有如下几方面问题需要进一步研究:(1)在掘进机行进过程中,由于路面颠簸等因素,掘进机使用刚体模型进行运动分析不够准确,如何对之进行准确建模和分析需要进一步研究;(2)单目视觉对位置测量的精度还有很大提高空间,需进一步分析测量误差来源,而建立误差关系等还有待于深入研究。

Reference (15)

Catalog

    /

    DownLoad:  Full-Size Img  PowerPoint
    Return
    Return