一种隧道环境机械臂实时碰撞预警系统及方法与流程

本发明涉及工程机械技术领域，特别地，涉及一种隧道环境机械臂实时碰撞预警系统，以及基于该系统的隧道环境机械臂实时碰撞预警方法。

背景技术：

工业机器人在隧道有限空间内进行多机多臂协同作业时，机械臂与机械臂、机械臂与机械臂下悬液压管路、机械臂与隧道环境之间容易发生碰撞，严重影响施工安全和施工效率，因此需要进行实时碰撞预警。

现有工业机器人的碰撞预警主要是通过加装力矩传感器或视觉系统等外部检测装置实现硬件设计，通过层次包围盒碰撞检测技术结合预定预警区域划分或降维算法等实现软件设计。但上述实现方式存在如下弊端：

1、在硬件方面，实现方式过于复杂，成本投入增大，且依据扭矩的碰撞检测方式需进行关节摩擦力估计，该摩擦力又受到机器人位姿、转速、温度和油脂状况等众多因素影响，难以准确建模和辨识，精度较差，因此不适用于隧道作业的液压重载机械臂。

2、在软件方面，预定预警区域的划分必须要合理，若划分粗略，则会导致检测精确度过低，若划分过细，则会增加算法复杂度，目前的预定预警区域划分方式未能很好地兼顾二者，采用的降维处理方法在简化算法复杂度的同时也降低了检测精确度。

3、现有的碰撞预警系统或防碰撞系统只能通过划分预定预警区域的方式提前人为评估不同区域内碰撞事故发生的可能性，进而对即将发生的具体碰撞事故实时检测，还不能做到实时预警，更没有划分预警级别。

此外，现有的碰撞预警系统仅针对机械臂与机械臂或机械臂与作业现场中障碍物的碰撞进行预警，缺少对机械臂与机械臂下悬液压管路间碰撞的预警，综合考虑不够全面。

技术实现要素：

本发明的目的在于提供一种在隧道作业环境下对机械臂与机械臂、机械臂与液压管路、机械臂与隧道环境进行实时检测的碰撞预警技术方案，以解决背景技术中提出的问题。

为实现上述目的，本发明提供一种隧道环境机械臂实时碰撞预警系统，包括逆运动学模块、正运动学模块、定位系统、数字化隧道设计模块、碰撞检测与预警模块和报警模块；所述逆运动学模块用于获取当机械臂达到期望位姿时的关节变量(即机械臂各关节的实时传感器值)，所述正运动学模块用于获取机械臂各关节相对于参考坐标系的实时位姿，所述定位系统用于获取隧道三维空间定位信息，所述数字化隧道设计模块用于提供隧道周向轮廓面信息，所述碰撞检测与预警模块用于对机械臂与机械臂、机械臂与液压管路以及机械臂与隧道环境进行碰撞检测和多级别预警，所述报警模块用于向外界发出预警信号。

本发明还提供了一种基于上述系统的隧道环境机械臂实时碰撞预警方法，首先通过定位系统和数字化隧道设计模块获取隧道三维空间定位信息以及隧道周向轮廓面信息，然后通过逆运动学模块计算得到当机械臂达到期望位姿时的关节变量，接着通过正运动学模块计算得到机械臂各关节相对于参考坐标系的实时位姿，然后在碰撞检测与预警模块中，分别对机械臂、液压管路以及隧道环境建立包围盒模型、离散悬链线模型以及离散平面模型，设置检测参数、报警检测阈值以及极限检测阈值，并将报警检测阈值和极限检测阈值与实时碰撞预警算法关联，实现基于不同预警级别的碰撞检测，利用层次包围盒碰撞检测技术和分离轴定理对包围盒与包围盒、包围盒与悬链线、包围盒与平面的相交情况进行判断，根据判断结果发出对应的碰撞预警指令，最后通过报警模块接收预警指令并向外界发出预警信号。

优选地，所述碰撞检测和多级别预警包括以下三种模式：

模式1：机械臂与机械臂的碰撞检测与预警

本模式采用obb(orientedboundingbox，方向包围盒)检测技术，基于正运动学模块，用变换矩阵、中心点和三个1/2边长表征obb模型，并根据机械臂的实际情况采用多个不同尺寸的obb代替机械臂连杆，将机械臂与机械臂的碰撞检测转化为obb与obb之间的相交检测，若任意两个待检测obb相交，则表示该obb所在的机械臂连杆之间将会发生碰撞，上位机显示碰撞预警信息并发出预警指令，同一连杆上的obb之间互不检测，相邻连杆的obb之间互不检测。

