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

技术特征：
1.一种悬臂式掘进机自动导航和定位截割的方法，其特征在于，所述方法包括：全自动陀螺全站仪自动找平和寻北，后视搜索并获取控制点棱镜的三维大地坐标；所述全自动陀螺全站仪根据所述控制点棱镜的三维大地坐标，和所述全自动陀螺全站仪到所述控制点棱镜的倾角、斜距，计算得到所述全自动陀螺全站仪的三维大地坐标，其中，所述全自动陀螺全站仪的三维大地坐标为所述全自动陀螺全站仪设站点的三维大地坐标；所述全自动陀螺全站仪根据自身的三维大地坐标，以及掘进巷道设计方位和倾角，前视搜索并跟踪机架棱镜，实时测量得到所述机架棱镜的三维大地坐标，并将所述机架棱镜的三维大地坐标发送给工控机；所述工控机根据所述机架棱镜的三维大地坐标，以及所述悬臂式掘进机的参数、第一位移值、第二位移值，运算得到所述悬臂式掘进机的截割头的三维大地坐标，所述第一位移值为回转油缸位移传感器测量得到的值，所述第二位移值为升降油缸位移传感器测量得到的值；所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式掘进机的位置和航向进行动态纠偏。2.根据权利要求1所述的方法，其特征在于，全自动陀螺全站仪自动找平和寻北，后视搜索并获取控制点棱镜的三维大地坐标，包括：所述全自动陀螺全站仪自动整平和寻北；所述全自动陀螺全站仪完成所述整平和所述寻北后，识别控制点点号，所述控制点点号对应所述控制点棱镜的三维大地坐标；所述全自动陀螺全站仪基于所述控制点点号，获取所述控制点棱镜的三维大地坐标；其中，所述控制点棱镜安装于掘进巷道导线控制点上。3.根据权利要求1所述的方法，其特征在于，所述工控机根据所述机架棱镜的三维大地坐标，以及所述悬臂式掘进机的参数、第一位移值、第二位移值，计算得到所述悬臂式掘进机的截割头的三维大地坐标，包括：所述工控机根据所述第一位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的水平回转角度；所述工控机根据所述第二位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的垂直升降角度；所述工控机根据所述水平回转角度、所述垂直升降角度、所述悬臂式掘进机的参数、所述机架棱镜的三维大地坐标，运算得到所述截割头的三维大地坐标。4.根据权利要求3所述的方法，其特征在于，所述工控机根据所述第一位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的水平回转角度，包括：所述悬臂式掘进机的参数包括：定义回转油缸的零位移长度值l
h0
、回转油缸的水平转动中心点o、回转油缸与机架的铰接点p、回转油缸与回转臂的铰接点q、在同一直线上悬臂式掘进机的机架纵向中轴线为aob、截割臂的纵向中轴线ot、铰接点p至水平转动中心点o的固定长度值l
po
、铰接点q至水平转动中心点o的固定长度值l
qo
、固定角度值：∠qot、∠aop；
则所述工控机根据所述第一位移值、所述回转油缸的零位移长度值l
h0
，运算得到铰接点p至铰接点q的长度值l
pq
＝l
h0
s
h
，其中，s
h
即所述第一位移值；因三角形poq的三边长度已知，所述工控机根据所述铰接点p至水平转动中心点o的固定长度值l
po
、所述铰接点q至水平转动中心点o的固定长度值l
qo
、所述铰接点p至铰接点q的长度值l
pq
，并结合余弦定理得到：则有：∠boq＝180-∠poq-∠aop，进而得到所述悬臂式掘进机的水平回转角度a
h
＝∠boq-∠qot，a
h
即∠bot。5.根据权利要求4所述的方法，其特征在于，所述工控机根据所述第二位移值、所述悬臂式掘进机的参数，运算得到所述悬臂式掘进机的垂直升降角度，包括：所述悬臂式掘进机的参数包括：定义升降油缸的零位移长度值l
v0
、升降油缸的垂直转动中心点o、升降油缸与机架的铰接点m、升降油缸与截割臂的铰接点n、固定角度值：∠mop、∠ony、铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、截割臂on水平时固定角度值∠mop；则所述工控机根据所述第二位移值、所述升降油缸的零位移长度值l
v0
，运算得到铰接点m至铰接点n的长度值l
mn
＝l
v0
s
v
，其中，s
v
即所述第二位移值；因三角形mon的三边长度已知，所述工控机根据铰接点m至垂直转动中心点o的固定长度值l
mo
、铰接点n至垂直转动中心点o的固定长度值l
no
、所述铰接点m至铰接点n的长度值l
mn
，并结合余弦定理得到：则得到所述悬臂式掘进机的垂直升降角度a
v
＝∠mon-∠mop，a
v
即∠pon。6.根据权利要求5所述的方法，其特征在于，所述工控机根据所述水平回转角度、所述垂直升降角度、所述悬臂式掘进机的参数、所述机架棱镜的三维大地坐标，运算得到所述截割头的三维大地坐标，包括：所述悬臂式掘进机的参数包括：定义所述悬臂式掘进机的机身坐标系为以所述回转油缸的水平转动中心点o、所述回转油缸与机架的铰接点p1和p2、以及所述回转油缸与所述回转臂的铰接点q1和q2，共五点构成的平面为xoy坐标系平面，以所述回转油缸的水平转动中心点o为坐标原点，p1和p2的中点为a，ao直线为x轴，垂直于ao的直线为y轴，构成的右手坐标系；定义截割头最远端点为t，ny为平行于截割臂的中心轴线，y点为t在ny直线上的垂足，则点t即为需要计算的截割头的位置，y点至t点的固定长度值lyt、y点至n点的固定长度值lny；而升降中心o点在机身坐标系下的坐标可以从所述悬臂式掘进机结构几何参数获取：(o
x
，o
y
，oz
)
；则所述工控机运算得到所述截割臂在xoy平面上投影长度：l
ot
＝l
no
*cosa
v
l
ny
*cos(∠ony a
v-180) l
yt
*sin(∠ony a
v-180)
则所述截割头在机身坐标系下的坐标为：x’＝o
x
l
ot
*cosa
h
；y’＝o
y
l
ot
*sina
h
；z’＝o
z
l
no
*sina
v
l
ny
*sin(∠ony a
v-180)-l
yt
*cos(∠ony a
v-180)；工控机接收惯导实时测量的机架欧拉角，所述欧拉角包括：航向角y、俯仰角p、翻滚角r；则所述工控机根据所述欧拉角，将所述截割头在机身坐标系下的坐标，转换为三维大地坐标系，转换使用的旋转矩阵如下：其中：其中：其中：其中，表示机身坐标系向大地坐标系转换的旋转矩阵；t
横
表示机身坐标系向大地坐标系转换时横滚方向的旋转矩阵；t
俯
表示机身坐标系向大地坐标系转换时俯仰方向的旋转矩阵；t
航
表示机身坐标系向大地坐标系转换时航向方向的旋转矩阵；若定义所述机身棱镜目标点为a，其在机身坐标系下坐标为(x’a
，y’a
，z’a
)，其在三维大地坐标系下由所述全自动陀螺全站仪跟踪测量得到的坐标为(x
a
，y
a
，z
a
)，则所述机身棱镜从机身坐标向三维大地坐标转换的平移向量为：则根据所述截割头最远端点t的机身坐标(x’，y’，z’)，运算得到所述截割头的三维大地坐标矩阵表达式为：根据上述方法即运算得到所述截割头的三维大地坐标。
7.根据权利要求1所述的采集系统，其特征在于，所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式位置和航向进行动态纠偏。包括：所述工控机确定基于截割断面坐标系的定位截割初始化位置；所述工控机根据所述截割头的三维大地坐标、所述定位截割初始化位置，结合所述欧拉角、所述截割头的截割断面坐标系，计算得到对应所述截割头的截割路径，以及所述悬臂式掘进机的导航修正参数，所述截割路径使得所述悬臂式掘进机按设定参数和工艺流程定位截割，所述导航修正参数用于对所述悬臂式位置和航向进行动态纠偏；所述工控机基于所述悬臂式掘进机的截割头的三维大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制所述悬臂式掘进机的升降油缸、回转油缸动作，使所述悬臂式掘进机按设定参数和工艺流程定位截割，并对所述悬臂式位置和航向进行动态纠偏。8.根据权利要求7所述的方法，其特征在于，所述截割头的截割断面坐标系定义如下：以矩形断面为例：基于所述截割头的坐标原点位置定义所述截割断面坐标系：矩形断面的高度h米，宽度w米，以经过所述截割头的坐标原点垂直于巷道中心线的纵断面为坐标平面，以平行于断面宽度w方向且经过原点的0的直线为x轴，以平行于断面高度h方向且经过原点的0的直线为y轴；所述截割头的坐标原点位置定义：以所述截割臂的回转角度为0度，所述截割臂的中心轴线方位角为巷道设计方位角。9.一种悬臂式掘进机自动导航和定位截割的系统，其特征在于，所述系统采用如权利要求1-8任一所述的方法实现悬臂式掘进机自动导航和定位截割的方法，所述系统包括：工控机、全自动陀螺全站仪、惯导、机架棱镜、控制点棱镜、升降油缸位移传感器以及回转油缸位移传感器；所述工控机与所述全自动陀螺全站仪交互，所述工控机运算得到所述悬臂式掘进机的截割头的三维大地坐标，以及运算得到对应所述截割头的截割路径，以及悬臂式掘进机的导航修正参数，并发送给所述全自动陀螺全站仪，所述工控机为安装有操作系统的工业计算机，所述工控机中部署有基于三维大地坐标的悬臂式掘进机自动导航的控制程序，和定位截割系统的控制程序；所述全自动陀螺全站仪与所述惯导、所述机架棱镜、所述控制点棱镜共体工作，得到所述机架棱镜的三维大地坐标并发送至所述工控机，所述全自动陀螺全站仪是一种集自动寻北、自动整平、自动目标识别、自动照准、自动测角测距、自动目标跟踪、自动计算、自动存储于一体的测量平台，所述全自动陀螺全站仪具有减震功能，防止掘进过程和整体推进过程产生的震动破坏和影响所述全自动陀螺全站仪；所述惯导与所述全自动陀螺全站仪交互，用于实时测量所述悬臂式掘进机机架的欧拉角，所述惯导刚性安装于所述悬臂式掘进机机架，跟随所述悬臂式掘进机一起运动，所述惯导工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航系统；所述机架棱镜与所述全自动陀螺全站仪交互，所述机架棱镜刚性安装于所述悬臂式掘
进机朝向所述全自动陀螺全站仪一侧的机架上，所述机架棱镜跟随所述悬臂式掘进机一起运动，所述悬臂式掘进机、所述惯导、所述机架棱镜三者刚性连接，整体运动；所述控制点棱镜与所述全自动陀螺全站仪交互，所述控制点棱镜为所述全自动陀螺全站仪测量的光学目标装置，所述控制点棱镜安装于掘进巷道顶板导线控制点上；所述升降油缸位移传感器与所述工控机交互，用于测量升降油缸的位移值；所述回转油缸位移传感器与所述工控机交互，用于测量回转油缸的位移值。10.根据权利要求9所述的系统，其特征在于，所述全自动陀螺全站仪固定安装于机尾巷道或集控舱顶部，所述全自动陀螺全站仪采用固定安装，或采用非固定安装；所述固定安装指固定一次安装，防护罩可以开闭；所述非固定安装指通过液压或者电机驱动，使得全自动陀螺全站仪工作时伸到合适位置，不工作时收缩回去并密封装箱到防护罩里。

技术总结
本发明提供一种悬臂式掘进机自动导航和定位截割的方法和系统，涉及煤矿智能开采技术领域，包括：设置掘进巷道设计方位、倾角、断面参数，全自动陀螺全站仪自动找平、寻北，后视搜索巷道导线点棱镜，计算设站点大地坐标，前视搜索并跟踪测量机架棱镜大地坐标，基于升降和回转油缸位移值及掘进机物理尺寸实时计算炮头大地坐标，结合掘进巷道设计方位、倾角、断面参数和截割工艺控制掘进机的升降和回转油缸动作，使掘进机按设定参数和工艺流程定位截割，并能对位置和航向动态纠偏。本发明实时计算截割头的绝对大地坐标和截割断面坐标系下的相对坐标，保证了绝对和相对坐标的统一，可动态修正和补偿悬臂式掘进机工作过程中机身姿态变化导致的误差。姿态变化导致的误差。姿态变化导致的误差。


技术研发人员：毛善君 李鑫超 陈华州 宋春久 朱义宏
受保护的技术使用者：北京龙软科技股份有限公司
技术研发日：2022.03.09
技术公布日：2022/7/12