基于红外运动捕捉的煤矿井下移动设备定位定姿系统

摘要

本发明公开了一种基于红外运动捕捉的煤矿井下移动设备定位定姿系统，包括红外姿态捕捉数据采集模块(2)、姿态校准模块(3)、移动设备刚体建模模块(4)、移动设备位姿解算模块(5)；所述红外姿态捕捉数据采集模块(2)布置在煤矿井下巷道(1‑1)两侧壁面上方，用于巷道内移动设备(1‑2)的位姿参数采集；所述姿态校准模块(3)用于定位定姿系统初始化的标定和校准；所述移动设备刚体建模模块(4)按照移动设备特征布置于移动设备(1‑2)表面，用于构建移动设备刚体模型(4‑2)；所述移动设备位姿解算模块(5)用于测量数据的处理，并对移动设备的位姿进行实时解算和显示。其突破现有定位技术在井下的环境限制问题。

说明书

技术领域

本发明涉及煤矿井下移动设备定位技术领域，尤其涉及一种基于红外运动捕捉的煤矿井下移动设备定位定姿系统。

背景技术

巷道是煤矿井下的典型环境，掘进机、煤矿机器人及运输车等为矿井巷道最常见的移动设备，普遍面临着工作环境恶劣，电磁环境复杂，高粉尘和低光照等难题，移动设备的精确位姿确定成为煤矿智能化发展的关键问题。诸多矿井移动设备组合导航措施被提出，已有煤矿井下移动设备定位方法通常基于惯性测量单元、倾角仪、超宽带节点、RFID、激光雷达、机器视觉、全站仪、里程计、磁力计或几种方式的联合方法进行，但现有基于惯性传感器的定位方式存在误差发散的不足，超宽带节点定位容易受巷道避免反射而产生的多径效应出现定位偏移，RFID定位误差较大无法适用于狭长巷道环境下的移动设备定位，激光雷达和全站仪的定位方式应用最为广泛但价格昂贵且需要频繁移动定位基站，机器视觉及工业相机对于高粉尘和低光照下的目标定位效果较差，里程计定位误差无法应对移动设备在井下湿滑路面出现的滑移现象，而磁力计和电子罗盘受井下复杂的电磁环境影响严重且定位精度低，基于多种定位方式组合的移动设备定位方法或可减弱部分环境影响，但仍尚无在定位精度、巷道环境和定位成本中取得一种较为平衡的位姿确定方式，能为实现煤矿智能化提供可靠的技术支撑。

发明内容

针对以上问题，本发明提出一种基于红外运动捕捉的煤矿井下移动设备定位定姿系统，能够解决现有技术中煤矿井下移动设备位姿无法实时准确估计的问题。

为实现本发明的目的，提供一种基于红外运动捕捉的煤矿井下移动设备定位定姿系统，包括红外姿态捕捉数据采集模块、姿态校准模块、移动设备刚体建模模块、移动设备位姿解算模块；

所述红外姿态捕捉数据采集模块布置在煤矿井下巷道两侧壁面上方，用于巷道内移动设备的位姿参数采集；所述姿态校准模块用于定位定姿系统初始化的标定和校准；所述移动设备刚体建模模块按照移动设备特征布置于移动设备表面，用于构建移动设备刚体模型；所述移动设备位姿解算模块用于测量数据的处理，并对移动设备的位姿进行实时解算和显示。

在一个实施例中，所述红外姿态捕捉数据采集模块包括多个红外捕捉镜头、距离采集系统和数据集成转换装置，距离采集系统集成在红外捕捉镜头装置中并一同安装在巷道壁面上方，红外捕捉镜头之间间隔设定距离，并布置在巷道两侧以保证巷道中移动设备机体的标识点能够全时域的被3个以上的红外捕捉镜头捕捉，红外捕捉镜头发射红外光谱，红外光谱经过移动设备机体上安装的标识点反射后，由距离采集系统采集到并计算出标识点与红外捕捉镜头的距离；所述数据集成转换装置通过网线与红外捕捉镜头和移动设备位姿解算模块中的系统计算机相连接，用于实现所有红外标识点在所有红外捕捉镜头中的距离数据集成和转换，并将距离数据集成和转换得到的结果发送至所述移动设备位姿解算模块。

