CN105547288A - 一种煤矿井下移动设备自主定位的方法及系统 - Google Patents

Abstract

本发明公开了一种煤矿井下移动设备自主定位的方法。首先在起始点通过三维激光扫描仪扫描得到三维点云数据P_i；然后移动设备开始移动，其间通过惯性传感器与里程计传感器的测量数据融合进行航位推算得到设备实时的位置；在距离起始点不远处停止移动，在停止点扫描得到三维点云数据P_i+1，通过配准P_i与P_i+1得到两点之间的位移与姿态变化后更新停止点的位置，停止点作为起始点重复上述步骤完成整个实时定位过程。相应地，本发明还提供了与该方法对应的定位系统。本发明对环境要求低，能够在煤矿井下恶劣的环境下实现高精度定位，同时可以建立整个环境的三维可视化模型。

Description

一种煤矿井下移动设备自主定位的方法及系统

技术领域

本发明属于定位技术领域，尤其涉及一种煤矿井下移动设备自主定位的方法及系统。

背景技术

煤矿井下高精度定位是数字矿山与绿色矿山的基础，在遥控采矿、无人采矿及矿难救援等应用领域，发挥着巨大的作用。一般的定位系统主要解决的是在接收到全球定位系统(GPS)信号时实现户外的高精度定位，而对于煤矿井下无GPS信号、地磁干扰严重的恶劣环境下，此类定位系统往往无用武之地。煤矿井下的定位多采用无线射频识别、ZigBee等辅助定位技术，需要在井下预先布置大量参考节点，井下设备利用参考节点确定自身位置。在实际应用中参考节点布置的密度和现场环境决定定位精度，系统存在定位精度低、稳定性差、结构复杂等缺点。惯性器件导航具有实时高、受外界环境影响小等优点，然而惯性于器件本身具有测量噪声，惯性导航系统(InertialNavigationSystem,INS)的误差积累使其不能满足井下长时间导航定位要求。在不外加光源煤矿井下能见度极低的情况下，视觉导航定位将由于光源的影响以及煤矿井下场景特征不明显等原因，可靠性将急剧下降。因此，亟需一套完整可行的煤矿井下定位方案，实现井下设备的高精度定位，加快矿山无人化的进程。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种融合多源信息的煤矿井下移动设备自主定位方法，该方法克服了传统无线通信定位与视觉定位的缺点，实现了煤矿井下移动设备的实时高精度定位。

为实现上述目的，按照本发明的一个方面，提出了一种煤矿井下移动设备自主定位的方法，其特征在于，该方法包括下列步骤：

(a)移动设备在起始点开始移动前,通过三维激光扫描仪采集当前场景的三维点云数据，得到起始点点云数据；

(b)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据采集，建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果；

(c)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止移动，并通过三维激光雷达扫描当前场景，得到停止点点云数据；

(d)以步骤(b)得到的移动设备定位结果为配准初始值，将步骤(a)得到的起始点点云数据与步骤(c)得到的停止点点云数据进行配准得到起始点与停止点间的位移与姿态变化；

(e)通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与姿态进行更新；

(f)将步骤(e)得到的移动设备停止点的位置信息作为起始点，重复执行步骤(a)-(e)，从而完成整个设备定位过程。

作为进一步优选的，所述步骤(b)具体包括：

(b1)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据采集，根据惯性传感器的误差模型建立基于Kalman滤波器的姿态测量模型，所述姿态测量模型包括状态方程及状态观测方程，所述状态方程为：

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{cc}A& 0\\ 0& B\end{array}\right]x\left(t\right)+Gw\left(t\right)$

