本发明属于机器人运动学、机器视觉、数据校验领域领域，公开了一种面向七轴机械臂视觉示教的可变姿态映射方法，首先采集演示者手臂的肩部、手肘和手腕坐标，并转化到以肩部为原点的坐标系下；然后用肩、肘、腕坐标值计算出演示者大臂长度、小臂长度以及末端距离，并根据最近n次计算、采集的值作为样本，基于Grubbs准则进行校正；再根据映射模式对腕部坐标进行调整；最后根据机械臂DH参数调整映射后的肘部坐标，从而完成姿态映射。本发明解决了现有示教技术难以实现的整体姿态控制的技术问题，同时为实现灵敏度可变的机器人姿态映射功能提供一种指导性的映射方法。
1.一种面向七轴机械臂视觉示教的可变姿态映射方法，其特征在于，包括如下步骤：步骤1：提取演示者骨架信息，采集肩部、手肘和手腕坐标数据；
步骤2：将各关节坐标从镜头坐标系转换到机器人的基坐标系下；
步骤3：根据各关节坐标值计算大、小臂长度及末端距离的初值；
步骤3.1：记录最初n次的采集、计算结果作为样本，求出样本的均值、标准差和最大残差，选定显著性水平a的取值，从Grubbs准则临界值表上查询到相应的临界值G(a,n)，Grubbs准则异常值的判断公式为：其中，为均值，σ为标准差，当上式成立时，说明xi为异常值，将其剔除，再补充最新采集、计算的结果到样本，以保持样本量为n；
步骤3.2：重复步骤3.1直到样本中不存在异常值为止，此时计算样本的均值即为大、小臂长度及末端距离的初值；
步骤4：对初始化后新采集、计算的大、小臂长度及末端距离进行校正；用Grubbs准则检验步骤3采集的值是否正常，样本容量、显著性水平等参数与初始化时一致，若判断此次采集无效，直接进行下一次采集，反之则将新值计入样本，将样本中最早采集的值删除，以保持样本量为n，并将样本均值作为此次采集、计算的校正值；
步骤5：根据预先选择的映射模式对腕部坐标进行调整；
根据自适应映射模式对腕部坐标进行调整的具体步骤如下：步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；
步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；
步骤5.4：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值进行线性缩放；
根据自定义映射模式对腕部坐标进行调整的具体步骤如下：步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；
步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；
步骤5.4：定义L1和L2之比为最大映射比值rmax；
步骤5.5：取最大映射比值rmax和手动输入比值ri之间的较小值作为实际映射比值r；
步骤5.6：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值进行线性缩放；
步骤6：根据机械臂DH参数及调整后的手腕坐标值计算映射后的机械臂肘部坐标值；
根据演示者肩、肘、腕三点坐标确定一参考平面A：A1x+B1y+C1z+D1＝0，并根据机械臂的大臂长度、小臂长度和调整后的末端距离之间构成的三角关系，求出空间中存在机械臂肘部映射点的圆，圆心坐标和半径根据余弦定理求出，设大臂长度校正值为d1，小臂长度校正值为d2，末端距离校正值为L，新坐标系下腕部映射点坐标为(xw,yw,zw)，则半径为：新坐标系下的圆心坐标(xc,yc,zc)为：从而计算圆的参数方程，其中acos表示反余弦函数，将参数方程带入平面A的方程式即求出该圆与平面A之间的两个交点p1和p2，已知肩部、腕部点的坐标(xs,ys,zs)、(xw,yw,zw)，从而求出从演示者肘部点(xe,ye,ze)到肩部‑腕部连线的垂足：其中
进而求出从垂足到肘部点的向量 同理求出两交点对应垂足到两交点的向量计算 与 的夹角余弦值：
2.根据权利要求1所述的一种面向七轴机械臂视觉示教的可变姿态映射方法，其特征在于，步骤1的具体步骤如下：
步骤1.1：利用KinectDK设备提取演示者的骨架信息；
步骤1.2：采集肩部、手肘和手腕坐标数据，此时的各关节坐标处于设备固有的绝对坐标系下，称为镜头坐标系，在采集过程中，演示者的整只手臂都应位于镜头视野范围内。
3.根据权利要求2所述的一种面向七轴机械臂视觉示教的可变姿态映射方法，其特征在于，步骤2的具体步骤如下：
步骤2.1：根据采集到的演示者躯干部分信息定义一个新坐标系，以演示者肩部为原点，以演示者躯干正前方的朝向为X轴方向，以演示者垂直正上方的方向为Z轴方向，Y轴方向则根据右手定则来确定；
步骤2.2：将设备采集到的各关节坐标转换到步骤2.1所述的新坐标系下。
一种面向七轴机械臂视觉示教的可变姿态映射方法
技术领域
[0001] 本发明属于机器人运动学、机器视觉、数据校验领域领域，尤其涉及一种 面向七轴机械臂视觉示教的可变姿态映射方法。
背景技术
[0002] 随着“机器换人”计划的提出，机器人被越来越多地应用在高强度、高重 复性、高危险性的工作环境中。在众多的机器人类型中，七轴机器人由于其 结构与人类手臂相近而
占有特殊的地位，冗余的自由度为七轴机器人的运动 控制带来了更多的灵活性，相比常见
的通用六轴机器人，七轴机器人可以完 成复杂度更高的工作。
[0003] 目前，在线示教是控制机器人运动的主要手段之一，传统的示教方法往往 只关注对末端位姿的控制，而七轴机器人可以在实现相同末端位姿的前提下 进行连续的整体姿
态变化，若要同时对机器人的末端位姿和整体姿态进行控 制，则传统的示教手段难以满足
这一要求，另外，传统示教过程相对繁琐且 不够直观，这些原因都制约了七轴机器人自身
灵活性的发挥。而视觉示教可 以很好地解决这些问题，随着机器视觉技术的高速发展，视
觉示教正在开始 受到人们的关注。
[0004] 现有的视觉示教技术可用于机械臂末端的运动控制，但对整体姿态控制的 讨论较少。事实上，基于视觉信息不仅可以追踪末端位姿，还可以获得演示 者的手臂姿态，从而
实现对机械臂整体姿态的控制，但是若要进行整体姿态 控制，则还需解决人类手臂与机械
臂的映射问题，因为两者的大、小臂长度 及臂展长度不同，直接将演示者的关节坐标用于
视觉示教会导致姿态失真， 甚至出现运动学逆解失败的问题。
发明内容
[0005] 本发明目的在于提供一种面向七轴机械臂视觉示教的可变姿态映射方法, 以解决现有示教技术难以实现整体姿态控制的技术问题。
[0006] 为解决上述技术问题，本发明的一种面向七轴机械臂视觉示教的可变姿 态映射方法的具体技术方案如下：
[0007] 一种面向七轴机械臂视觉示教的可变姿态映射方法，包括如下步骤：
[0008] 步骤1：提取演示者骨架信息，采集肩部、手肘和手腕坐标数据；
[0009] 步骤2：将各关节坐标从镜头坐标系转换到机器人的基坐标系下；
[0010] 步骤3：根据各关节坐标值计算大、小臂长度及末端距离的初值；
[0011] 步骤4：对初始化后新采集、计算的大、小臂长度及末端距离进行校正；
[0012] 步骤5：根据预先选择的映射模式对腕部坐标进行调整；
[0013] 步骤6：根据机械臂DH参数及调整后的手腕坐标值计算映射后的机械臂 肘部坐标值。
[0014] 进一步地，步骤1的具体步骤如下：
[0015] 步骤1.1：利用KinectDK设备提取演示者的骨架信息；
[0016] 步骤1.2：采集肩部、手肘和手腕坐标数据，此时的各关节坐标处于设 备固有的绝对坐标系下，称为镜头坐标系，在采集过程中，演示者的整只手 臂都应位于镜头视野范围
内。
[0017] 进一步地，步骤2的具体步骤如下：
[0018] 步骤2.1：根据采集到的演示者躯干部分信息定义一个新坐标系，以演示 者肩部为原点，以演示者躯干正前方的朝向为X轴方向，以演示者垂直正上 方的方向为Z轴方向，Y
轴方向则根据右手定则来确定；
[0019] 步骤2.2：将设备采集到的各关节坐标转换到步骤2.1所述的新坐标系下。 进一步地，步骤3的具体步骤如下：
[0020] 步骤3.1：记录最初n次的采集、计算结果作为样本，求出样本的均值、 标准差和最大残差，选定显著性水平a的取值，从Grubbs准则临界值表上查 询到相应的临界值G(a,n)，
Grubbs准则异常值的判断公式为：
[0021]
[0022] 其中，为均值，σ为标准差，当上式成立时，说明xi为异常值，将其 剔除，再补充最新采集、计算的结果到样本，以保持样本量为n；
[0023] 步骤3.2：重复步骤3.1直到样本中不存在异常值为止，此时计算样本的 均值即为大、小臂长度及末端距离的初值；
[0024] 进一步地，步骤4的具体步骤如下：
[0025] 用Grubbs准则检验步骤3采集的值是否正常，样本容量、显著性水平等 参数与初始化时一致，若判断此次采集无效，直接进行下一次采集，反之则 将新值计入样本，将样本
中最早采集的值删除，以保持样本量为n，并将样 本均值作为此次采集、计算的校正值。
[0026] 进一步地，步骤5根据自适应映射模式对腕部坐标进行调整，具体步骤如下：
[0027] 步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；
[0028] 步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；
[0029] 步骤5.3：L1和L2之比即为实际映射比值r。
[0030] 步骤5.4：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值 进行线性缩放。
[0031] 进一步地，步骤5根据自定义映射模式对腕部坐标进行调整，具体步骤如下：
[0032] 步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；
[0033] 步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；
[0034] 步骤5.3：手动输入映射比值ri，输入值大于0；
[0035] 步骤5.4：定义L1和L2之比为最大映射比值rmax；
[0036] 步骤5.5：取最大映射比值rmax和手动输入比值ri之间的较小值作为实际 映射比值r；
[0037] 步骤5.6：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值 进行线性缩放。
[0038] 进一步地，步骤6的具体步骤如下：
[0039] 根据演示者肩、肘、腕三点坐标确定一参考平面A：A1x+B1y+C1z+D1＝0， 并根据机械臂的大臂长度、小臂长度和调整后的末端距离之间构成的三角关 系，求出空间中存在机
械臂肘部映射点的圆，圆心坐标和半径根据余弦定理 求出，设大臂长度校正值为d1，小臂
长度校正值为d2，末端距离校正值为L，
[0040] 新坐标系下腕部映射点坐标为(xw,yw,zw)，则半径为：
[0041]
[0042] 新坐标系下的圆心坐标(xc,yc,zc)为：
[0043]
[0044] 从而计算圆的参数方程，将参数方程带入平面A的方程式即求出该圆与平面 A之间的两个交点p1和p2，已知肩部、腕部点的坐标(xs,ys,zs)、(xw,yw,zw)， 从而求出从演示者
肘部点(xe,ye,ze)到肩部‑腕部连线的垂足：
[0045]
[0046] 其中
[0047]
[0048] 进而求出从垂足到肘部点的向量 同理求出两交点对应垂足到两交点 的向量计算 与 的夹角余弦值：
[0049]
[0050] 取 中与 夹角余弦值较大的为机械臂肘部映射点。
[0051] 本发明的一种面向七轴机械臂视觉示教的可变姿态映射方法具有以下优 点：解决了现有示教技术难以实现整体姿态控制的技术问题，同时为实现灵 敏度可变的机器人
姿态映射功能提供一种指导性的映射方法。
附图说明
[0052] 图1为本发明的映射方法流程图；
[0053] 图2为本发明的肘部映射点计算方法示意图；
[0054] 图3为本发明的映射效果图。
具体实施方式
[0055] 为了更好地理解本发明的技术方案，下面结合附图，对本发明一种面向七 轴机械臂视觉示教的可变姿态映射方法做进一步详细的描述。
[0056] 本发明提出的方法可以用于符合SRS构型的七轴机械臂，即前三关节、后 三关节转轴各交于一点，形成球腕结构，本发明中所用于仿真测试的机器人 型号为KUKA iiwa14
七轴机械臂。如图1所示，本发明的具体实施步骤如下：
[0057] 步骤1：提取演示者骨架信息，采集肩部、手肘和手腕坐标数据。
[0058] 首先利用KinectDK等相关设备提取演示者的骨架信息，并采集肩部、手 肘和手腕坐标数据，此时的各关节坐标处于设备固有的绝对坐标系下，称为 镜头坐标系，在采集过
程中，演示者的整只手臂都应位于镜头视野范围内；
[0059] 步骤2：将各关节坐标从镜头坐标系转换到机器人的基坐标系下。
[0060] 由于机器人运动学逆解函数需要在机械臂的基坐标系下才能有效，故需要 将各关节坐标从镜头坐标系转换到机器人的基坐标系下，根据采集到的演示 者躯干部分信息
定义一个新坐标系，以演示者肩部为原点，以演示者躯干正 前方的朝向为X轴方向，以演示
者垂直正上方的方向为Z轴方向，Y轴方向 则根据右手定则来确定，并将设备采集到的各关
节坐标转换到此新坐标系下， 以便于后续运动学逆解计算；
[0061] 步骤3：根据各关节坐标值计算大、小臂长度及末端距离的初值。
[0062] 根据各关节坐标值计算演示者的大、小臂长度及腕部到肩部的距离，称该 距离为末端距离，考虑到外界干扰会导致粗大误差的情况，在功能开始时需 要用Grubbs准则对上
述值进行初始化，简单地说，记录最初n次的采集、计 算结果作为样本，求出样本的均值、标
准差和最大残差，选定显著性水平a 的取值，在此情况下可从Grubbs准则临界值表上查询
到相应的临界值G(a,n)， Grubbs准则异常值的判断公式为：
[0063]
[0064] 其中，为均值，σ为标准差，当上式成立时，说明xi为异常值，应将 其剔除，再补充最新采集、计算的结果到样本，以保持样本量为n，重复上 述步骤，直到样本中不存在异常
值为止，此时计算样本的均值即为大、小臂 长度及末端距离的初值；
[0065] 步骤4：对初始化后新采集、计算的大、小臂长度及末端距离进行校正。
[0066] 同样的，考虑到外界干扰会导致粗大误差的情况，也需要对初始化后新采 集、计算的大、小臂长度及末端距离进行校正，用Grubbs准则检验本次采集 的值是否正常，样本
容量、显著性水平等参数与初始化时一致，若判断此次 采集无效，直接进行下一次采集，反
之则将新值计入样本，将样本中最早采 集的值删除，以保持样本量为n，并将样本均值作为
此次采集、计算的校正 值；
[0067] 步骤5：根据预先选择的映射模式对腕部坐标进行调整。
[0068] 根据预先选择的映射模式对腕部坐标进行调整，本方法定义了自适应和自 定义两种映射模式，在自适应模式下能根据不同演示者的臂长自动确定映射 比例，在自定义模
式下映射比例可根据用户定义值变化。
[0069] 先根据机械臂DH参数计算出机械臂伸展长度L1，再根据大、小臂长度计 算演示者手臂伸展长度L2，若映射模式为自适应模式，则L1和L2之比即为 实际映射比值r，若映射模
式为自定义模式，则需要预先手动输入映射比值 ri，输入值必须大于0,另外定义L1和L2之
比为最大映射比值rmax，手动模式 下实际映射比值应取最大映射比值rmax和手动输入比值
ri之间的较小值作为 实际映射比值r，再根据实际映射比值r对新坐标系下的机械臂腕部
映射点 坐标值进行线性缩放。
[0070] 步骤6：根据机械臂DH参数及调整后的手腕坐标值计算映射后的机械臂 肘部坐标值。
[0071] 根据机械臂DH参数及调整后的手腕坐标值计算映射后的机械臂肘部坐标 值，根据演示者肩、肘、腕三点坐标确定一参考平面A：A1x+B1y+C1z+D1＝0， 并根据机械臂的大臂
长度、小臂长度和调整后的末端距离之间构成的三角关 系，求出空间中存在机械臂肘部映
射点的圆，圆心坐标和半径可以根据余弦 定理求出，设大臂长度校正值为d1，小臂长度校
正值为d2，末端距离校正值 为L，新坐标系下腕部映射点坐标为(xw,yw,zw)，则半径为：
[0072]
[0073] 新坐标系下的圆心坐标(xc,yc,zc)为：
[0074]
[0075] 从而可以计算圆的参数方程，将参数方程带入平面A的方程式即可求出该圆 与平面A之间的两个交点p1和p2，当前已知肩部、腕部点的坐标(xs,ys,zs)、 (xw,yw,zw)，那么就可
以求出从演示者肘部点(xe,ye,ze)到肩部‑腕部连线的垂 足：
[0076]
[0077] 其中
[0078]
[0079] 进而求出从垂足到肘部点的向量 同理可以求出两交点对应垂足到两 交点的向量 计算 与 的夹角余弦值：
[0080]
[0081] 取 中与 夹角余弦值较大的为机械臂肘部映射点，即可实现对演 示者手臂姿态的映射，如图2所示。本发明只涉及关节位置的映射，不涉及 末端姿态的变化，也
不涉及后续的逆解计算。映射的效果如图3所示，图中 粗实线表示演示者手臂肩、肘、腕三
点的连线，虚线表示末端距离根据映射 比值线性缩放的方向。
[0082] 可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉 的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行 各种改变或等效替换。
另外，在本发明的教导下，可以对这些特征和实施例 进行修改以适应具体的情况及材料而
不会脱离本发明的精神和范围。因此， 本发明不受此处所公开的具体实施例的限制，所有
落入本申请的权利要求范 围内的实施例都属于本发明所保护的范围内。