在一个实施例中，所述姿态校准模块包括设定数量的红外标识点，所述姿态校准模块放置在所有红外捕捉镜头采集范围内，红外捕捉镜头和数据集成转换装置获取姿态校准模块中所有红外标识点的距离数据，通过姿态校准模块中标识点的实际间距反推出系统定位定姿系统的距离与真实距离的映射关系，实现该定位定姿系统在启动初始化时的标定和校准。

在一个实施例中，所述移动设备刚体建模标识模块包括若干红外标识点、由标识点连接成的移动设备刚体模型，若干标识点按照目标移动设备的结构特征固定布置在机体各关节上并能够完全反应机体各关节的运动特征，按照一定的构型规则将红外标识点在软件中连接并定义为机体刚体模型。

在一个实施例中，所述移动设备位姿解算模块包括系统计算机和位姿转换软件；系统计算机连接红外姿态捕捉数据采集模块的数据集成转换装置，用于集成采集的数据并进行统计处理，位姿转换软件用于进行移动设备刚体模型的自由度位姿的实时解算和输出。

具体地，移动设备位姿解算模块在计算得到红外标识点的距离数据后，结合移动设备刚体模型，转换得到移动设备的位姿数据。

具体地，所述移动设备包括巷道内的第一运输车和第二运输车；所述红外捕捉镜头包括第一镜头、第二镜头和第三镜头，红外标识点包括第一标识点、第二标识点、第三标识点、第四标识点、第五标识点、第六标识点、第七标识点、第八标识点和第九标识点；第一运输车上设置第一标识点、第二标识点、第三标识点和第四标识点；第二运输车上设置第六标识点、第七标识点、第八标识点和第九标识点；第一运输车和第二运输车的连接点处设置第五标志点；红外捕捉镜头发射红外光谱，红外光谱经过移动设备机体上安装的标识点反射后，由距离采集系统采集到并计算出标识点与红外捕捉镜头的距离，第一标志点至第一镜头的距离为V

第一镜头的位置点为C1(xc1,yc1,zc1)，第二镜头的位置点为C2(xc2,yc2,zc2)，第三镜头的位置点为C3(xc3,yc3,zc3)；第一标识点的坐标为V1(x1,y1,z1)，第二标识点的坐标为V2(x2,y2,z2)，第三标识点的坐标为V3(x3,y3,z3)，第四标识点的坐标为V4(x4,y4,z4)，第五标识点的坐标为V5(x5,y5,z5)，第六标识点的坐标为V6(x6,y6,z6)，第七标识点的坐标为V7(x7,y7,z7)，第八标识点的坐标为V8(x8,y8,z8)，第九标识点的坐标为V9(x9,y9,z9)；

第一运输车的位置V

第一运输车的姿态A

在一个实施例中，所述移动设备刚体建模模块的工作过程包括：

a、由技术人员提前获取移动设备中各主体转动部件和移动部件的分类、运动部件的结构组成划分；

b、根据移动设备操作工艺和运动部件的结构组成划分来设定机体红外标识点的布置位置；

c、根据各镜头捕捉所有红外标识点在机体上的位置并在系统计算机和位姿转换软件中查看，根据步骤a中的主体转动部件和移动部件的分类情况依次连接各部件所属的红外标识点，形成移动设备刚体模型。

上述基于红外运动捕捉的煤矿井下移动设备定位定姿系统具有如下技术效果：

完全适用于煤矿井下巷道条件，突破现有定位技术在井下的环境限制问题。

基于红外运动捕捉进行煤矿井下移动设备的刚体系统模型搭建，通过对移动设备刚体模型进行运动部件划分来独立精确计算各个部件的6自由度位姿参数，提升了现有位姿测量手段的精度。

采用直接法测量机体表面红外标识点的位置参数，避免了现有位姿测量方法受电磁干扰和机体振动影响较大的难题，提升煤矿井下移动设备定位定姿的实时性和鲁棒性。

附图说明

图1是一个实施例中基于红外运动捕捉的煤矿井下移动设备定位定姿系统结构示意图；

图2是一个实施例中系统标定和校准示意图；

图3是一个实施例中煤矿井下移动设备刚体建模示意图；

图4是一个实施例中煤矿井下移动设备位姿解算示意图；

图5为一个实施例中基于红外运动捕捉的煤矿井下移动设备定位定姿方法流程图；

附图中各个标号的含义包括：1-1表示煤矿井下巷道，1-2表示巷道内的移动设备(如掘进机)，2表示红外姿态捕捉数据采集模块，2-1表示红外捕捉镜头，2-2表示距离采集系统，2-3表示数据集成转换装置，3表示姿态校准模块，4表示移动设备刚体建模模块，4-1表示红外标识点，4-2表示移动设备刚体模型，4-2-1表示移动设备刚体模型部件1，4-2-2表示移动设备刚体模型部件2，5表示移动设备位姿解算模块，5-1表示系统计算机，5-2表示位姿转换软件。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

