CN102207382A - 悬臂式掘进机的位姿测量系统 - Google Patents

Abstract

本发明涉及一种悬臂式掘进机的位姿测量系统，其包括：激光指向测距仪和光测角仪。激光指向测距仪分别发出可见激光与红外测距激光，可见激光经光测角仪凸透镜在其焦平面成像，由像点坐标得到光测角仪的光轴相对于激光光束的方向角α′和俯仰角β′，由于预设的光测角仪的光轴与掘进机机身轴线平行，则α′、β′为机身轴线相对于激光光束的方向角和俯仰角，通过平行于掘进机基准平面安装的倾角传感器可得到机身相对于水平面的横滚角γ，进而修正由γ对方向角α′、俯仰角β′测量的影响，完成掘进机相对激光光束的方向角α、俯仰角β及相对水平面横滚角γ姿态参数的测量。由测距激光得到距掘进机机身距离结合激光指向测距仪的位置坐标、光束发射角度参数可得到掘进机位置坐标。由系统输出数据还可得到掘进工作量等信息。

Description

悬臂式掘进机的位姿测量系统

技术领域

本发明涉及一种煤矿采掘设备的位置和姿态(以下简称：位姿)检测的技术领域，特别是涉及一种悬臂式掘进机的位姿测量系统。

背景技术

悬臂式掘进机是一种部分断面掘进机，其结构特点是在回转座上装有截割用可伸缩悬臂，在悬臂前端装有截割头，通过截割头的旋转和悬臂的上、下、左、右摆动，依次破落煤(岩)，根据对截割头的运动轨迹控制，可截割出梯形、拱形、矩形等断面的巷道，掘进机同时具有装载及行走功能。悬臂式掘进机是我国煤巷综合机械化生产中应用最广泛的巷道掘进机械装备。

采用悬臂式掘进机在巷道掘进作业中的一般工艺流程是：准备(掘进机行走到位)→割煤(截割)、进料、出煤(渣)→临时支护→永久支护→下一循环。人工操作悬臂式掘进机在巷道掘进作业时，主要依靠激光指向器在巷道断面上形成的光斑作为参考，通过手动操作控制掘进机截割头的运动轨迹，从而形成截割断面。由于工作环境极其恶劣，影响作业人员的健康，安全隐患较大，且截割过程中产生的粉尘严重影响操作人员的视线，极易造成超挖、欠挖现象，虽然在实际施工时还配备了辅助人员反馈前方截割情况，但效果不理想，截割断面不规整，且人员的增加导致安全系数进一步降低，所以在截割作业时采用可使操作者远离掘进机的自动控制截割头运动的模式是发展方向。

若要实现截割头运动的自动控制，必须确定其运动轨迹，使其所形成的断面符合设计的要求，首先发布确定在截割前掘进机姿态与该点(位置)设计轴线(方向)间的关系(差值)。

现有的测量掘进机位置、姿态的方法均为：通过精密的“全站仪类”设备和空间位置参考点测量出截割作业前掘进机所处的精确空间位置，比较其与设计巷道轴线所经过点的关系(差值)，测得掘进机相对水平面的姿态参数，与设计中经过该点的轴线方向比较，求得两者间的关系(差值)。

中国专利文献公告号为CN201013380Y公开的全自动掘进机，其通过航天导航定位仪，实时监测掘进机在惯性空间的绝对位置坐标，对掘进机和截割头进行控制，实现全自动掘进。该方案的关键是要测量出掘进机的精确位置，而煤矿井下恶劣的环境以及掘进机自身的震动已远超出了精密的航天仪器的正常工作条件，故无法得到所需的精确结果，造成其可靠性较低，另外，精密航天仪器的造价较高，导致其实施成本较高。

