本发明涉及激光雷达定位技术领域，尤其涉及一种井下有限区域移动设备定位系统。
背景技术：
在社会生产生活、工业施工中，巷道这一场景很常见且条件复杂，主要表现为路面不平整、光照不充足、无gps信号等。
移动机器人的定位方法大致可分为两类，一类传感器是携带于机器人本体上的，例如轮式编码器、相机、激光雷达等，通过运动模型估计机器人本体位置。另一类是安装于环境之中，例如地面架设导轨、墙壁安装用于识别的二维码等，通过测量到标志物的距离进行定位。
巷道中地面移动机器人和其他设备的定位研究很多，如掘进机、钻孔机等。
现有技术的缺点包括：井下巷道内通信条件差，在没有预先架设通信设备的情况下，利用携带于机器人本体传感器进行定位、建图的信息存储于机器人本体，不能实时传输到控制中心。当机器人出现故障时，不利于实现操作人员远程实时监控和操控。以自身传感器获得的信息为起点开始进行估计，整个过程定位的准确性会随着时间的推移累积误差，无人干预的情况下，累积的误差会没有上限。可见传统的井下有限区域移动设备定位往往存在实时性差、误差大的问题。
技术实现要素：
针对以上问题，本发明提出一种井下有限区域移动设备定位系统。
为实现本发明的目的，提供一种井下有限区域移动设备定位系统，包括三维激光雷达、雷达旋转机构、以及数据处理器；
所述雷达旋转机构控制所述三维激光雷达绕竖直方向旋转，所述三维激光雷达在旋转过程中扫描目标物，获得点云数据，将点云数据发送至所述数据处理器；
所述数据处理器对点云数据进行预处理，以减少数据冗余，从预处理后的点云数据中分割出移动设备数据，识别移动设备数据的设备中心坐标，将设备中心坐标从激光雷达坐标系转换至世界坐标系，根据转换后的设备中心坐标确定移动设备的位置。
在一个实施例中，井下有限区域移动设备定位系统，还包括监控终端；
所述监控终端获取数据处理器确定的移动设备的位置，以供用户读取。
在一个实施例中，所述预处理后的点云数据为p′n＝(x′n,y′n,z′n)；其中x′n表示预处理后的第一维点云数据坐标，y′表示预处理后的第二维点云数据坐标，z′表示预处理后的第三维点云数据坐标，xmin≤x′n≤xmax，ymin≤y′n≤ymax，zmin≤z′n≤zmax，dmin＝{xmin,ymin,zmin}表示移动设备所在区域的最小距离，dmax＝{xmax,ymax,zmax}表示移动设备所在区域的最大距离。
在一个实施例中，所述从预处理后的点云数据中分割出移动设备数据的过程包括：
采用基于强度的方法进行分割，根据点云数据中各像素对应的返回强度值不同，将属于移动设备的点云数据从整体点云数据中区分出来，得到移动设备数据。
在一个实施例中，将设备中心坐标从激光雷达坐标系转换至世界坐标系的过程包括：
采用齐次变换公式将设备中心坐标从激光雷达坐标系转换至世界坐标系。
作为一个实施例，所述齐次变换公式包括：
其中，r表示激光雷达坐标系至世界坐标系的平移矩阵，d表示激光雷达坐标系至世界坐标系的旋转矩阵。
在一个实施例中，设定数量的三维激光雷达布置在巷道的顶部，三维激光雷达通过旋转机构不断扫描巷道环境及环境中的物体，得到点云数据发送至所述数据处理器；所述数据处理器从点云数据中识别并分割出移动设备数据，得到移动设备数据到固定的激光雷达的距离信息而实现定位。
上述井下有限区域移动设备定位系统中，通过雷达旋转机构控制三维激光雷达绕竖直方向旋转，使三维激光雷达在旋转过程中扫描目标物，获得点云数据，将点云数据发送至所述数据处理器；所述数据处理器对点云数据进行预处理，以减少数据冗余，从预处理后的点云数据中分割出移动设备数据，识别移动设备数据的设备中心坐标，将设备中心坐标从激光雷达坐标系转换至世界坐标系，根据转换后的设备中心坐标确定移动设备的位置，以实时、准确地对井下有限区域的移动设备进行定位。
附图说明
图1是一个实施例的井下有限区域移动设备定位系统结构示意图；
图2是一个实施例的工作原理示意图；
图3是一个实施例的三维雷达模块示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
参考图1所示，图1为一个实施例的井下有限区域移动设备定位系统流程图，包括三维激光雷达11、雷达旋转机构12、以及数据处理器13；
所述雷达旋转机构12控制所述三维激光雷达11绕竖直方向(铅锤方向)旋转，所述三维激光雷达11在旋转过程中扫描目标物(如需要被定位的移动设备)，获得点云数据，将点云数据发送至所述数据处理器13；
所述数据处理器13对点云数据进行预处理，以减少数据冗余，从预处理后的点云数据中分割出移动设备数据，识别移动设备数据的设备中心坐标，将设备中心坐标从激光雷达坐标系转换至世界坐标系，根据转换后的设备中心坐标确定移动设备的位置。
上述需要被定位的移动设备可以为井下有限区域的移动机器人。
具体地，可以将设定数量的三维激光雷达布置在巷道的顶部，三维激光雷达通过旋转机构不断扫描巷道环境及环境中的物体，得到点云数据发送至所述数据处理器；所述数据处理器从点云数据中识别并分割出移动设备数据，得到移动设备数据到固定的激光雷达的距离信息而实现定位。
上述井下有限区域移动设备定位系统中，通过雷达旋转机构12控制三维激光雷达11绕竖直方向旋转，使三维激光雷达11在旋转过程中扫描目标物，获得点云数据，将点云数据发送至所述数据处理器13；所述数据处理器13对点云数据进行预处理，以减少数据冗余，从预处理后的点云数据中分割出移动设备数据，识别移动设备数据的设备中心坐标，将设备中心坐标从激光雷达坐标系转换至世界坐标系，根据转换后的设备中心坐标确定移动设备的位置，以实时、准确地对井下有限区域的移动设备进行定位。
在一个实施例中，井下有限区域移动设备定位系统，还包括监控终端；
所述监控终端获取数据处理器确定的移动设备的位置，以供用户读取。
本实施例可以使用户及时获知移动设备的位置。
具体地，上述井下有限区域移动设备定位系统，包括硬件设备层、算法处理层、和用户层。参考图2所示，硬件设备层包含井下有限区域移动设备定位系统所需的全部硬件，如图2所示的三维激光雷达、旋转机构、以及数据处理器，以获取数据，并将数据传输给算法处理单元。算法处理层将从设备层获取的数据进行处理，通过对点云的识别、分割，获得巷道环境建模和移动机器人定位结果。用户层(如图2所示监控终端)可以接收算法处理层的解算结果，并显示给使用者(用户)，用于监控终端的显示以及操控，并提供开放的可进一步开发的api接口。
用户层接收算法处理层的定位解算结果，将移动机器人的三维坐标，传输到监控终端，显示实时的定位结果和运动轨迹。进一步的用户层还包括数据库接口，可以保存日常数据用于后续分析。
具体地，硬件设备层中可以设置三维雷达模块，包括三维激光雷达、三维激光雷达旋转机构、三维激光雷达数据处理器，用于获取下一步算法处理层所需的原始数据，具体为三维激光雷达输出的点云(包含目标点距离激光雷达坐标原点的距离、角度、信号强度)。其中三维激光雷达旋转机构使三维激光雷达能够绕z轴(铅锤方向)旋转，能够扫描到更大角度范围内的目标物。
在一个示例中，如图3示三维雷达模块包括三维激光雷达1-1、三维激光雷达旋转机构1-2、和三维激光雷达数据处理器1-3，用于获取下一步算法处理层所需的原始数据，具体为三维激光雷达输出的点云(包含目标点距离激光雷达坐标原点的距离、角度、信号强度)。其中三维激光雷达旋转机构使三维激光雷达能够绕z轴(铅锤方向)旋转，能够扫描到更大角度范围内的目标物。
进一步的，可以在巷道的顶部沿着巷道纵深的方向悬挂三维雷达模块，三维雷达模块到两侧巷道壁的距离相等，所有三维雷达模块到巷道地面的距离相同，任意两个相邻三维激光雷达的距离相同且不大于最佳测量范围，保证任意两个三维激光雷达扫描范围有一定重合范围、不出现扫描空区。
在一个实施例中，所述预处理后的点云数据为p′n＝(x′n,y′n,z′n)；其中x′n表示预处理后的第一维点云数据坐标，y′表示预处理后的第二维点云数据坐标，z′表示预处理后的第三维点云数据坐标，xmin≤x′n≤xmax，ymin≤y′n≤ymax，zmin≤z′n≤zmax，dmin＝{xmin,ymin,zmin}表示移动设备所在区域的最小距离，dmax＝{xmax,ymax,zmax}表示移动设备所在区域的最大距离。
本实施例可以对点云数据进行点云预处理，以减少数据冗余。
在一个实施例中，所述从预处理后的点云数据中分割出移动设备数据的过程包括：
采用基于强度的方法进行分割，根据点云数据中各像素对应的返回强度值不同，将属于移动设备的点云数据从整体点云数据中区分出来，得到移动设备数据。
在一个实施例中，将设备中心坐标从激光雷达坐标系转换至世界坐标系的过程包括：
采用齐次变换公式将设备中心坐标从激光雷达坐标系转换至世界坐标系。
作为一个实施例，所述齐次变换公式包括：
其中，r表示激光雷达坐标系至世界坐标系的平移矩阵，d表示激光雷达坐标系至世界坐标系的旋转矩阵，h表示齐次变换公式。
采用齐次变换公式将设备中心坐标从激光雷达坐标系转换至世界坐标系的过程可以包括：
ptn'＝hptn，
ptn表示转换前的设备中心坐标，ptn'表示转换前的设备中心坐标。
具体地，算法处理层的流程包括点云预处理、点云分割和坐标变换。点云预处理对原始激光雷达数据做滤波处理，减少数据冗余；预处理得到点云数据，根据返回强度的不同将移动机器人从整体点云中分割出来；坐标变换将移动机器人相对激光雷达坐标系的变换关系，转换到世界坐标系下。
进一步地，点云预处理采用带通滤波和体素体滤波结合的方式，在保证精确度的前提下减少计算力要求。其中带通滤波器将过远或过近的点云数据过滤掉。设原始点云为s＝{p1，p2，p3，…，pn}，其中点pn＝(xn,yn,zn)满足xn∈x，yn∈y，zn∈z，x表示第一维点云数据坐标，y表示第一维点云数据坐标，z表示第一维点云数据坐标。经滤波后的点云s′＝{p′1，p′2，p′3，…，p′n}，其中点pn′＝(x′n,y′n,z′n)满足：
实际中根据实地环境设置最小距离dmin和最大距离dmax，其中dmin＝{xmin,ymin,zmin}、dmax＝{xmax,ymax,zmax}，针对不同的巷道截面尺寸设置dmin和dmax，实现系统对不同尺寸巷道的适应性和可调节性。然后将带通滤波器处理过的激光雷达点云数据按照固定尺寸划分为立方体，处于同一体素体的所有点只保留处于立方体体心的点，这样可以进一步减少数据冗余，提高数据处理速度。
点云分割采用基于强度的方法进行分割，三维激光雷达扫描的点云信息包含点的强度信息，根据不同物体返回强度值不同将属于移动机器人的点云从整体点云中区分出来，实现移动机器人的识别。
进一步地，计算属于移动机器人的点云的中心，并以中心为原点建立移动机器人坐标系。
进一步地世界坐标系为t1，三维激光雷达坐标系为tn，三维激光雷达返回的移动机器人中心点ptn＝(xtn,ytn,ztn)。三维激光雷达按照上述方式布置时，其在世界坐标系中的位置是可以提前准确标定的，相应的变换关系：平移矩阵为r、旋转矩阵为d。通过坐标变换就能够将中心点ptn转换到界坐标系中，齐次变换公式为：
上述井下有限区域移动设备定位系统，针对现有技术的不足之处，立足于设计一种基于激光雷达适用于煤矿地下巷道中的移动机器人定位系统。该系统及相应的工作方法根据激光雷达的特性，并结合煤矿井下巷道的结构特点，将激光雷达固定于巷道顶部，通过将移动机器人从环境点云中识别并分割出来，从而实现移动机器人等移动设备的定位，并根据定位结果实现监控和控制的需求。
具体的实现原理可以包括：
不同于传统的移动机器人自身携带激光雷达传感器通过扫描匹配定位的方式，本发明将一定数量的激光雷达布置在巷道的顶部，激光雷达通过特定的旋转机构不断扫描巷道环境及环境中的物体，通过从激光雷达扫描得到的点云中识别并分割出移动机器人，得到移动机器人到固定的激光雷达的距离信息而实现定位。
上述系统是一种基于激光雷达的煤矿井下巷道中移动机器人的定位，系统包括底层硬件设备层、算法处理层、用户层。所述设备应用于煤矿井下时，需要考虑防爆安全要求，因此必须是本质安全型设备或做必要防爆处理。
所述硬件设备层包括通过布置合适的激光雷达传感器，以此获得实现可靠定位和环境构建所需的数据。
所述算法处理层将从设备层获取的数据进行处理获得巷道环境建模和移动机器人定位结果。
所述用户层接收算法处理层的结算结果，并显示给使用者，用于监控终端的显示以及操控，并提供开放的可进一步开发的api接口。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
需要说明的是，本申请实施例所涉及的术语“第一\第二\第三”仅仅是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二\第三”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二\第三”区分的对象在适当情况下可以互换，以使这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
本申请实施例的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或模块的过程、方法、装置、产品或设备没有限定于已列出的步骤或模块，而是可选地还包括没有列出的步骤或模块，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或模块。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。
1.一种井下有限区域移动设备定位系统，其特征在于，包括三维激光雷达、雷达旋转机构、以及数据处理器；
所述雷达旋转机构控制所述三维激光雷达绕竖直方向旋转，所述三维激光雷达在旋转过程中扫描目标物，获得点云数据，将点云数据发送至所述数据处理器；
所述数据处理器对点云数据进行预处理，以减少数据冗余，从预处理后的点云数据中分割出移动设备数据，识别移动设备数据的设备中心坐标，将设备中心坐标从激光雷达坐标系转换至世界坐标系，根据转换后的设备中心坐标确定移动设备的位置。
2.根据权利要求1所述的井下有限区域移动设备定位系统，其特征在于，还包括监控终端；
所述监控终端获取数据处理器确定的移动设备的位置，以供用户读取。
3.根据权利要求1所述的井下有限区域移动设备定位系统，其特征在于，所述预处理后的点云数据为p′n＝(x′n,y′n,z′n)；其中x′n表示预处理后的第一维点云数据坐标，y′表示预处理后的第二维点云数据坐标，z′表示预处理后的第三维点云数据坐标，xmin≤x′n≤xmax，ymin≤y′n≤ymax，zmin≤z′n≤zmax，dmin＝{xmin,ymin,zmin}表示移动设备所在区域的最小距离，dmax＝{xmax,ymax,zmax}表示移动设备所在区域的最大距离。
4.根据权利要求1所述的井下有限区域移动设备定位系统，其特征在于，所述从预处理后的点云数据中分割出移动设备数据的过程包括：
采用基于强度的方法进行分割，根据点云数据中各像素对应的返回强度值不同，将属于移动设备的点云数据从整体点云数据中区分出来，得到移动设备数据。
5.根据权利要求1至4任一项所述的井下有限区域移动设备定位系统，其特征在于，将设备中心坐标从激光雷达坐标系转换至世界坐标系的过程包括：
采用齐次变换公式将设备中心坐标从激光雷达坐标系转换至世界坐标系。
6.根据权利要求5所述的井下有限区域移动设备定位系统，其特征在于，所述齐次变换公式包括：
其中，r表示激光雷达坐标系至世界坐标系的平移矩阵，d表示激光雷达坐标系至世界坐标系的旋转矩阵。
7.根据权利要求1至4任一项所述的井下有限区域移动设备定位系统，其特征在于，设定数量的三维激光雷达布置在巷道的顶部，三维激光雷达通过旋转机构不断扫描巷道环境及环境中的物体，得到点云数据发送至所述数据处理器；所述数据处理器从点云数据中识别并分割出移动设备数据，得到移动设备数据到固定的激光雷达的距离信息而实现定位。
技术总结