CN111637888B - 基于惯导和激光雷达单点测距的掘进机定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于惯导和激光雷达单点测距的掘进机定位方法，首先利用激光雷达对掘进参照面进行扫描，参照面上存在一个控制点，该控制点的坐标已知，求得该控制点到激光雷达的距离，再运用掘进机、激光雷达之间的安装误差阵和惯性导航系统提供的姿态矩阵将该距离转换到导航坐标系，从而可得到掘进机的位置，又将该值与惯导提供的位置信息运用卡尔曼滤波器进行融合，得到高精度的位置信息。该定位方法能保证掘进机长时间高精度定位定向要求，具有被动测量、自成体系、不易受干扰且定位精准的优点。同时，本发明还公开了一种基于惯导和激光雷达单点测距的掘进机定位系统。

Description

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

技术领域

本发明涉及掘进机的定位技术领域，特别是涉及一种基于惯导和激光雷达单点测距的掘进机定位方法及系统。

背景技术

掘进机的精确定位定向系统是巷道、隧道等掘进的关键问题，定位定向系统直接影响着掘进机工作的效率和质量。掘进机等矿用工程机械工作环境粉尘大、噪声强烈、照明条件差，严重影响了操作手的身心健康与工作效率，为了减少掘进机操作人员的工作量和提高巷道掘进工人的安全性，掘进机逐渐向自动化转化。

掘进机的自主定位定向系统是实现掘进机自动化的关键技术，现阶段相关技术主要有基于激光定位的掘进机自主定位技术、基于惯性导航系统的掘进机自主定位技术、基于惯导和视觉组合的掘进机自主定位技术和基于超宽带测量技术的掘进机自主定位技术，但上述技术均存在不足之处，其中：

(1)基于激光定位的掘进机自主定位技术的缺点：因为该方法是利用激光定位传感器根据激光靶接收的激光束生成位置信号，得到掘进机所在位置，这种方法只能得到掘进机的位置信息，而不能有效获取掘进机在巷道行进的方向和姿态。

(2)基于惯性导航系统的掘进机自主定位技术的缺点是：因为该方法是利用惯性导航系统来实时确定运载体的位置信息，但是惯性导航系统是利用惯性测量元件测量载体加速度、经过积分得到载体的姿态和位置信息，陀螺漂移等因素引起误差随时间积累，而掘进机通常需在地下工作一周甚至更长时间，因此将基于惯性测量技术的位置测量技术用于掘进机自主定位定向系统的误差随时间累积，定位精度随时间降低。

(3)基于惯导和视觉组合导航的掘进机自主定位技术利用SLAM视觉模块和I MU惯导模块的掘进机位置信息，结合位置数据库的导航定位参数，得到掘进机的位置信息。其中视觉模块是通过获取图像特征来得到目标位置，而在地下掘进环境，粉尘大，不利于图像特征的获取。

(4)基于超宽带测量技术的位置测量方法的缺点是：因为该方法是利用超宽带信号进行距离测量的无线电测距技术，超宽带信号在空气中传播时容易受到周围环境的影响，而掘进机的工作环境存放着大量的金属设备，电磁环境复杂，大幅影响了超宽带信号的正常传播，造成系统的定位信息与真实的位置信息误差较大。

发明内容

有鉴于此，本发明提出一种基于惯导和激光雷达单点测距的掘进机定位方法及系统，利用激光雷达单点测距技术得到激光雷达所在位置，然后与惯导进行组合导航，对惯导的累积误差进行修正，得到高精度的位置信息，具有被动测量、自成体系、不易受干扰且定位精准的优点。

一方面，本发明提供了一种基于惯导和激光雷达单点测距的掘进机定位方法，包括以下步骤：

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

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

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

(4)根据已知的参照面中单一控制点的坐标和激光雷达到单一控制点在导航坐标系中的距离，得到掘进机在导航坐标系中的位置坐标；

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

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

进一步地，所述基于惯导和激光雷达单点测距的掘进机定位方法通过如下公式建立惯导和激光雷达定位的状态模型：

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

式中：

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

进一步地，设定参照面中的单一控制点为O，激光雷达到参照面中的单一控制点O的距离为

惯导提供的姿态矩阵为

激光雷达到单一控制点O在导航坐标系中的距离为Rⁿ，则：

式中，

表示激光雷达坐标系到载体系的旋转矩阵。

进一步地，所述步骤(4)中单一控制点O的坐标已知，设定求得的激光雷达到控制点O之间的距离Rⁿ，激光雷达在导航坐标系中的位置

则：

