[发明专利]一种快速的反向动力学角度硬约束方法

权利要求书

1.一种快速的反向动力学角度硬约束方法，其特征在于，包括以下步骤：

S1、通过反向运动学数值解法计算出各关节的角速度；

S11、确定一个或者几个目标位置，让各个骨骼链做旋转变换，从而得到末端子骨骼所处的实际位置；

S12、比较末端子骨骼的目标位置与实际位置，算出速度修正量；

S13、利用角速度转换矩阵算出末端子骨骼的预期速度，加入速度修正量后得到末端子骨骼速度；

S14、通过反向动力学数值解法利用算法矩阵将得到的末端子骨骼速度转换为各关节的角速度；

S2、使用各关节当前的旋转角度以及各关节的角度限制求得各关节的角速度上下限；

S3、比较并判断各关节的角速度是否处于各关节的角速度上下限之间，

若是，则使用上述各关节的角速度更新各个关节的约束；

若否，比较并判断各关节的角速度是否大于各关节的角速度上限，

若是，则将超过角速度上限的关节的角速度设置成该关节的角速度上限，其余关节角速度上限保持不变，然后更新各个关节的约束，并进入S5；

若否，则进入S1；

S4、判断各关节的约束是否满足反向动力学数值解法的终止条件，

若是，则获得的各关节的约束为反向动力学数值解法的结果；

若否，则返回S1；

S5、使用更新后的各关节执行S1-S3，同时将此时S3中所有超过角速度上限的关节的角速度对应的算法矩阵的行或列向量的所有元素置零；

S6、返回S1继续循环，最终获得各个关节的反向动力学数值解法结果。

2.根据权利要求1所述的一种快速的反向动力学角度硬约束方法，其特征在于，所述S5和所述S14中算法矩阵相同，其包括雅克比矩阵、伪逆矩阵和黑塞矩阵。

3.根据权利要求1所述的一种快速的反向动力学角度硬约束方法，其特征在于，所述S5中算法矩阵的行或列向量包括：当算法矩阵为行主序时为列向量，当算法矩阵为列主序时候为行向量。