一种煤矿井下机械臂姿态检测控制方法、装置及设备

技术领域

本发明涉及煤矿特种机器人及机械臂自动化控制技术领域，尤其涉及一种煤矿井下机械臂姿态检测控制方法、装置、设备及计算机存储介质。

背景技术

我国是世界第一产煤大国，煤炭产量占世界总产量的50％以上，煤炭行业已经成为国民经济高速发展的重要基础。地下井工开采是煤矿的主要生产方式，井下作业的绝大部分操作均需煤矿工人在工作面现场完成，随着计算机、自动化等领域科技水平的发展，研究学者们在煤矿特种机器人领域开展了大量研究，以期利用机器人代替工人完成巷道掘进、打钻、冲尘等复杂工作任务，机械臂姿态检测及控制是机器人功能实现的重要基础，研发针对煤矿井下重载机械臂的姿态检测及控制方法，对煤矿特种机器人的发展有重要意义。

目前，现有机械臂仍需工作人员进行周围环境数据获取及控制，大大影响了工作进度，人工获取环境数据存在难测量、效率慢等问题，直接影响到机械臂的移动精度和工作效率，且现有机械臂控制技术无法有效降低使用过程中设备碰撞风险，影响井下工作人员安全。

综上所述可以看出，如何设计一种高精度、高效率且碰撞风险低的煤矿井下机械臂姿态检测控制方法是目前待解决的问题。

发明内容

本发明的目的是提供一种煤矿井下机械臂姿态检测控制方法、装置及应用，以解决现有技术机械臂效率、测量难且无法自主规避碰撞的问题。

为解决上述技术问题，本发明提供一种煤矿井下机械臂姿态检测控制方法，包括：

在接收到机械臂运动指令时，获取所述机械臂当前位姿数据和环境数据；

利用所述当前位姿数据进行机械臂动力学计算，得到机械臂姿态数字模型；

利用所述环境数据对所述机械臂的作业空间进行三维重建，得到三维环境模型；

基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，得到当前位置下所述机械臂的安全移动路径。

优选地，所述基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，得到当前位置下所述机械臂的安全移动路径包括：

将所述机械臂姿态数字模型与所述三维环境模型进行坐标系转换计算，同步到同一坐标系；

基于所述机械臂的各关节点和所述机械臂的末端机构坐标点，计算所述机械臂在当前姿态下与环境中其他物体的距离，生成距离数据集合；

将所述距离数据集合中低于安全阈值的距离数据标记碰撞风险，得到风险标记数据；

将所述风险标记数据进行计算，解算出距离数据集合均符合安全阈值的新姿态数据，得到当前位置下所述机械臂的安全移动路径。

优选地，所述机械臂当前位姿数据包括：关节角度值、关节角加速度、关节移动轨迹；所述环境数据包括：图像视频和环境点云数据。

优选地，所述利用所述当前位姿数据进行机械臂动力学计算，得到机械臂姿态数字模型包括：

基于所述机械臂的关节角度值、所述关节角加速度、所述关节移动轨迹，采用牛顿-欧拉方法依次计算所述机械臂底座每一个连杆的速度、加速度，一直计算到机械臂末端机构，再从所述机械臂末端机构所受外力开始依次倒序求解各关节扭矩，得到所述机械臂姿态数字模型。

优选地，所述机械臂姿态数字模型计算公式为：

其中，M(q)为惯性力项，

优选地，所述利用所述环境数据对所述机械臂的作业空间进行三维重建，得到三维环境模型包括：

利用数据分析算法筛除所述环境数据中激光点云数据噪声干扰，得到消除噪声的点云数据；

将所述环境数据中图像数据进行多视图几何重建，得到带有纹理信息的点云模型；

将所述点云模型与所述消除噪声的点云数据融合计算，得到三维环境模型。

优选地，所述三维环境模型包括致密的三维空间点云和准确的纹理数据。

本发明还提供一种煤矿井下机械臂姿态检测控制装置，包括：

数据采集模块，用于在接收到机械臂运动指令时，获取所述机械臂当前位姿数据和环境数据；

机械臂姿态构建模块，利用所述当前位姿数据进行机械臂动力学计算，得到机械臂姿态数字模型；

三维环境构建模块，利用所述环境数据对所述机械臂的作业空间进行三维重建，得到三维环境模型；

避碰分析模块，基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，得到当前位置下所述机械臂的安全移动路径。

本发明还提供一种煤矿井下机械臂姿态检测控制设备，包括：

摄像头，用于获取环境图像视频；

惯性导航单元，用于获取机械臂加速度；

激光雷达，用于获取环境点云数据；

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现如上述所述一种煤矿井下机械臂姿态检测控制方法的步骤。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述所述一种煤矿井下机械臂姿态检测控制方法的步骤。

