多自由度机构运动学建模方法、装置、设备和存储介质

技术领域

本申请涉及运动学建模技术领域，尤其涉及一种多自由度机构运动学建模方法、装置、设备和存储介质。

背景技术

用于实现复杂曲面加工的多自由度机构，例如五轴机床和工业机器人等，在制造过程中存在结构尺寸误差和装配误差，从而影响机构末端的定位误差，造成精度损失。运动学标定是一种能有效提高多自由度机构精度的方法，旨在通过引入误差参数，建立运动学模型和误差模型，找出末端误差与结构误差和电机驱动量的关系，最后根据逆运动学模型进行误差补偿以提高绝对定位精度。由于引入误差参数的运动学模型比较复杂，现有方法不能对逆运动学模型进行解析解求解，只能采用耗时较长的数值迭代法求解，或者能够得到解析解，但代价是引入不必要的冗余参数，破坏了模型的最小性，导致需将直线轴和旋转轴分离，进行分步式的运动学标定，影响了末端整体运动的误差标定精度。

发明内容

本申请提供了一种多自由度机构运动学建模方法、装置、设备和存储介质，用于改善现有技术以引入冗余参数为代价，将直线轴和旋转轴分离进行分步式的运动学标定，存在影响机构末端整体运动的误差标定精度的技术问题。

有鉴于此，本申请第一方面提供了一种多自由度机构运动学建模方法，包括：

将多自由度机构的三个直线轴作为一个整体构建点坐标系，基于齐次线性方程组构建所述点坐标系的变换矩阵；

分别构建两个旋转轴的变换矩阵；

构建多自由度机构的末端坐标系到工件坐标系的变换矩阵；

基于所述点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型；

依据姿态表示方法，通过多自由度机构末端的旋转矩阵求解得到旋转轴电机的值，并将多自由度机构末端的平移矩阵作为一个非齐次线性方程组，通过克莱姆法则求解得到直线轴电机的值，得到逆运动学模型。

可选的，所述分别构建两个旋转轴的变换矩阵，包括：

当多自由度机构为先平移后旋转的机构时，采用三个旋转矩阵构建第一旋转轴的变换矩阵，并基于建模方法确定第二旋转轴的变换矩阵；

当多自由度机构为先旋转后平移的机构时，采用三个旋转矩阵构建第二旋转轴的变换矩阵，并基于建模方法确定第一旋转轴的变换矩阵。

可选的，所述基于所述点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型，包括：

当多自由度机构为先平移后旋转的机构时，采用所述点坐标系的变换矩阵依次右乘所述第一旋转轴的变换矩阵、所述第二旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵，得到正运动学模型；

当多自由度机构为先旋转后平移的机构时，采用所述点坐标系的变换矩阵依次左乘所述第二旋转轴的变换矩阵、所述第一旋转轴的变换矩阵，再右乘末端坐标系到工件坐标系的变换矩阵，得到正运动学模型。

本申请第二方面提供了一种多自由度机构运动学建模装置，包括：

直线轴建模单元，用于将多自由度机构的三个直线轴作为一个整体构建点坐标系，基于齐次线性方程组构建所述点坐标系的变换矩阵；

旋转轴建模单元，用于分别构建两个旋转轴的变换矩阵；

工件坐标系建模单元，用于构建多自由度机构的末端坐标系到工件坐标系的变换矩阵；

正运动学模型构建单元，用于基于所述点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型；

逆运动学模型构建单元，用于依据姿态表示方法，通过多自由度机构末端的旋转矩阵求解得到旋转轴电机的值，并将多自由度机构末端的平移矩阵作为一个非齐次线性方程组，通过克莱姆法则求解得到直线轴电机的值，得到逆运动学模型。

可选的，所述旋转轴建模单元，具体用于：

当多自由度机构为先平移后旋转的机构时，采用三个旋转矩阵构建第一旋转轴的变换矩阵，并基于建模方法确定第二旋转轴的变换矩阵；

当多自由度机构为先旋转后平移的机构时，采用三个旋转矩阵构建第二旋转轴的变换矩阵，并基于建模方法确定第一旋转轴的变换矩阵。

可选的，所述正运动学模型构建单元，具体用于：

当多自由度机构为先平移后旋转的机构时，采用所述点坐标系的变换矩阵依次右乘所述第一旋转轴的变换矩阵、所述第二旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵，得到正运动学模型；

