悬臂掘进机头位姿的测量系统及其方法

1.悬臂掘进机头位姿的测量系统，包含有悬臂掘进机(8)及其悬臂升降油缸(9)、悬臂回转油缸(10)和掘进机头伸缩油缸(11)，反射棱镜(4)，激光机动全站仪(3)，无线通讯模块(2)，双轴倾角传感器(7)，油缸行程传感器(12)，车体偏摆角传感器(5)，数据采集模块(13)和计算机(1)，其特征在于激光机动全站仪(3)设置在已掘成形的巷道壁上的位置是在悬臂掘进机(8)车体后与掘进机(8)的后机架(6)同水平的、距掘进机车体1m～10m之间的巷道壁上，反射棱镜(4)和双轴倾角传感器(7)安装在悬臂掘进机(8)的后机架(6)上，车体偏摆角传感器(5)安装在反射棱镜(4)后，悬臂升降油缸(9)、悬臂回转油缸(10)和掘进机头伸缩油缸(11)中安装有油缸行程传感器(12)，两个无线通讯模块(2)分别安装在激光机动全站仪(3)的支架上和掘进机(8)的控制箱(14)上，无线通讯模块(2)将激光机动全站仪(3)的测量数据传输给计算机(1)，数据采集模块(13)和计算机(1)安装在掘进机(8)的控制箱(14)里，数据采集模块(13)采集车体偏摆角传感器(5)、双轴倾角传感器(7)和油缸行程传感器(12)的数据，计算机(1)对测量数据进行处理，计算掘进机头的位置和姿态。
2.如权利要求1所述的悬臂掘进机头位姿的测量系统，其特征在于反射棱镜(4)是能够180度反射激光机动全站仪(3)的测量激光的激光测量棱镜。
3.如权利要求1所述的悬臂掘进机头位姿的测量系统，其特征在于悬臂升降油缸(9)、悬臂回转油缸(10)和掘进机头伸缩油缸(11)中安装有油缸行程传感器(12)是沿着轴线，分别对悬臂升降油缸(9)、悬臂回转油缸(10)和掘进机头伸缩油缸(11)的缸体(20)和缸杆(21)钻孔，用于安装油缸行程传感器(12)，油缸行程传感器(12)由传感器引线(15)、感应磁环(17)和测量杆(19)构成，传感器引线(15)从缸体(20)的安装端(16)引出，测量杆(19)从缸体(20)的安装端(16)沿轴线插装在缸体(20)和缸杆(21)中，感应磁环(17)固定在缸杆(21)上，并随油缸活塞(18)和缸杆(21)的运动沿着测量杆(19)滑动，油缸行程传感器(12)的测量杆(19)通过螺纹固定在缸体(20)中，感应磁环(17)由压紧螺母固定在缸杆(21)中。
4.一种用于悬臂掘进机头位姿的测量系统的测量方法，其特征在于激光机动全站仪(3)测量掘进机(8)车体在大地坐标系中的位置坐标，掘进机车体偏摆角传感器(5)测量掘进机(8)车体的偏摆角γ，双轴倾角传感器(7)测量掘进机(8)车体的仰俯角α和横滚角β，油缸行程传感器(12)测量悬臂升降油缸(9)、悬臂回转油缸(10)和掘进机头伸缩油缸(11)的行程，无线通讯模块(2)将激光机动全站仪(3)的测量数据传输给计算机(1)，数据采集模块(13)采集油缸行程传感器(12)、双轴倾角传感器(7)和车体偏摆角传感器(5)的数据，计算机(1)对采集的数据进行处理和计算，实现悬臂掘进机头的位姿的测量。
5.如权利要求4所述的悬臂掘进机头位姿的测量系统的测量方法，其特征在于在掘进机(8)车体后已掘成形的、与掘进机(8)的后机架(6)同水平的、距掘进机车体1米至10米之间的巷道壁上设置有激光机动全站仪(3)的安装平台，激光机动全站仪(3)测量掘进机(8)车体在大地坐标系中的位置坐标是根据激光机动全站仪(3)在巷道壁上设置的安装平台的三维坐标和姿态角，对激光机动全站仪(3)的测量数据经过坐标变换得到。

