[发明专利]基于二维码的容错自校正工业机器人手臂控制方法

摘要

本发明提供了一种基于二维码的容错自校正工业机器人手臂控制方法，所述工业机器人手臂具有M个关节，从工业机器人手臂的基座开始，将M个关节依顺定义为第一个关节至第M个关节。本发明依靠摄像机和依附在机械臂上的二维码AprilTag，如果机械臂发生故障，在某个关节损坏的情况下，用摄像机和二维码观测计算出损坏关节的损坏角度，依靠别的关节的适应变动，使机械臂的手部依旧可以到达指定空间位置，继续完成工作。

主权项

1.一种基于二维码的容错自校正工业机器人手臂控制方法，所述工业机器人手臂具有M个关节，从工业机器人手臂的基座开始，将M个关节依顺定义为第一个关节至第M个关节，其特征在于，所述控制方法包括以下步骤：步骤1、在工业机器人手臂的第M个关节上固定AprilTag，利用摄像机拍摄工业机器人手臂的正常工作区域；步骤2、若工业机器人手臂的第k个关节在工作过程中发生损坏，工业机器人手臂停止运动，获取当前位置下，从摄像机所在位置到AprilTag所在位置的转换矩阵CQRT，则计算得到当前位置下第k‑1个关节到第k个关节的转换矩阵(k‑1)kT′，则有：(k‑1)kT′＝(k‑1)(k‑2)T×(k‑2)(k‑3)T×...×21T×1BT×BCT×CQRT×QRET×E(M‑1)T×(M‑1)(M‑2)T×...×(k+1)kT式中，(k‑1)(k‑2)T表示第k‑1个关节到第k‑2个关节的转换矩阵，(k‑2)(k‑3)T表示第k‑2个关节到第k‑3个关节的转换矩阵，21T表示第2个关节到第1个关节的转换矩阵，1BT表示第1个关节到工业机器人手臂的基座的转换矩阵，BCT表示工业机器人手臂的基座到摄像机的转换矩阵，QRET表示AprilTag到工业机器人手臂的转换矩阵，E(M‑1)T表示工业机器人手臂到第M‑1个关节的转换矩阵，(M‑1)(M‑2)T表示第M‑1个关节到第M‑2个关节的转换矩阵，(k+1)kT表示第k+1个关节到第k个关节的转换矩阵；步骤3、利用转换矩阵(k‑1)kT′中第一行第一项cosθ_n和第二行第一项sinθ_n的值计算得到损坏角度θ_n的值；步骤4、工业机器人手臂回归到初始位置，将第k个关节的角度值用步骤3计算得到的损坏角度θ_n的值替换与其他剩余M‑1个关节的角度值相结合；步骤5、根据M个相应的连杆的属性结合上一步得到M个角度值，得到M个转换矩阵，从基坐标依次将M个矩阵相乘，得到从基坐标到工业机器人手臂的当前的转换矩阵，把当前的转换矩阵和目标的转换矩阵相减得到12个式子，把这些式子分别对M个关节的角度求偏导得到雅可比矩阵，用牛顿迭代法算出新的M个关节的角度值；步骤6、工业机器人手臂的M个关节依据计算得到的角度值动作后，若工业机器人手臂未达到目标位置，则返回步骤5，直到工业机器人手臂达到目标位置。