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

权利要求书

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

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

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

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

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

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

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

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

而姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，Y_b轴与O_nX_nY_n水平面的夹角，用θ表示；当Y_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轴与掘进机坐标系各轴之间的夹角。

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

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

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

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

d＝l′cosγ (3-2)

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

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

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