其中状态变量x(t)＝[q₀q₁q₂q₃δf_xδf_yδf_z]^T，q₀、q₁、q₂、q₃为表示姿态数据的四元数，δf_x、δf_y、δf_z分别为惯性传感器中三轴陀螺仪的三轴漂移误差；其中， $A=\left[\begin{array}{cccc}0& -\frac{1}{2}{\mathrm{\ω}}_x& -\frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z\\ \frac{1}{2}{\mathrm{\ω}}_x& 0& \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y\\ \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z& 0& \frac{1}{2}{\mathrm{\ω}}_x\\ \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_x& 0\end{array}\right],$ ω_x、ω_y、ω_z分别为惯性传感器中三轴陀螺仪在三维测量轴x,y,z上的测量值； $B=\left[\begin{array}{ccc}-{\mathrm{\β}}_{fx}& 0& 0\\ 0& -{\mathrm{\β}}_{fy}& 0\\ 0& 0& -{\mathrm{\β}}_{fz}\end{array}\right],$ 其中β_fx、β_fy、β_fz分别为惯性传感器中三轴陀螺仪的三轴随机漂移的反时间常数； $G={\left[\begin{array}{ccccccc}0& 0& 0& 0& \sqrt{2{\mathrm{\β}}_{fx}{\mathrm{\σ}}_{fx}^2}& \sqrt{2{\mathrm{\β}}_{fy}{\mathrm{\σ}}_{fy}^2}& \sqrt{2{\mathrm{\β}}_{fz}{\mathrm{\σ}}_{fz}^2}\end{array}\right]}^T,$ 其中σ_fx、σ_fy、σ_fz分别表示一阶马尔科夫模型的随机偏差；w(t)为系统状态噪声；

所述状态观测方程为：

$y=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g\\ 2(q_2{q}_3+q_0{q}_1)g\\ (q_0^2-q_1^2-q_2^2+q_3^2)g\\ \arctan \left(2\right(q_0{q}_3+q_2{q}_1)/1-2(q_2^2+q_3^2))\end{array}\right]+v\left(t\right)$

其中观测量a_x、a_y、a_z分别为惯性传感器中三轴加速度计在三维测量轴x,y,z上的测量值；为前一次测量得到的偏航角，为由里程计传感器测量值计算得到的偏航角变化量，R为移动设备的左右两轮轴距，s_L、s_R分别为安装在左右两轮上的里程计传感器测量值；v(t)为测量噪声，t表示时间；g为重力加速度常量。

(b2)根据所述姿态测量模型，在一次采样周期T内完成一次Kalman滤波器的迭代，得到融合后的姿态数据；

(b3)将里程计传感器测量值求和平均得到采样周期T内移动设备在载体坐标系中移动的距离，结合(b2)中的姿态数据，得到移动设备在采样周期T内在导航坐标系中的位移，进行航位推算得到移动设备定位结果。

作为进一步优选的，所述给定距离为根据三维激光扫描仪的有效扫描距离范围，取有效扫描距离范围内居中的一个距离值。

作为进一步优选的，所述步骤(d)具体包括：

(d1)对起始点点云数据P_i与停止点点云数据P_i+1进行降采样及降噪；