当多自由度机构为先旋转后平移的机构时，采用所述点坐标系的变换矩阵依次左乘所述第二旋转轴的变换矩阵、所述第一旋转轴的变换矩阵，再右乘末端坐标系到工件坐标系的变换矩阵，得到正运动学模型。

本申请第三方面提供了一种多自由度机构运动学建模设备，所述设备包括处理器以及存储器；

所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；

所述处理器用于根据所述程序代码中的指令执行第一方面任一种所述的多自由度机构运动学建模方法。

本申请第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储程序代码，所述程序代码被处理器执行时实现第一方面任一种所述的多自由度机构运动学建模方法。

从以上技术方案可以看出，本申请具有以下优点：

本申请提供了一种多自由度机构运动学建模方法，包括：将多自由度机构的三个直线轴作为一个整体构建点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵；分别构建两个旋转轴的变换矩阵；构建多自由度机构的末端坐标系到工件坐标系的变换矩阵；基于点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型；依据姿态表示方法，通过多自由度机构末端的旋转矩阵求解得到旋转轴电机的值，并将多自由度机构末端的平移矩阵作为一个非齐次线性方程组，通过克莱姆法则求解得到直线轴电机的值，得到逆运动学模型。

本申请中，将直线轴作为一个整体建立点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵，在保持模型最小性的情况下简化了运动学模型，并通过克莱姆法则得到逆运动学模型的解析解，同时本申请没有引入冗余参数，直线轴与旋转轴的参数不具备相关性，因此无需将直线轴与旋转轴分离，能够在两者误差参数互不影响的情况下直接用于整体标定，改善了现有技术以引入冗余参数为代价，将直线轴和旋转轴分离进行分步式的运动学标定，存在影响机构末端整体运动的误差标定精度的技术问题。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1为本申请实施例提供的一种多自由度机构运动学建模方法的一个流程示意图；

图2为本申请实施例提供的一种五轴机床的运动副示意图；

图3为本申请实施例提供的逆运动学模型求解对比图；

图4为本申请实施例提供的正运动学模型辨识误差的二范数和计算时间对比图；

图5为本申请实施例提供的一种多自由度机构运动学建模装置的一个结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

现有的运动学建模方法通常采用DH(Denavit-Hartenberg)、POE(Product ofExponentials Formula)、CPC等方法进行模型的建立，使得每个轴的误差参数至少有4个，例如DH模型，对于第

；

若运动学标定的对象为五轴机床，则要经过五个这样的变换矩阵相乘才能得到运动学模型，要从这复杂的运动学模型中得到逆运动学模型就更加困难了。为了解决这一问题，首先要简化运动学模型，现有技术对直线轴的运动学模型进行了改进，以引入冗余参数为代价，将旋转部分和平移部分分开，分别用一个旋转矩阵和一个非齐次线性方程组表示。但这需要按照直线轴和旋转轴的顺序进行分步式的运动学标定，否则在直线轴中引入的冗余参数会影响旋转轴原本的参数，导致最终的标定效果较差，而且末端误差是所有轴的综合影响，这种分步式的标定相比整体标定，无法具有更好的标定效果。

为了改善上述问题，本申请提出一种线性等效直线轴的多自由度机构运动学建模方法，能够在保持模型最小性的情况下，将运动学模型进行简化，并通过克莱姆法则得到逆运动学模型的解析解；同时，该方法没有引入冗余参数，直线轴与旋转轴的参数不具备相关性，因此无需将直线轴与旋转轴分离，能够在两者误差参数互不影响的情况下直接用于整体标定。

为了便于理解，请参阅图1，本申请实施例提供了一种多自由度机构运动学建模方法，包括：

步骤101、将多自由度机构的三个直线轴作为一个整体构建点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵。

本申请实施例以五轴机床为例，五轴机床的运动副示意图如图2所示，从下往上，一共有五个运动副，分别为Y轴电机、X轴电机、Z轴电机、B轴电机与C轴电机。将全局坐标系

本申请实施例将直线轴整合成一个整体，如一个点，并在其放置一个坐标系，其原点与相邻旋转轴坐标系的原点重合，方向与基坐标系平行，然后采用线性方程组描述该点坐标系的运动。具体的，在进行多自由度机构运动学建模时，将三个直线轴（X轴、Y轴和Z轴）视作一个整体，比如一个点，该点始终是Z轴轴线与B轴轴线的交点，会跟随着电机进行

；

式中，

步骤102、分别构建两个旋转轴的变换矩阵。

