悬臂式掘进机自动导航和定位截割的方法和系统与流程

1.本发明涉及煤矿智能开采技术领域，特别是一种悬臂式掘进机自动导航和定位截割的方法和系统。


背景技术：

2.目前悬臂式掘进机在井下巷道掘进作业时，主要通过生产矿井测绘技术人员基于巷道导线点布设的激光指向仪，在巷道截断面上形成的激光光斑作为掘进作业时导航及定位的依据。在煤矿巷道掘进作业过程中，掘进工作面环境十分恶劣，噪音大、粉尘大、湿度大、温度高，人员工作劳动危险性高，劳动强度大。虽然传统的激光指向仪也能满足日常生产要求，但是很大程度上要依赖掘进机司机的经验，对人员综合素质要求高，更无法满足智能化掘进要求。
3.目前，也有针对悬臂式掘进机自动导航及定位截割做了相关的技术改进，但该技术主要存在以下问题：
4.(1)悬臂式掘进机通过两侧的履带蠕动前进，编码器无法精确测量行走里程，无法精确定位。
5.(2)悬臂式掘进机的截割头坐标计算采用的机身相对坐标系，无法精准掌握截割头掘进的实时绝对位置，无法动态修正和补偿悬臂式掘进机工作过程中机身姿态变化导致的误差。
6.(3)基于导线点固定架设全自动全站仪，需要人工对中导线点和固定安装，无法跟随悬臂式掘进机机尾自动推进。
7.(4)基于全自动全站仪后视两个控制点距离交会方法，由于巷道的狭长通道形状导致后视两个控制点和全站仪设站点水平夹角过小，后方交会计算会出现危险圆，导致大地坐标计算失效。
8.(5)基于点云、视觉图像的定位技术，由于掘进工作面粉尘、湿度非常大，无法常态化应用。


技术实现要素：

