本发明公开了一种基于视觉的机器人示教方法，基于示教手柄实现，包括以下步骤：A.对示教手柄的中心点O进行双目视觉系统中的位置标定；B.手握示教手柄按示教期望轨迹进行移动，由双目视觉系统采集示教手柄的四个小球在双目视觉坐标系c下的位置；C.对示教手柄进行位姿估计得到机器人运动轨迹；D.将得到的机器人运动轨迹传送给机器人，由机器人运行到给定的位置，实现示教手柄移动曲线轨迹的再现。本发明的示教方法可实现对复杂的曲线轨迹快速示教，大大减少编程的时间，提高了示教编程的效率。
1.一种基于视觉的机器人示教方法，其特征在于，基于示教手柄实现，所述示教手柄上固定有四个小球且其球心分别为Pa、Pb、Pc、Pd；
A.对示教手柄的中心点O进行双目视觉系统中的位置标定；
B.手握示教手柄按示教期望轨迹进行移动，由双目视觉系统采集示教手柄的四个小球在双目视觉坐标系c下的位置；
D.将得到的机器人运动轨迹传送给机器人，由机器人运行到给定的位置，实现示教手柄移动曲线轨迹的再现；
所述四个小球分别与示教手柄的四角固定连接，且Pa、Pb、Pc、Pd中每两个点之间的距离不相等，在示教手柄上建立有手柄坐标系o，且该手柄坐标系o以示教手柄的中心点O为原点，X轴方向与Pa、Pc连线方向平行，Y轴方向与Pa、Pb连线方向平行，Z轴方向与Pa、Pb、Pc所在平面的垂线方向平行；将Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别记为oPa、oPb、oPc、oPd，则：其中， d3为Pd到Pa、Pb、Pc三点所确定的平面的距离。
2.根据权利要求1所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤A具体包括：A1.将示教手柄固定在机器人的末端法兰盘上，移动机器人使示教手柄位于双目视觉系统的视觉范围内的点1，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr01，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa01、cPb01、cPc01、cPd01；
A2.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点2，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr02，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa02、cPb02、cPc02、cPd02；
A3.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点3，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr03，并记录此时四个小球的球心在双目视觉坐标c c c c系c下的位置，分别为：Pa03、Pb03、Pc03、Pd03。
3.根据权利要求2所述的一种基于视觉的机器人示教方法，其特征在于，所述点2及点3距离点1的距离应不小于预设阈值，且点1与点2的连线与点2与点3的连线之间的夹角需满足预设角度范围。
4.根据权利要求2所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤B包括：B1.将示教手柄从机器人上取下，手握示教手柄按示教期望轨迹进行移动；
B2.双目视觉系统每隔m秒即采集一次四个球心的位置，第i个采样周期采集的四个球心的位置分别记为Pai、Pbi、Pci、Pdi；其中，i＝1,2,3......，四个球心点在双目视觉坐标系c下的位置如下：
5.根据权利要求4所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤C包括：C1.设手柄坐标系o在双目视觉坐标系c下的位姿为：
px、py、pz、C、B、A均为待求解的变量，其中，C、B、A分别表示绕双目视觉坐标系c的Z轴、Y轴、X轴旋转的角度，px、py、pz表示沿双目视觉坐标系c的X、Y、Z轴平移的距离；
C2.已知Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别为oPa、oPb、oPc、oPd，根据坐标系变换c c c c规则，Pa、Pb、Pc、Pd在双目视觉坐标系c下的位置为Pa、Pb、Pc、Pd，则：C3.将公式(1)、(2)、(3)带入公式(4-1)到(4-4)可得到关于px、py、pz、C、B、A的非线性表达式即(5-1)到(5-12)共12个等式：F1＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xa＝0 (5-1)F2＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-ya＝0 (5-2)F3＝pz+d1*sin(B)-d2*cos(B)*sin(A)-za＝0 (5-3)F4＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d1*cos(B)*cos(C)-xb＝0 (5-4)F5＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yb＝0 (5-5)F6＝pz-d1*sin(B)-d2*cos(B)*sin(A)-zb＝0 (5-6)F7＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xc＝0 (5-7)F8＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-yc＝0 (5-8)F9＝pz+d1*sin(B)+d2*cos(B)*sin(A)-zc＝0 (5-9)F10＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d3*(sin(A)*sin(C)+cos(A)*cos(C)*sin(B))+d1*cos(B)*cos(C)-xd＝0 (5-10)F11＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d3*(cos(C)*sin(A)-cos(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yd＝0 (5-11)F12＝pz+d1*sin(B)-d2*cos(B)*sin(A)+d3*cos(A)*cos(B)-zd＝0 (5-12)C4.对上述12个方程组采用高斯-牛顿法求解得到由px、py、pz、C、B、A六个待求解的变量构成的矢量X的值，其中，X＝[px py pz A B C]T；
C5.对步骤A中3次测量的四个小球的球心在双目视觉坐标系c下的位置数据采用上述方法计算得到 计算得到用户坐标系u在双目视觉坐标系c下的位置和姿态：
按照上述方式得到用户坐标系u在机器人基坐标系b下的位置和姿态为：其中，n＝unit(bPr01-bPr02)；a＝cross(unit(bPr01-bPr02)，unit(bPr01-bPr03))；
双目视觉坐标系c在机器人基坐标系b下的位置和姿态为：
C6.按照步骤C1至C4，对步骤B中第i个采样周期的Pai、Pbi、Pci、Pdi进行计算得到手柄坐标系o在双目视觉坐标系c下的位置和姿态C7.采用坐标变换得到第i个采样周期手柄坐标系o在机器人基坐标系b下的位置和姿态C8.根据公式(3)将 转换成 Ai、Bi、Ci；
C9.对步骤C8得到的结果进行滤波，得到滤波后的位置数据 及滤波后的姿态数据
6.根据权利要求5所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤C4具体包括：C4.1.随机给定一组初值X＝rand(1,6)；
C4.3对F1到F12关于px、py、pz、C、B、A求偏导数，求雅克比矩阵J：C4.4.计算步长dX：dX＝(JT*J*)JT*dY；
C4.6.判断范数norm(dX)是否小于eps，若小于则当前X值即所求的值，否则，重复步骤C4.1至C4.5，其中，eps为定义的足够小的常数。
7.根据权利要求5所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤C9中具体是采用移动平均值法进行滤波。
8.根据权利要求5或7所述的一种基于视觉的机器人示教方法，其特征在于，所述步骤D中具体是将滤波后得到的位置数据 及姿态数据 传送给机器人。
一种基于视觉的机器人示教方法
技术领域
[0001] 本发明涉及机器人示教技术领域，特别涉及一种基于视觉的机器人示教方法。
背景技术
[0002] 示教编程是机器人的基本功能，传统的示教编程方式为工人操作示教器使机器人沿着预先定义的某个虚拟的方向(坐标系的某个轴)移动或者旋转机器人，要求操作人员对坐标系的方向熟悉；在对某个点进行示教编程时需要不断地示教-观察-再示教不断重复，直到目标点。因此，传统的示教编程存在对操作人员要求高、编程繁琐困难、效率低、难以示教复杂的曲线轨迹等诸多缺点。
发明内容
[0003] 本发明的目的是克服上述背景技术中不足，提供一种基于视觉的机器人示教方法，可快速地应用于负载曲线、姿态变化较大的曲线的示教，以达到工人操作示教手柄在空间中示教三维曲线，机器人实时记录下示教轨迹的位置和姿态，并能够再现该三维曲线。
[0004] 为了达到上述的技术效果，本发明采取以下技术方案：
[0005] 一种基于视觉的机器人示教方法，基于示教手柄实现，所述示教手柄上固定有四个小球且其球心分别为Pa、Pb、Pc、Pd；
[0006] 所述基于视觉的机器人示教方法包括以下步骤：
[0007] A.对示教手柄的中心点O进行双目视觉系统(双目相机构成的视觉系统)中的位置标定；
[0008] B.手握示教手柄按示教期望轨迹进行移动，由双目视觉系统采集示教手柄的四个小球在双目视觉坐标系c下的位置；
[0009] C.对示教手柄进行位姿估计得到机器人运动轨迹；
[0010] D.将得到的机器人运动轨迹传送给机器人，由机器人运行到给定的位置，实现示教手柄移动曲线轨迹的再现。
[0011] 进一步地，所述四个小球分别与示教手柄的四角固定连接，且Pa、Pb、Pc、Pd中每两个点之间的距离不相等，在示教手柄上建立有手柄坐标系o，且该手柄坐标系以示教手柄的中心点为原点，X轴方向与Pa Pc连线方向平行，Y轴方向与Pa Pb连线方向平行，Z轴方向与Pa、Pb、Pc所在平面的垂线方向平行；将Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别记为oPa、oPb、oPc、oPd，则：
[0012]
[0013] 其中， d3为Pd到Pa、Pb、Pc三点所确定的平面的距离。
[0014] 进一步地，所述步骤A具体包括：
[0015] A1.将示教手柄固定在机器人的末端法兰盘上，移动机器人使示教手柄位于双目视觉系统的视觉范围内的点1，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr01，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa01、cPb01、cPc01、cPd01；
[0016] A2.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点2，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr02，并记录此时四个小球的球心在双目视觉c c c c坐标系c下的位置，分别为：Pa02、Pb02、Pc02、Pd02；
[0017] A3.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点3，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr03，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa03、cPb03、cPc03、cPd03。
[0018] 进一步地，所述点2及点3距离点1的距离应不小于预设阈值，且点1与点2的连线与点2与点3的连线之间的夹角需满足预设角度范围。
[0019] 进一步地，所述步骤B包括：
[0020] B1.将示教手柄从机器人上取下，手握示教手柄按示教期望轨迹进行移动；
[0021] B2.双目视觉系统每隔m秒即采集一次四个球心的位置，第i个采样周期采集的四个球心的位置分别记为Pai、Pbi、Pci、Pdi；其中，i＝1,2,3......，记此时四个球心点在双目视觉坐标系c下的位置如下：
[0022]
[0023] 进一步地，所述步骤C包括：
[0024] C1.设手柄坐标系o在双目视觉坐标系c下的位姿为：
[0025]
[0026] px、py、pz、C、B、A均为待求解的变量，其中，C、B、A分别表示绕双目视觉坐标系c的Z轴、Y轴、X轴旋转的角度，px、py、pz表示沿双目视觉坐标系c的X、Y、Z轴平移的距离；
[0027] C2.已知Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别为oPa、oPb、oPc、oPd，根据坐标系变换规则，Pa、Pb、Pc、Pd在双目视觉坐标系c下的位置为cPa、cPb、cPc、cPd，则：
[0028]
[0029]
[0030]
[0031]
[0032] C3.将公式(1)、(2)、(3)带入公式(4-1)到(4-4)可得到关于px、py、pz、C、B、A的非线性表达式即(5-1)到(5-12)共12个等式：
[0033] F1＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xa＝0 (5-1)[0034] F2＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-ya＝0 (5-2)[0035] F3＝pz+d1*sin(B)-d2*cos(B)*sin(A)-za＝0 (5-3)
[0036] F4＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d1*cos(B)*cos(C)-xb＝0 (5-4)[0037] F5＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yb＝0 (5-5)[0038] F6＝pz-d1*sin(B)-d2*cos(B)*sin(A)-zb＝0 (5-6)
[0039] F7＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xc＝0 (5-7)[0040] F8＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-yc＝0 (5-8)[0041] F9＝pz+d1*sin(B)+d2*cos(B)*sin(A)-zc＝0 (5-9)
[0042] F10＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d3*(sin(A)*sin(C)+cos(A)*cos(C)*sin(B))+d1*cos(B)*cos(C)-xd＝0 (5-10)
[0043] F11＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d3*(cos(C)*sin(A)-cos(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yd＝0 (5-11)
[0044] F12＝pz+d1*sin(B)-d2*cos(B)*sin(A)+d3*cos(A)*cos(B)-zd＝0 (5-12)[0045] C4.对上述12个方程组采用高斯-牛顿法求解得到由px、py、pz、C、B、A六个待求解的变量构成的矢量X的值，其中，
[0046] X＝[px py pz A B C]T；
[0047] C5.对步骤A中3次测量的四个小球的球心在双目视觉坐标系c下的位置数据采用上述方法计算得到 计算得到用户坐标系u在双目视觉坐标系c下的位置和姿态：
[0048] 其中，
[0049]
[0050] o＝cross(a,o)；
[0051] 按照上述方式得到用户坐标系u在机器人基坐标系b下的位置和姿态为：
[0052]
[0053] 其中，n＝unit(bPr01-bPr02)；a＝cross(unit(bPr01-bPr02)，unit(bPr01-bPr03))；
[0054] o＝cross(a,o)；p＝bPr01；
[0055] 双目视觉坐标系c在机器人基坐标系b下的位置和姿态为：
[0056] C6.按照步骤C1至C4，对步骤B中第i个采样周期的Pai、Pbi、Pci、Pdi进行计算得到手柄坐标系o在双目视觉坐标系c下的位置和姿态
[0057] C7.采用坐标变换得到第i个采样周期手柄坐标系o在机器人基坐标系b下的位置和姿态
[0058] C8.根据公式(3)将 转换成 Ai、Bi、Ci；
[0059] C9.对步骤C8得到的结果进行滤波，得到滤波后的位姿数据 及滤波后的姿态数据 进一步地，所述步骤C4具体包括：
[0060] C4.1.随机给定一组初值X＝rand(1,6)；
[0061] C4.2.将X代入F1到F12，计算得到dY：
[0062] C4.3对F1到F12关于px、py、pz、C、B、A求偏导数，求雅克比矩阵J：
[0063]
[0064] C4.4.计算步长dX：dX＝(JT*J*)JT*dY；
[0065] C4.5.更新X的值：X＝X-dX；
[0066] C4.6.判断范数norm(dX)是否小于eps，若小于则当前X值即所求的值，否则，重复步骤C4.1至C4.5，其中，eps为定义的足够小的常数。
[0067] 进一步地，所述步骤C9中具体是采用移动平均值法进行滤波。
[0068] 进一步地，所述步骤D中具体是将滤波后得到的位姿数据 及的姿态数据 传送给机器人。
[0069] 本发明与现有技术相比，具有以下的有益效果：
[0070] 本发明的基于视觉的机器人示教方法，克服了传统示教方式的一系列缺点，可快速地应用于负载曲线、姿态变化较大的曲线的示教，以达到工人操作示教手柄在空间中示教三维曲线，机器人实时记录下示教轨迹的位置和姿态，并能够再现该三维曲线，实现对复杂的曲线轨迹快速示教，大大减少编程的时间，提高了示教编程的效率。
附图说明
[0071] 图1是本发明的一个实施例中的示教手柄的示意图。
[0072] 图2是本发明的一个实施例中滤波前后得到的轨迹效果示意图。
具体实施方式
[0073] 下面结合本发明的实施例对本发明作进一步的阐述和说明。
[0074] 实施例：
[0075] 实施例一：
[0076] 一种基于视觉的机器人示教方法，基于示教手柄实现，具体的，如图1所示为本实施例的示教手柄示意，在示教手柄上固定有四个大小相同的小球，四个小球分别与示教手柄的四角固定连接，记四个小球的球心分别为Pa、Pb、Pc、Pd，且Pa、Pb、Pc、Pd中每两个点之间的距离不相等。
[0077] 具体的，在示教过程中，需保证四个小球的球心均位于双目视觉系统(双目相机构成的视觉系统)的视觉范围内，由双目视觉系统对四个小球的球心位置进行实时识别。
[0078] 具体的，在示教手柄上建立有手柄坐标系o，且该手柄坐标系以示教手柄的底座的中心点为原点，X轴方向与Pa Pc连线方向平行，即X轴方向平行于 Y轴方向与Pa Pb连线方向平行，即Y轴方向平行于 Z轴根据右手法则可得到，具体为Z轴方向与Pa、Pb、Pc所在平面的垂线方向平行。
[0079] 将Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别记为oPa、oPb、oPc、oPd，则：
[0080]
[0081] 其中， d3为Pd到Pa、Pb、Pc三点所确定的平面的距离。
[0082] 具体的，本实施例的基于视觉的机器人示教方法包括以下步骤：
[0083] 步骤1.进行手眼标定，即对示教手柄的中心点O进行双目视觉系统(双目相机构成的视觉系统)中的位置标定。
[0084] 具体包括：
[0085] 步骤1.1.将示教手柄固定在机器人的末端法兰盘上，移动机器人使示教手柄位于双目视觉系统的视觉范围内的点1，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr01，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa01、cPb01、cPc01、cPd01。
[0086] 步骤1.2.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点2，记录此b时示教手柄的中心点O在机器人基坐标系b下的位置Pr02，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa02、cPb02、cPc02、cPd02。
[0087] 步骤1.3.移动机器人使示教手柄位于双目视觉系统的视觉范围内的点3，记录此时示教手柄的中心点O在机器人基坐标系b下的位置bPr03，并记录此时四个小球的球心在双目视觉坐标系c下的位置，分别为：cPa03、cPb03、cPc03、cPd03。
[0088] 具体的，上述点2及点3距离点1的距离应尽量大，即点2及点3应距离点1尽量远，且点1与点2的连线与点2与点3的连线之间的夹角需尽量为90°。
[0089] 步骤2.手握示教手柄按示教期望轨迹进行移动，由双目视觉系统采集示教手柄的四个小球在双目视觉坐标系c下的位置。
[0090] 具体包括：
[0091] 步骤2.1.将示教手柄从机器人上取下，手握示教手柄按示教期望轨迹进行移动。
[0092] 步骤2.2.双目视觉系统每隔10ms(本实施例中设定为间隔10ms，实际中可根据具体情况设定为其他时间)即采集一次四个球心的位置，，第i个采样周期采集的四个球心的位置分别记为Pai、Pbi、Pci、Pdi；其中，一个采样周期为10ms，i＝1,2,3......，四个球心点在双目视觉坐标系c下的位置如下：
[0093]
[0094] 步骤3.对示教手柄进行位姿估计得到机器人运动轨迹；具体包括：
[0095] 步骤3.1.设手柄坐标系o在双目视觉坐标系c下的位姿为：
[0096]
[0097] px、py、pz、C、B、A均为待求解的变量，从双目视觉坐标系c变换至手柄坐标系o需沿着双目视觉坐标系c进行三次平移和三次旋转，其中，C、B、A分别表示绕双目视觉坐标系c的Z轴、Y轴、X轴旋转的角度，px、py、pz表示沿双目视觉坐标系c的X、Y、Z轴平移的距离；
[0098] 步骤3.2.已知Pa、Pb、Pc、Pd在手柄坐标系o下的位置分别为oPa、oPb、oPc、oPd，根据坐标系变换规则，Pa、Pb、Pc、Pd在双目视觉坐标系c下的位置为cPa、cPb、cPc、cPd，则：
[0099]
[0100]
[0101]
[0102]
[0103] 步骤3.3.将公式(1)、(2)、(3)带入公式(4-1)到(4-4)可得到(5-1)到(5-12)共12个等式：
[0104] F1＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xa＝0 (5-1)[0105] F2＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-ya＝0 (5-2)[0106] F3＝pz+d1*sin(B)-d2*cos(B)*sin(A)-za＝0 (5-3)
[0107] F4＝px+d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d1*cos(B)*cos(C)-xb＝0 (5-4)[0108] F5＝py-d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yb＝0 (5-5)[0109] F6＝pz-d1*sin(B)-d2*cos(B)*sin(A)-zb＝0 (5-6)
[0110] F7＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))-d1*cos(B)*cos(C)-xc＝0 (5-7)[0111] F8＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d1*cos(B)*sin(C)-yc＝0 (5-8)[0112] F9＝pz+d1*sin(B)+d2*cos(B)*sin(A)-zc＝0 (5-9)
[0113] F10＝px-d2*(cos(A)*sin(C)-cos(C)*sin(A)*sin(B))+d3*(sin(A)*sin(C)+cos(A)*cos(C)*sin(B))+d1*cos(B)*cos(C)-xd＝0 (5-10)
[0114] F11＝py+d2*(cos(A)*cos(C)+sin(A)*sin(B)*sin(C))-d3*(cos(C)*sin(A)-cos(A)*sin(B)*sin(C))+d1*cos(B)*sin(C)-yd＝0 (5-11)
[0115] F12＝pz+d1*sin(B)-d2*cos(B)*sin(A)+d3*cos(A)*cos(B)-zd＝0 (5-12)[0116] 上述12方程是关于px、py、pz、C、B、A的非线性表达式，由于变量的数目为6，方程组数目为12，方程组数目大于变量数目，可实现求解。
[0117] 步骤3.4.对上述12个方程组采用高斯-牛顿法求解得到由px、py、pz、C、B、A六个待求解的变量构成的矢量X的值，其中，
[0118] X＝[px py pz A B C]T。
[0119] 具体求解方法如下：
[0120] 步骤3.4.1.随机给定一组初值X＝rand(1,6)；
[0121] 步骤3.4.2.将X代入F1到F12，计算得到dY：
[0122] 步骤3.4.3对F1到F12关于px、py、pz、C、B、A求偏导数，求雅克比矩阵J：
[0123]
[0124] 步骤3.4.4.计算步长dX：dX＝(JT*J*)JT*dY；
[0125] 步骤3.4.5.更新X的值：X＝X-dX；
[0126] 步骤3.4.6.判断范数norm(dX)(即dX中6个元素的平方和，再开根号)是否小于eps，若小于则当前X值即所求的值，否则，重复步骤3.4.1至3.4.5，其中，eps为用户定义的足够小的常数，如1e-12。
[0127] 步骤3.5.对步骤1中3次测量的四个小球的球心在双目视觉坐标系c下的位置数据采用上述方法计算得到 计算得到用户坐标系u在双目视觉坐标系c下的位置和姿态：
[0128] 其中， o＝cross(a,o)；
[0129] 按照上述方式得到用户坐标系u在机器人基坐标系b下的位置和姿态：
[0130]
[0131] 其中， a＝cross(unit(bPr01-bPr02)，unit(bPr01-bPr03))；o＝cross(a,o)；p＝bPr01；
[0132] 双目视觉坐标系c在机器人基坐标系b下的位置和姿态为：
[0133] 步骤3.6.按照步骤3.1至3.4，对步骤2中第i个采样周期的Pai、Pbi、Pci、Pdi进行计算得到手柄坐标系o在双目视觉坐标系c下的位置和姿态
[0134] 步骤3.7.采用坐标变换得到第i个采样周期手柄坐标系o在机器人基坐标系b下的位置和姿态
[0135] 步骤3.8.根据公式(3)将 转换成 Ai、Bi、Ci；具体如下：
[0136]
[0137]
[0138]
[0139]
[0140]
[0141]
[0142] 步骤3.9.对步骤3.8得到的结果进行滤波，得到滤波后的位姿数据 及滤波后的姿态数据
[0143] 具体的，本实施例中采用移动平均值法进行滤波，方法如下：
[0144]
[0145] 步骤4.将滤波后得到的位姿数据 及的姿态数据 传送给机器人，由机器人运行到给定的位置，实现示教手柄移动曲线轨迹的再现。
[0146] 具体的，本实施例中，机器人再现的示教手柄移动曲线轨迹效果图如图2所示。
[0147] 综上可知，本发明的基于视觉的机器人示教方法，根据双目视觉识别到手柄靶球4个球心的位置，采用高斯牛顿迭代方法求解出手柄的位置和姿态，通过坐标变换得到了手柄在机器人坐标系下的位姿(滤波前)，然后再经过滤波器对位置和姿态滤波，即可得到的光滑位置曲线(滤波后)，因此，本发明的技术方案可对复杂的曲线轨迹快速示教，大大减少编程的时间，提高了示教编程的效率。
[0148] 可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。