当多自由度机构为先平移后旋转的机构时，采用三个旋转矩阵构建第一旋转轴（即B轴）的变换矩阵，并基于建模方法确定第二旋转轴（即C轴）的变换矩阵；当多自由度机构为先旋转后平移的机构时，采用三个旋转矩阵构建第二旋转轴的变换矩阵，并基于建模方法确定第一旋转轴的变换矩阵。

以先平移后旋转的多自由度机构为例，在建立好直线轴的运动学模型后，接下来建立旋转轴的运动学模型。在步骤101中忽视了直线轴的旋转部分，故需要在旋转轴的运动学模型中体现。需要注意的是，此时在步骤101建立的点就是B轴的原点，因为已经将Z轴轴线移到与B轴轴线相交的位置了。所以步骤101建立的点坐标系到B轴电机的坐标系只有旋转变换，接下来只需保证这个旋转变换能够达到就行。因为含有三个参数的旋转矩阵能够保证任一旋转，所以采用含有三个旋转参数的POE建模方法时，可直接采用其公式。但是若采用含有两个旋转参数的DH时，还需加多一个绕Y轴的旋转矩阵，以此将直线轴的旋转部分在此体现。在本申请实施例轴，可以用一个

；

式中，

在前4个电机轴的运动学模型建立完成后，接下来可以采用常规方法建立第五个电机轴（即C轴）的运动学模型即可。例如，采用DH法。第五个电机轴的运动学模型可以为：

；

式中，

当多自由度机构为先旋转后平移的机构时，在建立旋转轴的运动学模型时，先建立C轴的运动学模型，再建立B轴的运动学模型。其中，C轴的运动学模型以及B轴的运动学模型的具体建立过程可以参考上述过程，在此不再进行赘述。

步骤103、构建多自由度机构的末端坐标系到工件坐标系的变换矩阵。

在建立五个电机轴的运动学模型后，最后加上工件坐标系即可完成正运动学模型的建立。由于工件坐标系与C轴电机坐标系是平行关系，因此也只有平移运动，故多自由度机构的末端坐标系到工件坐标系的变换矩阵

；

式中，

步骤104、基于点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型。

当多自由度机构为先平移后旋转的机构时，采用点坐标系的变换矩阵依次右乘第一旋转轴的变换矩阵、第二旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵，得到正运动学模型。以先平移后旋转的多自由度机构为例，令

；

；

式中，

当多自由度机构为先旋转后平移的机构时，采用点坐标系的变换矩阵依次左乘第二旋转轴的变换矩阵、第一旋转轴的变换矩阵，再右乘末端坐标系到工件坐标系的变换矩阵，得到正运动学模型。当多自由度机构为先旋转后平移的机构时，采用点坐标系的变换矩阵依次左乘第二旋转轴的变换矩阵、第一旋转轴的变换矩阵，且对旋转轴的建模顺序也要从后往前。

步骤105、依据姿态表示方法，通过多自由度机构末端的旋转矩阵求解得到旋转轴电机的值，并将多自由度机构末端的平移矩阵作为一个非齐次线性方程组，通过克莱姆法则求解得到直线轴电机的值，得到逆运动学模型。

通过前述4个步骤可以建立得到正运动学模型，接下来需要建立逆运动学模型，这需要从步骤104推导的结果中逆向推导电机值，即此时是将正运动学模型中的

将旋转轴电机求出来后，就可以得到旋转矩阵的值，进而得到运动学模型中

；

式中，

通过上述过程可以求解得到多自由度机构末端的旋转矩阵求解得到旋转轴电机的值以及直线轴电机的值，至此逆运动学模型建立完成。

为了验证本申请实施例所提方法的有效性，利用本申请实施例所提方法和数值迭代法对逆运动学模型进行30次仿真，每次仿真进行100个逆运动学模型求解，记录其求解误差与计算时间，得到实验效果如图3所示。通过图3可知，本申请的逆运动学模型在计算精度、计算稳定性与计算效率都远高于数值法。

至于改进的正运动学模型与原运动学模型的区别，本申请实施例以一种五轴串联运动平台为研究对象，实际测量60个点，每次随机选取15个点进行参数辨识，记录辨识精度与计算时间，得到的本申请模型与原模型的辨识效果如图4所示。通过图4可知，本申请实施例的正运动学模型在辨识精度与计算效率上也高于原模型，说明了本申请模型在保留了原模型最小性的同时，也优化了原模型的误差参数表示。本申请实施例中所提方法无需引入冗余参数，即可简化运动学模型，也能保证模型的最小性；由于每个参数都是独立可辨识的，故不需要分步式标定，可直接对多自由度机构进行整体标定，更能体现每个轴的误差对末端误差的综合影响。