中国专利文献公告号为CN101629807A，公开了一种掘进机机身位姿参数测量系统及方法，该方案采用线状激光器发射扇形激光光束，通过两路光敏元件感应掘进机机身的线性光斑，通过检测光斑位置，结合已知的光敏元件与掘进机机身位置关系计算得到掘进机的位姿参数。该方法的问题在于并没有所需的掘进机姿态与该点设计轴线方向间的关系，而若获得所需的关系参数必先获得掘进机的精确空间位置，而该方案中却没有进行相应的测量。另由于该方案采样基准为扇形激光光束，扇形激光光束限制了其工作距离非常有限，需要频繁调整基准光束位置，施工工作量增加，施工难度较大，影响生产效率，通过两路光敏元件感应线状激光的方式也不能限定掘进机截割动作开始前在巷道横向位置区域，且该方案不能测量掘进机在地球绝对坐标系中的位置坐标，不能测量基准参考点距掘进机的距离值，因而也无法得到掘进机的掘进工作量信息。

因此，如何彻底解决悬臂式掘进机位姿测量问题并提供一种经济实用的测量系统，是本领域要解决的技术问题。

发明内容

本发明要解决的技术问题在于提供一种可靠性较好的、经济实用的悬臂式掘进机位的姿测量系统，以完成悬臂式掘进机机身位姿参数的绝对测量，解决井下掘进机位姿测量无核心设备可用的关键问题。

为解决上述技术问题，本发明的悬臂式掘进机的位姿测量系统的包括：设于巷道内的用于给出巷道掘进方向的激光指向测距仪1、设于掘进机上且与掘进机机身轴线平行的光测角仪2；光测角仪2包括平行于掘进机基准平面设置的倾角传感器10。

工作时，激光指向测距仪1分别发出可见激光与红外测距激光，所述可见激光经光测角仪2中的凸透镜11在其焦平面成像，由所述可见激光在该焦平面上的像点坐标得到光测角仪2的光轴相对于所述可见激光的方向角α′和俯仰角β′，也即掘进机机身轴线相对于所述可见激光的方向角和俯仰角，通过所述倾角传感器可得到掘进机机身相对于水平面的横滚角γ，进而通过该横滚角γ对所述方向角α′和俯仰角β′进行修正，从而得出掘进机相对所述可见激光的方向角α、俯仰角β及相对水平面横滚角γ，即得到掘进机的姿态信息；然后，通过所述测距激光得到激光指向测距仪1与掘进机机身的间距，结合激光指向测距仪1所在点的位置坐标、所述可见激光的光束发射角度参数可得到掘进机的位置坐标，从而完成了掘进机的位置、姿态信息的测量。

激光指向测距仪1同时提供其作为巷道设计基准的可见激光束指向、测量掘进机4与空间位置参考点间距离以及向光测角仪输出空间位置参考点坐标、基准指向激光光束方向参数的三大功能。

进一步，所述激光指向测距仪1设于用于控制激光指向测距仪1的朝向的调整机构3上，激光指向测距仪1包括：激光指向器5、激光测距传感器6、以及与激光指向器5和激光测距传感器6相连的控制电路7；所述光测角仪2还包括：设于所述凸透镜11的焦平面上的面阵光敏元件9、与该面阵光敏元件9相连的用于检测所述可见激光在该焦平面上的像点坐标的A/D转换及数据处理模块8；所述倾角传感器10将测得的数据传传输至A/D转换及数据处理模块8。

作为最优的方案，所述激光指向测距仪1与光测角仪2之间通过所述可见激光进行光数字通信，以使激光指向测距仪1将测得的激光指向测距仪1与掘进机机身的距离、激光指向测距仪1所在点的位置坐标、所述可见激光的光束发射角度参数发送至光测角仪2，光测角仪2结合所述掘进机相对所述可见激光的方向角α、俯仰角β及相对水平面横滚角γ，得出掘进机的位姿信息。

进一步，所述光测角仪2在进行角度测量和光数字通信时，共用所述面阵光敏元件9。

进一步，所述光测角仪还包括：设于所述凸透镜11外围的光电引导靶标13，当所述可见激光照射到光电引导靶标13上，A/D转换及数据处理模块8可通过扫描光电引导靶标13上的光敏元件得到所述可见激光照射的部位并输出所述可见激光所在位置的数据信息，以供掘进机驾驶员参考并调整掘进机4机身位姿，以使所述凸透镜11能接收所述可见激光。