该模式可实现机械臂自身或机械臂之间的碰撞检测与预警。

模式2：机械臂与液压管路的碰撞检测与预警

本模式用离散悬链线代替液压管路，将机械臂与液压管路的碰撞检测转化为obb与离散线段之间的相交检测，若任意一个obb与离散悬链线上的任意一条线段相交，则表示该obb所在的机械臂连杆与液压管路将会发生碰撞，上位机显示碰撞预警信息并发出预警指令，离散悬链线两个悬挂点所在连杆的obb与该离散悬链线之间互不检测。

模式3：机械臂与隧道环境的碰撞检测与预警

本模式用一系列离散的隧道周向轮廓平面和掌子面代替隧道环境，将机械臂与隧道环境的碰撞检测转化为obb与平面之间的相交检测，而基于空间几何，该相交检测即为判断obb八个顶点与平面的位置关系，若任意一个obb的八个顶点均在平面内，则表示该obb所在的机械臂连杆与隧道环境不会发生碰撞，是安全的，反之，只要有任何一个顶点在平面上或平面外，则表示该obb所在的机械臂连杆与隧道环境将会发生碰撞，上位机显示碰撞预警信息并发出预警指令。

优选地，所述模式1中用obb代替机械臂连杆的过程如下：

11)基于正运动学模型计算各包围盒的变换矩阵，并在隧道工程装备的三维模型上基于关节坐标原点测量获得各包围盒几何中心坐标和长宽高参数，建立不同机械臂几何构型的包围盒模型。

优选地，所述模式2中用离散悬链线代替液压管路的过程如下：

21)在隧道工程装备上基于关节坐标原点测量液压管路两个悬挂点的坐标以及液压管路的长度，并标记液压管路所在机械臂的编号及两个悬挂点所在连杆的编号，设置液压管路的离散点数量，获得液压管路的离散悬链线模型。

优选地，所述模式3中用一系列离散的隧道周向轮廓平面与掌子面代替隧道环境的过程如下：

31)根据检测精度要求，在隧道的周向轮廓线上取n个离散点，将相邻离散点用线段连接并沿当前钻进方向的反方向延伸，得到一系列沿隧道周向轮廓分布的离散平面，这些离散平面和掌子面共同构成隧道环境模型；

32)用单位法向量和常量d定义隧道中的各离散平面，其中，x为该离散平面上的任意一点，由数字化隧道设计模块计算掌子面的单位法向量和常量dface以及各周向轮廓平面的单位法向量和常量dti，获得隧道环境的离散平面模型。

优选地，所述模式3中对obb顶点与平面间位置关系的判断过程如下：

33)基于步骤11)得到的包围盒模型获得obb八个顶点的坐标；

34)对于obb的任意顶点q，若则表示顶点q位于掌子面上或隧道空间外，若则表示顶点q位于周向轮廓平面上或隧道空间外，即该obb所在的机械臂连杆与掌子面或周向轮廓平面将会发生碰撞，反之则表示该obb所在的机械臂连杆与掌子面或周向轮廓平面不会发生碰撞，是安全的。

考虑到实际作业时机械臂的末端执行器必须与掌子面接触，因此可通过减小机械臂末端执行器上obb末端尺寸的方式来避免正常工作时触发预警。

须注意的是，本发明中提到的机器人就是指代工程装备的机械臂。

优选地，所述报警模块包括蜂鸣器、警示灯以及车载计算机，所述报警模块发出的预警信号包括蜂鸣器发出不同频率的响声、警示灯发出不同颜色的灯光以及车载计算机发出强制切断操作指令。

优选地，结合报警检测阈值与极限检测阈值对碰撞检测信息进行多级别预警，具体如下：

一、安全级别：未检测到任何碰撞，不发出预警信号；

二、一般预警级别：当碰撞检测值达到报警检测阈值时，蜂鸣器发出低频响声，警示灯颜色为黄色；

三、严重预警级别：当碰撞检测值达到极限检测阈值时，蜂鸣器发出高频响声，警示灯颜色为红色，系统强制中断机械臂动作。

