一种基于双矢量的煤矿掘进机姿态测量方法

：
1.本发明属于煤矿掘进机自动化技术，尤其涉及一种基于双矢量的煤矿掘进机姿态测量方法。


背景技术：

2.目前在煤矿掘进过程中，受限于位姿传感与测量方式的不确定性，掘进机仍然难以完全实现自主导航。如何在地下封闭、恶劣且未知的煤矿巷道中进行有效的传感信息获取与位姿解算，是实现掘进机自主导航进而尽快将井下施工作业人员从高危、高强度的采掘劳动中解脱出来的核心问题。在大部分导航算法中，姿态为位置的解算前提。而相比于位置，姿态信息的准确解算通常更为困难。
3.现存解决掘进机姿态结算问题的方法可以分成两类，第一类为“航迹推测”方法，以惯性导航系统为代表，其特点是独立、自主、稳定可靠且短时精度高，缺点是误差随时间累积且成本昂贵；第二类是“绝对定位”方法，以基于uwb、全站仪、视觉等依赖基站布设的测量方式为代表，优点是精度高，缺点是采煤过程中产生的大量粉尘会降低成像质量，且“有源”传感系统需要人工设站和标定，这在复杂恶劣的煤矿施工环境下的很难实现。
4.在目前的巷道掘进施工中，巷道挖掘的质量主要依靠人工保证。掘进机司机在控制机身掘进的同时同步观察高能激光指向仪投影在前方工作面的光斑，基于光斑的位置对掘进机姿态进行调整。


技术实现要素：

