CN108427282A - 一种基于示教学习的机器人逆运动学求解方法 - Google Patents

Abstract

本发明属于机器人运动学求解领域，并具体公开了一种基于示教学习的机器人逆运动学求解方法，包括如下步骤：采集N组机器人的关节角度及末端执行器的笛卡尔位置和欧拉角，获得数据集f；对数据集f进行优化获得数据集f₁；对数据集f₁迭代求解出高斯模型个数2～50的高斯混合模型参数；计算各高斯混合模型对应的贝叶斯信息准则值，绘制贝叶斯信息准则曲线，根据曲线确定高斯个数k；求解高斯模型个数为k的高斯混合模型的参数；利用模型参数对数据集f₁的笛卡尔位置和欧拉角进行高斯回归处理求解逆运动学关节角度值；对比关节角度值与数据集f₁中的关节角度值，修正高斯模型个数得到期望的求解精度。本发明具有计算速度快，适用性强，实时控制效果好等优点。

Description

一种基于示教学习的机器人逆运动学求解方法

技术领域

本发明属于机器人运动学求解领域，更具体地，涉及一种基于示教学习的机器人逆运动学求解方法。

背景技术

机器人逆运动学求解是机器人实时控制的基础。它直接关系到机器人离线编程、轨迹规划，运动控制等工作，对于任意构型的机器人如nR型机器人，PR型机器人以及柔性机器人，只有脱离传统机器人运动学表达式，采用数据驱动逆运动学求解的形式，才能实现对任意机器人构型的笛卡尔空间与关节空间建立映射关系，对于实时控制具有重要的意义。

专利CN201410121131.5公开了一种多自由度机器人的逆运动学通用求解方法，运用共性空间理论建立nR机器人的通用运动学方程，通过确定各关节矢量的投影加权值实现逆运动学的半解析求解，同时兼顾空间避障，能够提高计算速度与解算精度，但是仅限于nR型机器人且仍然依赖于逆运动学的解析形式，无法扩展于其他构型机器人，实时控制效果无法进一步提升。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于示教学习的机器人逆运动学求解方法，通过采用高斯混合模型的机器学习算法，由此解决依赖于解析表达式的机器人逆运动学求解计算速度慢的问题，具有计算速度快，适用性强，实时控制效果好等优点，可应用于各类型机器人的控制中。

为实现上述目的，本发明提出了一种基于示教学习的机器人逆运动学求解方法，该方法包括如下步骤：

(a)采集N组机器人的关节角度θ以及机器人末端执行器的笛卡尔位置P和欧拉角R，获得数据集f＝{θ(i),P(i),R(i),i＝1,2,...,N}；

(b)对数据集f进行优化处理，获得具有逆运动学映射的数据集f₁＝{θ(j),P(j),R(j),j＝1,2,...,n(n＜N)}；

(c)对数据集f₁进行迭代求解，分别求解出高斯模型个数m为2～50的高斯混合模型的参数；

(d)分别计算高斯模型个数m为2～50的高斯混合模型所对应的贝叶斯信息准则值BIC，并绘制贝叶斯信息准则曲线，根据贝叶斯信息准则曲线确定贝叶斯信息准则曲线下降速度最缓处对应的高斯个数k；

(e)重新对数据集f₁进行迭代求解，以求解高斯模型个数为k的高斯混合模型的参数(π,μ,Σ)；

(f)利用高斯混合模型的参数(π,μ,Σ)对数据集f₁中的笛卡尔位置P(j)和欧拉角R(j)进行高斯回归处理，以求解逆运动学关节角度值θ^*(j)；

(g)对比逆运动学关节角度值θ^*(j)与数据集f₁中的关节角度值θ(j)，判断是否满足逆运动学求解精度要求，若否，则修正高斯模型个数为k+1并返回步骤(e)，若是，则θ^*(j)即为所需的逆运动学关节角度值；以此方式完成机器人的逆运动学求解。

作为进一步优选的，步骤(b)中对数据集f进行优化处理具体为：首先删除数据集中不满足条件的数据点，然后从剩下的数据点中筛选出满足性能要求的数据点。

