一种基于超声波技术的掘进机导航定位方法、系统及装置与流程

本发明涉及掘进机自动化领域，特别是涉及一种基于超声波技术的掘进机导航定位方法、系统及装置。

背景技术：

1、掘进工作面空间狭窄、环境恶劣，危险性高，人员密集，工人劳动强度大，职业健康威胁很高，因此，掘进工作面少人化一直是业界追求的目标。掘进作业关键工序是断面截割，其中关键指标是断面轮廓边界的控制精度，通常由固定在巷道后方的经过精密调整的激光指向仪向断面投射光斑，工人以此光斑为基准判断断面边界并控制掘进机进行截割。这种方法主要是将激光指示仪悬挂于巷道后方顶部，在激光传播过程中容易被障碍物遮挡，需要随着进尺的增加而进行悬挂位置的移动。现有导航方法的缺点是随着掘进机前进，二者之间的距离增加，导致导航系统检测分辨率和精度下降，此外，必须保持二者之间无遮挡，在布置了掘进机、除尘系统、通风系统、运输皮带等大型装备的巷道中，这一条件经常难以满足。

2、近年来，机器视觉技术在掘进机导航技术研究中成为热点，现有掘进机视觉导航系统一般由巷道顶板悬挂的视觉模块和激光发射器模块组成，激光发射器将激光束照射到掘进机机尾感光板上，机器视觉采集激光束在感光板上的排列图像，然后通过图像处理、位姿计算算法得到掘进机的空间位置信息。但是这种方法并没有解决激光传播途径的易遮挡问题，且视觉方法易受环境中光线、粉尘等因素的影响。

技术实现思路

1、本发明的目的是提供一种基于超声波技术的掘进机导航定位方法、系统及装置，可通过超声波测距并结合掘进机的位姿参数确定掘进机在大地坐标系下的坐标，提高掘进机导航定位的适应性和精确度。

2、为实现上述目的，本发明提供了如下方案：

3、一种基于超声波技术的掘进机导航定位方法，所述方法包括：

4、建立大地坐标系和所述掘进机的载体坐标系；

5、获取四个超声波接收模块在载体坐标系下的坐标和所述掘进机的位姿参数；

6、比较位于巷道顶板依次等间距设置的多个超声波发射模块中每个超声波发射模块与四个所述超声波接收模块的距离之和，将距离之和最短的所述超声波发射模块确定为目标超声波发射模块；

7、基于所述目标超声波发射模块与四个所述超声波接收模块的距离以及四个所述超声波接收模块在载体坐标系下的坐标确定所述目标超声波发射模块在载体坐标系下的坐标；

8、基于所述掘进机的位姿参数和所述目标超声波发射模块在载体坐标系下的坐标，确定所述掘进机在大地坐标系下的坐标。

9、可选的，建立载体坐标系，具体包括：

10、以所述掘进机的机身上表面中心为坐标原点om，以掘进机的机身纵轴为ym轴，以ym轴顺时针旋转90°为xm，以垂直xm om ym平面向上为zm轴构建载体坐标系。

11、可选的，建立大地坐标系，具体包括：

12、以地球球心为坐标原点on，以大地正东为xn轴，以大地正北为yn轴，zn轴垂直地面指向天向构建大地坐标系。

13、可选的，基于所述目标超声波发射模块与四个所述超声波接收模块的距离以及四个所述超声波接收模块在载体坐标系下的坐标确定所述目标超声波发射模块在载体坐标系下的坐标，具体包括：

14、设所述目标超声波发射模块在载体坐标系下的位置坐标为w(x,y,z)；

15、基于目标超声波发射模块与四个所述超声波接收模块的距离s1、s2、s3、s4以及四个所述超声波接收模块在载体坐标系下的坐标确定所述目标超声波发射模块在载体坐标系下的坐标k1(x1,y1,0)、k2(x2,y2,0)、k3(x3,y3,0)、k4(x4,y4,0)，建立距离方程组如下：

16、

17、利用最小二乘估计法将距离方程组转换为矩阵，得到最小二乘估计e如下：

18、

19、其中，