本发明利用已有的激光指向光束，该光束提供了巷道中各点(位置)的设计方向，通过光学测量方式直接获得掘进机机身轴线方向与设计基准轴线方向之间的关系即方向角、俯仰角、掘进机机身与地平面间横滚角；通过测量掘进机与空间参考点间的距离，及将空间参考点坐标、设计基准轴线方向的数字化后，求得掘进机的位置坐标参数。用此方法在每次截割动作开始前对掘进机位姿信息的测量，即可求得符合要求的截割轨迹，实现对截割头运动的自动控制。

本发明的特点是充分利用已有的激光指向光束，避开了需精确测量掘进机位置，而目前无在掘进工作面条件下可用的精密测量仪器的难题；其次，所有测量设备的误差与距离无关；再之，该方法对其测量仪器的精度要求远远小于位置测量对仪器的精度要求，便于在其恶劣工作环境条件下的实现。

本发明进一步要解决的技术问题在于提供一种悬臂式掘进机的位置、姿态参数的在线测量的系统，得到实时的掘进机位置、姿态参数，为悬臂式掘进机的全自动掘进提供数据支持。

激光指向测距仪由煤矿地测部门安装于巷道顶或巷道壁等固定位置，包括：激光指向器、激光测距传感器、控制电路及调整机构。激光指向器发出的可见激光光束是巷道掘进的方向基准，确定了掘进机截割时应处的位置区域，激光指向器激光光束内还含有调制信号，包括：激光指向测距仪端的数据信息，光测角仪通过解调光信号实现两者之间的数字传输。激光测距传感器与激光指向器平行安装，保证出射激光光束平行，激光测距传感器可测量其距掘进机机身的距离值，距离值参与计算掘进机的位置参数，通过距离值还可计算得到掘进机的工作量信息，如班进尺、日进尺、月进尺等。控制电路含输入、输出、数据处理及通讯模块等，系统使用之前，应写实激光指向测距仪所在点的位置坐标、激光指向器激光光束的发射角度并输入至控制电路。控制电路可以控制激光测距传感器工作状态、读取激光测距传感器测量数据并处理、显示数据，输入的数据信息也可通过显示模块显示，将数据信息汇总后通过调制激光信号传输。调整机构可四自由度调节，实现上下、左右方向的移动与转动，通过调整机构调整激光光束的出射角度，以符合规划设计的巷道掘进方向，调整到位后通过锁紧装置锁紧即可。

光测角仪包括：凸透镜、面阵光敏元件、A/D转换及数据处理模块、倾角传感器、光电引导靶标及钢化玻璃。光测角仪安装在掘进机机身合适位置，光轴与掘进机机身轴线平行，凸透镜所在面朝向激光指向测距仪所在方向，用于接收来自激光指向测距仪的激光光束，激光光束通过凸透镜成像在焦平面处面阵光敏元件上，光敏元件将其像点的光信号转换成电信号，经A/D转换及数据处理模块采集处理后计算得到像点在焦平面上的位置坐标，而凸透镜焦距已知，则由凸透镜成像规律可计算得到光测角仪光轴与激光光束之间的方向角α、俯仰角β。由于光测角仪光轴与掘进机机身轴线之间的为已知的平行关系，则α、β即为掘进机机身轴线与激光光束之间的方向角、俯仰角。A/D转换及数据处理模块还可通过面阵光敏元件解调激光光束信号，得到激光指向测距仪端的数据信息，结合光测角仪内倾角传感器测量得到掘进机相对于地球平面的横滚角γ，即可得到掘进机的绝对位置、姿态参数。光电引导靶标安装在凸透镜外围，若可见激光光束照射到靶标上，A/D转换及数据处理模块可采集并上传光束照射形成光斑的位置信息，以便调整掘进机位姿至合适范围。

所述悬臂式掘进机位姿测量系统中，所述的激光指向测距仪可替代传统的激光指向器，也可以与激光指向器同时使用，但应保证有且仅有激光指向测距仪的可见激光光束通过光测角仪凸透镜成像，且两者之间的激光指示方向应一致，是巷道掘进的方向基准。