作为进一步优选的，步骤(b)中对数据集f进行优化处理优选为：首先删除数据集f中机器人处于奇异的数据点以及逆运动学冗余解，然后从剩下的数据点中筛选出满足性能要求的数据点。

作为进一步优选的，所述性能要求优选为灵巧性要求或操作性要求。

作为进一步优选的，所述灵巧性要求具体为使灵巧性评价指标k_F(J(θ(i)))大于灵巧性预设值，所述灵巧性评价指标k_F(J(θ(i)))采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，s为雅克比矩阵J(θ(i))的行数，θ(i)为数据集中第i个数据点的关节角度值，tr(·)为求矩阵迹的运算符号。

作为进一步优选的，所述操作性要求具体为使操作性评价指标w大于操作性预设值，所述操作性评价指标w采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，det(·)为求矩阵行列式的值。

作为进一步优选的，步骤(c)和步骤(e)中优选采用期望最大算法对数据集进行迭代求解。

作为进一步优选的，贝叶斯信息准则值BIC采用如下公式计算：

其中，L为数据集f₁的高斯混合模型似然概率值的对数形式，m为高斯混合模型中高斯模型的个数，D为数据集f₁的数据维度，n为数据集f₁的数据点个数。

作为进一步优选的，数据集f₁的高斯混合模型似然概率值的对数形式L采用如下公式计算：

其中，n为数据集f₁中数据点的个数，m为高斯混合模型中高斯模型的个数，N为高斯分布计算式，j为数据集f₁中第j个数据点，i为高斯混合模型中第i个高斯模型，ξ_j＝[θ(j),P(j),R(j)]^T，θ(j)为数据集f₁中第j个关节角度，P(j)为数据集f₁中第j个笛卡尔位置，R(j)为数据集f₁中第j个欧拉角度，π_i,μ_i,Σ_i为高斯混合模型中第i个高斯模型对应的高斯参数。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明通过采用高斯混合模型的建模方法，建立机器人笛卡尔空间与关节空间的单一映射，通过计算贝叶斯信息准则(BIC)值，确定高斯模型的高斯个数，可以完全脱离传统的逆运动学解析表达式，提高逆运动求解的计算速度。

2.本发明通过针对任意机器人构型的示教运动数据集进行离线逆运动学映射建模与在线逆运动学求解结合的方式进行机器人逆运动学求解，可以根据机器人性能评价指标，筛选适当的示教运动数据集。

3.本发明通过利用基于运动示教与高斯混合模型结合的离线处理与在线逆运动学求解的方式，离线处理过程可以根据机器人性能评价指标，建立机器人笛卡尔空间与关节空间的单一映射，在线逆运动学求解过程，根据离线处理得到的高斯混合模型参数，实现在线对机器人关节角度的高实时性计算，整个过程既兼顾了机器人性能，同时又不依赖于机器人逆运动学的解析表达式，并且可以实现机器人逆运动学的高实时性计算，能有效满足对机器人***实时控制的要求。

附图说明

图1是按照本发明的优选实施例构建的基于示教学习的机器人逆运动学求解方法流程图；

图2是按照本发明的优选实施例构建的机器人运动示教获取机器人运动数据过程中所使用的设备图；

图3是按照本发明的优选实施例构建的机器人运动示教采集到的原始数据集的离线处理的过程；

图4是按照本发明的优选实施例的BIC曲线图；

图5是机器人关节角度值与数据集中的理论关节角度值之间的偏差值示意图；

图6是按照本发明的优选实施例构建的高斯回归逆运动学在线实时求解的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明实施例提供的一种基于示教学习的机器人逆运动学求解方法，该方法包括如下步骤：

(a)使示教机器人运动，采集N组机器人的关节角度θ以及机器人末端执行器的笛卡尔位置P和欧拉角R，将N组关节角度θ、笛卡尔位置P和欧拉角R结合生成多维数据集，构建数据集f＝{θ(i),P(i),R(i),i＝1,2,...,N}；

(b)对数据集f进行优化处理，获得具有一对一逆运动学映射的数据集f₁＝{θ(j),P(j),R(j),j＝1,2,...,n(n＜N)}；具体采用如下方式进行数据集f的优化处理：首先删除数据集f中不满足条件的数据点，然后从剩下的数据点中筛选出满足性能要求的数据点，将这些数据点构建获得数据集f₁。