9.鉴于上述问题，本发明提出了一种悬臂式掘进机自动导航和定位截割的方法和系统。
10.本发明实施例提供了一种悬臂式掘进机自动导航和定位截割的方法，所述方法包括：
11.全自动陀螺全站仪自动找平和寻北，后视搜索并获取控制点棱镜的三维大地坐标；
12.所述全自动陀螺全站仪根据所述控制点棱镜的三维大地坐标，和所述全自动陀螺全站仪到所述控制点棱镜的倾角、斜距，计算得到所述全自动陀螺全站仪的三维大地坐标，其中，所述全自动陀螺全站仪的三维大地坐标为所述全自动陀螺全站仪设站点的三维大地
坐标；
13.所述全自动陀螺全站仪根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪机架棱镜，实时测量得到所述机架棱镜的三维大地坐标，并将所述机架棱镜的三维大地坐标发送给工控机；
14.所述工控机根据所述机架棱镜的三维大地坐标，以及所述悬臂式掘进机的参数、第一位移值、第二位移值，运算得到所述悬臂式掘进机的截割头的三维大地坐标，所述第一位移值为回转油缸位移传感器测量得到的值，所述第二位移值为升降油缸位移传感器测量得到的值；
15.所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式掘进机的位置和航向进行动态纠偏。
16.可选地，全自动陀螺全站仪自动找平和寻北，后视搜索并获取控制点棱镜的三维大地坐标，包括：
17.所述全自动陀螺全站仪自动整平和寻北；
18.所述全自动陀螺全站仪完成所述整平和所述寻北后，识别控制点点号，所述控制点点号对应所述控制点棱镜的三维大地坐标；
19.所述全自动陀螺全站仪基于所述控制点点号，获取所述控制点棱镜的三维大地坐标；
20.其中，所述控制点棱镜安装于掘进巷道导线控制点上。
21.可选地，所述工控机根据所述机架棱镜的三维大地坐标，以及所述悬臂式掘进机的参数、第一位移值、第二位移值，计算得到所述悬臂式掘进机的截割头的三维大地坐标，包括：
22.所述工控机根据所述第一位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的水平回转角度；
23.所述工控机根据所述第二位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的垂直升降角度；
24.所述工控机根据所述水平回转角度、所述垂直升降角度、所述悬臂式掘进机的参数、所述机架棱镜的三维大地坐标，运算得到所述截割头的三维大地坐标。
25.可选地，所述工控机根据所述第一位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的水平回转角度，包括：
26.所述悬臂式掘进机的参数包括：定义回转油缸的零位移长度值l
h0
、回转油缸的水平转动中心点o、回转油缸与机架的铰接点p、回转油缸与回转臂的铰接点q、在同一直线上悬臂式掘进机的机架纵向中轴线为aob、截割臂的纵向中轴线ot、铰接点p至水平转动中心点o的固定长度值l
po
、铰接点q至水平转动中心点o的固定长度值l
qo
、固定角度值：∠qot、∠aop；
27.则所述工控机根据所述第一位移值、所述回转油缸的零位移长度值l
h0
，运算得到铰接点p至铰接点q的长度值l
pq
＝l
h0
sh，其中，sh即所述第一位移值；
28.因三角形poq的三边长度已知，所述工控机根据所述铰接点p至水平转动中心点o
的固定长度值l
po
、所述铰接点q至水平转动中心点o的固定长度值l
qo
、所述铰接点p至铰接点q的长度值l
pq
，并结合余弦定理得到：
[0029][0030]
则有：∠boq＝180-∠poq-∠aop，进而得到所述悬臂式掘进机的水平回转角度ah＝∠boq-∠qot，ah即∠bot。
[0031]
可选地，所述工控机根据所述第二位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的垂直升降角度，包括：
[0032]
所述悬臂式掘进机的参数包括：定义升降油缸的零位移长度值l
v0
、升降油缸的垂直转动中心点o、升降油缸与机架的铰接点m、升降油缸与截割臂的铰接点n、固定角度值：∠mop、∠ony、铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、截割臂on水平时固定角度值∠mop；
[0033]
则所述工控机根据所述第二位移值、所述升降油缸的零位移长度值l
v0
，运算得到铰接点m至铰接点n的长度值l
mn
＝l
v0
sv，其中，sv即所述第二位移值；
[0034]
因三角形mon的三边长度已知，所述工控机根据铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、所述铰接点m至铰接点n的长度值l
mn
，并结合余弦定理得到：
[0035][0036]
则得到所述悬臂式掘进机的垂直升降角度av＝∠mon-∠mop，av即∠pon。
[0037]
可选地，所述工控机根据所述水平回转角度、所述垂直升降角度、所述悬臂式掘进机的参数、所述机架棱镜的三维大地坐标，运算得到所述截割头的三维大地坐标，包括：
[0038]
所述悬臂式掘进机的参数包括：定义所述悬臂式掘进机的机身坐标系为以所述回转油缸的水平转动中心点o、所述回转油缸与机架的铰接点p1和p2、以及所述回转油缸与所述回转臂的铰接点q1和q2，共五点构成的平面为xoy坐标系平面，以所述回转油缸的水平转动中心点o为坐标原点，p1和p2的中点为a，ao直线为x轴，垂直于ao的直线为y轴，构成的右手坐标系；
[0039]
定义截割头最远端点为t，ny为平行于截割臂的中心轴线，y点为t在ny直线上的垂足，则点t即为需要计算的截割头的位置，y点至t点的固定长度值l
yt
、y点至n点的固定长度值l
ny
；
[0040]
而升降中心o点在机身坐标系下的坐标可以从所述悬臂式掘进机结构几何参数获取：(o
x
，oy，oz)；
[0041]
则所述工控机运算得到所述截割臂在xoy平面上投影长度：l
ot
＝l
no
*cosav l
ny
*cos(∠ony a
v-180) l
yt
*sin(∠ony a
v-180)
[0042]
则所述截割头在机身坐标系下的坐标为：
[0043]
x’＝o
x
l
ot
*cosah；
[0044]
y’＝oy l
ot
*sinah；
[0045]
z’＝oz l
no
*sinav l
ny
*sin(∠ony a
v-180)-l
yt
*cos(∠ony a
v-180)；
[0046]
工控机接收惯导实时测量的机架欧拉角，所述欧拉角包括：航向角y、俯仰角p、翻滚角r；
[0047]
则所述工控机根据所述欧拉角，将所述截割头在机身坐标系下的坐标，转换为三维大地坐标系，转换使用的旋转矩阵如下：
[0048][0049]
其中：
[0050][0051][0052][0053]
其中，表示机身坐标系向大地坐标系转换的旋转矩阵；
[0054]
t
横
表示机身坐标系向大地坐标系转换时横滚方向的旋转矩阵；
[0055]
t
俯
表示机身坐标系向大地坐标系转换时俯仰方向的旋转矩阵；
[0056]
t
航
表示机身坐标系向大地坐标系转换时航向方向的旋转矩阵；
[0057]
若定义所述机身棱镜目标点为a，其在机身坐标系下坐标为(x’a
，y’a
，z’a
)，其在三维大地坐标系下由所述全自动陀螺全站仪跟踪测量得到的坐标为(xa，ya，za)，则所述机身棱镜从机身坐标向三维大地坐标转换的平移向量为：
[0058][0059]
则根据所述截割头最远端点t的机身坐标(x’，y’，z’)，运算得到所述截割头的三维大地坐标矩阵表达式为：
[0060][0061]
根据上述方法即运算得到所述截割头的三维大地坐标。
[0062]
可选地，所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式位置和航向进行动态纠偏。包括：
[0063]
所述工控机确定基于截割断面坐标系的定位截割初始化位置；
[0064]
所述工控机根据所述截割头的三维大地坐标、所述定位截割初始化位置，结合所述欧拉角、所述截割头的截割断面坐标系，计算得到对应所述截割头的截割路径，以及所述悬臂式掘进机的导航修正参数，所述截割路径使得所述悬臂式掘进机按设定参数和工艺流程定位截割，所述导航修正参数用于对所述悬臂式位置和航向进行动态纠偏；
[0065]
所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式位置和航向进行动态纠偏。
[0066]
可选地，所述截割头的截割断面坐标系定义如下：
[0067]
以矩形断面为例：
[0068]
基于所述截割头的坐标原点位置定义所述截割断面坐标系：矩形断面的高度h米，宽度w米，以经过所述截割头的坐标原点垂直于巷道中心线的纵断面为坐标平面，以平行于断面宽度w方向且经过原点的0的直线为x轴，以平行于断面高度h方向且经过原点的0的直线为y轴；
[0069]
所述截割头的坐标原点位置定义：以所述截割臂的回转角度为0度，所述截割臂的中心轴线方位角为巷道设计方位角。
[0070]
本发明实施例提供一种悬臂式掘进机自动导航和定位截割的系统，所述系统采用如上任一所述的方法实现悬臂式掘进机自动导航和定位截割的方法，所述系统包括：工控机、全自动陀螺全站仪、惯导、机架棱镜、控制点棱镜、升降油缸位移传感器以及回转油缸位移传感器；
[0071]
所述工控机与所述全自动陀螺全站仪交互，所述工控机运算得到所述悬臂式掘进机的截割头的三维大地坐标，以及运算得到对应所述截割头的截割路径，以及悬臂式掘进机的导航修正参数，并发送给所述全自动陀螺全站仪，所述工控机为安装有操作系统的工业计算机，所述工控机中部署有基于三维大地坐标的悬臂式掘进机自动导航的控制程序，和定位截割系统的控制程序；
[0072]
所述全自动陀螺全站仪与所述惯导、所述机架棱镜、所述控制点棱镜共体工作，得到所述机架棱镜的三维大地坐标并发送至所述工控机，所述全自动陀螺全站仪是一种集自动寻北、自动整平、自动目标识别、自动照准、自动测角测距、自动目标跟踪、自动计算、自动存储于一体的测量平台，所述全自动陀螺全站仪具有减震功能，防止掘进过程和整体推进过程产生的震动破坏和影响所述全自动陀螺全站仪；
[0073]
所述惯导与所述全自动陀螺全站仪交互，用于实时测量所述悬臂式掘进机机架的欧拉角，所述惯导刚性安装于所述悬臂式掘进机机架，跟随所述悬臂式掘进机一起运动，所述惯导工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航系统；
[0074]
所述机架棱镜与所述全自动陀螺全站仪交互，所述机架棱镜刚性安装于所述悬臂式掘进机朝向所述全自动陀螺全站仪一侧的机架上，所述机架棱镜跟随所述悬臂式掘进机一起运动，所述悬臂式掘进机、所述惯导、所述机架棱镜三者刚性连接，整体运动；
[0075]
所述控制点棱镜与所述全自动陀螺全站仪交互，所述控制点棱镜为所述全自动陀
螺全站仪测量的光学目标装置，所述控制点棱镜安装于掘进巷道顶板导线控制点上；
[0076]
所述升降油缸位移传感器与所述工控机交互，用于测量升降油缸的位移值；
[0077]
所述回转油缸位移传感器与所述工控机交互，用于测量回转油缸的位移值。
[0078]
可选地，所述全自动陀螺全站仪固定安装于机尾巷道或集控舱顶部，所述全自动陀螺全站仪采用固定安装，或采用非固定安装；
[0079]
所述固定安装指固定一次安装，防护罩可以开闭；
[0080]
所述非固定安装指通过液压或者电机驱动，使得全自动陀螺全站仪工作时伸到合适位置，不工作时收缩回去并密封装箱到防护罩里。
[0081]
本发明提供的悬臂式掘进机自动导航和定位截割的方法，全自动陀螺全站仪获取控制点棱镜的三维大地坐标；接着根据控制点棱镜的三维大地坐标，和全自动陀螺全站仪到控制点棱镜的水平角、倾角、斜距，运算得到全自动陀螺全站仪的三维大地坐标。之后全自动陀螺全站仪根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪测量机架棱镜，实时测量得到机架棱镜的三维大地坐标，并将机架棱镜的三维大地坐标发送给工控机。
[0082]
工控机根据机架棱镜的三维大地坐标，以及悬臂式掘进机的参数、第一位移值、第二位移值，运算得到悬臂式掘进机的截割头的三维大地坐标，工控机再根据截割头的三维大地坐标，运算得到对应截割头的截割路径，以及悬臂式掘进机的导航修正参数，并发送给全自动陀螺全站仪；全自动陀螺全站仪根据截割路径，控制悬臂式掘进机进行挖掘，并根据导航修正参数，控制悬臂式掘进机进行自动导航推进。
[0083]
本发明的方法，使得悬臂式掘进机基于绝对大地坐标的导航定位，可动态修正和补偿悬臂式掘进机工作过程中机身姿态变化导致的误差。基于全自动陀螺全站仪毫米级可靠的高精度测量，无需利用点云、视觉图像的定位技术，基本不受掘进工作面粉尘、湿度影响，保证了悬臂式掘进机导航定位的持续高精度。工作过程中，工控机实时计算截割头的绝对大地坐标和在截割断面坐标系下的相对坐标，保证了绝对和相对的统一。另外，对悬臂式掘进机初始化状态没有严格限制和要求，可以基于机身棱镜坐标和机身惯导的欧拉角动态规划截割头的截割路径。而采用全自动陀螺全站仪，高精度寻北，后视只需一个导线点，无需使用后视两个控制点距离交会方法以及人工维护，不会导致大地坐标计算失效，全自动陀螺全站仪自动找平并跟随悬臂式掘进机机尾自动推进。
附图说明
[0084]
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
[0085]
图1是本发明实施例的悬臂式掘进机自动导航和定位截割的方法的流程图；
[0086]
图2是本发明实施例中悬臂式掘进机俯视结构示意图1；
[0087]
图3是本发明实施例中悬臂式掘进机俯视结构示意图2；
[0088]
图4是本发明实施例中悬臂式掘进机侧视结构示意图1；
[0089]
图5是本发明实施例中悬臂式掘进机垂直升降几何示意图；
[0090]
图6是本发明实施例中悬臂式掘进机水平回转几何示意图；
[0091]
图7是本发明实施例中悬臂式掘进机侧面结构示意图2；
[0092]
图8是本发明实施例中悬臂式掘进机垂直升降时截割头几何计算示意图；
[0093]
图9是本发明实施例中定义截割断面坐标系以及以矩形断面为例的截割路径的示意图。
具体实施方式
[0094]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。应当理解，此处所描述的具体实施例仅用以解释本发明，仅仅是本发明一部分实施例，而不是全部的实施例，并不用于限定本发明。
[0095]
参照图1，示出了本发明实施例的悬臂式掘进机自动导航和定位截割的方法的流程图，该方法包括：
[0096]
步骤101：全自动陀螺全站仪自动找平和寻北，后视搜索并获取控制点棱镜的三维大地坐标。
[0097]
本发明实施例的悬臂式掘进机自动导航和定位截割的方法，首先需要获取控制点棱镜的三维大地坐标。一般情况下，控制点棱镜安装于掘进巷道顶板导线控制点上，基于其安装地点，可以获取到其三维大地坐标。具体获取的方法包括：
[0098]
全自动陀螺全站仪在开始工作后，首先需要自动整平和寻北，这是获取三维大地坐标的基础。全自动陀螺全站仪在完成整平和寻北后，后视搜索并可以识别到控制点点号，该控制点点号对应控制点棱镜的位置；最后全自动陀螺全站仪即可基于控制点点号，获取控制点棱镜的三维大地坐标。
[0099]
步骤102：全自动陀螺全站仪根据控制点棱镜的三维大地坐标，和全自动陀螺全站仪到控制点棱镜的倾角、斜距，计算得到全自动陀螺全站仪的三维大地坐标，其中，全自动陀螺全站仪的三维大地坐标为全自动陀螺全站仪设站点的三维大地坐标。
[0100]
本发明实施例中，在获取控制点棱镜的三维大地坐标后，全自动陀螺全站仪可以根据控制点棱镜的三维大地坐标，和全自动陀螺全站仪到控制点棱镜的倾角、斜距，运算得到全自动陀螺全站仪的三维大地坐标。这其中，全自动陀螺全站仪到控制点棱镜的倾角、斜距这两项参数，全自动陀螺全站仪可以自动获取到。结合这两项参数，再加上控制点棱镜的三维大地坐标，全自动陀螺全站仪即可精确得到自身的三维大地坐标。
[0101]
需要说明的是，全自动陀螺全站仪可以固定安装于机尾巷道或集控舱顶部。全自动陀螺全站仪既可以固定安装，也可非固定安装。固定安装指固定一次安装，防护罩可以开闭；非固定安装指通过液压或者电机驱动，使得全自动陀螺全站仪工作时伸到合适位置，不工作时收缩回去并密封装箱到防护罩里，以防水、防尘、防碰撞。无论哪种安装方式，全自动陀螺全站仪安装的位置即为其设站点。因此全自动陀螺全站仪的三维大地坐标，即为全自动陀螺全站仪设站点的三维大地坐标。
[0102]
步骤103：全自动陀螺全站仪根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪机架棱镜，实时测量得到机架棱镜的三维大地坐标，并将机架棱镜的三维大地坐标发送给工控机。
[0103]
本发明实施例中，全自动陀螺全站仪得到自身的三维大地坐标后，需要根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪机架棱镜，搜索并跟踪机
架棱镜成功后，全自动陀螺全站仪可以实时测量得到机架棱镜的三维大地坐标，同时将机架棱镜的三维大地坐标发送给工控机。
[0104]
步骤104：工控机根据机架棱镜的三维大地坐标，以及悬臂式掘进机的参数、第一位移值、第二位移值，运算得到悬臂式掘进机的截割头的三维大地坐标，第一位移值为回转油缸位移传感器测量得到的值，第二位移值为升降油缸位移传感器测量得到的值。
[0105]
本发明实施例中，工控机接收到机架棱镜的三维大地坐标后，同时还会接收到第一位移值、第二位移值。所谓第一位移值为回转油缸位移传感器测量得到的值，所谓第二位移值为升降油缸位移传感器测量得到的值。
[0106]
悬臂式掘进机的参数一般在悬臂式掘进机制造完成后即可测量得到，可由工作人员输入到工控机中。在获得上述参数后，工控机可以根据机架棱镜的三维大地坐标，以及悬臂式掘进机的参数、第一位移值、第二位移值，运算得到悬臂式掘进机的截割头的三维大地坐标。在具体的运算过程中，可以分成以下三步：
[0107]
步骤s1：工控机根据第一位移值、悬臂式掘进机的参数，运算得到悬臂式掘进机的水平回转角度。
[0108]
本发明实施例中，在运算悬臂式掘进机的水平回转角度的过程中，用到的悬臂式掘进机的参数包括：定义回转油缸的零位移长度值l
h0
、回转油缸的水平转动中心点o、回转油缸与机架的铰接点p、回转油缸与回转臂的铰接点q、在同一直线上悬臂式掘进机的机架纵向中轴线为aob、截割臂的纵向中轴线ot、铰接点p至水平转动中心点o的固定长度值l
po
、铰接点q至水平转动中心点o的固定长度值l
qo
、固定角度值：∠qot、∠aop。参数具体可结合图2、图3所示的悬臂式掘进机俯视结构示意图得到更直观的理解。图2、图3中均包括：回转油缸10、机架20、回转臂30、截割臂40。
[0109]
基于上述参数，工控机根据第一位移值、回转油缸的零位移长度值l
h0
，运算得到铰接点p至铰接点q的长度值l
pq
＝l
h0
sh，其中，sh即第一位移值；因三角形poq的三边长度已知，所以工控机根据铰接点p至水平转动中心点o的固定长度值l
po
、铰接点q至水平转动中心点o的固定长度值l
qo
、铰接点p至铰接点q的长度值l
pq
，并结合余弦定理即可得到：
[0110][0111]
则有：∠boq＝180-∠poq-∠aop，进而可以得到悬臂式掘进机的水平回转角度ah＝∠boq-∠qot，ah即∠bot。
[0112]
步骤s2：工控机根据第二位移值、悬臂式掘进机的参数，运算得到悬臂式掘进机的垂直升降角度。
[0113]
本发明实施例中，在运算悬臂式掘进机的垂直升降角度的过程中，用到的悬臂式掘进机的参数包括：定义升降油缸的零位移长度值l
v0
、升降油缸的垂直转动中心点o、升降油缸与机架的铰接点m、升降油缸与截割臂的铰接点n、固定角度值：∠mop、∠ony、铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、截割臂on水平时固定角度值∠mop。参数具体可结合图4所示的悬臂式掘进机侧视结构示意图1，和图5所示的悬臂式掘进机垂直升降位移示意图得到更直观的理解。
[0114]
图4中包括：升降油缸50、机架20、截割臂40、截割头60。图5中n1和n2分别表示升降
油缸与截割臂的铰接点n，在不同升降高度时的位置。y1和y2分别表示y点在悬臂式掘进机不同升降高度时的位置，y点的含义在下文解释，先不赘述。
[0115]
基于上述参数，工控机根据第二位移值、升降油缸的零位移长度值l
v0
，运算得到铰接点m至铰接点n的长度值l
mn
＝l
v0
sv，其中，sv即第二位移值；因三角形mon的三边长度已知，所以工控机根据铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、铰接点m至铰接点n的长度值l
mn
，并结合余弦定理得到：
[0116][0117]
则工控机得到悬臂式掘进机的垂直升降角度av＝∠mon-∠mop，av即∠pon。
[0118]
步骤s3：工控机根据水平回转角度、垂直升降角度、悬臂式掘进机的参数、机架棱镜的三维大地坐标，运算得到截割头的三维大地坐标。
[0119]
本发明实施例中，得到悬臂式掘进机的水平回转角度、垂直升降角度后，工控机根据水平回转角度、垂直升降角度、悬臂式掘进机的参数、机架棱镜的三维大地坐标，运算得到截割头的三维大地坐标。具体的，在运算截割头的三维大地坐标的过程中，用到的悬臂式掘进机的参数包括：定义悬臂式掘进机的机身坐标系为以回转油缸的水平转动中心点o、回转油缸与机架的铰接点p1和p2、以及回转油缸与回转臂的铰接点q1和q2，共五点构成的平面为xoy坐标系平面，以回转油缸的水平转动中心点o为坐标原点，p1和p2的中点为a，ao直线为x轴，垂直于ao的直线为y轴，构成的右手坐标系。该坐标系由图6得到直观的理解。
[0120]
定义截割头最远端点为t，ny为平行于截割臂的中心轴线，y点为t在ny直线上的垂足，则点t即为需要计算的截割头的位置，y点至t点的固定长度值l
yt
、y点至n点的固定长度值lny。由图7所示的悬臂式掘进机侧面结构示意图2可以直观的理解。图7中包括：升降油缸50、机架20、截割臂40、截割头60。悬臂式掘进机侧视垂直升降时截割头的位移参见图8所示的示意图示出。
[0121]
而升降中心o点在机身坐标系下的坐标可以从所述悬臂式掘进机结构几何参数获取：(o
x
，oy，oz)；
[0122]
则工控机运算得到截割臂在xoy平面上投影长度：l
ot
＝l
no
*cosav l
ny
*cos(∠ony a
v-180) l
yt
*sin(∠ony a
v-180)
[0123]
则截割头在机身坐标系下的坐标为：
[0124]
x’＝o
x
l
ot
*cosah；
[0125]
y’＝oy l
ot
*sinah；
[0126]
z’＝oz l
no
*sinav l
ny
*sin(∠ony a
v-180)-l
yt
*cos(∠ony a
v-180)；
[0127]
工控机接收惯导实时测量的机架欧拉角，欧拉角包括：航向角y、俯仰角p、翻滚角r；则工控机根据欧拉角，将截割头在机身坐标系下的坐标，转换为三维大地坐标系，转换使用的旋转矩阵如下：
[0128][0129]
其中：
[0130][0131][0132][0133]
其中，表示机身坐标系向大地坐标系转换的旋转矩阵；
[0134]
t
横
表示机身坐标系向大地坐标系转换时横滚方向的旋转矩阵；
[0135]
t
俯
表示机身坐标系向大地坐标系转换时俯仰方向的旋转矩阵；
[0136]
t
航
表示机身坐标系向大地坐标系转换时航向方向的旋转矩阵；
[0137]
若定义所述机身棱镜目标点为a，其在机身坐标系下坐标为(x’a
，y’a
，z’a
)，其在三维大地坐标系下由所述全自动陀螺全站仪跟踪测量得到的坐标为(xa，ya，za)，则所述机身棱镜从机身坐标向三维大地坐标转换的平移向量为：
[0138][0139]
则根据截割头最远端点t的机身坐标(x’，y’，z’)，运算得到截割头的三维大地坐标矩阵表达式为：
[0140][0141]
根据上述方法工控机即运算得到截割头的三维大地坐标。
[0142]
步骤105：工控机基于悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制悬臂式掘进机的升降油缸、回转油缸动作，使悬臂式掘进机按设定参数和工艺流程定位截割，并对悬臂式掘进机的位置和航向进行动态纠偏。
[0143]
本发明实施例中，工控机得到截割头的三维大地坐标后，可根据截割头的三维大地坐标，运算得到对应截割头的截割路径，以及悬臂式掘进机的导航修正参数。具体的：
[0144]
工控机首先确定定位截割初始化位置；之后根据截割头的三维大地坐标、定位截割初始化位置，结合欧拉角、截割头的截割断面坐标系，运算得到对应截割头的截割路径，以及悬臂式掘进机的导航修正参数。截割路径使得悬臂式掘进机按设定参数和工艺流程定位截割，导航修正参数用于对悬臂式位置和航向进行动态纠偏。
[0145]
其中，截割头的截割断面坐标系定义如下：
[0146]
本发明实施例中，以矩形断面为例：
[0147]
基于截割头的坐标原点位置定义截割断面坐标系：矩形断面的高度h米，宽度w米，以经过截割头的坐标原点垂直于巷道中心线的纵断面为坐标平面，以平行于断面宽度w方向且经过原点的0的直线为x轴，以平行于断面高度h方向且经过原点的0的直线为y轴；截割头的坐标原点位置定义：以截割臂的回转角度为0度，截割臂的中心轴线方位角为巷道设计方位角。
[0148]
截割头的截割路径的可以为：基于定位截割初始化位置和截割头的实时大地坐标，结合机身惯导的欧拉角，动态修正和补偿空间计算偏差，通过控制悬臂式掘进机的水平回转油缸和垂直升降油缸，使得悬臂式掘进头沿y轴向上移动至巷道顶部；然后根据悬臂式掘进头的掘进尺寸依次沿x轴从左至右、沿y轴从上至下截割完整个断面；最后截割头居中沿y轴向上返回坐标原点o，完成一次作业循环。本发明实施例的断面类型包括：矩形断面、梯形断面、拱形断面。此处以矩形断面为例，可由图9得到直观理解。
[0149]
本发明实施例中，工控机基于悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制悬臂式掘进机的升降油缸、回转油缸动作，使悬臂式掘进机按设定参数和工艺流程定位截割，并对悬臂式掘进机的位置和航向进行动态纠偏。
[0150]
本发明实施例中，基于上述悬臂式掘进机自动导航和定位截割的方法，还提出一种悬臂式掘进机自动导航和定位截割的系统，该系统采用如上述步骤101～步骤105任一所述的方法实现悬臂式掘进机自动导航和定位截割的方法，所述系统包括：工控机、全自动陀螺全站仪、惯导、机架棱镜、控制点棱镜、升降油缸位移传感器以及回转油缸位移传感器；
[0151]
所述工控机与所述全自动陀螺全站仪交互，所述工控机运算得到所述悬臂式掘进机的截割头的三维大地坐标，以及运算得到对应所述截割头的截割路径，以及悬臂式掘进机的导航修正参数，所述工控机为安装有操作系统的工业计算机，所述工控机中部署有基于三维大地坐标的悬臂式掘进机自动导航的控制程序，和定位截割系统的控制程序；
[0152]
所述全自动陀螺全站仪与所述惯导、所述机架棱镜、所述控制点棱镜共体工作，得到所述机架棱镜的三维大地坐标并发送至所述工控机，所述全自动陀螺全站仪是一种集自动寻北、自动整平、自动目标识别、自动照准、自动测角测距、自动目标跟踪、自动计算、自动存储于一体的测量平台，所述全自动陀螺全站仪具有减震功能，防止掘进过程和整体推进过程产生的震动破坏和影响所述全自动陀螺全站仪；
[0153]
所述惯导与所述全自动陀螺全站仪交互，用于实时测量所述悬臂式掘进机机架的欧拉角，所述惯导刚性安装于所述悬臂式掘进机机架，跟随所述悬臂式掘进机一起运动，所述惯导工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航系统；
[0154]
所述机架棱镜与所述全自动陀螺全站仪交互，所述机架棱镜刚性安装于所述悬臂式掘进机朝向所述全自动陀螺全站仪一侧的机架上，所述机架棱镜跟随所述悬臂式掘进机一起运动，所述悬臂式掘进机、所述惯导、所述机架棱镜三者刚性连接，整体运动；
[0155]
所述控制点棱镜与所述全自动陀螺全站仪交互，所述控制点棱镜为所述全自动陀螺全站仪测量的光学目标装置，所述控制点棱镜安装于掘进巷道顶板导线控制点上；
[0156]
所述升降油缸位移传感器与所述工控机交互，用于测量升降油缸的位移值；
[0157]
所述回转油缸位移传感器与所述工控机交互，用于测量回转油缸的位移值。
[0158]
可选地，所述全自动陀螺全站仪固定安装于机尾巷道或集控舱顶部，所述全自动陀螺全站仪采用固定安装，或采用非固定安装；
[0159]
所述固定安装指固定一次安装，防护罩可以开闭；所述非固定安装指通过液压或者电机驱动，使得全自动陀螺全站仪工作时伸到合适位置，不工作时收缩回去并密封装箱到防护罩里，以防水、防尘、防碰撞。
[0160]
综上所述，本发明的悬臂式掘进机自动导航和定位截割的方法，全自动陀螺全站仪获取控制点棱镜的三维大地坐标；接着根据控制点棱镜的三维大地坐标，和全自动陀螺全站仪到控制点棱镜的倾角、斜距，运算得到全自动陀螺全站仪的三维大地坐标。之后全自动陀螺全站仪根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪测量机架棱镜，实时测量得到机架棱镜的三维大地坐标，并将机架棱镜的三维大地坐标发送给工控机。
[0161]
工控机根据机架棱镜的三维大地坐标，以及悬臂式掘进机的参数、第一位移值、第二位移值，运算得到悬臂式掘进机的截割头的三维大地坐标，工控机再根据截割头的三维大地坐标，运算得到对应截割头的截割路径，以及悬臂式掘进机的导航修正参数；工控机根据截割路径，控制悬臂式掘进机进行挖掘，并根据导航修正参数，控制悬臂式掘进机进行自动导航推进。
[0162]
本发明的方法，使得悬臂式掘进机基于绝对大地坐标的导航定位，可动态修正和补偿悬臂式掘进机工作过程中机身姿态变化导致的误差。基于全自动陀螺全站仪毫米级可靠的高精度测量，无需利用点云、视觉图像的定位技术，基本不受掘进工作面粉尘、湿度影响，保证了悬臂式掘进机导航定位的持续高精度。工作过程中，工控机实时计算截割头的绝对大地坐标和在截割断面坐标系下的相对坐标，保证了绝对和相对的统一。另外，对悬臂式掘进机初始化状态没有严格限制和要求，可以基于机身棱镜坐标和机身惯导的欧拉角动态规划截割头的截割路径。而采用全自动陀螺全站仪，高精度寻北，后视只需一个导线点，无需使用后视两个控制点距离交会方法以及人工维护，不会导致大地坐标计算失效，全自动陀螺全站仪自动找平并跟随悬臂式掘进机机尾自动推进。
[0163]
尽管已描述了本发明实施例的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。
[0164]
最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
[0165]
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员
在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。