CN109059942A - 一种井下高精度导航地图构建系统及构建方法 - Google Patents

Abstract

本发明涉及一种井下高精度导航地图构建系统及构建方法，基于包括井下地理信息系统、本质安全型建模装置、以及布设于井下各巷道的各个本质安全型超宽带雷达UWB锚节点所设计的系统，进一步设计引入构建方法，整个设计能够快速构建井下高精度导航地图、以及动态更新环境模型功能，并将建模结果输入井下地理信息系统，构建可以为井下自主移动车辆，提供辅助定位与路径规划的多层次地图模型。

Description

一种井下高精度导航地图构建系统及构建方法

技术领域

本发明涉及一种井下高精度导航地图构建系统及构建方法，属于煤矿环境建模技术领域。

背景技术

自动化和智能化是井下移动设备的发展趋势，而定位导航技术是实现上述目标的关键。目前井下的定位技术主要集中于采煤机、刮板机、矿车等轨道式装备的定位以及人员定位中，通常采用编码器或惯性器件进行航迹推算、射频定位等定位方式。井下目前用于支护、凿岩、装载、铲运、钻孔、瓦斯抽采、采煤与救援等施工装备与机器人仍然主要依靠人工驾驶或遥控操作，危险性高、劳动强度大。煤矿辅助运输设备如无轨胶轮车、梭式矿车等承担着除煤炭运输外的包括人员、材料、工具、设备、矸石等其他各种生产资料的运输任务，是煤矿生产企业重点使用且不可或缺的设备。但其目前也主要依赖人工手动驾驶，在井下恶劣环境中驾驶安全性无法得到保障。煤矿对于上述移动装备的无人化、智能化具有极大的需求。

为实现上述井下移动设备的自主作业，研发面向定位导航任务的、满足井下环境应用的地图构建方法是关键技术之一。井下地理信息系统(Geographic informationsystem，GIS)是一种简单高效的数字地图，在矿山地质测量、井下供电、通讯、通风、供水等领域得到应用，提高了矿山的数字化程度。目前GIS在煤矿应用中，主要利用空间数据库建立矿区巷道的平面图，人员定位分布图、设备物资分布图。这类地图的要素主要是简单的地理信息与拓扑关系等，无法实现辅助移动设备进行厘米级的高精度定位与导航功能。

激光点云地图在地面机器人、无人驾驶领域已经得到广泛应用。利用车载高精度激光雷达及其他多类型传感器采集数据构建高精度地图，可以辅助自动驾驶车辆进行精确定位。但是这类技术通常需要价格昂贵的高精度、多类型传感器提供精确的定位信息，依赖于GPS、视觉相机等传感器融合技术，而这些传感器由于工作环境的限制无法直接在煤矿使用或效果较差。目前在环境测绘领域使用的激光扫描仪进行巷道变形测量、巷道环境建模的的方法尽管精度较高，但是价格昂贵且必须经过单点扫描后通过后期拼接完成建模，工作效率低。现有的基于激光SLAM的方式在煤矿巷道进行建模时，由于井巷存在退化、动态等复杂场景，建模精度低，无法满足自主移动设备的定位与导航需求。综上所述，目前尚不具备可以满足井下自主移动设备定位与导航的专用地图构建方法及系统。

发明内容

本发明所要解决的技术问题是提供一种能够辅助井下自主移动设备，实现精确定位、辅助路径规划与导航的井下高精度导航地图构建系统。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种井下高精度导航地图构建系统，用于实现井下多层次地图模型的构建，包括井下地理信息系统、本质安全型建模装置、以及布设于井下各巷道的各个本质安全型超宽带雷达UWB锚节点；

其中，本质安全型建模装置包括激光雷达、超宽带雷达UWB模块、里程计、惯性导航模块、第一矿用本安型防爆电源、第二矿用本安型防爆电源、工控机、无线网桥、RS232信号隔离栅装置和网络信号隔离栅装置；井下地理信息系统设置于本质安全型建模装置中的工控机当中；第一矿用本安型防爆电源分别为激光雷达、超宽带雷达UWB模块、里程计、惯性导航模块、RS232信号隔离栅装置、网络信号隔离栅装置进行供电；第二矿用本安型防爆电源分别为工控机、无线网桥进行供电；里程计、惯性导航模块分别通过RS232信号隔离栅装置与所述工控机相连接；激光雷达、超宽带雷达UWB模块分别通过网络信号隔离栅装置与所述工控机相连接，无线网桥与工控机相连接；