本发明提供的技术方案至少具有如下有益效果：

1、本发明综合考虑了机械臂与机械臂、机械臂与液压管路、以及机械臂与隧道环境的三种碰撞情况，结合方向包围盒检测技术和分离轴定理实现对碰撞事故的实时预警，预警结果置信度高，确保工程装备在受限空间中安全高效作业。

2、本发明无需额外加装硬件检测装置，受外界影响因素小，采用的算法简洁高效，整体节约了开发成本；本发明对于碰撞检测还设置了多级别预警，可在人机交互中提前发布预警信号，若为严重预警还会进行强制中断操作，兼顾了作业时的流畅性和安全性，减少施工事故发生，提高施工效率与施工安全。

附图说明

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

图1是本发明实施例1中碰撞预警系统的架构图；

图2是本发明实施例1中碰撞预警方法的流程图；

图3是本发明实施例1的仿真图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

实施例1

参见图1，一种隧道环境机械臂实时碰撞预警系统，包括逆运动学模块、正运动学模块、定位系统、数字化隧道设计模块、碰撞检测与预警模块和报警模块；所述逆运动学模块用于获取当机械臂达到期望位姿时的关节变量，所述正运动学模块用于获取机械臂各关节相对于参考坐标系的实时位姿，所述定位系统用于获取隧道三维空间定位信息，所述数字化隧道设计模块用于提供隧道周向轮廓面信息，所述碰撞检测与预警模块用于对机械臂与机械臂、机械臂与液压管路以及机械臂与隧道环境进行碰撞检测和多级别预警，所述报警模块用于向外界发出预警信号。

参见图2，基于上述系统可实现对隧道环境机械臂的实时碰撞预警，首先通过定位系统和数字化隧道设计模块获取隧道三维空间定位信息以及隧道周向轮廓面信息，然后通过逆运动学模块计算得到当机械臂达到期望位姿时的关节变量，接着通过正运动学模块计算得到机械臂各关节相对于参考坐标系的实时位姿，然后在碰撞检测与预警模块中，分别对机械臂、液压管路以及隧道环境建立包围盒模型、离散悬链线模型以及离散平面模型，在配置文件中设置检测参数、报警检测阈值以及极限检测阈值，并将报警检测阈值和极限检测阈值与实时碰撞预警算法关联，实现基于不同预警级别的碰撞检测，利用层次包围盒碰撞检测技术和分离轴定理对包围盒与包围盒、包围盒与悬链线、包围盒与平面的相交情况进行判断，根据判断结果发出对应的碰撞预警指令，最后通过报警模块接收预警指令并向外界发出预警信号。

在本实施例中，所述参考坐标系设置在隧道装备上的激光盒处。

所述碰撞检测和多级别预警包括以下三种模式：

模式1：机械臂与机械臂的碰撞检测与预警

本模式采用obb检测技术，基于正运动学模块，用变换矩阵、中心点和三个1/2边长表征obb模型，并根据机械臂的实际情况采用多个不同尺寸的obb代替机械臂连杆，将机械臂与机械臂的碰撞检测转化为obb与obb之间的相交检测，同一连杆上的obb之间互不检测，相邻连杆的obb之间互不检测。具体过程为：

11)基于正运动学模型计算各包围盒的变换矩阵，并在隧道工程装备的三维模型上基于关节坐标原点测量获得各包围盒几何中心坐标和长宽高参数，建立不同机械臂几何构型的包围盒模型；

12)对于obb与obb的相交检测，采用分离轴定理进行碰撞预警算法实现，若任意两个待检测obb相交，则表示该obb所在的机械臂连杆之间将会发生碰撞，上位机显示碰撞预警信息并发出预警指令。

模式2：机械臂与液压管路的碰撞检测与预警

本模式用离散悬链线代替液压管路，将机械臂与液压管路的碰撞检测转化为obb与离散线段之间的相交检测，离散悬链线两个悬挂点所在连杆的obb与该离散悬链线之间互不检测。具体过程为：

21)在隧道工程装备上基于关节坐标原点测量液压管路两个悬挂点的坐标以及液压管路的长度，并标记液压管路所在机械臂的编号及两个悬挂点所在连杆的编号，设置液压管路的离散点数量，获得液压管路的离散悬链线模型；