进一步地，所述步骤(6)具体表现为：基于惯导的误差方程建立系统方程，将惯导和激光雷达得到的位置作差，并将该值作为量测值输入卡尔曼滤波器，建立量测方程，从而完成了惯导和激光雷达数据的融合，实现了掘进机的高精度定位定向。

进一步地，量测方程的建立和量测值的获取分别通过如下公式表达，其中：

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

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ⁿ表示通过惯导解算出的掘进机在导航坐标系的位置信息，

表示通过激光雷达得到掘进机在导航坐标系中的位置坐标。

进一步地，所述步骤(1)之间还包括如下步骤：

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

通过其上描述，本发明首先利用激光雷达对掘进参照面进行扫描，参照面上存在一个控制点，该控制点的坐标已知，求得该控制点到激光雷达的距离，再运用掘进机、激光雷达之间的安装误差阵和惯性导航系统提供的姿态矩阵将该距离转换到导航坐标系，从而可得到掘进机的位置，又将该值与惯导提供的位置信息运用卡尔曼滤波器进行融合，得到高精度的位置信息。该定位方法能保证掘进机长时间高精度定位定向要求，具有被动测量、自成体系、不易受干扰且定位精准的优点。

另一方面，本发明还提供了一种基于惯导和激光雷达单点测距的掘进机定位系统，基于其上任一项所述的基于惯导和激光雷达单点测距的掘进机定位方法，所述掘进机定位系统包括掘进机、激光雷达、惯导和导航计算机，所述掘进机分别与激光雷达、惯导和导航计算机相搭载，所述激光雷达用于对参照面进行扫描，得到激光雷达到参照面中的单一控制点的距离，为后续提供激光雷达在导航坐标系中的位置提供输入，所述惯导用于得到掘进机的姿态矩阵，将激光雷达到控制点的距离转换到导航坐标系中，同时得到掘进机的位置信息，所述导航计算机用于对激光雷达和惯导的信息进行处理。

进一步地，所述导航计算机与惯导内设置的陀螺仪通过惯导接口相连，进行数据的双向传递；所述导航计算机与激光雷达通过激光雷达接口相连，进行数据的双向传递。

本发明提供的掘进机定位系统显然具有前述基于惯导和激光雷达单点测距的掘进机定位方法，定位定向精度精准、自成体系、不易受干扰的优点。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明一实施例提供的基于惯导和激光雷达单点测距的掘进机定位方法的流程图；

图2为本发明另一实施例提供的基于惯导和激光雷达单点测距的掘进机定位方法的流程图；

图3为激光雷达单点测距原理图；

图4为激光雷达单点测距和惯导数据融合的流程图；

图5为本发明实施例提供的基于惯导和激光雷达单点测距的掘进机定位系统的结构简图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

为更好地理解本发明，对如下名称做出解释：

惯导(INS，惯性导航系统)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

图1是本发明一实施例提供的基于惯导和激光雷达单点测距的掘进机定位方法的流程图。如图1所示，该实施例中基于惯导和激光雷达单点测距的掘进机定位方法，包括以下步骤：

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

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

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

(4)根据已知的参照面中单一控制点的坐标和激光雷达到单一控制点在导航坐标系中的距离，得到掘进机在导航坐标系中的位置坐标；

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

(6)根据卡尔曼滤波器的原理，完成惯导和激光雷达数据的融合，实现掘进机的高精度定位定向。需要说明的是，该步骤基于惯导的误差方程建立系统方程，将惯导和激光雷达得到的位置作差，并将该值作为量测值输入卡尔曼滤波器，建立量测方程，从而完成了惯导和激光雷达数据的融合，实现了掘进机的高精度定位定向，图4即为激光雷达单点测距和惯导数据融合的流程图，陀螺仪和加速度计构成图4中的惯性测量单元，用于提供速度增量和角度增量。

通过其上描述，本发明首先利用激光雷达对掘进参照面进行扫描，参照面上存在一个控制点，该控制点的坐标已知，求得该控制点到激光雷达的距离，再运用掘进机、激光雷达之间的安装误差阵和惯性导航系统提供的姿态矩阵将该距离转换到导航坐标系，从而可得到掘进机的位置，又将该值与惯导提供的位置信息运用卡尔曼滤波器进行融合，得到高精度的位置信息。该定位方法能保证掘进机长时间高精度定位定向要求，具体地，能保证掘进机在100m，东、北、天(需要说明的是，东、北、天是针对地理坐标系而言，其中东表示x轴向，北表示y轴向，天表示z轴向，俗称东北天坐标系，本发明中导航坐标系即选用地理坐标系)三个方向的位置误差在10cm以内，即掘进机的定位精度在0.1D％以内，且具有被动测量、自成体系、不易受干扰且定位精准的优点。