优选的，首先删除数据集f中机器人处于奇异的数据点，同时删除数据集中的逆运动学冗余解，然后从剩下的数据点中筛选出满足性能要求的数据点。所述删除数据集中的逆运动学冗余解指对多组关节角度对应同一组笛卡尔位置P和欧拉角R的情况进行筛选，按统一准则保留其中一组，而删除其他组关节角度，筛选准则可根据实际需要进行限定，例如保留最靠近机器人零位的关节角度等。

进一步的，性能要求优选为灵巧性要求或操作性要求，当然也可以为其他性能要求，其中，灵巧性要求具体为使灵巧性评价指标k_F(J(θ(i)))大于灵巧性预设值，即将计算的灵巧性评价指标k_F(J(θ(i)))满足大于灵巧性预设值条件的对应数据点筛选出，而计算的灵巧性评价指标k_F(J(θ(i)))不满足大于灵巧性预设值条件的对应数据点予以删除，该灵巧性预设值可根据实际需要进行限定，灵巧性评价指标k_F(J(θ(i)))采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，s为雅克比矩阵J(θ(i))的行数，θ(i)为数据集f中第i个数据点的关节角度值，tr(·)为求矩阵迹的运算符号。

而操作性要求具体为使操作性评价指标w大于操作性预设值，即将计算的操作性评价指标w满足大于操作性预设值条件的对应数据点筛选出，而计算的操作性评价指标w不满足大于操作性预设值条件的数据点予以删除，该操作性预设值可根据实际需要进行限定，操作性评价指标w采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，θ(i)为数据集f中第i个数据点的关节角度值，det(·)为求矩阵行列式的值。

(c)对数据集f₁进行迭代求解，分别求解出高斯模型个数m为2～50的高斯混合模型(GMM)的参数，即分别求解出高斯模型个数m为2的高斯混合模型对应的参数，高斯模型个数m为3的高斯混合模型对应的参数，依次类推，直至求解出高斯模型个数m为50的高斯混合模型对应的参数；对于高斯混合模型中的每个高斯模型均需要进行迭代求解，以分别求解出每个高斯模型对应的高斯分布的先验概率(也称高斯分布的系数)、均值和协方差这三个参数，各个高斯模型对应的三个参数组成参数集合即为高斯混合模型的参数，而对于迭代的次数可根据实际需要进行选择。

优选的，采用期望最大算法对数据集f₁迭代求解高斯个数m从2到50的高斯混合模型参数，采用期望最大算法计算高斯混合模型参数为现有技术，在此不赘述，仅作简要说明。

例如以高斯模型个数m为3的高斯混合模型为例进行简要说明，其需要对高斯混合模型中的各高斯模型分别进行迭代求解以求解出各自对应的参数，即分别迭代求解第kt个(kt为1,2,3)高斯模型的高斯分布的先验概率、均值和协方差这三个参数，经过u+1次迭代后的三个参数分别采用如下公式计算：

其中，为第kt个高斯模型的第u+1次迭代的高斯分布的先验概率，为第kt个高斯模型的第u+1次迭代的高斯分布的均值，为第kt个高斯模型的第u+1次迭代的高斯分布的协方差：kt为高斯模型的序号；n为数据集f₁的数据点个数；为ξ_j的后验概率，ξ_j为数据集f₁中第j个数据点，ξ_j＝[θ(j),P(j),R(j)]^T；u为迭代次数；

具体的：

其中，为第kt个高斯模型的第u次迭代的高斯分布的先验概率；表示ξ_j服从分布的概率；ξ_j为数据集f₁中第j个数据点，ξ_j＝[θ(j),P(j),R(j)]^T；为第kt个高斯模型的第u次迭代的高斯分布的均值；为第kt个高斯模型的第u次迭代的高斯分布的协方差；为第i个高斯模型的第u次迭代的高斯分布的先验概率；表示ξ_j服从分布的概率；为第i个高斯模型的第u次迭代的高斯分布的均值；为第i个高斯模型的第u次迭代的高斯分布的协方差；m为高斯混合模型中高斯模型的个数。