基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中的超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点进行观测交互，本质安全型建模装置中工控机根据该观测交互信息、以及其所连激光雷达、里程计、惯性导航模块的实时数据，构建高精度点云模型；

本质安全型建模装置中工控机应用井下地理信息系统，同时实时构建高精度地图模型，进而通过结合地理信息系统的电子地图与高精度点云模型，构建井下多层次地图模型。

作为本发明的一种优选技术方案：所述本质安全型建模装置还包括电源电压转换模块，所述第一矿用本安型防爆电源经过电源电压转换模块分别为里程计、惯性导航模块、RS232信号隔离栅装置、网络信号隔离栅装置进行供电。

作为本发明的一种优选技术方案：还包括布设于井下各巷道的各个反射标记，用于确定所述井下多层次地图模型的精度。

与上述相对应，本发明还要解决的技术问题是提供一种基于井下高精度导航地图构建系统的构建方法，能够辅助井下自主移动设备，实现精确定位、辅助路径规划与导航，并具有高效的工作效率。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种基于井下高精度导航地图构建系统的构建方法，所述本质安全型建模装置中工控机中存储井下GIS电子地图，实时执行如下构建方法步骤：

步骤A.针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设，并获得各个本质安全型超宽带雷达UWB锚节点的绝对位置信息，存储于本质安全型建模装置中工控机中的井下GIS电子地图中，然后进入步骤B；

步骤B.基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中工控机依次做如下各操作：

基于井下GIS电子地图，通过本质安全型建模装置中超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点的观测交互，应用UWB无线定位模型，获得本质安全型建模装置在井下GIS电子地图中的绝对位姿，构建因子图中绝对位姿观测约束信息X；

利用里程计与惯性导航模块应用融合所构航迹推算模型计算的相对位姿变换结果，提供航迹推算运动约束，构建因子图中运动约束边U；

利用激光雷达对巷道壁扫描所获模型，计算相对位姿变换结果提供的约束，构建因子图中扫描匹配观测约束边Z；

利用巷道点云分割获得的平面结果提供平面约束，构建因子图中平面观测顶点Q；

利用回环检测结果提供相对位姿变换约束，构建因子图中回环约束边L；

步骤C.伴随本质安全型建模装置在井下各巷道中的移动，基于因子图中各类约束信息，进行多传感器SLAM过程，并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图，然后进入步骤D；

步骤D.将优化点云地图与井下GIS电子地图进行对齐，并进行栅格化处理；接着根据使用需求构建三维栅格地图或二维栅格地图，为巷道中障碍物及局部环境设定权值，然后结合井下GIS电子地图，针对栅格地图，构建井下多层次地图模型，再进入步骤E；

步骤E.应用利用点云分割与目标识别方法，判断井下巷道是否发生结构变化，是则针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设，并返回步骤B；否则即获得井下多层次地图模型。

作为本发明的一种优选技术方案：所述步骤A中还包括，针对井下各巷道，进行各个反射标记的布设，并获得各个反射标记的绝对位置信息，存储于本质安全型建模装置中工控机中的井下GIS电子地图中；

还包括步骤D-E如下，执行完步骤D后，进入步骤D-E，执行完步骤D-E后，进入步骤E；

步骤D-E.基于多层次地图模型，针对布设于井下各巷道的各个反射标记，计算各反射标记之间的真实距离、以及其在点云地图上的距离，进而获得多层次地图模型的精度。

作为本发明的一种优选技术方案，所述步骤A包括如下步骤：

步骤A1.利用矿井建设或井下GIS电子地图时所使用的控制导线点，作为基准控制点，并根据基准控制点，结合预设建模精度，利用全站仪测绘确定井下各巷道中的控制点，结合各基准控制点，作为井下各巷道中的控制点，然后进入步骤A2；

步骤A2.根据UWB信号传输要求、以及信号覆盖范围，针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设，同时针对巷道空间地理结构变化的分叉、风门位置，进行各个反射标记的布设，并基于各个控制点，利用全站仪测量获得各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，然后进入步骤A3；

步骤A3.将各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，标注于本质安全型建模装置中工控机中的井下GIS电子地图中。

作为本发明的一种优选技术方案，所述步骤B中，基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中工控机依次做如下各步骤：

