捷联惯导和激光感知复合的掘进机位姿检测系统与方法

1.本发明涉及一种捷联惯导和激光感知复合的掘进机位姿检测系统与方法，属煤矿井下掘进机自主导航领域。


背景技术：

2.作为我国目前极为重要的主体能源，煤炭一直以来都与我国的经济发展和人民生活息息相关，而煤炭行业以及煤矿井下的相关问题也备受关注。从我国开始开采煤炭以来，煤矿因其环境恶劣、地层结构等问题时常发生井下安全事故，其中采掘区事故发生最为频繁和严重，对传统在煤矿井下工作的工人来说是巨大的生命威胁。故而致力于煤矿井下采掘工作的少人化或无人化的研究对于保障工人的健康及生命安全是非常重要的，同时也是近年来煤炭行业研究领域的热点。
3.在传统的煤矿井下掘进作业中，一直使用的都是人工瞄准，即在煤矿巷道顶板上使用一个激光指向仪来进行掘进方向的标定，在截割面上形成一个光斑，工人则凭借自己的感觉和经验沿着激光指向的路径进行掘进工作。在这样的工作模式下，由于巷道掘进的偏向精度和掘进深度完全由操作工人的感觉和手法决定，故而截割精度难以得到保证，而且人体不易感知俯仰和横滚方向上的偏差，就使得掘出的巷道形状可能有高低不平，截面不符合要求等种种问题。
4.在近些年的煤矿智能化掘进研究中，国内的企业和研究团队已经提出了多种掘进机位姿检测方法。就目前看来，已经提出的方法主要有：基于室内定位系统(igps)的掘进机位姿测量系统；基于机器视觉技术的掘进机位姿检测系统；基于高精度全自动全站仪的掘进机位姿检测方法；基于超宽带测距原理的掘进机定向方法；以及基于捷联式惯导系统的掘进机位姿检测方法。
5.所述的基于室内定位系统(igps)的掘进机位姿测量系统主要是由激光发射站、解算系统和激光接收器三大部分组成。通过固定在巷道空间内不同位置的激光发射站发射具有特定角度和转速特征的扇面激光，再由固定在掘进机上的激光接收器接收激光信号，即可得发射站和掘进机之间的水平和垂直方向角度，在经过进一步解算获得掘进机的空间位姿信息。使用该方法进行掘进机定位虽精度较好，但是易受环境干扰，难以在恶劣煤矿巷道条件下取得应用。
6.所述的基于机器视觉技术的掘进机位姿检测系统主要由激光指向仪、摄像头、若干光靶以及图像处理系统组成。其中激光指向仪与摄像头固定在煤矿巷道顶板上并刚性连接，作为掘进机位姿检测基准，摄像头不断采集固定在机身上的若干光靶图像，借助人工智能在图像处理系统中解算信号，以达到实时更新掘进机在惯性空间下的位置和姿态。此方法安装方便，且无需对现有掘进机进行较大改造，但是由于摄像头分辨率有限，随着摄像头与光靶之间距离的增加，位姿检测精度会有明显下降。
7.所述的基于高精度全自动全站仪的掘进机位姿检测方法主要由固定在煤矿巷道侧壁的检测装置(全站)以及固定在全站后方和掘进机机身上的若干棱镜组成。全站后方的
棱镜按照巷道设计基准安装，主要起定位基准作用，全站通过检测掘进机身上的棱镜的空间位置关系来确定掘进机在惯性空间下的位置和姿态。此法精度较高，但是全站一次仅能检测一个棱镜的位置，其动态检测能力很差。
8.所述的基于超宽带测距原理的掘进机定向方法以无线电波测距技术作为可行性基础，主要由基站信号发射群以及定位接收点组成。基站信号发射群固定在掘进机后方的巷道空间内，发射频率为ghz量级的无线电波，由固定在掘进机身不同位置的若干定位接收点接收无线信号，得到基站到不同接收点的距离信息，在结合定位点的安装位置，得到掘进机在巷道坐标系下的的位姿信息。此法的主要缺点是无线电波易受阻挡，且实际煤矿巷道中障碍物又较多，故而信号传播易受干扰，对检测精度有较大影响。
9.所述的基于捷联式惯导系统的掘进机位姿检测方法以牛顿惯性定律作为主要检测基础。在掘进机身上固定有高精度三轴陀螺仪及加速度计，可以实时测得掘进机在惯性空间下的三轴角速度及加速度信息，然后在捷联解算系统下进行积分解算和坐标变换，能够实时求得掘进机空间位姿信息。此法是目前唯一无需外部设备辅助的掘进机位姿检测方法，其动态性、自主性、环境适应性均较强，且姿态检测精度较高，但是随着时间推移，捷联系统的位置解算误差呈发散状态，定位精度很低，只有在借助其他方式标定的情况下才能正常工作。


