一种煤矿井下移动设备自主定位的方法及系统的制作方法
【技术领域】
[0001] 本发明属于定位技术领域，尤其涉及一种煤矿井下移动设备自主定位的方法及系 统。
【背景技术】
[0002] 煤矿井下高精度定位是数字矿山与绿色矿山的基础，在遥控采矿、无人采矿及矿 难救援等应用领域，发挥着巨大的作用。一般的定位系统主要解决的是在接收到全球定位 系统(GPS)信号时实现户外的高精度定位，而对于煤矿井下无 GPS信号、地磁干扰严重的恶 劣环境下，此类定位系统往往无用武之地。煤矿井下的定位多采用无线射频识别、ZigBee等 辅助定位技术，需要在井下预先布置大量参考节点，井下设备利用参考节点确定自身位置。 在实际应用中参考节点布置的密度和现场环境决定定位精度，系统存在定位精度低、稳定 性差、结构复杂等缺点。惯性器件导航具有实时高、受外界环境影响小等优点，然而惯性于 器件本身具有测量噪声，惯性导航系统（Inertial Navigation System, INS)的误差积累使 其不能满足井下长时间导航定位要求。在不外加光源煤矿井下能见度极低的情况下，视觉 导航定位将由于光源的影响以及煤矿井下场景特征不明显等原因，可靠性将急剧下降。因 此，亟需一套完整可行的煤矿井下定位方案，实现井下设备的高精度定位，加快矿山无人化 的进程。
【发明内容】
[0003] 针对现有技术的以上缺陷或改进需求，本发明提供了一种融合多源信息的煤矿井 下移动设备自主定位方法，该方法克服了传统无线通信定位与视觉定位的缺点，实现了煤 矿井下移动设备的实时高精度定位。
[0004] 为实现上述目的，按照本发明的一个方面，提出了一种煤矿井下移动设备自主定 位的方法，其特征在于，该方法包括下列步骤：
[0005] (a)移动设备在起始点开始移动前，通过三维激光扫描仪采集当前场景的三维点 云数据，得到起始点点云数据；
[0006] (b)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行 数据采集，建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的 惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果；
[0007] (c)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止 移动，并通过三维激光雷达扫描当前场景，得到停止点点云数据；
[0008] (d)以步骤(b)得到的移动设备定位结果为配准初始值，将步骤(a)得到的起始点 点云数据与步骤(c)得到的停止点点云数据进行配准得到起始点与停止点间的位移与姿态 变化；
[0009] (e)通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与 姿态进行更新；
[0010] (f)将步骤(e)得到的移动设备停止点的位置信息作为起始点，重复执行步骤(a)_ (e)，从而完成整个设备定位过程。
[0011] 作为进一步优选的，所述步骤(b)具体包括：
[0012] (bl)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行 数据采集，根据惯性传感器的误差模型建立基于Kalman滤波器的姿态测量模型，所述姿态 测量模型包括状态方程及状态观测方程，所述状态方程为： ~\j( g~
[0013] x(〇= Λ x{t) + Gw(t) ? β」 '
[0014] 其中状态变量X(t) = [q0 qi q2 q3 5fx 5fy 3乜]7^叫1、92、93为表示姿态数据的四元数，5亡、 〇 --? 咕、吣分别为惯性传感器中三轴陀螺仪的三轴漂移误差;其中，t ^ ^，ωχ、％ 0 - 0 0 ~ ωζ分另伪撼义在3倕贝疆轴c，y，z上·_直;B:= 0 -βΙΥ 0 ，其中&、i3fy、i3fz _ 0 0 -ββ_ 分别为暖性传感器中三轴陀螺働三轴随机漂移的反0寸间常数;c=[0 0 ο 0 其中0&、0&、0^分别表不一阶马尔科夫模型的随机偏差;w(t)为系统状态噪声；
[0015] 所述状态观测方程为： 2{q,q,-qi)q2)g
[0016] y= {iil-if；-ill+il])g + ν：(〇· arctan +^2^1)/ 9 _ V /1 - 2{q2 +q~i))_
[0017] 其中观测直y.= [ax气：.βζ. A炉+死]，ax、ay、az*别为惯性传感器中二轴加速度 计在三维测量轴x，y，z上的测量值；I为前一次测量得到的偏航角，Δρ为由里程计传感器 测量值计算得到的偏航角变化量，= 为移动设备的左右两轮轴距，SL、SR分别 为安装在左右两轮上的里程计传感器测量值;v(t)为测量噪声，t表示时间；g为重力加速度 常量。
[0018] (b2)根据所述姿态测量模型，在一次采样周期T内完成一次Kalman滤波器的迭代， 得到融合后的姿态数据；
[0019] (b3)将里程计传感器测量值求和平均得到采样周期T内移动设备在载体坐标系中 移动的距离，结合(b2)中的姿态数据，得到移动设备在采样周期T内在导航坐标系中的位 移，进行航位推算得到移动设备定位结果。
[0020] 作为进一步优选的，所述给定距离为根据三维激光扫描仪的有效扫描距离范围， 取有效扫描距离范围内居中的一个距离值。
[0021]作为进一步优选的，所述步骤(d)具体包括：
[0022 ] (d 1)对起始点点云数据Pi与停止点点云数据Pi+ι进行降采样及降噪；
[0023] (d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(Xm ym Zm)与姿态 变化矩阵T3X3作为?1与&+1配准的初始值，最后采用点云配准算法对两幅点云配准得到起始 点与停止点间的位移与姿态变化。
[0024] 按照本发明的另一方面，提出了一种煤矿井下移动设备自主定位的系统，其特征 在于，所述系统包括:里程计惯性定位模块、以太网无线交换机、三维激光扫描仪以及地面 定位计算机；
[0025]所述里程计惯性定位模块包括惯性传感器、两个里程计传感器及嵌入式控制板； 所述惯性传感器，用于对移动设备运动过程中的惯性传感器数据进行测量;所述里程计传 感器，用于对移动设备运动过程中的里程计数据进行测量;所述嵌入式控制板，用于读取上 述惯性传感器数据和里程计数据，并建立基于Kalman滤波器的姿态测量模型，并利用所述 姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移 动设备定位结果，将所述移动设备定位结果通过以太网无线交换机上传至地面定位计算 机；
[0026]所述三维激光扫描仪，用于扫描移动设备当前场景的三维点云数据并通过以太网 无线交换机上传至地面定位计算机；
[0027]所述地面定位计算机，用于接收所述移动设备定位结果与所述三维点云数据，完 成三维点云数据的配准，并存储和实时显示移动设备的当前位置。
[0028]作为进一步优选的，所述惯性传感器具体包括用于对移动设备当前的三轴加速度 进行实时检测的加速度传感器、用于对移动设备当前三轴角速度进行实时检测的陀螺仪。 [0029]作为进一步优选的，所述惯性传感器的三维测量轴与三维激光扫描仪的三维测量 轴方向一致。
[0030] 作为进一步优选的，所述里程计传感器可分别与移动设备非导向轮的左右轮轴进 行刚性连接。
[0031] 作为进一步优选的，所述里程计传感器为光电编码器。
[0032] 作为进一步优选的，所述嵌入式控制板具体包括与里程计传感器通信的光电编码 器接口、与惯性传感器通信的通信接口