(d)根据步骤(c)计算的参数分别计算高斯模型个数m为2～50的高斯混合模型所对应的贝叶斯信息准则值BIC，并绘制贝叶斯信息准则(BIC)曲线，根据BIC曲线选择曲线下降速度最缓处对应的高斯个数k；

具体的，下降速度最缓处对应一线段，该线段对应两个高斯模型个数值，以前面的点作为k，即以小的那一个作为k，例如，如图4所示，下降速度最缓处对应的是通过高斯个数为44及高斯个数为45的线段，则以小的那个高斯个数为k，即k为44。若有两处下降速度一致，且为下降速度最缓处，则以前面的点为k，即有两段线段斜率一致，且下降速度最平缓，那么这两段线段对应四个高斯模型个数，则以四个高斯模型个数中最小的那个高斯模型个数为k

进一步的，贝叶斯信息准则值BIC采用如下公式计算：

其中，L为数据集f₁的高斯混合模型似然概率值的对数形式，m为高斯混合模型中高斯模型的个数，D为数据集f₁的数据维度，n为数据集f₁的数据点个数。

进一步的，数据集f₁的高斯混合模型似然概率值的对数形式L采用如下公式计算：

ξ_j＝[θ(j),P(j),R(j)]^T

其中，n为数据集f₁中数据点的个数，m为高斯混合模型中高斯模型的个数，N为高斯分布计算式，j为数据集f₁中第j个数据点，i为高斯混合模型中第i个高斯模型，ξ_j为数据集f₁中第j个数据点，θ(j)为数据集f₁中第j个关节角度，P(j)为数据集f₁中第j个笛卡尔位置，R(j)为数据集f₁中第j个欧拉角度，π_i,μ_i,Σ_i为高斯混合模型中第i个高斯模型对应的高斯参数，即π_i,μ_i,Σ_i分别为高斯混合模型中第i个高斯模型的先验概率，高斯混合模型中第i个高斯模型的均值，高斯混合模型中第i个高斯模型的协方差。

(e)重新对数据集f₁进行迭代求解，以求解高斯模型个数为k的高斯混合模型参数π,μ,Σ，优选利用期望最大算法对数据集f₁迭代求解高斯个数为k的高斯模型(GMM)的参数π,μ,Σ，该π,μ,Σ三个参数均为集合，分别包括1-k个对应的参数，即π为具有k个高斯模型的高斯混合模型中第一个高斯模型先验概率至第k个高斯模型先验概率的集合，μ为高斯混合模型中第一个高斯模型的均值至第k个高斯模型的均值的集合，Σ为高斯混合模型中第一个高斯模型的协方差至第k个高斯模型的协方差的集合。

其中，每个高斯模型的均值用μ_i表示，i＝1,2,...,k；μ_i为12×1的矩阵，将其表示为 为μ_i位于上方的6×1矩阵，定义为输入信号的第i个高斯模型的均值，为μ_i位于下方的6×1矩阵，定义为输出信号的第i个高斯模型的均值，即将矩阵μ_i等分为上下两块；每个高斯模型的协方差用Σ_i表示，i＝1,2,...,k；Σ_i为12×12的矩阵，将其表示为 为Σ_i左上角对应的6×6矩阵，定义为输入信号的第i个高斯模型的协方差，为Σ_i右上角对应的6×6矩阵，定义为输入-输出信号的第i个高斯模型的协方差，为Σ_i左下角对应的6×6矩阵，定义为输入-输出信号的第i个高斯模型的协方差的转置矩阵，为Σ_i右下角对应的6×6矩阵，定义为输出信号的第i个高斯模型的协方差，即将矩阵Σ_i等分为上下左右四块。

(f)利用高斯混合模型的参数π,μ,Σ对数据集f₁中的笛卡尔位置P(j)和欧拉角R(j)进行高斯回归处理(GMR)，求解获得逆运动学关节角度值θ^*(j)；

具体的，逆运动学关节角度值θ^*(j)采用如下公式计算获得：

其中，k为高斯混合模型中高斯模型的个数，i为高斯模型的序号；

为输出信号的第i个高斯模型的均值，为输入信号的第i个高斯模型的均值，为输入-输出信号的第i个高斯模型的协方差的转置矩阵，为输入信号的第i个高斯模型的协方差，