步骤B1.基于TOA原理，利用同时观测所获至少四颗本质安全型超宽带雷达UWB锚节点的信息，构建UWB无线定位模型h_g(·)，并结合UWB无线定位观测信息g_i及噪声协方差矩阵构建本质安全型建模装置绝对位姿x_i的位姿约束信息，即构建因子图中绝对位姿观测约束信息G；

步骤B2.利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算的航迹相对位姿变换结果u_j、以及满足正态分布的航迹噪声协方差矩阵提供航迹推算运动约束，构建因子图中运动约束边U；

步骤B3.利用激光雷达针对巷道壁进行扫描，构建激光点云配准测量模型h_k(·)，并应用GICP方法进行相邻帧点云配准，计算点云间相对位姿变换结果z_k、以及满足正态分布的点云间噪声协方差矩阵并构建因子图中扫描匹配观测约束边Z；

步骤B4.利用RANSAC点云分割方法检测巷道的平面结构，构建因子图中平面观测顶点P，利用平面法向量和垂直距离构建平面约束测量模型h_v(·)，获得各个平面约束测量信息v_m，并计算观测约束不确定度作为信息矩阵；

步骤B5.利用Appearance-based激光点云回环检测方法，构建回环检测约束测量模型h_l(·)，计算回环关键帧之间的回环帧相对位姿变换结果l_n、以及满足正态分布的回环帧噪声协方差矩阵并构建因子图中回环约束边L。

作为本发明的一种优选技术方案，所述步骤C包括如下步骤：

步骤C1.伴随本质安全型建模装置在井下各巷道中的移动，进行多传感器SLAM过程，利用高斯随机变量模拟因子图中各类约束信息，构建位姿和约束的联合概率分布如下，然后进入步骤C2；

其中，I表示随本质安全型建模装置井下移动、同时观测到至少四颗本质安全型超宽带雷达UWB锚节点的实时次数；J表示随本质安全型建模装置井下移动、利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算航迹相对位姿变换结果的次数；K表示随本质安全型建模装置井下移动、基于激光点云配准测量模型h_k(·)，应用GICP方法进行相邻帧点云配准的次数；M表示随本质安全型建模装置井下移动、平面约束测量信息的个数；N表示随本质安全型建模装置井下移动、其移动路径出现回环的次数；x_i,x_j,x_j-1,x_k,x_k+1,x_m,x_n分别表示各约束信息所对应的本质安全型建模装置绝对位姿；

步骤C2.针对位姿和约束的联合概率分布，按如下公式，利用最大后验估计求解：

并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图。

作为本发明的一种优选技术方案，所述步骤D包括如下步骤：

步骤D1.根据各本质安全型超宽带雷达UWB锚节点、各反射标记在地理信息系统中的绝对位置信息、以及在点云模型中的相对位置信息，将优化点云地图与井下GIS电子地图进行对齐，获得统一坐标系的两层次地图模型，然后进入步骤D2；

步骤D2.针对二维平面内导航需求，将优化点云地图转换到二维高程图，存储为二维占据栅格地图；针对三维空间内导航需求，利用Octree数据结构，将优化点云地图存储为三维占据栅格地图，然后进入步骤D3；

步骤D3.根据井下GIS电子地图中井下巷道空间数据，结合步骤D2生成的栅格地图，实时为巷道中障碍物及局部环境设定权值，构建井下多层次地图模型。

作为本发明的一种优选技术方案，所述步骤E中，基于井下巷道发生结构变化，按如下方法，针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设；

步骤E1.利用全站仪测量获得已布设本质安全型超宽带雷达UWB锚节点，作为已布设置节点，并配置为广播信号模式；将准备加入到网络中的待定位本质安全型超宽带雷达UWB锚节点配置为发射和接受模式，然后进入步骤E2；

步骤E2.根据应用需求判断需要二维定位或三维定位，确定相对定位参与节点位置，即部署新加入本质安全型超宽带雷达UWB锚节点的空间位置，且对于三维定位需求，新加入本质安全型超宽带雷达UWB锚节点的位置，需满足同时检测到4个以上本质安全型超宽带雷达UWB锚节点信号的位置，然后进入步骤E3；

步骤E3.基于已布设本质安全型超宽带雷达UWB锚节点，利用TDOA原理，确定新加入本质安全型超宽带雷达UWB锚节点的位置，实现井下新巷道中本质安全型超宽带雷达UWB锚节点的布设。

本发明所述一种井下高精度导航地图构建系统及构建方法采用以上技术方案与现有技术相比，具有以下技术效果：

