[发明专利]一种用于多臂凿岩机器人的机械臂碰撞检测方法

权利要求书

1.一种用于多臂凿岩机器人的机械臂碰撞检测方法，其特征在于：包括如下步骤：

S1、将凿岩机器人的机械臂转化为若干个圆柱体包络盒的集合，将隧道顶壁转换为圆弧线段并且基于圆弧线段构建扇形检测面；

S2、在凿岩机器人动作过程中实时检测任意两个圆柱体包络盒之间的最小距离值判断机械臂之间是否发生碰撞；

S3、基于圆柱体包络盒在检测面上的投影判断机械臂与隧道顶壁是否发生碰撞。

2.如权利要求1所述的一种用于多臂凿岩机器人的机械臂碰撞检测方法，其特征在于：S1中，机械臂包括工作臂和吊篮臂，工作臂和吊篮臂均包括两个及以上的动作部件，将所有动作部件均转换为圆柱体包络盒；

将圆弧线段的两个端点分别记为T₁和T₂，将圆弧线段的圆心记为O，则检测面为扇形T₁OT₂，半径为R,并且以O为极点，OT₂为极轴构建极坐标系。

3.如权利要求2所述的一种用于多臂凿岩机器人的机械臂碰撞检测方法，其特征在于：S2中，参与检测的两个圆柱体包络盒的轴线之间的最小距离为d，则两个圆柱体包络盒之间的最小距离为d_min＝d-(r₁+r₂)，其中r₁和r₂分别为两个圆柱体包络盒的半径，若d_min＞0则两个圆柱体包络盒对应的动作部件未发生碰撞，否则两个圆柱体包络盒对应的动作部件发生碰撞。

4.如权利要求3所述的一种用于多臂凿岩机器人的机械臂碰撞检测方法，其特征在于：S2中，计算两圆柱体轴线间最小距离d的方法为：

S21、记两个圆柱体包络盒的轴线段为l₁和l₂，l₁的两个端点坐标分别为P₁(x₁,y₁,z₁)和P₂(x₂,y₂,z₂)，l₂的两个端点坐标分别为Q₁(x₃,y₃,z₃)和Q₂(x₄,y₄,z₄)，则l₁和l₂上任意一点的坐标可以表示为：

其中向量向量λ₁和λ₂为系数；

S22、将最小距离d的求解过程转化为求系数λ₁和λ₂的最优解过程，

S23、基于极小值条件对S22中的公式进行简化得到

S24、当0≤λ₁,λ₂≤1时d²＝f(λ₁,λ₂)，否则执行S25；

S25、当或时，分别计算P₁到l₂的距离d₁、P₂到l₂的距离d₂、Q₁到l₁的距离d₃和Q₂到l₂的距离d₄，并且有d＝{d₁,d₂,d₃,d₄}_min。