π_i为第i个高斯模型的先验概率，N为高斯分布计算式，π_s为第s个高斯模型的先验概率，为输入信号的第s个高斯模型的均值，为输入信号的第s个高斯模型的协方差；

(g)将逆运动学关节角度值θ^*(j)与数据集f₁中对应的关节角度值θ(j)进行比较，判断是否满足逆运动学求解精度要求，若否，则修正高斯模型个数为k+1并返回步骤(e)，以继续求解高斯模型个数为k+1的高斯混合模型的参数π,μ,Σ，即将高斯模型个数k更新为k+1(使步骤(e)中的k＝k+1)，以求解高斯模型个数为k+1的高斯混合模型的参数；若是，则θ^*(j)即为所需的逆运动学关节角度值；以此方式完成机器人的逆运动学求解，机器人控制器根据求得的逆运动学关节角度值控制机器人的各关节运动。

具体的，逆运动学求解精度可根据实际需要进行限定，例如，将求解精度定义为0.002rad(弧度)，则使|θ^*(j)-θ(j)|≤0.002rad即为满足求解精度要求。

实际应用中，可将本发明的上述步骤编写成程序，并嵌入机器人的控制器中以实现机器人逆运动学实时求解。

下面结合具体的实例对本发明的方法做进一步的说明。

如图2所示，本发明使用到的设备包括机器人本体1、Kinect视觉相机2、机器人控制器3和工业计算机4，机器人本体1固定于水平基台上，用于在桌面空间范围内移动和作业；Kinect视觉相机2用于测量机器人末端的笛卡尔位置和欧拉角度；机器人控制器3位于机器人所在水平基台的下方内，用于驱动机器人各个关节电机以实现机器人运动；工业计算机4用于离线处理示教数据集以及编写机器人逆运动学实时在线求解程序，通过以太网实现与机器人控制器3的通信。

在实施方式中，目标机器人本体包括机器人操作臂、机器人驱动器，其中：机器人操作臂具有六自由度关节式结构；机器人驱动器根据工业计算机提供的控制程序控制机器人按照用户的要求驱动机器人各关节电机转动。

图3为按照本发明的优选实施例构建的机器人运动示教采集到的原始数据集的离线处理过程，用以得到示教数据集的高斯混合模型参数(π,μ,Σ)。

图4为BIC曲线图，用以确定高斯混合模型中高斯模型的个数。图5为数据集的高斯回归离线处理得到的机器人关节角度值与数据集中的理论关节角度值之间的偏差值(只列举第三关节角度)，用以修正高斯混合模型中高斯的个数并提高逆运动学求解精度。图6为按照本发明的优选实施例构建的高斯回归逆运动学在线实时求解的流程示意图，如图6所示，在实际实施中，采集机器人末端位置和欧拉角度，进入高斯混合模型，进而通过高斯回归得到机器人的关节角度结果。

该具体实施例的具体求解过程为：首先进行机器人运动示教实验，采集得到实验过程中的数据集f，根据图3所示的离线处理过程对数据集f进行离线处理，得到高斯混合模型参数(π,μ,Σ)，然后根据高斯混合模型参数(π,μ,Σ)对笛卡尔位置和欧拉角进行高斯回归处理(GMR)求解逆运动学关节角度值θ^*，最后机器人的关节控制器根据逆运动学关节角度值θ^*控制机器人的运动。

本发明中采用基于运动示教数据集的高斯混合模型进行机器人运动控制的在线实时逆运动学求解的方法，建立离线的基于运动示教采样数据集的高斯混合模型聚类过程，根据离线得到的高斯混合模型参数实现在线的机器人关节角度的实时逆运动学求解处理。

通过本发明，可以通过离线数据分析与在线逆运动学求解相结合的方法实现给定机器人末端执行器位置的实时在线逆运动学求解，解决机器人逆运动学的奇异位置规避与冗余解的选解等问题，同时无需计算显式的机器人逆运动学表达式，提高机器人***的控制实时性。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于示教学习的机器人逆运动学求解方法，其特征在于，该方法包括如下步骤：

