[发明专利]一种工业机器人防碰撞方法

权利要求书

1.一种工业机器人防碰撞方法，其特征在于，包括顺次相接的以下步骤：

步骤一，分别建立初始状态下机器人、工件及外部工装的几何模型，及简化合并外部工装的AABB包围盒模型；

步骤二，机器人动作时，在第一控制周期内，根据观测的机器人运动状态预测m周期后机器人各关节、杆件、工件的位置和姿态，根据预测的机器人各关节、杆件和工件的位置和姿态刷新机器人、工件的几何模型；

步骤三，检测工件与机器人本体各关节、杆件几何模型是否相交；若相交，认为工件与机器人本体将发生碰撞，立即停止机器人；若不相交，继续机器人、工件与外部工装碰撞检测；

步骤四，根据预测m周期后机器人各关节、杆件、工件的位置和姿态，生成机器人-工件的AABB包围盒模型，检测机器人-工件的AABB包围盒模型与简化合并外部工装的AABB包围盒模型是否相交；若不相交，认为无潜在碰撞风险发生；若相交，筛选相交区域，提取待检测外部工装列表；

步骤五，检测机器人、工件几何模型与待检测外部工装列表中的外部工装几何模型是否相交；若相交，认为发生碰撞，立即停止机器人；若不相交，认为无潜在碰撞风险发生；继续下一个控制周期；

步骤六，在下一个控制周期内重复步骤2至步骤5，直到机器人完成全部动作。

2.如权利要求1所述的工业机器人防碰撞方法，其特征在于，步骤一所述机器人、工件及外部工装的几何模型为由机器人、工件、外部工装的机械属性定义的几何模型。

3.如权利要求1所述的工业机器人防碰撞方法，其特征在于，步骤一所述简化合并外部工装的AABB包围盒模型的生成方法为：通过示教区域的方式将临近的外部工装的机械属性定义的几何模型合并，生成包含多个外部工装的AABB包围盒模型。

4.如权利要求1所述的工业机器人防碰撞方法，其特征在于，步骤二所述m周期的计算方法为：

获取机器人在本周期内的各关节最大速度以及最大加速度计算各关节在最大速度下停止所需时间即各关节最大停止时间，选取停止时间最长的关节停止时间为机器人动作停止最长时间t_stop；获取控制器运算周期Δt，计算预测周期数：

m＝t_stop/Δt。

5.如权利要求4所述的工业机器人防碰撞方法，其特征在于，在确认预测周期m的方法中引入补偿周期参数γ，m＝t_stop/Δt+γ；其中γ为补偿周期参数。

6.如权利要求1所述的工业机器人防碰撞方法，其特征在于，步骤二所述根据预测的机器人各关节、杆件和工件的位置和姿态刷新机器人、工件的几何模型的方法为：

⑴、从控制器中获取当前时刻机器人各关节位置θ₁,θ₂,θ₃…θ_i，关节速度

⑵、使用雅可比方程：计算各关节和工件在坐标系下合成速度V_i，其中v_i(t)为关节线速度，ω_i(t)为t时刻关节姿态速度；

⑶、设定关节在当前时刻t做匀速直线运动，通过状态方程计算m周期后各关节、杆件和工件几何模型位置和姿态；

P_i(t+m·Δt)＝v_i(t)·m·Δt+P_i(t)

O_i(t+m·Δt)＝ω_i(t)·m·Δt+O_i(t)

其中Δt为控制器运算周期，P_i(t)为控制器运算周期，Δt为控制器运算周期，P_i(t)为当前时刻t下各关节、杆件和工件几何模型位置，O_i(t)为当前时刻t下各关节、杆件和工件几何模型姿态，计算公式为：

⑷、根据机器人各关节，杆件和工件位置和姿态刷新机器人、工件几何模型。

7.如权利要求1所述的工业机器人防碰撞方法，其特征在于，步骤四所述生成机器人-工件的AABB包围盒模型的方法为：通过更新机器人运动状态，获取更新的机器人模型，基于基坐标系获取机器人和工件各x、y、z方向上的顶点，生成AABB包围盒模型。