0 引言
掘进机高精度姿态参数测量是实现其自主导航和定向截割的基础。现有掘进机姿态参数测量方法主要有基于激光靶[1-3]、基于视觉测量[4-6]、基于多点测量[7-9]和基于航位推算的方法[10]。基于激光靶的方法一般只能获得部分位姿参数，若要获得全部位姿参数，会导致测量系统复杂性较高，成熟度不足。基于视觉测量的方法解算较复杂，且易受环境干扰而失效。基于多点测量的方法一般需要先计算出机身多个点的坐标，再据此计算机身的姿态，因此姿态角测量误差较大，且测量实时性较差。基于航位推算的方法存在累积误差，且惯导的价格较为昂贵。
双轴倾角仪具有产品成熟、价格便宜、解算简单、测量频率高、不易受外界环境干扰等优点，可以实现较可靠的角度测量。然而，不少掘进机位姿测量研究中对双轴倾角仪的使用存在误区，即将双轴倾角仪输出的倾角直接作为掘进机的俯仰角和横滚角。实际上，掘进机的俯仰角和横滚角分别通过机身坐标系Y轴和X轴向巷道坐标系平面投影的方式定义[11]，因此，必须同时确定机身坐标系Y轴和X轴在巷道坐标系中的方向，才能同时得到掘进机的俯仰角和横滚角。根据双矢定姿原理[12]，必须首先得到掘进机机身坐标系与巷道坐标系的姿态转换矩阵，才能解算出俯仰角和横滚角。而通过双轴倾角仪可以解算的倾角是两轴相对于大地参考水平面、通过欧拉角定义的俯仰角和翻滚角[13]。
在水平巷道中，仅使用双轴倾角仪无法得到航向信息，从而无法确定掘进机机身坐标系和巷道坐标系的姿态转换矩阵，所以不能解算出掘进机的俯仰角和横滚角。在倾斜巷道中，尽管巷道倾角已知，但是由于缺少航向信息，不能将双轴倾角仪相对于水平面的参数转换到巷道坐标系，同样不能解算出掘进机的俯仰角和横滚角。因此，不论是在水平巷道还是倾斜巷道中，单独使用双轴倾角仪均无法进行俯仰角和横滚角测量，需将双轴倾角仪与能获取掘进机航向信息的定位方法进行融合，才能同时解算出掘进机全姿态参数。
本文首先介绍了掘进机姿态参数测量及双轴倾角仪测量的原理，以此为基础，针对水平巷道和倾斜巷道2种情况，对融合扇形激光与双轴倾角仪、融合机身两点坐标与双轴倾角仪2种掘进机姿态解算方法进行了推导和验证。
1 掘进机姿态参数测量及双轴倾角仪测量原理
1.1 掘进机姿态参数测量原理
掘进机姿态参数测量坐标系如图1所示。设巷道坐标系为{h}，其中Yh轴与巷道设计中轴线方向一致，Zh轴垂直于巷道底板。设机身坐标系为{b}，其中Xb轴为机身横轴，Yb轴为机身中线。设安装在掘进机上的双轴倾角仪的坐标系为{q}，假设不考虑双轴倾角仪的角度安装误差，则双轴倾角仪坐标系与掘进机机身坐标系对应轴方向一致。在实际掘进过程中，由于许多巷道具有一定坡度，所以还需要解析双轴倾角仪与巷道水平坐标系{d}之间的关系。巷道水平坐标系原点Od与巷道坐标系原点Oh重合，Xd轴与Xh轴重合，Zd轴与该坐标系原点处的水平面垂直，Yh轴与Yd轴的夹角为θ，即倾斜巷道的坡度。
图1 掘进机姿态参数测量坐标系
Fig.1 Coordinate system of attitude parameter measurement for roadheader
掘进机位姿解算就是通过机身坐标系与巷道坐标系的相对位姿变换关系，获得掘进机相对于巷道坐标系的三维坐标(x,y,z)、偏航角α、俯仰角β和横滚角γ。将机身坐标系平移到巷道坐标系原点，如图2所示。偏航角α为Yb轴在巷道坐标系XhOhYh上的投影与Yh轴的夹角；俯仰角β为Yb轴在YhOhZh上的投影与Yh轴的夹角；横滚角γ为Xb轴在XhOhZh上的投影与Xh轴的夹角。
图2 掘进机姿态参数
Fig.2 Roadheader attitude parameters
为坐标系{b}到坐标系{h}的旋转矩阵，
中的第1行第2列元素，后文中沿用此标记约定。若已知姿态转换矩
则可计算出掘进机的偏航角α、俯仰角β和横滚角γ。
(1)
(2)
1.2 双轴倾角仪测量原理
双轴倾角仪通过检测重力方向来测量Xq轴和Yq轴与水平面的夹角ωr和ωp，如图3(a)所示。考虑到移动设备的航向角α′、俯仰角β′和翻滚角γ′采用Z-X-Y(旋转轴顺序)方式的欧拉角定义[14]，根据双轴倾角仪坐标系{q}和欧拉角的定义，基于O点建立与测量参考坐标系方向一致的坐标系{1}；随后，将该坐标系绕Z1轴旋转α′后形成坐标系{2}，如图3(b)所示；再将坐标系{2}绕X2轴旋转β′后，形成坐标系{3}，如图3(c)所示；最后，将坐标系{3}绕Y3轴旋转γ′，形成坐标系{q}，如图3(d)所示。对比图2中的α、β和γ与图3中的α′、β′和γ′可知，即使在水平巷道中，β与β′、γ与γ′均不相等，进一步说明了不能仅通过双轴倾角仪解算掘进机俯仰角和横滚角。
(a) 倾角仪测量参数
(b) 坐标系{1}绕Z1轴旋转α′
(c) 坐标系{2}绕X2轴旋转β′
(d) 坐标系{3}绕Y3轴旋转γ′
图3 双轴倾角仪测量参数转换
Fig.3 Conversion of measurement parameters of biaxial inclinometer
在图3(d)中，将Yq轴上的B点向水平面投影得到B′，则∠BOB′=ωp。显然，B′在Y2轴上，则β′=ωp。将Xq轴上的A点向水平面投影得到A′，则∠AOA′=ωr。因为A′不在X2轴上，所以ωr≠γ′。坐标系{q}到坐标系{2}的旋转矩阵定义为
(3)
Xq轴与水平面的夹角为ωr，则Xq轴与Z2轴正方向的夹角为
根据式(3)可
则有
sin γ′=sin ωr/cos β′
(4)
基于此，通过双轴倾角仪可以解算出移动设备与水平面间通过欧拉角方式表达的俯仰角β′和翻滚角γ′。
2 水平巷道中掘进机姿态解算方法
水平巷道掘进时，巷道坐标系的XhOhYh面在水平面上，如图4所示。当采用不同偏航角测量方法时，其对应的俯仰角β和横滚角γ的解算方式存在差异。
图4 水平巷道中机身坐标系与竖直方向的关系
Fig.4 Relationship between roadheader body coordinate system and vertical direction in horizontal roadway
2.1 融合扇形激光和双轴倾角仪的姿态解算方法
将扇面激光发射器安装在巷道后方的顶板上，形成扇形激光束；通过平行安装在掘进机机身上方的2个激光标靶所获得的扇形激光束光斑位置，可实时确定掘进机相对于巷道坐标系的偏航角α[1]。
对比底板水平时掘进机偏航角α与欧拉角中航向角α′的定义可知，两者数值相等，即α=α′。通过双轴倾角仪可以获得掘进机机身相对于巷道坐标系的俯仰角β′和翻滚角γ′，则坐标系{b}与坐标系{h}的姿态转换矩阵为
(5)
基于式(1)和式(2)，可求得掘进机的偏航角α、俯仰角β和横滚角γ：
(6)
2.2 融合机身两点坐标和双轴倾角仪的姿态解算方法
当采用全站仪[15]、UWB(Ultra Wide Band，超宽带)[7]、iGPS(indoor GPS，室内全球定位系统)[16]与双轴倾角仪融合进行姿态解算时，先测量掘进机机身上2个待测点在巷道坐标系下的三维坐标，记为(X1h,Y1h,Z1h)，(X2h,Y2h,Z2h)，然后利用双矢定姿原理[12]确定掘进机姿态。2个待测点相对于机身坐标系的三维坐标(X1b,Y1b,Z1b)，(X2b,Y2b,Z2b)可通过标定获得。基于2个待测点的连线方向构造矢量v1，求得v1在机身坐标系和巷道坐标系下的表达v1b,v1h：
(7)
(8)
之间满足如下关系：
(9)
在水平巷道中，巷道坐标系{h}的Zh轴正方向竖直向上。因此，竖直向上的单位矢量v2在巷道坐标系中的表达为
(10)
设用双轴倾角仪获得的Xb轴和Yb轴与水平面的夹角分别为ωr与ωp，则两轴与水平面的法向量的夹角分别为ωr+π/2和π/2-ωp，v2在Xb轴和Yb轴上的投影分别为-sin ωr与sin ωp。单位矢量的模为1，则单位矢量v2在掘进机机身坐标系中的表达为[17]
(11)
之间满足如下关系：
(12)
基于v1和v2构造矢量v3=v1×v2。v3在巷道坐标系的映射为v3h=v1h×v2h，在掘进机机身坐标系的映射为v3b=v1b×v2b。结合式(9)和式
之间满足如下关系
(13)
当v1与v2不共线时，联立式(9)、式(12)和式(13)构造矩阵方程：
(14)
坐标系{b}到坐标系{h}的姿态转换矩
为
(15)
综合上述过程，结合式(1)、式(2)即可得到掘进机的姿态参数α,β和γ。
3 倾斜巷道中掘进机姿态解算方法
在倾斜巷道中，巷道坐标系的XhOhYh面与水平面不一致，如图5所示，为此,需要建立巷道水平坐标系{d}。此时，采用双轴倾角仪解算获得的是掘进机机身相对于巷道水平坐标系的俯仰角β″和翻滚角γ″，不是相对于巷道坐标系的姿态角。坐标系{d}可等效为坐标系{h}绕Xh轴顺时针旋转坡度角θ，因
为
(16)
式
图5 倾斜巷道中机身坐标系与竖直方向的关系
Fig.5 Relationship between roadheader body coordinate system and vertical direction in inclined roadway
在掘进巷道坡度已知的条件下，可确
进行水平巷道掘进时，可由式(10)计算v2在巷道水平坐标系的映射v2h，而在倾斜巷道掘进时，
除此以外，在倾斜巷道掘进时，融合机身两点坐标和双轴倾角仪的掘进机姿态解算方法与水平巷道工况下的方法一致。
在倾斜巷道中，通过扇形激光可直接测得掘进机相对巷道坐标系的偏航角α，则Yb轴在巷道底板上的投影与Yh轴之间的夹角为α。在掘进机机身坐标系的Yb轴上找一点A(图5)，使OA=1，则矢量OA在机身坐标系的映射v1b为
(17)
通过A点分别作巷道底面和巷道水平投影面的垂线，与2个面分别交于C点和B点。连接线段OC与OB，则∠AOB=β″，且|AB|=sin β″。通过C点作Yh轴的垂线CD，由掘进机偏航角α的定义可知∠COD=α。设∠AOC=β，则|AC|=sin β，|OC|=cos β，|CD|=|OC|sin α=cos β sin α，|OD|=|OC|cos α=cos β cos α。OA在巷道坐标系的映射为
(18)
因为式(18)中的sin β和cos β未知，所以需求解这2个量。将v1h变换到巷道水平坐标系，得到OA在巷道水平坐标系的映射：
(19)
由式(19)可得|AB|=sin θ cos β cos α+cos θ sin β，由前面分析可知|AB|=sin β″，联立两式得
sin β″=sin θ cos β cos α+cos θ sin β
(20)
根据sin2β+cos2β=1，结合式(20)，可求出掘进机上坡工况下，即θ∈[0°,20°]，α∈[-45°,45°]，β∈[-20°,20°]，γ∈[-20°,20°]时，满足如下关系：
(21)
(22)
掘进机在下坡工况下，即θ∈[-20°,0°]，α∈[-45°,45°]，β∈[-20°,20°]，γ∈[-20°,20°]时，cos β与上坡工况下一致，sin β满足如下关系：
(23)
将式(21)—式(23)代入式(18)可得到v1h。竖直方向的单位矢量v2在巷道水平坐标系的映射为
(24)
对矢量v2d进行转换，得到OA在巷道坐标系的映射：
(25)
采用类似于2.1节中坐标系{b}与坐标系{h}的姿态转换矩的计算方式，可解算出掘进机的姿态参数α,β和γ。
4 仿真分析
4.1 仿真模型搭建
为了验证本文给出的掘进机位姿解算方法的正确性，采用Gazebo搭建仿真环境[18]，构建水平巷道和倾斜巷道，如图6所示。通过在底板放置木板来模拟巷道中的起伏地形，控制差速小车在模拟巷道中运行，模拟掘进机在巷道中的行驶过程。在模拟过程中，采用Gazebo发布的小车机身翻滚角和俯仰角作为双轴倾角仪的测量值。验证水平巷道中融合扇形激光和双轴倾角仪的姿态解算方法时，采用Gazebo发布的小车机身偏航角作为扇形激光的测量值。在倾斜巷道中，需将发布的偏航角转换到巷道坐标系。为了方便描述，将融合扇形激光和双轴倾角仪的姿态解算方法记为方法1，将融合机身两点坐标和双轴倾角仪的姿态解算方法记为方法2，将把双轴倾角仪输出参数直接作为掘进机俯仰角和横滚角的方法记为原方法。
(a) 水平巷道
(b) 倾斜巷道
图6 模拟水平巷道和倾斜巷道
Fig.6 Simulation of horizontal roadway and inclined roadway
选取掘进机机身上同一高度的4个参考点A′(xA′,yA′,zA′),B′(xB′,yB′,zB′),C′(xC′,yC′,zC′),D′(xD′,yD′,zD′)，以D′A′和B′C′分别作为机身纵向的Yb轴和横向的Xb轴，如图7所示。采用rosbag命令行工具对各点坐标与机身参数进行录制，并将得到的bag文件导入Matlab进行姿态解算。
图7 掘进机机身上的4个参考点
Fig.7 Four reference points on roadheader
4.2 水平巷道中解算方法验证
验证水平巷道中的2种解算方法时，根据掘进机姿态参数定义方法，得到其偏航角、俯仰角和横滚角的参考真值αc,βc和γc。
(26)
将基于小车机身各点坐标与角度信息解算获得的姿态参数与参考真值进行比较，2种方法的测量误差曲线分别如图8和图9所示。解算值与参考真值误差的均值E和标准差δ见表1。
图8 水平巷道中方法1姿态参数测量误差
Fig.8 Attitude measurement error of method 1 in horizontal roadway
图9 水平巷道中方法2姿态参数测量误差
Fig.9 Attitude measurement error of method 2 in horizontal roadway
表1 水平巷道姿态参数误差均值与标准差
Table 1 mean value and standard deviation of attitude parameter error of horizontal roadway (°)
解算方法E(α)E(β)E(γ)δ(α)δ(β)δ(γ)原方法-0.0490.029- 0.100.16方法1--4.94×10-41.95×10-5-0.003 10.001 3方法2-0.004 9-5.59×10-4-1.25×10-180.0190.003 11.46×10-16
仿真分析结果表明，原方法的角度误差较大，而方法1和方法2解算误差均很小，最大角度误差均值为0.004 9°，说明在水平巷道中，这2种掘进机姿态解算方法是合理的。
4.3 倾斜巷道中解算方法验证
验证倾斜巷道中的2种解算方法时，将4个参考点A′,B′,C′,D′在世界坐标系下的坐标转换到巷道坐标系下，再根据掘进机姿态参数定义方法，得到偏航角、俯仰角和横滚角的参考真值：
(27)
设巷道倾角为-15，-10，10，15°，利用各倾角下测得的机身坐标与角度信息，采用2种方法进行姿态解算。将解算出的姿态参数与参考真值进行比较，得到测量误差曲线，如图10所示。解算值与参考真值误差的均值E和标准差δ见表2。
由图10和表2可见，在4种不同倾角的巷道中，原方法的误差较大，难以满足掘进机姿态参数测量需要；方法1和方法2的误差都很小，最大角度误差均值为0.025°，验证了在倾斜巷道中2种掘进机姿态解算方法的合理性。此外，图10中的误差曲线在中间段的波动是由于Gazebo本身存在一定的模型误差，且在运行中段小车方向发生较大改变时造成了误差的叠加。
图10 不同倾角巷道中2种方法的姿态参数测量误差
Fig.10 Attitude measurement errors of two methods in roadways with different inclination angles
表2 倾斜巷道姿态参数测量误差均值与标准差
Table 2 Mean and standard deviation of attitude parameter measurement error of inclined roadway (°)
巷道倾角解算方法E(α)E(β)E(γ)δ(α)δ(β)δ(γ)原方法--14.930.43-0.131.36-15方法1-8.33×10-40.025-0.002 50.03方法2-0.007 4-0.003 9-1.37×10-180.009 60.0095.53×10-17原方法--9.970.62-0.0260.33-10方法1-3.44×10-40.007 6-0.0010.005 8方法2-0.002 2-8.22×10-44.29×10-190.003 56.82×10-44.98×10-17原方法-9.910.514-0.141.2110方法1-7.75×10-4-0.004 3-0.001 30.008方法2-7.41×10-40.001 2-1.65×10-190.003 10.001 34.95×10-17原方法-14.59-0.95-0.413.3815方法1--0.0140.022-0.0260.018方法20.0170.0211.25×10-180.0190.0271.34×10-16
5 结论
(1) 将双轴倾角仪的输出角度直接作为掘进机的俯仰角、横滚角会造成较大测量误差，需将双轴倾角仪与其他姿态参数测量方式融合，才能同时解算出掘进机全姿态参数。
(2) 给出了融合扇形激光和双轴倾角仪的姿态解算方法、融合机身两点坐标和双轴倾角仪的姿态解算方法。2种解算方法在模拟水平巷道中的最大误差均值为0.004 9°，在模拟倾斜巷道中的最大误差均值为0.025°，提高了掘进机基于双轴倾角仪的角度测量精度，扩大了双轴倾角仪的适用范围，使其可在掘进机倾斜巷道姿态参数测量中使用。
(3) 由于给出的2种位姿解算方法不需要使用高精度惯导等昂贵设备，所以在中小煤矿的掘进机智能化改造中具有较强的经济适用性。但是，本文旨在推导和验证2种掘进机姿态解算方法，所以未考虑各传感器的测量误差，下一步将结合测量误差对各组合传感器的测量值进行滤波处理。
[1] 李睿.悬臂掘进机机身位姿参数检测系统研究[D].北京:中国矿业大学(北京),2012.
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 & Technology(Beijing),2012.
[2] 杜雨馨,刘停,童敏明,等.基于机器视觉的悬臂式掘进机机身位姿检测系统[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.
[3] 薛光辉,张云飞,候称心,等.基于激光靶向扫描的掘进机位姿测量方法[J].煤炭科学技术,2020,48(11):19-25.
XUE Guanghui,ZHANG Yunfei,HOU Chenxin,et al.Measurement of roadheader position and posture based on orientation laser scanning[J].Coal Science and Technology,2020,48(11):19-25.
[4] 田原.基于四点式光靶的掘进机自动定位方法研究[J].煤炭科学技术,2018,46(12):35-40.
TIAN Yuan.Research on automatic positioning method of roadheader based on four point light target[J].Coal Science and Technology,2018,46(12):35-40.
[5] YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Laser beams-based localization methods for boom-type roadheader using underground camera non-uniform blur model[J].IEEE Access,2020,8:190327-190341.
[6] 杨文娟,张旭辉,马宏伟,等.悬臂式掘进机机身及截割头位姿视觉测量系统研究[J].煤炭科学技术,2019,47(6):50-57.
YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Research on position and posture measurement system of body and cutting head for boom-type roadheader based on machine vision[J].Coal Science and Technology,2019,47(6):50-57.
[7] 符世琛,李一鸣,杨健健,等.基于超宽带技术的掘进机自主定位定向方法研究[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.
[8] FU Shichen,LI Yiming,ZONG Kai,et al.Ultra-wideband pose detection method based on TDOA positioning model for boom-type roadheader[J].AEU-International Journal of Electronics and Communications,2018,99:70-80.
[9] 吴淼,贾文浩,华伟,等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[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.
[10] 田原.悬臂式掘进机自动定位技术研究与试验[J].矿山机械,2019,47(7):6-10.
TIAN Yuan.Research and test on automatic positioning technology for boomed roadheader[J].Mining & Processing Equipment,2019,47(7):6-10.
[11] 田原.悬臂掘进机自动导向和定位技术探索[J].工矿自动化,2010,36(8):26-29.
TIAN Yuan.Discussion of automatic guidance and location technology for cantilever excavator[J].Industry and Mine Automation,2010,36(8):26-29.
[12] 严恭敏,翁浚.捷联惯导算法与组合导航原理[M].西安:西北工业大学出版社,2019.
YAN Gongmin,WENG Jun.Strapdown inertial navigation algorithm and integrated navigation principle[M].Xi'an:Northwestern Polytechnical University Press,2019.
[13] 佟刚,王涛,吴志勇,等.高精度倾角传感器在测量车载平台变形中的应用[J].光学精密工程,2010,18(6):1347-1353.
TONG Gang,WANG Tao,WU Zhiyong,et al.Application of high accuracy inclinometer to deformation measurement for vehicular platform[J].Optics and Precision Engineering,2010,18(6):1347-1353.
[14] 约翰J.克雷格.机器人学导论[M].贠超,王伟,译.北京:机械工业出版社,2018.
CRAIG J J.Introduction to robotics[M].YUN Chao,WANG Wei,translate.Beijing:China Machine Press,2018.
[15] 陶云飞.掘进机位姿激光自动测量方法及系统研究[D].北京:中国矿业大学(北京),2017.
TAO Yunfei.Research on automatic measurement method and system of position and attitude of roadheader based on laser measurement system[D].Beijing:China University of Mining & Technology(Beijing),2017.
[16] 陶云飞,宗凯,张敏骏,等.基于iGPS的掘进机单站多点分时机身位姿测量方法[J].煤炭学报,2015,40(11):2611-2616.
TAO Yunfei,ZONG Kai,ZHANG Minjun,et al.A position and orientation measurement method of single-station,multipoint and time-sharing for roadheader body based on iGPS[J].Journal of China Coal Society,2015,40(11):2611-2616.
[17] GAO Yang,LIN Jiarui,HE Feiyan,et al.A monocular vision and inclinometer combined system for 6DOF measurement of double shield TBM[J].Sensors and Actuators A:Physical,2016,249:155-162.
[18] 胡春旭.ROS机器人开发实践[M].北京:机械工业出版社,2018.
HU Chunxu.ROS robot development practice[M].Beijing:China Machine Press,2018.