5.现有技术中对煤矿掘进机姿态测量方法均存在不同程度的短板和缺陷，高能激光指向仪在施工场景中又是必不可少的设备，如果能够通过一道指示激光顺利完成对煤矿掘进机机身姿态的测量任务是本领域技术人员长期以来不断进行研究。本发明提出了一种基于双矢量的煤矿掘进机姿态测量方法，该姿态测量算法无数据漂移和误差累积，不需初始对准，具有测量精度高、成本低、受现场干扰小的优点，可在巷道掘进中替代惯导系统，为未来巷道掘进机实现自动导航提供了理论依据和技术支撑。本发明借鉴卫星姿态测量中的核心传感装置-星敏感器的测量原理，通过融合工业相机获取的图像信息和倾斜仪获取的双轴姿态信息，利用双矢量定姿算法，实现掘进机坐标系相对于现场施工坐标系空间姿态的高精度测量。
6.为了解决现有技术问题本发明采用如下技术方案：
7.一种基于双矢量的煤矿掘进机姿态测量方法，所述姿态测量方法基于倾斜仪、双目相机和激光器，所述倾斜仪、双目相机设置在掘进机上；所述激光沿隧道设计轴线，所述姿态测量方法包括如下步骤：
8.步骤1、倾斜仪坐标系s下，按照所述倾斜仪的x、y轴输出建立重力矢量模型gs：
[0009][0010]
其中：重力矢量的俯仰角β”和滚转角γ”；
[0011]
步骤2、在双目相机中的左相机坐标系c下计算光矢量，并通过坐标系变换，计算建立倾斜仪坐标系s下光矢量模型：
[0012][0013]
其中：为旋转矩阵，lc为左相机坐标系下的光平面交线；
[0014]
步骤3、根据所述重力矢量模型和所述光矢量模型建立参考坐标系d，通过双矢量定姿算法解算倾斜仪坐标系与施工坐标系之间的姿态变换矩阵，从而得到掘进机在施工坐标系n下的姿态角，包括方位角α、俯仰角β和滚转角γ：
[0015][0016]
其中：为施工坐标系n与倾斜仪坐标系s之间的姿态变换矩阵。
[0017]
进一步，步骤1中通过所述倾斜仪的x、y轴输出建立重力矢量模型gs过程：
[0018]
201、设倾斜仪x、y轴的输出分别为γ'、β'。根据坐标系的定义，重力矢量的俯仰角β”和滚转角γ”分别为：
[0019][0020]
202、根据重力矢量的俯仰角β”和滚转角γ”建立重力矢量模型。
[0021]
进一步，步骤2中通过所述双目相机在左相机坐标系c下之间变换计算建立光矢量模型过程：
[0022]
301、根据所述双目相机的光心分别与拍摄到的所述激光器投射的激光线段按照霍夫变换算法获得指示激光参数；所述指示激光参数为离散化的(ρ,θ)曲线，其中：ρ为坐标系原点到直线的最短距离，θ为x轴与连接原点和最近点直线之间的夹角；
[0023]
302、将所述指示激光参数利用相机投影模型转换到左、右相机坐标系下，结合左、右相机光心，分别建立左相机坐标系下左光平面和右相机坐标系下右光平面的平面方程，得到左相机坐标系下的左光平面的法向量n
lc
和右相机坐标系下的右光平面的法向量n
lr
；
[0024]
303、按照如下公式将右光平面的法向量统一左相机坐标系c下：
[0025][0026]
其中，n
rc
为左相机坐标系下右光平面的法向量，为左相机坐标系和右相机坐标系之间的姿态变换矩阵，(n
lr
)
t
为n
lr
的转置；
[0027]
304、根据左相机坐标系c下左、右光平面的法向量按如下公式求解交线：
[0028]
lc＝n
lc
×nrc
[0029]
其中，lc为左相机坐标系下的光矢量；
[0030]
305、结合倾斜仪坐标系s与左相机坐标系c之间的旋转矩阵建立光矢量模型。
[0031]
进一步，所述步骤3中所述得到掘进机在施工坐标系n下的姿态角过程，包括如下步骤：
[0032]
401、根据所述重力矢量模型和所述光矢量模型建立参考坐标系d，其在倾斜仪坐标系s下的正交坐标基为：
[0033][0034]
402、倾斜仪坐标系s到参考坐标系d的姿态变换矩阵为：
[0035][0036]
其中，a
t
为a的转置，b
t
为b的转置，c
t
为c的转置，(a
t
,b
t
,c
t
)
t
为(a
t
,b
t
,c
t
)的转置；
[0037]
403、根据所述重力矢量模型和所述光矢量模型建立参考坐标系d，其在施工坐标系n下的正交坐标基为：
[0038][0039]
404、施工坐标系n到参考坐标系d的姿态变换矩阵为：
[0040][0041]
其中，a
t
为a的转置，b
t
为b的转置，c
t
为c的转置，(a
t
,b
t
,c
t
)
t
为(a
t
,b
t
,c
t
)的转置；
[0042]
405、根据倾斜仪坐标系s到参考坐标系d的姿态变换矩阵和施工坐标系n到参考坐标系d的姿态变换矩阵获得倾斜仪坐标系s相对于施工坐标n系的姿态变换矩阵
[0043][0044]
其中，为矩阵的逆；
[0045]
406、通过倾斜仪坐标系s相对于施工坐标系n的姿态变换矩阵得到掘进机在施工坐标系n下的姿态角，包括方位角α、俯仰角β和滚转角γ。
[0046]
有益效果：
[0047]
1.本发明提出一种利用惯性倾角测量和双目视觉测量技术实现掘进机机身姿态解算的方法，相比于惯导系统，该方法不会产生数据漂移和误差累积，且不需初始校准，测量精度高；
[0048]
2.本发明通过双目相机和倾斜仪组成，体积小、成本低，具有长航程及高精度的特
点和较强的现场应用推广价值，可应用于巷道掘进现场；
[0049]
3.本发明利用双目相机识别激光线段，在掘进现场烟尘较大的情况下，由于丁达尔效应，光条成像效果更好，更易于识别，所以该测量方案在高粉尘、低照度的施工环境下鲁棒性更强。
附图说明：
[0050]
图1为本发明一种基于双矢量的煤矿掘进机姿态测量方法的原理图；
[0051]
图2为本发明一种基于双矢量的煤矿掘进机姿态测量方法中倾斜仪坐标系下重力矢量模型；
[0052]
图3为本发明涉及倾斜仪与水平面的位置关系示意图；
[0053]
图4为本发明一种基于双矢量的煤矿掘进机姿态测量方法中光矢量模型；
[0054]
图5为本发明一种基于双矢量的煤矿掘进机姿态测量方法应用的坐标系定义图。
具体实施方式
[0055]
本发明中提出了一种基于双矢量的煤矿掘进机姿态测量方法，以下结合图1对本发明专利的实施过程做进一步详细说明。
[0056]
步骤1、利用倾斜仪计算重力矢量模型：
[0057]
重力矢量在倾斜仪坐标系s下的数学表达是根据倾斜仪双轴输出计算得到的。以双轴倾斜仪的x、y轴建立坐标系，如图2所示，其中倾斜仪与水平面的位置关系如图3所示。设倾斜仪x、y轴的输出分别为γ'、β'。根据坐标系的定义，重力矢量的俯仰角β”和滚转角γ”分别为
[0058][0059]
重力矢量模型可表示为
[0060][0061]
步骤二、利用双目相机计算光矢量模型：
[0062]
光矢量的计算原理如图4所示，左、右相机的光心分别为oc、or。双目相机拍摄指示激光，设左相机拍摄得到的激光线段为ab，右相机拍摄得到的激光线段为cd。利用左相机光心oc和激光线段ab建立左光平面，c右相机光心or和激光线段cd建立右光平面，则指示激光可由左、右光平面的交线表示。
[0063]
本发明只需保证双目相机拍摄到指示激光即可，不必拍摄相同的激光线段。因此该方法简单易行，且在捕捉目标的过程中无理论误差产生。
[0064]
左、右相机拍摄得到激光线段后，结合霍夫变换算法，即将图像上的每一个像素点(x,y)转换为一个离散化的(ρ,θ)曲线，其中ρ为坐标系原点到直线的最短距离，θ为x轴与连接原点和最近点直线之间的夹角。对具有相同(ρ,θ)曲线的像素点的个数进行累加，通过设
置阈值，查找累加值大于阈值的像素点，检测出激光线段，并得到指示激光在各相机图像坐标系的数学表达。根据相机内参将其转换到相机坐标系下从而得到左、右光平面的平面方程。由此可求得左相机坐标系c下左光平面的法向量n
lc
和右相机坐标系r下右光平面的法向量n
lr
。
[0065]
为求交线，必须将两个法向量统一在同一个坐标系下，现将其统一在左相机坐标系c下。根据标定的右相机坐标系r到左相机坐标系c的姿态变换矩阵左相机坐标系c下右光平面的法向量为
[0066][0067]
左相机坐标系c下两个光平面的交线为
[0068]
lc＝n
lc
×nrc
。
[0069]
结合标定的倾斜仪坐标系s与左相机坐标系c之间的旋转矩阵倾斜仪坐标系s下交线为
[0070][0071]
步骤3、双矢量定姿：
[0072]
将对巷道掘进施工工艺的理解和现有导向技术相结合，目前现场施工人员是通过一道带有已知方位信息的高能激光对掘进机实施导向，即这道激光的光矢量l在施工坐标系n中是已知的。而通常n系的onx
nyn
面与大地水准面平行，因此重力矢量g在n系中也为已知量。
[0073]
如图5所示，在煤矿采掘过程中施工坐标系定义为(n系)，掘进机载体坐标系定义为o
s-x
syszs
(s系)，需要做的就是计算倾斜仪坐标系s相对施工坐标系n的姿态变换矩阵。也就是施工人员对掘进机姿态控制的数据表示。
[0074]
根据空间中存在两个矢量模型即为重力矢量模型和光矢量模型，表示为g和l，其在倾斜仪坐标系s和施工坐标系n下分别表示为gs、gn、ls、ln(下角标表示坐标系名称)。以这两个光矢量模型建立参考坐标系d，在倾斜仪坐标系s下构建正交坐标基为：
[0075][0076]
倾斜仪坐标系s到参考坐标系d的姿态变换矩阵为：
[0077][0078]
同理，参考坐标系d在施工坐标系n下的坐标正交基为：
[0079][0080]
施工坐标系n到参考坐标系d的姿态变换矩阵为：
[0081][0082]
根据倾斜仪坐标系s到参考坐标系d的姿态变换矩阵和施工坐标系n到参考坐标系d的姿态变换矩阵获得倾斜仪坐标系s相对于施工坐标n系的姿态变换矩阵：
[0083][0084]
通过倾斜仪坐标系s相对于施工坐标系n的姿态变换矩阵得到掘进机在施工坐标系n下的姿态角，包括方位角α、俯仰角β和滚转角γ。
[0085][0086]
本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内。