一种六自由度机械臂奇异位形的快速判断方法

摘要

本发明属于机械臂奇异位形判断领域，具体涉及一种六自由度机械臂奇异位形的快速判断方法。本发明包括：从六自由度机械臂的角度编码器中读取六个关节的当前角度值；判断六个关节角是否都没超出自身的实际运动范围；判断第3个关节角；判断第2个关节角；判断第5个关节角。本发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异位形的判断速度。

主权项

一种六自由度机械臂奇异位形的快速判断方法，其特征在于：(1)从六自由度机械臂的角度编码器中读取六个关节的当前角度值θ_i，i＝1～6；(2)判断六个关节角θ_i是否都没超出自身的实际运动范围，如果都没有超出实际运动范围则执行步骤(3)，如果有关节角超出实际运动范围则判定该关节为奇异点，机械臂停止运动；(3)判断第3个关节角θ₃，若tanθ₃＝l₃/l₂，则第3个关节为奇异点，机械臂停止运动；若tanθ₃≠l₃/l₂，则执行步骤(4)；(4)判断第2个关节角θ₂，若l₁cosθ₂+l₃sin(θ₂‑θ₃)+l₂cos(θ₂‑θ₃)+d₁＝0，则第2个关节为奇异点，机械臂停止运动；若l₁cosθ₂+l₃sin(θ₂‑θ₃)+l₂cos(θ₂‑θ₃)+d₁≠0，则执行步骤(5)，l₁为六自由度机械臂2号坐标系原点与3号坐标系原点之间的距离，l₂为3号坐标系原点与4号坐标系原点之间的距离，l₃为4号坐标系与机械臂腕部点O之间的距离，d₁为1号坐标系z轴与基坐标系z轴之间的距离，θ_i表示机械臂第i个关节转过的角度；(5)判断第5个关节角θ₅，若θ₅＝±π/2，则第5个关节为奇异点，机械臂停止运动；若θ₅≠±π/2，则判定关节不为奇异点，机械臂继续运动。