激光指向测距仪中的激光指向器工作距离区间为15-600米，且在此工作区间范围内激光光束直径不大于40mm。激光指向器还带调制功能，调制频率≥100KHZ。

激光指向测距仪中激光测距传感器采用红外激光光源测量，量程为15-600米，测量精度±1米。安装完成后即可自动测量其距掘进机机身的距离值，测量得到的数据传输至激光指向测距仪内控制电路，由控制电路识别并处理数据，得到距掘进机机身的距离值，控制电路将得到的距离值、角度值、位置信息等组合成一帧完整的数据由可见激光光束发送至光测角仪端。

所述悬臂式掘进机位姿测量系统中，所述的光测角仪安装在掘进机机身，安装时保证光测角仪光轴与掘进机机身轴线平行，对安装造成的平行度误差，可通过更高精度的量具对其标校。具体做法为：在掘进机机身安装固定光测角仪，在掘进机后方安装激光指向测距仪，调整激光指向测距仪位置及激光光束出射角度，并用更高级别仪器标定，使激光指向测距仪可见激光光束与掘进机机身轴线平行且激光光束经过光测角仪凸透镜成像，光测角仪记录此时像点位置，即为系统测量的相对零点。

光测角仪内凸透镜与面阵光敏元件组成一组光学成像系统，凸透镜直径为200mm，焦距f已知，通过检测焦平面处可见激光光束像点坐标即可计算出激光光束与凸透镜成像系统光轴之间的方向角、俯仰角，由于光测角仪光轴与掘进机机身轴线之间为已知的平行关系，则可得到掘进机机身轴线与激光光束之间的方向角、俯仰角。

光测角仪内面阵光敏元件及光电引导靶标内的光敏元件具有较强的抗干扰能力，只对某一波段范围的可见光敏感，当且仅当照射在光敏元件上的光波长在光敏元件敏感区间内且光强超过一定值时，光电转换元件才产生感应电流，产生的感应电流大小与照射光强的能量在一定范围内成正比，直至饱和。针对光敏元件的这一特性，研究实施了带自识别功能的加权平均算法，算法保证系统能滤除可能存在的系统外光源干扰，准确识别像斑位置，计算像斑“重心”坐标，得到测量结果，这一算法的实施使得系统对激光光源的识别能力大为增强，得到可信度、精确度高的数据信息。系统测量方向角α、俯仰角β的量程为±5°，测量误差≤0.5°。悬臂式掘进机掘进巷道宽度为5米左右，相对于掘进机2.2-3米的机身宽度，超过5°的方向角、俯仰角偏差已经非常明显，故对方向角、俯仰角的测量量程在±5°即可满足实际使用需要；又因掘进机悬臂的长度一般不超过10m，0.5°的测量误差在断面上产生的与基准的偏差Δx为：10×sin 0.5°＝0.08726(m)，因截割过程中引起煤自行塌落产生的偏差远大于0.08726米，故0.08726米的偏差对施工结果的影响较小，且这一偏差不会在掘进机的多次作业循环中产生累积而影响巷道的走向，其结果是影响巷道壁的平整度。

光测角仪内倾角传感器水平安装在光测角仪底部，可测量光测角仪相对于大地水平面的横滚角。由于光测角仪水平安装在掘进机机身，因而倾角传感器也即测量掘进机相对于大地水平面的横滚角，其为掘进机机身与大地水平面之间在机身左右方向的绝对夹角，横滚角测量量程为±45°，测量精度为±0.05°。

光测角仪内A/D转换及数据处理模块采用基于Cortex-M3内核的LPC1765芯片，辅以相应的外围电路组成，芯片主频100MHZ，满足本系统数据采集需求。

光测角仪内钢化玻璃安装在光测角仪的最前端，用于保护光测角仪内设备，且能使激光光束能透过玻璃照射在凸透镜或者光电引导靶标上。

本发明悬臂式掘进机位姿测量系统与现有技术相比，具有特征鲜明的独创性、明显的优点及出色的效果。由以上所述技术方案，本发明提供的悬臂式掘进机位姿测量系统，可提高悬臂式掘进机的技术进步性、实用性，并具有产业化的推广使用价值，其至少具有如下优点：

