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

权利要求书

1.一种六自由度仿人灵巧臂逆运动学的快速求解方法，所述六自由度仿人灵巧臂由肩关节两个自由度、肘关节一个自由度、腕关节三个自由度、颈部以及连接于各关节间的连杆组成，其特征在于，所述六自由度仿人灵巧臂逆运动学的快速求解方法包括如下步骤：

a、根据六自由度仿人灵巧臂的结构建立其关节结构模型，其中，肩心对应肩关节轴心，肘心对应肘关节轴心，腕心对应腕关节轴心，颈心对应颈部；

b、确定所述六自由度仿人灵巧臂目标连杆的位姿(p^ref，R^ref)；

c、根据所述目标连杆位姿通过解析法计算出六自由度仿人灵巧臂的逆运动学关节角矢量q，

所述步骤c中通过解析法计算逆运动学关节角矢量q的具体实现如下：

定义世界坐标系∑_W，所述肩关节、肘关节以及腕关节在世界坐标系下绕轴分别对应为y，x，x，z，x，y轴，[r_x r_y r_z]为以肩关节为原点的世界坐标系下腕心的位置坐标，肘关节角q₃由所述肩心、肘心以及腕心构成的三角关系求取，肩关节俯仰角q₁和偏摆角q₂由所述肩心、肘心、腕心和颈心构成的平面关系求取，分别采用如下等式：

q3=-arccos(L12+L22-C22L1·L2)+π]]>

q1=arctan(rx/rz)ifrz<0q1=-π+arctan(rx/rz)ifrz>0]]>

q2=π/2-arccos(L12+C2-L222L1·C)-arccos(C2+D2-F22CD)]]>

其中，D为肩心与颈心之间的长度，L1为肩心与肘心之间的长度，L2为肘心至腕心之间的长度，F为腕心和颈心之间的距离。