周期T内在导航坐标系中的位移， 进行航位推算得到移动设备定位结果。
[0056] (c)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止 移动，并通过三维激光雷达扫描当前场景，得到停止点点云数据p1+1，所述给定距离为根据 三维激光扫描仪的有效扫描距离范围，取有效扫描距离范围内居中的一个距离值；
[0057] (d)将步骤(a)得到的起始点点云数据?1与步骤(c)得到的停止点点云数据P 1+1进 行配准得到起始点与停止点间的位移与姿态变化，其具体配准过程为：
[0058] (d 1)对起始点点云数据Pi与停止点点云数据Pi+i进行降采样及降噪；
[0059] (d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(Xm ym Zm)与姿态 变化矩阵T3X3作为?1与&+1配准的初始值，最后采用点云配准算法对两幅点云配准得到起始 点与停止点间的位移与姿态变化。
[0060] (e)通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与 姿态进行更新；
[0061] (f)将步骤(e)中得到的移动设备停止点的位置信息作为起始点，重复执行步骤 (a)-(e)，由此完成整个设备定位过程。
[0062]本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以 限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含 在本发明的保护范围之内。
【主权项】
1. 一种煤矿井下移动设备自主定位的方法，其特征在于，该方法包括下列步骤： (a) 移动设备在起始点开始移动前，通过Ξ维激光扫描仪采集当前场景的Ξ维点云数 据，得到起始点点云数据； (b) 移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据 采集，建立基于Kalman滤波器的姿态测量模型，并利用所述姿态测量模型将采集到的惯性 传感器数据与里程计数据融合，继而进行航位推算，得到移动设备定位结果； (C)当由步骤(b)的航位推算结果得到移动设备已移动给定距离后，移动设备停止移 动，并通过Ξ维激光雷达扫描当前场景，得到停止点点云数据； (d) W步骤(b)得到的移动设备定位结果为配准初始值，将步骤(a)得到的起始点点云 数据与步骤（C)得到的停止点点云数据进行配准得到起始点与停止点间的位移与姿态变 化； (e) 通过步骤(d)中得到的起始点到停止点的位移与姿态变化对停止点的位置与姿态 进行更新； (f) 将步骤(e)得到的移动设备停止点的位置信息作为起始点，重复执行步骤(a)-(e)， 从而完成整个设备定位过程。2. 如权利要求1所述的方法，其特征在于，所述步骤(b)具体包括： (bl)移动设备开始移动，在移动过程中通过惯性传感器与里程计传感器分别进行数据 采集，根据惯性传感器的误差模型建立基于Kalman滤波器的姿态测量模型，所述姿态测量 模型包括状态方程及状态观测方程，所述状态方程为：其中状态变量x(t) = [qo巧Q2巧也Sfy Sfz]T，q〇、(ii、(i2、q3为表示姿态数据的四元数，Sfx、Sfy、SfzA、 别为惯性传感器中Ξ轴巧螺仪的Ξ轴漂移误差;其中，，COnCOy、 。分另执胺鮮織视爵义在3擲隱妃y，z上胸H區擅其中&、％、&分 另执胺鮮专感器中3#视割谢Θ鹏抓票觀勺励炯磯也其中〇fx、％、〇fz分别表不一阶马尔科夫模型的随机偏差;w(t)为系统状态噪声； 所述状态观测方程为：其中观测量ax、ay、az分别为惯性传感器中Ξ轴加速度计在 Ξ维测量轴x，y，z上的测量值；終为前一次测量得到的偏航角，Δ巧.为由里程计传感器测量 值计算得到的偏航角变化量，，3为移动设备的左右两轮轴距，SL、SR分别为安 装在左右两轮上的里程计传感器测量值;v(t)为测量噪声，t表示时间；g为重力加速度常 量； 化2)根据所述姿态测量模型，在一次采样周期T内完成一次Kalman滤波器的迭代，得到 融合后的姿态数据； (b3)将里程计传感器测量值求和平均得到采样周期T内移动设备在载体坐标系中移动 的距离，结合(b2)中的姿态数据，得到移动设备在采样周期T内在导航坐标系中的位移，进 行航位推算得到移动设备定位结果。3. 如权利要求1或2所述的方法，其特征在于，所述给定距离为根据Ξ维激光扫描仪的 有效扫描距离范围，取有效扫描距离范围内居中的一个距离值。4. 如权利要求1或2所述的方法，其特征在于，所述步骤(d)具体包括： (dl)对起始点点云数据Pi与停止点点云数据Pw进行降采样及降噪； (d2)将步骤(b)中的航位推算得到的起始点与停止点之间位移(Xm ym Zm)与姿态变化 矩阵T3X3作为Pi与Pw配准的初始值，最后采用点云配准算法对两幅点云配准得到起始点与 停止点间的位移与姿态变化。5. -种煤矿井下移动设备自主定位的系统，其特征在于，所述系统包括:里程计惯性定 位模块、W太网无线交换机、Ξ维激光扫描仪W及地面定位计算机； 所述里程计惯性定位模块包括惯性传感器、两个里程计传感器及嵌入式控制板;所述 惯性传感器，用于对移动设备运动过程中的惯性传感器数据进行测量;所述里程计传感器， 用于对移动设备运动过程中的里程计数据进行测量;所述嵌入式控制板，用于读取上述惯 性传感器数据和里程计数据，并建立基于Kalman滤波器的姿态测量模型，并利用所述姿态 测量模型将采集到的惯性传感器数据与里程计数据融合，继而进行航位推算，得到移动设 备定位结果，将所述移动设备定位结果通过W太网无线交换机上传至地面定位计算机； 所述Ξ维激光扫描仪，用于扫描移动设备当前场景的Ξ维点云数据并通过W太网无线 交换机上传至地面定位计算机； 所述地面定位计算机，用于接收所述移动设备定位结果与所述Ξ维点云数据，完成Ξ 维点云数据的配准，并存储和实时显示移动设备的当前位置。6. 如权利要求5所述的系统，其特征在于，所述惯性传感器具体包括用于对移动设备当 前的Ξ轴加速度进行实时检测的加速度传感器、用于对移动设备当前Ξ轴角速度进行实时 检测的巧螺仪。7. 如权利要求5或6所述的系统，其特征在于，所述惯性传感器的Ξ维测量轴与Ξ维激 光扫描仪的Ξ维测量轴方向一致。8. 如权利要求5或6所述的系统，其特征在于，所述里程计传感器可分别与移动设备非 导向轮的左右轮轴进行刚性连接。9. 如权利要求5或6所述的系统，其特征在于，所述里程计传感器为光电编码器。10. 如权利要求5或6所述的系统，其特征在于，所述嵌入式控制板具体包括与里程计传 感器通信的光电编码器接口、与惯性传感器通信的通信接口、将移动设备定位结果上传至 地面定位计算机的W太网接口W及嵌入式处理器。
【专利摘要】本发明公开了一种煤矿井下移动设备自主定位的方法。首先在起始点通过三维激光扫描仪扫描得到三维点云数据Pi；然后移动设备开始移动，其间通过惯性传感器与里程计传感器的测量数据融合进行航位推算得到设备实时的位置；在距离起始点不远处停止移动，在停止点扫描得到三维点云数据Pi+1，通过配准Pi与Pi+1得到两点之间的位移与姿态变化后更新停止点的位置，停止点作为起始点重复上述步骤完成整个实时定位过程。相应地，本发明还提供了与该方法对应的定位系统。本发明对环境要求低，能够在煤矿井下恶劣的环境下实现高精度定位，同时可以建立整个环境的三维可视化模型。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN105547288
【申请号】CN201510900007
【发明人】肖力, 赵梓乔, 王卓, 孙志刚
【申请人】华中科技大学
【公开日】2016年5月4日
【申请日】2015年12月8日