本申请实施例中，将直线轴作为一个整体建立点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵，在保持模型最小性的情况下简化了运动学模型，并通过克莱姆法则得到逆运动学模型的解析解，同时本申请没有引入冗余参数，直线轴与旋转轴的参数不具备相关性，因此无需将直线轴与旋转轴分离，能够在两者误差参数互不影响的情况下直接用于整体标定，改善了现有技术以引入冗余参数为代价，将直线轴和旋转轴分离进行分步式的运动学标定，存在影响机构末端整体运动的误差标定精度的技术问题。

以上为本申请提供的一种多自由度机构运动学建模方法的一个实施例，以下为本申请提供的一种多自由度机构运动学建模装置的一个实施例。

请参考图5，本申请实施例提供的一种多自由度机构运动学建模装置，包括：

直线轴建模单元，用于将多自由度机构的三个直线轴作为一个整体构建点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵；

旋转轴建模单元，用于分别构建两个旋转轴的变换矩阵；

工件坐标系建模单元，用于构建多自由度机构的末端坐标系到工件坐标系的变换矩阵；

正运动学模型构建单元，用于基于点坐标系的变换矩阵、两个旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵构建正运动学模型；

逆运动学模型构建单元，用于依据姿态表示方法，通过多自由度机构末端的旋转矩阵求解得到旋转轴电机的值，并将多自由度机构末端的平移矩阵作为一个非齐次线性方程组，通过克莱姆法则求解得到直线轴电机的值，得到逆运动学模型。

作为进一步地改进，旋转轴建模单元，具体用于：

当多自由度机构为先平移后旋转的机构时，采用三个旋转矩阵构建第一旋转轴的变换矩阵，并基于建模方法确定第二旋转轴的变换矩阵；

当多自由度机构为先旋转后平移的机构时，采用三个旋转矩阵构建第二旋转轴的变换矩阵，并基于建模方法确定第一旋转轴的变换矩阵。

作为进一步地改进，正运动学模型构建单元，具体用于：

当多自由度机构为先平移后旋转的机构时，采用点坐标系的变换矩阵依次右乘第一旋转轴的变换矩阵、第二旋转轴的变换矩阵以及末端坐标系到工件坐标系的变换矩阵，得到正运动学模型；

当多自由度机构为先旋转后平移的机构时，采用点坐标系的变换矩阵依次左乘第二旋转轴的变换矩阵、第一旋转轴的变换矩阵，再右乘末端坐标系到工件坐标系的变换矩阵，得到正运动学模型。

本申请实施例中，将直线轴作为一个整体建立点坐标系，基于齐次线性方程组构建点坐标系的变换矩阵，在保持模型最小性的情况下简化了运动学模型，并通过克莱姆法则得到逆运动学模型的解析解，同时本申请没有引入冗余参数，直线轴与旋转轴的参数不具备相关性，因此无需将直线轴与旋转轴分离，能够在两者误差参数互不影响的情况下直接用于整体标定，改善了现有技术以引入冗余参数为代价，将直线轴和旋转轴分离进行分步式的运动学标定，存在影响机构末端整体运动的误差标定精度的技术问题。

本申请实施例还提供了一种多自由度机构运动学建模设备，设备包括处理器以及存储器；

存储器用于存储程序代码，并将程序代码传输给处理器；

处理器用于根据程序代码中的指令执行前述方法实施例中的多自由度机构运动学建模方法。

本申请实施例还提供了一种计算机可读存储介质，计算机可读存储介质用于存储程序代码，程序代码被处理器执行时实现前述方法实施例中的多自由度机构运动学建模方法。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等（如果存在）是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

应当理解，在本申请中，“至少一个（项）”是指一个或者多个，“多个”是指两个或两个以上。“和/或”，用于描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：只存在A，只存在B以及同时存在A和B三种情况，其中A，B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达，是指这些项中的任意组合，包括单项（个）或复数项（个）的任意组合。例如，a，b或c中的至少一项（个），可以表示：a，b，c，“a和b”，“a和c”，“b和c”，或“a和b和c”，其中a，b，c可以是单个，也可以是多个。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以通过一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（英文全称：Read-OnlyMemory，英文缩写：ROM）、随机存取存储器（英文全称：Random Access Memory，英文缩写：RAM）、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。