技术实现要素：

10.技术问题：本发明的主要目的是针对于煤矿井下恶劣工作环境以及已有的悬臂式掘进机位姿检测方法的缺陷，设计了一种由捷联惯导和激光感知复合的掘进机位姿检测系统与方法，其中主要包括了捷联惯导系统和激光感知系统的硬件、算法设计以及二者实现组合导航的方法和控制策略，解决位姿参数实时、精准感知的难题。
11.技术方案：本发明的实现如下：该系统主要组成部分包括捷联惯导系统、激光感知系统、导航计算机以及人机交互显示系统。其中，捷联惯导系统主要包括三轴光纤陀螺仪、三轴石英加速度计、数据处理模块；激光感知系统主要包括激光偏距感知系统和激光前距感知系统。激光偏距感知系统主要包括扇形激光发射装置、光敏标靶以及数据处理模块；激光前距感知系统主要由两台本安型激光测距仪与数据处理模块组成。
12.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其特征是：所述的捷联惯导系统包括三轴光纤陀螺仪、三轴石英加速度计、数据处理模块。其中三轴光纤陀螺仪可以实时测得掘进机在载体坐标系下相对于惯性空间的三轴角速度信息；三轴石英加速度计可以实时测得掘进机在载体坐标系下相对于惯性空间的三轴加速度信息；数据处理模块中内置了捷联惯导位姿解算算法，将陀螺仪测得的角速度信息和加速度计测得的加速度信息实时地转化为掘进机的空间姿态角(偏向角、俯仰角、横滚角)信息，最后将解算获得的姿态数据发送给导航计算机，由其作进一步处理。捷联惯导系统安装在掘进机防爆电控箱中，通过总线与导航计算机以及人机交互显示系统相联。
13.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其特征是：所述的激光感知系统主要包括激光偏距感知系统、激光前距感知系统。激光偏距感知系统包括扇形激光发射装置、光敏标靶以及数据处理模块。其中，扇形激光发射装置共分为安装架和扇面激光发射器两部分。安装架安装在掘进机后方30-100m处的巷道顶板上，精确位置由测绘部门
人员利用全站仪确定。安装架上设有透光小孔，用以校准激光发射器发射扇面激光的方向。扇面激光发射器固定在安装架上，可以发射出扇形激光面，调整安装角度可以使其发射的激光面刚好垂直于巷道底面并且与巷道中线平行；光敏标靶固定在掘进机机身上部，平行于掘进机横向截面，由一行或多行紧密排列的光敏元件、连接电路、防爆外壳和透光罩组成。光敏元件通过电路相互连接，均设定有序号，可以接受由扇形激光发射装置发出的激光，在理想状态下在光敏标靶上的投影光斑为一条竖直的直线，接受到照射的光敏元件被刺激可以产生电信号并发送给数据处理模块；数据处理模块则可以接收由光敏元件传输过来的电信号，结合对应的元件编号通过内置算法可以知道现有的激光投影与标定状况下的偏差距离，结合捷联惯导系统发送的偏向角可以用来计算掘进机的偏距，再将数据发送给导航计算机处理。
14.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其特征是：所述的激光前距感知系统主要由两台本安型激光测距仪与数据处理模块组成。两台本安型激光测距仪安装在掘进机机身前部两侧，发射端可以发射出高频正弦调制激光信号，激光信号发射出去后经由巷道截割面反射被接收端接收，数据处理模块接收到来自两端的信号，计算发射信号和接收信号之间的相位差，即可求出测距仪到巷道截割面之间的距离，再结合掘进机本身的结构参数，可求得掘进机的车前距信息。然后将信息发送给导航计算机做进一步处理。
15.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其特征是：所述的导航计算机安装在掘进机的电控箱内部，通过总线分别与捷联惯导系统、激光偏距感知系统以及激光前距感知系统相连接并可以互相收发信息。通过内置的导航算法对来自于这三个系统的数据进行解算，可以获得精确的掘进机的导航信息，包括三个姿态角(偏向角、俯仰角、横滚角)和位置坐标(横坐标：偏距、纵坐标：前距)，进而将这些数据发送给人机交互显示系统，供远程操控人员实时监控掘进机行走信息。
16.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其特征是：所述的人机交互显示系统将导航计算机发送来的位姿信息进行数字化显示。通过掘进机的三维模型同步显示真实掘进机在工作过程中的位姿状态，同时生成掘进机的行进的轨迹，显示实际掘进机掘进的巷道与设计巷道之间的偏差，并将偏差信息发送给控制系统。同时还可以设置巷道的设计方向角、掘进机的初始位置等信息。
17.所述的捷联惯导和激光感知复合的掘进机位姿检测系统，其位姿检测方法是：按照掘进机实际的工作流程与工作状态，掘进机一般先由初始位置向前位移到截割面附近，然后其截割臂开始截割作业，在按照截面要求截割完毕后，掘进机向后退出一段距离以供工作人员对刚刚掘进出的巷道进行顶板支护，支护完毕后掘进机再次向前行驶，如此循环。具体的位姿检测流程为：
18.(1)在上一工作循环结束之后(即掘进机完成一次截割并后退之后)，且在下一次掘进机前进之前，此时掘进机相对于惯性空间静止，这时捷联惯导系统进行初始对准，获得掘进机相对于惯性空间的初始航向角、俯仰角以及横滚角。航向角与输入的巷道设计方向角做差，可得掘进机的偏向角。完成对准之后，以上一循环结束时掘进机的位置坐标为起点，作为本次循环的坐标原点，并建立巷道坐标系，完成系统的初始化。
19.(2)初始化完成后，在前方支护完成后掘进机开始向前行进，在行进过程中，捷联
惯导系统通过三轴光纤陀螺仪可以高频率地实时感知掘进机的角速度信息，并且通过数据处理模块实时解算掘进机的姿态角信息；激光偏距感知系统通过感知扇形激光投影照射到光敏标靶的光敏元件编号以及通过捷联惯导系统测得的航向角信息，通过其数据处理模块即可实时计算掘进机与巷道设计中线之间的偏距；激光前距感知系统能够实时测定掘进机与现有巷道截割面之间的距离，即为前距；三个系统将其处理获得的数据发送给导航计算机，由计算机对数据进行坐标变换和数据处理，这样就可实时得到掘进机的速度、位置与姿态信息，再将这些信息整合发送给人机交互显示系统，以完成掘进机在行进过程中的位姿监测。
20.(3)掘进机在行驶到靠近截割面位置，停车准备掏槽之前。这时可以利用掘进机停车时速度为零的特点，对捷联惯导系统的姿态观测以及计算得到的掘进机速度与坐标位置做出修正。因为停车时速度、角速度均视为0值，则此时捷联惯导系统的陀螺仪输出值以及计算出的速度值即可视为观测误差。将所获得的误差值经过处理，与现有的输出参数进行叠加，即可对掘进机的位姿参数进行修正，以保证之后工作循环的输出参数尽可能准确。
21.本发明所述的捷联惯导和激光感知复合的掘进机位姿检测系统与现有的几种掘进机位姿检测系统相比，具有比较突出的优点，由上方的技术方案中的内容可知，本发明提供的捷联惯导系统和激光感知系统复合的掘进机位姿检测系统可以达到相当的技术进步性以及实用性，具有较好的产业实际使用价值，本发明至少有如下几个优点：
22.(1)本发明使用捷联惯导系统作为组成部分之一，其结构简单、安装方便、重量轻、易于维护，环境适应性极强，能够自主工作，在无需借助外部设备的前提下获得较高的掘进机姿态定位精度。
23.(2)本发明使用基于扇形激光发射装置和光敏标靶结合的激光偏距感知系统，其使用方便，定位精度高，后期调试维护方便，并且能很好地适应多种煤矿巷道条件及恶劣工作面，是解决捷联惯导系统定位误差大的良好替代方案。
24.(3)本发明使用基于本安式激光测距仪的激光前距感知系统，能对掘进机的行进距离、掏槽深度等有精度很高的控制水平，且能够在多种巷道截面形状下正常工作，适应性强且精度高。除此之外还能通过与激光偏距感知系统的结合给出捷联惯导系统初始对准信息。
25.(4)本发明对捷联惯导技术和激光感知技术进行了良好的结合，在利用了捷联惯导系统姿态解算精度高，适应性强，自主性好等的基础上，对于其在位置精度误差大的情况选择了良好的替代方案，构建了一个完整的位姿检测精度高、调试维护方便、能够适应多种煤矿巷道条件及恶劣工作面的掘进机全参数位姿检测系统。为实现煤矿掘进作业无人化、智能化提供了基础。
26.综上所述，本发明捷联惯导和激光感知复合的掘进机位姿检测系统与方法，能够很好地实现在掘进机行进及截割过程中进行智能自主的位姿检测。此系统能够实时提供掘进机精确的导航参数，不仅能够将传统的掘进机操控人员从危险的巷道工作面中解放出来，此系统相比于人工还能够实现更加精准的掘进机定位操作，能够比人工完成更加精准、复杂的巷道掘进工作。此发明构思具有良好的创新性，对于相应行业技术人员确有许多启发作用，为实现掘进机自动截割打下基础，向煤矿掘进作业无人化、远程化、智能化迈进了一步。
27.如上所述是本发明的基本构思。但是，在本发明的技术领域内，只要具备最基本的知识，可以对本发明的其他可操作的实施例进行改进。在本发明中对实质性技术方案提出了专利保护请求，其保护范围应包括具有上述技术特点的一切变化方式。
28.以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。
附图说明：
29.图中：
30.图1是本发明系统图
31.图2是本发明系统组成俯视图
32.图3是本发明系统组成左视图
33.图4是光敏标靶机构示意图
34.图5是激光感知系统原理示意图
35.1：掘进机
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
2：捷联惯导系统
36.3：激光感知系统
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
4：导航计算机
37.5：人机交互显示系统
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
6：三轴光纤陀螺仪
38.7：三轴石英加速度计
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
8：数据处理模块1
39.9：激光感知系统
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
10：激光偏距感知系统
40.11：激光前距感知系统
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
12：光敏标靶
41.13：扇形激光发射装置
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
14：激光测距仪1
42.15：激光测距仪2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
16：数据处理模块2
43.17：扇形激光面
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
18：测距线激光
44.19：光敏元件
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
20：激光光斑
45.21：标靶外壳
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
22：测量轮
46.23：巷道边界
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
24：电控箱
具体实施方式：
47.为更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对依据本发明提出的一种捷联惯导和激光感知复合的掘进机位姿检测系统与方法的具体实施方式、结构、特征及其功效，详细说明如后。
48.有关本发明的前述及其他技术内容、特点及功效，在以下配合参考图式的较佳实施例的详细说明中将可清楚呈现。通过具体实施方式的说明，当可对本发明为达成预定目的所采取的技术手段及功效得以更加深入且具体的了解，所附图式仅提供参考与说明之用，并非用来对本发明加以限制。
49.本发明较佳实施例的一种捷联惯导和激光感知复合的掘进机位姿检测系统与方法，如图1所示，由捷联惯导系统2、激光感知系统3、导航计算机4以及人机交互显示系统5组
成。所述捷联惯导系统2安装在掘进机电控箱24内部，包括三轴光纤陀螺仪6、三轴石英加速度计7以及数据处理模块8；所述的激光感知系统3，包括激光偏距感知系统10、激光前距感知系统11；所述的激光偏距感知系统10由安装在掘进机1后方巷道顶板上的扇形激光发射装置13、安装在掘进机机身上部中线的光敏标靶12以及信号处理单元【图里面没找到】组成；所述的激光前距感知系统11由安装在掘进机机身前部两侧的本安型激光测距仪14、15和数据处理模块16组成；所述的导航计算机4安装在掘进机机身电控箱24内部；所述人机交互显示系统5，可以实时数字化同步掘进机的位姿状态，显示掘进机的运动轨迹。其中：
50.捷联惯导系统2包含三轴光纤陀螺仪6、三轴石英加速度计7与数据处理模块8，安装在掘进机防爆电控箱24内部。应保证陀螺仪、加速度计与掘进机身的空间三维坐标轴(意即各自的前后、左右、上下面)保持平行。三轴光纤陀螺仪6可以实时测得掘进机1在载体坐标系下相对于惯性空间的三轴角速度信息；三轴石英加速度计7可以实时测得掘进机1在载体坐标系下相对于惯性空间的三轴加速度信息；数据处理模块8中内置了捷联惯导位姿解算算法，将陀螺仪测得的角速度信息和加速度计测得的加速度信息实时地转化为掘进机的空间姿态角(偏向角、俯仰角、横滚角)信息，最后将解算获得的姿态数据发送给导航计算机4，由其作进一步处理，通过串口与导航计算机4以及人机交互显示系统5相联。
51.激光感知系统3主要包括激光偏距感知系统10、激光前距感知系统11以及数据处理模块16。其布置方式如图5所示。
52.激光偏距感知系统10中扇形激光发射装置13安装在掘进机后方巷道30-130m处，精确位置由测绘部门人员利用全站仪确定。光敏标靶10中心与掘进机质心重合，平行于掘进机机身横轴。如图所示，光敏元件19横向紧密排列，且均设定有序号，通过连接电路相互紧密连接，并连同数据处理模块16一同安装到防爆外壳21内部。在光敏元件19感光的一侧裁去外壳并覆盖透光罩。在激光偏距感知系统10的工作过程中，由扇形激光面17垂直投射在光敏标靶12的感光面上，投影形状为竖直线型激光光斑20。被照射到的光敏元件19受到刺激，产生电信号并通过连接电路传送给数据处理模块16。数据处理模块16依照对应的元件编号来计算光斑位置与基准点之间的距离，同时结合掘进机偏向角信息可以计算得出掘进机1的横向偏距。
53.激光前距感知系统11中两个本安型激光测距仪14、15分别安装在掘进机1机身前部的左右两侧。两个测距仪对称安装,能同时检测自身与巷道截割面的距离，再将数据发送至数据处理模块16，由其通过内置算法来进行掘进机1车前距和行走距离的计算。
54.激光感知系统的原理如图5所示，将掘进机简化为矩形，其中
55.o点——光敏标靶的中点，一般视作偏距为0的基准点；
56.o’点——两激光测距仪安装位置连线的中点；
57.a点——从竖直方向看，扇形激光面在光敏标靶上的垂直投影位置点；
58.b点——过o点做巷道中线的垂线，交于巷道中线的点；
59.c点——激光测距仪1的安装位置；
60.d点——激光测距仪1发射的测距激光在巷道截面的测量点；
61.e点——激光测距仪2的安装位置；
62.f点——激光测距仪1发射的测距激光在巷道截面的测量点；
63.g点——f点和d点连线的中点
64.h点——是过o点作垂直于ob的直线与df的交点
65.p点——从竖直方向看，扇形激光发射装置安装位置点
66.由图5可得如下关系：
67.|ob|＝|oa|
·
cosε-l4ꢀꢀꢀ
(1)
[0068][0069]
设lh为掘进机的横向偏移距离，即偏距；lq为掘进机的前移距离，即前距；巷道设计方向角为α(已知)；捷联惯导所测得的绝对航向角为β；l1为激光测距仪1所测得的距离；l2为激光测距仪2所测得的距离；l3为两激光测距仪安装位置的中点与掘进机质心的距离(固定已知)。可得：lh＝|ob|，lq＝|oh|,l3＝|oo'|，oa|＝nd。∠opb为掘进机的偏向角且∠opb＝∠aob＝∠goh＝ε,ε＝β-α，则式(1)和式(2)分别写为：
[0070]
lh＝ndcos(β-α)-l4ꢀꢀꢀ
(3)
[0071][0072]
式中，n为感知到光斑的光敏元件到零基准点之间光敏元件的个数，d为单个光敏元件的直径，l4为扇形激光发射装置安装位置与巷道设计中线之间的水平距离(如果重合，l4＝0)。
[0073]
所述的捷联惯导和激光感知复合的掘进机位姿检测系统主要包括捷联惯导系统、激光感知系统、导航计算机和人机交互显示系统四个部分。在掘进机完成一次工作循环静止时，捷联惯导系统对自身进行零速初始对准，测得掘进机相对于惯性空间的初始姿态角(包括航向角、俯仰角、横滚角)，再根据巷道设计方向角求得掘进机偏向角，然后以现掘进机位置为原点建立巷道坐标系，激光感知系统可以测得掘进机在水平面上的初始位置信息(包括偏距、车前距)。导航计算机获得数据后使用内置算法进行数据计算与坐标变换获得掘进机位姿参数。在掘进机向前行进过程中由捷联惯导系统实时高频率地给出掘进机姿态角信息，由激光感知系统给出掘进机实时平面位置信息。这些信息在经由导航计算机整合变换后，发送给远端人机交互显示系统，以便于工作人员在远端对掘进机工作的位置和姿态进行实时监控和追踪。
[0074]
捷联惯导和激光感知复合的掘进机位姿检测系统及方法的实施步骤为：
[0075]
按照掘进机实际的工作流程与工作状态，掘进机一般先由初始位置向前位移到截割面附近，然后其截割臂开始截割作业，在按照截面要求截割完毕后，掘进机向后退出一段距离以供工作人员对刚刚掘进出的巷道进行顶板支护，支护完毕后掘进机再次向前行驶，如此循环。具体的位姿检测流程为：
[0076]
(1)在上一工作循环结束之后(即掘进机完成一次截割并后退之后)，且在下一次掘进机前进之前，此时掘进机相对于惯性空间静止，这时捷联惯导系统进行初始对准，获得掘进机相对于惯性空间的初始航向角、俯仰角以及横滚角。航向角与输入的巷道设计方向角做差，可得掘进机的偏向角。完成对准之后，以上一循环结束时掘进机的位置坐标为起
点，作为本次循环的坐标原点，并建立巷道坐标系，完成系统的初始化。
[0077]
(2)初始化完成后，在前方支护完成后掘进机开始向前行进，在行进过程中，捷联惯导系统通过三轴光纤陀螺仪可以高频率地实时感知掘进机的角速度信息，并且通过数据处理模块实时解算掘进机的姿态角信息；激光偏距感知系统通过感知扇形激光投影照射到光敏标靶的光敏元件编号以及通过捷联惯导系统测得的航向角信息，通过其数据处理模块即可实时计算掘进机与巷道设计中线之间的偏距；激光前距感知系统能够实时测定掘进机与现有巷道截割面之间的距离，即为前距；三个系统将其处理获得的数据发送给导航计算机，由计算机对数据进行坐标变换和数据处理，这样就可实时得到掘进机的速度、位置与姿态信息，再将这些信息整合发送给人机交互显示系统，以完成掘进机在行进过程中的位姿监测。
[0078]
(3)掘进机在行驶到靠近截割面位置，停车准备掏槽之前。这时可以利用掘进机停车时速度为零的特点，对捷联惯导系统的姿态观测以及计算得到的掘进机速度与坐标位置作出修正。因为停车时速度、角速度均视为0值，则此时捷联惯导系统的陀螺仪输出值以及计算出的速度值即可视为观测误差。将所获得的误差值经过处理，与现有的输出参数进行叠加，即可对掘进机的位姿参数进行修正，以保证之后工作循环的输出参数尽可能准确。