20、将所述目标超声波发射模块在载体坐标系下的位置坐标中在x轴方向和y轴方向的坐标代入距离方程组中任一方程，计算得到z轴方向的坐标，进而确定所述目标超声波发射模块在载体坐标系下的坐标。

21、可选的，基于所述掘进机的位姿参数和所述目标超声波发射模块在载体坐标系下的坐标，确定所述掘进机在大地坐标系下的坐标，具体包括：

22、基于所述掘进机在载体坐标系下的位姿参数以逆时针旋转为正，构建在大地坐标系下的位姿参数的旋转矩阵，所述位姿参数的旋转矩阵具体包括：横滚角的旋转矩阵、俯仰角的旋转矩阵和航向角的旋转矩阵；

23、所述航向角的旋转矩阵为：

24、所述横滚角的旋转矩阵为：

25、所述俯仰角的旋转矩阵为：

26、基于所述横滚角的旋转矩阵、所述俯仰角的旋转矩阵、所述航向角的旋转矩阵计算得到所述掘进机在大地坐标系的t时刻位置坐标为：

27、

28、其中，jt-1(xt-1,yt-1,zt-1)为所述掘进机在载体坐标系的t-1时刻实测位置坐标，jt(xt,yt,zt)为所述掘进机在大地坐标系的t时刻位置坐标，t为在机载坐标系下t-1到t时刻所述掘进机位移变化量；

29、t＝wt(xt,yt,zt)-wt-1(xt-1,yt-1,zt-1)；

30、wt-1(xt-1,yt-1,zt-1)为t-1时刻超声波发射模块在机载坐标系下的位置，wt(xt,yt,zt)为t时刻超声波发射模块在机载坐标系下的位置。

31、一种基于超声波技术的掘进机导航定位系统，所述系统包括：

32、坐标系建立模块，用于建立大地坐标系和所述掘进机的载体坐标系；

33、获取模块，用于获取四个超声波接收模块在载体坐标系下的坐标和所述掘进机的位姿参数；

34、比较模块，用于比较位于巷道顶板依次等间距设置的多个超声波发射模块中每个超声波发射模块与四个所述超声波接收模块的距离之和，将距离之和最短的所述超声波发射模块确定为目标超声波发射模块；

35、载体坐标计算模块，用于基于所述目标超声波发射模块与四个所述超声波接收模块的距离以及四个所述超声波接收模块在载体坐标系下的坐标确定所述目标超声波发射模块在载体坐标系下的坐标；

36、大地坐标计算模块，用于基于所述掘进机的位姿参数和所述目标超声波发射模块在载体坐标系下的坐标，确定所述掘进机在大地坐标系下的坐标。

37、一种基于超声波技术的掘进机导航定位装置，所述装置包括：所述装置包括：多个超声波发射模块、四个超声波接收模块、信号收发模块、惯性导航和控制器；

38、多个所述超声波发射模块和所述信号收发模块设置于巷道顶板上，多个所述超声波发射模块依次等间距设置；四个所述超声波接收模块分别设置于掘进机顶部四角；多个所述超声波发射模块和四个所述超声波接收模块均与所述信号收发模块无线连接，所述信号收发模块与所述控制器连接；

39、所述惯性导航装置设置于所述掘进机顶部中心，所述惯性导航装置与所述信号收发模块无线连接；所述控制器应用于权利要求1-5任一项所述的方法计算掘进机在大地坐标系下的坐标。

40、可选的，所述控制器为计算机。

41、一种电子设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如以上所述的方法。

42、一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被执行时实现如以上所述的方法。

43、根据本发明提供的具体实施例，本发明公开了以下技术效果：

44、本发明通过超声波测距确定目标超声波发射模块，并基于以超声波发射模块与掘进机的距离和四个超声波接收模块在载体坐标系下的坐标确定超声波发射模块在载体坐标系下的坐标，然后结合掘进机的位姿参数，确定掘进机在大地坐标系下的坐标。本发明克服了机器视觉、激光雷达等传感器环境适应性差、传播易被遮挡的问题，提高了掘进机导航定位的适应性和精确度。