(d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(x_my_mz_m)与姿态变化矩阵T_3×3作为P_i与P_i+1配准的初始值，最后采用点云配准算法对两幅点云配准得到起始点与停止点间的位移与姿态变化。

按照本发明的另一方面，提出了一种煤矿井下移动设备自主定位的系统，其特征在于，所述系统包括：里程计惯性定位模块、以太网无线交换机、三维激光扫描仪以及地面定位计算机；

所述里程计惯性定位模块包括惯性传感器、两个里程计传感器及嵌入式控制板；所述惯性传感器，用于对移动设备运动过程中的惯性传感器数据进行测量；所述里程计传感器，用于对移动设备运动过程中的里程计数据进行测量；所述嵌入式控制板，用于读取上述惯性传感器数据和里程计数据，并建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果，将所述移动设备定位结果通过以太网无线交换机上传至地面定位计算机；

所述三维激光扫描仪，用于扫描移动设备当前场景的三维点云数据并通过以太网无线交换机上传至地面定位计算机；

所述地面定位计算机，用于接收所述移动设备定位结果与所述三维点云数据，完成三维点云数据的配准，并存储和实时显示移动设备的当前位置。

作为进一步优选的，所述惯性传感器具体包括用于对移动设备当前的三轴加速度进行实时检测的加速度传感器、用于对移动设备当前三轴角速度进行实时检测的陀螺仪。

作为进一步优选的，所述惯性传感器的三维测量轴与三维激光扫描仪的三维测量轴方向一致。

作为进一步优选的，所述里程计传感器可分别与移动设备非导向轮的左右轮轴进行刚性连接。

作为进一步优选的，所述里程计传感器为光电编码器。

作为进一步优选的，所述嵌入式控制板具体包括与里程计传感器通信的光电编码器接口、与惯性传感器通信的通信接口、将移动设备定位结果上传至地面定位计算机的以太网接口以及嵌入式处理器。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明公开了一种煤矿井下移动设备自主定位的方法，提出了一种惯性传感器与左右轮速差数据融合的Kalman滤波模型，从而减小航位推算过程中姿态数据的测量误差，提高航位推算定位的精度；

2.本发明方法中光照条件对三维激光扫描仪的影响小，而惯性传感器与轮速来自移动设备自身，所以对环境要求低，尤其适用于工程实际使用；

3.针对航位推算定位结果不够准确及三维激光扫描仪测量周期长，不满足实时定位的需要的特点，本发明方法中采用航位推算得到实时定位结果，其间通过点云配准得到的数据不断校正航位推算的定位结果，这样既保证了定位的实时性，又提高了定位的精度。

附图说明

图1是本发明所使用的硬件结构的示意图；

图2是本发明所述方法的流程图；

图3是本发明所述系统的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是按照本发明的煤矿井下移动设备自主定位系统的硬件结构示意图。可选的，在图1所示的实施例中，所述光电编码器即里程计传感器。如图1所示，该系统主要包括里程计惯性定位模块、以太网无线交换机、三维激光扫描仪以及地面定位计算机。其中里程计惯性定位模块包括惯性传感器、两个光电编码器以及包含以太网接口的嵌入式控制板。具体而言惯性传感器模块譬如为AnalogDevices公司的ADIS16405BMLZ，其中包括用于对移动设备加速度进行实时检测的三轴加速度传感器、用于对移动设备角速度进行实时检测的三轴陀螺仪；嵌入式控制板包含光电编码器接口、与惯性传感器通信的SPI通信接口、将定位数据上传地面定位计算机的以太网接口以及嵌入式处理器。作为其具体构成形式，譬如可采用ST公司的STM32F4系列处理器芯片作为嵌入式控制板的处理器。三维激光扫描仪为本实验室自主研制的含以太网接口的矿用防爆三维扫描仪。地面定位计算机用于接收点云数据与里程计惯性定位信息，完成点云数据的配准并存储和实时显示设备当前位置。

两个光电编码器的测量轴分别与非导向轮的左右轮轴刚性连接；如同本领域技术人员所熟知的，惯性传感器的三维测量轴与三维激光扫描仪的三维测量轴方向一致，或经过小角度的数学校正与平移后坐标系可重合。

如图2和图3所示，本发明公开一种用于煤矿井下移动设备自主定位的方法。设备的定位结果在导航坐标系下表示为一个(x_Ly_Lz_L)向量，导航坐标系与本地地图坐标系重合。本地地图坐标系具体可以选择地理东方为X轴，地理北方位Y轴，地理正上方为Z轴。载体坐标系与设备固联，作为其具体构成形式，设备正前方为X轴方向，左侧为Y轴方向，正上方为Z轴方向建立笛卡尔载体坐标系。具体包括下列步骤：

(a)在起始点开始移动前,移动设备通过三维激光扫描仪采集当前场景的三维点云数据，得到起始点点云数据P_i；

(b)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器，即光电编码器分别进行数据采集，建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果；

(b1)移动设备开始移动，在移动过程中通过惯性传感器与光电编码器分别进行数据采集，根据惯性传感器的误差模型建立基于Kalman滤波器的姿态测量模型，所述姿态测量模型包括状态方程及状态观测方程，所述状态方程为：

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{cc}A& 0\\ 0& B\end{array}\right]x\left(t\right)+Gw\left(t\right)---\left(1\right)$