参考图1所示，图1为一个实施例的基于红外运动捕捉的煤矿井下移动设备定位定姿系统结构示意图，包括红外姿态捕捉数据采集模块2、姿态校准模块3、移动设备刚体建模模块4、移动设备位姿解算模块5；

所述红外姿态捕捉数据采集模块2布置在煤矿井下巷道1-1两侧壁面上方，用于巷道内移动设备1-2的位姿参数采集；所述姿态校准模块3用于定位定姿系统初始化的标定和校准；所述移动设备刚体建模模块4按照移动设备特征布置于移动设备1-2表面，用于构建移动设备1-2刚体模型4-2；所述移动设备位姿解算模块5用于测量数据的处理，并对移动设备的位姿进行实时解算和显示。

上述移动设备1-2主要包括巷道掘进机、运输车、连续采煤机、掘锚一体机、煤矿机器人及运输车等轮式和/或履带移动设备装备等。

上述红外姿态捕捉数据采集模块2中的红外捕捉镜头2-1数量可以随着巷道环境的不断延伸而不断增加。红外捕捉镜头2-1之间的间距应与红外光强度及镜头(红外捕捉镜头)属性相对应。红外捕捉镜头2-1、数据集成转换装置2-3、系统计算机5-1等硬件均为本质安全型，以适应煤矿井下环境。

在一个实施例中，所述红外姿态捕捉数据采集模块2包括多个红外捕捉镜头2-1、距离采集系统2-2和数据集成转换装置2-3，距离采集系统2-2集成在红外捕捉镜头2-1装置中并一同安装在巷道1-1壁面上方，红外捕捉镜头之间间隔设定距离，并布置在巷道两侧以保证巷道中移动设备1-2机体的标识点能够全时域的被3个以上的红外捕捉镜头2-1捕捉，红外捕捉镜头2-1发射红外光谱，红外光谱经过移动设备1-2机体上安装的标识点4-1反射后，由距离采集系统2-2采集到并计算出标识点4-1与红外捕捉镜头2-1的距离；所述数据集成转换装置2-3通过网线与红外捕捉镜头2-1和移动设备位姿解算模块5中的系统计算机5-1相连接，用于实现所有红外标识点4-1在所有红外捕捉镜头2-1中的距离数据集成和转换，并将距离数据集成和转换得到的结果发送至所述移动设备位姿解算模块5。

在一个实施例中，如图2所示，所述姿态校准模块3包括设定数量的红外标识点4-1(一般为4个标识点的T型装置)，各个红外标识点4-1形成红外标识装置，上述红外标识装置具有固定尺寸和形状，所述姿态校准模块3放置在所有红外捕捉镜头2-1采集范围内，红外捕捉镜头2-1和数据集成转换装置2-3获取姿态校准模块3中所有红外标识点4-1的距离数据，通过姿态校准模块3中标识点的实际间距反推出系统定位定姿系统的距离与真实距离的映射关系，实现该定位定姿系统在启动初始化时的标定和校准。

在一个实施例中，所述移动设备刚体建模标识模块4包括若干红外标识点4-1、由标识点4-1连接成的移动设备刚体模4-2，若干标识点4-1按照目标移动设备1-2的结构特征固定布置在机体各关节上并能够完全反应机体各关节的运动特征，按照一定的构型规则将红外标识点4-1在软件中连接并定义为机体刚体模型4-2。

在一个实施例中，参考图3所示，所述移动设备刚体建模模块4构建移动设备刚体模型4-2的过程包括：

a、由技术人员提前获取移动设备1-2中各主体转动部件和移动部件的分类、运动部件的结构组成划分；即获取移动设备1-2的结构和运动特性，主要包括各主体转动部件和移动部件的分类、运动部件的结构组成划分。

b、根据移动设备操作工艺和运动部件的结构组成划分来设定机体红外标识点4-1的布置位置。

c、根据各镜头2-1捕捉所有红外标识点4-1在机体上的位置并在系统计算机5-1和位姿转换软件5-2中查看，根据步骤a中的主体转动部件和移动部件的分类情况依次连接各部件所属的红外标识点4-1，形成移动设备刚体模型4-2。

