CN114102578B - 工具中心点的校正方法、机械手臂的教导方法及其系统 - Google Patents

工具中心点的校正方法、机械手臂的教导方法及其系统 Download PDF

Info

Publication number
CN114102578B
CN114102578B CN202110007956.4A CN202110007956A CN114102578B CN 114102578 B CN114102578 B CN 114102578B CN 202110007956 A CN202110007956 A CN 202110007956A CN 114102578 B CN114102578 B CN 114102578B
Authority
CN
China
Prior art keywords
tool
point
camera
mechanical arm
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110007956.4A
Other languages
English (en)
Other versions
CN114102578A (zh
Inventor
黄成凯
林依颍
许秉澄
陈俊皓
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Industrial Technology Research Institute ITRI
Original Assignee
Industrial Technology Research Institute ITRI
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Industrial Technology Research Institute ITRI filed Critical Industrial Technology Research Institute ITRI
Publication of CN114102578A publication Critical patent/CN114102578A/zh
Application granted granted Critical
Publication of CN114102578B publication Critical patent/CN114102578B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B25J13/089—Determining the position of the robot with reference to its environment
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/023—Cartesian coordinate type
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39054—From teached different attitudes for same point calculate tool tip position

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

本发明公开一种工具中心点的校正方法、机械手臂的教导方法及机械手臂系统。首先，机械手臂驱动工具的工具轴向投影于测试平面的投射点相对测试平面的参考点进行相对运动。然后，依据相对运动，建立第一转换关系。然后，取得工具相对机械手臂的装设面参考坐标系的工具轴向量。然后，执行校正点信息组取得步骤，包括：(a1)机械手臂驱动工具中心点重合于测试平面的参考点，并记录机器手臂的校正点信息组；(a2)机械手臂驱动工具改变工具轴向的角度；及，(a3)重复步骤(a1)及(a2)，以取得数个校正点信息组。然后，依据校正点信息组，取得工具中心点相对装设面参考坐标系的工具中心点坐标。

Description

工具中心点的校正方法、机械手臂的教导方法及其系统
技术领域
本发明涉及一种校正方法、教导方法及应用其的机械手臂系统，且特别是涉及一种工具中心点的校正方法、机械手臂的教导方法及应用其的机械手臂系统。
背景技术
随着科技的进步，机器手臂在各个产业上的应用也愈来愈广泛地；一般而言，机器手臂为具有多个关节的关节型机械手臂，而其一端则设置有一工具，如焊接工具或钻孔工具等等，以执行各种不同的作业；而在机器人进行作业前，其工具的工具中心点(ToolCenter Point，TCP)的位置需要事先进行精确的校正，如此机器手臂的控制器才可根据工具中心点使工具可运行于正确的路径。然而，现有技术的机器手臂的工具中心点校正技术确有着许多缺点有待改进。例如，根据现有技术的机器手臂的工具中心点校正技术，使用者可能需要手动操作机器手臂以校正机械人的工具中心点，因此容易产生人为误差，无法精确地校正工具中心点，因此校正精确度低且需要较高人力成本及时间成本。此外，目前的工具中心点的校正方法也无法适用于虚拟工具中心点。
发明内容
本发明提供一种工具中心点的校正方法、机械手臂的教导方法及应用其的机械手臂系统，可改善前述现有问题。
本发明一实施例提出一种工具中心点的校正方法。工具中心点校正方法包括以下步骤。执行一机械手臂的一机械手臂参考坐标系与一摄像器参考坐标系之间一第一转换关系建立步骤，包括：一机械手臂驱动工具的一工具轴向投影于测试平面的一投射点相对测试平面的一参考点进行一相对运动；及依据相对运动，建立第一转换关系；取得工具相对机械手臂的一装设面参考坐标系之间的一工具轴向量；执行一校正点信息组取得步骤，包括：(a1)机械手臂驱动一工具中心点重合于测试平面的参考点，并记录机器手臂的一校正点信息组；(a2)机械手臂驱动工具改变工具轴向的角度；及，(a3)重复步骤(a1)及(a2)，以取得多个校正点信息组；以及，依据此些校正点信息组，取得工具中心点相对装设面参考坐标系之间的一工具中心点坐标。
本发明另一实施例提出一种机械手臂的教导方法。教导方法包括以下步骤。(d1)使用前述工具中心点校正方法，取得工具中心点坐标，且驱动工具至一第一位置，并使在第一位置，工具中心点重合于一检测面的一指定点；(d2)平移工具一平移距离至一第二位置；(d3)依据平移距离及工具的工具中心点沿工具轴向的一行程差，取得工具的一检测角度；(d4)判断检测角度是否符合一规格角度；(d5)当检测角度不符合规格角度，驱动工具回到第一位置；以及，(d6)调整机械手臂的姿态，执行步骤(d1)～(d6)，直到检测角度符合规格角度。
本发明另一实施例提出一种机械手臂系统。机械手臂系统包括一机械手臂及一控制器。机械手臂用以装载一工具，工具具有一工具轴向。控制器用以：控制机械手臂驱动工具的工具轴向投影于一测试平面的一投射点相对测试平面的一参考点进行一相对运动；依据相对运动，建立机械手臂的一机械手臂参考坐标系与摄像器的一摄像器参考坐标系的一第一转换关系；取得工具相对机械手臂的一装设面参考坐标系的一工具轴向量；执行一校正点信息组取得步骤，包括：(a1)机械手臂驱动一工具中心点重合于测试平面的参考点，并记录机器手臂的一校正点信息组；(a2)机械手臂驱动工具改变工具轴向的角度；及，(a3)重复步骤(a1)及(a2)，以取得多个校正点信息组；以及，依据此些校正点信息组，取得该工具中心点相对该装设面参考坐标系的一工具中心点坐标。
为了对本发明的上述及其他方面有更佳的了解，下文特举实施例，并配合所附的附图详细说明如下：
附图说明
图1为本发明一实施例的用以校正工具中心点的机械手臂系统的示意图；
图2A～图2D为图1的机械手臂系统的校正工具中心点的流程图；
图3A为图1的机械手臂于空间相对参考点运动的示意图；
图3B为图1的摄像器所提取的投射点于测试平面上移动的影像的示意图；
图4A～图4C、图5A～图5C、图6A～图6C、图7A～图7C、图8、图9A～图9B为本发明一实施例的工具轴向量的取得过程示意图；
图10A为图1的光源所发出的第二光线与工具沿工具轴向所发出的第一光线交会于工具中心点的示意图；
图10B为图10A的光源所发出的第二光线投射于测试平面的投射点与工具沿工具轴向所发出的第一光线投射于测试平面的投射点为分离的二点的影像示意图；
图11A为图10A中心点重合于测试平面的示意图；
图11B为图11A中心点与参考点相距投影点移动向量的影像示意图；
图12A～图12B为图11A中心点重合于参考点的示意图；
图13为本发明一实施例的机械手臂系统的自动教导方法的流程图；
图14A为图1的机械手臂系统对工具中心点进行一第一检测教导的示意图；
图14B为图1的机械手臂系统对工具中心点进行一第二检测教导的示意图。
符号说明
10:工具
20:测试平面
100:机械手臂系统
110:机械手臂
110s:装设面
111:基座
120:摄像器
130:光源
131:转动支点
140:控制器
150:移动器
A1:工具轴向
LR:长度
L1:第一光线
L2:第二光线
J1～J6:关节
M1:影像
O1:参考点
P1、P1’,Px,Py,Pz,P’x,P’y,P’z:投射点
S1:第一位置
S2:第二位置
S110～S133、S210～S260:步骤
SW:投影点移动向量
SR:机械手臂移动向量
T1:第一转换关系
T2:第二转换关系
Tez:工具轴向量
T3:校正点信息组矩阵
TP:工具中心点坐标
第一空间向量
第二空间向量
第三空间向量
(xR-yR-zR):机械手臂参考坐标系
(xC-yC-zC):摄像器参考坐标系
(xf-yf-zf):装设面坐标系
W1:工具中心点
zw:向量
θV,θH:检测角度
ΔTZ1,ΔTZ2:行程差
具体实施方式
请参照图1，其绘示依照本发明一实施例的用以校正工具中心点的机械手臂系统的示意图。机械手臂系统100包括机械手臂110、摄像器120、光源130及控制器140。机械手臂110用以装载工具10，工具10具有工具轴向A1。控制器140用以：(1).控制机械手臂110驱动工具10的工具轴向A1投影于测试平面20的投射点P1相对测试平面20的参考点O1进行一相对运动；(2).依据相对运动，建立机械手臂110的机械手臂参考坐标系(xR-yR-zR)与摄像器120的摄像器参考坐标系(xC-yC-zC)的第一转换关系T1；(3).取得工具10相对机械手臂110的装设面(或，称为法兰面)参考坐标系(xf-yf-zf)的工具轴向量Tez；(4).执行一校正点信息组取得步骤，包括：(a1).控制机械手臂110驱动工具中心点W1重合于测试平面20的参考点O1，并记录机器手臂110的一校正点信息组；(a2).控制机械手臂110驱动工具10改变工具轴向A1的角度；及(a3).重复步骤(a1)及(a2)，以取得数个校正点信息组；以及，(5).依据此些校正点信息组，取得工具中心点相对装设面参考坐标系(xf-yf-zf)的工具中心点坐标TP。
工具10以辉度计为例说明。在另一实施例中，工具10例如是加工刀具。
在本实施例中，测试平面20例如是一实体屏幕的表面。此实体屏幕例如是透光屏幕或不透光屏幕。以不透光屏幕来说，实体屏幕的测试平面20例如是白色，然而只要可清楚呈现工具10所发出的第一光线L1及光源130所发出的第二光线L2(第二光线L2绘示于图10A)即可，本发明实施例不限定实体屏幕的表面颜色。以透光屏幕来说，屏幕例如是玻璃或塑胶。当屏幕是不透光屏幕时，摄像器120与机械手臂110可位于测试平面20的同一侧，如图1所示。当屏幕是透光屏幕时，摄像器120与机械手臂110可分别位于测试平面20的相对二侧，然而也可位于测试平面20的同一侧。此外，摄像器120正对测试平面20，使所提取的影像为摄像器参考坐标系(xC-yC-zC)的xC-yC平面的影像。
请参照图2A～图2D，其绘示图1的机械手臂系统100的校正工具中心点的流程图。
在步骤S110中，机械手臂系统100执行机械手臂110的机械手臂参考坐标系(xR-yR-zR)与摄像器120的摄像器参考坐标系(xC-yC-zC)之间的第一转换关系T1建立步骤。步骤S110包含子步骤S111～S117。第一转换关系T1建立步骤包括以下步骤：机械手臂110驱动工具10的工具轴向A1投影于测试平面20的投射点P1相对测试平面20的参考点O1进行一相对运动。然后，控制器140依据该相对运动，建立机械手臂110的机械手臂参考坐标系(xR-yR-zR)与摄像器参考坐标系(xC-yC-zC)之间的第一转换关系T1。
举例来说，请同时参照图3A及图3B，图3A绘示图1的机械手臂110于空间相对参考点O1运动的示意图，而图3B绘示图1的摄像器120所提取的投射点Px、Py及Pz于测试平面20上移动的影像M1的示意图。在校正过程中，摄像器120可持续提取投射点Px、Py及Pz于测试平面20上移动的影像M1，使控制器140即时分析投射点Px、Py及Pz于测试平面20移动的轨迹变化。在图3A中，xC-yC-zC是摄像器参考坐标系，空间向量是投射点从摄像器参考坐标系(xC-yC-zC)的参考点O1(原点)出发，分别沿着机械手臂参考坐标系(xR-yR-zR)的各轴xR、yR、zR移动一长度LR的向量。在一实施例中，沿着机械手臂参考坐标系(xR-yR-zR)的各轴xR、yR、zR的移动长度LR可相等或不相等。在图3B中，影像M1是一平面影像，zC轴向垂直影像M1。虽然图3B有绘示摄像器参考坐标系(xC-yC)及向量箭头，然而实际影像M1中，可不具有坐标影像及箭头影像。图3A的空间中的投射点Px(x1,y1,z1)、Py(x2,y2,z2)及Pz(x3,y3,z3)例如是向量终点，其分别对应至图3B的影像M1的P’x(x1,y1)、P’y(x2,y2)及P’z(x3,y3)。P’x(x1,y1)、P’y(x2,y2)及P’z(x3,y3)分别为空间中的投射点Px(x1,y1,z1)、Py(x2,y2,z2)及Pz(x3,y3,z3)投影至或投射至测试平面20的投影点。
在步骤S111中，如图1及图3A所示，控制器140控制机械手臂110移动，使工具10所发出第一光线L1的投射点Px从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第一轴向(例如，xR轴向)移动一第一空间向量(x1,y1,z1)。此外，第一空间向量/>的值(长度)为LR，且第一空间向量/>的终点为图3A的投射点Px(x1,y1,z1)。此外，参考点O1可以是测试平面20的任一点，例如是测试平面20的中心点。
在本步骤S111中，控制器140可分析摄像器120所提取的影像M1，如图3B所示，判断影像M1中的投射点Px是否对应(或位于/重合)影像中的参考点O1。当投射点Px尚未对应影像M1中的参考点O1，则控制机械手臂110移动，直到投射点Px对应影像M1中的参考点O1。当投射点Px对应影像中的参考点O1时，控制器140再控制机械手臂110移动，使投射点Px从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第一轴向(例如，xR轴向)移动第一空间向量(x1,y1,z1)。在移动过程，控制器140分析摄像器120所提取的影像M1，判断影像M1中的投射点P1是否已移动第一空间向量/>(x1,y1,z1)。
在步骤S112中，控制器140可分析摄像器120所提取的影像M1，如图3B所示，影像M1为平面影像，故点Px(x1,y1,z1)成为P’x(x1,y1)，以取得第一空间向量(x1,y1,z1)的投射点P’x的第一平面坐标P’x(x1,y1)的数值，即第一轴向坐标值x1及第二轴向坐标值y1。
在步骤S113中，机械手臂110驱动工具10从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第二轴向(例如，yR轴向)移动一第二空间向量(x2,y2,z2)。第二空间向量/>(x2,y2,z2)的值(长度)为LR，且第二空间向量/>(x2,y2,z2)的终点为图3A的投射点Py(x2,y2,z2)。
相似地，在本步骤S113中，控制器140可分析摄像器120所提取的影像M1，判断影像M1中的投射点P’y是否对应(或位于)影像中的参考点O1。当投射点P’y尚未对应影像M1中的参考点O1，则控制机械手臂110移动，直到将投射点P’y对应影像M1中的参考点O1。当投射点P1对应影像中的参考点O1时，控制器140再控制机械手臂110移动，使投射点P’y从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第二轴向移动第二空间向量(x2,y2,z2)。在移动过程，控制器140分析摄像器120所提取的影像M1，判断影像M1中的投射点P’y是否已移动第二空间向量/>(x2,y2,z2)。
在步骤S114中，控制器140可分析摄像器120所提取的影像，以取得第二空间向量(x2,y2,z2)的投射点P’y的第二平面坐标P’y(x2,y2)的数值，即第一轴向坐标值x2及第二轴向坐标值y2。
在步骤S115中，机械手臂110驱动工具10从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第三轴向(例如，zR轴向)移动一第三空间向量(x3,y3,z3)。第三空间向量/>(x3,y3,z3)的值(长度)为LR，且第三空间向量/>(x3,y3,z3)的终点为图3A的投射点Pz(x3,y3,z3)。
相似地，在本步骤S115中，控制器140可分析摄像器120所提取的影像M1，判断影像M1中的投射点P’z是否对应(或位于)影像M1中的参考点O1。当投射点P’z尚未对应影像M1中的参考点O1，则控制机械手臂110移动，直到将投射点P’z对应影像M1中的参考点O1。当投射点P’z对应影像中的参考点O1时，控制器140再控制机械手臂110移动，使投射点P’z从参考点O1沿机械手臂参考坐标系(xR-yR-zR)的第三轴向移动第三空间向量(x3,y3,z3)。在移动过程，控制器140分析摄像器120所提取的影像M1，判断影像M1中的投射点P’z是否已移动第三空间向量/>(x3,y3,z3)。
在步骤S116中，控制器140可分析摄像器120所提取的影像M1，以取得第三空间向量(x3,y3,z3)的投射点P’z的第三平面坐标P’z(x3,y3)的数值，即第一轴向坐标值x3及第二轴向坐标值y3。
在步骤S117中，控制器140依据第一空间向量第二空间向量/>与第三空间向量/>互相正交的特性，建立摄像器参考坐标系(xC-yC-zC)与机械手臂参考坐标系(xR-yR-zR)的第一转换关系T1。例如，控制器140可采用下式(1)～(3)求得第三轴向坐标值z1、z2及z3。如此，控制器140取得x1、x2及x3、y1、y2及y3以及z1、z2及z3。然后，控制器140依据下式(4)建立第一转换关系T1。
如式(5)所示，控制器140可采用第一转换关系T1，将投射点移动向量SW转换成机械手臂移动向量SR，其中投射点移动向量SW为测试平面20上的投射点P1相对摄像器参考坐标系(xC-yC-zC)的移动向量，而机械手臂移动向量SR表示机械手臂110相对机械手臂参考坐标系(xR-yR-zR)的移动向量。机械手臂参考坐标系(xR-yR-zR)可建立于机械手臂110的任何位置，如机械手臂110的基座111。式(1)、(2)、(3)代表空间向量相互正交。式(4)的第一转换关系T1为空间向量/>除以向量长度后(单位向量)的反矩阵。式(5)代表第一转换关系T1与投射点移动向量SW的点积等于机械手臂移动向量SR。
然后，在步骤S120中，机械手臂系统100取得工具10相对于装设面参考坐标系(xf-yf-zf)的工具轴向量Tez。
举例来说，请同时参照图4A～图9B，其绘示依照本发明一实施例的工具轴向量Tez的取得过程示意图。图4A绘示往图1的摄像器120所提取的投射点P1于测试平面20上的影像M1的示意图，图4B绘示往图1的-yC轴向观看测试平面20的示意图，而图4C绘示往图1的-xC轴向观看测试平面20的示意图。如图4B及图4C所示，工具10的工具轴向A1相对摄像器参考坐标系(xC-yC-zC)的xC-yC平面倾斜，即，工具轴向A1未垂直于摄像器参考坐标系(xC-yC-zC)的xC-yC平面。然而通过以下工具轴向量Tez的取得过程，可将工具10的工具轴向A1调整至垂直于摄像器参考坐标系(xC-yC-zC)的xC-yC平面，如图8及图9B所示。然后，控制器140可依据此状态(即，工具轴向A1垂直于摄像器参考坐标系(xC-yC-zC)的xC-yC平面)下的机械手臂110的各关节J1～J6的关节角度，取得工具轴向量Tez。以下进一步举例说明。
在步骤S121中，如图4B及图4C所示，工具10所发出沿工具轴向A1的第一光线L1于测试平面20投射出一投射点P1。然后，摄像器120提取测试平面20的影像M1，如图4A所示，影像M1具有投射点P1的影像。然后，控制器140依据所提取到的影像M1，取得工具10投射(或投影)于测试平面20上的投射点P1相对参考点O1的投射点移动向量SW。
在步骤S122中，控制器140依据第一转换关系T1及投射点移动向量SW，取得机械手臂移动向量SR。例如，控制器140可将投射点移动向量SW代入上式(5)，以计算取得机械手臂110要将投射点P1移动至接近或重合于参考点O1所需的机械手臂移动向量SR。步骤S122及S123目的在于避免机械手臂移动后或转动后的投射点P1’不会掉出测试平面20外。
在步骤S123中，如图5A～图5C所示，图5A绘示往图4A的投射点P1重合于测试平面20的参考点O1的影像示意图，图5B绘示往图1的-yC轴向观看测试平面20的示意图，而图5C绘示往图1的-xC轴向观看测试平面20的示意图。在本步骤S123中，如图5B～图5C所示，控制器140控制机械手臂110移动该机械手臂移动向量SR，以将工具10的投射点P1移动至接近参考点O1，本发明实施例以投射点P1移动至重合参考点O1为例说明；然而在另一实施例中，投射点P1可移动至接近但不重合参考点O1。然后，摄像器120提取测试平面20的影像M1，如图5A所示，影像M1具有投射点P1的影像。
由于本步骤S123将投射点P1往参考点O1靠近，因此在后续步骤S124A中移动后的投射点P1’(移动后的投射点P1’绘示于图6A)不会掉出测试平面20外，且/或在后续步骤S124B中工具10转动后的投射点P1’(工具10转动后的投射点P1’绘示于图7A)不会掉出测试平面20外。在另一实施例中，若在步骤S124A中移动后的投射点P1’不会掉出测试平面20外且在步骤S124B中工具10转动后的投射点P1’不会掉出测试平面20外，则可省略步骤S122及S123。
然后，在步骤S124中，控制器140可执行工具10的工具轴向A1相对第一轴向(例如，xC轴向)的偏移修正。以下进一步以步骤S124A～S124C进行说明。
在步骤S124A中，如图6A～图6C所示，图6A绘示往图5A的投射点P1的位置脱离测试平面20的参考点O1的影像示意图，图6B绘示往图1的-yC轴向观看测试平面20的示意图，而图6C绘示往图1的-xC轴向观看测试平面20的示意图。在本步骤S124A中，如图6B及图6C所示，机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的第三轴向(例如，zC轴向)移动，如箭头所示工具10往-zC轴向移动或平移。然后，摄像器120提取测试平面20的影像M1，如图6A所示，影像M1具有移动后投射点P1’的影像。由于工具轴向A1未垂直测试平面20，因此，在工具10沿摄像器参考坐标系(xC-yC-zC)的第三轴向(例如，zC轴向)移动后，图5A所示的投射点P1的位置改变至如图6A所示的投射点P1’的位置。
在步骤S124B中，控制器140依据摄像器120所提取的影像，判断于测试平面20的投射点P1于第一轴向(例如，xC轴向)的位置是否改变。若是(例如，第一轴向/xC轴向的平移测试中，图5B的投射点P1的位置沿-xC轴向移动至图6B的投射点P1’的位置，代表第一轴向/xC轴向有所偏差，后续需要转动调整)，流程进入S124C；若否(代表第一轴向/xC轴向无偏差)，流程进入S125A。
在步骤S124C中，如图7A及图7B所示，其绘示图6B的工具10绕摄像器参考坐标系(xC-yC-zC)的yC轴向转动的示意图。机械手臂110驱动工具10绕摄像器参考坐标系(xC-yC-zC)的第二轴向(例如，yC轴向)转动一角度α1，以减少工具轴向A1与zC轴向间的角度β1，即，使工具轴向A1往平行于zC轴向的趋势发展。此外，角度α1例如是任意角度，此处采取试误法移动角度α1，详言之，以yC轴向为支点或中心，逆时钟旋转弧角角度α1，用于逐渐减少工具轴向A1与zC轴向间的角度β1，通常旋转后，在测试平面20的投影点P1可能不会保持在原先的位置。
详言之，在步骤S124A中，在机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的+/-zC轴向移动或平移后，如图7A所示，若投射点P1’往负第一轴向(例如，-xC轴向)移动或偏移，则机械手臂110驱动工具10绕正第二轴向(例如，+yC轴向)转动，以减少工具轴向A1与zC轴向间的角度β1，即，使工具轴向A1(于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影)往平行zC轴向的趋势发展(或说是使工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影往垂直测试平面20的趋势发展)。此外，只要工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影往平行zC轴向的趋势发展即可，本发明实施例不限定转动过程致中投射点P1’的位置是否改变。
在另一实施例中，如图7C所示，其绘示另一实施例中图6B的投影点P1’往正第一轴向(例如，+xC轴向)偏移的示意图。在步骤S124A中，在机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的+/-zC轴向移动后，若投射点P1’往正第一轴向(例如，+xC轴向)移动或偏移，则机械手臂110驱动工具10绕负第二轴向(例如，-yC轴向)转动，以减少工具轴向A1与zC轴向间的角度β1，以投射点P1’为支点，顺时钟旋转弧角角度α1，用于逐渐减少工具轴向A1与zC轴向间的角度β1，即，使工具轴向A11于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影往平行于zC轴向的趋势发展(或说是使工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影往垂直测试平面20的趋势发展)。
控制器140重复执行步骤S124A～S124C，直到工具10的工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影(例如图8的视角)是平行于摄像器参考坐标系(xC-yC-zC)的zC轴向，或使工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影垂直测试平面20，如图8所示。至此，完成工具10的工具轴向A1相对xC轴向的偏移修正。进一步来说，当机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的+/-zC轴向移动时，若投射点P1’于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影沿xC轴向的一位置改变量实质上等于0(即，投射点P1’于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影沿xC轴向的位置不再改变)，表示工具轴向A1于摄像器参考坐标系(xC-yC-zC)的xC-zC平面上的投影已平行于摄像器参考坐标系(xC-yC-zC)的zC轴向，则流程可进入步骤S125，控制器140执行工具10的工具轴向A1相对第二轴向(例如，yC轴向)的偏移修正，如步骤S125A～S125C的流程。
在步骤S125A～S125C中，控制器140及机械手臂110可采用相似步骤S124A～S124C的流程，完成yC轴向的偏移修正。以下进一步以图6A及图6C以及图9A～图9B举例说明。
在本步骤S125A中，如图6C所示，机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的第三轴向(例如，zC轴向)移动，如箭头所示工具10往-zC轴向移动或平移。然后，摄像器120提取测试平面20的影像M1，如图6A所示，影像M1具有移动后投射点P1’的影像。由于工具轴向A1未垂直测试平面20，因此，在工具10沿摄像器参考坐标系(xC-yC-zC)的第三轴向(例如，zC轴向)移动后，图5A所示的投射点P1的位置改变至如图6A所示的投射点P1’的位置。在另一实施例中，若已执行步骤S124A，则可选择性省略本步骤S125A。
在步骤S125B中，控制器140依据摄像器120所提取的影像M1，判断于测试平面20的投射点P1于第二轴向(例如，yC轴向)的位置是否改变。若是(例如，图5C的投射点P1的位置沿-yC轴向移动至图6C的投射点P1’的位置)，流程进入S125C；若否，流程进入S126。
在步骤S125C中，如图9A所示，其绘示图6C的工具10绕摄像器参考坐标系(xC-yC-zC)的xC轴向转动的示意图。机械手臂110驱动工具10绕摄像器参考坐标系(xC-yC-zC)的第一轴向(例如，-xC轴向)转动一角度α2，以减少工具轴向A1与zC轴向间的角度β2，即，使工具轴向A1往平行于zC轴向的趋势发展。此外，角度α2例如是任意角度。
详言之，在步骤S125A中，在机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的+/-zC轴向移动后，如图9A所示，若投射点P1’往负第二轴向(例如，-yC轴向)移动或偏移，则机械手臂110驱动工具10绕负第一轴向(例如，-xC轴向)转动，以减少工具轴向A1与zC轴向间的角度β2，以投射点P1’为支点，顺时钟旋转弧角角度α2，用于逐渐减少工具轴向A1与zC轴向间的角度β2，即，使工具轴向A1于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影往平行zC轴向的趋势发展(或说是使工具轴向A1(于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影往垂直测试平面20的趋势发展)。此外，只要工具轴向A1于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影往平行zC轴向的趋势发展即可，本发明实施例不限定转动过程中投射点P1’的位置是否改变。
控制器140重复执行步骤S125A～S125C，直到工具10的工具轴向A1于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影(例如图9B的视角)是平行于摄像器参考坐标系(xC-yC-zC)的zC轴向，或使工具轴向A1于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影垂直测试平面20，如图9B所示。至此，完成工具10的工具轴向A1相对yC轴向的偏移修正。进一步来说，当机械手臂110驱动工具10沿摄像器参考坐标系(xC-yC-zC)的+/-zC轴向移动时，若投射点P1’于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影于yC轴向一位置改变量实质上等于0(即，投射点P1’于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影沿yC轴向的位置不再改变)，表示工具轴向A1于摄像器参考坐标系(xC-yC-zC)的yC-zC平面上的投影已平行于摄像器参考坐标系(xC-yC-zC)的zC轴向，则流程可进入步骤S126。
在步骤S126中，在完成第一轴向及第二轴向的偏移修正后(表示工具轴向A1垂直于测试平面20，步骤S124、S125的目的为修正xC与yC轴向偏移)，控制器140依据工具轴向A1垂直于测试平面20时机械手臂110的姿态，建立第二转换关系T2，并依据第二转换关系T2，取得工具轴向量Tez，工具轴向量Tez例如是平行于或重合于工具轴向A1。例如，控制器140依据工具轴向A1垂直于测试平面20时机械手臂110的各关节J1～J6的关节角度，建立第二转换关系T2。第二转换关系T2是工具10的装设面110s的装设面坐标系(xf-yf-zf)(或，法兰面)相对机械手臂参考坐标系(xR-yR-zR)的转换关系。工具10可装设于装设面110s上，且工具10的工具轴向A1不限于垂直于装设面110s。在一实施例中，第二转换关系T2可表示于下式(6)，式(6)中的元素可由机械手臂110的连杆参数(Denavit-Hartenberg Parameters)、关节J1～J6的坐标及工具中心点W1相对于装设面坐标系(xf-yf-zf)的信息获得，其中，连杆参数可包含连杆偏移(Link offset)、关节角度(Joint angle)、连杆长度(Link length)及连杆扭转(Link twist)。此外，可采用已知的运动学方法建立第二转换关系T2。
如下式(7)，向量zw为测试平面20的法向量(即，zC轴向)相对机械手臂参考坐标系(xR-yR-zR)的向量，而向量Tez为工具轴向A1相对装设面坐标系(xf-yf-zf)的向量(本文称「工具轴向量」)。控制器140可通过第二转换关系T2的反矩阵，将向量zw转换成工具轴向量Tez。
Tez＝T2-1·zw……(7)
在步骤S130中，机械手臂系统100执行校正点信息组取得步骤。以下进一步举例说明。
在步骤S131中，请同时参考图10A～图10B，图10A绘示图1的光源130所发出的第二光线L2与工具10沿工具轴向A1所发出的第一光线L1交会于工具中心点W1的示意图，而图10B绘示图10A的光源130所发出的第二光线L2投射于测试平面20的投射点PL2与工具10沿工具轴向A1所发出的第一光线L1投射于测试平面20的投射点PL1为分离的二点的影像示意图。在本步骤中，可调整光源130的角度，使光源130所发出的第二光线L2与工具10所发出的第一光线L1交会于工具中心点W1，如图10A所示。
在实施例中，工具10是以辉度计为例说明，其工具中心点W1为虚拟工具中心点。工具中心点W1例如是工具10所投射第一光线L1(检测光)的焦点。在另一实施例中，工具10例如是加工刀具，其工具中心点W1为工具中心点，如实体刀尖点。综上可知，本发明实施例的工具中心点可以是实体工具中心点或虚拟工具中心点。
在其中一种调整光源130的角度的方法中，控制器140可依据下式(8)计算出光源130所发出的第二光线L2与工具10沿与工具轴向A1垂直的方向(自转动支点131至工具轴向A1的虚线)所发出的第一光线L1的夹角θ，然后可采用手动或控制器140控制一机构(未绘示)的方式将光源130的角度调整至夹角θ，使光源130所发出的第二光线L2与工具10所发出的第一光线L1交会于工具中心点W1。前述机构例如是连杆机构、齿轮组机构等各种可以驱动光源130转动的机构。由于已知夹角θ，因此可快速地调整光源130的角度，使所发出的第二光线L2与工具10所发出的第一光线L1交会于工具中心点W1。当将光源130的角度调整至夹角θ时，可固定光源130与工具10间的相对关系，以固定工具中心点W1与工具10的相对关系。
式(8)中，H1是工具中心点W1与工具10的光发射面10s之间沿工具轴向A1的距离(例如，第一光线L1的焦距)，H2是工具10的光发射面10s与光源130的转动支点131之间沿工具轴向A1的距离，而H3是光源130的转动支点131与工具轴向A1之间的垂直距离(垂直于工具轴向A1)。
如图10B所示，由于工具中心点W1尚未重合于测试平面20，因此光源130所发出的第二光线L2投射于测试平面20的投射点PL2与工具10沿工具轴向A1所发出的第一光线L1投射于测试平面20的投射点PL1为分离的二点。
在步骤S132中，控制器140执行一校正点信息组取得步骤。例如，控制器140可控制机械手臂110在数个不同姿态下工具中心点W1重合于测试平面20的参考点O1的多个校正点，并据以记录各校正点的校正点信息组。例如，控制器140可控制机械手臂110在一姿态下，工具中心点W1重合于测试平面20的参考点O1，并记录此姿态下的一校正点信息组，然后再改变机械手臂110于另一不同姿态下工具中心点W1重合于测试平面20，并记录此不同姿态下的一校正点信息组。依此原则，控制器140可取得机械手臂110于数个不同姿态下的多组校正点信息。各校正点信息组可包含关节J1～J6的坐标，而各个关节的坐标可为各个关节相对于其预设起始点的转动角度。不同姿态下的机械手臂110的数个转节角度的至少一者可能相异。
进一步举例来说，请同时参考图11A～图11B，图11A绘示图10A的工具中心点W1重合于测试平面20的示意图，而图11B绘示图11A的工具中心点W1与参考点O1相距投影点移动向量SW的影像示意图。在步骤S132A～S132B中，控制器140可控制机械手臂110驱动工具10沿工具轴向A1移动，直到工具中心点W1重合于测试平面20，如图11A所示。
在步骤S132A中，如图11A所示，机械手臂110驱动工具10沿工具轴向量Tez移动。在一实施例中，机械手臂110可驱动工具10沿工具轴向A1的正方向或负方向移动，此时工具轴向量Tez例如是平行于或重合于工具轴向A1，由步骤S126所取得的结果。
在步骤S132B中，如图11B所示，控制器140依据(例如，分析)摄像器120所提取的测试平面20的影像M1，判断工具中心点W1是否重合于测试平面20。若是，流程进入步骤S132C；若否，控制器140重复执行步骤S132A～S132B，直到工具中心点W1重合于测试平面20，如图11B所示。进一步来说，当工具中心点W1重合于测试平面20时，在测试平面20上会呈现出一个光点(即，工具中心点W1)。控制器140可分析摄像器120所提取的测试平面20的影像M1，判断测试平面20是否已出现一个光点；若是，表示工具中心点W1已重合于测试平面20(例如图11A所示)，流程进入到步骤S132C；若否(例如，出现二个光点，即投射点PL1与投射点PL2)，表示工具中心点W1尚未重合于测试平面20，则流程回到步骤S132A，机械手臂110继续驱动工具10沿工具轴向A1的正方向或负方向移动，直到工具中心点W1重合于测试平面20。
在步骤S132C中，如图11B所示，控制器140依据摄像器120提取的测试平面20的影像，取得投射点移动向量SW。
在步骤S132D中，控制器140依据第一转换关系T1及投射点移动向量SW，取得机械手臂移动向量SR。例如，控制器140可将图11B的投射点移动向量SW代入上式(5)，以计算出机械手臂110要将工具中心点W1移动至重合于参考点O1的所需移动向量SR。
在步骤S132E中，请同时参考图12A～图12B，其绘示图11A的工具中心点W1重合于参考点O1的示意图。在本步骤S132E中，控制器140控制机械手臂110移动该机械手臂移动向量SR，使工具中心点W1重合于参考点O1的。
在步骤S132F中，控制器140依据(或分析)摄像器120所提取的测试平面20的影像(例如，图12B所示的影像M1)，判断工具中心点W1与测试平面20的参考点O1是否重合。若是，流程进入步骤S132G；若否，流程回到步骤S132A。
进一步来说，若图10A的工具轴向A1非平行于zC轴向，则在步骤S132E后，可能因为机械手臂的移动误差使得控制命令与实际动作不一致，实际运动未完全沿着机械手臂移动向量SR动作，而使测试平面20上可能再度出现光源130所发出的第二光线L2投射于测试平面20的投射点PL2与工具10沿工具轴向A1所发出的第一光线L1投射于测试平面20的投射点PL1为分离的二光点的情况(例如图10B所示)，此情况表示工具中心点W1脱离测试平面20(表示工具中心点W1与测试平面20的参考点O1也未重合)。因此流程可进入步骤S132A，让工具中心点W1与测试平面20重合。当步骤S132F中工具中心点W1与测试平面20的参考点O1重合时，表示工具中心点W1与测试平面20重合且工具中心点W1与参考点O1也重合，则流程才进入步骤S132G；若否，则流程回到步骤S132A。
在步骤S132G中，控制器140记录工具中心点W1与测试平面20的参考点O1重合的状态下机械手臂110的各关节J1～J6的关节角度，并做为一校正点信息组。
在步骤S132H中，控制器140判断校正点信息组的组数是否已达一预定数量，如至少3组，然而也可更多。当校正点信息组的组数已达预定数量，流程进入步骤S133；当校正点信息组的组数未达预定数量，流程进入步骤S132I。
在步骤S132I中，控制器140控制机械手臂110改变工具10的姿态。例如，控制器140控制机械手臂110改变工具10的工具轴向A1相对xC轴向的角度、yC轴向与zC轴向中至少一者的角度，改变的角度例如是30度、60度或其它任意角度值。举例来说，控制器140可通过乱数产生器产生一方位角(Euler Angle)增量ΔRx,ΔRy,ΔRz以修正机械手臂110的方位角，由此改变机械手臂110的姿态；此时，机械手臂110的方位角可表示为(Rx+ΔRx,Ry+ΔRy,Rz+ΔRz)，其中(Rx,Ry,Rz)为机械手臂110原来的方位角；其中Rx表示偏航角(Yaw angle)；Ry表示螺倾角(Pitch angle)；Rz表示滚转角(Roll angle)。若修正的方位角超过机械手臂110的运动范围，则控制器140可通过乱数产生器重新产生方位角增量。
然后，流程回到步骤S132A，以记录工具10在新(不同)姿态下机械手臂110的校正点信息组。进一步来说，控制器140控制机械手臂110改变工具10的姿态后，工具10的工具中心点W1可能脱离测试平面20，因此流程回到步骤S132A，让工具中心点W1与参考点O1再度重合，并在工具中心点W1与参考点O1重合的状态下，记录机械手臂110的不同姿态下的另一校正点信息组。重复步骤S132A～S132I，直到控制器140所记录的校正点信息组的组数达到预定数量。
在步骤S133中，当控制器140所记录的校正点信息组的组数达到预定数量时，控制器140依据此些校正点信息组，取得工具中心点相对装设面坐标系(xf-yf-zf)的工具中心点坐标TP。
如下式(9)所示，工具中心点坐标TP可依据机械手臂110在数个不同姿态下的数个校正点信息组所建立。控制器140可根据该些校正点信息组计算工具中心点W1的坐标；其中各校正点信息组的坐标可通过机械手臂110的连杆参数(Denavit-HartenbergParameters)、关节J1～J6的坐标及工具中心点W1相对于装设面坐标系(xf-yf-zf)的信息获得，其中，连杆参数可包含连杆偏移(Link offset)、关节角度(Joint angle)、连杆长度(Link length)及连杆扭转(Link twist)。
工具中心点W1的坐标可由下式(9)计算：
式(9)中矩阵T2i是将第i个校正点信息组的坐标，由机械手臂参考坐标系(xR-yR-zR)转换至装设面坐标系(xf-yf-zf)的4×4齐次转换矩阵，式(9)的矩阵W1f包含[Tx Ty Tz 1]t为工具中心点W1相对于装设面坐标系(xf-yf-zf)的坐标W1f(Tx,Ty,Tz)，矩阵[Px Py Pz 1]t包含工具中心点W1在空间中相对于机械手臂参考坐标系(xR-yR-zR)的坐标W1R(Px,Py,Pz)。各校正点信息组可通过式(9)得到三条线性方程式，因此n个校正点信息组可得到3n条方程式，然后可采用虚拟反矩阵(Pseudo-inverse matrix)求得工具中心点W1的坐标。
进一步说明，式(9)中，(e11i,e21i,e31i)表示第i个校正点信息组在第一轴向(例如，xf轴向)的向量相对于机械手臂参考坐标系(xR-yR-zR)的方向；(e12i,e22i,e32i)表示第i个校正点信息组在第二轴向(例如，yf轴向)的向量相对于机械手臂参考坐标系(xR-yR-zR)的方向；(e13i,e23i,e33i)表示第i个校正点信息组在第三轴向(例如，zf轴向)的向量相对于机械手臂参考坐标系(xR-yR-zR)的方向。由式(9)可推得下式(10)及式(11)：
/>
其中，
式(11)中，为T3的转置矩阵(Transpose matrix)，/>为/>的反矩阵(Inverse matrix)，式(10)、(11)中的坐标(Tx Ty Tz)为工具中心点坐标TP，矩阵T3为数个校正点信息组所构成的校正点信息组矩阵。
若校正点信息组的组数已足够，则将已知的第i个校正点信息组所对应的矩阵T2i内各元素代入式(10)并将矩阵T3移项后得式(11)，取得工具中心点W1相对于装设面坐标系(xf-yf-zf)的坐标W1f(Tx,Ty,Tz)及工具中心点W1相对于机械手臂参考坐标系(xR-yR-zR)的坐标W1R(Px,Py,Pz)。
当然，上述工具中心点校正方法仅为举例，机器手臂系统100的各元件及/或校正方法均可依实际需求改变，本发明实施例并不以此为限。
在取得工具中心点坐标TP后，控制器140可据以驱动机械手臂110，控制工具中心点W1到所欲位置。如此，机械手臂系统100可执行机械手臂的自动教导过程，以下进一步以图13及图14A～图14B举例说明。
请参照图13及图14A～图14B，图13绘示依照本发明一实施例的机械手臂系统100的自动教导方法的流程图，图14A绘示图1的机械手臂系统100对工具10的工具中心点W1进行一第一检测教导的示意图，而图14B绘示图1的机械手臂系统100对工具10的工具中心点W1进行一第二检测教导的示意图。
以下以步骤S210～S260说明机械手臂系统100对工具10的工具中心点W1进行一第一检测教导的流程。
在步骤S210中，如图14A所示，控制器140利用工具中心点坐标TP(Tx Ty Tz)，驱动工具10至第一位置S1，并使在第一位置S1，工具中心点W1重合于检测面30(xd-yd平面)的指定点31。详言之，由于控制器140可依据校正点信息组矩阵T3(例如，上式(10))计算得出工具中心点坐标TP，因而可控制工具10的工具中心点坐标TP移动至所欲位置，使得移动工具中心点坐标TP至重合于检测面30的指定点31。检测面30例如是显示器的显示面，指定点31例如是检测面30的任意点，如检测面30的中心点。
在步骤S220中，如图14A所示，控制器140平移工具10一平移距离LH至一第二位置S2。平移工具10的方式例如是，机械手臂系统100更包括一移动器150，其可滑移地配置于机械手臂110，工具10及光源130可配置在移动器150上，使工具10及光源130(光源130未绘示于图14A)可随移动器150平移。在实施例中，控制器140可控制移动器150平移该平移距离LH，以同步带动工具10平移该平移距离LH。
在步骤S230中，如图14A所示，依据平移距离LH及工具10的工具中心点W1沿工具轴向A1的一行程差ΔTZ1(利用移动器150的平移轴模块搭配三角测距法求得)，取得工具10的一检测角度θH，如下式(13)。检测角度θH例如是工具轴向A1投影在xd-yd平面时，工具轴向A1与轴向xd的夹角。
θH＝π/2-tan-1(ΔTZ1/LH)……(13)
在步骤S240中，控制器140判断检测角度θH是否符合一第一规格角度。当检测角度θH不符合第一规格角度，流程进入步骤S250，控制器140驱动工具10回到第一位置S1。例如，控制器140可控制移动器150平移，以带动工具10回到至第一位置S1。第一规格角度例如是产品的规格值，其是工具10对检测面30沿第一检测方向进行检测时所要求的规格值。详言之，当检测角度θH符合第一规格角度时，工具10(例如是辉度计)对显示器的分析结果未超出范围(例如，以歪斜视角观看显示器的显示画面时不会产生黑屏或是色彩异常等状况)。第一规格角度的数值可视产品种类而定，例如是平面显示器的最大视角或可视角度，本发明实施例不加以限定。
在步骤S260中，当回到第一位置时，控制器140调整机械手臂110的姿态，以改变工具轴向A1相对检测面30的角度，然后流程回到步骤S210。控制器140可重复步骤S210～S260，直到检测角度θH是否符合第一规格角度。例如，若检测角度θH不符合第一规格角度，控制器140控制机械手臂110转动，使工具10绕第二轴向(例如，yd轴向)转动一角度，然后流程回到步骤S210。依此原则重复步骤S210～S260，直到检测角度θH符合第一规格角度。
相似地，如图14B所示，机械手臂系统100可采用相似方法(采用图13的流程)，对工具10的工具中心点W1进行一第二检测教导的流程。
例如，在步骤S210中，如图14B所示，控制器140利用工具中心点坐标TP(Tx Ty Tz)，驱动工具至第一位置S1，并使在第一位置S1，工具中心点W1重合于检测面30的指定点31。
在步骤S220中，如图14B所示，控制器140平移工具10一平移距离LV至第二位置S2。在实施例中，控制器140可控制移动器150平移该平移距离LV，以同步带动工具10平移该平移距离LV。
在步骤S230中，如图14B所示，依据平移距离LV及工具10的工具中心点W1沿工具轴向A1的一行程差ΔTZ2，取得工具10的一检测角度θV，如下式(14)。检测角度θV例如是工具轴向A1投影在xd-yd平面时，工具轴向A1与轴向zd的夹角。行程差ΔTZ2例如是可采用移动器150的平移轴模块搭配三角测距法求得。
θV＝tan-1(ΔTZ2/LV)……(14)
在步骤S240中，控制器140判断检测角度θV是否符合一第二规格角度。当检测角度θV不符合第二规格角度，流程进入步骤S250，控制器140驱动工具10回到第一位置S2。例如，控制器140可控制移动器150平移，以带动工具10回到至第一位置S2。第二规格角度例如是产品的规格值，其是工具10对检测面30沿第二检测方向进行检测时所要求的规格值。详言之，当检测角度θV符合第二规格角度时，工具10(例如是辉度计)对显示器的分析结果未超出范围(例如，以歪斜视角观看显示器的显示画面时不会产生黑屏或是色彩异常等状况)。第二规格角度的数值可视产品种类而定，本发明实施例不加以限定。
在步骤S260中，当回到第一位置S1时，控制器140调整机械手臂110的姿态，以改变工具轴向A1相对检测面30的角度，然后流程回到步骤S210。控制器140可重复步骤S210～S260，直到检测角度θV符合第二规格角度。例如，若检测角度θV不符合第二规格角度，控制器140控制机械手臂110转动，使工具10绕第一轴向(例如，xd轴向)转动一角度，直到检测角度θV符合第二规格角度。
当检测角度θH符合第一规格角度且检测角度θV符合第二规格角度时，控制器140依据机械手臂110当时的姿态，记录关节坐标组合或进行检测。例如，控制器140记录机械手臂110在步骤S210～S260过程中关节J1～J6的运动参数改变量。
综上，依照本发明实施例的机械手臂系统及应用其工具中心点的校正方法，可不需配置额外的感测器及测量器(例如，三次元测量装备)，即可进行对工具中心点的校正及机械手臂的自动教导。
综上所述，虽然结合以上实施例公开了本发明，然而其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，可作各种的更动与润饰。因此，本发明的保护范围应当以附上的权利要求所界定的为准。

Claims (16)

1.一种工具中心点的校正方法，包括：
执行机械手臂的机械手臂参考坐标系与摄像器参考坐标系之间第一转换关系建立步骤，包括：
机械手臂驱动该工具的工具轴向投影于测试平面的投射点相对该测试平面的参考点进行相对运动；及
依据该相对运动，建立该第一转换关系；
取得该工具相对该机械手臂的装设面参考坐标系的工具轴向量；
执行校正点信息组取得步骤，包括：
(a1)该机械手臂驱动工具中心点重合于该测试平面的该参考点，并记录该机器手臂的校正点信息组；
(a2)该机械手臂驱动该工具改变该工具轴向的角度；及
(a3)重复步骤(a1)及(a2)，以取得多个校正点信息组；以及
依据该些校正点信息组，取得该工具中心点相对该装设面参考坐标系的工具中心点坐标。
2.如权利要求1所述的校正方法，其中在该机械手臂驱动该工具的该工具轴向投影于该测试平面的该投射点相对该测试平面的该参考点进行该相对运动的步骤还包括：
该机械手臂驱动该工具从该参考点沿该机械手臂参考坐标系的多个轴向分别移动一空间向量；
其中，于执行该第一转换关系建立步骤还包括：
摄像器提取该投射点于该测试平面移动的影像；
其中，于建立该第一转换关系的步骤还包括：
分析该摄像器所提取的该影像，以取得各该空间向量的平面坐标的数值；及
依据该些空间向量互相正交的特性，建立该机械手臂参考坐标系与该摄像器参考坐标系间的该第一转换关系。
3.如权利要求1所述的校正方法，其中在该机械手臂驱动该工具的该工具轴向投影于该测试平面的该投射点相对该测试平面的该参考点进行该相对运动的步骤还包括：
该机械手臂驱动工具沿该机械手臂参考坐标系的第一轴向从该参考点移动第一空间向量；
该机械手臂驱动工具沿该机械手臂参考坐标系的第二轴向从该参考点移动第二空间向量；
该机械手臂驱动工具沿该机械手臂参考坐标系的第三轴向从该参考点移动第三空间向量；
其中，于执行该第一转换关系建立步骤还包括：摄像器提取该投射点于该测试平面移动的一影像；
其中，于建立该第一转换关系的步骤还包括：
分析该摄像器所提取的该影像，以取得该第一空间向量的第一平面坐标的数值；
分析该摄像器所提取的该影像，以取得该第二空间向量的第二平面坐标的数值；
分析该摄像器所提取的该影像，以取得该第三空间向量的第三平面坐标的数值；及
依据该第一空间向量、该第二空间向量与该第三空间向量互相正交的特性，建立该机械手臂参考坐标系与该摄像器参考坐标系间的该第一转换关系。
4.如权利要求1所述的校正方法，其中在取得该工具轴向量的步骤包括：
执行该工具轴向相对该摄像器参考坐标系的第一轴向的偏移修正，包括：
(b1)驱动该工具沿该摄像器参考坐标系的第三轴向移动；
(b2)依据一摄像器所提取的该工具相对该测试平面移动的影像，判断于该测试平面的该投射点于该摄像器参考坐标系的该第一轴向的位置是否改变；
(b3)当该投射点于该第一轴向的位置有改变时，驱动该工具绕该摄像器参考坐标系的第二轴向转动一角度；以及
(b4)重复步骤(b1)～(b3)，直到该测试平面的该投射点于该摄像器参考坐标系的该第一轴向的一位置改变量实质上等于0。
5.如权利要求4所述的校正方法，其中在取得该工具轴向量的步骤还包括：
当该投射点于该第一轴向的该位置改变量实质上等于0时，执行该工具轴向相对该摄像器参考坐标系的该第二轴向的偏移修正，包括：
(c1)驱动该工具沿该摄像器参考坐标系的该第三轴向移动；
(c2)依据该摄像器所提取的该工具相对该测试平面移动的该影像，判断于该测试平面的该投射点于该摄像器参考坐标系的第二轴向的位置是否改变；
(c3)当该投射点于该第二轴向的位置有改变时，驱动该工具绕该摄像器参考坐标系的第一轴向转动一角度；以及
(c4)重复步骤(c1)～(c3)，直到该测试平面的该投射点于该摄像器参考坐标系的该第二轴向的位置改变量实质上等于0。
6.如权利要求1所述的校正方法，其中在取得该工具相对该机械手臂的该装设面参考坐标系的该工具轴向量的步骤包括：
驱动该工具的该工具轴向垂直该测试平面；以及
依据该工具轴向垂直于该测试平面时该机械手臂的姿态，取得该工具轴向量。
7.如权利要求1所述的校正方法，其中于取得该工具中心点坐标的步骤包括：
调整光源的角度，使该工具所发出的第一光线与该光源所发出的第二光线交会于该工具中心点；
控制该机械手臂在多个不同姿态下该工具中心点重合于该参考点的多个校正点信息组；
驱动该工具沿该工具轴向量移动；
依据该些校正点信息组，建立校正点信息组矩阵；以及
依据该校正点信息组矩阵，取得该工具中心点坐标。
8.一种机械手臂教导方法，包括：
(d1)使用如权利要求1所述的校正方法，取得该工具中心点坐标，且驱动该工具至第一位置，并使在该第一位置，该工具中心点重合于检测面的指定点；
(d2)平移该工具平移距离至第二位置；
(d3)依据该平移距离及该工具的该工具中心点沿该工具轴向的行程差，取得该工具的一检测角度；
(d4)判断该检测角度是否符合规格角度；
(d5)当该检测角度不符合该规格角度，驱动该工具回到该第一位置；以及
(d6)调整该机械手臂的姿态，执行步骤(d2)～(d6)，直到该检测角度符合该规格角度。
9.一种机械手臂系统，其特征在于，包括：
机械手臂，用以装载工具，该工具具有工具轴向；
控制器，用以：
控制该机械手臂驱动该工具的该工具轴向投影于一测试平面的投射点相对该测试平面的参考点进行相对运动；
依据该相对运动，建立该机械手臂的机械手臂参考坐标系与摄像器的摄像器参考坐标系的第一转换关系；
取得该工具相对该机械手臂的装设面参考坐标系的工具轴向量；
执行校正点信息组取得步骤，包括：
(a1)该机械手臂驱动工具中心点重合于该测试平面的该参考点，并记录该机器手臂的校正点信息组；
(a2)该机械手臂驱动该工具改变该工具轴向的角度；及
(a3)重复步骤(a1)及(a2)，以取得多个校正点信息组；以及
依据该些校正点信息组，取得该工具中心点相对该装设面参考坐标系的工具中心点坐标。
10.如权利要求9所述的机械手臂系统，还包括：
摄像器，用以提取该投射点于该测试平面移动的影像；
其中，该控制器还包括：
控制该机械手臂驱动该工具从该参考点沿该机械手臂参考坐标系的多个轴向分别移动空间向量；
分析该摄像器所提取的该影像，以取得各该空间向量的平面坐标的数值；及
依据该些空间向量互相正交的特性，建立该机械手臂参考坐标系与该摄像器参考坐标系间的该第一转换关系。
11.如权利要求9所述的机械手臂系统，还包括：
摄像器，用以提取该投射点于该测试平面移动的影像；
其中，该控制器还包括：
控制该机械手臂驱动工具沿该机械手臂参考坐标系的第一轴向从该参考点移动第一空间向量；
控制该机械手臂驱动工具沿该机械手臂参考坐标系的第二轴向从该参考点移动第二空间向量；
控制该机械手臂驱动工具沿该机械手臂参考坐标系的第三轴向从该参考点移动第三空间向量；
分析该摄像器所提取的该影像，以取得该第一空间向量的第一平面坐标的数值；
分析该摄像器所提取的该影像，以取得该第二空间向量的第二平面坐标的数值；
分析该摄像器所提取的该影像，以取得该第三空间向量的第三平面坐标的数值；及
依据该第一空间向量、该第二空间向量与该第三空间向量互相正交的特性，建立该机械手臂参考坐标系与该摄像器参考坐标系间的该第一转换关系。
12.如权利要求9所述的机械手臂系统，还包括：
摄像器，用以提取该投射点于该测试平面移动的影像；
其中，该控制器还用以：执行该工具轴向相对该摄像器参考坐标系的第一轴向的偏移修正，包括：
(b1)驱动该工具沿该摄像器参考坐标系的第三轴向移动；
(b2)依据一摄像器所提取的该工具相对该测试平面移动的影像，判断于该测试平面的该投射点于该摄像器参考坐标系的该第一轴向的位置是否改变；
(b3)当该投射点于该第一轴向的位置有改变时，驱动该工具绕该摄像器参考坐标系的第二轴向转动一角度；以及
(b4)重复步骤(b1)～(b3)，直到该测试平面的该投射点沿该摄像器参考坐标系的该第一轴向的位置改变量实质上等于0。
13.如权利要求12所述的机械手臂系统，其中该控制器还用以：当该投射点于该第一轴向的该位置改变量实质上等于0时，执行该工具轴向相对该摄像器参考坐标系的该第二轴向的偏移修正，包括：
(c1)驱动该工具沿该摄像器参考坐标系的该第三轴向移动；
(c2)依据该摄像器所提取的该工具相对该测试平面移动的该影像，判断于该测试平面的该投射点于该摄像器参考坐标系的第二轴向的位置是否改变；
(c3)当该投射点于该第二轴向的位置有改变时，驱动该工具绕该摄像器参考坐标系的第一轴向转动一角度；以及
(c4)重复步骤(c1)～(c3)，直到该测试平面的该投射点于该摄像器参考坐标系的该第二轴向的位置改变量实质上等于0。
14.如权利要求9所述的机械手臂系统，其中该控制器还用以：
驱动该工具的该工具轴向垂直该测试平面；
依据该工具轴向垂直于该测试平面时该机械手臂的姿态，取得该工具相对该机械手臂的该装设面参考坐标系的该工具轴向量。
15.如权利要求9所述的机械手臂系统，其中该工具所发出的第一光线与光源所发出的第二光线交会于该工具中心点；该控制器还包括：
控制该机械手臂在多个不同姿态下该工具中心点重合于该参考点的多个校正点信息组；
驱动该工具沿该工具轴向量移动；
依据该些校正点信息组，建立校正点信息组矩阵；以及
依据该校正点信息组矩阵，取得该工具中心点坐标。
16.如权利要求9所述的机械手臂系统，其中该控制器还包括：
(d1)驱动该工具至第一位置，并使在该第一位置，该工具中心点重合于检测面的指定点；
(d2)平移该工具平移距离至第二位置；
(d3)依据该平移距离及该工具的该工具中心点沿该工具轴向的行程差，取得该工具的检测角度；
(d4)判断该检测角度是否符合规格角度；
(d5)当该检测角度不符合该规格角度，驱动该工具回到该第一位置；以及
(d6)调整该机械手臂的姿态，执行步骤(d2)～(d6)，直到该检测角度符合该规格角度。
CN202110007956.4A 2020-08-31 2021-01-05 工具中心点的校正方法、机械手臂的教导方法及其系统 Active CN114102578B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
TW109129784A TWI748626B (zh) 2020-08-31 2020-08-31 工具中心點的校正方法、機械手臂的教導方法及應用其之機械手臂系統
TW109129784 2020-08-31

Publications (2)

Publication Number Publication Date
CN114102578A CN114102578A (zh) 2022-03-01
CN114102578B true CN114102578B (zh) 2023-12-12

Family

ID=80356179

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110007956.4A Active CN114102578B (zh) 2020-08-31 2021-01-05 工具中心点的校正方法、机械手臂的教导方法及其系统

Country Status (3)

Country Link
US (1) US20220063104A1 (zh)
CN (1) CN114102578B (zh)
TW (1) TWI748626B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7684898B2 (en) * 2004-03-03 2010-03-23 Advintec Gmbh Method for calibrating a tool center point of tools for industrial robots
CN106168466A (zh) * 2015-05-21 2016-11-30 财团法人工业技术研究院 全域式影像检测系统及其检测方法
DE102015109960A1 (de) * 2015-06-22 2016-12-22 Deutsches Zentrum für Luft- und Raumfahrt e.V. Vorrichtung und Verfahren zum optischen Bestimmen einer Position und/oder Orientierung eines Manipulators
KR20180078100A (ko) * 2016-12-29 2018-07-09 대우조선해양 주식회사 용접로봇의 tcp 캘리브레이션 장치 및 그를 이용한 캘리브레이션 방법
CN109571546A (zh) * 2017-09-29 2019-04-05 财团法人工业技术研究院 机器人工具中心点校正系统及其方法
CN111267092A (zh) * 2019-08-27 2020-06-12 上海飞机制造有限公司 标定机器人工具坐标系的方法和系统

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE0400320D0 (sv) * 2004-02-06 2004-02-06 Abb Ab Control method for robots
DE102005048136B4 (de) * 2005-10-06 2010-01-21 Kuka Roboter Gmbh Verfahren zum Bestimmen eines virtuellen Tool-Center-Points
US9151607B2 (en) * 2010-05-31 2015-10-06 University Of North Carolina At Charlotte Dimensional measurement through a combination of photogrammetry and optical scattering
TWI404609B (zh) * 2010-10-21 2013-08-11 Ind Tech Res Inst 機械手臂系統參數的校正方法與校正裝置
TWI408037B (zh) * 2010-12-03 2013-09-11 Ind Tech Res Inst 機械手臂的定位方法及校正方法
US9188973B2 (en) * 2011-07-08 2015-11-17 Restoration Robotics, Inc. Calibration and transformation of a camera system's coordinate system
US10547796B2 (en) * 2015-07-14 2020-01-28 Industrial Technology Research Institute Calibration equipment and calibration method of a mechanical system
JP2018094648A (ja) * 2016-12-09 2018-06-21 セイコーエプソン株式会社 制御装置、ロボットおよびロボットシステム
EP3338969A3 (en) * 2016-12-22 2018-07-25 Seiko Epson Corporation Control apparatus, robot and robot system
TWI699264B (zh) * 2019-07-05 2020-07-21 上銀科技股份有限公司 視覺導引機器手臂校正方法
TWI701123B (zh) * 2019-12-18 2020-08-11 財團法人工業技術研究院 機器人工件座標系自動校正系統與方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7684898B2 (en) * 2004-03-03 2010-03-23 Advintec Gmbh Method for calibrating a tool center point of tools for industrial robots
CN106168466A (zh) * 2015-05-21 2016-11-30 财团法人工业技术研究院 全域式影像检测系统及其检测方法
DE102015109960A1 (de) * 2015-06-22 2016-12-22 Deutsches Zentrum für Luft- und Raumfahrt e.V. Vorrichtung und Verfahren zum optischen Bestimmen einer Position und/oder Orientierung eines Manipulators
KR20180078100A (ko) * 2016-12-29 2018-07-09 대우조선해양 주식회사 용접로봇의 tcp 캘리브레이션 장치 및 그를 이용한 캘리브레이션 방법
CN109571546A (zh) * 2017-09-29 2019-04-05 财团法人工业技术研究院 机器人工具中心点校正系统及其方法
CN111267092A (zh) * 2019-08-27 2020-06-12 上海飞机制造有限公司 标定机器人工具坐标系的方法和系统

Also Published As

Publication number Publication date
TW202210973A (zh) 2022-03-16
US20220063104A1 (en) 2022-03-03
TWI748626B (zh) 2021-12-01
CN114102578A (zh) 2022-03-01

Similar Documents

Publication Publication Date Title
JP6429473B2 (ja) ロボットシステム、ロボットシステムの校正方法、プログラム、およびコンピュータ読み取り可能な記録媒体
US9517560B2 (en) Robot system and calibration method of the robot system
CN108818536B (zh) 一种机器人手眼标定的在线偏移修正方法及装置
US10906182B2 (en) Method of teaching robot and robot system
CA2977077C (en) Robotic arm camera system and method
US8918210B2 (en) Method of detecting an inter-axis offset of 6-axis robot
CN113001535B (zh) 机器人工件坐标系自动校正系统与方法
JP4267005B2 (ja) 計測装置及びキャリブレーション方法
US5297238A (en) Robot end-effector terminal control frame (TCF) calibration method and device
CN111801198B (zh) 一种手眼标定方法、系统及计算机存储介质
US20090234502A1 (en) Apparatus for determining pickup pose of robot arm with camera
US10239206B2 (en) Robot controlling method, robot apparatus, program and recording medium
US20090118864A1 (en) Method and system for finding a tool center point for a robot using an external camera
WO1998057782A1 (en) Method and device for robot tool frame calibration
JP2015089575A (ja) ロボット、制御装置、ロボットシステム及び制御方法
JP3644991B2 (ja) ロボット−センサシステムにおける座標系結合方法
CN111002304B (zh) 用于获取机械手的末端执行器的位置和定位的装置
KR20080088165A (ko) 로봇 캘리브레이션 방법
CN115091456A (zh) 基于矩阵求解的机器人手眼标定方法
EP3602214B1 (en) Method and apparatus for estimating system error of commissioning tool of industrial robot
CN115446847A (zh) 用于提高机器人系统的3d眼手协调准确度的系统和方法
Deniz et al. In-line stereo-camera assisted robotic spot welding quality control system
JP2013010149A (ja) ６軸ロボットの軸間オフセット検出方法
CN114102578B (zh) 工具中心点的校正方法、机械手臂的教导方法及其系统
CN115972192A (zh) 具有可变空间分辨率的3d计算机视觉系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant