一种六自由度仿人灵巧臂逆运动学的快速求解方法

1.一种六自由度仿人灵巧臂逆运动学的快速求解方法，所述六自由度仿人灵巧臂由肩关节两个自由度、肘关节一个自由度、腕关节三个自由度、颈部以及连接于各关节间的连杆组成，其特征在于，所述六自由度仿人灵巧臂逆运动学的快速求解方法包括如下步骤：
a、根据六自由度仿人灵巧臂的结构建立其关节结构模型，其中，肩心对应肩关节轴心，肘心对应肘关节轴心，腕心对应腕关节轴心，颈心对应颈部；
ref ref
b、确定所述六自由度仿人灵巧臂目标连杆的位姿(p ，R )；
c、根据所述目标连杆位姿通过解析法计算出六自由度仿人灵巧臂的逆运动学关节角矢量q，
所述步骤c中通过解析法计算逆运动学关节角矢量q的具体实现如下：
定义世界坐标系∑W，所述肩关节、肘关节以及腕关节在世界坐标系下绕轴分别对应为y，x，x，z，x，y轴，[rx ry rz]为以肩关节为原点的世界坐标系下腕心的位置坐标，肘关节角q3由所述肩心、肘心以及腕心构成的三角关系求取，肩关节俯仰角q1和偏摆角q2由所述肩心、肘心、腕心和颈心构成的平面关系求取，分别采用如下等式：
其中，D为肩心与颈心之间的长度，L1为肩心与肘心之间的长度，L2为肘心至腕心之间的长度，F为腕心和颈心之间的距离。
2.根据权利要求1所述的六自由度仿人灵巧臂逆运动学的快速求解方法，其特征在于：在求取肘关节角q3、肩关节俯仰角q1和偏摆角q2后，所述腕关节三个关节角q4、q5和q6通过与六自由度仿人灵巧臂末端姿态Rref关系求取，采用如下等式：
其中，R0为颈部姿态矩阵， 为绕关节轴a(单位矢量)转动qj时的旋转矩阵。
3.根据权利要求1所述的六自由度仿人灵巧臂逆运动学的快速求解方法，其特征在于：所述步骤c中还包括验证所求逆解关节角矢量q的有效性的方法，该验证方法包括如下步骤：
1)分析所述六自由度仿人灵巧臂的正运动学模型，根据所求逆解关节角矢量q由灵巧臂正运动学计算目标连杆的位姿(p，R)；
ref T ref
2)计算目标连杆位姿的误差err(Δp，ΔR)＝err(p -p，RR )；
3)验证所求逆运动学解的有效性，当位姿误差err(Δp，ΔR)足够小接近于零时所求取逆解有效。
4.根据权利要求3所述的六自由度仿人灵巧臂逆运动学的快速求解方法，其特征在于：所述腕关节连接有灵巧手，所述灵巧手设有手心，所述步骤1)中：分析六自由度仿人灵巧臂的正运动学模型，根据所求各关节角矢量q由正运动学计算各连杆的位姿(p，R)，采用如下等式：
其中p0、R0分别为颈部的位置和姿态，p7、R7分别为手心的位置和姿态，L3为腕心至手心的距离；q＝[q1，q2，…，q6]为从肩关节至腕关节的关节角矢量； 为绕轴aj转动qj时旋转矩阵。

暂无