1)本发明所设计井下高精度导航地图构建系统及构建方法，建模精度高，利用基于优化的方法，充分利用UWB无线传感器网络提供的绝对位姿观测约束、航迹推算模型的运动约束边、激光点云配准的扫描匹配观测约束边、平面约束测量、回环约束边，得到一致性和精度极高的环境点云地图，克服了井下由于场景退化带来的建模精度差的问题，利用非线性优化求解最大后验估计过程中的稀疏性，可以满足实时计算的要求；

2)本发明所设计井下高精度导航地图构建系统及构建方法，定位精度高，在构建静态地图后，利用全站仪确定本质安全型超宽带雷达UWB锚节点与反射标记的绝对位置，容易实现将GIS系统的电子地图、构建的高精度地图与UWB锚节点、人工标志进行坐标系统一。提供了UWB锚节点动态扩展的方法，根据环境特点、工作条件及使用需求还可以采用相对定位方法确定UWB锚节点位置。UWB锚节点定位精度高，保证了移动建模模块本身的定位精度高，进一步保证环境建模精度以及依赖本发明进行定位的移动设备的定位精度。同时本发明也提供了判断建模精度的方法；

3)本发明所设计井下高精度导航地图构建系统及构建方法，地理信息系统强化建模精度、辅助决策规划：利用GIS系统提供的地理信息以及电子地图，与SLAM过程建立的地图进行匹配，实现了多层次的导航地图构建；GIS系统提供的电子拓扑地图用于全局路径规划，结合栅格地图的局部路径规划共同实现不同层次的规划任务；

4)本发明所设计井下高精度导航地图构建系统及构建方法，实现了动态更新环境模型，当巷道掘进或环境中出现长期障碍时，利用防爆型移动式建模模块在已有地图基础上进行环境地图更新；避免了传统方法重新使用防爆激光扫描仪进行单点建模、后期拼接的耗时过程，可以实时更新环境模型；

5)本发明所设计井下高精度导航地图构建系统及构建方法，构建了满足防爆要求的本质安全型建模装置，该模块可以完成增量式更新地图，或者实时更新已经变化的局部环境地图。同时该模块可以安装在各类井下移动装备，进行精确定位。

附图说明

图1是本发明中所设计本质安全型建模装置的内部电气原理图；

图2是因子图优化原理图；

图3是本质安全型超宽带雷达UWB锚节点相对定位网络原理图。

具体实施方式

下面结合说明书附图对本发明的具体实施方式作进一步详细的说明。

本发明设计了一种井下高精度导航地图构建系统，用于实现井下多层次地图模型的构建，包括井下地理信息系统、本质安全型建模装置、布设于井下各巷道的各个本质安全型超宽带雷达UWB锚节点、以及布设于井下各巷道的各个反射标记，用于确定所述井下多层次地图模型的精度。

其中，如图1所示，本质安全型建模装置包括激光雷达、超宽带雷达UWB模块、里程计、惯性导航模块、第一矿用本安型防爆电源、第二矿用本安型防爆电源、工控机、无线网桥、电源电压转换模块、RS232信号隔离栅装置、网络信号隔离栅装置、第一开关、第二开关；井下地理信息系统设置于本质安全型建模装置中的工控机当中；第一矿用本安型防爆电源经过第一开关后、一方面分别为激光雷达、超宽带雷达UWB装置进行供电，另一方面同时经过电源电压转换模块分别为里程计、惯性导航装置、RS232信号隔离栅装置、网络信号隔离栅装置进行供电；第二矿用本安型防爆电源经过第二开关分别为工控机、无线网桥进行供电；里程计、惯性导航模块分别通过RS232信号隔离栅装置与所述工控机相连接；激光雷达、超宽带雷达UWB模块分别通过网络信号隔离栅装置与所述工控机相连接，无线网桥与工控机相连接。

基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中的超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点进行观测交互，本质安全型建模装置中工控机根据该观测交互信息、以及其所连激光雷达、里程计、惯性导航模块的实时数据，构建高精度点云模型。

本质安全型建模装置中工控机应用井下地理信息系统，同时实时构建高精度地图模型，进而通过结合地理信息系统的电子地图与高精度点云模型，构建井下多层次地图模型。

基于上述所设计的井下高精度导航地图构建系统，本发明还设计提供一种基于井下高精度导航地图构建系统的构建方法，实际应用中，所述本质安全型建模装置中工控机中存储井下GIS电子地图，实时执行如下构建方法步骤。