其中状态变量x(t)＝[q₀q₁q₂q₃δf_xδf_yδf_z]^T，q₀、q₁、q₂、q₃为表示姿态数据的四元数，δf_x、δf_y、δf_z分别为惯性传感器中三轴陀螺仪的三轴漂移误差；其中， $A=\left[\begin{array}{cccc}0& -\frac{1}{2}{\mathrm{\ω}}_x& -\frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z\\ \frac{1}{2}{\mathrm{\ω}}_x& 0& \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y\\ \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z& 0& \frac{1}{2}{\mathrm{\ω}}_x\\ \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_x& 0\end{array}\right],$ ω_x、ω_y、ω_z分别为惯性传感器中三轴陀螺仪在三维测量轴x,y,z上的测量值； $B=\left[\begin{array}{ccc}-{\mathrm{\β}}_{fx}& 0& 0\\ 0& -{\mathrm{\β}}_{fh}& 0\\ 0& 0& -{\mathrm{\β}}_{fz}\end{array}\right],$ 其中β_fx、β_fy、β_fz分别为惯性传感器中三轴陀螺仪的三轴随机漂移的反时间常数； $G={\left[\begin{array}{ccccccc}0& 0& 0& 0& \sqrt{2{\mathrm{\β}}_{fx}{\mathrm{\σ}}_{fx}^2}& \sqrt{2{\mathrm{\β}}_{fy}{\mathrm{\σ}}_{fy}^2}& \sqrt{2{\mathrm{\β}}_{fz}{\mathrm{\σ}}_{fz}^2}\end{array}\right]}^T,$ 其中σ_fx、σ_fy、σ_fz分别表示一阶马尔科夫模型的随机偏差；w(t)为系统状态噪声。

姿态数据用于表示导航坐标系与载体坐标系的旋转关系，有四元数、欧拉角与姿态矩阵等多种数学表示方式，三种表示方式可以相互转换。四元数表示的姿态矩阵可表示为 $C_n^b=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right],$ 其中b表示载体坐标系，n表示导航坐标系，通过姿态矩阵可将两坐标系中点进行相互转换。采用欧拉角表示的姿态矩阵为其中θ、φ、分别表示俯仰角、横滚角、偏航角。其中偏航角可由四元数计算得到偏航角的变化量由左右光电编码器的测量值计算得到，可作为偏航角的观测量。

导航坐标系中重力加速度可表示为[00g]^T，结合四元数表示的姿态矩阵，重力加速度在载体坐标系中可表示为 $C_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g\\ 2(q_2{q}_3+q_0{q}_1)g\\ (q_0^2-q_1^2-q_2^2+q_3^2)g\end{array}\right].$ 移动设备的运动加速度相对重力加速度可以忽略，可以认为三轴加速度计的测量值就是重力加速度在载体坐标系XYZ各轴的加速度分量，由此可将三轴加速度计的测量值作为重力加速度的观测量。

建立状态观测方程为：

$y=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g\\ 2(q_2{q}_3+q_0{q}_1)g\\ (q_0^2-q_1^2-q_2^2+q_3^2)g\\ \arctan \left(2\right(q_0{q}_3+q_2{q}_1)/1-2(q_2^2+q_3^2))\end{array}\right]+v\left(t\right)---\left(2\right)$

其中观测量a_x、a_y、a_z分别为惯性传感器中三轴加速度计在三维测量轴x,y,z上的测量值；为前一次测量得到的偏航角，为由光电编码器测量值计算得到的偏航角变化量，R为移动设备的左右两轮轴距，s_L、s_R分别为安装在左右两轮上的光电编码器测量值；v(t)为测量噪声，t表示时间；g为重力加速度常量。

