基于旋转扇面激光的悬臂式掘进机位姿测量方法
【技术领域】
[0001] 本发明属于工程机械设备位姿测量技术领域，特别涉及一种对煤矿用悬臂式掘进 机的位置和姿态进行测量，进而对悬臂式掘进机掘进方向进行精确指向。
【背景技术】
[0002] 传统的悬臂式掘进机位姿测量方法是人工目视法，悬臂式掘进机在煤巷中唯一的 参考来自于点激光指向仪。在悬臂式掘进机后方安放1台点激光指向仪（指示巷道设计方 向），其在煤巷前部掘进断面形成一个光斑，悬臂式掘进机司机在作业中须以肉眼观察掘进 断面上光斑的位置，时时注意巷道走向是否正确。该方法精度低，操作复杂，耗费时间，可靠 性差，全凭操作司机的经验来保证巷道成型质量。
[0003] 随着技术的发展，出现了大量的现代化位姿测量方式，如自动全站仪位姿测量系 统和惯性导航位姿测量系统。对于自动全站仪位姿测量系统，机器人在地下行进过程中可 以自动跟踪标靶，对目标点进行实时连续检测。但全站仪使用激光束作为测量介质来对空 间中某点进行定位，只能实现点对点的测量，抗遮挡能力差，煤矿掘进工作面设备杂多，且 煤巷走向起伏不平，通视情况差，故使用全站仪的话其激光束很容易被遮挡。对于惯性导航 系统，定位误差会随时间而增大，不适用于长距离长时间的测量，需要定期的矫正。而煤矿 掘进工作面一般位于地下几十至几百米处，无法使用GPS等定位技术来对惯性导航系统进 行校正。故绝大部分现代化的位姿测量方式不适用于悬臂式掘进机位姿测量。
【发明内容】
[0004] 本发明的目的在于提出一种利用基于扇面激光测角的空间点三维坐标测量系统 进行悬臂式掘进机位姿测量的方法，可实现对悬臂式掘进机的位置和姿态进行测量，大大 提高了对悬臂式掘进机位姿的测量精确性，为悬臂式掘进机控制提供精确数据，以提高巷 道成型质量。
[0005] 本发明的技术方案：利用基于扇面激光测角的空间点三维坐标测量系统进行悬臂 式掘进机位姿测量，包含以下步骤：
[0006] (1)将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激 光接收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。
[0007] (2)由扇面激光发射端构建一坐标系，称为机身坐标系（O-XYZ)。扇面激光发射端 由高精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置 旋转轴为Z轴，高精度滑台移动方向为X轴。
[0008] (3)由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标 系（O' -X' Y' Z，）。
[0009] (4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的 作用下，在两个位置（初始位置和移动位置）发射出三束扇面激光交汇到一点。依次测 得3个扇面激光接收器在机身坐标系（O-XYZ)的三维坐标值，即（ΧρΥρΖ)、（x 2, y2, Z2)、 (X3, y3，Z3)。
[0010] (5)如图I所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系（O' -Xr Y1 )相对于机身坐标系（O-XYZ)的各变换参数。T1为巷道坐标系（(V -Xr Y1 Z')的r轴在机身坐标系（O-XYZ)下的方向向量，T2、T^别为r轴的方向向量， X。为巷道坐标系（CV -X' Y' Z')的原点在机身坐标系（O-XYZ)下的坐标。
[0011] 从而获得平移矩阵D1和旋转矩阵R lt)
[0012] (6)用矿用激光全站仪测得3个扇面激光接收器相对于空间大地坐标系的三维坐 标值，同样按照上述方法求得巷道坐标系（CV -X' Y' Z')与空间大地坐标系的平移矩 阵D2和旋转矩阵R 2，以及巷道坐标系原点在大地坐标系下的坐标（a，b，C)。
[0013] (7)将扇面激光发射端上固定三点，即机身坐标系（O-XYZ)上不共线固定3点变换 至空间大地坐标系中，假设所取三点坐标分别为（100,0, 0)、（0, 100, 0)、（〇,〇, 100)，单位为 mm。以（100,0,0)为例，变换公式如下：
[0014] (X，Y，Z，1) = (100,0,0, 1) · D1 · R1 · D2 · R2
[0015] (X，Υ，Ζ)即为机身坐标系中（100,0,0)在空间大地坐标系中的坐标值，依照上述 公式分别求出（〇, 100, 〇)、（〇,〇, 100)进行坐标系变换后的坐标值。
[0016] 由于扇面激光发射端与悬臂式掘进机固联，故扇面激光发射端上固定三点就是悬 臂式掘进机上固定三点。
[0017] (8)根据悬臂式掘进机上不共线固定三点相对于空间大地坐标系的三维坐标值， 即可表示悬臂式掘进机在空间大地坐标系内的位置和姿态情况，由此实现对悬臂式掘进机 的定位和姿态测量，为其进一步掘进提供精确数据。
[0018] 本发明的有益效果：①基于旋转扇面激光测角的空间点三维坐标测量系统是一种 全自动化、高可靠性、高精度、低成本的三维坐标测量系统，悬臂式掘进机在煤巷掘进时，将 本系统的扇面激光接收端（3个扇面激光接收器）固定在已掘巷道固定点处，将扇面激光发 射端安装在悬臂式掘进机上，使其与悬臂式掘进机固联，实现对悬臂式掘进机位姿状态的 高精度、高自动化测量；②可达到Icm的定位精度和优于0. Γ的姿态检测精度，完全满足 对悬臂式掘进机的位姿测量需求，极大地提高了煤巷成型质量。
【附图说明】
[0019] 图1为本发明实施例中悬臂式掘进机位姿测量过程示意图。
[0020] 1 :扇面激光发射器 2 :高精度转台；
[0021] 3:高精度滑台 4:机身坐标系
[0022] 5 :巷道坐标系 6 :扇面激光接收器1
[0023] 7 :扇面激光接收器2 8 :扇面激光接收器3
[0024] 9 :空间大地坐标系
【具体实施方式】
[0025] 如图1所示，利用基于扇面激光测角的空间点三维坐标测量系统进行悬臂式掘进 机位姿测量的方法，位姿测量过程包含以下步骤：
[0026] (1)将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激 光接收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。
[0027] (2)由扇面激光发射端构建一坐标系，称为机身坐标系（O-XYZ)。扇面激光发射端 由高精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置 旋转轴为Z轴，高精度滑台移动方向为X轴。
[0028] (3)由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标 系（O' -X' Y' Z，）。
[0029] (4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的作 用下，在两个位置（初始位置和移动位置）发射出三束扇面激光交汇到一点。依次测得3 个扇面激光接收器在机身坐标系（O-XYZ)的三维坐标值，即扇面激光接收器I :(Xl，yi，Z1)， 扇面激光接收器2 : (x2, y2, z2)，扇面激光接收器3 : (x3, y3, z3)。
[0030] (5)如图1所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系（0' -Xr Y' )相对于机身坐标系（O-XYZ)的各变换参数。T1为巷道坐标系（(V -Xr Y1 Z')的t轴在机身坐标系（O-XYZ)下的方向向量，T2、T^别为t轴的方向向量， X。为巷道坐标系（CV -X' Y' Z')的原点在机身坐标系（O-XYZ)下的坐标。
[0031] 因表达式过于复杂，故用t来代替部分表达式，设：
【主权项】
1. 一种用于煤矿掘进工作面的悬臂式掘进机位姿测量方法，其特征在于位姿测量过程 包含以下步骤： (1) 将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激光接 收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。 (2) 由扇面激光发射端构建一坐标系，称为机身坐标系（O-XYZ)。扇面激光发射端由高 精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置旋转 轴为Z轴，高精度滑台移动方向为X轴。 (3) 由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标系 (0，-X，Y，Z，）。 (4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的作用下， 在两个位置（初始位置和移动位置）发射出三束扇面激光交汇到一点。依次测得3个扇面 激光接收器在机身坐标系（O-XYZ)的三维坐标值，即（X1,Y1,Z1)、（x2,y2,z2)、（x3,y3,Z3)。 (5) 如图1所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系 ((T-X'Y'Z')相对于机身坐标系（O-XYZ)的各变换参数。T1为巷道坐标系 (O' -X^ )的t轴在机身坐标系（O-XYZ)下的方向向量，T2、T3*别为t、Z' 轴的方向向量，Xtl为巷道坐标系（〇' -X'Y'Z')的原点在机身坐标系（O-XYZ)下的坐 标。 从而获得平移矩阵D1和旋转矩阵Rlt) (6) 用矿用激光全站仪测得3个扇面激光接收器相对于空间大地坐标系的三维坐标 值，同样按照上述方法求得巷道坐标系（〇' -X'Y'Z')与空间大地坐标系的平移矩阵 D2和旋转矩阵R2，以及巷道坐标系原点在大地坐标系下的坐标（a，b，c)。 (7) 将扇面激光发射端上固定三点，即机身坐标系（O-XYZ)上不共线固定3点变换至空 间大地坐标系中，假设所取三点坐标分别为（100,0, 0)、（0, 100, 0)、（0,0,100)，单位为mm。 以（100, 0,0)为例，变换公式如下： (X，Y，Z，1) = (100,0,0, 1) ?D1 ?R1 ?D2 ?R2 (X，Y，Z)即为机身坐标系中（100,0,0)在空间大地坐标系中的坐标值，依照上述公式 分别求出（〇, 100, 〇)、（〇,〇, 100)进行坐标系变换后的坐标值。 由于扇面激光发射端与悬臂式掘进机固联，故扇面激光发射端上固定三点就是悬臂式 掘进机上固定三点。 (8) 根据悬臂式掘进机上不共线固定三点相对于空间大地坐标系的三维坐标值，即可 表示悬臂式掘进机在空间大地坐标系内的位置和姿态情况，由此实现对悬臂式掘进机的定 位和姿态测量，为其进一步掘进提供精确数据。
【专利摘要】本发明涉及一种用于煤矿掘进工作面的悬臂式掘进机位姿测量方法，充分利用基于旋转扇面激光测角的空间点三维坐标测量系统，即将该系统的扇面激光发射端安装在悬臂式掘进机机身上，扇面激光接收端（包含3个不共线的扇面激光接收器）固定安装在已经由悬臂式掘进机掘出的煤巷后方，扇面激光接收器相对于空间大地坐标系的坐标值由其它测量方式测得。由扇面激光发射端测得扇面激光接收器相对于发射端的坐标值，再通过相应的坐标转换，计算出悬臂式掘进机上固定三点（不共线）相对于空间大地坐标系的三维坐标值。若已知空间刚体上固定三点（不共线）相对于空间大地坐标系的三维坐标值，可计算出该空间刚体的位姿（位置和姿态）数据。
【IPC分类】G01C15-00, G01C21-00
【公开号】CN104729501
【申请号】CN201510121265
【发明人】吴淼, 贾文浩, 符世琛, 陶云飞, 张敏骏, 宗凯, 薛光辉, 杨健健
【申请人】中国矿业大学(北京)
【公开日】2015年6月24日
【申请日】2015年3月19日