步骤A.针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设、以及各个反射标记的布设，并获得各个本质安全型超宽带雷达UWB锚节点的绝对位置信息、以及各个反射标记的绝对位置信息，存储于本质安全型建模装置中工控机中的井下GIS电子地图中，然后进入步骤B。

实际应用中，步骤A具体包括如下步骤：

步骤A1.利用矿井建设或井下GIS电子地图时所使用的控制导线点，作为基准控制点，并根据基准控制点，结合预设建模精度，利用全站仪测绘确定井下各巷道中的控制点，结合各基准控制点，作为井下各巷道中的控制点，然后进入步骤A2。

步骤A2.根据UWB信号传输要求、以及信号覆盖范围，针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设，同时针对巷道空间地理结构变化的分叉、风门位置，进行各个反射标记的布设，并基于各个控制点，利用全站仪测量获得各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，然后进入步骤A3。

步骤A3.将各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，标注于本质安全型建模装置中工控机中的井下GIS电子地图中。

步骤B.基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中工控机依次做如下各操作。

基于井下GIS电子地图，通过本质安全型建模装置中超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点的观测交互，应用UWB无线定位模型，获得本质安全型建模装置在井下GIS电子地图中的绝对位姿，构建因子图中绝对位姿观测约束信息X。

利用里程计与惯性导航模块应用融合所构航迹推算模型计算的相对位姿变换结果，提供航迹推算运动约束，构建因子图中运动约束边U。

利用激光雷达对巷道壁扫描所获模型，计算相对位姿变换结果提供的约束，构建因子图中扫描匹配观测约束边Z。

利用巷道点云分割获得的平面结果提供平面约束，构建因子图中平面观测顶点Q。

利用回环检测结果提供相对位姿变换约束，构建因子图中回环约束边L。

上述步骤B在具体的执行中，具体如下：

步骤B中，基于本质安全型建模装置在井下各巷道中的移动，基于图2所示，本质安全型建模装置中工控机依次做如下各步骤：

步骤B1.基于TOA原理，利用同时观测所获至少四颗本质安全型超宽带雷达UWB锚节点的信息，构建UWB无线定位模型h_g(·)，并结合UWB无线定位观测信息g_i及噪声协方差矩阵构建本质安全型建模装置绝对位姿x_i的位姿约束信息，即构建因子图中绝对位姿观测约束信息G。

步骤B2.利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算的航迹相对位姿变换结果u_j、以及满足正态分布的航迹噪声协方差矩阵提供航迹推算运动约束，构建因子图中运动约束边U。

步骤B3.利用激光雷达针对巷道壁进行扫描，构建激光点云配准测量模型h_k(·)，并应用GICP(Generalized Iterative Closest Point)方法进行相邻帧点云配准，计算点云间相对位姿变换结果z_k、以及满足正态分布的点云间噪声协方差矩阵并构建因子图中扫描匹配观测约束边Z。

步骤B4.利用RANSAC(Random Sample Consensus)点云分割方法检测巷道的平面结构，构建因子图中平面观测顶点P，利用平面法向量和垂直距离构建平面约束测量模型h_v(·)，获得各个平面约束测量信息v_m，并计算观测约束不确定度作为信息矩阵。

步骤B5.利用Appearance-based激光点云回环检测方法，构建回环检测约束测量模型h_l(·)，计算回环关键帧之间的回环帧相对位姿变换结果l_n、以及满足正态分布的回环帧噪声协方差矩阵并构建因子图中回环约束边L。

步骤C.伴随本质安全型建模装置在井下各巷道中的移动，基于因子图中各类约束信息，进行多传感器SLAM过程，并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图，然后进入步骤D。

上述步骤C实际应用中，具体包括如下步骤：

步骤C1.伴随本质安全型建模装置在井下各巷道中的移动，进行多传感器SLAM过程，利用高斯随机变量模拟因子图中各类约束信息，构建位姿和约束的联合概率分布如下，然后进入步骤C2。

其中，I表示随本质安全型建模装置井下移动、同时观测到至少四颗本质安全型超宽带雷达UWB锚节点的实时次数；J表示随本质安全型建模装置井下移动、利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算航迹相对位姿变换结果的次数；K表示随本质安全型建模装置井下移动、基于激光点云配准测量模型h_k(·)，应用GICP方法进行相邻帧点云配准的次数；M表示随本质安全型建模装置井下移动、平面约束测量信息的个数；N表示随本质安全型建模装置井下移动、其移动路径出现回环的次数；x_i,x_j,x_j-1,x_k,x_k+1,x_m,x_n分别表示各约束信息所对应的本质安全型建模装置绝对位姿。

