[发明专利]一种掘进机位姿数据测量系统及其测量方法

权利要求书

1.一种基于旋转标靶的掘进机位姿数据测量方法，其特征在于：包括如下步骤：

步骤一：将掘进机设置在待掘进巷道中，在掘进机后侧的巷道顶板上安装激光指向仪（1），所述激光指向仪（1）具体设置在巷道顶板的中心线位置，所述激光指向仪（1）的激光发射角度与巷道的掘进方向相一致；

步骤二：在掘进机机身上安装一对旋转标靶，所述旋转标靶垂直安装在掘进机机身上侧的旋转电机（4）上，所述两个旋转标靶上的靶框（3）相互之间平行设置；

步骤三：控制掘进机开始掘进作业，通过人工调整激光指向仪（1）的安装位置和激光发射角度，使激光指向仪（1）发射的激光束可以打在两个旋转标靶的靶框（3）上，所述旋转标靶可以通过激光捕获电路板获取到当前激光束打在相应标靶单元（7）上的坐标位置，坐标位置数据包括左右位置（Ax，Bx）和上下位置（Ay，By），标靶单元（7）将所述的坐标位置数据通过导线发送至微控制器（6）做进一步处理；

步骤四：倾角传感器（5）同时采集掘进机当前的掘进俯仰角数据Yaw，倾角传感器（5）将所述的俯仰角数据通过导线发送至微控制器（6）做进一步处理；

步骤五：所述微控制器（6）的计算参数中包括预设的距离参数，其中定义激光指向仪（1）发射激光束到达第一个旋转标靶的距离为L1，两个旋转标靶之间的距离为L0，结合接收到的激光坐标数据可以计算并得出当前掘进机的位姿测量数据：

其中俯仰角Yaw的计算公式为：

；

航向角Pitch的计算公式为：

；

翻滚角Roll数据即为倾角传感器（5）的测量数据；

水平位移dx的计算公式为：

；

高度位移dy的计算公式为：

；

深度位移dz的计算公式为：

；

在上式的坐标系中，定义X轴为巷道掘进面的宽度，Y轴为巷道掘进面的高度，Z轴为巷道掘进方向的深度；

上述Ax和Bx分别是激光束打在第一个和第二个旋转标靶上的点所在位置的x坐标；

上述Ay和By分别是激光束打在第一个和第二个旋转标靶上的点所在位置的y坐标；

所述步骤三中激光束打在旋转标靶上的坐标数据计算方法如下：

要求计算得出的坐标数据包括激光打在第一个旋转标靶上的A点坐标（Ax，Ay）和激光打在第二个旋转标靶上的B点坐标（Bx，By）；

进行数据处理前，微控制器（6）首先对旋转电机（4）反馈的旋转角度数据θ进行接收处理，即已知激光捕获电路板的旋转角度θ和激光束打在旋转标靶上的光点半径r，可以将极坐标转换为相应的直角坐标：

所述旋转标靶内部由多个标靶单元（7）并列设置在靶框（3）中，掘进机位姿变化时，不同位置的标靶单元（7）都有可能捕获到激光点，由此将单个标靶单元（7）上的A点激光坐标转换到整个旋转标靶的坐标为：

根据同样算法和数学模型，对于另一个平行放置的激光标靶，可以获取相应的B点激光坐标为：

所述微控制器（6）将计算得出的上述坐标数据发送至数据存储模块（8）进行存储；

为实现基于旋转标靶的掘进机位姿数据测量方法提供的测量系统，包括掘进机和活动设置在掘进巷道顶板上的激光指向仪（1），所述激光指向仪（1）设置在掘进机的后侧，所述掘进机的机身上侧平行设置有一对旋转标靶，所述旋转标靶包括连杆（2）和靶框（3），所述连杆（2）的一端与靶框（3）焊接固定，所述连杆（2）的另一端与掘进机机身上设置的旋转电机（4）的动作端相连，所述旋转电机（4）嵌入掘进机机身内部；

所述掘进机的内部还设置有控制电路板和倾角传感器（5），所述控制电路板上集成有微控制器（6）；

所述旋转标靶用于接收激光指向仪（1）发出的激光束，所述靶框（3）的框内并列设置有多个标靶单元（7），所述标靶单元（7）具体为由光敏二极管组成的激光捕获电路板；

所述微控制器（6）通过导线与标靶单元（7）的信号输出端相连；

所述微控制器（6）通过导线与旋转电机（4）、倾角传感器（5）相连；

所述微控制器（6）还通过导线连接有数据存储模块（8）；

所述微控制器（6）的电源输入端与电源模块（9）相连；

所述微控制器（6）使用的芯片为控制芯片U1，所述微控制器（6）的电路结构为：

所述控制芯片U1的1脚、9脚、24脚、36脚、48脚接3.3V输入电源；

所述控制芯片U1的2脚、3脚、38脚、39脚、45脚、46脚与旋转电机（4）的控制端相连；

所述控制芯片U1的4脚串接电阻R61和二极管LED1后接地；

所述控制芯片U1的5脚并接晶振XT1的一端后与电容C16的一端相连；

所述控制芯片U1的6脚并接晶振XT1的另一端后与电容C17的另一端相连，所述电容C17的另一端并接电容C16的另一端、电容C14的一端后接地；

所述电容C14的另一端并接电阻R59的一端、控制开关SW1的一端后与控制芯片U1的7脚相连；

所述电阻R59的另一端接3.3V输入电源，所述控制开关SW1的另一端接地；

所述控制芯片U1的10脚至22脚通过多路复用器与标靶单元（7）的信号输出端相连；

所述控制芯片U1的25脚至28脚与数据存储模块（8）相连；

所述控制芯片U1的29脚、30脚、31脚通过485收发器与旋转电机（4）的信号输出端相连；

所述控制芯片U1的34脚、37脚与倾角传感器（5）的信号输出端相连；

所述控制芯片U1的型号为STM32F103CBU6。

2.根据权利要求1所述的一种基于旋转标靶的掘进机位姿数据测量方法，其特征在于：所述多路复用器内部使用的芯片型号为74HC4051；

所述485收发器的型号为MAX3485；

所述数据存储模块（8）内部使用的芯片型号为74HC595；

所述电源模块（9）内部使用的芯片型号为配电开关TPS2552和稳压器RT9013。