(a)采集N组机器人的关节角度θ以及机器人末端执行器的笛卡尔位置P和欧拉角R，获得数据集f＝{θ(i),P(i),R(i),i＝1,2,...,N}；

(b)对数据集f进行优化处理，获得具有逆运动学映射的数据集f₁＝{θ(j),P(j),R(j),j＝1,2,...,n(n＜N)}；

(c)对数据集f₁进行迭代求解，分别求解出高斯模型个数m为2～50的高斯混合模型的参数；

(d)分别计算高斯模型个数m为2～50的高斯混合模型所对应的贝叶斯信息准则值BIC，并绘制贝叶斯信息准则曲线，根据贝叶斯信息准则曲线确定贝叶斯信息准则曲线下降速度最缓处对应的高斯个数k；

(e)重新对数据集f₁进行迭代求解，以求解高斯模型个数为k的高斯混合模型的参数(π,μ,Σ)；

(f)利用高斯混合模型的参数(π,μ,Σ)对数据集f₁中的笛卡尔位置P(j)和欧拉角R(j)进行高斯回归处理，以求解逆运动学关节角度值θ^*(j)；

(g)对比逆运动学关节角度值θ^*(j)与数据集f₁中的关节角度值θ(j)，判断是否满足逆运动学求解精度要求，若否，则修正高斯模型个数为k+1并返回步骤(e)，若是，则θ^*(j)即为所需的逆运动学关节角度值；以此方式完成机器人的逆运动学求解。

2.如权利要求1所述的基于示教学习的机器人逆运动学求解方法，其特征在于，步骤(b)中对数据集f进行优化处理具体为：首先删除数据集中不满足条件的数据点，然后从剩下的数据点中筛选出满足性能要求的数据点。

3.如权利要求2所述的基于示教学习的机器人逆运动学求解方法，其特征在于，步骤(b)中对数据集f进行优化处理优选为：首先删除数据集f中机器人处于奇异的数据点以及逆运动学冗余解，然后从剩下的数据点中筛选出满足性能要求的数据点。

4.如权利要求2或3所述的基于示教学习的机器人逆运动学求解方法，其特征在于，所述性能要求优选为灵巧性要求或操作性要求。

5.如权利要求4所述的基于示教学习的机器人逆运动学求解方法，其特征在于，所述灵巧性要求具体为使灵巧性评价指标k_F(J(θ(i)))大于灵巧性预设值，所述灵巧性评价指标k_F(J(θ(i)))采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，s为雅克比矩阵J(θ(i))的行数，θ(i)为数据集中第i个数据点的关节角度值，tr(·)为求矩阵迹的运算符号。

6.如权利要求4或5所述的基于示教学习的机器人逆运动学求解方法，其特征在于，所述操作性要求具体为使操作性评价指标w大于操作性预设值，所述操作性评价指标w采用如下公式计算：

其中，J(θ(i))为机器人运动学雅克比矩阵，det(·)为求矩阵行列式的值。

7.如权利要求1所述的基于示教学习的机器人逆运动学求解方法，其特征在于，步骤(c)和步骤(e)中优选采用期望最大算法对数据集进行迭代求解。

8.如权利要求1所述的基于示教学习的机器人逆运动学求解方法，其特征在于，贝叶斯信息准则值BIC采用如下公式计算：

其中，L为数据集f₁的高斯混合模型似然概率值的对数形式，m为高斯混合模型中高斯模型的个数，D为数据集f₁的数据维度，n为数据集f₁的数据点个数。

9.如权利要求8所述的基于示教学习的机器人逆运动学求解方法，其特征在于，数据集f₁的高斯混合模型似然概率值的对数形式L采用如下公式计算：

其中，n为数据集f₁中数据点的个数，m为高斯混合模型中高斯模型的个数，N为高斯分布计算式，j为数据集f₁中第j个数据点，i为高斯混合模型中第i个高斯模型，ξ_j＝[θ(j),P(j),R(j)]^T，θ(j)为数据集f₁中第j个关节角度，P(j)为数据集f₁中第j个笛卡尔位置，R(j)为数据集f₁中第j个欧拉角度，π_i,μ_i,Σ_i为高斯混合模型中第i个高斯模型对应的高斯参数。