(1)本发明充分利用已有的激光指向光束，避开了需精确测量掘进机位置，而目前无在掘进工作面条件下可用的精密测量仪器的难题，克服了掘进机位姿测量无核心设备可用的关键问题。

(2)本发明所有测量设备的误差与距离无关，且测量误差不会在掘进机的多次作业循环中产生累积而影响巷道的走向，其结果仅影响巷道壁的平整度。

(3)本发明通过间接测量的方式测量掘进机的位姿参数，其对测量仪器的精度要求远远小于直接位姿测量对仪器的精度要求，便于在其恶劣工作环境条件下的实现。

(4)本发明创造性的利用凸透镜成像原理检测像点位置的方法来测量掘进机机身轴线与巷道基准轴之间的夹角，测量精度≤0.5°，满足实际使用需求。

(5)本发明系统参考点(激光指向测距仪)与被测点(掘进机端光测角仪)之间采用调制激光通讯，光测角仪端面阵光敏元件采用分时复用技术采集像斑坐标、解调光信号。光通讯方式的使用，避免了有线通讯方式通讯电缆的铺设，射频通讯方式基站、天线的架设，在采掘工作面，工矿环境非常复杂，两端之间铺设电缆、安置天线等会给施工带来诸多麻烦与不便；面阵光敏元件分时复用技术的实施，减少系统外围电路，降低系统成本，提高系统可靠性。

(6)本发明内激光指向测距仪同时提供其作为巷道设计基准的可见激光束指向、测量掘进机与空间位置参考点间距离以及向光测角仪输出空间位置参考点坐标、基准指向激光光束方向参数的三大功能。多功能的实现使得系统使用方便，测量可靠，具有传统的激光指向器无法比拟的优势。

(7)与传统方式相比，本发明自动化水平明显提高，可减少采掘工作面人数，降低施工人员的劳动强度，提高煤矿生产人员的安全保障且能实时、在线测量，从而为自动化掘进提供数据支持。同时，系统还可辅助测量出巷道掘进工作量信息，为煤矿日常管理提供方便。

综上所述，本发明采用独创性的技术方案测量悬臂式掘进机位置、姿态参数，彻底解决了井下掘进机位姿测量无核心设备可用的关键问题，满足煤矿井下巷道掘进时对掘进机位置、姿态参数实时测量的需要，并为自动化掘进奠定了良好的基础。本系统适用于悬臂式掘进机井下煤巷掘进，具有产业化的推广使用价值。本系统的测量原理、实施方案未见公开发表或使用而确属创新，与已公开的相近专利在测量原理和实施方案上有很大不同，且增进了多项功能，实用性更强，在煤矿采掘设备应用、检测测量领域有较大进步，推动了行业的技术创新，具有广泛利用价值，为一独创、新颖、实用的新发明。

上述说明仅是本发明技术方案的概述，为使能更进一步了解本发明的技术方法，并可依其内容实施，以下配合附图说明本发明的较佳实施案例。

附图说明

图1为本发明系统框图；

图2为本发明结构组成示意图；

图3为本发明结构组成俯视示意图；

图4为本发明激光指向测距仪组成示意图；

图5为本发明光测角仪组成示意图；

图6为本发明姿态测量原理图；

图7为本发明位置计算原理图。

图中的附图标记：1：激光指向测距仪，2：光测角仪，3：调整机构，4：悬臂式掘进机，5：激光指向器，6：激光测距传感器，7：控制电路，8：A/D转换及数据处理模块，9：面阵光敏元件，10：倾角传感器，11：凸透镜，12钢化玻璃，13：光电引导靶标。

具体实施方式

以下结合附图，通过具体实施方式，对测量原理进一步说明，更深入介绍本发明的技术手段及功能，但所附图及具体实施方式仅是提供参考与说明，并非用于对本发明加以限制。

图1为本发明系统框图，描述了系统涉及的各部件之间的组成关系与内在联系。