需要说明的是，本发明的思路即通过激光雷达单点测距的定位结果，用于修正掘进机惯导的误差，修正算法的优选设计步骤如下：

1)惯导和激光雷达融合定位的状态模型的建立，如下式：

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

式中：

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

2)基于激光雷达的单点测距

(a)激光雷达扫描

如图3所示，对参照面进行扫描，得到激光雷达到参照面中的单一控制点O的距离

(b)转换坐标系

经由载体系和激光雷达的安装误差阵，将激光雷达到参照面中的单一控制点O的距离

转换至载体系得到R^b，再运用惯导提供的姿态矩阵

将R^b转换到导航坐标系得到Rⁿ。

式中，

表示激光雷达坐标系到载体坐标系的旋转矩阵。

(c)求得激光雷达在导航坐标系中的位置

由于已知控制点的坐标已知，又已求得激光雷达到控制点O之间的距离Rⁿ，所以可以求得激光雷达在导航坐标系中的位置

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ⁿ表示通过惯导解算出的掘进机在导航坐标系的位置信息，

表示通过激光雷达得到掘进机在导航坐标系中的位置坐标。

同时，如图2所示，本发明还提供了基于惯导和激光雷达单点测距的掘进机定位方法的了另一实施例。如图2所示，该实施例与图1所示的实施例其他步骤均相同，唯一不同之处在于，步骤(1)之间还包括如下步骤：

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

设置该步骤的原因，在于惯导随着掘进机行驶，其定位误差会不断积累，通常误差比例为1/1000，即车辆行驶100千米，定位误差在100米左右。所以，当行驶一段距离，惯导定位误差较大时，可利用激光雷达单点测距技术，开始定位修正。

另一方面，如图5所示，本发明还提供了一种基于惯导和激光雷达单点测距的掘进机定位系统，基于其上所述的基于惯导和激光雷达单点测距的掘进机定位方法，所述掘进机定位系统包括掘进机、激光雷达、惯导和导航计算机，掘进机分别与激光雷达、惯导和导航计算机相搭载，激光雷达用于对参照面进行扫描，得到激光雷达到参照面中的单一控制点的距离，为后续提供激光雷达在导航坐标系中的位置提供输入，惯导用于得到掘进机的姿态矩阵，将激光雷达到控制点的距离转换到导航坐标系中，同时得到掘进机的位置信息，导航计算机用于对激光雷达和惯导的信息进行处理；优选地，导航计算机与惯导内设置的陀螺仪通过惯导接口相连，进行数据的双向传递，导航计算机与激光雷达通过激光雷达接口相连，进行数据的双向传递。

该掘进机定位系统显然具有前述基于惯导和激光雷达单点测距的掘进机定位方法，定位定向精度精准、自成体系、不易受干扰的优点。

综上所述，本发明具有如下优点：

(1)本发明根据激光雷达单点测距和惯导获得掘进机的位置，可以不依赖GPS卫星信号，在掘进机工作环境也能完全依靠系统自身实现定位，具有被动测量、自成体系、不易受干扰的优点；

(2)本发明利用激光雷达单点测距和惯导相融合获得掘进机的位置信息，掘进机通常需要在地底下工作一周甚至是更长的时间，这种方法能满足掘进机长时间的高精度的定位定向需求。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

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)启动掘进机，惯导随着掘进机行驶一段距离。

6.基于惯导和激光雷达单点测距的掘进机定位系统，其特征在于，基于权利要求1-5中任一项所述的基于惯导和激光雷达单点测距的掘进机定位方法，所述掘进机定位系统包括掘进机、激光雷达、惯导和导航计算机，所述掘进机分别与激光雷达、惯导和导航计算机相搭载，所述激光雷达用于对参照面进行扫描，得到激光雷达到参照面中的单一控制点的距离，为后续提供激光雷达在导航坐标系中的位置提供输入，所述惯导用于得到掘进机的姿态矩阵，将激光雷达到控制点的距离转换到导航坐标系中，同时得到掘进机的位置信息，所述导航计算机用于对激光雷达和惯导的信息进行处理。

7.根据权利要求6所述的基于惯导和激光雷达单点测距的掘进机定位系统，其特征在于，所述导航计算机与惯导内设置的陀螺仪通过惯导接口相连，进行数据的双向传递；所述导航计算机与激光雷达通过激光雷达接口相连，进行数据的双向传递。

Images