本发明所提供的一种煤矿井下机械臂姿态检测控制方法，通过获取所述机械臂当前位姿数据和环境数据，实现煤矿井下重载机械臂的精准姿态检测，重新构建机械臂姿态数字模型及三维环境模型，并基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，获取当前位置下所述机械臂的安全移动路径，提升控制精度和作业效率的同时降低了操作安全风险，实现了煤矿井下重载机械臂的自动化位姿检测和自主避碰决策，有效降低了使用过程中的设备碰撞风险，提高了煤矿机械设备的智能化水平。

附图说明

为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明所提供的煤矿井下机械臂姿态检测控制方法的第一种具体实施例的流程图；

图2为本发明所提供的一种煤矿井下机械臂姿态检测控制系统示意图；

图3为本发明实施例提供的一种煤矿井下机械臂姿态检测控制装置的结构框图。

具体实施方式

本发明的核心是提供一种煤矿井下机械臂姿态检测控制方法、装置及应用，实现了实现了煤矿井下重载机械臂的超视距智能控制，提升控制精度和作业效率的同时降低了操作安全风险。

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明所提供的煤矿井下机械臂姿态检测控制方法的第一种具体实施例的流程图；具体操作步骤如下：

S101：在接收到机械臂运动指令时，获取所述机械臂当前位姿数据和环境数据；

获取所述机械臂的关节角度值、关节角加速度、关节移动轨迹、图像视频和环境点云数据。

S102：利用所述当前位姿数据进行机械臂动力学计算，得到机械臂姿态数字模型；

基于所述机械臂的关节角度值、所述关节角加速度、所述关节移动轨迹，采用牛顿-欧拉方法依次计算所述机械臂底座每一个连杆的速度、加速度，一直计算到机械臂末端机构，再从所述机械臂末端机构所受外力开始依次倒序求解各关节扭矩，得到所述机械臂姿态数字模型；

所述机械臂姿态数字模型计算公式为：

/>

其中，M(q)为惯性力项，

S103：利用所述环境数据对所述机械臂的作业空间进行三维重建，得到三维环境模型；

利用数据分析算法筛除所述环境数据中激光点云数据噪声干扰，得到消除噪声的点云数据；

将所述环境数据中图像数据进行多视图几何重建，得到带有纹理信息的点云模型；

将所述点云模型与所述消除噪声的点云数据融合计算，得到三维环境模型；

所述三维环境模型包括致密的三维空间点云和准确的纹理数据。

S104：基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，得到当前位置下所述机械臂的安全移动路径；

将所述机械臂姿态数字模型与所述三维环境模型进行坐标系转换计算，同步到同一坐标系；

基于所述机械臂的各关节点和所述机械臂的末端机构坐标点，计算所述机械臂在当前姿态下与环境中其他物体的距离，生成距离数据集合；

将所述距离数据集合中低于安全阈值的距离数据标记碰撞风险，得到风险标记数据；

将所述风险标记数据进行计算，解算出距离数据集合均符合安全阈值的新姿态数据，得到当前位置下所述机械臂的安全移动路径。

本发明提供一种煤矿井下机械臂姿态检测控制方法，通过获取所述机械臂当前位姿数据和环境数据，实现煤矿井下重载机械臂的精准姿态检测，重新构建机械臂姿态数字模型及三维环境模型，并基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，获取当前位置下所述机械臂的安全移动路径，提升控制精度和作业效率的同时降低了操作安全风险，利用自动化姿态检测与控制技术代替现场目测操控，实现了煤矿井下重载机械臂的智能化控制，提高机械臂作业效率。

基于上述实施例，本实施例对机械臂姿态检测控制方法进行详细阐述，具体如下：

S201：采集环境数据，包括图像视频、关节角度值、关节角加速度、关节移动轨迹和环境点云数据，数据通过数据传输单元传递至自主决策控制单元；

所述采集环境数据，采集机械臂本体和周围环境数据，包括目标物、障碍物位置信息以及实现对机械臂本体的定位；所述数据传输单元用于传输数据采集单元获取到的数据及系统控制指令；自主决策控制单元用于进行数据处理和运动分析决策，实现机械臂移动、停机、紧急避障等指令的下达；所述机械臂机动单元用于实现机械臂运动指令的执行。

S202：进行机械臂动力学计算，求解出机械臂当前位姿以及机械臂末端线速度和线加速度，还原出机械臂运动过程的数字化模型。动力学模型根据牛顿-欧拉方程建立，动力学方程为：

其中，M(q)为惯性力项，

进一步的，所述重载机械臂动力学计算采用“牛顿-欧拉”方法，从重载机械臂的底座开始依次计算每一个连杆的速度、加速度，一直到机械臂末端机构；而后，从机械臂末端机构所受外力开始依次倒序求解各关节扭矩，最终获得机械臂姿态数字模型。

S203：根据图像视频和环境点云环境，进行机械臂环境空间三维重建，并生成三维环境模型，将所述三维环境模型与当前机械臂位姿数据进行坐标系转换计算，同步到同一坐标系进行避碰分析，筛选当前位置下的安全移动方向。