图2为实施本发明的悬臂式掘进机位姿测量方法的示意图，由图可知，本实施例的悬臂式掘进机的位姿测量系统，包括：设于巷道内的用于给出巷道掘进方向的激光指向测距仪1、设于掘进机上且与掘进机机身轴线平行的光测角仪2；光测角仪2包括平行于掘进机基准平面设置的倾角传感器10；工作时，激光指向测距仪1分别发出可见激光与红外测距激光，所述可见激光经光测角仪2中的凸透镜11在其焦平面成像，由所述可见激光在该焦平面上的像点坐标得到光测角仪2的光轴相对于激光光束的方向角α′和俯仰角β′，也即掘进机机身轴线相对于激光光束的方向角和俯仰角，通过所述倾角传感器可得到掘进机机身相对于水平面的横滚角γ，进而通过该横滚角γ对所述方向角α′和俯仰角β′进行修正，从而得出掘进机相对激光光束的方向角α、俯仰角β及相对水平面横滚角γ，即达到掘进机的姿态信息；然后，通过所述测距激光得到激光指向测距仪1与掘进机机身的距离，然后结合激光指向测距仪1的位置坐标、光束发射角度参数可得到掘进机的位置坐标。进而得出掘进机的位姿信息。

所述激光指向测距仪1设于用于控制激光指向测距仪1的朝向的调整机构3上，激光指向测距仪1包括：激光指向器5、激光测距传感器6、以及与激光指向器5和激光测距传感器6相连的控制电路7；所述光测角仪2还包括：设于所述焦平面上的面阵光敏元件9、与该面阵光敏元件9相连的用于检测所述可见激光在该焦平面上的像点坐标的A/D转换及数据处理模块8；所述倾角传感器10将测得的数据传传输至A/D转换及数据处理模块8。

所述光测角仪还包括：设于所述凸透镜11外围的光电引导靶标13，当所述可见激光照射到光电引导靶标13上，A/D转换及数据处理模块8可通过扫描光电引导靶标13上的光敏元件得到受所述可见激光照射的部位并输出所述可见激光所在位置的数据信息，以供掘进机驾驶员参考并调整掘进机4机身位姿，以使所述凸透镜11能接收所述可见激光。

所述激光指向测距仪1由煤矿地测部门固定在巷道壁或者巷道顶部，固定完成后通过四自由度调整机构3调整激光指向测距仪1的指向，使其与预设的掘进路径一致，调整完成后锁紧，以固定出射方向角，并显示激光指向测距仪1所在点的经纬度坐标(LON，LAT)、激光光束在空间坐标系内的方向角α_光、俯仰角β_光并将数据输入至激光指向测距仪1的主控电路7。由于设计时已保证激光测距传感器6的测距激光与指向激光光束平行，因而此时激光测距传感器6已对准掘进机4机身，即可测量得到掘进机4机身至激光指向测距仪1之间的距离值。

光测角仪2安装在掘进机4机身合适位置处，安装时应使其光轴与掘进机4机身轴线平行且应保证测量时激光指向测距仪1与光测角仪2在巷道横向位置大致一致。安装完成后可通过更高精度的量具标定光测角仪2光轴与掘进机4机身轴线之间的平行度误差，并通过光测角仪2内A/D转换及数据处理模块8设置相对零点。

图3为图2的俯视图。由图可知，激光指向测距仪1发出两束激光，其中可见激光光束照射在光测角仪2上，不可见激光光束(测距激光)照射在掘进机4机身经反射后返回至激光测距传感器6。

图4为本发明激光指向测距仪1组成示意图。由图可知，激光指向器5调焦部分凸出在壳体之外，使用方便，激光测距传感器6与激光指向器5之间有一定的间距，保证测距激光光束不会照射到光测角仪2钢化玻璃12上产生镜面反射，从而保证距离测量功能可靠实现。

图5为本发明光测角仪2组成示意图。光测角仪2内倾角传感器10水平安装在光测角仪2底部，面阵光敏元件9安装在凸透镜11焦平面处，光电引导靶标13安装在凸透镜11外围。倾角传感器10、面阵光敏元件9、光电引导靶标13等数据都归口到A/D转换及数据处理模块8，由其统一处理并输出。

