[发明专利]基于轴不变量的多轴机器人正运动学计算方法

权利要求书

1.一种基于轴不变量的多轴机器人正运动学计算方法，其特征是，

多轴机器人装置包含杆件序列与关节序列，将树链中的关节序列转换成对应的轴序列及其父轴序列，所述轴序列的轴为平动轴或转动轴；

使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；

利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映所述多轴机器人运动链的拓扑关系与链序关系；

计算运动链的迭代式正运动学数值；

计算基于轴不变量的迭代式偏速度。

2.根据权利要求1所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

惯性空间记为i，给定由i至杆件n的运动链ⁱl_n，杆件l,n,j均∈A，n>l，s是杆件l上的任一点，A为轴序列；当转动矢量有测量噪声时，运动链ⁱl_n的迭代式正运动学数值计算步骤包括：

【1】链节正运动学计算步骤；

运动副对应的运动链通过区间符表示为：

其中：是l的前继即父，l是的后继即子；为链节，是运动链中的一个基本环节；

【2】运动链ⁱl_n的位形计算步骤；

【3】运动链ⁱl_n的速度及加速度计算步骤。

3.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

链节正运动学计算步骤为：

【1-1】已知转动矢量根据欧拉四元数公式计算欧拉四元数

【1-2】由欧拉四元数计算旋转变换阵

【1-3】由下式计算链节速度：

式中，运动副表示连接杆件及杆件l的运动副；转动副R，棱柱副P；沿轴的线位置绕轴的角位置轴矢量角速度线速度表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i；为i到的旋转变换矩阵，为转动速度对参考基i的投影矢量；为平动速度对参考基i的投影矢量；

【1-4】由下式计算链节加速度：

式中，转动加速度平动加速度表示角速度的导数即角加速度，表示线速度的导数即线加速度。

4.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

运动链ⁱl_n的位形计算步骤为：

【2-1】由欧拉四元数的链关系，将四元数乘法运算用其共轭矩阵运算替代，计算欧拉四元数序列为欧拉四元数；

【2-2】用欧拉四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算，计算旋转变换阵序列{ⁱQ_j|j∈A}，ⁱQ_j为i到j的旋转变换矩阵；

【2-3】由下式计算位置矢量

式中，轴矢量沿轴的线位置的含义是坐标矢量对参考基i的投影矢量，为在自然坐标系下由杆件到杆件l的坐标矢量；n_S表示杆件n中的点S，为由i至n_S的运动链；表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i；为零位时的平动矢量对参考基i的投影矢量，为轴矢量对参考基i的投影矢量。