[发明专利]一种六自由度仿人灵巧臂逆运动学的快速求解方法

摘要

本发明公开一种六自由度仿人灵巧臂逆运动学的快速求解方法，该方法首先建立六自由度仿人臂关节结构模型，由灵巧臂的肩心-肘心-腕心-颈心构成的平面关系和灵巧臂末端位置求取肘关节角及肩关节两关节角，在此基础上利用灵巧臂末端姿态关系求取腕关节三关节角；所求取的逆运动学解通过正运动学模型计算灵巧臂目标连杆的位姿误差验证解的有效性。相对于机器人逆运动学的数值求解方法，本发明提出的六自由度灵巧臂逆运动学求解方法精度高，速度快，可用于高精度，高实时性要求的仿人灵巧臂作业任务中。

主权项

一种六自由度仿人灵巧臂逆运动学的快速求解方法，所述六自由度仿人灵巧臂由肩关节两个自由度、肘关节一个自由度、腕关节三个自由度、颈部以及连接于各关节间的连杆组成，其特征在于，所述六自由度仿人灵巧臂逆运动学的快速求解方法包括如下步骤：a、根据六自由度仿人灵巧臂的结构建立其关节结构模型，其中，肩心对应肩关节轴心，肘心对应肘关节轴心，腕心对应腕关节轴心，颈心对应颈部；b、确定所述六自由度仿人灵巧臂目标连杆的位姿(pref，Rref)；c、根据所述目标连杆位姿通过解析法计算出六自由度仿人灵巧臂的逆运动学关节角矢量q，所述步骤c中通过解析法计算逆运动学关节角矢量q的具体实现如下：定义世界坐标系∑W，所述肩关节、肘关节以及腕关节在世界坐标系下绕轴分别对应为y，x，x，z，x，y轴，[rx ry rz]为以肩关节为原点的世界坐标系下腕心的位置坐标，肘关节角q3由所述肩心、肘心以及腕心构成的三角关系求取，肩关节俯仰角q1和偏摆角q2由所述肩心、肘心、腕心和颈心构成的平面关系求取，分别采用如下等式： q 3 = - arccos ( L 1 2 + L 2 2 - C 2 2 L 1 · L 2 ) + π q 1 = arctan ( r x / r z ) if r z < 0 q 1 = - π + arctan ( r x / r z ) if r z > 0 q 2 = π / 2 - arccos ( L 1 2 + C 2 - L 2 2 2 L 1 · C ) - arccos ( C 2 + D 2 - F 2 2 CD ) 其中，D为肩心与颈心之间的长度，L1为肩心与肘心之间的长度，L2为肘心至腕心之间的长度，F为腕心和颈心之间的距离。