技术领域本发明涉及一种掘进机截割头位置测量系统，可用于煤矿掘进机的定位定姿领域。
背景技术：
目前煤矿的掘进机主要是以激光束为指导方向，操作手依据此作为前进的方向，井下的环境恶劣，粉尘浓度大，光线黑暗，严重影响了挖掘的视线，因此容易造成挖掘不准确，欠挖和过挖现象。随着科学技术的智能化和数字化发展，实现煤矿掘进机的数字化和智能化挖掘将成为一个大的趋势。采用设备实时监测掘进机截割头在巷道断面的位置信息，使用数字化通信进行数据传输，采用可视化界面进行显示，便于操作手实时掌握准确的截割头位置信息，严格安装巷道设计者的要求进行挖掘。因此需要设计出一种可靠的截割头位置测量系统。
技术实现要素：
本发明的技术解决问题：克服现有技术的不足，提供了一种掘进机截割头位置测量系统，该系统可用于煤矿挖掘机运动状态的测量，环境适应性强，将获得的截割头位置测量信息传递给操作手，提高了挖掘的准确度。本发明的技术解决方案：一种掘进机截割头位置测量系统，包括：惯组模块、激光测距仪模块、掘进机参数获取模块、CPU计算模块、输入CAN通信模块、输出CAN通信模块、上位机模块、回转角度传感器、俯仰角度传感器和位移传感器；激光测距仪模块用于瞄准惯组的安装方向，保证惯组安装的前向与掘进机的截割臂的轴线方向一致；回转角度传感器和俯仰角度传感器分别测量截割头的回转角度和俯仰角度，并通过输入CAN通信模块发送给CPU计算模块，位移传感器测量掘进机的前向距离和垂直方向距离，并通过输入CAN通信模块发送给CPU计算模块；惯组模块通过测量掘进机的角速度和加速度信息进行导航解算，获得掘进机的导航信息并发送给CPU计算模块；掘进机参数获取模块获得掘进机的各项参数信息，形成掘进机参数表并传输给CPU计算模块；CPU计算模块根据所有的输入信息进行处理，计算得到掘进机截割头的位置信息，最后CPU计算模块计算得到的掘进机截割头的位置信息通过输出CAN通信模块发送给上位机模块进行截割头位置显示。所述回转角度传感器、俯仰角度传感器和位移传感器安装在截割头与掘进机机身之间的连接臂上。所述惯组模块包括三轴陀螺和三轴加表，所述导航信息包括位置信息、速度信息、姿态信息、数据标志以及时间，位置信息包括经度、纬度和高度，速度信息包括东向速度、北向速度和天向速度，姿态信息包括俯仰角、横滚角和航向角。所述掘进机参数表具体为：序号参数项目单位1惯导中心至回转轴水平距离mm2惯导中心至回转轴垂直距离mm3惯导中心至回转平面的高度mm4回转轴和俯仰轴公垂线距离mm5安装平台至履带平面垂向高度mm6截割大臂长度mm7截割头最大外包络径向长度mm8掘进机回转轴的零位°9掘进机俯仰轴的零位°。所述CPU计算模块根据所有的输入信息进行处理，计算得到掘进机截割头的位置信息，具体为：(1)计算掘进机的截割头在机身坐标系下的位置信息：z_head_imu=sin(θ_head)*(L_head+L_arm)+H_r2rplane+H_imu2r-H_imu2cp]]>其中，x_head_imu、y_head_imu和z_head_imu分别表示计算的机身坐标系下的截割头的位置坐标值，L_head表示截割头的径向长度，L_arm表示截割大臂的长度，L_r2s表示回转轴和俯仰轴公垂线距离，L_imu2r表示惯导中心至回转轴水平距离，H_r2rplane表示回转轴心至回转平面的高度，H_imu2r表示惯导中心至回转轴垂直距离，H_imu2cp表示惯组安装位置至掘进机履带平面的垂向高度，为回转传感器获得截割头相对惯组的回转角，为俯仰角度传感器获得截割头相对惯组的俯仰角，x_body_g和z_body_g分别为位移传感器获得掘进机侧向移动距离和垂直方向的移动距离；(2)计算掘进机的截割头在巷道坐标系下的位置信息，具体转换方式如下所示：x_head_gy_head_gz_head_g=T*x_head_imuy_head_imuz_head_imu]]>其中，x_head_g、y_head_g和z_head_g分别表示计算的巷道坐标系下的截割头X、Y、Z的位置值，T表示转换矩阵；(3)根据惯组上电工作的时间长度来判断是否需要校标补偿，如果惯组上电工作的时间大于等于10h，则需要校标补偿，进入步骤(4)，否则直接进入步骤(5)；(4)选取截割大臂上的任一点作为标志点，对截割头在巷道坐标系下的位置信息进行校正，将所述标志点旋转至巷道的激光束上任意点，获得此时截割头的位置误差Δx和Δz，具体校正公式如下所示；x′_head_g＝x_head_g-Δxz′_head_g＝z_head_g-Δz其中，x′_head_g和z′_head_g分别表示巷道坐标系下的截割头校正后的位置值，Δx和Δz分别表示巷道坐标系下的截割头修正量；(5)对动态机身位置进行校正，通过位移传感器获得前向的位置信息x_body_g和z_body_g，通过以下的公式分解为X向和Z向的位置值；若从步骤(3)进入步骤(5)则使用公式x″_head_g＝x_head_g+x_body_g；z″_head_g＝z_head_g+z_body_g；进行校正；若从步骤(4)进入步骤(5)则使用公式x″_head_g＝x′_head_g+x_body_g；z″_head_g＝z′_head_g+z_body_g；进行校正，其中x″_head_g和z″_head_g分别表示巷道坐标系下的截割头动态机身校正后的位置值。所述机身坐标系定义为：原点O为机身上安装惯组的位置；X轴指向机身的右，Y轴指向机身的前，Z轴由右手定则确定，且垂直向上；所述巷道坐标系定义为：原点O为巷道入口中心，X轴指向巷道的右，Y轴指向巷道的前，Z轴由右手定则确定，且垂直向上。所述转换矩阵T为3×3的矩阵，具体为：T[0][1]＝sin(γ)*cos(θ)；T[2][0]＝sin(γ)*cos(θ)；T[2][1]＝-sin(θ)；T[2][2]＝cos(γ)*cos(θ)；其中γ、θ为机身的横滚角、偏航角和俯仰角。本发明与现有技术相比带来的有益效果为：(1)本发明设计了七个模块，从外界传感数据的接收、截割头位置的解算至数据的发送和显示，形成了一个完整的数字化测量和显示系统；采用CAN通信模块进行数据传输，提高了系统的传输速率和可靠性；本发明可通过设计掘进机参数获取模块修改不同机身的参数，提高了系统的实用性和灵活性；采用激光测距仪模块保证了系统安装的准确性，为截割头计算提供了有力保障；数据通过CAN通信模块发送给上位机，形成可视化，让操作工人能直观的了解到当前时刻的截割头位置信息，为进一步的挖掘提供了数据基础，避免了肉眼观察带来的判断失误。(2)本发明充分利用了惯性导航的原理，建立了机身坐标与巷道坐标之间的转换关系，仔细分析了掘进机的机身结构以及各关节之间的连接关系，通过测量截割大臂的俯仰关节和旋转关节的角度计算出截割头的位置坐标，计算思路清晰，易解；同时本发明还提出了标志点校正方法，消除了惯导系统因长时间工作存在的累积误差，提高了测量的准确度；考虑到掘进机截割过程中存在侧滑现象，设计了采用位移传感器获得机身的实时位置，从而对截割头的位置信息进行校正，进一步的提高了测量的准确性。附图说明图1为本发明系统框架示意图；图2接收流程设计图3截割头位置解算流程设计图4发送流程设计具体实施方式煤矿行业，挖掘技术相对传统，目测激光束来保证前进的路线，与设计的巷道会存在较大的挖掘误差，而煤矿的作业环境恶劣，传统的作业方式工作量大、效率低。针对这些问题，本发明提出了将惯导技术与掘进机挖掘技术相结合，采用工业常用的CAN总线传输数据，计算掘进机截割头的位置，通过实际搭载的平台进行了试验，获得误差小于10cm的准确定位信息。如图1所示，本发明提供的一种掘进机截割头位置测量系统，包括：惯组模块、激光测距仪模块、掘进机参数获取模块、CPU计算模块、输入CAN通信模块、输出CAN通信模块、上位机模块、回转角度传感器、俯仰角度传感器和位移传感器；激光测距仪模块用于瞄准惯组的安装方向，保证惯组安装的前向与掘进机的截割臂的轴线方向一致；所述回转角度传感器、俯仰角度传感器和位移传感器安装在截割头与掘进机机身之间的连接臂上。回转角度传感器和俯仰角度传感器分别测量截割头的回转角度和俯仰角度，并通过输入CAN通信模块发送给CPU计算模块，位移传感器测量掘进机的前向距离和垂直方向距离，并通过输入CAN通信模块发送给CPU计算模块。CAN通信模块设计的波特率为250kbps，传输频率100HZ，用于实现传输，验收滤波，中断响应等相关功能。设计CAN接收信息如图2所示，CAN控制器采用查询的接收方式。CPU读取CAN控制器的状态寄存器，首先需要判断控制器是否处于数据溢出状态，若是，则读空报文，释放缓存，清楚数据溢出位；其次查询接收缓存器的状态标志是否为满，满表示有报文可以接收，然后读取报文的识别码，判断是否是满足要求的报文，读取数据长度，根据读取的数据长度使CPU从接收缓存区读取报文；最后置位命令寄存器，释放接收缓存器，等待下次报文的接收。俯仰角、横滚角和位移值是通过不同的传感器感知获得，因此属于三种不同的信息，分三次读取，每次最多可读取10个字节，前2个字节用于为信息报文的ID号和字节长度，后8个字节用于表示具体信息。对获取的数字量进行数据解析，转换成截割头相对机身的俯仰角度、回转角度和机身的位移量。惯组模块通过测量掘进机的角速度和加速度信息进行导航解算，获得掘进机的导航信息并发送给CPU计算模块；所述惯组模块包括三轴陀螺和三轴加表，所述导航信息包括位置信息、速度信息、姿态信息、数据标志以及时间，位置信息包括经度、纬度和高度，速度信息包括东向速度、北向速度和天向速度，姿态信息包括俯仰角、横滚角和航向角。掘进机参数获取模块获得掘进机的各项参数信息，形成掘进机参数表并传输给CPU计算模块；所述掘进机参数表具体为：序号参数项目单位1惯导中心至回转轴水平距离mm2惯导中心至回转轴垂直距离mm3惯导中心至回转平面的高度mm4回转轴和俯仰轴公垂线距离mm5安装平台至履带平面垂向高度mm6截割大臂长度mm7截割头最大外包络径向长度mm8掘进机回转轴的零位°9掘进机俯仰轴的零位°。此参数表依据不同的掘进机可进行修改，提高了系统的灵活性和通用性。所述机身坐标系定义为：原点O为机身上安装惯组的位置；X轴指向机身的右，Y轴指向机身的前，Z轴由右手定则确定，且垂直向上；所述巷道坐标系定义为：原点O为巷道入口中心，X轴指向巷道的右，Y轴指向巷道的前，Z轴由右手定则确定，且垂直向上。将惯导系统安装至掘进机的中心线位置，以惯导系统作为基准系统，首先结合掘进机机身的结构特征和尺寸，截割大臂和截割头的长度、宽度和弧度，截割大臂可转动的角度，计算出截割头在机身坐标系下的三维坐标；其次将截割头的位置坐标转换至巷道坐标系下；对系统进行校标补偿判定，若需要则进行误差校正，若不需要则直接进入数据发送。CPU计算模块根据所有的输入信息进行处理，计算得到掘进机截割头的位置信息，最后CPU计算模块计算得到的掘进机截割头的位置信息通过输出CAN通信模块发送给上位机模块进行截割头位置显示。如图3所示，CPU计算模块根据所有的输入信息进行处理，计算得到掘进机截割头的位置信息，具体为：(1)计算掘进机的截割头在机身坐标系下的位置信息：z_head_imu=sin(θ_head)*(L_head+L_arm)+H_r2rplane+H_imu2r-H_imu2cp]]>其中，x_head_imu、y_head_imu和z_head_imu分别表示计算的机身坐标系下的截割头的位置坐标值，L_head表示截割头的径向长度，L_arm表示截割大臂的长度，L_r2s表示回转轴和俯仰轴公垂线距离，L_imu2r表示惯导中心至回转轴水平距离，H_r2rplane表示回转轴心至回转平面的高度，H_imu2r表示惯导中心至回转轴垂直距离，H_imu2cp表示惯组安装位置至掘进机履带平面的垂向高度，为回转传感器获得截割头相对惯组的回转角，为俯仰角度传感器获得截割头相对惯组的俯仰角，x_body_g和z_body_g分别为位移传感器获得掘进机侧向移动距离和垂直方向的移动距离；(2)计算掘进机的截割头在巷道坐标系下的位置信息，具体转换方式如下所示：x_head_gy_head_gz_head_g=T*x_head_imuy_head_imuz_head_imu]]>其中，x_head_g、y_head_g和z_head_g分别表示计算的巷道坐标系下的截割头X、Y、Z的位置值，T表示转换矩阵；所述转换矩阵T为3×3的矩阵，具体为：T[0][1]＝sin(γ)*cos(θ)；T[2][0]＝sin(γ)*cos(θ)；T[2][1]＝-sin(θ)；T[2][2]＝cos(γ)*cos(θ)；其中γ、θ为机身的横滚角、偏航角和俯仰角。(3)根据惯组上电工作的时间长度来判断是否需要校标补偿，如果惯组上电工作的时间大于等于10h，则需要校标补偿，进入步骤(4)，否则直接进入步骤(5)；(4)选取截割大臂上的任一点作为标志点，对截割头在巷道坐标系下的位置信息进行校正，将所述标志点旋转至巷道的激光束上任意点，获得此时截割头的位置误差Δx和Δz，具体校正公式如下所示；x′_head_g＝x_head_g-Δxz′_head_g＝z_head_g-Δz其中，x′_head_g和z′_head_g分别表示巷道坐标系下的截割头校正后的位置值，Δx和Δz分别表示巷道坐标系下的截割头修正量；(5)对动态机身位置进行校正，通过位移传感器获得前向的位置信息x_body_g和z_body_g，通过以下的公式分解为X向和Z向的位置值；若从步骤(3)进入步骤(5)则使用公式x″_head_g＝x_head_g+x_body_g；z″_head_g＝z_head_g+z_body_g；进行校正；若从步骤(4)进入步骤(5)则使用公式x″_head_g＝x′_head_g+x_body_g；z″_head_g＝z′_head_g+z_body_g；进行校正，其中x″_head_g和z″_head_g分别表示巷道坐标系下的截割头动态机身校正后的位置值。本发明通过上述两种方式计算得到最终的截割头位置信息，考虑到惯导误差长时间积累效应，采用校正方式，消除此误差，获得准确的测量值，提高系统的精度。通过CAN通信模块将计算出的截割头位置信息发送给用户方。设计接口协议、发送部分以及传输的频率，来完成数据的打包和发送，发送流程设计如图4所示：数据的发送由CAN通信模块独立完成，CPU首先将要发送的报文(报文)传送到发送缓冲器，然后将命令寄存器中的“发送请求位”标志置位。采用查询发送方式。只要控制器正在发送数据，发送缓存器就被写锁定，将新的报文放入发送缓冲器之前CPU必须检查状态寄存器的发送缓存器的状态。发送数据如下顺序：1)查询状态寄存器，判断是否正在接收或者发送，是否缓存区被锁；2)更新发送缓存器的报文数据；3)配置命令寄存器，启动发送。由于CAN通信模块出现故障，过于繁忙等错误状态，需要进行一定的处理，这里有两种情况，一是总线过于繁忙而报文优先级过低，导致报文在正常时间内无法发送成功；二是总线出现故障，或控制器由于强烈干扰等原因发送出错，不能返回发送成功标志。需要对发送进行监控和出错处理。本发明设计从煤矿行业的掘进机掘进的实际工作出发，能改变传统的掘进方式，通过数字化设计测量准确的位置信息，为煤矿行业的自动化掘进、自主截割、远程操控等智能化发展迈出了一大步，减少了人力成本，解放了人力资源，也提高了煤矿行业的安全性。本发明在一台实际的煤矿掘进机上进行了试验，安装了需要的俯仰角、旋转角和位移传感器，将系统安装至掘进机的操控台右侧，近乎机身中心线位置，设计了截割头的运动方式，采用本发明设计的系统对运动过程中截割头的位置信息进行测量，并使用全站仪测量系统对各点的位置信息进行测量，将两者进行比较，得到最终的误差值，误差均在100mm以内，充分证明了本发明的有效性。机身的参数如下所示：表1机身参数序号参数项目数值单位1惯导中心至回转轴水平距离1631mm2惯导中心至回转轴垂直距离10mm3惯导中心至回转平面的高度-125mm4回转轴和俯仰轴公垂线距离600mm5安装平台至履带平面垂向高度1655mm6截割大臂长度3010mm7截割头最大外包络径向长度1130mm8掘进机回转轴的零位0.97°9掘进机俯仰轴的零位3.85°实施例1：将俯仰关节放置5.0°位置，从右到左旋转截割大臂，间隔不同的回转角度测量截割头的位置信息，结果如下所示：表2回转关节转动测量数据1实施例2：将回转关节放置14.0°位置，从下到上抬起截割大臂，间隔不同的俯仰角度测量截割头的位置信息，结果如下所示：表3俯仰关节转动测量数据1实施例3：机身前进一段距离，将回转关节和俯仰关节放置任意角度，选取随机的10个位置测量截割头的位置信息，结果如下所示：表4机身前进一段距离测量数据通过以上的数据可以看出，X向和Z向的误差均在100mm以内，证明了本发明在静态和动态情况下均能获高精度的测量结果，验证了本发明的可行性和优点。本发明未详细描述内容为本领域技术人员公知技术。