(b2)根据所述姿态测量模型，在一次采样周期T内完成一次Kalman滤波器的迭代，得到融合后的姿态数据；

(b3)将光电编码器测量值求和平均得到采样周期T内移动设备在载体坐标系中移动的距离，结合(b2)中的姿态数据，得到移动设备在采样周期T内在导航坐标系中的位移，进行航位推算得到移动设备定位结果。

(c)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止移动，并通过三维激光雷达扫描当前场景，得到停止点点云数据P_i+1，所述给定距离为根据三维激光扫描仪的有效扫描距离范围，取有效扫描距离范围内居中的一个距离值；

(d)将步骤(a)得到的起始点点云数据P_i与步骤(c)得到的停止点点云数据P_i+1进行配准得到起始点与停止点间的位移与姿态变化，其具体配准过程为：

(d1)对起始点点云数据P_i与停止点点云数据P_i+1进行降采样及降噪；

(d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(x_my_mz_m)与姿态变化矩阵T_3×3作为P_i与P_i+1配准的初始值，最后采用点云配准算法对两幅点云配准得到起始点与停止点间的位移与姿态变化。

(e)通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与姿态进行更新；

(f)将步骤(e)中得到的移动设备停止点的位置信息作为起始点，重复执行步骤(a)-(e)，由此完成整个设备定位过程。

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

Claims (10)

1.一种煤矿井下移动设备自主定位的方法，其特征在于，该方法包括下列步骤：

(a)移动设备在起始点开始移动前,通过三维激光扫描仪采集当前场景的三维点云数据，得到起始点点云数据；

(b)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据采集，建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果；

(c)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止移动，并通过三维激光雷达扫描当前场景，得到停止点点云数据；

(d)以步骤(b)得到的移动设备定位结果为配准初始值，将步骤(a)得到的起始点点云数据与步骤(c)得到的停止点点云数据进行配准得到起始点与停止点间的位移与姿态变化；

(e)通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与姿态进行更新；

(f)将步骤(e)得到的移动设备停止点的位置信息作为起始点，重复执行步骤(a)-(e)，从而完成整个设备定位过程。

2.如权利要求1所述的方法，其特征在于，所述步骤(b)具体包括：

(b1)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据采集，根据惯性传感器的误差模型建立基于Kalman滤波器的姿态测量模型，所述姿态测量模型包括状态方程及状态观测方程，所述状态方程为：

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{cc}A& 0\\ 0& B\end{array}\right]x\left(t\right)+Gw\left(t\right)$

其中状态变量x(t)＝[q₀q₁q₂q₃δf_xδf_yδf_z]^T，q₀、q₁、q₂、q₃为表示姿态数据的四元数，δf_x、δf_y、δf_z分别为惯性传感器中三轴陀螺仪的三轴漂移误差；其中， $A=\left[\begin{array}{cccc}0& -\frac{1}{2}{\mathrm{\ω}}_x& -\frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z\\ \frac{1}{2}{\mathrm{\ω}}_x& 0& \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y\\ \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_z& 0& \frac{1}{2}{\mathrm{\ω}}_x\\ \frac{1}{2}{\mathrm{\ω}}_z& \frac{1}{2}{\mathrm{\ω}}_y& -\frac{1}{2}{\mathrm{\ω}}_x& 0\end{array}\right],$ ω_x、ω_y、ω_z分别为惯性传感器中三轴陀螺仪在三维测量轴x,y,z上的测量值； $B=\left[\begin{array}{ccc}-{\mathrm{\β}}_{fx}& 0& 0\\ 0& -{\mathrm{\β}}_{fy}& 0\\ 0& 0& -{\mathrm{\β}}_{fz}\end{array}\right],$ 其中β_fx、β_fy、β_fz分别为惯性传感器中三轴陀螺仪的三轴随机漂移的反时间常数； $G={\left[\begin{array}{ccccccc}0& 0& 0& 0& \sqrt{2{\mathrm{\β}}_{fx}{\mathrm{\σ}}_{fx}^2}& \sqrt{2{\mathrm{\β}}_{fy}{\mathrm{\σ}}_{fy}^2}& \sqrt{2{\mathrm{\β}}_{f2}{\mathrm{\σ}}_{fz}^2}\end{array}\right]}^T,$ 其中σ_fx、σ_fy、σ_fz分别表示一阶马尔科夫模型的随机偏差；w(t)为系统状态噪声；

