[发明专利]一种隧道环境机械臂实时碰撞预警系统及方法

权利要求书

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

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

3.根据权利要求2所述的隧道环境机械臂实时碰撞预警方法，其特征在于，所述碰撞检测和多级别预警包括以下三种模式：

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

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

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

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

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

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

4.根据权利要求3所述的隧道环境机械臂实时碰撞预警方法，其特征在于，所述模式1中用OBB代替机械臂连杆的过程如下：

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

5.根据权利要求3所述的隧道环境机械臂实时碰撞预警方法，其特征在于，所述模式2中用离散悬链线代替液压管路的过程如下：

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