技术领域
[0001] 本
发明属于人机示教技术领域，具体涉及一种基于相似性度量的多
自由度机械臂仿人运动示教方法。
背景技术
[0002] 目前人机示教方式主要是直接记录人体动作。当仅关注运动捕捉时，可以使用各种现有基于视觉、
外骨骼或其他可佩戴的运动
传感器的运动
跟踪系统。这些跟踪人体运动的外部装置返回对关节的
角位移的精确测量，用于全身运动的各种工作。这些方法的优点在于它们允许人自由运动，但是需要对关节匹配问题有良好的解决方案。通常，通过人和
机器人关节之间的显性映射来实现，但是如果机器人结构与人类有较大的差异，则显性映射可能是相当困难的。主要原因是无法准确定义多自由度机械臂肘关节，使得人机关节映射有较大困难。
[0003] 随着
机器人技术的发展，越来越多的行业对
仿人机器人技术提出了更高的要求。尤其是对于提高机器人的通用性，简化运算来说，使用不是类人机械臂的机器人，模仿人体动作，具有很高的价值。
发明内容
[0004] 本发明针对目前人机示教方式需要提高机器人的通用性的技术问题，提出基于相似性度量的机械臂仿人运动示教方法。
[0005] 为实现上述技术目的，本发明采用以下技术方案：
[0006] 本发明提出基于相似性度量的机械臂仿人运动示教方法包括以下步骤：
[0007] 采用运动捕捉系统获取人手臂的肩、肘和腕三个点的空间坐标，基于获得的空间坐标确定人手臂弯曲后的各
关节角度；基于预先利用机械臂与人手臂的相似度确定的机械臂肘关节，计算机械臂其余的关节角，实现仿人运动。
[0008] 进一步地，预先确定机械臂肘关节的方法如下：
[0009] 以机械臂
基座为起点，从第一个关节开始至最后一个关节，依次改变关节角度使机械臂关节角与预先确定的人手臂弯曲后的肘关节角一致，并基于每次机械臂改变的关节确定机械臂与人手臂的相似度；
[0010] 使用运动量化的比较
算法，将相似度最高时对应的被改变的机械臂关节设定为机械臂肘关节。
[0011] 再进一步地，采用运动捕捉系统获取人的肩、肘和腕三个点的空间坐标，基于获得的空间坐标确定人手臂弯曲后的肘关节角度，计算公式如下：
[0012] d(ja，jb)＝||j(xa，ya，za)-j(xb，yb，zb)||
[0013]
[0014] d(ja，jb)是关节ja和关节jb之间的欧几里得距离；θ肘为手臂弯曲后的肘关节角度；j(xa，ya，za)是关节ja的空间坐标，j(xb，yb，zb)是关节jb的空间坐标。
[0015] 进一步地，预先确定机械臂肘关节的具体方法包括如下步骤：
[0016] 采集机械臂改变每个关节的关键
帧图像，同时采集手肘弯曲至肘关节角度θ肘时的动作关键帧图像。
[0017] 将采集的人手臂和多自由度机械臂运动的关键帧图像分别做量化处理：
[0018] 求人手臂和多自由度机械臂全局运动变换矩阵；
[0019] 求人手臂和多自由度机械臂局部运动变换矩阵；
[0020] 计算肢体之间以及多自由度机械臂弯曲关节的角度，通过检查每个帧中肢体之间以及多自由度机械臂弯曲关节的角度的大小来确定关节的方向；
[0021] 使用比较算法计算人手臂和机械臂中全局运动变换矩阵、局部运动变换矩阵和弯曲关节的角之间的差值，然后根据全局运动变换矩阵、局部运动变换矩阵和弯曲关节的角的给定
阈值来评估它们的相似性并根据相似性确定机械臂肘关节
位置。
[0022] 再进一步地，采集机械臂改变每个关节的关键帧图像的方法为：
[0023] 采用自适应
采样，对关键帧进行提取，用K-means
聚类算法，求取每组动作的N帧关键帧数；选取角度0为初始帧，肘关节角度θ肘为结束帧。
[0024] 进一步地，全局变换矩阵与局部变换矩阵的计算方法相同，仅选取的原点不同，计算公式如下：
[0025]
[0026]
[0027]
[0028]
[0029] Setα是肩、肘、腕三维
节点的起始点，其中第1节点是肩节点，第2节点是肘节点，第3节点是腕节点；Setβ是肩、肘、腕三维节点的结束点； 是起始帧中第i节点的三维节点坐标， 是结束帧中第i节点的三维节点坐标；Centα是Setα的质心；Centβ是Setβ的质心；
[0030]
[0031] [M，N，O]＝SVD(H)
[0032] R＝OMt
[0033] T＝Centβ-R*Centα
[0034] 其中H是协方差矩阵；SVD奇异值分解是将矩阵H分解为三个矩阵(M，N和O)的奇异值分解函数；旋转矩阵R是由O和M计算得到的，其中T表示沿x，y，z轴的点运动的平移矩阵；
[0035]
[0036] TF是SetA和SetB关节运动的变换矩阵；
[0037] θx，θY，θZ＝Euler(R)
[0038] 其中Euler是一个函数，分别计算旋转矩阵R绕X，Y，Z的旋转角θX，θY，θZ，获得旋转矩阵R，其中θX表示旋转矩阵R绕X轴的旋转角，θy表示旋转矩阵R绕Y轴的旋转角，θZ表示旋转矩阵R绕Z轴的旋转角。
[0039] 进一步地，人手臂全局运动变换，以右手臂判断相似性，以颈部为原点；多自由度机械臂全局运动变换以基座标为原点。
[0040] 再进一步地，求人手臂和多自由度机械臂局部运动变换矩阵分别以人手臂和机械臂各关节的父关节点为原点。
[0041] 再进一步地，根据全局运动变换矩阵、局部运动变换矩阵和弯曲关节的角的给定阈值来评估它们的相似性的方法如下：
[0042] (1)对于N帧的运动图像，计算人手臂和多自由度机械臂第i帧量化之后第j关节之间的全局关节平移Gdt和局部关节平移Ldθ以及全局旋转角Gdθ和局部旋转角Ldθ；
[0043] Gdt＝||TG手臂-TG机械臂||
[0044] Ldt＝||TL手臂-TL机械臂||
[0045] Gdθ＝||RG手臂-RG机械臂||
[0046] Ldθ＝||RL手臂-RL机械臂||
[0047] 其中，TG手臂表示人手臂第i帧量化之后第j关节的全局平移矩阵；TG机械臂表示机械臂第i帧量化之后第j关节的全局平移矩阵；TL手臂表示人手臂第i帧量化之后第j关节的局部平移矩阵；TL机械臂表示机械臂第i帧量化之后第j关节的局部平移矩阵；RG手臂表示人手臂第i帧量化之后第j关节的全局旋转矩阵；RG机械臂表示机械臂第i帧量化之后第j关节的全局旋转矩阵；RL手臂表示人手臂第i帧量化之后第j关节的局部旋转矩阵；RL机械臂表示机械臂第i帧量化之后第j关节的局部旋转矩阵；
[0048] (2)根据相似性的严格程度设置第j关节的全局旋转角阈值Gθ，局部旋转角阈值Lθ，全局关节平移阈值Gt，局部关节平移阈值Lt。
[0049] 当Gdθ≤Gθ时
[0050]
[0051] 当Gdt≤Gt时
[0052]
[0053] 当Ldθ≤Lθ时
[0054]
[0055] 当Ldt≤Gt时
[0056]
[0057] 其中，GFrame％为初始全局百分比，优选地设为0；LFrame％为初始局部百分比，优选地设为0；GFrame1j％、GFrame2j％为全局百分比，LFrame1j％、LFrame2j％为局部百分比，为全局旋转角百分比， 为局部旋转角百分比， 为全局平移百分比， 为局部平移百分比，且满足下式：
[0058] 计算人手臂和多自由度机械臂量化之后第i帧，a人肩和b机械臂肩的差值d1；计算a人肘和b机械臂肘之间的差值d2，其中，a人肩为人左手肩部的夹角；b机械臂肩为机械臂肩关节的夹角；a人肘为人左手臂肘部的夹角；b机械臂肘为机械臂肘关节的夹角：
[0059] d1＝||a人肩-b机械臂肩||
[0060] d2＝||a人肘-b机械臂肘||
[0061] 当d1≤θ1时，
[0062] AngFrame_1％＝AngFrame％+a1％
[0063] 当d2≤θ2时，
[0064] AngFrame_2％＝AngFrame_1％+a2％
[0065] θ1是肩关节角阈值，θ2为肘关节角阈值，a1％、a2％为角度百分比，且
[0066] a1％+a2％＝100％，根据权重自行设定百分比。AngFrame％为初始角度百分比，AngFrame_1％和AngFrame_2％为角度百分比。
[0067] 当角度百分比AngFrame_2％＞50％时，两帧之间相似度百分比Framei％为：
[0068] Framei％＝((GFrame_2％+LFrame_2％)+AngFrame_2％)/2
[0069] 如果角度百分比AngFrame_2％≤50％，则Framei％＝0则两个运动的相似度百分比totalsim为：
[0070]
[0071] 有益技术效果：
[0072] 本发明通过
图像处理中相似度量的方法来确定多自由度机械臂肘关节，实现关节数和关节长度未定的多自由度机械臂的仿人运动，将调用程序封装为功能模
块，适合大多数机器人平台，提高机器人的通用性。通过改进的运动量化的比较算法，提高了对两组相似动作相似性判定的准确性。通过全局变换矩阵、局部变换矩阵和弯曲的关节角三种方式的计算形式来联合计算人手臂和机械臂的相似性百分比，可以显著提高计算
精度，减小误差。通过此方法定义多自由度机械臂的肘关节，结合人机示教技术，可以实现任意自由度的机械臂对人体动作的模仿，大大降低了人机示教技术对机械臂的技术要求。
附图说明
[0073] 图1是本发明具体
实施例的设计流程
框图；
[0074] 图2是本发明具体实施例的肘关节相似度对比图；
[0075] 图3是本发明具体实施例中人体手臂和机械臂的全局和局部运动坐标；
[0076] 图4是本发明具体实施例中运动量化和比较方法的
框架。
具体实施方式
[0077] 下面结合附图对本发明作进一步描述，以下实施例仅用于更加清除地说明本发明的技术方案，而不能以此限制本发明的保护和适用范围。
[0078] 如图1所示，本发明使用Kinect(在其他实施例中，也可采用其它运动捕捉系统)获取人手臂上肩肘腕三个点的空间坐标，通过余弦定理获取肘关节角度。以多自由度机械臂基座为起点，先改变第一个关节角度，让机械臂关节角与人手肘关节角一致，然后按关节序列逐个选取机械臂关节，依次使用改进的运动量化的比较算法来计算多自由度机械臂模仿人手臂运动的相似程度，在相似度最高的情况下，将此时选取的机械臂关节定义为机械臂肘关节。使用Kinect识别人手臂的运动状态，计算多自由度机械臂其余的关节角，实现仿人运动。通过这套方法能够实现基于相似性判定的多自由度机械臂的肘关节定义及其示教过程，包括如下步骤：
[0079] S1、获取人手臂弯曲后的肘关节角度。
[0080] S1-1、使用Kinect识别人手臂上肩肘腕三个点的空间坐标。
[0081] S1-2、通过余弦定理获取人手臂弯曲后的肘关节角度θ肘。
[0082] d(ja，jb)＝||j(xa，ya，za)-j(xb，yb，zb)||
[0083]
[0084] d(ja，jb)是关节ja和关节jb之间的欧几里得距离；θ肘为手臂弯曲后的肘关节角度；j(xa，ya，za)是关节ja的空间坐标，j(xb，yb，zb)是关节jb的空间坐标。
[0085] S2、如图2所示，通过相似度确定机械臂肘关节位置。
[0086] S2-1、对人手臂和多自由度机械臂运动作量化处理，以多自由度机械臂基座为起点，按关节序列逐个选取机械臂关节，将此关节变化为所得人手肘关节角度θ肘，如图2所示。通过
降维将骨骼数据进行预处理，把视频序列分解成多个图像帧然后提取选择关键图像。
采用自适应采样，对关键帧进行提取，用K-means聚类算法，求取每组动作的N帧关键帧数。
选取角度0为初始帧，角度θ肘为结束帧，对于i到i+1帧(1＜i≤n帧)，对人手臂和多自由度机械臂运动作量化处理：
[0087] A＝R*B+T
[0088] 其中R是旋转矩阵，T是平移向量，A是运动的起点，B是运动的终点。
[0089] S2-1-1、求人手臂和多自由度机械臂全局运动的变换矩阵，对于全局变换，人手臂以右手臂判断相似性，以颈部为原点，机械臂以基座标为原点。以人右手臂判断相似性，以颈部为原点，机械臂以基座标为原点，如图3所示：
[0090] 人手臂的全局运动变换矩阵：
[0091]
[0092]
[0093]
[0094]
[0095] Setα是人手臂肩、肘、腕三维节点的起始点；Setβ是人手臂肩、肘、腕三维节点的结束点； 是起始帧中第i节点的三维节点坐标， 是结束帧中第i节点的三维节点坐标。Centα是Setα的质心；Centβ是Setβ的质心。
[0096]
[0097] [M，N，O]＝SVD(H)
[0098] R＝OMt
[0099] T＝Centβ-R*Centα
[0100] 其中H是协方差矩阵；SVD奇异值分解是将矩阵H分解为三个矩阵(M，N和O)的奇异值分解函数。旋转矩阵R是由O和M计算得到的，其中T＝[tx，tY，tZ]t，它表示沿x，y，z轴的点的运动，其中tx表示平移矩阵T沿x轴的点的运动，ty表示平移矩阵T沿y轴的点的运动；tZ表示平移矩阵T沿z轴的点的运动；
[0101]
[0102] TF是SetA和SetB关节运动的变换矩阵。
[0103] θx，θY，θZ＝Euler(R)
[0104] 其中Euler是一个函数，分别计算旋转矩阵R绕X，Y，Z的旋转角θX，θY，θZ。
[0105] 多自由度机械臂全局运动变换矩阵和局部运动变换矩阵的计算方法方法和人手臂的全局运动变换矩阵和局部运动变换矩阵的计算方法相同，将人手臂图像关键帧中第i节点的三维节点坐标换成多自由度机械臂图像关键帧中第i节点的三维节点坐标即可。
[0106] S2-1-2、求局部运动的变换矩阵，局部变换矩阵，以父关节为原点，如图3所示，j1、j2、j3、j4，分别对应人体颈部、肩部、肘部和腕部和多自由度机械臂的基座，1到3关节，4关节，5到7关节，j3即为j4的父关节点，局部变换矩阵计算方法和全局运动相同，仅选取的原点不同。
[0107] S2-1-3、计算肢体之间以及多自由度机械臂弯曲关节的角度，通过检查每个帧中肢体之间以及多自由度机械臂弯曲关节的角度的大小来确定关节的方向。和S1-2相似，利用余弦定理来计算基于其
顶点坐标的手臂或多自由度机械臂形成的三角形的角度。在每个帧中计算肢体和机械臂弯曲关节之间的夹角。
[0108] d(ja，jb)＝||j(xa，ya，za)-j(xb，yb，zb)||
[0109]
[0110] 同理可以计算出a人肩，b机械臂肩，b机械臂肘的角度，其中，a人肩为人左手肩部的夹角；b机械臂肩为机械臂肩关节的夹角；a人肘为人左手臂肘部的夹角；b机械臂肘为机械臂肘关节的夹角。
[0111] S2-2、通过相似度量确定多自由度机械臂肘关节位置，如图4所示，使用改进的运动量化的比较算法，计算多自由度机械臂各关节角每次变化之后，和人手臂运动的相似程度。使用比较算法计算两个运动度量之间的距离，然后根据每个度量的给定阈值来评估它们的相似性。通过选取相似程度最高的一种状态，将此时变化的关节作为模仿人臂运动的多自由度机械臂的肘关节。
[0112] S2-2-1、对于N帧的运动图像，计算人手臂和多自由度机械臂第i帧量化之后第j关节之间的全局关节平移Gdt和局部关节平移Ldθ以及全局旋转角Gdθ和局部旋转角Ldθ。
[0113] Gdt＝||TG手臂-TG机械臂||
[0114] Ldt＝||TL手臂-TL机械臂||
[0115] Gdθ＝||RG手臂-RG机械臂||
[0116] Ldθ＝||RL手臂-RL机械臂||
[0117] 其中，TG手臂表示人手臂第i帧量化之后第j关节的全局平移矩阵；TG机械臂表示机械臂第i帧量化之后第j关节的全局平移矩阵；TL手臂表示人手臂第i帧量化之后第j关节的局部平移矩阵；TL机械臂表示机械臂第i帧量化之后第j关节的局部平移矩阵；RG手臂表示人手臂第i帧量化之后第j关节的全局旋转矩阵；RG机械臂表示机械臂第i帧量化之后第j关节的全局旋转矩阵；RL手臂表示人手臂第i帧量化之后第j关节的局部旋转矩阵；RL机械臂表示机械臂第i帧量化之后第j关节的局部旋转矩阵。
[0118] S2-2-2、根据相似性的严格程度设置第j关节的全局旋转角阈值Gθ，局部旋转角阈值Lθ，全局关节平移阈值Gt，局部关节平移阈值Lt。
[0119] 当Gdθ≤Gθ时
[0120]
[0121] 当Gdt≤Gt时
[0122]
[0123] 当Ldθ≤Lθ时
[0124]
[0125] 当Ldt≤Gt时
[0126]
[0127] 其中，GFrame％＝0为初始全局百分比，LFrame％＝0为初始局部百分比，GFrame1j％、GFrame2j％为全局百分比，LFrame1j％、LFrame2j％为局部百分比， 为全局旋转角百分比， 为局部旋转角百分比， 为全局平移百分比， 为局部平移百
分比，且 根据权重自行设定百
分比。
[0128] 计算人手臂和多自由度机械臂量化之后第i帧，a人肩和b机械臂肩的差值d1；计算a人肘和b机械臂肘之间的差值d2，其中，a人肩为人左手肩部的夹角；b机械臂肩为机械臂肩关节的夹角；a人肘为人左手臂肘部的夹角；b机械臂肘为机械臂肘关节的夹角：
[0129] d1＝||a人肩-b机械臂肩||
[0130] d2＝||a人肘-b机械臂肘||
[0131] 当d1≤θ1时，
[0132] AngFrame_1％＝AngFrame％+a1％
[0133] 当d2≤θ2时，
[0134] AngFrame_2％＝AngFrame_1％+a2％
[0135] θ1，θ2为各个关节角阈值，a1％、a2％为角度百分比，且
[0136] a1％+a2％＝100％，根据权重自行设定百分比。AngFrame％为初始角度百分比，AngFrame_1％和AngFrame_2％为角度百分比。
[0137] 当角度百分比AngFrame_2％＞50％时，两帧之间相似度百分比Framei％为：
[0138] Framei％＝((GFrame_2％+LFrame_2％)+AngFrame_2％)/2
[0139] 如果角度百分比AngFrame_2％≤50％，则Framei％＝0则两个运动的相似度百分比totalsim为：
[0140]
[0141] 本发明通过全局变换矩阵、局部变换矩阵和弯曲的关节角三种方式的计算形式来联合计算人手臂和机械臂的相似性百分比，可以显著提高计算精度，减小误差。通过此方法定义多自由度机械臂的肘关节，结合人机示教技术，可以实现任意自由度的机械臂对人体动作的模仿，大大降低了人机示教技术对机械臂的技术要求。
[0142] S3、使用Kinect识别人手臂的运动状态，计算多自由度机械臂其余的关节角，实现仿人运动。
[0143] S3-1、进行人机关节映射，将人手臂与多自由度机械臂的肩肘腕关节进行一一映射。
[0144] S3-2、进行人臂运动的二次描述，通过平移和旋转变换，将人臂运动中坐标的变化，通过Kinect识别，在人臂基座标系中进行描述。
[0145] S3-3、根据前期捕捉到的若干帧关节数据作为训练样本，进行关节预测，用以修复肢体遮挡等原因造成的关节点丢失。利用泰勒公式(不带余项)的n阶导数后向差分来推导骨骼关节预测公式，如下式所示：
[0146]
[0147] 式中，xn+1|n为用第n帧骨骼数据及其前几帧数据来预测第n+1帧骨骼数据；N为预测样本数；f(i)(n)为泰勒公式的i阶导数。
[0148] 泰勒公式的一阶导数、二阶导数和三阶导数可以分别用下式近似表示：
[0149]
[0150] 式中，xn为输入的第n帧骨骼数据。
[0151] 以上可以预测出n+1帧x坐标的骨骼数据，同理可以得出其余两个坐标。同时使用改进的丢失关节修复算法来修复丢失的连续多个关节点或肢体末端关节点。
[0152] 本发明通过图像处理中相似度量的方法来确定多自由度机械臂肘关节，实现关节数和关节长度未定的多自由度机械臂的仿人运动，将调用程序封装为功能模块，适合大多数机器人平台，提高机器人的通用性。通过改进的运动量化的比较算法，提高了对两组相似动作相似性判定的准确性。
[0153] S3-4、结合运动学齐次变换法和空间向量法，使用运动学逆解方法来求解多自由度机械臂各关节角的角度。然后利用消抖滤波法消除因kinect出现不能识别手部
姿态从而给出异常信息的现象，减少机械臂的抖动。最后，多自由度机械臂可以实现仿人运动。
[0154] S4、对多自由度机械臂的仿人动作进行测评，观察是否满足要求。
[0155] 本领域内的技术人员应明白，本
申请的实施例可提供为方法、系统、或
计算机程序产品。因此，本申请可采用完全
硬件实施例、完全
软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘
存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
[0156] 本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的
流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程
数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中
指定的功能的装置。
[0157] 这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0158] 这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0159] 以上结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和
权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。
