[发明专利]基于惯导和激光雷达单点测距的掘进机定位方法及系统

权利要求书

1.基于惯导和激光雷达单点测距的掘进机定位方法，其特征在于，包括以下步骤：

(1)利用激光雷达对参照面进行扫描，扫描到参照面上的单一控制点，得到激光雷达到单一控制点在激光雷达坐标系中的距离；

(2)利用激光雷达与掘进机之间的安装误差阵，将该距离转换为在载体坐标系中的距离，即得到激光雷达到单一控制点在载体坐标系中的距离；

(3)利用惯导提供的姿态矩阵，将激光雷达到单一控制点在载体坐标系中的距离转换为激光雷达到单一控制点在导航坐标系中的距离；

(4)根据已知的参照面中单一控制点的坐标和激光雷达到单一控制点在导航坐标系中的距离，得到掘进机在导航坐标系中的位置坐标，掘进机在导航坐标系中的位置坐标具体通过如下公式表达：

式中，为激光雷达在导航坐标系中的位置，Rⁿ为求得的激光雷达到控制点O之间的距离，O为参照面中的单一控制点，其中，Rⁿ通过下式求得：

式中，表示激光雷达坐标系到载体坐标系的旋转矩阵，表示惯导提供的姿态矩阵，表示激光雷达到参照面中的单一控制点O的距离；

(5)惯导通过其内部设置的陀螺仪和加速度计提供掘进机的加速度和角速度信息，从而解算出掘进机的在导航坐标系的位置信息；

(6)根据卡尔曼滤波器的原理，完成惯导和激光雷达数据的融合，实现掘进机的高精度定位定向。

2.根据权利要求1所述的基于惯导和激光雷达单点测距的掘进机定位方法，其特征在于，通过如下公式建立惯导和激光雷达定位的状态模型：

式中，X(t)为惯导和激光雷达定位的状态模型的状态向量，W(t)为惯导和激光雷达定位的状态模型的白噪声，G(t)为惯导和激光雷达定位的状态模型的系统噪声，F(t)为惯导和激光雷达定位的状态模型的系统转移矩阵，其中，X(t)通过如下公式表达：

式中：为捷联惯导平台失准角；δVⁿ为捷联惯导速度误差；δPⁿ为捷联惯导位置误差；ε为陀螺常值漂移；为加速度计常值零偏。

3.根据权利要求1至2中任一项所述的基于惯导和激光雷达单点测距的掘进机定位方法，其特征在于，所述步骤(6)具体表现为：基于惯导的误差方程建立系统方程，将惯导和激光雷达得到的位置作差，并将该值作为量测值输入卡尔曼滤波器，建立量测方程，从而完成了惯导和激光雷达数据的融合，实现了掘进机的高精度定位定向。

4.根据权利要求3所述的基于惯导和激光雷达单点测距的掘进机定位方法，其特征在于，量测方程的建立和量测值的获取分别通过如下公式表达，其中：

卡尔曼滤波器的量测方程如下式：

Z＝HX+V (5)

式中，Z表示量测值，X表示惯导和激光雷达组合的导航系统的状态量，V表示惯导和激光雷达组合的导航系统的噪声，H为量测矩阵，其表达式如下：

H＝[0_3×3 0_3×3 I_3×3 0_3×3 0_3×3] (6)

量测值Z的表达方程如下式：

式中，Pⁿ表示通过惯导解算出的掘进机在导航坐标系的位置信息，表示通过激光雷达得到掘进机在导航坐标系中的位置坐标。

5.根据权利要求1所述的基于惯导和激光雷达单点测距的掘进机定位方法，其特征在于，所述步骤(1)之间还包括如下步骤：

(0)启动掘进机，惯导随着掘进机行驶一段距离。