CN112902952B - 基于毫米波雷达和惯性导航的掘进机位姿检测方法 - Google Patents

Abstract

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

Description

基于毫米波雷达和惯性导航的掘进机位姿检测方法

技术领域

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

背景技术

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

公号为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轴方向为垂直于水平面向上；而姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，X_b轴与O_nX_nY_n水平面的夹角，用θ表示；当X_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与O_nY_nZ_n平面的夹角，用γ表示，以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用表示。以载体右偏为正，反之为负，旋转范围是0°～360°。

S303～将三轴加速度作为一个向量，它的三个方向余弦分别是该向量与三个坐标轴之间的转换矩阵，分别用xi、yj、zk、表示载体坐标系中的三轴加速度，x’i’、y’j’、z’k’表示地理坐标系中的三轴加速度，因加速度向量A是唯一的，所以满足式(1)：

A＝xi+yj+zk＝x'i'+y'j'+z'k' (1)

用矩阵的形式表示式(1)：

变换矩阵得到式(3)：

两单位向量的乘机等于他们的夹角的余弦值，即：

式中，α_m,m＝1,2,3地理坐标系x轴与掘进机坐标系各轴之间的夹角；β_n,n＝1,2,3表示地理坐标系y轴与载体坐标系各轴之间的夹角；φ_p,p＝1,2,3地理坐标系z轴与掘进机坐标系各轴之间的夹角。

步骤S400以下步骤，设定s为掘进机没有姿态变化时侧身与煤壁侧帮之间的垂直距离；l为掘进机机身发生角度的偏航时侧身与煤壁侧帮探测点之间的测量距离，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，d为掘进机侧身与煤壁侧帮之间的垂直距离。

当掘进机发生γ角度的翻滚时，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

d＝l′cosγ (3-2)

式中，l′为掘进机机身发生γ角度的翻滚时侧身与煤壁侧帮探测点之间的测量距离。

当掘进机机身发生角度的偏航同时发生了γ角度的翻滚，掘进机侧身与煤壁侧帮探测点之间的测量距离为L，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，D掘进机侧身与煤壁侧帮之间的垂直距离。

与现有技术相比，本发明提出以惯性导航检测掘进机航向、翻滚和俯仰方向的角度，利用毫米波雷达测量掘进机侧身与煤壁侧帮的距离。对惯性导航采集的掘进机航向、翻滚和俯仰方向的角度结合毫米波雷达测量的掘进机侧身与煤壁侧帮的距离进行数据融合，从而得到掘进机侧身距离煤壁侧帮的垂直距离，实现掘进机的导航定位。本发明具有不易受周围环境干扰、稳定性好、精度高等优点，为井下设备的自主导航提供帮助。

附图说明

图1为掘进机位姿检测示意图；

图2为载体坐标系示意图；

图3为俯仰角、翻滚角和偏航角示意图；

图4为掘进机位置求解示意图；

图中1-惯性导航，2-掘进机，3-毫米波雷达，4-煤壁侧帮，5-数据处理模块，6-上一时刻掘进机侧身，7-下一时刻掘进机侧身。

具体实施方式

一种基于毫米波雷达和惯性导航的掘进机位姿检测方法，包括以下步骤。

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轴方向为垂直于水平面向上；而姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，X_b轴与O_nX_nY_n水平面的夹角，用θ表示；当X_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与O_nY_nZ_n平面的夹角，用γ表示，以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用表示。以载体右偏为正，反之为负，旋转范围是0°～360°。

S303～将三轴加速度作为一个向量，它的三个方向余弦分别是该向量与三个坐标轴之间的转换矩阵，分别用xi、yj、zk、表示载体坐标系中的三轴加速度，x’i’、y’j’、z’k’表示地理坐标系中的三轴加速度，因加速度向量A是唯一的，所以满足式(1)：

A＝xi+yj+zk＝x'i'+y'j'+z'k' (1)

用矩阵的形式表示式(1)：

变换矩阵得到式(2)：

两单位向量的乘机等于他们的夹角的余弦值，即：

式中，α_m,m＝1,2,3地理坐标系x轴与掘进机坐标系各轴之间的夹角；β_n,n＝1,2,3表示地理坐标系y轴与掘进机坐标系各轴之间的夹角；φ_p,p＝1,2,3地理坐标系z轴与掘进机坐标系各轴之间的夹角。

步骤S400以下步骤，设定s为掘进机没有姿态变化时侧身与煤壁侧帮之间的距离；l为掘进机机身发生角度的偏航时侧身与煤壁侧帮之间的测量距离，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，d为掘进机侧身与煤壁侧帮之间的垂直距离。

当掘进机发生γ角度的翻滚时，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

d＝l′cosγ (3-2)

式中，l′为掘进机机身发生γ角度的翻滚时侧身与煤壁侧帮之间的测量距离。

当掘进机机身发生角度的偏航同时发生了γ角度的翻滚时，掘进机侧身与煤壁侧帮之间的测量距离为L，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，D掘进机侧身与煤壁侧帮之间的垂直距离。

Claims (2)

1.一种基于毫米波雷达和惯性导航的掘进机位姿检测方法，其特征在于：包括以下步骤，

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

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

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

步骤S300采取以下步骤，

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

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

而姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，X_b轴与O_nX_nY_n水平面的夹角，用θ表示；当X_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°；

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与O_nY_nZ_n平面的夹角，用γ表示，以载体向右倾斜为正，反之为负，旋转范围是-180°～180°；

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用表示，以载体右偏为正，反之为负，旋转范围是0°～360°；

S303～将三轴加速度作为一个向量，它的三个方向余弦分别是该向量与三个坐标轴之间的转换矩阵，分别用xi、yj、zk、表示载体坐标系中的三轴加速度，x’i’、y’j’、z’k’表示地理坐标系中的三轴加速度，因加速度向量A是唯一的，所以满足式(1)：

A＝xi+yj+zk＝x'i'+y'j'+z'k' (1)

用矩阵的形式表示式(1)：

变换矩阵得到式(2)：

两单位向量的乘机等于他们的夹角的余弦值，即：

式中，α_m,m＝1,2,3地理坐标系x轴与掘进机坐标系各轴之间的夹角；β_n,n＝1,2,3表示地理坐标系y轴与掘进机坐标系各轴之间的夹角；φ_p,p＝1,2,3地理坐标系z轴与掘进机坐标系各轴之间的夹角；

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

2.根据权利要求1所述的基于毫米波雷达和惯性导航的掘进机位姿检测方法，其特征在于：所述的步骤S400以下步骤，

设定s为掘进机没有姿态变化时侧身与煤壁侧帮之间的距离；l为掘进机机身发生角度的偏航时侧身与煤壁侧帮之间的测量距离，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，d为掘进机侧身与煤壁侧帮之间的垂直距离；

当掘进机发生γ角度的翻滚时，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

d＝l′cosγ (3-2)

式中，l′为掘进机机身发生γ角度的翻滚时侧身与煤壁侧帮之间的测量距离；

当掘进机机身发生角度的偏航同时发生了γ角度的翻滚时，掘进机侧身与煤壁侧帮之间的测量距离为L，可以计算此时掘进机侧身与煤壁侧帮之间的垂直距离为：

式中，D掘进机侧身与煤壁侧帮之间的垂直距离。