步骤C2.针对位姿和约束的联合概率分布，按如下公式，利用最大后验估计求解：

并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图。

步骤D.将优化点云地图与井下GIS电子地图进行对齐，并进行栅格化处理；接着根据使用需求构建三维栅格地图或二维栅格地图，为巷道中障碍物及局部环境设定权值，然后结合井下GIS电子地图，针对栅格地图，构建井下多层次地图模型，再进入步骤D-E。

实际应用中，步骤D具体包括如下步骤：

步骤D1.根据各本质安全型超宽带雷达UWB锚节点、各反射标记在地理信息系统中的绝对位置信息、以及在点云模型中的相对位置信息，将优化点云地图与井下GIS电子地图进行对齐，获得统一坐标系的两层次地图模型，然后进入步骤D2。

步骤D2.针对二维平面内导航需求，将优化点云地图转换到二维高程图，存储为二维占据栅格地图；针对三维空间内导航需求，利用Octree数据结构，将优化点云地图存储为三维占据栅格地图，然后进入步骤D3。

步骤D3.根据井下GIS电子地图中井下巷道空间数据，结合步骤D2生成的栅格地图，实时为巷道中障碍物及局部环境设定权值，构建井下多层次地图模型，并应用井下地理信息系统的电子地图，提供高层次全局路径规划能力，应用栅格地图提供低层次局部路径规划与避障能力。

步骤D-E.基于多层次地图模型，针对布设于井下各巷道的各个反射标记，计算各反射标记之间的真实距离、以及其在点云地图上的距离，进而获得多层次地图模型的精度，然后进入步骤E。

步骤E.应用利用点云分割与目标识别方法，判断井下巷道是否发生结构变化，是则针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设，并返回步骤B；否则即获得井下多层次地图模型。

上述步骤E中，基于井下巷道发生结构变化，如图3所示，按如下方法，针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设。

步骤E1.利用全站仪测量获得已布设本质安全型超宽带雷达UWB锚节点，作为已布设置节点，并配置为广播信号模式；将准备加入到网络中的待定位本质安全型超宽带雷达UWB锚节点配置为发射和接受模式，然后进入步骤E2。

步骤E2.根据应用需求判断需要二维定位(3自由度，2个平移x、y及1个旋转yaw)或三维定位(6自由度，三个平移x、y、z及三个旋转roll、pitch、yaw)，确定相对定位参与节点位置，即部署新加入本质安全型超宽带雷达UWB锚节点的空间位置，且对于三维定位需求，新加入本质安全型超宽带雷达UWB锚节点的位置，需满足同时检测到4个以上本质安全型超宽带雷达UWB锚节点信号的位置，然后进入步骤E3。

步骤E3.基于已布设本质安全型超宽带雷达UWB锚节点，利用TDOA原理，确定新加入本质安全型超宽带雷达UWB锚节点的位置，实现井下新巷道中本质安全型超宽带雷达UWB锚节点的布设。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (10)

1.一种井下高精度导航地图构建系统，用于实现井下多层次地图模型的构建，其特征在于，包括井下地理信息系统、本质安全型建模装置、以及布设于井下各巷道的各个本质安全型超宽带雷达UWB锚节点；

其中，本质安全型建模装置包括激光雷达、超宽带雷达UWB模块、里程计、惯性导航模块、第一矿用本安型防爆电源、第二矿用本安型防爆电源、工控机、无线网桥、RS232信号隔离栅装置和网络信号隔离栅装置；井下地理信息系统设置于本质安全型建模装置中的工控机当中；第一矿用本安型防爆电源分别为激光雷达、超宽带雷达UWB模块、里程计、惯性导航模块、RS232信号隔离栅装置、网络信号隔离栅装置进行供电；第二矿用本安型防爆电源分别为工控机、无线网桥进行供电；里程计、惯性导航模块分别通过RS232信号隔离栅装置与所述工控机相连接；激光雷达、超宽带雷达UWB模块分别通过网络信号隔离栅装置与所述工控机相连接，无线网桥与工控机相连接；

基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中的超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点进行观测交互，本质安全型建模装置中工控机根据该观测交互信息、以及其所连激光雷达、里程计、惯性导航模块的实时数据，构建高精度点云模型；

