[发明专利]基于毫米波雷达和惯性导航的掘进机位姿检测方法

说明书

本发明属于掘进机导航定位技术领域，具体是一种基于毫米波雷达和惯性导航的掘进机位姿检测方法。包括以下步骤。S100～掘进机机身后端侧面安装的毫米波雷达，实时测量掘进机后端侧面与煤壁侧帮雷达探测点之间的距离，S200～掘进机机身前端侧面安装的毫米波雷达，实时测量掘进机前端侧面与煤壁侧帮雷达探测点之间的距离，S300～惯性导航固定在掘进机机身顶部，实时测量掘进机在航向、翻滚和俯仰三个方向的偏转角度，S400～当掘进机没有姿态变化时，毫米波雷达测量距离为机身侧面与煤壁侧帮之间的垂直距离；当机身发生姿态变化时，毫米波雷达测量的距离为机身侧面与煤壁侧帮的斜距，利用数据融合方法处理得到掘进机侧身与煤壁侧帮之间的垂直距离。

技术领域

本发明属于掘进机导航定位技术领域，具体是一种基于毫米波雷达和惯性导航的掘进机位姿检测方法。

背景技术

我国矿井下掘进工作面由于巷道断面有限，空间狭小，有大量的粉尘、有害气体，且温度、湿度较高，环境较为恶劣，粉尘会危险职工的身体健康。为了减少工作人员，推进井下自动化的发展，掘进机的位姿检测变得至关重要，大量的科研人员致力于掘进机导航定位的研究。

公号为CN104295297A的发明专利“一种掘进机截割头定位系统及方法和掘进机”，该专利主要利用超声波传感器、惯性测量单元和全站仪进行掘进机机身位置信息的测量，再根据掘进机机身相对于巷道的位置、截割臂相对于机身、截割头相对于截割臂之间的位置关系，计算出截割头相对于巷道的位置信息。该方法提供了一种掘进机截割头的位姿参数检测方法，技术可行，但该方法结构复杂，且全站仪配套棱镜易受到粉尘污染。

公号为CN102419433A的发明专利“一种掘进机姿态定位系统”，该专利应用GPS定位原理，采用无线信号进行掘进机位置检测。该方法对环境的适应性较强，为掘进机位置检测提供了方向，但煤矿井下掘进工作面环境复杂，会影响无线信号对掘进机位置的计算精度。

发明内容

本发明为了解决上述问题，提供一种基于毫米波雷达和惯性导航的掘进机位姿检测方法。

本发明采取以下技术方案：一种基于毫米波雷达和惯性导航的掘进机位姿检测方法，包括以下步骤。

S100～掘进机机身后端侧面安装的毫米波雷达，实时测量掘进机后端侧面与煤壁侧帮雷达探测点之间的距离，并将测量结果传输到数据处理模块。

S200～掘进机机身前端侧面安装的毫米波雷达，实时测量掘进机前端侧面与煤壁侧帮雷达探测点之间的距离，并将测量结果传输到数据处理模块。

S300～惯性导航固定在掘进机机身顶部，实时测量掘进机在航向、翻滚和俯仰三个方向的偏转角度，并将惯性导航采集的航向、翻滚方向和俯仰方向的角度转换为掘进机的姿态角。

S400～当掘进机没有姿态变化时，毫米波雷达测量距离为机身侧面与煤壁侧帮之间的垂直距离；当机身发生姿态变化时，毫米波雷达测量的距离为机身侧面与煤壁侧帮的斜距，利用数据融合方法处理得到掘进机侧身与煤壁侧帮之间的垂直距离。

步骤S300采取以下步骤：

S301～建立载体坐标系，载体坐标系的原点为掘进机的质心O_b，则X_b轴方向为掘进机横轴正右方，Y_b轴方向为掘进机纵轴正前方，Z_b轴方向正交X_b轴、Y_b轴并竖直向上。

S302～将地理坐标系下的姿态角转换为掘进机的姿态角，其中地理坐标系用n表示，地理坐标系的原点一般为载体的质心O_n，取“东北天”三个方向作为坐标轴方向，即X_n轴方向为水平向东，Y_n轴方向为北极方向，Z_n轴方向为垂直于水平面向上；而姿态角的定义：