[发明专利]一种多自由度机械臂的逆运动学通用求解方法及装置

权利要求书

1.一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤：

1）将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；

2）判断所述运动旋量是否小于等于误差，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;

3）计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入步骤1）和2），进行迭代计算，直到其满足计算终止条件。

2.根据权利要求1所述的一种多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：

1.1）利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵，构建末端正解的表达公式。

3.根据权利要求2所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1.1）还包括以下步骤：

1.1.1） 利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵；自由度机械臂的单位旋转轴矩阵为：

其中为自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，为自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；

1.1.2） 通过指数积法表示空间构型，机械臂末端正解公式为：

其中为机械臂初始状态下末端的齐次变换矩阵，其中为机械臂第个自由度的单位旋转矩阵, 为第个自由度的旋转角度。

4.根据权利要求3所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：

1.2） 令为机械臂各关节自由度当前角度，通过上述步骤1.1.2）得到机械臂末端当前状态下的齐次变换矩阵。

5.根据权利要求4所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：

1.3）输入机械臂末端目标状态下的齐次变换矩阵，计算机械臂从至的运动旋量；

将机械臂正解对时间求导得到为：

将机械臂正解求逆得到为：

其运动旋量为：

其中为机械臂第个自由度的单位旋转矩阵, 为第次迭代的第个自由度的旋转角度, 为第次迭代的第个自由度的旋转速度。

6.根据权利要求5所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤3）还包括以下步骤：

3.1） 计算机械臂各自由度的空间雅可比矩阵，表示为关于的函数：

3.2） 其空间旋量表示为：，

单位时间内，各自由度角度的变化表示为：

其中为其机械臂空间雅可比的伪逆；

3.3）下一迭代自由度角度表示为：

将新的各自由度角度代入步骤2）中进行迭代计算。

7.一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：

所述运动计算单元，用于将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；

所述判断单元，用于判断所述运动旋量是否小于等于误差，是否计算终止；

所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入所述运动计算单元进行迭代计算，直到满足计算终止条件。

8.一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于：所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。