[发明专利]一种五自由度首末对称机械臂逆运动学计算的优化方法

摘要

本发明提供了一种五自由度首末对称机械臂逆运动学计算的优化方法，增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；计算球型手腕中心的位置坐标，通过求齐次变换矩阵并进行转换，从而求得所求目标转角。本发明对五自由度首末对称机械臂的逆运动学求解过程未经过迭代、分类、试凑等技术手段，而是直接根据机械臂自身参数得到了关节角与关节形位的闭式表达式，因此可以可快速求解五自由度首末对称机械臂的逆运动学问题，提高了计算精度和计算速度，有助于实现机械臂的实时响应。

主权项

1.一种五自由度首末对称机械臂逆运动学计算的优化方法，其特征在于包括下述步骤：a)将五自由度首末对称机械臂的各个关节按从基座至执行器的顺序进行标号，基座处的关节标记为第一关节，执行器处的关节标记为第五关节，并在第三和第四关节之间增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；b)给定目标形位相对于惯性坐标系O₀X₀Y₀Z₀的齐次变换矩阵T_O，取T_O的前3行前3列子矩阵为目标位置O₆相对于惯性坐标系的姿态矩阵，记为R₆，取T_O的第4列前3元素为目标位置O₆相对于惯性坐标系的坐标，记为o₆，将虚拟关节和第四第五关节命名为球型手腕，将虚拟关节和第四第五关节的关节轴延长线的交点命名为球型手腕中心，计算球型手腕中心O_c的位置坐标o_c；球型手腕中心O_c的位置坐标o_c计算方法为：o_c＝o₆‑d₆·R₆·{0,0,1}^T                 (1)其中，d₆为目标位置沿Z₅方向相对手腕中心的偏置距离；c)提取o_c中的三个元素x_c、y_c、z_c，据几何关系求目标转角θ_O1、θ_O2、θ_O3，目标转角θ_O1、θ_O2、θ_O3的计算方法为：θ_O1＝Atan2(x_c,y_c)                   (2)其中，D_O称为球腕点参数，D_O＝[x_c²+y_c²+(z_c‑l₁)²‑l₂²‑l₃²]/(2l₂l₃)；Atan2为双变量反正切函数，l₁、l₂和l₃分别为机械臂的第一段连杆的长度、第二段连杆的长度和第三段连杆的长度；d)计算目标转角为θ_O1、θ_O2、θ_O3时前三关节的齐次变换矩阵为T₃⁰，求虚拟关节和后两关节的目标齐次变换矩阵虚拟关节和后两关节的目标齐次变化矩阵T₅³的计算方法为：T₅³＝(T₃⁰)^TT_O             (5)e)记T₅³中第i行第j列元素为r_ij，据齐次变换关系，目标转角θ_O4、θ_O5的计算方法为：θ_O4＝Atan2(r₃₃,‑r₁₃)             (6)θ_O5＝Atan2(r₂₂,r₂₁)                         (7)用公式(6)和(7)即可求得目标转角θ_O4、θ_O5。