CN111637889A

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

Info

Publication number: CN111637889A
Application number: CN202010541455.XA
Authority: CN
Inventors: 韩松来; 任星宇; 董晶; 芦佳振
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2020-06-15
Filing date: 2020-06-15
Publication date: 2020-09-08

Abstract

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

Description

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

技术领域

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

背景技术

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

掘进机的自主定位定向系统是实现掘进机自动化的关键技术，现阶段相关技术主要有激光标靶法、基于三激光标记点图像的掘进机定位技术、基于惯性导航系统的掘进机自主定位技术、基于机器视觉的掘进机自主定位技术和基于iGPS(indoorGPS，室内GPS)测量技术的掘进机自主定位技术，但上述技术均存在不足之处，其中：

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

(2)基于三激光标记点图像的掘进机定位技术的缺点是：该方法需利用图像对掘进巷道参照面的图像处理，巷道的粉尘大，光照条件低，影响了掘进机的位置精度。

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

(4)基于机器视觉的掘进机自主定位的缺点是：因为该方法是根据摄像机获得的图像视觉信息对目标的位置和姿态进行测量，而掘进机的工作环境大多为狭长的巷道，巷道的粉尘大，且照明度低，该方法在这种情况下易产生误判，故将机器视觉用于掘进机的自主定位定向的精度较低，实用性不高。

(5)基于iGPS测量技术的位姿测量方法的缺点是：由于该方法利用三角测量原理来确定接收器位置坐标的坐标测量技术，iGPS测量技术测量精度高，但系统复杂度较高，造价昂贵，且有效测量距离很短。

发明内容

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

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

(1)利用激光雷达对参照面进行扫描，扫描到参照面上的三个控制点，得到激光雷达分别到三个控制点的距离；

(2)根据已知的参照面中三个控制点的坐标和激光雷达分别到三个控制点在的距离联立方程，得到激光雷达的位置信息；

(3)根据卡尔曼滤波器的原理，利用惯导提供的掘进机在导航坐标系中的位置信息和已得到的得到激光雷达的位置信息，完成惯导和激光雷达数据的融合，实现掘进机的高精度定位定向。

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

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

式中：

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

进一步地，设定参照面中的三个控制点分别为A、B和C，激光雷达到参照面中的三个控制点A、B和C的距离为R₁、R₂、R₃，其中，A的坐标为(x₁ y₁ z₁)，B的坐标为(x₂ y₂ z₂)，C的坐标为(x₃ y₃ z₃)，激光雷达的坐标为(x y z)，则R₁、R₂、R₃通过下式(3)求解，激光雷达的位置信息通过下式(9)求得：

式中，

表示激光雷达的位置信息，Y通过以下公式表达：

Y＝(A^TA)^-1A^Tb (8)

式中，T表示矩阵倒置，A和b通过以下公式表达：

进一步地，步骤(3)中惯导通过其内部设置的陀螺仪和加速度计提供掘进机的加速度和角速度信息，根据惯性导航算法解算出掘进机在导航坐标系的位置信息。

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

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

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

Z＝HX+V (10)

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

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

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

式中，Pⁿ表示通过惯导解算出的掘进机在导航坐标系的位置信息，

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

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

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

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

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

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

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

附图说明

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

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

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

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

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

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

具体实施方式

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

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

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

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

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

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

在进一步地技术方案中，上述步骤中惯导通过其内部设置的陀螺仪和加速度计提供掘进机的加速度和角速度信息，根据惯性导航算法解算出掘进机在导航坐标系的位置信息。

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

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

式中：

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

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

(a)激光雷达扫描

如图3所示，对参照面进行扫描，得到激光雷达到参照面中的三个控制点A、B、C的距离R₁、R₂、R₃。

(b)求得激光雷达的位置

设定已知激光雷达到三个控制点A、B、C的距离R₁、R₂、R₃，且已知A、B、C的坐标，A的坐标为(x₁ y₁ z₁)，B的坐标为(x₂ y₂ z₂)，C的坐标为(x₃ y₃ z₃)，激光雷达的坐标为(x yz)，则联立距离公式得

展开得：

联立得：

将公式(5)转换为下式

AY＝b (6)

其中，

用最小二乘法求解上述方程，得

Y＝(A^TA)^-1A^Tb (8)

式中，T表示矩阵倒置；

则激光雷达所处的位置信息：

式中，

表示激光雷达的位置信息。

3)量测方程的建立和量测值的获取

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

Z＝HX+V (10)

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

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

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

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

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

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

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

此外值得提及的是，上述惯导根据惯导内部陀螺仪输出的角速度，加速度计输出的加速度，在导航计算机中根据惯性导航算法进行计算，提供掘进机在导航坐标系中的位置信息。

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

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

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

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

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

Claims

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

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

式中：

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

为加速度计常值零偏。

3.根据权利要求2所述的基于惯导和激光雷达三点测距的掘进机定位方法，其特征在于，设定参照面中的三个控制点分别为A、B和C，激光雷达到参照面中的三个控制点A、B和C的距离为R₁、R₂、R₃，其中，A的坐标为(x₁ y₁ z₁)，B的坐标为(x₂ y₂ z₂)，C的坐标为(x₃ y₃z₃)，激光雷达的坐标为(x y z)，则R₁、R₂、R₃通过下式(3)求解，激光雷达的位置信息通过下式(9)

求得：

式中，

表示激光雷达的位置信息，Y通过以下公式表达：

Y＝(A^TA)^-1A^Tb (8)

式中，T表示矩阵倒置，A和b通过以下公式表达：

4.根据权利要求1所述的基于惯导和激光雷达三点测距的掘进机定位方法，其特征在于，步骤(3)中惯导通过其内部设置的陀螺仪和加速度计提供掘进机的加速度和角速度信息，根据惯性导航算法解算出掘进机在导航坐标系的位置信息。

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

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

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

Z＝HX+V (10)

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

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

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

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

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

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

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