悬臂掘进机头位姿的测量系统及其方法\n技术领域\n[0001] 本发明涉及一种煤矿井下作业的悬臂掘进机的掘进头，具体而言是一种悬臂掘进机头的位姿(位置和姿态)的测量系统及其测量方法。\n背景技术\n[0002] 现有煤矿井下巷道掘进广泛采用悬臂式掘进机。悬臂式掘进机是集截割、装运、行走、灭尘等功能于一体的巷道联合掘进机组，通过掘进机体的行走、悬臂的回转摆动、垂直摆动、伸缩运动以及掘进头的旋转截割出巷道断面。掘进机的操纵采用手动方式，司机通过人眼观察掘进机头的运动位置、运动速度和运动趋势，手动操纵悬臂升降、回转油缸和掘进机行走马达的液压控制阀，调整掘进机体的位置和掘进机头的空间位置，实现掘进机头的位置和姿态的定位；由于掘进过程中工作面粉尘较大，作业现场光线较差，特别是掘进机头在掘进巷道底板时，司机的视线要受到掘进机体的遮挡，使得掘进机头在巷道掘进过程中，其位置和姿态难于准确判断，给掘进机头的行进控制造成很大困难；另外，在掘进准备和掘进过程中，手动操纵的掘进机经常需要人工测量已掘进巷道的轮廓尺寸，工人劳动强度大，掘进速度缓慢，生产效率低，巷道超挖、和欠挖现象严重，成本较高，施工质量很大程度上取决于掘进司机的经验和熟练程度。虽然有些掘进机上采用了遥控器操纵，如公开号为CN2864092，名称为“掘进机恒功率截割的远程控制装置”的实用新型专利提出了一种远程控制装置，但只是操作者可以远离操作位置进行远程操作，仍然为目测掘进机头位置和姿态的手工操作方式，没有解决掘进机头位置和姿态的自动测量和定位。公开号为CN201013380，名称为“全自动掘进机”的实用新型专利提出采用航天导航定位仪测量掘进机的位置，但没有测量掘进机车体的位置和姿态和掘进机头的位置和姿态。\n[0003] 掘进机车体及掘进机头的位置和姿态的检测是掘进巷道断面成形控制的基础，为了提高掘进速度和掘进质量，保证所掘巷道的空间位置、形状的准确性，掘进过程中必须对掘进机头的位姿进行实时测量和定位，一方面为掘进操作提供参考依掘，另一方面通过掘进机头位姿的检测与反馈，可实现掘进过程的自动控制。\n发明内容\n[0004] 本发明要解决的问题是悬臂掘进机在掘进时掘进头的位姿的测量问题，解决人工目测观察掘进头的位姿造成巷道掘进断面成形质量差的问题，其目的是提供一种悬臂掘进机头位姿的测量系统及其测量方法。\n[0005] 实现上述目的，本发明所采用的技术方案包括悬臂掘进机及其悬臂升降油缸、悬臂回转油缸和掘进机头伸缩油缸，反射棱镜，激光机动全站仪，无线通讯模块，双轴倾角传感器，油缸行程传感器，车体偏摆角传感器，数据采集模块和计算机，其特征在于激光机动全站仪设置在已掘成形的巷道壁上，反射棱镜和双轴倾角传感器安装在悬臂掘进机的后机架上，车体偏摆角传感器安装在反射棱镜后，悬臂升降油缸、悬臂回转油缸和掘进机头伸缩油缸中安装有油缸行程传感器，两个无线通讯模块分别安装在激光机动全站仪的支架上和掘进机的控制箱上，无线通讯模块将激光机动全站仪的测量数据传输给计算机，数据采集模块和计算机安装在掘进机的控制箱里，数据采集模块采集车体偏摆角传感器、双轴倾角传感器和油缸行程传感器的数据，计算机对测量数据进行处理，计算掘进机头的位置和姿态。\n[0006] 本发明上述的技术方案所述的激光机动全站仪设置在已掘成形的巷道壁上的位置是在悬臂掘进机车体后与掘进机的后支架同水平的、距掘进机车体1m～10m之间的巷道壁上；所述的悬臂掘进机头的位姿测量系统，其特征在于反射棱镜是180度的激光测量棱镜；所述的悬臂升降油缸、悬臂回转油缸和掘进机头伸缩油缸中安装有油缸行程传感器是油缸行程传感器的测量杆从缸体的安装端沿轴线插装在缸体和缸杆里，感应磁环固定在缸杆上，感应磁环随着缸杆和油缸活塞的运动沿着测量杆滑动，传感器引线从缸体的安装端引出。\n[0007] 本发明用于悬臂掘进机头位姿的测量系统的测量方法，该方法在于激光机动全站仪测量掘进机车体在大地坐标系中的位置坐标，掘进机车体偏摆角传感器测量掘进机车体的偏摆角γ，双轴倾角传感器测量掘进机车体的仰俯角α和横滚角β，油缸行程传感器测量悬臂升降油缸、悬臂回转油缸和掘进机头伸缩油缸的行程，无线通讯模块将激光机动全站仪的测量数据传输给计算机，数据采集模块采集油缸行程传感器、双轴倾角传感器和车体偏摆角传感器的数据，计算机对采集的数据进行处理和计算，实现悬臂掘进机头的位姿的测量。\n[0008] 上述中的悬臂掘进机头位姿的测量系统的测量方法，其特征在于激光机动全站仪测量掘进机车体在大地坐标系中的位置坐标是根据激光机动全站仪在巷道壁上的安装平台的三维坐标和姿态角，对激光机动全站仪的测量数据经过坐标变换得到。\n[0009] 本发明悬臂掘进机头位姿的测量系统及其方法与现有技术相比，其优点与积极效果在于：\n[0010] (1)本发明悬臂掘进机头位姿的测量系统中设置了测量掘进机车体位姿的激光机动全站仪、180度激光反射棱镜、掘进机车体偏摆角传感器、双轴倾角传感器和油缸行程传感器，实现了掘进机车体相对大地坐标系的三维位置和姿态以及掘进机头相对掘进机车体坐标系的位置和姿态的测量，使掘进机头的位姿的测量不仅相对于掘进机车体，而且可相对于掘进巷道的大地坐标系，前者有助于掘进过程中对掘进机头的位姿的测量与定位控制，后者可实现对成形的巷道轮廓及巷道中心线相对于大地坐标系的定位，本测量系统的角度测量精度可达1分，位置测量和定位精度在±20mm以内。\n[0011] (2)本发明悬臂掘进机头位姿的测量系统，根据矿井的特殊环境，设置防爆型测量系统，测量过程自动化，位姿测量精度高，解决了掘进司机目测掘进机头位姿的定位不准确，超挖和欠挖现象严重，控制精度低的问题，降低了劳动强度，改善了工作环境，提高了巷道成形质量和掘进速度，并为掘进的自动控制提供了条件。\n[0012] (3)本发明悬臂掘进机头位姿的测量系统的测量方法，分别测量掘进机车体的位置和姿态以及掘进机悬臂各关节的变量值，采用空间坐标变换计算掘进机头在掘进机车体坐标系和大地坐标系中的位置和姿态，提高了成形巷道轮廓及其中心线的定位精度。\n附图说明\n[0013] 图1是本发明的结构组成示意图\n[0014] 图2是本发明的掘进机车体及悬臂坐标系统示意图\n[0015] 图3是本发明的掘进机车体姿态角示意图\n[0016] 图4是本发明的掘进机作业坐标系统示意图\n[0017] 图5是本发明的油缸行程传感器安装示意图\n[0018] 图中：1：计算机 2：无线通讯模块 3：激光机动全站仪 4：反射棱镜 5：车体偏摆角传感器 6：后机架 7：双轴倾角传感器 8：掘进机 9：悬臂升降油缸 10：悬臂回转油缸 \n11：掘进机头伸缩油缸 12：油缸行程传感器 13：数据采集模块 14：控制箱 15：传感器引线 16：安装端 17：感应磁环 18：油缸活塞 19：测量杆 20：缸体 21：缸杆具体实施方式\n[0019] 本发明在上述基础上，下面分别用悬臂掘进机头的位姿测量系统和悬臂掘进机头的位姿的测量方法的具体实施例来详细说明本发明的技术实施方案。本发明只提供悬臂掘进机头位姿的测量系统及其测量方法，不提供有关计算机的软件程序。\n[0020] 实施例1\n[0021] 如图1和图5所示，悬臂掘进机头的位姿测量系统由悬臂掘进机8的悬臂升降油缸9、悬臂回转油缸10和掘进机头伸缩油缸11，油缸行程传感器12，激光机动全站仪3，反射棱镜4，无线通讯模块2，车体偏摆角传感器5，双轴倾角传感器7，数据采集模块13和计算机1构成。\n[0022] 1)在掘进机8车体后已掘成形的、与掘进机8的后支架6同水平的、距掘进机车体\n1米至10米之间的巷道壁上设置有激光机动全站仪3的安装平台，并测量安装平台在大地坐标系中的三维坐标和姿态角，将激光机动全站仪3安装在该平台上，激光机动全站仪3的测量方向朝向掘进机8。\n[0023] 2)在掘进机8的后机架6上安装双轴倾角传感器7，双轴倾角传感器7的两个测量轴分别平行于掘进机车体的纵轴和横轴。\n[0024] 3)在掘进机8的后支架6上安装反射棱镜4，反射棱镜4是一个可以180度反射激光机动全站仪3的测量激光的棱镜，反射棱镜4的横轴平行于掘进机车体的横轴安装，反射棱镜4的反射面朝向激光机动全站仪3。\n[0025] 4)在反射棱镜4后紧贴反射棱镜4安装掘进机车体偏摆角传感器5。\n[0026] 5)沿着轴线，分别对悬臂升降油缸9、悬臂回转油缸10和掘进机头伸缩油缸11的缸体20和缸杆21钻孔，用于安装油缸行程传感器12。油缸行程传感器12由传感器引线\n15、感应磁环17和测量杆19构成，传感器引线15从缸体20的安装端16引出，测量杆19从缸体20的安装端16沿轴线插装在缸体20和缸杆21中，感应磁环17固定在缸杆21上，并随油缸活塞18和缸杆21的运动沿着测量杆19滑动。油缸行程传感器12的测量杆19通过螺纹固定在缸体20中，感应磁环17由压紧螺母固定在缸杆21中。\n[0027] 6)无线通讯模块2有两个，一个安装在激光机动全站仪3的支架上，另一个安装在掘进机8的控制箱14上。\n[0028] 7)计算机1和数据采集模块13安装在掘进机8的防爆控制箱14里。\n[0029] 实施例2\n[0030] 如图2和图4所示，悬臂掘进机头位姿的测量方法的解决是建立大地坐标系OXYZ、测量坐标系OcXcYcZc、掘进机车体坐标系O0X0Y0Z0、悬臂回转关节坐标系O1X1Y1Z1、悬臂升降关节坐标系O2X2Y2Z2、掘进机头伸缩关节坐标系O3X3Y3Z3和巷道断面坐标系OhXhYhZh，各坐标系统串联构成空间坐标变换尺寸链。\n[0031] 图2所示的掘进机系统的简化模型上建立了掘进机车体坐标系统和悬臂关节坐标系统。θ1、θ2和d分别是掘进机悬臂的回转关节、升降关节和掘进机头伸缩关节变量，即悬臂水平回转角θ1、悬臂垂直升降角θ2和掘进机头伸缩量d，掘进机悬臂连杆参数为：\n[0032] \n连杆序号i ai-1 αi-1 di θi 关节变量\n1 a1 0° b2 θ1 θ1\n2 a2 -90° 0 θ2 θ2\n3 b3 -90° a3+d 0° d\n[0033] 相邻两连杆的坐标变换关系ii-1T为：\n[0034] \n[0035] 掘进机头坐标系O3X3Y3Z3相对掘进机车体坐标系O0X0Y0Z0的位姿变换矩阵30T可表示为：\n[0036] \n[0037] 掘进机车体坐标系O0X0Y0Z0相对大地坐标系OXYZ的变换0T由掘进机车体相对测c\n量仪器坐标系OcXcYcZc的变换0T和测量仪器坐标系OcXcYcZc相对大地坐标系OXYZ的变换cTc\n两部分组成。0T的参数包括激光机动全站仪3测量的掘进机车体的三维坐标(Sx，Sy，Sz)，双轴倾角传感器7测量的掘进机车体的仰俯角α和横滚角β以及掘进机车体偏摆角传感c\n器5测量的掘进机车体偏摆角γ。0T的计算公式为：\n[0038] \n[0039] 其中，Rot(Z，γ)是绕Z轴旋转γ角的齐次旋转变换矩阵；Rot(Y，β)是绕Y轴旋转β角的齐次旋转变换矩阵；Rot(X，α)是绕X轴旋转α角的齐次旋转变换矩阵；\nTrans(Sx，Sy，Sz)是车体坐标系O0X0Y0Z0相对测量坐标系OcXcYcZc的齐次平移变换矩阵。\n[0040] 测量坐标系OcXcYcZc相对大地坐标系OXYZ的变换cT由激光机动全站仪3的安装平台的三维位置坐标与姿态角决定，该安装平台在大地坐标系中的三维坐标和姿态角通过全站仪预先测得，所以cT为已知量。\n[0041] 掘进机车体坐标系O0X0Y0Z0相对大地坐标系OXYZ的变换0T可由下式计算：\n[0042] \n[0043] 设掘进机头在坐标系O3X3Y3Z3中的齐次坐标为：\n[0044] 3P＝[Px Py Pz 1]T (5)\n[0045] 式中：记号[]T表示矩阵[]的转置。\n[0046] 由(2)式可计算掘进机头相对掘进机车体坐标系O0X0Y0Z0的位置坐标0P为：\n[0047] \n[0048] 由(2)式和(4)式可计算掘进机头相对大地坐标系OXYZ的位置坐标P为：\n[0049] \n[0050] 当掘进机悬臂按水平回转角θ1和垂直升降角θ2摆动时，由(6)式和(7)式可分别计算掘进机头在掘进机车体坐标系和大地坐标系中的位姿，实现掘进机头的位置和姿态的测量。\n[0051] 悬臂掘进机头的位姿测量方法的步骤为：\n[0052] 1)启动激光机动全站仪3，激光机动全站仪3自动捕捉和跟踪掘进机8上的180度激光反射棱镜4，测量掘进机车体的三维坐标Sx、Sy和Sz。\n[0053] 2)安装在180度激光反射棱镜4后的掘进机车体偏摆角传感器5测量激光机动全站仪3的测量激光射入反射棱镜4的水平入射角，也就是掘进机车体的偏摆角γ。\n[0054] 3)掘进机8的后机架6上安装的双轴倾角传感器7测量掘进机车体的仰俯角α和横滚角β。\n[0055] 4)掘进机8的悬臂升降油缸9、悬臂回转油缸10和掘进机头伸缩油缸11中的油缸行程传感器12为线性测量传感器，感应磁环17相对测量杆19的位置通过测量杆19里的测量电路转换成反映油缸行程量的电信号，由油缸行程传感器12输出电压信号，经下式计算悬臂各关节的变量值θ1、θ2和d：\n[0056] X＝kV\n[0057] 其中，X为各关节的变量值θ1或θ2或d；V为油缸行程传感器的输出电压值；k为测量系数，由传感器、电路及机械参数决定。\n[0058] 5)一对无线通讯模块2将上述1)中所述的激光机动全站仪3的测量数据传输给掘进机8控制箱14中的计算机1。\n[0059] 6)掘进机8控制箱14里的数据采集模块13采集车体偏摆角传感器5、双轴倾角传感器7和油缸行程传感器12的测量数据，并传输给计算机1。\n[0060] 7)计算机1根据上述测量数据，由上述(6)式和(7)式分别计算掘进机头相对掘进机车体坐标系和大地坐标系的位置和姿态，即测量得到掘进机头的位置和姿态。