[发明专利]一种面向七轴机械臂视觉示教的可变姿态映射方法

权利要求书

1.一种面向七轴机械臂视觉示教的可变姿态映射方法，其特征在于，包括如下步骤：

步骤1：提取演示者骨架信息，采集肩部、手肘和手腕坐标数据；

步骤2：将各关节坐标从镜头坐标系转换到机器人的基坐标系下；

步骤3：根据各关节坐标值计算大、小臂长度及末端距离的初值；

步骤3.1：记录最初n次的采集、计算结果作为样本，求出样本的均值、标准差和最大残差，选定显著性水平a的取值，从Grubbs准则临界值表上查询到相应的临界值G(a,n)，Grubbs准则异常值的判断公式为：

其中，为均值，σ为标准差，当上式成立时，说明x_i为异常值，将其剔除，再补充最新采集、计算的结果到样本，以保持样本量为n；

步骤3.2：重复步骤3.1直到样本中不存在异常值为止，此时计算样本的均值即为大、小臂长度及末端距离的初值；

步骤4：对初始化后新采集、计算的大、小臂长度及末端距离进行校正；用Grubbs准则检验步骤3采集的值是否正常，样本容量、显著性水平等参数与初始化时一致，若判断此次采集无效，直接进行下一次采集，反之则将新值计入样本，将样本中最早采集的值删除，以保持样本量为n，并将样本均值作为此次采集、计算的校正值；

步骤5：根据预先选择的映射模式对腕部坐标进行调整；

根据自适应映射模式对腕部坐标进行调整的具体步骤如下：

步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；

步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；

步骤5.3：L1和L2之比即为实际映射比值r；

步骤5.4：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值进行线性缩放；

根据自定义映射模式对腕部坐标进行调整的具体步骤如下：

步骤5.1：根据机械臂DH参数计算出机械臂伸展长度L1；

步骤5.2：根据大、小臂长度计算演示者手臂伸展长度L2；

步骤5.3：手动输入映射比值r_i，输入值大于0；

步骤5.4：定义L1和L2之比为最大映射比值r_max；

步骤5.5：取最大映射比值r_max和手动输入比值r_i之间的较小值作为实际映射比值r；

步骤5.6：根据实际映射比值r对新坐标系下的机械臂腕部映射点坐标值进行线性缩放；

步骤6：根据机械臂DH参数及调整后的手腕坐标值计算映射后的机械臂肘部坐标值；

根据演示者肩、肘、腕三点坐标确定一参考平面A：A₁x+B₁y+C₁z+D₁＝0，并根据机械臂的大臂长度、小臂长度和调整后的末端距离之间构成的三角关系，求出空间中存在机械臂肘部映射点的圆，圆心坐标和半径根据余弦定理求出，设大臂长度校正值为d₁，小臂长度校正值为d₂，末端距离校正值为L，新坐标系下腕部映射点坐标为(x_w,y_w,z_w)，则半径为：

新坐标系下的圆心坐标(x_c,y_c,z_c)为：

从而计算圆的参数方程，其中acos表示反余弦函数，将参数方程带入平面A的方程式即求出该圆与平面A之间的两个交点p₁和p₂，已知肩部、腕部点的坐标(x_s,y_s,z_s)、(x_w,y_w,z_w)，从而求出从演示者肘部点(x_e,y_e,z_e)到肩部-腕部连线的垂足：

其中

进而求出从垂足到肘部点的向量同理求出两交点对应垂足到两交点的向量计算与的夹角余弦值：

取中与夹角余弦值较大的为机械臂肘部映射点。