本质安全型建模装置中工控机应用井下地理信息系统，同时实时构建高精度地图模型，进而通过结合地理信息系统的电子地图与高精度点云模型，构建井下多层次地图模型。

2.根据权利要求1所述一种井下高精度导航地图构建系统，其特征在于：所述本质安全型建模装置还包括电源电压转换模块，所述第一矿用本安型防爆电源经过电源电压转换模块分别为里程计、惯性导航模块、RS232信号隔离栅装置、网络信号隔离栅装置进行供电。

3.根据权利要求2所述一种井下高精度导航地图构建系统，其特征在于：还包括布设于井下各巷道的各个反射标记，用于确定所述井下多层次地图模型的精度。

4.一种基于权利要求1至3中任意一项所述井下高精度导航地图构建系统的构建方法，其特征在于，所述本质安全型建模装置中工控机中存储井下GIS电子地图，实时执行如下构建方法步骤：

步骤A.针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设，并获得各个本质安全型超宽带雷达UWB锚节点的绝对位置信息，存储于本质安全型建模装置中工控机中的井下GIS电子地图中，然后进入步骤B；

步骤B.基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中工控机依次做如下各操作：

基于井下GIS电子地图，通过本质安全型建模装置中超宽带雷达UWB模块与各巷道中各本质安全型超宽带雷达UWB锚节点的观测交互，应用UWB无线定位模型，获得本质安全型建模装置在井下GIS电子地图中的绝对位姿，构建因子图中绝对位姿观测约束信息X；

利用里程计与惯性导航模块应用融合所构航迹推算模型计算的相对位姿变换结果，提供航迹推算运动约束，构建因子图中运动约束边U；

利用激光雷达对巷道壁扫描所获模型，计算相对位姿变换结果提供的约束，构建因子图中扫描匹配观测约束边Z；

利用巷道点云分割获得的平面结果提供平面约束，构建因子图中平面观测顶点Q；

利用回环检测结果提供相对位姿变换约束，构建因子图中回环约束边L；

步骤C.伴随本质安全型建模装置在井下各巷道中的移动，基于因子图中各类约束信息，进行多传感器SLAM过程，并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图，然后进入步骤D；

步骤D.将优化点云地图与井下GIS电子地图进行对齐，并进行栅格化处理；接着根据使用需求构建三维栅格地图或二维栅格地图，为巷道中障碍物及局部环境设定权值，然后结合井下GIS电子地图，针对栅格地图，构建井下多层次地图模型，再进入步骤E；

步骤E.应用利用点云分割与目标识别方法，判断井下巷道是否发生结构变化，是则针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设，并返回步骤B；否则即获得井下多层次地图模型。

5.根据权利要求4所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于：所述步骤A中还包括，针对井下各巷道，进行各个反射标记的布设，并获得各个反射标记的绝对位置信息，存储于本质安全型建模装置中工控机中的井下GIS电子地图中；

还包括步骤D-E如下，执行完步骤D后，进入步骤D-E，执行完步骤D-E后，进入步骤E；步骤D-E.基于多层次地图模型，针对布设于井下各巷道的各个反射标记，计算各反射标记之间的真实距离、以及其在点云地图上的距离，进而获得多层次地图模型的精度。

6.根据权利要求5所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于，所述步骤A包括如下步骤：

步骤A1.利用矿井建设或井下GIS电子地图时所使用的控制导线点，作为基准控制点，并根据基准控制点，结合预设建模精度，利用全站仪测绘确定井下各巷道中的控制点，结合各基准控制点，作为井下各巷道中的控制点，然后进入步骤A2；

步骤A2.根据UWB信号传输要求、以及信号覆盖范围，针对井下各巷道，进行各个本质安全型超宽带雷达UWB锚节点的布设，同时针对巷道空间地理结构变化的分叉、风门位置，进行各个反射标记的布设，并基于各个控制点，利用全站仪测量获得各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，然后进入步骤A3；

步骤A3.将各本质安全型超宽带雷达UWB锚节点、各反射标记的绝对位置信息，标注于本质安全型建模装置中工控机中的井下GIS电子地图中。

7.根据权利要求5所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于，所述步骤B中，基于本质安全型建模装置在井下各巷道中的移动，本质安全型建模装置中工控机依次做如下各步骤：

步骤B1.基于TOA原理，利用同时观测所获至少四颗本质安全型超宽带雷达UWB锚节点的信息，构建UWB无线定位模型h_g(·)，并结合UWB无线定位观测信息g_i及噪声协方差矩阵构建本质安全型建模装置绝对位姿x_i的位姿约束信息，即构建因子图中绝对位姿观测约束信息G；