S204：获取机械臂运动指令，按照运动指令进行动力学计算，重新执行避碰分析，确认该指令运动轨迹无风险后将指令解算为具体数值下达至机械臂机动单元，开始执行相应运动指令；

所述机械臂运动指令的解算方法为：根据动力学计算获得的数字化模型，输入重载机械臂末端机构需要移动的目的位置或轨迹坐标，推导出机械臂各关节的驱动机构需要在各时间点施加多少扭矩，输出电信号到重载机械臂的电液伺服阀，实现重载机械臂运动的精确控制。

本实施例提供一种煤矿井下机械臂姿态检测控制方法，更加具体的阐述了机械臂姿态数字模型和三维环境模型的构建方法，通过机械臂上装载的传感器阵列，采集图像视频、机械臂关节数据和环境点云模型等数据，据此计算机械臂当前运动状态和作业环境信息，实现煤矿井下重载机械臂的精准姿态检测，同时利用姿态信息与环境点云模型相比对，分析是否存在碰撞风险并直接进行自主避碰控制，避免机械臂运动中与井下其他设备的位置干涉。同时，数据传输单元将数据信息传输至地面控制终端，实现远程监控和机械臂遥操作指令下达，使机械臂同时具备自主避碰和远端控制两种控制功能。

如图2所示，本实施例提供一种煤矿井下机械臂姿态检测控制系统，具体如下：

硬件设备主要由数据采集单元、数据传输单元、控制单元和机械臂机动单元组成。

所述数据采集单元包括摄像头、惯性导航单元、激光雷达、编码器和应力应变传感器，在机械臂使用过程中，采集图像视频、关节角度值、关节角加速度、关节移动轨迹和环境点云数据；

所述数据传输单元包括信号收发终端、矿用本安交换机和井下通讯光缆设备，共同组成井下数据通信网络；

所述控制单元为包含图形计算单元的主控制器，负责进行数据处理和分析决策，实现机械臂移动、停机、紧急避障等指令的下达；

所述机械臂机动单元包括液压机械臂本体机构、电液伺服阀和液压油缸，通过液压驱动重载机械臂的运动。

基于上述实施例，本实施例对避碰分析进行详细阐述，具体如下：

S301：根据图像视频数据和环境点云数据进行基于数据融合的三维重建，获取机械臂当前所在区域的三维环境模型；

S302：将三维环境模型与机械臂姿态数字模型进行坐标系转换计算，同步到同一坐标系；

S303：以机械臂各关节点和机械臂末端机构坐标点为基准，计算机械臂在当前姿态下与环境中其他物体的距离，生成距离数据集合；

S304：分析距离数据集合中是否存在低于安全阈值的距离数据，并返回该部分数据对应的关节代码，并标记碰撞风险；

S305：将包含碰撞风险标记的机械臂关节重新输入动力学模型进行计算，解算出距离数据集合均符合安全阈值的新姿态数据，输出避碰运动指令。

重复步骤S301至步骤S305，可实现煤矿井下机械臂使用过程中的实时避碰分析与自主避碰控制决策。

本发明实施例一种煤矿井下机械臂姿态检测控制方法，通过机械臂上装载的传感器采集图像视频、机械臂关节数据和环境点云模型等数据，据此计算机械臂当前运动状态和作业环境信息，实现煤矿井下重载机械臂的精准姿态检测，同时利用姿态信息与环境点云模型进行计算，分析是否存在碰撞风险并直接进行自主避碰控制。实现了远程监控和机械臂遥操作指令下达，使机械臂同时具备自主避碰和远端控制两种控制功能。本实施例利用自动化姿态检测与控制技术代替现场目测操控，实现了煤矿井下重载机械臂的智能化控制，提高机械臂作业效率。

请参考图3，图3为本发明实施例提供的一种煤矿井下机械臂姿态检测控制装置的结构框图；具体装置可以包括：

数据采集模块100，用于在接收到机械臂运动指令时，获取所述机械臂当前位姿数据和环境数据；；

机械臂姿态构建模块200，利用所述当前位姿数据进行机械臂动力学计算，得到机械臂姿态数字模型；

三维环境构建模块300，利用所述环境数据对所述机械臂的作业空间进行三维重建，得到三维环境模型；

避碰分析模块400，基于所述机械臂姿态数字模型、所述三维环境模型进行避碰分析，得到当前位置下所述机械臂的安全移动路径。

本实施例的一种煤矿井下机械臂姿态检测控制装置用于实现前述的一种煤矿井下机械臂姿态检测控制方法，因此一种煤矿井下机械臂姿态检测控制装置中的具体实施方式可见前文中的一种煤矿井下机械臂姿态检测控制方法的实施例部分，例如，数据采集模块100，机械臂姿态构建模块200，三维环境构建模块300，避碰分析模块400，分别用于实现上述一种煤矿井下机械臂姿态检测控制方法中步骤S101，S102，S103，S104，所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。

为了实现上述实施例，本发明还提出一种计算机程序产品，当所述计算机程序产品中的指令处理器执行时，执行一种基于人工智能的方法，所述方法包括：。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。