图6为本发明的姿态计算原理图。如图6a所示，以凸透镜11焦点为原点，在其焦平面内建立直角坐标系，设激光光束经凸透镜11所成像点坐标为P(m，n)，凸透镜焦距为f，将激光光束看作平行光线并分别投影到水平面及竖直平面内，两平面相交于光轴所在直线，则凸透镜成像系统光轴相对于激光光束的方向角α′及俯仰角β′为：

$\left\{\begin{array}{c}{\mathrm{\α}}^{\′}=\mathrm{arctg}\frac{m}{f}\\ {\mathrm{\β}}^{\′}=\mathrm{arctg}\frac{n}{f}\end{array}\right.---\left(1\right)$

本系统中凸透镜11焦距f已知，则由式(1)即可求出凸透镜成像系统光轴与激光光束之间的方向角、俯仰角。而凸透镜成像系统的光轴与掘进机4机身轴线平行，则可得到激光光束与掘进机4机身轴线之间的方向角、俯仰角。掘进机4机身相对于地面的横滚角由倾角传感器10给出。由于掘进机4存在横滚角γ，导致焦平面XOY坐标轴与实际有效坐标轴X′OY′之间夹角为γ，如图6b所示，设此夹角导致实际有效像点坐标为(m′，n′)，由相似三角形及正余弦三角函数的相关定义、公式可得：

$\left\{\begin{array}{c}{m}^{\′}=m\cos \mathrm{\γ}+n\sin \mathrm{\γ}\\ n^{\′}=n\cos \mathrm{\γ}-m\sin \mathrm{\γ}\end{array}\right.---\left(2\right)$

则掘进机机身轴线相对于激光光束的方向角α、俯仰角β有：

$\left\{\begin{array}{c}\mathrm{\α}=\mathrm{arctg}\frac{m\cos \mathrm{\γ}+n\sin \mathrm{\γ}}{f}\\ \mathrm{\β}=\mathrm{arctg}\frac{n\cos \mathrm{\γ}-m\sin \mathrm{\γ}}{f}\end{array}\right.---\left(3\right)$

由式(3)即可求出掘进机4机身相对于激光光束的方向角、俯仰角。

上述参数的正负号定义如下：

方向角：掘进机4机身轴线与激光光束在水平平面内形成的夹角，顺时针旋转为正，逆时针旋转为负；

俯仰角：掘进机4机身轴线相对于激光光束在竖直方向的夹角，向上旋转为正，反之为负；

又由倾角传感器10可得到掘进机机身相对于水平面的横滚角，至此，可得到掘进机全部的姿态参数。

图7为本发明位置计算原理图。以激光指向测距仪1所在点A为固定参考点，设其经纬度坐标为A(LON，LAT)，激光指向测距仪1所在点在空间坐标系内坐标为A(x，y，z)，该固定参考点射出的激光光束在空间坐标系内的方向角为α_光，俯仰角为β_光。设光测角仪2所在点经纬度坐标为B(LON′，LAT′)，其在空间坐标系内坐标为B(x′，y′，z′)，激光测距传感器读数为s，并设固定参考点和光测角仪距空间坐标系原点距离都为R，如图7所示，根据大地直角坐标系与大地坐标系之间的关系式有：

$\left\{\begin{array}{c}x=R\cos \mathrm{LAT}\cos \mathrm{LON}\\ y=R\cos \mathrm{LAT}\sin \mathrm{LON}\\ z=R\sin \mathrm{LAT}\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{x}^{\′}=R\cos \mathrm{LA}{T}^{\′}\cos \mathrm{LO}{N}^{\′}\\ y^{\′}=R\cos \mathrm{LA}{T}^{\′}\sin \mathrm{LO}{N}^{\′}\\ z^{\′}=R\sin \mathrm{LA}{T}^{\′}\end{array}\right.---\left(5\right)$

根据Bursa-Wolf公式有：

$\left\{\begin{array}{c}{x}^{\′}=x+\mathrm{\Δx}\\ y^{\′}=y+\mathrm{\Δy}\\ z^{\′}=z+\mathrm{\Δz}\end{array}\right.---\left(6\right)$

其中

激光指向测距仪1所在点的经纬度坐标、其射出的激光光束在空间坐标系内的方向角α_光，俯仰角β_光由为已知量，又间距在15-600米范围变化矿井巷道内两点相对与空间坐标系原点距离可视作相等，为地球半径R，故由式(8)即可求出光测角仪2所在点也即掘进机4的位置坐标信息。

至此，就完成了掘进机4机身位置、姿态参数的测量。

上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而这些属于本发明的精神所引伸出的显而易见的变化或变动仍处于本发明的保护范围之中。

Claims (5)

1.一种悬臂式掘进机的位姿测量系统，其特征在于：设于巷道内的用于给出巷道掘进方向的激光指向测距仪(1)、设于掘进机上且与掘进机机身轴线平行的光测角仪(2)；光测角仪(2)包括平行于掘进机基准平面设置的倾角传感器(10)；

工作时，激光指向测距仪(1)分别发出可见激光与红外测距激光，所述可见激光经光测角仪(2)中的凸透镜(11)在其焦平面成像，由所述可见激光在该焦平面上的像点坐标得到光测角仪(2)的光轴相对于所述可见激光的方向角α′和俯仰角β′，由于光测角仪(2)的光轴与掘进机机身轴线平行，α′、β′即为掘进机机身轴线相对于所述可见激光的方向角和俯仰角，通过所述倾角传感器可得到掘进机机身相对于水平面的横滚角γ，进而通过该横滚角γ对所述方向角α′和俯仰角β′进行修正，从而得出掘进机相对所述可见激光的方向角α、俯仰角β及相对水平面横滚角γ，即得到掘进机的姿态信息；

然后，通过所述测距激光得到激光指向测距仪(1)与掘进机机身的间距，结合激光指向测距仪(1)所在点的位置坐标、所述可见激光的光束发射角度参数可得到掘进机的位置坐标，从而完成了掘进机的位置、姿态信息的测量。

2.根据权利要求1所述悬臂式掘进机的位姿测量系统，其特征在于：所述激光指向测距仪(1)设于用于控制激光指向测距仪(1)的朝向的调整机构(3)上，激光指向测距仪(1)包括：激光指向器(5)、激光测距传感器(6)、以及与激光指向器(5)和激光测距传感器(6)相连的控制电路(7)；

所述光测角仪(2)还包括：设于所述凸透镜(11)的焦平面上的面阵光敏元件(9)、与该面阵光敏元件(9)相连的用于检测所述可见激光在该焦平面上的像点坐标的A/D转换及数据处理模块(8)；所述倾角传感器(10)将测得的数据传传输至A/D转换及数据处理模块(8)。

3.根据权利要求1所述悬臂式掘进机的位姿测量系统，其特征在于：激光指向测距仪(1)与光测角仪(2)之间通过所述可见激光进行光数字通信，以使激光指向测距仪(1)将激光指向测距仪(1)所在点的位置坐标、所述可见激光的光束发射角度参数以及激光指向测距仪(1)测得的其与掘进机机身的距离发送至光测角仪(2)，光测角仪(2)结合所述掘进机相对所述可见激光的方向角α、俯仰角β及相对水平面横滚角γ，得出掘进机的位姿信息。

4.根据权利要求2所述悬臂式掘进机的位姿测量系统，其特征在于：光测角仪(2)在进行角度测量和光数字通信时，共用所述面阵光敏元件(9)。

5.根据权利要求2所述悬臂式掘进机的位姿测量系统，其特征在于：所述光测角仪还包括：设于所述凸透镜(11)外围的光电引导靶标(13)，当所述可见激光照射到光电引导靶标(13)上，A/D转换及数据处理模块(8)可通过扫描光电引导靶标(13)上的光敏元件得到所述可见激光照射的部位并输出所述可见激光所在位置的数据信息，以供掘进机驾驶员参考并调整掘进机(4)机身位姿，以使所述凸透镜(11)能接收所述可见激光。

Images