一种六自由度工业机器人末端工具坐标系标定装置及方法与流程

本发明属于机器人末端工具坐标系标定
技术领域：
，更具体地，涉及一种六自由度工业机器人末端工具坐标系标定装置及方法。
背景技术：
：工业机器人是通过末端安装不同的工具完成各种作业任务的，工具坐标系的准确度直接影响机器人的轨迹精度。尤其对于将工业机器人运用于铣削、钻孔、打磨等机械加工场合，对机器人末端的轨迹精度要求就非常高，因此，在使用之前通常需要标定机器人的工具坐标系。标定机器人的工具坐标系，一般是指测量法兰末端装配的工具的末端点相对于法兰中心点坐标系的偏移。传统的六自由度工业机器人末端工具坐标系标定是目前生产现场普遍采用的，如图1所示，在工作台上放置具有尖端的标定工具1，机器人夹持着同样具有尖端的工具2，调整机器人的位置和姿态，工具2的尖端对准工具1的尖端并使两个尖端几乎处于接触的状态(实际上不可能做到)，记录此时机器人法兰中心点的位置和姿态坐标此处定义的位姿坐标是一个六维的向量，其中x,y,z表示位置分量，A,B,C表示姿态分量；然后调整机器人以另外三种不同的姿态使得工具2的尖端指向并接触工具1的尖端，记录下最后通过工具坐标系的四点标定算法，计算出此时工具末端相对于法兰末端坐标系的偏移量，通过此偏移量即可确定装配在法兰上的工具相对于机器人基座标系的坐标转换。传统的标定方法对操作人员的要求比较高，需要操作员娴熟地调整机器人的位姿，还要时刻地观察两个尖端的对正的接触状况，同时标定过程中工具2的尖端要指向并尽可能地保持接触到工具1的尖端，实际很难做到。因此，需设计一种能够不完全取决于操作员肉眼判断的六自由度工业机器人末端工具坐标系标定工具，以提高工业机器人在机械加工领域的应用。技术实现要素：针对现有技术的以上缺陷或改进需求，本发明提供了一种六自由度工业机器人末端工具坐标系标定装置及方法，其通过设置可形成圆环的经度条对，以形成标定所需的光路，可实现4点及以上(最多13个点)的工具坐标系标定，解决现有标定装置标定结果过分依赖于操作员的熟练程度和肉眼观察的限制，提高工具坐标系的标定精度。为实现上述目的，按照本发明的一个方面，提出了一种六自由度工业机器人末端工具坐标系标定装置，该装置包括经度条、上联结板、下联结板、光路发送管和光路接收管，其中：所述经度条的两端分别与所述上联结板和下联结板相连，该经度条两两组成一对，每对经度条通过上联结板和下联结板连接成一个完整的圆环；所述经度条上半部的30°、45°和60°纬度处分别设有一开设有第一中心孔的圆凸台，该第一中心孔由上半部分的锥孔和下半部分的圆柱孔组合而成，该锥孔和圆柱孔的轴线重合；所述经度条下半部的30°、45°和60°纬度处同样分别设有一开设有第二中心孔的圆凸台，该第二中心孔为圆柱孔；所述上联结板的侧面设有安装所述经度条的凹槽，并且其中部为开设有第三中心孔的圆凸台，该第三中心孔由上半部分的锥孔和下半部分的圆柱孔组合而成，该锥孔和圆柱孔的轴线重合；所述下联结板的侧面同样设有安装所述经度条的凹槽，并且其中部为开设有第四中心孔的圆凸台，该第四中心孔为圆柱孔；所述光路发送管为圆柱形结构，其一端设计成锥形，该锥形的一端正好与所述第一中心孔的锥孔相配合，并且该端的内部开设有与所述第一中心孔的圆柱孔尺寸相同的孔，所述光路发送管的另一端开设有圆柱孔，该圆柱孔内安装有激光发射头；所述光路接收管为管状结构，其内设置有大小不一的两个轴线重合的圆柱孔，其中尺寸较小的圆柱孔的直径与所述第二中心孔的直径相同，尺寸较大的圆柱孔内安装有激光接收头。作为进一步优选的，所述标定装置优选设置有两对经度条，并且该两对经度条组成的两个圆环彼此垂直。作为进一步优选的，所述上联结板和下联结板上均设置有四个互成90°的用于安装所述经度条的矩形凹槽。作为进一步优选的，所述经度条的两端设计成一水平平台，便于与上、下联结板定位连接，所述经度条中部偏下位置设置一延伸的耳部结构，以便于标定装置的固定，所述经度条中部的内侧设置有加强筋，以加强经度条的刚性。作为进一步优选的，所述第一中心孔的锥孔的底面与一对经度条组成的圆环的外环直径相切；所述第一中心孔的圆柱孔的直径小于第一中心孔的锥孔底部的直径。作为进一步优选的，所述光路接收管的一端为圆盘状结构，该圆盘状结构的直径与所述经度条下半部的圆凸台的直径相同，其上间隔均布有将所述光路接收管定位并固定在经度条下部分的圆凸台上的六个螺钉孔。按照本发明的另一方面，提供了一种六自由度工业机器人末端工具坐标系标定方法，该方法包括如下步骤：(1)将权利要求1-6任一项所述的标定装置固定在机器人工作空间的工作台上，调整机器人的位置和姿态，使光路发送管的锥形端插入上联结板(2)的锥孔中；(2)微调机器人的姿态，使光路发送管(4)中激光发射头(6)发出的激光通过下联结板(3)上的光路接收管(5)，并被光路接收管末端的激光接收头(7)接收，记下此时机器人的位姿(3)调整机器人的位置和姿态，使光路发送管(4)拔出上联结板(2)的锥孔，并位于锥孔的正上方，光路接收管末端的激光接收头(7)接收光路发送管(4)中激光发射头(6)发出的激光，记下此时机器人的位姿(4)调整机器人沿着机器人基座标系的Y轴方向运动一段距离，记下此时机器人的位姿(5)调整机器人的位置和姿态，使光路发送管(4)插入经度条(1)的三个圆凸台的锥孔中，并保证光路接收管末端的激光接收头(7)能接收到光路发送管(4)中激光发射头(6)发出的激光，分别记下此时机器人的位姿和(6)通过采集的上述六个数据，计算获得新建工具坐标系相对于法兰中心坐标系的位姿偏移，以此完成工具坐标系的标定。作为进一步优选的，所述步骤(6)包括如下子步骤：(6.1)根据采集到的所述六个数据转换得到对应的齐次坐标矩阵(6.2)从各个所述齐次坐标矩阵中提取前三行的前三列，得到对应的旋转矩阵提取各个所述齐次坐标变换矩阵第四列的前三个元素得到对应的位置偏移向量BPEO1、BPEO2、BPEO3、BPEO4、BPEO5、BPEO6；(6.3)根据所述步骤(6.2)中的结果计算获得虚拟新建工具坐标系相对于法兰中心坐标系的位置偏移向量EPVTO；然后计算获得虚拟新建工具坐标系Z轴在法兰中心坐标系下的方向向量TEa，并初次计算虚拟新建工具坐标系X轴在法兰中心坐标系下的方向向量(6.4)根据所述和采用矢量积的方法计算获得虚拟新建工具坐标系Y轴在法兰中心坐标系下的方向向量然后再次计算获得虚拟新建工具坐标系X轴在法兰中心坐标系下的方向向量(6.5)根据所述EPVTO和所述采用下式计算实际新建工具坐标系相对于法兰中心坐标系的偏移向量EPTO：PETO=PEVTO+RTEa;]]>其中，R为补偿半径，其等于标定装置的一对经度条组成的圆环的外圆半径；(6.6)根据所述和EPTO采用下式获得TTE=nTEoTEaTEPETO0001;]]>(6.7)通过齐次坐标矩阵到欧拉角的反解得到实际新建工具坐标系相对于法兰中心坐标系的位姿偏移。总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：1.本发明可用于六自由度工业机器人末端工具坐标系中心点的标定，采用激光束通过狭小空间管的方式限定工具坐标系标定过程中的姿态偏差，锥形孔定位的方式可有效减小位置偏移的误差，克服传统标定工具标定精度取决于操作员肉眼判断的弊端，对于提高工业机器人在机械加工领域的应用大有帮助。2.本发明的标定装置通过设置经度条对以形成圆环，并使圆环上对应的光路发送管和光路接收管的中心轴线重合，以形成在一直线上的激光光路，使得从光路发送管发射的激光可顺利的被光路接收管中的激光接收头接收，进一步提高标定精度。3.本发明使用时将标定装置固定在机器人工作空间的工作台上，调整机器人位置和姿态，使得光路发送管锥形的一端插入选定工作对的锥形孔，微调机器人姿态，使得激光发射头发出的激光通过选定工作对对应的光路接收管被光路接收管末端的激光接收器接收，实现机器人位姿的标记与检测，为机器人末端工具坐标系的标定提供可靠数据。附图说明图1是传统工具坐标系标定装置的示意图；图2是本发明工具坐标系标定装置的装配图；图3是本发明工具坐标系标定装置的剖视图；图4(a)-(b)是本发明的经度条的结构示意图；图5(a)-(c)是本发明的上联结板的结构示意图；图6(a)-(c)是本发明的下联结板的结构示意图；图7(a)-(b)是本发明的光路发送管的结构示意图；图8(a)-(b)是本发明的光路接收管的结构示意图；图9是本发明的基座标系、法兰中心坐标系、新建工具坐标系和虚拟新建坐标系之间的关系图；图10是本发明的标定结果的计算流程图。具体实施方式为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。本发明实施例提供的一种六自由度工业机器人末端工具坐标系标定装置，该装置主要包括经度条1、上联结板2、下联结板3、光路发送管4和光路接收管5，其中，经度条1安装在上联结板2、下联结板3上，并用于安装光路发送管4和光路接收管5，为标定装置的标记提供所需的光路，上联结板2、下联结板3主要作为安装部件用于安装经度条，并与两个经度条组成一个圆环，使得一个经度条上激光发射头发射的激光能被另一个经度条上的激光接收头检测到，光路发送管4用于将激光发射头安装在经度条上，并为激光发射提供光路，光路接收管5用于将激光接收头安装在经度条上，并为激光的接收提供光路。通过上述各个部件的相互配合，可实现六自由度工业机器人末端4点及以上(最多13个点)的工具坐标系标定，具有标定精度高、操作方便等优点。下面将分别对各个部件进行详细的说明和描述。如图2-4所示，经度条1为半圆形条状件，其两端分别通过螺栓与上联结板2和下联结板3相连，该经度条1两两组成一对，每对经度条1通过上联结板2和下联结板3连接成一个完整的圆环；经度条1上半部的30°、45°和60°纬度处分别设有一开设有第一中心孔的圆凸台，该第一中心孔由上半部分的锥孔和下半部分的圆柱孔组合而成，该锥孔和圆柱孔的轴线重合，该锥孔的底面与一对经度条组成的圆环的外环直径相切，圆柱孔的直径小于锥孔底部的直径，以保证光路发送管的径向定位，同时下端圆柱孔还给激光发射头发射的激光提供了通道。经度条1下半部的30°、45°和60°纬度处同样分别设有一开设有第二中心孔的圆凸台，该第二中心孔为圆柱孔，该圆柱孔的直径理论上只需设计成大于激光的光束直径即可。作为优选的实施例，标定装置设置有两对经度条1，并且该两对经度条1组成的两个圆环彼此垂直，上联结板2和下联结板3上均设置有四个互成90°的用于安装经度条1的矩形凹槽。具体的，经度条1的两端设计成一水平平台，便于与上、下联结板定位连接，经度条1中部偏下位置设置一延伸的耳部结构，以便于标定装置的固定，经度条1中部的内侧设置有加强筋，以加强经度条的刚性，提高其整体的抗变形能力。如图5所示，上联结板2主体类似圆盘结构，其侧面设有安装经度条1的凹槽，安装时，经度条1嵌装在凹槽内，并且上联结板2的中部设计成开设有第三中心孔的圆凸台，该第三中心孔由上半部分的锥孔和下半部分的圆柱孔组合而成，该锥孔和圆柱孔的轴线重合，该第三中心孔的形状和尺寸与第一中心孔的形状和尺寸完全相同。如图6所示，下联结板3主体也类似圆盘结构，其侧面同样设有安装经度条1的凹槽，安装时，经度条1嵌装在凹槽内，并且下联结板3的中部设计成开设有第四中心孔的圆凸台，该第四中心孔为圆柱孔，该圆柱孔为激光束提供通道。如图7所示，光路发送管4为圆柱形结构，其一端设计成锥形(即设计锥度)，锥度大小与经度条上半部第一中心孔的锥孔一致，并且该端的内部开设有与第一中心孔的圆柱孔尺寸相同的孔，光路发送管4的另一端开设有圆柱孔，该圆柱孔的直径与激光发射头6的直径一致，以用于安装有激光发射头6，光路发送管4两端开设的孔相连通，并且轴线重合。如图8所示，光路接收管5为管状结构，其内设置有大小不一的两个轴线重合的圆柱孔，其中尺寸较小的圆柱孔的直径与第二中心孔的直径相同，尺寸较大的圆柱孔内设有螺纹，该圆柱孔通过螺纹连接安装有激光接收头7。优选的，光路接收管5开设有尺寸较小圆柱孔的一端设计成圆盘状结构，该圆盘状结构的直径与经度条下半部的圆凸台的直径相同，其上间隔均布有六个螺钉孔(即按60°的间隔均匀分布)，通过螺钉可将光路接收管定位并固定在经度条下部分的圆凸台上。实际使用时，将本发明的标定装置安装在支架上，该支架的底端为一方形底板，底板设计有一定的厚度，以增加底板的重量，使整个标定装置的中心下移，提高装置的稳定性，支架的顶端为一圆环状结构，圆环上每隔90°布置一组螺钉孔，共四组，每组设3个螺钉孔，用于将经度条、上联结板和下联结板组合的整体固定在支架上，具体的，通过螺钉将经度条1上的耳部结构固定在支架顶端的圆环状结构上，由此实现标定装置的整体安装与固定。支架的底端和顶端通过四根圆柱形的柱子相连，柱子的底端留有凸台，在该凸台上通过螺栓将柱子固定在底板上。下面将对本发明标定装置的标定方法进行示例性说明。首先对标定装置中的各个部件进行组装，其共设有四条经度条，分别用A、B、C、D表示，其中A和B为一组，与上下联结板连接后组成一个圆环，C和D为一组，与上下联结板连接后组成一个圆环，每条经度条上半部的凸台从上往下分别记为1、2、3，下半部的凸台从上往下分别记为4、5、6，光路接收管5共设有13个，其中的12个固定在四条经度条的下半部的凸台上(即固定在A4、A5、A6、B4、B5、B6、C4、C5、C6、D4、D5和D6凸台上)，通过将光路接收管的底端与经度条下半部凸台的端面贴齐，螺钉旋入对应的螺钉孔，将二者固定，剩余的一个光路接收管固定在下联结板的凸台端面上，同样通过螺钉联结固定。如此，A1凸台的锥孔与B6凸台中心孔中的光路接收管工作时组成一对，同理锥孔A2与B5光路接收管组成一对，锥孔A3与B4光路接收管组成一对，共组成12对工作对组合，所有工作对组合如表1所示。表1组装后，每一工作对的锥孔和光路接收管的中心孔轴线重合，并定义该轴线为工作对轴线，所有工作对轴线都交于空间一点，该点也是以经度条A、B组成的圆的圆心(以经度条C、D组成的圆的圆心与A、B组成的圆的圆心重合)。此外，还有一个特殊的工作对组合，即上联结板顶端凸台的锥孔和安装在下连接板凸台上的光路接收管中心孔，记为锥孔0光路接收管中心孔0组合，简称0-0组合。最后，将装有激光发射头的光路发送管4固定在机器人的末端执行器上。为便于后续标定方法的描述，在标定装置进行标定之前，进行前提设定与介绍，记原始工具坐标系(法兰中心点建立的坐标系)到机器人基座标系的齐次坐标变换矩阵为新建工具坐标系到机器人基座标系的齐次坐标变换矩阵为新建工具坐标系到原始工具坐标的齐次坐标变换矩阵为和之间存在转换关系：其中和可以写成以下的分块形式：TEB=REBPBEOi01---(1);]]>TTB=RTBPBTOi01---(2);]]>TTE=RTEPETOi01---(3);]]>其中，是法兰中心坐标系{E}相对于机器人基座标系{B}的三阶旋转矩阵，可以由三个方向矢量构造而成，即是法兰中心坐标系{E}的X轴在机器人基座标系{B}下的方向余弦，即是新建工具坐标系{T}相对于机器人基座标系{B}的三阶旋转矩阵；是新建工具坐标系{T}相对于法兰中心坐标系{E}的三阶旋转矩阵，其构造与的构造类似。由和(1)、(2)、(3)可得到(4)：PBTO=REB·PETO+PBEO---(4);]]>机器人工具坐标系的标定主要是将标定结果用于计算通常机器人法兰末端中心点的位置和姿态可通过机器人控制器获得，进而可计算出通过和计出由可解出新建工具坐标系在基座标系下的位置和姿态新建工具坐标系到原始工具坐标的齐次坐标变换矩阵的计算，需要通过标定计算得出，标定计算可以分为两个部分：中心点标定和姿态标定。所述中心点标定计算，调整机器人位姿，使得新工具的末端中心点从不同方位接触空间某一固定点，此时机器人法兰中心点位姿BPEO1、BPEO2、…BPEOn，由此计算得出n为标定计算所需测得点数。因为末端工具是安装在法兰末端的，末端中心点相对于法兰中心坐标系的位置是不变的，即EPTO＝EPTO1＝EPTO2＝…＝EPTOn。同时标定过程中，新工具的末端中心点保证接触空间某一固定点，BPTO1＝BPTO2＝…＝BPTOn，即:REB1·PETO+PBEO1=REB2·PETO+PBEO2REB2·PETO+PBEO2=REB3·PETO+PBEO3......REBn-1·PETO+PBEOn-1=REBn·PETO+PBEOn---(5)]]>所述式(5)中各项等式通过简单的左右移项变换，再写成矩阵形式如下：REB1-REB2REB2-REB3...REBn-1-REBn.PETO=PBEO2-PBEO1PBEO3-PBEO2...PBEOn-PBEOn-1---(6)]]>所述式(6)中左边的系数矩阵和右边的矩阵是3(n-1)×3的矩阵，n为标定过程中的实测点数。通过矩阵论的知识，当n≥3时，可以通过求系数矩阵的加号广义逆，解出EPTO如(7)式所示：PETO=PBEO2-PBEO1PBEO3-PBEO2...PBEOn-PBEOn-1REB1-REB2REB2-REB3...REBn-1-REBn+---(7)]]>采用本发明装置测量的点利用上述计算过程得到的EPTO不是实际新建工具坐标系的EPTO，而是一个虚拟的EPTO，可以记为EPVTO，如图9所示，描述了机器人基座标系、法兰中心坐标系、新建工具坐标系和虚拟新建工具坐标系的关系。为方便计算，将刀轴矢量方向定义为新建工具坐标系的Z轴，虚拟工具坐标系只是沿着实际新建工具坐标系的Z轴正方向做了平移，现在只需要测出新建工具坐标系Z轴在法兰中心坐标系下的矢量描述即可将实际工具坐标系相对于法兰中心坐标系的偏移EPTO计算出来。记新建工具坐标系Z轴在法兰中心坐标系下的单位矢量为关于的详细计算在姿态标定部分会详细说明，由图9可推出：PETO=PEVTO+R·TEa---(8)]]>所述式(8)中，R为补偿半径，其值等于标定工具经度条的外圆半径，通过式(8)可以求出实际新建工具坐标系相对于法兰中心坐标系的偏移分量，这一步完成了中心点标定，接下来进行姿态标定。所述姿态标定，即确定新建工具坐标系各坐标轴在法兰中心坐标系下的矢量描述。本发明中采取先确定Z轴的方案，并定义Z轴沿刀具轴线方向向下。具体实施过程中第一个测点取标定工具顶端的点，得到再从经度条上取三个测点分别得到通过这四个点可以计算出EPVTO；继续调整机器人位置，使测头位于标定工具顶端测点的正上方，调整机器人姿态，使得测头发出的激光束能被底端的激光接收器接收到，记录此时的机器人位姿通过和B,C)可以确定新建工具坐标系的Z轴；然后再调整机器人沿机器人基座标系的X轴(或Y轴，此约定为X轴)移动一段距离，记下此时的机器人位姿通过和可以确定新建工具坐标系的X轴，在确定Z轴、X轴的基础上可以利用矢量积的方法进一步确定Y轴。其中，位姿坐标是一个六维的向量，x,y,z表示位置分量，A,B,C表示姿态分量所述姿态标定中计算新建工具坐标系各坐标轴在法兰坐标系下的方向余弦的具体步骤如下：已知和可以得到(9)和(10)，式中和BPEO1、BPEO5可以通过和计算得到，EPVTO可以通过(7)计算得到。PBVTO1=REB1·PEVTO+PBEO1---(9)]]>PBVTO5=REB5·PEVTO+PBEO5---(10)]]>由(9)和(10)可以得到新建工具坐标系Z轴在机器人基座标系下的矢量描述如(11)所示：VBTz=PBVTO5-PBVTO1|PBVTO5-PBVTO1|---(11)]]>再将BVTZ通过旋转变换转换到法兰中心坐标系{E}下即可得到：aTE=R5-1EB·VBTz=R5-1EB(PBVTO-PBVTO1)|PBVTO5-PBVTO1|---(12)]]>同理，通过可以得到：PBVTO6=REB6·PEVTO+PBEO6---(13)]]>通过(10)和(13)可得到新建工具坐标系X轴在机器人基座标系下的矢量描述如(14)所示：VBTx=PBVTO5-PBVTO6|PBVTO5-PBVTO6|---(14)]]>再将BVTX通过旋转变换转换到法兰中心坐标系{E}下即可得到：nTE=R5-1EB·VBTx=R5-1EB(PBVTO5-PBVTO6)|PBVTO5-PBVTO6|---(15)]]>通过矢量积的方法，可以求出新建工具坐标系的Y轴在法兰中心坐标系下的矢量描述为：oTE=aTE×nTE---(16)]]>为进一步保证X，Y，Z矢量的单位化、正交化，可以通过重新求解X轴的矢量分量，如下：nTE=oTE×aTE---(17)]]>通过以上过程的计算，可以得到新建工具坐标系到法兰中心坐标系的齐次坐标变换矩阵为：TTE=nTEoTEaTEPETO0001---(18).]]>下面采用本发明的标定装置进行标定，其主要包括如下步骤：(1)将组装好的标定装置固定在机器人工作空间的工作台上，保证其摆放平稳端正，调整机器人的位置和姿态，使其上的光路发送管4的锥形端插入上联结板2的锥孔中(即0-0组合锥形孔)；(2)微调机器人的姿态，使光路发送管4中激光发射头6发出的激光通过0-0光路，被下联结板3上的光路接收管5末端的激光接收头7接收，记下此时机器人的位姿(3)调整机器人的位置和姿态，使光路发送管4离开0-0组合锥形孔到达锥形孔正上方，光路接收管末端的激光接收头7接收光路发送管4中激光发射头6发出的激光，记下此时机器人的位姿(4)调整机器人沿着机器人基座标系的Y轴方向运动一段距离，记下此时机器人的位姿(5)在12组工作对中任意选择三个工作对，调整机器人的位置和姿态，使光路发送管4插入经度条1的任意三个圆凸台的锥孔中，并保证光路接收管末端的激光接收头7能接收到光路发送管4中激光发射头6发出的激光，分别记下此时机器人的位姿(6)标定操作完成之后，进入计算流程，通过得到的初始数据计算新建工具坐标系相对于法兰中心坐标系的位姿偏移，以此完成工具坐标系的标定，计算流程如图10所示。所述步骤(6)具体包括如下子步骤：(6.1)根据得到的初始数据通过欧拉角与齐次坐标矩阵之间的转换得到对应的齐次坐标矩阵(6.2)从各个所述齐次坐标矩阵中提取前三行的前三列，得到对应的旋转矩阵提取各个所述齐次坐标变换矩阵第四列的前三个元素组成的向量可以得到对应的位置偏移向量，此处的位置偏移向量与前述的位姿是不一样的，位置偏移向量是一个三维向量，而位姿描述向量是一个六维向量，为区别开来，用如下的方式描述位置偏移向量：BPEO1、BPEO2、BPEO3、BPEO4、BPEO5、BPEO6；(6.3)将所述步骤(6.2)中的结果带入公式(7)中，计算获得虚拟新建工具坐标系相对于法兰中心坐标系的偏移向量EPVTO(x，y，z)；并由公式(9)、(10)、(11)、(12)联立计算得到虚拟新建工具坐标系Z轴在法兰中心坐标系下的方向向量由公式(10)、(13)、(14)、(15)联立初次计算得到虚拟新建工具坐标系X轴在法兰中心坐标系下的方向向量(6.4)通过矢量积的方法，由公式(16)计算获得虚拟新建工具坐标系Y轴在法兰中心坐标系下的方向向量由公式(17)再次计算得到虚拟新建工具坐标系X轴在法兰中心坐标系下的方向向量(6.5)根据所述EPVTO(x，y，z)和所述由公式(8)计算实际新建工具坐标系相对于法兰中心坐标系的偏移向量EPTO(x，y，z)；(6.6)根据所述和EPTO(x，y，z)组合可以得到如式(18)；(6.7)通过齐次坐标矩阵到欧拉角的反解得到实际新建工具坐标系相对于法兰中心坐标系的位姿偏移。本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。当前第1页1 2 3