优选地，所述移动设备刚体模型4-2在能够反映移动设备结构、运动特性和冗余度的基础上尽可能少的红外标识点4-1，以减小系统的实时计算量。

在一个实施例中，所述移动设备位姿解算模块5包括系统计算机5-1和位姿转换软件5-2；系统计算机5-1连接红外姿态捕捉数据采集模块2的数据集成转换装置2-3，用于集成采集的数据并进行统计处理，位姿转换软件5-2用于进行移动设备刚体模型4-2的自由度位姿的实时解算和输出。

具体地，移动设备位姿解算模块5在计算得到红外标识点4-1的距离数据后，结合移动设备刚体模型4-2，转换得到移动设备1-2的位姿数据。

具体地，参考图4所示，以巷道运输车为例，所述移动设备1-2包括巷道内的第一运输车和第二运输车；所述红外捕捉镜头2-1包括第一镜头、第二镜头和第三镜头，红外标识点4-1包括第一标识点、第二标识点、第三标识点、第四标识点、第五标识点、第六标识点、第七标识点、第八标识点和第九标识点；第一运输车上设置第一标识点、第二标识点、第三标识点和第四标识点；第二运输车上设置第六标识点、第七标识点、第八标识点和第九标识点；第一运输车和第二运输车的连接点处设置第五标志点；红外捕捉镜头2-1发射红外光谱，红外光谱经过移动设备1-2机体上安装的标识点反射后，由距离采集系统2-2采集到并计算出标识点与红外捕捉镜头2-1的距离，第一标志点至第一镜头的距离为V

第一镜头的位置点为C1(xc1,yc1,zc1)，第二镜头的位置点为C2(xc2,yc2,zc2)，第三镜头的位置点为C3(xc3,yc3,zc3)；第一标识点的坐标为V1(x1,y1,z1)，第二标识点的坐标为V2(x2,y2,z2)，第三标识点的坐标为V3(x3,y3,z3)，第四标识点的坐标为V4(x4,y4,z4)，第五标识点的坐标为V5(x5,y5,z5)，第六标识点的坐标为V6(x6,y6,z6)，第七标识点的坐标为V7(x7,y7,z7)，第八标识点的坐标为V8(x8,y8,z8)，第九标识点的坐标为V9(x9,y9,z9)；

第一运输车的位置V

第一运输车的姿态A

在一个示例中，上述第一运输车的位置V

在一个实施例中，参考图5所示，上述基于红外运动捕捉的煤矿井下移动设备定位定姿系统的使用过程包括：

a、启动基于红外运动捕捉的煤矿井下巷道移动设备定位定姿系统，为红外姿态捕捉数据采集模块2和移动设备位姿解算模块5通电，设定红外捕捉镜头2-1数量和网络IP地址；

b、将姿态校准模块放置3在红外捕捉镜头2-1视野中，调整红外捕捉镜头2-1的焦距和进光量使得捕捉点完整且清晰，完成系统尺度标定；多次将姿态校准模块3动态布置在红外捕捉镜头2-1视野范围内，使校准模块能将所有区域扫描，即完成系统校准；

c、根据目标移动设备的结构特征和运动特点将红外标识点4-1固定在移动设备1-2机体表面易于观察的位置，并使用相应刚体模型构建方法进行煤矿巷道目标移动设备刚体模型4-2的构建；

d、实时移动目标移动设备1-2，移动设备位姿解算模块5实时获取目标移动设备的机体标识点4-1距离，通过集成数据转换解算得到移动设备特定点和指定部件的位置和姿态，并实时输出和显示。

本实施例能够实时捕捉煤矿井下巷道移动设备的自由度位姿参数，对巷道移动设备进行结构和运动分析后在巷道移动设备表面布置红外标识点，建立巷道移动设备的刚体模型，通过红外捕捉镜头实时获取巷道移动设备机体表面外红标识点的距离而获得巷道移动设备的位姿参数，实时掌握巷道移动设备的运行状态和形式轨迹，增强煤矿井下移动设备的定位定姿精度和鲁棒性，能够为煤矿井下巷道移动设备的智能化控制和自动化作业提供条件。

需要说明的是，本申请实施例所涉及的术语“第一\第二\第三”仅仅是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二\第三”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二\第三”区分的对象在适当情况下可以互换，以使这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。

本申请实施例的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或模块的过程、方法、装置、产品或设备没有限定于已列出的步骤或模块，而是可选地还包括没有列出的步骤或模块，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或模块。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。