22)对于obb与线段的相交检测，采用分离轴定理进行碰撞预警算法实现，若任意一个obb与离散悬链线上的任意一条线段相交，则表示该obb所在的机械臂连杆与液压管路将会发生碰撞，上位机显示碰撞预警信息并发出预警指令。

模式3：机械臂与隧道环境的碰撞检测与预警

本模式用一系列离散的隧道周向轮廓平面和掌子面代替隧道环境，将机械臂与隧道环境的碰撞检测转化为obb与平面之间的相交检测，而基于空间几何，该相交检测即为判断obb八个顶点与平面的位置关系。具体过程为：

31)根据检测精度要求，在隧道的周向轮廓线上取n个离散点，将相邻离散点用线段连接并沿当前钻进方向的反方向延伸，得到一系列沿隧道周向轮廓分布的离散平面，这些离散平面和掌子面共同构成隧道环境模型；

32)用单位法向量和常量d定义隧道中的各离散平面，其中，x为该离散平面上的任意一点，由数字化隧道设计模块计算掌子面的单位法向量和常量dface以及各周向轮廓平面的单位法向量和常量dti，获得隧道环境的离散平面模型；

33)基于步骤11)得到的包围盒模型获得obb八个顶点的坐标；

34)对于obb的任意顶点q，若则表示顶点q位于掌子面上或隧道空间外，若则表示顶点q位于周向轮廓平面上或隧道空间外，若任意一个obb的八个顶点均在平面内，则表示该obb所在的机械臂连杆与隧道环境不会发生碰撞，是安全的，反之，只要有任何一个顶点在平面上或平面外，则表示该obb所在的机械臂连杆与隧道环境将会发生碰撞，上位机显示碰撞预警信息并发出预警指令。

在本实施例中，所述报警模块包括蜂鸣器、警示灯以及车载计算机。同时在所述碰撞检测与预警模块中，实时碰撞预警方法结合设置的报警检测阈值与极限检测阈值可实现对碰撞检测信息的三级预警，具体如下：

一、安全级别：未检测到任何碰撞，不发出预警信号；

二、一般预警级别：当碰撞检测值达到报警检测阈值时，蜂鸣器发出低频响声，警示灯颜色为黄色；

三、严重预警级别：当碰撞检测值达到极限检测阈值时，蜂鸣器发出高频响声，警示灯颜色为红色，系统强制中断机械臂动作。

在本实施例中，所述报警模块中的车载计算机通过can总线与机械臂运动感知传感器连接，读取机械臂运动信息，运行碰撞检测与预警算法，并根据检测结果输出预警指令。所述报警模块中的蜂鸣器、警示灯与车载计算机的输出端口连接，根据不同预警级别发出不同频率的声音及不同颜色的灯光。

所述车载计算机上设有人机交互界面，可实现如下功能：

①选择功能：可选择是否开启碰撞检测功能，包括是否进行碰撞检测、是否进行机械臂自身碰撞检测、是否进行机械臂与机械臂的碰撞检测、是否进行机械臂与机械臂下悬液压管路的碰撞检测、以及是否进行机械臂与隧道环境的碰撞检测等；可选择仅对指定机械臂进行碰撞检测；可选择是否进行声音报警或是否进行强制中断操作；

②参数设置功能：可设置机械臂自身、机械臂与机械臂、机械臂与机械臂下悬液压管路、以及机械臂与隧道环境碰撞检测的报警检测阈值和极限检测阈值等参数；

③显示功能：当在可报警范围内检测到机械臂碰撞时，报警标志闪烁，实时动态显示碰撞预警信息。

参见图3，本发明技术的典型实验对象如某隧道工程装备，典型结构为3个机械臂和10条主要下悬液压管路，机械臂连杆用包围盒表示，机械臂下悬液压管路用悬链线表示，隧道环境用掌子面和隧道周向轮廓平面表示。实验设置典型报警检测阈值为0.1米，典型极限检测阈值为0.05米，当机械臂将发生碰撞时，系统可成功发出预警，即将发生碰撞的包围盒与悬链线用虚线示出。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利保护范围，对于本领域的技术人员来说，本发明可以有各种更改和变化。在本发明的精神和原则之内，凡是利用本发明说明书及附图内容所作的任何改进或等同替换，直接或间接运用在其它相关的技术领域，均应包括在本发明的专利保护范围内。