所述状态观测方程为：

$y=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g\\ 2(q_2{q}_3+q_0{q}_1)g\\ (q_0^2-q_1^2-q_2^2+q_3^2)g\\ arctan\left(2\right(q_0{q}_3+q_2{q}_1)/1-2(q_2^2+q_3^2\left)\right)\end{array}\right]+v\left(t\right)$

其中观测量a_x、a_y、a_z分别为惯性传感器中三轴加速度计在三维测量轴x,y,z上的测量值；为前一次测量得到的偏航角，为由里程计传感器测量值计算得到的偏航角变化量，R为移动设备的左右两轮轴距，s_L、s_R分别为安装在左右两轮上的里程计传感器测量值；v(t)为测量噪声，t表示时间；g为重力加速度常量；

(b2)根据所述姿态测量模型，在一次采样周期T内完成一次Kalman滤波器的迭代，得到融合后的姿态数据；

(b3)将里程计传感器测量值求和平均得到采样周期T内移动设备在载体坐标系中移动的距离，结合(b2)中的姿态数据，得到移动设备在采样周期T内在导航坐标系中的位移，进行航位推算得到移动设备定位结果。

3.如权利要求1或2所述的方法，其特征在于，所述给定距离为根据三维激光扫描仪的有效扫描距离范围，取有效扫描距离范围内居中的一个距离值。

4.如权利要求1或2所述的方法，其特征在于，所述步骤(d)具体包括：

(d1)对起始点点云数据P_i与停止点点云数据P_i+1进行降采样及降噪；

(d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(x_my_mz_m)与姿态变化矩阵T_3×3作为P_i与P_i+1配准的初始值，最后采用点云配准算法对两幅点云配准得到起始点与停止点间的位移与姿态变化。

5.一种煤矿井下移动设备自主定位的系统，其特征在于，所述系统包括：里程计惯性定位模块、以太网无线交换机、三维激光扫描仪以及地面定位计算机；

所述里程计惯性定位模块包括惯性传感器、两个里程计传感器及嵌入式控制板；所述惯性传感器，用于对移动设备运动过程中的惯性传感器数据进行测量；所述里程计传感器，用于对移动设备运动过程中的里程计数据进行测量；所述嵌入式控制板，用于读取上述惯性传感器数据和里程计数据，并建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果，将所述移动设备定位结果通过以太网无线交换机上传至地面定位计算机；

所述三维激光扫描仪，用于扫描移动设备当前场景的三维点云数据并通过以太网无线交换机上传至地面定位计算机；

所述地面定位计算机，用于接收所述移动设备定位结果与所述三维点云数据，完成三维点云数据的配准，并存储和实时显示移动设备的当前位置。

6.如权利要求5所述的系统，其特征在于，所述惯性传感器具体包括用于对移动设备当前的三轴加速度进行实时检测的加速度传感器、用于对移动设备当前三轴角速度进行实时检测的陀螺仪。

7.如权利要求5或6所述的系统，其特征在于，所述惯性传感器的三维测量轴与三维激光扫描仪的三维测量轴方向一致。

8.如权利要求5或6所述的系统，其特征在于，所述里程计传感器可分别与移动设备非导向轮的左右轮轴进行刚性连接。

9.如权利要求5或6所述的系统，其特征在于，所述里程计传感器为光电编码器。

10.如权利要求5或6所述的系统，其特征在于，所述嵌入式控制板具体包括与里程计传感器通信的光电编码器接口、与惯性传感器通信的通信接口、将移动设备定位结果上传至地面定位计算机的以太网接口以及嵌入式处理器。