-
惯性导航[7]是利用载体上安装的陀螺和加速度计测量到的载体相对惯性空间的角速度信息和比力信息,通过解算常微分方程组,得到载体在导航坐标系中的位置、速度、姿态等信息。根据惯性导航系统(inertial navigation system, INS)是否具有实体的物理平台来划分,惯性导航可以分为以下几类:(1)平台式惯性导航系统(gimballed inertial navigation system,GINS)具有实体的机电平台系统,陀螺、加速度计均安装在该机电台体上; (2)捷联式惯性导航系统没有实体的机电平台系统,只有虚拟的数学平台,这样省略了复杂的物理实体平台,结构简单、体积小、重量轻、可靠性高,还可以通过余度技术提高系统的容错能力。在越来越多的场合,SINS已经逐渐取代了GINS。图 1为SINS的原理简图,图中v为速度,p为位置。
如图 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)像素坐标的关系如下:
$ \rho \left[ {\begin{array}{*{20}{c}} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{a_x}}&0&{{u_0}}&0\\ 0&{{a_y}}&{{v_0}}&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} \mathit{\boldsymbol{R}}&\mathit{\boldsymbol{T}}\\ {{{\bf{0}}^{\rm{T}}}}&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{X_{\rm{w}}}}\\ {{Y_{\rm{w}}}}\\ {{Z_{\rm{w}}}}\\ 1 \end{array}} \right] $
(1) 式中,ax,ay为u, v轴上的尺度因子,也称为u, v轴上的等效(归一化)焦距;u0, v0为图像的中心点像素坐标;R是相机坐标系与世界坐标系之间的旋转矩阵(姿态矩阵); T是坐标系原点之间的平移矩阵。
当相机安装在固定位置时,相机的外参量矩阵是不变的;但当相机安装在运动载体上时,相机相对于载体坐标系是不变的,而相对于世界坐标系却是变化的。
由此,将相机安装在掘进机上进行测量时[10],需要知道相机的实时姿态(即姿态矩阵R)。虽然捷联惯导不能单独进行位姿测量,但3自由度的姿态测量却是十分精确。由此本文中提出的组合位姿测量方案即利用捷联惯导测量掘进机姿态角,并将该姿态角传递给视觉测量的外参量矩阵,再由视觉测量给出掘进机的位置偏差。
-
在进行测量前,首先需要建立掘进机的姿态模型:在不考虑路面颠簸的情况下,掘进机的姿态可以用刚体的姿态角模型表示。刚体的姿态矩阵可用绕x, y, z3个坐标轴的旋转角ω, φ, β来表示。旋转角沿着坐标轴的方向望去,旋转方向逆时针为正,旋转矩阵R为:
$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{R}} = {\mathit{\boldsymbol{R}}_\beta }{\mathit{\boldsymbol{R}}_\varphi }{\mathit{\boldsymbol{R}}_\omega } = \left[ {\begin{array}{*{20}{c}} {\cos \beta }&{ - \sin \beta }&0\\ {\sin \beta }&{\cos \beta }&0\\ 0&0&1 \end{array}} \right] \times }\\ {\left[ {\begin{array}{*{20}{c}} {\cos \varphi }&0&{ - \sin \varphi }\\ 0&1&0\\ {\sin \varphi }&0&{\cos \varphi } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{\cos \omega }&{ - \sin \omega }\\ 0&{\sin \omega }&{\cos \omega } \end{array}} \right]} \end{array} $
(2) 而捷联惯导姿态更新采用四元数微分方程,矩阵形式如下:
$ {\rm{d}}\mathit{\boldsymbol{Q}}/{\rm{d}}t = \mathit{\boldsymbol{M'}}\left( {\mathit{\boldsymbol{\omega }}_{{\rm{nb}}}^{\rm{b}}} \right)\mathit{\boldsymbol{Q}}/2 $
(3) $ \left[ {\begin{array}{*{20}{c}} {{{\dot q}_0}}\\ {{{\dot q}_1}}\\ {{{\dot q}_2}}\\ {{{\dot q}_3}} \end{array}} \right] = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} 0&{ - {\omega _x}}&{ - {\omega _y}}&{ - {\omega _z}}\\ {{\omega _x}}&0&{{\omega _z}}&{ - {\omega _y}}\\ {{\omega _y}}&{ - {\omega _z}}&0&{{\omega _x}}\\ {{\omega _z}}&{{\omega _y}}&{ - {\omega _x}}&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{q_0}}\\ {{q_1}}\\ {{q_2}}\\ {{q_3}} \end{array}} \right] $
(4) 式中,ωnbb为载体坐标系相对于导航坐标系的旋转角速度,即为载体坐标系b相对导航坐标系n的旋转角速度矢量在载体坐标系b中的表示。ωnbb的获取按下式进行:
$ \mathit{\boldsymbol{\omega }}_{{\rm{nb}}}^{\rm{b}} = \mathit{\boldsymbol{\omega }}_{{\rm{ib}}}^{\rm{b}} - \mathit{\boldsymbol{C}}_{\rm{b}}^{\rm{n}}\left( {\mathit{\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{b}} + \mathit{\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{b}}} \right) $
(5) 式中,ωibb是捷联陀螺的输出, 它是载体坐标系相对于惯性坐标系的旋转角速度,又称载体的绝对角速度;(ωien+ωenn)合称牵连角速度,是由地球自转和位置运动引起的有害角速度,ωien和ωenn分别是位置速率和地球自转速率;i表示地心惯性坐标系、e表示地球坐标系、g表示地理坐标系。
四元数可确定出b系(载体坐标系)到n系(导航坐标系)的坐标变化矩阵:
$ \begin{array}{*{20}{c}} {\mathit{\boldsymbol{C}}_{\rm{b}}^{\rm{n}} = }\\ {\left[ {\begin{array}{*{20}{c}} {q_0^2 + q_1^2 - q_2^2 - q_3^2}&{2\left( {{q_1}{q_2} - {q_0}{q_3}} \right)}&{2\left( {{q_1}{q_3} + {q_0}{q_2}} \right)}\\ {2\left( {{q_1}{q_2} + {q_0}{q_3}} \right)}&{q_0^2 - q_1^2 + q_2^2 - q_3^2}&{2\left( {{q_2}{q_3} - {q_0}{q_1}} \right)}\\ {2\left( {{q_1}{q_3} - {q_0}{q_2}} \right)}&{2\left( {{q_2}{q_3} + {q_0}{q_1}} \right)}&{q_0^2 - q_1^2 - q_2^2 + q_3^2} \end{array}} \right]} \end{array} $
(6) 记,由于旋转过程中坐标系始终保持直角坐标系(正交坐标系),所以:
$ \mathit{\boldsymbol{C}}_{\rm{b}}^{\rm{n}} = {\left( {\mathit{\boldsymbol{C}}_{\rm{b}}^{\rm{n}}} \right)^{\rm{T}}} = \left[ {\begin{array}{*{20}{c}} {{T_{11}}}&{{T_{21}}}&{{T_{31}}}\\ {{T_{12}}}&{{T_{22}}}&{{T_{32}}}\\ {{T_{13}}}&{{T_{23}}}&{{T_{33}}} \end{array}} \right] $
(7) 设掘进机的航向角为Ψ(习惯上以北偏东为正),俯仰角为θ,横滚角为γ,并规定x, y, z的指向依次为东、北、天,则比较(2)式和(7)式可得:
$ \left\{ \begin{array}{l} \theta = \arcsin {T_{32}}\\ {\gamma _{\rm{p}}} = \arctan \left( { - {T_{31}}/{T_{33}}} \right)\\ {\mathit{\Psi }_{\rm{p}}} = \arctan \left( {{T_{12}}/{T_{22}}} \right) \end{array} \right. $
(8) 位置测量示意图 2所示。图中1号光斑为矿用激光指向仪光斑,2号光斑和3号光斑为引入的一组参考光斑,参考光斑由两束与相机光轴(Yc轴)平行的点激光器(与激光指向仪的绿光波长相同)提供。在测量景深(3.0±0.2)m内,通过机械上的调整,两光斑在Xc轴方向距离可以保证为(100.0±2.0)mm,以此作为测量的参考基准。CCD相机和两个点激光器一起固定在掘进机侧壁上,而捷联惯导系统一般固定在掘进机侧下方。
本文中定义如下正交坐标系,相机坐标系为Oc-XcYcZc(Yc即为相机光轴方向),捷联惯导坐标系为Os-XsYsZs,虚拟世界坐标系Ow-XwYwZw,并且Ow-XwYw面与水平面平行,OwZw轴与重力方向相反。
设相机坐标系到捷联惯导坐标系的旋转矩阵为Rcs,捷联惯导坐标系到世界坐标系的旋转矩阵为Rsw,则相机坐标系和世界坐标系之间的关系可以表示为:
$ \left\{ \begin{array}{l} \mathit{\boldsymbol{R}}_{\rm{c}}^{\rm{w}} = \mathit{\boldsymbol{R}}_{\rm{c}}^{\rm{s}}\mathit{\boldsymbol{R}}_{\rm{s}}^{\rm{w}}\\ \mathit{\boldsymbol{R}}_{\rm{w}}^{\rm{c}} = {\left( {\mathit{\boldsymbol{R}}_{\rm{c}}^{\rm{w}}} \right)^{ - 1}} = {\left( {\mathit{\boldsymbol{R}}_{\rm{c}}^{\rm{w}}} \right)^{\rm{T}}} \end{array} \right. $
(9) 式中,Rcs可以利用参考文献[11]和参考文献[12]中的方法标定得到,Rsw则由捷联惯导系统通过姿态更新得到。
为方便起见,一般假设Ow-XwYwZw和Oc-XcYcZc的坐标原点重合,即(2)式中T=000]T,R=Rwc。
实际拍摄到的光斑如图 3所示,由于巷道一般曲率很小,可以保证1号光斑(激光指向仪)始终在最左方,以此区分3个光斑。根据参考光斑2和参考光斑3在Xc轴方向上距离不变,利用比例关系即可求得垂直偏移和水平偏移。
-
对于井下恶劣环境,视觉测量不免受到杂散光源、粉尘,甚至水雾的影响。本文中提出的光斑中心提取算法,主要分为以下4步。
-
矿工头灯、矿用探照灯等光线是杂散光源的主要来源,图 4a为滤除杂散光源前的拍摄图,可见出现大面积干扰光源。通过杂散光波的波长与激光指向仪发出的绿光(535nm)的波长不同,在相机镜头前安装窄带滤光片可以滤除这些杂散光源,滤光后效果如图 4b所示。
-
对于杂散光斑,主要是由煤壁上反射光照射到空气中粉尘产生。在掘进机掘进过程中会产生大量粉尘,必须将这些杂散光斑与主光斑分离开来。首先对图像进行均值滤波预处理,规避光斑边界处不规则,光斑距离较近的影响;再依据杂散光斑和主光斑面积大小的不同,根据ZHANG等人提出的一种四连通序贯算法[13-15],采用连通域标记的方法可以将杂散光斑和主光斑分离。若不进行均值滤波预处理,四连通序贯算法容易将主光斑和杂散光斑识别为一体。图 5为分离杂散光斑前后对比图。
-
采用质心法提取激光光斑中心首先要确定光斑边界。根据连通域标记可知主光斑的大致位置,以此作为Sobel算子提取边界的大致区域;取Sobel算子确定的光斑左右、上下边界组成的矩形作为质心法的计算区域。
-
首先设置背景阈值,对边界确定区域内图像做阈值化处理,低于背景阈值的点灰度值均为0;再采用平方加权重心法突出离中心较近的较大灰度值像素点对中心位置的影响。通过重复性实验对此光斑提取算法进行评价,光斑中心提取的重复精度在0.11pixel内。
-
实验中选用激光捷联惯导系统,测量精度0.1°;相机选用Toshiba Teli公司的CS8600标准模拟黑白数字照相机;中心波长535nm(绿光)的点激光器及相应滤光片;作为长度参考的光电测距仪测量精度为±2mm。
-
对于激光捷联惯导系统,姿态测量精度虽为0.1°,但在井下环境随时间漂移大小未知。在掘进机保持静态时,每隔30min记录激光捷联惯导的欧拉角输出,如表 1所示。
Table 1. Euler angles
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 表 1中各姿态角基本恒定,进一步分析欧拉角统计特征值,如表 2所示,其标准差不大于0.25°,能够满足实际应用中的精度需求。
Table 2. Statistical characteristics of Euler angles
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 -
在神华神东煤炭集团下属大柳塔煤矿的快掘设备上,对本测量方案进行了位置测量精度实验,测试现场如图 6所示,测试数据如表 3所示。使用光电测距仪测得数据作为参考值进行比对,可以看到单目视觉定位精度最大测量误差在1cm以内,满足了设计需求。
Table 3. Position precision
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
基于视觉/惯导的掘进机实时位姿测量方法研究
Study on measurement method of realtime position and attitude of roadheader based on vision/inertial navigation system
-
摘要: 针对煤矿井下掘进机实时位姿自动检测的难题,通过分析比较现有几种位姿测量方法的优缺点,提出了一种基于视觉/惯导的掘进机位姿组合测量方法。该方法通过激光捷联式惯导系统得到掘进机的姿态信息、单目视觉测量掘进机的位置信息,从而实现掘进机实时位姿的5自由度测量。依托大柳塔煤矿的快掘设备对该方法进行了实验验证。结果表明,姿态测量精度为0.1°,静态漂移的标准差不大于0.25°,位置定位精度优于1cm。该方法能够用于掘进机实时位姿测量,具有广阔的工程应用前景。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.
-
Table 1. Euler angles
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 2. Statistical characteristics of Euler angles
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 3. Position precision
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 -
[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.