步骤B2.利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算的航迹相对位姿变换结果u_j、以及满足正态分布的航迹噪声协方差矩阵提供航迹推算运动约束，构建因子图中运动约束边U；

步骤B3.利用激光雷达针对巷道壁进行扫描，构建激光点云配准测量模型h_k(·)，并应用GICP方法进行相邻帧点云配准，计算点云间相对位姿变换结果z_k、以及满足正态分布的点云间噪声协方差矩阵并构建因子图中扫描匹配观测约束边Z；

步骤B4.利用RANSAC点云分割方法检测巷道的平面结构，构建因子图中平面观测顶点P，利用平面法向量和垂直距离构建平面约束测量模型h_v(·)，获得各个平面约束测量信息v_m，并计算观测约束不确定度作为信息矩阵；

步骤B5.利用Appearance-based激光点云回环检测方法，构建回环检测约束测量模型h_l(·)，计算回环关键帧之间的回环帧相对位姿变换结果l_n、以及满足正态分布的回环帧噪声协方差矩阵并构建因子图中回环约束边L。

8.根据权利要求7所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于，所述步骤C包括如下步骤：

步骤C1.伴随本质安全型建模装置在井下各巷道中的移动，进行多传感器SLAM过程，利用高斯随机变量模拟因子图中各类约束信息，构建位姿和约束的联合概率分布如下，然后进入步骤C2；

其中，I表示随本质安全型建模装置井下移动、同时观测到至少四颗本质安全型超宽带雷达UWB锚节点的实时次数；J表示随本质安全型建模装置井下移动、利用里程计与惯性导航模块应用融合所构航迹推算模型f_j(·)计算航迹相对位姿变换结果的次数；K表示随本质安全型建模装置井下移动、基于激光点云配准测量模型h_k(·)，应用GICP方法进行相邻帧点云配准的次数；M表示随本质安全型建模装置井下移动、平面约束测量信息的个数；N表示随本质安全型建模装置井下移动、其移动路径出现回环的次数；x_i,x_j,x_j-1,x_k,x_k+1,x_m,x_n分别表示各约束信息所对应的本质安全型建模装置绝对位姿；

步骤C2.针对位姿和约束的联合概率分布，按如下公式，利用最大后验估计求解：

并利用本质安全型建模装置的绝对位姿更新点云地图，获得优化点云地图。

9.根据权利要求5所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于，所述步骤D包括如下步骤：

步骤D1.根据各本质安全型超宽带雷达UWB锚节点、各反射标记在地理信息系统中的绝对位置信息、以及在点云模型中的相对位置信息，将优化点云地图与井下GIS电子地图进行对齐，获得统一坐标系的两层次地图模型，然后进入步骤D2；

步骤D2.针对二维平面内导航需求，将优化点云地图转换到二维高程图，存储为二维占据栅格地图；针对三维空间内导航需求，利用Octree数据结构，将优化点云地图存储为三维占据栅格地图，然后进入步骤D3；

步骤D3.根据井下GIS电子地图中井下巷道空间数据，结合步骤D2生成的栅格地图，实时为巷道中障碍物及局部环境设定权值，构建井下多层次地图模型。

10.根据权利要求5所述一种基于井下高精度导航地图构建系统的构建方法，其特征在于，所述步骤E中，基于井下巷道发生结构变化，按如下方法，针对井下新巷道，进行本质安全型超宽带雷达UWB锚节点布设；

步骤E1.利用全站仪测量获得已布设本质安全型超宽带雷达UWB锚节点，作为已布设置节点，并配置为广播信号模式；将准备加入到网络中的待定位本质安全型超宽带雷达UWB锚节点配置为发射和接受模式，然后进入步骤E2；

步骤E2.根据应用需求判断需要二维定位或三维定位，确定相对定位参与节点位置，即部署新加入本质安全型超宽带雷达UWB锚节点的空间位置，且对于三维定位需求，新加入本质安全型超宽带雷达UWB锚节点的位置，需满足同时检测到4个以上本质安全型超宽带雷达UWB锚节点信号的位置，然后进入步骤E3；

步骤E3.基于已布设本质安全型超宽带雷达UWB锚节点，利用TDOA原理，确定新加入本质安全型超宽带雷达UWB锚节点的位置，实现井下新巷道中本质安全型超宽带雷达UWB锚节点的布设。