Fast calibration method of robot tool coordinate

机器人工具坐标系的快速标定方法 Fast calibration method of robot tool coordinate first-author 李广云（1965-），男，江苏南京人，教授，博士生导师，中国测绘地理信息学会常务理事、中国卫星导航与定位协会副会长，战略支援部队信息工程大学地理空间信息学院教授，主要研究方向为精密工程测量、卫星导航与应用。E-mail：guangyun_li_chxy@163.com http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423163&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423169&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423166&type= 22.01333618 32.00404358 李广云 （1965-），男，江苏南京人，教授，博士生导师，中国测绘地理信息学会常务理事、中国卫星导航与定位协会副会长，战略支援部队信息工程大学地理空间信息学院教授，主要研究方向为精密工程测量、卫星导航与应用。E-mail： guangyun_li_chxy@163.com guangyun_li_chxy@163.com corresp E-mail： 1728731316@qq. com E-mail： 1728731316@qq. com 罗豪龙（1996-），男，河南许昌人，硕士研究生，2018年于战略支援部队信息工程大学获得学士学位，现为战略支援部队信息工程大学地理空间信息学院硕士研究生，主要从事精密工程测量及三维激光扫描等方面的研究。E-mail：1728731316@qq.com http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423172&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423179&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423176&type= 22.01333618 32.00403595 罗豪龙 （1996-），男，河南许昌人，硕士研究生，2018年于战略支援部队信息工程大学获得学士学位，现为战略支援部队信息工程大学地理空间信息学院硕士研究生，主要从事精密工程测量及三维激光扫描等方面的研究。E-mail： 1728731316@qq.com 1728731316@qq.com 战略支援部队信息工程大学 地理空间信息学院，河南 郑州 450001 战略支援部队信息工程大学 地理空间信息学院，河南 郑州 450001 Strategic Support Force Information Engineering University， Zhengzhou 450001， China Strategic Support Force Information Engineering University， Zhengzhou 450001， China 为了解决机器人工具坐标系标定时标定精度不高、效率较低的问题，提出一种基于激光跟踪仪的工具坐标系快速标定方法。首先，分析机器人末端法兰盘的结构，根据法兰盘上各点的相对位置关系，利用激光跟踪仪及几何法原理进行工具坐标系的位置标定；其次，控制机器人沿工具坐标系的 X 轴和 Z 轴方向分别运动，根据工具坐标系和法兰末端坐标系的相对位姿关系进行工具坐标系姿态标定；最后，利用基于距离约束的工具坐标系标定方法和本文提出的几何法标定结果进行精度对比分析。实验结果表明几何法进行工具坐标系后机器人定位精度能达到0.692 mm，与距离约束法的工具坐标系标定精度相当；同时几何法不需要机器人运动，只需测量机器人末端的6个点就能实现工具坐标系的位置标定，且机器人末端更换新的工具后只需测量一个点就能实现新工具的位置标定。实验表明，几何法标定实验过程为3 min，而距离约束法实验过程为8 min，标定效率提高了62.5%，说明该方法具有高效率和高精度的特点，能够满足高精度任务作业的实际需要。 To address the problem of low precision or low efficiency in the calibration of a robot tool coordinate system， a fast calibration method of the tool coordinate system by a laser tracker is proposed. First， the structure of the robot end flange is analyzed based on the relative position relationship of each point on the flange. The laser tracker is used to calibrate the position of the tool coordinate system based on the principle of the geometric method. Second， the robot is controlled to move along the X - and Z -axes of the tool coordinate system， and the posture of the tool coordinate system is calibrated based on the relative position and posture relationship between the tool coordinate system and the flange end coordinate system. Finally， the calibration method based on the distance constraint proposed in previous studies is used to calibrate the tool coordinate system， and the accuracy is compared with that of the geometric method proposed in this paper. The experimental results show that the accuracy of the proposed calibration method can reach 0.692 mm， which is equivalent to that of the distance constraint method. Meanwhile， in the geometric method， the robot does not need to be moved； it only needs to measure six points at its end， to calibrate the position of the tool coordinate system. In addition， after the robot end is replaced with a new tool， the position of the new tool coordinate system can be obtained by measuring one point only. It shows that the proposed geometric method for a robot tool coordinate system calibration has high efficiency and high precision. 机器人 工具坐标系 几何法 法兰盘 激光跟踪仪 robot tool coordinate system geometric method flange laser tracker 1 引　言 1 随着制造业转型升级和社会自动化水平的不断提高，工业机器人在餐饮、电子、医疗和军事等领域应用越来越广泛 ［ 1 1 - 5 5 1-5 ］ 。众所周知，工业机器人在工作之前需要为其末端安装执行机构即工具。工业机器人的工具好比人类的手，根据任务的不同，机器人末端安装的工具也往往不相同。例如焊接机器人末端安装焊枪，喷绘机器人末端安装喷枪，搬运机器人末端安装手爪等等 ［ 6 6 - 7 7 6-7 ］ 。同时为使机器人正常工作，还需要获取机器人末端工具相对于机器人的位置和姿态，即进行工具坐标系标定。工具坐标系标定的精度关系着工业机器人的生产质量和工作效率；同时，机器人在实际工作中要求能够快速地进行机器人工具坐标系标定 ［ 8 8 - 10 10 8-10 ］ 。因此，研究一种能够快速、准确的工具坐标系标定方法对提高机器人的定位精度，拓展机器人的应用市场和提高机器人的智能化、柔性化水平具有重要意义。 目前，工业机器人的工具坐标系标定方法主要有以下两种：一是不借助外界测量设备，基于机器人本体进行工具坐标系标定 ［ 11 11 - 13 13 11-13 ］ 。牛雪娟 ［ 14 14 ］ 基于奇异值分解的原理通过三点五步法实现了工具坐标系标定；刘成业 ［ 15 15 ］ 提出工具坐标系的位置和姿态分开标定的方法，利用四个点标定工具坐标系位置，三个点标定工具坐标系姿态。但上述方法存在的问题是标定精度往往不高且要求工具具有尖端点特征。二是基于外界测量设备（如激光跟踪仪）进行工具坐标系标定 ［ 16 16 - 19 19 16-19 ］ 。齐立哲 ［ 20 20 ］ 利用距离约束原则，即空间中任意两点在基坐标系下和跟踪仪坐标系下保持距离不变进行工具坐标系标定，存在的问题是机器人的定位误差会引入建立的约束方程中，且标定过程易受机器人空间运动范围的限制；张博 ［ 21 21 ］ 提出控制机器人进行单轴运动，利用激光跟踪仪测量的机器人运动数据拟合机器人各关节轴的运动轨迹进行工具坐标系的标定。该方法的缺点是，标定过程繁琐且工作和计算量大，同时机器人的定位误差会降低标定精度。综上所述，传统的工具坐标系标定方法不能兼顾工具坐标系的标定精度和标定效率。为了解决标定精度不高、操作繁琐和标定效率较低等问题，本文通过分析机器人末端的法兰盘结构，基于法兰盘上各点的几何关系，提出一种几何法快速标定工具坐标系的方法。同时利用距离约束法进行了工具坐标系标定实验并比较了二者的精度。 2 几何法标定原理 1 几何法标定原理是分开标定工具坐标系的位置和姿态。首先，分析法兰盘上各点位置的几何关系，利用激光跟踪仪测量靶球在机器人法兰盘上的多个位置进行工具坐标系位置标定；之后控制机器人沿工具坐标系的坐标轴方向运动进行工具坐标系姿态标定。 2.1　坐标系定义 2 工具坐标系标定过程中涉及到多个坐标系，首先对坐标系进行定义。机器人底座建立的坐标系称为机器人基坐标系，基坐标系用｛B｝表示；世界坐标系一般默认与基坐标系重合，用｛W｝表示；在机器人末端法兰建立的坐标系称为法兰末端坐标系，法兰末端坐标系用｛E｝表示；机器人末端安装的工具上建立的坐标系称为工具坐标系，工具坐标系用｛T｝表示；激光跟踪系统建立的坐标系为激光跟踪仪坐标系，激光跟踪仪坐标系用｛L｝表示 ［ 22 22 ］ 。实验系统原理如 图1 图1 所示。工具坐标系标定的实质是获取机器人法兰末端坐标系到工具坐标系的位姿变换矩阵。 2.2　位置标定 2 工具坐标系的位置标定是求取工具坐标系的原点，即TCP（Tool Center Point）在法兰末端坐标系下的位置。工具坐标系位置标定过程主要分为以下三步：（1）求取法兰坐标系在跟踪仪坐标系下的位置；（2）获取TCP在跟踪仪坐标系下的位置；（3）通过坐标系转换得到TCP的位置（法兰末端坐标系下）。工具坐标系位置标定的求解过程如 图2 图2 所示，第一、第二、第三平面分别代表本文后面定义的平面 <math id="M1"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 ， <math id="M2"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 和 <math id="M3"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 。 工业机器人末端法兰平面示意图如 图3 图3 所示。法兰盘平面上有六个大小相同的小孔，编号分别为1，2，…，6。法兰盘的圆心 <math id="M4"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 为法兰坐标系的原点，小孔中心与法兰盘圆心 <math id="M5"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 的距离相等，且相邻小孔的夹角为 <math id="M6"><msup><mrow><mn mathvariant="normal">60</mn></mrow><mrow><mo>∘</mo></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423246&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423242&type= 4.48733330 3.04800010 。法兰盘圆心 <math id="M7"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 到小孔7的圆心方向为法兰坐标系的 <math id="M8"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴正方向，过法兰盘圆心 <math id="M9"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 且垂直于法兰盘平面的方向为法兰坐标系的 <math id="M10"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423827&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423825&type= 2.37066650 2.79399991 轴方向。定义法兰盘所在的平面为 <math id="M11"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 。由于法兰盘圆心 <math id="M12"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 在实际的法兰盘上是悬空的理想位置，无法直接测量，因此考虑利用激光跟踪仪间接测量得到法兰坐标系的位置。 首先将跟踪仪靶球依次放在1，3，5三个孔的位置处，假设3个位置处的靶球球心依次为 <math id="M13"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">01</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">02</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">03</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423397&type= 19.04999924 4.14866638 ， <math id="M14"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">01</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">02</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">03</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423397&type= 19.04999924 4.14866638 所在的平面为 <math id="M15"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 。利用跟踪仪测得 <math id="M16"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">01</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">02</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">03</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423397&type= 19.04999924 4.14866638 的坐标依次为 <math id="M17"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423341&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423338&type= 15.83266735 4.74133301 ， <math id="M18"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423349&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423344&type= 15.83266735 4.74133301 ， <math id="M19"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423361&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423357&type= 15.83266735 4.74133301 。由于3个小孔的圆心均匀分布在同一个圆上，故 <math id="M20"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">01</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">02</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">03</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423397&type= 19.04999924 4.14866638 也位于同一个圆上，假设圆心为 <math id="M21"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423393&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423391&type= 20.65866661 4.74133301 。根据 公式（1） 公式（1） 和 公式（2） 公式（2） 可以分别得到 <math id="M22"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">01</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">02</mn></mrow></msub><mo>,</mo><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">03</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423397&type= 19.04999924 4.14866638 所在的平面 <math id="M23"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 方程和圆心 <math id="M24"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 的位置。 <math display="block" id="M25"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>:</mo><mtext> </mtext><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><mi>A</mi><mo stretchy="false">(</mo><mi>x</mi><mo>-</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>B</mi><mo stretchy="false">(</mo><mi>y</mi><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>C</mi><mo stretchy="false">(</mo><mi>z</mi><mo>-</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mi>A</mi><mo stretchy="false">(</mo><mi>x</mi><mo>-</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>B</mi><mo stretchy="false">(</mo><mi>y</mi><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>C</mi><mo stretchy="false">(</mo><mi>z</mi><mo>-</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mi>A</mi><mo stretchy="false">(</mo><mi>x</mi><mo>-</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>B</mi><mo stretchy="false">(</mo><mi>y</mi><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo stretchy="false">)</mo><mo>+</mo><mi>C</mi><mo stretchy="false">(</mo><mi>z</mi><mo>-</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo stretchy="false">)</mo><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423426&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423421&type= 69.76532745 16.51000023 ， <math display="block" id="M26"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>:</mo><mtext> </mtext><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msup><mrow><mi>R</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><msup><mrow><mo>-</mo><mi>x</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><msup><mrow><mo>-</mo><mi>y</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><msup><mrow><mo>-</mo><mi>z</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mtd></mtr><mtr><mtd><msup><mrow><mi>R</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><msup><mrow><mo>-</mo><mi>x</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><msup><mrow><mo>-</mo><mi>y</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><msup><mrow><mo>-</mo><mi>z</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mtd></mtr><mtr><mtd><msup><mrow><mi>R</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><msup><mrow><mo>-</mo><mi>x</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><msup><mrow><mo>-</mo><mi>y</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mo stretchy="false">(</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><msup><mrow><mo>-</mo><mi>z</mi><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423434&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423430&type= 68.24134064 16.51000023 ， 整理得到，平面 <math id="M27"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 的一般方程为： <math display="block" id="M28"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>:</mo><mi>A</mi><mi>x</mi><mo>+</mo><mi>B</mi><mi>y</mi><mo>+</mo><mi>C</mi><mi>z</mi><mo>+</mo><msub><mrow><mi>d</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>=</mo><mn mathvariant="normal">0</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423451&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423447&type= 43.09533310 4.65666676 . 根据确定的 <math id="M29"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 方程和圆心 <math id="M30"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 的坐标 <math id="M31"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423481&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423473&type= 15.83266735 4.74133301 ，可以得到经过圆心 <math id="M32"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 且垂直于平面 <math id="M33"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 的法线方程 <math id="M34"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423667&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423665&type= 2.37066650 3.72533321 为： <math display="block" id="M35"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>:</mo><mfrac><mrow><mi>x</mi><mo>-</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub></mrow><mrow><mi>A</mi></mrow></mfrac><mo>=</mo><mfrac><mrow><mi>y</mi><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub></mrow><mrow><mi>B</mi></mrow></mfrac><mo>=</mo><mfrac><mrow><mi>z</mi><mo>-</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub></mrow><mrow><mi>C</mi></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423513&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423508&type= 47.07466888 7.53533268 . 由于法兰坐标系的 Z 轴与法线 <math id="M36"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423667&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423665&type= 2.37066650 3.72533321 的方向重合，因此得到法兰坐标系 Z 轴的单位方向向量为： <math display="block" id="M37"><mi mathvariant="bold-italic">Z</mi><mo>=</mo><mfrac><mrow><mo stretchy="false">(</mo><mi>A</mi><mo>,</mo><mi>B</mi><mo>,</mo><mi>C</mi><mo stretchy="false">)</mo></mrow><mrow><mroot><mrow><msup><mrow><mi>A</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><msup><mrow><mi>B</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><msup><mrow><mi>C</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow><mrow/></mroot></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423528&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423525&type= 33.27399826 9.31333351 . 将靶球固定在法兰盘平面任意位置处（如 图3 图3 中A点处），设此时靶球球心的位置为 <math id="M38"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">04</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423559&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423556&type= 5.07999992 3.72533321 ，过 <math id="M39"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">04</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423559&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423556&type= 5.07999992 3.72533321 且平行于平面 <math id="M40"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 的平面为平面 <math id="M41"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 。根据平面 <math id="M42"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 方程和 <math id="M43"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">04</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423559&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423556&type= 5.07999992 3.72533321 的位置，得到平面 <math id="M44"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 的方程为： <math display="block" id="M45"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>:</mo><mi>A</mi><mi>x</mi><mo>+</mo><mi>B</mi><mi>y</mi><mo>+</mo><mi>C</mi><mi>z</mi><mo>+</mo><msub><mrow><mi>d</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>=</mo><mn mathvariant="normal">0</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423569&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423567&type= 43.09533310 4.65666676 ， 工具坐标系位置标定中各点的位置关系如 图5 图5 所示，且根据平面的定义可知 <math id="M46"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">∥</mo><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">∥</mo><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423574&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423572&type= 18.54199982 4.57200003 。根据求得的平面 <math id="M47"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 方程和平面 <math id="M48"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 方程，计算平面 <math id="M49"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 与平面 <math id="M50"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 之间的距离 <math id="M51"><mi>d</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423595&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423593&type= 2.20133328 2.79399991 为： <math display="block" id="M52"><mi>d</mi><mo>=</mo><mo stretchy="false">|</mo><msub><mrow><mi>d</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>-</mo><msub><mrow><mi>d</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">|</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423600&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423598&type= 19.98133469 4.40266657 . 由于平面 <math id="M53"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 与法兰盘平面 <math id="M54"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 之间的距离为靶球半径，且靶球半径 <math id="M55"><mi>r</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423624&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423623&type= 1.52400005 2.79399991 为已知量。因此得到平面 <math id="M56"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 与法兰平面 <math id="M57"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 之间的距离 <math id="M58"><mi>s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423683&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423681&type= 1.26999998 2.79399991 为： <math display="block" id="M59"><mi>s</mi><mo>=</mo><mi>r</mi><mo>-</mo><mi>d</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423641&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423639&type= 14.64733315 2.79399991 . 根据法线 <math id="M60"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423667&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423665&type= 2.37066650 3.72533321 与平面 <math id="M61"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 的几何关系可知，法兰盘圆心 <math id="M62"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 ，即法兰末端坐标系的原点即为法线 <math id="M63"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423667&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423665&type= 2.37066650 3.72533321 与平面 <math id="M64"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 的交点。因此，根据上述求得的法线 <math id="M65"><msub><mrow><mi>l</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423667&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423665&type= 2.37066650 3.72533321 方程和圆心 <math id="M66"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 的坐标，以及平面 <math id="M67"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 与平面 <math id="M68"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423678&type= 3.13266683 3.72533321 之间的距离 <math id="M69"><mi>s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423683&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423681&type= 1.26999998 2.79399991 ，可以得到法兰坐标系原点 <math id="M70"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 的位置为： <math display="block" id="M71"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>O</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>=</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>±</mo><mfrac><mrow><mi>s</mi><mo>⋅</mo><mi>A</mi></mrow><mrow><mi>A</mi><mo>+</mo><mi>B</mi><mo>+</mo><mi>C</mi></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mrow><mi>O</mi></mrow><mrow><mi>y</mi></mrow></msub><mo>=</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>±</mo><mfrac><mrow><mi>s</mi><mo>⋅</mo><mi>B</mi></mrow><mrow><mi>A</mi><mo>+</mo><mi>B</mi><mo>+</mo><mi>C</mi></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mrow><mi>O</mi></mrow><mrow><mi>z</mi></mrow></msub><mo>=</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub><mo>±</mo><mfrac><mrow><mi>s</mi><mo>⋅</mo><mi>C</mi></mrow><mrow><mi>A</mi><mo>+</mo><mi>B</mi><mo>+</mo><mi>C</mi></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423690&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423689&type= 37.16867065 28.19400024 . 之后根据激光跟踪仪与法兰盘实际的相对位置关系，得到法兰盘中心 <math id="M72"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 的唯一解。 将靶球固定在小孔7位置处，激光跟踪仪测得靶球球心的坐标 <math id="M73"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">07</mn></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423721&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423719&type= 21.59000015 4.74133301 。由于圆心 <math id="M74"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 与靶球球心 <math id="M75"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">07</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423704&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423703&type= 5.07999992 3.72533321 连线的方向与法兰坐标系的 <math id="M76"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴方向平行，因此得到法兰坐标系 <math id="M77"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴的单位方向向量为： <math display="block" id="M78"><mi mathvariant="bold-italic">X</mi><mo>=</mo><mfrac><mrow><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">07</mn></mrow></msub></mrow><mrow><mfenced open="|" close="|" separators="|"><mrow><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">07</mn></mrow></msub></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423714&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423713&type= 20.65866661 9.65200043 . 由于小孔7位于小孔3和小孔4与圆心 <math id="M79"><mi>O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423717&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423715&type= 2.96333337 2.87866688 的角平分线上，因此根据靶球在小孔3和小孔4处的位置也可以计算得到靶球在小孔7处的位置 <math id="M80"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">07</mn></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">7</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423721&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423719&type= 21.59000015 4.74133301 ，进而计算法兰坐标系的 <math id="M81"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴方向向量。根据 公式（5） 公式（5） 和 公式（10） 公式（10） 得到的法兰坐标系 <math id="M82"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423827&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423825&type= 2.37066650 2.79399991 轴和 <math id="M83"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴的单位方向向量，通过右手定则得到法兰坐标系 <math id="M84"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423845&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423842&type= 2.87866688 2.79399991 轴的单位方向向量为： <math display="block" id="M85"><mi mathvariant="bold-italic">Y</mi><mo>=</mo><mi mathvariant="bold-italic">X</mi><mo>×</mo><mi mathvariant="bold-italic">Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423739&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423737&type= 18.11866570 2.79399991 . 对上述得到的法兰坐标系 <math id="M86"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴、 <math id="M87"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423845&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423842&type= 2.87866688 2.79399991 轴和 <math id="M88"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423827&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423825&type= 2.37066650 2.79399991 轴的单位方向向量进行施密特正交化，得到法兰末端坐标系的三轴单位向量（激光跟踪仪坐标系下）为： <math display="block" id="M89"><mtable columnalign="left"><mtr><mtd><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo>=</mo><mi mathvariant="bold-italic">X</mi></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi><mo>=</mo><mi mathvariant="bold-italic">Y</mi><mo>-</mo><mfrac><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">Y</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo stretchy="false">]</mo></mrow><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi><mo stretchy="false">]</mo></mrow></mfrac><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">z</mi><mo>=</mo><mi mathvariant="bold-italic">Z</mi><mo>-</mo><mfrac><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">Z</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo stretchy="false">]</mo></mrow><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo stretchy="false">]</mo></mrow></mfrac><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">x</mi><mo>-</mo><mfrac><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">Z</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi><mo stretchy="false">]</mo></mrow><mrow><mo stretchy="false">[</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi><mo>,</mo><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi><mo stretchy="false">]</mo></mrow></mfrac><mi mathvariant="bold-italic">n</mi><mi mathvariant="bold-italic">y</mi></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423753&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423751&type= 59.94400024 23.70666695 . 保持机器人不动，在机器人法兰末端安装工具，将靶球固定在工具合适位置。靶球球心认为是TCP，即工具坐标系的原点，激光跟踪仪测量得到TCP的位置。根据 公式（13） 公式（13） 得到TCP在法兰末端坐标系下的位置。 <math display="block" id="M90"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><mi>P</mi><mo>=</mo><mo stretchy="false">[</mo><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><mi mathvariant="bold-italic">x</mi></mrow></msub></mtd><mtd><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><mi mathvariant="bold-italic">y</mi></mrow></msub></mtd><mtd><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><mi mathvariant="bold-italic">z</mi></mrow></msub></mtd></mtr></mtable><mo stretchy="false">]</mo><mo>⋅</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mo>+</mo><mi mathvariant="bold-italic">O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423756&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423755&type= 44.95800018 3.64066648 ， 公式（13） 公式（13） 中， <math id="M91"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423780&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423778&type= 4.14866638 4.48733330 ， <math id="M92"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423891&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423889&type= 4.14866638 4.48733330 和 <math id="M93"><mi mathvariant="bold-italic">O</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423768&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423766&type= 2.96333337 2.79399991 均为3×1向量， <math id="M94"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423780&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423778&type= 4.14866638 4.48733330 为待求量。其中 <math id="M95"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423891&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423889&type= 4.14866638 4.48733330 表示TCP在跟踪仪坐标系下的坐标， <math id="M96"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423780&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423778&type= 4.14866638 4.48733330 表示TCP在法兰末端坐标系下的坐标。由此，工具坐标系位置标定完成。同时，可以发现当利用激光跟踪仪得到机器人法兰末端的位置和姿态后，机器人末端任意更换新的工具时，只需要再测量1个点，即靶球在新工具上的位置，利用 公式（13） 公式（13） 就能实现新工具的TCP标定。 2.3　姿态标定 2 根据法兰末端的位置和姿态和 公式（13） 公式（13） ，可以得到机器人在任意位置处TCP在基坐标系下的位置为： <math display="block" id="M97"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><mi>P</mi><mo>=</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">R</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">B</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mo>⋅</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>E</mi></mrow><mrow><mi>T</mi></mrow></mmultiscripts><mo>+</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>B</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423785&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423782&type= 30.31066895 4.57200003 . 其中， <math id="M98"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">B</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423789&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423787&type= 4.57200003 4.48733330 和 <math id="M99"><mmultiscripts><mrow/><mprescripts/><mrow><mi>E</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423792&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423791&type= 4.14866638 4.48733330 为已知量，可以由机器人得到。 控制机器人运动到新的位置处，调整机器人的末端姿态使工具坐标系与机器人基坐标系保持平行，记录此时法兰末端的姿态 <math id="M100"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423797&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423794&type= 4.57200003 4.48733330 ，同时根据 公式（15） 公式（15） 计算得到此时TCP的位置 <math id="M101"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423802&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423800&type= 5.33400011 4.48733330 ；保持工具的姿态不变，使机器人沿工具坐标系的 <math id="M102"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴运动一段距离，计算得到此时TCP的位置 <math id="M103"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423810&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423807&type= 5.33400011 4.48733330 ；保持工具的姿态不变，使机器人沿工具坐标系的 <math id="M104"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423827&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423825&type= 2.37066650 2.79399991 轴方向运动一段距离，计算得到此时TCP的位置 <math id="M105"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423819&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423816&type= 5.33400011 4.48733330 。 由于工具坐标系的坐标轴方向与基坐标的坐标轴方向平行，故工具坐标系的 <math id="M106"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423823&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423821&type= 2.62466669 2.79399991 轴、 <math id="M107"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423827&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423825&type= 2.37066650 2.79399991 轴方向向量分别为： <math display="block" id="M108"><mi mathvariant="bold-italic">P</mi><mi mathvariant="bold-italic">X</mi><mo>=</mo><mfrac><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mfenced open="|" close="|" separators="|"><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423833&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423830&type= 28.19400024 9.90600014 ， <math display="block" id="M109"><mi mathvariant="bold-italic">P</mi><mi mathvariant="bold-italic">Z</mi><mo>=</mo><mfrac><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mfenced open="|" close="|" separators="|"><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423839&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423836&type= 29.04066467 9.90600014 ， 根据右手原则确定工具坐标系的 <math id="M110"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423845&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423842&type= 2.87866688 2.79399991 轴的单位方向向量为： <math display="block" id="M111"><mi mathvariant="bold-italic">P</mi><mi mathvariant="bold-italic">Y</mi><mo>=</mo><mi mathvariant="bold-italic">P</mi><mi mathvariant="bold-italic">X</mi><mo>×</mo><mi mathvariant="bold-italic">P</mi><mi mathvariant="bold-italic">Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423851&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423848&type= 25.14599991 2.79399991 . 根据 公式（12） 公式（12） 对工具坐标系三轴方向向量进行施密特正交化得到工具坐标系的三轴正交单位向量 <math id="M112"><mi>p</mi><mi>x</mi><mo>,</mo><mi>p</mi><mi>y</mi><mo>,</mo><mi>p</mi><mi>z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423860&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423853&type= 14.47799969 3.72533321 。工具坐标系到基坐标系的姿态变换矩阵为： <math display="block" id="M113"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">R</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi mathvariant="bold-italic">p</mi><mi mathvariant="bold-italic">x</mi></mtd><mtd><mi mathvariant="bold-italic">p</mi><mi mathvariant="bold-italic">y</mi></mtd><mtd><mi mathvariant="bold-italic">p</mi><mi mathvariant="bold-italic">z</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423866&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423863&type= 30.98800087 4.82600021 . 得到工具坐标系到法兰末端坐标系的姿态变换矩阵为： <math display="block" id="M114"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">R</mi><mo>=</mo><mo stretchy="false">(</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">R</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup><mo>×</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">R</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423874&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423871&type= 28.61733437 5.16466665 . 其中 <math id="M115"><mmultiscripts><mrow/><mprescripts/><mrow><mi>E</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><mi>R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423882&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423878&type= 4.40266657 4.48733330 为机器人法兰末端的姿态（相对于基坐标系）。 结合 公式（13） 公式（13） 得到工具坐标系到法兰末端坐标系的位姿变换矩阵为： <math display="block" id="M116"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">T</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">R</mi></mtd><mtd><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mi>P</mi></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423887&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423884&type= 24.55333328 8.55133343 . 3 距离约束法标定原理 1 空间中任意一点在不同坐标系下的位置坐标可能不同，但空间中任意两点在不同坐标系下的距离是保持不变的 ［ 23 23 ］ （假设坐标系间的尺度因子为1）。通过测量空间中多个点在基坐标系和激光跟踪仪坐标系下的位置，根据空间两点距离不变这一约束条件建立方程组，通过最小二乘求解可以得到TCP在法兰末端坐标系下的位置；根据2.2中姿态标定的方法进行工具坐标系姿态标定。 将靶球固定到机器人末端，操纵机器人运动到空间多个位置处（各点尽可能在空间分布均匀且各点间的距离尽可能大）。激光跟踪仪测量靶球在各个位置处的坐标 <math id="M117"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423891&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423889&type= 4.14866638 4.48733330 ，同时通过机器人得到法兰末端在各个位置处的位姿矩阵 <math id="M118"><mmultiscripts><mrow/><mprescripts/><mrow><mi>E</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><mi>T</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423896&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423894&type= 3.89466691 4.48733330 。假设第 <math id="M119"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423975&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423973&type= 1.10066664 2.79399991 个位置处，靶球在基坐标系下和跟踪仪坐标系下的位置分别为 <math id="M120"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423905&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423902&type= 4.91066647 4.48733330 和 <math id="M121"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423909&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423907&type= 4.91066647 4.48733330 。以靶球到达的第1、 <math id="M122"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423975&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423973&type= 1.10066664 2.79399991 两个位置为例，根据两点在不同坐标系下的距离不变原则，建立约束方程为： <math display="block" id="M123"><mfenced open="‖" close="‖" separators="|"><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow></mfenced><mo>=</mo><mfenced open="‖" close="‖" separators="|"><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423918&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423915&type= 43.85733414 4.91066647 . 靶球球心在第 <math id="M124"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423975&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423973&type= 1.10066664 2.79399991 个位置处的坐标（基坐标系下）又可表示为： <math display="block" id="M125"><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>B</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>=</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">T</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">i</mi></mrow></msub><mo>⋅</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423926&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423924&type= 22.01333237 4.48733330 . 结合 公式（21） 公式（21） 得到： <math display="block" id="M126"><mfenced open="‖" close="‖" separators="|"><mrow><mo stretchy="false">(</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">T</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">T</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo><mo>⋅</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts></mrow></mfenced><mo>=</mo><mfenced open="‖" close="‖" separators="|"><mrow><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423930&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423928&type= 52.32400131 4.91066647 . 令 <math display="block" id="M127"><mtable columnalign="left"><mtr><mtd><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi mathvariant="bold-italic">T</mi></mrow><mprescripts/><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="bold-italic">B</mi></mrow></mmultiscripts><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">11</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">12</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">13</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">14</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">21</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">22</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">23</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">24</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">31</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">32</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">33</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">34</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced><mo>,</mo></mtd></mtr><mtr><mtd><mmultiscripts><mrow/><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>E</mi></mrow></mmultiscripts><mi>P</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>x</mi></mtd></mtr><mtr><mtd><mi>y</mi></mtd></mtr><mtr><mtd><mi>z</mi></mtd></mtr><mtr><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced><mo>,</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><mmultiscripts><mrow><mi>P</mi></mrow><mprescripts/><mrow><mi>T</mi></mrow><mrow><mi>L</mi></mrow></mmultiscripts><msub><mrow><mi>P</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>x</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>y</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>z</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced><mo>.</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423936&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423933&type= 52.91666412 39.70866776 由 公式（24） 公式（24） 可以得到： <math display="block" id="M128"><mfenced open="‖" close="‖" separators="|"><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">11</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">12</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">13</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">14</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">21</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">22</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">23</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">24</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">31</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">32</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">33</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd><mtd><msubsup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">34</mn></mrow><mrow><mi>i</mi></mrow></msubsup></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced><mo>⋅</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>x</mi></mtd></mtr><mtr><mtd><mi>y</mi></mtd></mtr><mtr><mtd><mi>z</mi></mtd></mtr><mtr><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced></mrow></mfenced><mo>=</mo><mfenced open="‖" close="‖" separators="|"><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>x</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>y</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>P</mi></mrow><mrow><mi>z</mi><mi>i</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423940&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423938&type= 54.35599899 20.06599998 ， 展开得到： <math display="block" id="M129"><mtable columnalign="left"><mtr><mtd><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>x</mi><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>y</mi><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>z</mi><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow><mo>-</mo></mtd></mtr><mtr><mtd><mo stretchy="false">(</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>x</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>y</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>z</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo stretchy="false">)</mo><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423945&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423943&type= 46.14333344 15.15533447 . 对于非线性方程 <math id="M130"><mi>f</mi><mo stretchy="false">(</mo><mi>x</mi><mo>,</mo><mi>y</mi><mo>,</mo><mi>z</mi><mo stretchy="false">)</mo><mo>=</mo><mn mathvariant="normal">0</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423951&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423947&type= 21.33600044 3.80999994 ，通过一阶泰勒级数展开，忽略高阶项整理得到： <math display="block" id="M131"><mtable><mtr><mtd><mn mathvariant="normal">2</mn><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">)</mo></mrow></mtd></mtr><mtr><mtd><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>x</mi><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>y</mi><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mi>z</mi><mo stretchy="false">)</mo><mo>+</mo><mi>m</mi><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423956&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423953&type= 46.22800064 15.40933418 ， 其中， <math display="block" id="M132"><mtable><mtr><mtd><mi>m</mi><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>-</mo></mrow></mtd></mtr><mtr><mtd><mo stretchy="false">(</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>x</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>y</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>P</mi></mrow><mrow><mi>z</mi><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo stretchy="false">)</mo><mo>-</mo><mn mathvariant="normal">2</mn><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">)</mo></mrow></mtd></mtr><mtr><mtd><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo stretchy="false">)</mo><mo>.</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423958&type= 75.77667236 26.07733345 上式经化简得到： <math display="block" id="M133"><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi><mn mathvariant="normal">3</mn></mrow></msub></mtd></mtr></mtable></mrow></mfenced><mo>⋅</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>x</mi></mtd></mtr><mtr><mtd><mi>y</mi></mtd></mtr><mtr><mtd><mi>z</mi></mtd></mtr></mtable></mrow></mfenced><mo>=</mo><mo>-</mo><mi>m</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423964&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423962&type= 40.38600159 14.39333248 ， 其中， <math display="block" id="M134"><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi>a</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo>=</mo><mn mathvariant="normal">2</mn><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mrow><mi>a</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo>=</mo><mn mathvariant="normal">2</mn><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mrow><mi>a</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo>=</mo><mn mathvariant="normal">2</mn><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">3</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">1</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">2</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">3</mn></mrow><mrow><mi>i</mi></mrow></msubsup><msub><mrow><mi>z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>+</mo><msubsup><mrow><mi>t</mi></mrow><mrow><mi>j</mi><mn mathvariant="normal">4</mn></mrow><mrow><mi>i</mi></mrow></msubsup><mo stretchy="false">)</mo></mrow></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423970&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423967&type= 57.82733154 30.14133072 . 机器人在空间运动 <math id="M135"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423975&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423973&type= 1.10066664 2.79399991 个位置时，可以得到 <math id="M136"><mi>i</mi><mo stretchy="false">(</mo><mi>i</mi><mo>-</mo><mn mathvariant="normal">1</mn><mo stretchy="false">)</mo><mo>/</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423985&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423982&type= 16.51000023 3.72533321 条边的距离。根据 公式（21） 公式（21） 可以建立 <math id="M137"><mi>i</mi><mo stretchy="false">(</mo><mi>i</mi><mo>-</mo><mn mathvariant="normal">1</mn><mo stretchy="false">)</mo><mo>/</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423985&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423982&type= 16.51000023 3.72533321 个方程。为了使方程组有唯一解，机器人至少需要运动3个位置。为减少随机误差的影响，通常机器人的位置数要远大于3。根据建立的方程组，通过最小二乘迭代解算未知数 <math id="M138"><mo stretchy="false">(</mo><mi>x</mi><mo>,</mo><mi>y</mi><mo>,</mo><mi>z</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423993&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423990&type= 12.53066730 3.80999994 ，得到TCP在法兰末端坐标下的位置。由此基于距离约束的工具坐标系位置标定完成。 3.2　姿态标定 2 姿态标定的方法与2.3中的方法相同，控制机器人运动到新的位置处，调整机器人的末端姿态使工具坐标系与机器人基坐标系保持平行，使机器人沿着工具坐标系的 X 轴和 Z 轴方向分别运动进行法兰末端的姿态标定。 4 实验与结果分析 1 4.1　几何法实验 2 本文中所使用的激光跟踪仪型号为Leica AT901-B，其在80 m范围内的测量精度为 <math id="M139"><mo>±</mo><mo stretchy="false">(</mo><mn mathvariant="normal">15</mn><mtext> </mtext><mi mathvariant="normal">μ</mi><mi mathvariant="normal">m</mi><mo>+</mo><mn mathvariant="normal">6</mn><mtext> </mtext><mi mathvariant="normal">μ</mi><mi mathvariant="normal">m</mi><mo>/</mo><mi mathvariant="normal">m</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423999&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17423995&type= 32.51200104 3.89466691 ，激光跟踪仪实物如 图6 图6 所示。与激光跟踪仪Leica AT901-B配套的靶球型号为CCR1.5角偶棱镜，其半径为19.05 mm。本文中所使用的工业机器人为6自由度工业机器人ABB IRB2600，其负载为20 kg，工作范围（半径）1.65 m。 实验过程中，为减少随机误差对结果的影响，对每个位置处的点测量三次取平均值。首先将靶球依次放置在法兰盘小孔1，3，5三个位置处，利用激光跟踪仪测量靶球在每个位置处的坐标。计算得到平面 <math id="M140"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 方程和圆心 <math id="M141"><msub><mrow><mi>O</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424023&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424020&type= 4.06400013 3.72533321 的坐标，进而得到法兰坐标系的 Z 轴方向向量。其次，将靶球固定在法兰盘平面任意位置，激光跟踪仪测量该点的位置，计算得到平面 <math id="M142"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424025&type= 3.13266683 3.72533321 和平面 <math id="M143"><msub><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424033&type= 3.13266683 3.72533321 的距离。之后，将靶球固定在第七个小孔处，利用激光跟踪仪测量靶球的位置，通过计算得到法兰末端坐标系在激光跟踪仪坐标系下的位姿（位置和姿态）。 给机器人ABB IRB2600末端安装工具，本文中机器人末端安装的工具如 图7 图7 所示，该工具根据法兰盘的结构设计制作，可直接安装在机器人末端，工具末端凹槽由磁铁制成用于吸附跟踪仪靶球。将靶球固定到机器人末端工具上，利用激光跟踪仪测量靶球球心的位置（即TCP）。计算得到TCP在法兰末端坐标系下的位置为 <math id="M144"><msup><mrow><mo stretchy="false">(</mo><mn mathvariant="normal">0.025</mn><mtext> </mtext><mn mathvariant="normal">6</mn><mo>,</mo><mo>-</mo><mn mathvariant="normal">0.143</mn><mtext> </mtext><mn mathvariant="normal">8,39.312</mn><mtext> </mtext><mn mathvariant="normal">4</mn><mo stretchy="false">)</mo></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424049&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424043&type= 46.90533447 3.80999994 ，工具坐标系的位置标定完成。TCP位置标定过程中激光跟踪仪测量的靶球位置数据如 表1 表1 所示。 点号 X Y Z 1 5 495.480 69 352.015 92 46.091 631 2 5 495.478 925 352.020 73 46.094 796 3 5 495.479 443 352.020 13 46.099 543 4 5 500.231 14 352.217 08 89.129 658 5 5 500.228 155 352.229 1 89.118 457 6 5 500.230 898 352.222 82 89.123 305 7 5 496.839 924 389.552 72 67.531 169 8 5 496.841 443 389.562 34 67.533 947 9 5 496.841 611 389.577 23 67.531 293 10 5 494.784 693 364.457 15 42.689 996 11 5 494.782 808 364.470 86 42.694 551 12 5 494.783 39 364.458 84 42.684 025 13 5 494.589 423 358.874 06 42.512 417 14 5 494.574 15 358.957 56 42.398 413 15 5 494.564 492 359.063 52 42.350 2 16 5 477.059 104 364.016 59 69.678 174 17 5 477.058 817 364.019 49 69.680 318 18 5 477.059 478 364.011 2 69.683 699 mm 控制机器人运动到新的位置处，调整机器人的姿态使工具坐标系的三轴方向和基坐标系的三轴平行，之后控制机器人沿工具坐标系的 X 轴和 Z 轴方向各运动1个位置，计算得到工具坐标系到法兰末端坐标系的旋转矩阵。结合求得的TCP的位置得到工具坐标系到法兰末端坐标系的转换矩阵为： <math display="block" id="M145"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">T</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mn mathvariant="normal">1</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.000</mn><mtext> </mtext><mn mathvariant="normal">175</mn></mtd><mtd><mn mathvariant="normal">0.025</mn><mtext> </mtext><mn mathvariant="normal">6</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0.999</mn><mtext> </mtext><mn mathvariant="normal">999</mn></mtd><mtd><mn mathvariant="normal">0.001</mn><mtext> </mtext><mn mathvariant="normal">396</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.143</mn><mtext> </mtext><mn mathvariant="normal">8</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0.000</mn><mtext> </mtext><mn mathvariant="normal">175</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.001</mn><mtext> </mtext><mn mathvariant="normal">396</mn></mtd><mtd><mn mathvariant="normal">0.999</mn><mtext> </mtext><mn mathvariant="normal">999</mn></mtd><mtd><mn mathvariant="normal">39.312</mn><mtext> </mtext><mn mathvariant="normal">4</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424098&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424094&type= 89.66200256 18.54199982 . 4.2　距离约束法实验 2 将跟踪仪靶球固定在机器人ABB IRB2600末端，靶球的球心即为TCP。控制机器人运动到空间任意25个位置处。利用激光跟踪仪测量靶球在25个位置处的坐标，同时记录机器人法兰末端在25个位置处的位置和姿态，距离约束法的工具坐标系标定实验如 图9 图9 所示。标定过程中记录的数据如 表2 表2 所示，其中靶球位置数据由激光跟踪仪测量得到，法兰末端的位姿数据（基坐标系下）由机器人直接得到，法兰末端的姿态用四元数Q1，Q2，Q3和Q4表示。根据实验数据计算得到TCP在基坐标系下的位置为 <math id="M146"><msup><mrow><mo stretchy="false">(</mo><mo>-</mo><mn mathvariant="normal">0.114</mn><mtext> </mtext><mn mathvariant="normal">6</mn><mo>,</mo><mo>-</mo><mn mathvariant="normal">0.209</mn><mtext> </mtext><mn mathvariant="normal">9,39.993</mn><mtext> </mtext><mn mathvariant="normal">2</mn><mo stretchy="false">)</mo></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424107&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424103&type= 50.46133423 3.80999994 。之后根据2.3中的姿态标定方法控制机器人沿工具坐标系的 X 轴和 Z 轴方向分别运动，计算工具坐标系在法兰末端坐标系下的变换矩阵。最后得到工具坐标系到法兰末端坐标系下的转换矩阵为： <math display="block" id="M147"><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="bold-italic">E</mi></mrow></mmultiscripts><mi mathvariant="bold-italic">T</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mn mathvariant="normal">0.999</mn><mtext> </mtext><mn mathvariant="normal">999</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.000</mn><mtext> </mtext><mn mathvariant="normal">006</mn></mtd><mtd><mn mathvariant="normal">0.001</mn><mtext> </mtext><mn mathvariant="normal">571</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.112</mn><mtext> </mtext><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0.999</mn><mtext> </mtext><mn mathvariant="normal">994</mn></mtd><mtd><mn mathvariant="normal">0.003</mn><mtext> </mtext><mn mathvariant="normal">491</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.210</mn><mtext> </mtext><mn mathvariant="normal">6</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0.001</mn><mtext> </mtext><mn mathvariant="normal">571</mn></mtd><mtd><mo>-</mo><mn mathvariant="normal">0.003</mn><mtext> </mtext><mn mathvariant="normal">491</mn></mtd><mtd><mn mathvariant="normal">0.999</mn><mtext> </mtext><mn mathvariant="normal">993</mn></mtd><mtd><mn mathvariant="normal">39.940</mn><mtext> </mtext><mn mathvariant="normal">8</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424119&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=17424115&type= 85.85199738 18.54199982 . 2 点号 3 靶球位置 7 法兰末端位姿参数 X Y Z X Y Z Q1 Q2 Q3 Q4 1 5 307.591 027 89.645 148 142.607 807 961.68 283.75 1 273.16 0.489 21 0.676 82 0.355 742 0.419 567 2 5 165.741 329 -5.850 498 154.785 814 1 114.48 354.73 982.4 0.337 674 -0.583 9 0.507 177 -0.536 48 3 5 245.947 467 91.608 817 71.440 088 1 025.19 268.34 1 211.79 0.510 494 -0.315 55 0.758 925 -0.252 7 4 5 092.241 958 1 164.812 38 -417.501 265 1 168.79 -805.27 734.53 0.526 258 0.181 26 0.830 732 0.009 064 5 5 820.248 52 -280.241 81 -205.477 761 472.55 655.68 914.24 0.330 18 -0.630 01 0.540 635 -0.449 2 6 4 760.423 785 -86.987 004 -548.915 706 1 535.7 435.47 594.85 0.557 053 -0.351 57 0.594 83 -0.460 72 7 4 907.645 048 1 214.935 977 -492.431 368 1 354.99 -856.96 666.28 0.504 299 -0.036 94 0.843 3 -0.182 11 8 5 632.157 365 368.641 051 124.563 164 628.49 0 1 244.05 0.637 328 7.26E-09 0.770 592 5.25E-09 9 5 785.431 665 1 028.737 008 -168.243 452 471.79 -647.07 956.32 0.438 642 0.651 322 0.539 378 0.304 047 10 5 760.301 951 780.354 021 -26.188 164 496.62 -396.89 1 090.29 0.566 03 0.535 403 0.567 327 0.266 635 11 5 591.586 334 1 434.201 98 -299.765 688 655.58 -1 063.77 828.66 0.322 051 0.715 846 0.381 477 0.488 184 12 5 506.819 907 273.115 289 -111.747 281 765.51 87.86 1 015.34 0.599 333 -0.220 19 0.761 232 -0.113 32 13 5 611.973 129 284.052 424 -166.479 046 662.58 78.37 957.2 0.599 387 0.014 557 0.795 59 0.086 947 14 4 866.061 069 527.168 983 -225.707 829 1 401.57 -173.5 910.64 0.370 599 0.621 869 0.205 475 0.658 57 15 5 022.972 202 900.218 457 -104.689 984 1 229 -547.71 1 031.97 0.621 731 0.348 212 0.619 089 0.330 042 16 5 299.830 441 272.157 991 -61.779 153 969.66 91.93 1 055.96 0.624 29 0.443 601 0.463 69 0.445 502 17 5 260.332 659 -69.003 496 -60.088 329 1 017.47 427.28 1 065.78 0.448 968 0.559 719 0.431 124 0.547 06 18 5 558.325 983 -99.202 255 203.588 703 711.41 460.79 1 318.71 0.385 555 -0.639 91 0.320 444 -0.582 39 19 5 161.414 307 -433.065 66 -468.402 377 1 143.09 778.77 670.11 0.070 815 -0.776 86 -0.062 32 -0.622 57 20 5 414.413 039 1 095.611 28 218.101 662 821.18 -734.95 1 343.2 0.639 855 0.176 708 0.719 099 0.205 565 21 5 400.293 223 1 108.300 183 103.140 893 839.17 -749.85 1 222.54 0.333 874 -0.638 72 0.245 95 -0.648 13 22 4 890.708 186 475.831 285 -84.937 31 1 373.3 -132.33 1 064.42 0.614 775 0.057 265 0.779 246 0.107 463 23 5 364.035 815 609.575 233 -73.292 637 897.5 -236.82 1 049.92 0.301 627 0.708 366 0.095 354 0.630 989 24 5 737.847 625 160.247 398 -12.176 936 538.22 226.84 1 107.79 0.559 408 0.095 289 0.802 349 -0.184 98 25 5 460.132 317 705.674 786 -84.098 064 805.03 -322.31 1 046.89 0.018 447 -0.668 63 0.452 401 -0.589 85 mm 4.3　精度对比 2 为对比两种工具坐标系标定方法的标定精度，控制机器人运动到空间任意20个位置处，根据两种方法得到的工具坐标系标定结果分别计算靶球球心在20个位置处的坐标（基坐标系下），并将其作为测量值。激光跟踪仪测得的靶球在20个位置处的坐标作为真实值，通过公共点转换得到两种工具坐标系标定方法的误差。 图10 图10 表示两种标定方法下机器人在空间任意20个位置处的误差。 表3 表3 表示两种标定方法下机器人在基坐标系三个轴向上的均方根误差。 2 标定方法 4 RMS X 轴 Y 轴 Z 轴 位置 几何法 0.459 0.369 0.362 0.692 距离约束法 0.435 0.430 0.321 0.691 mm 从图中可以看出，两种方法的精度转换误差都在1.2 mm以内。靶球在不同位置处的误差有所不同但相差不大，符合机器人在不同位置时的定位误差不同的特点。同时，两种标定方法在任意位置处的误差相差不大，说明两种方法的标定精度相当。 从 表3 表3 中可以看出：几何法和距离约束法的标定误差在三轴方向上有所不同，但是位置均方根误差基本相同。再次验证了两种方法的标定精度基本一致。 经过分析发现，本文提出的几何法工具坐标系标定误差来源主要有以下两个方面：一是法兰盘定位孔径本身存在一定的误差，6个定位孔的大小并不能保证完全地相同，同时6个未定位孔的位置并不能保证完全对称，该误差主要是由于生产加工机器人过程中产生的系统误差引起的；另一方面是利用多个点进行圆心拟合以及平面方程拟合时会存在一定的误差，导致计算得到的法兰盘中心和实际的法兰盘中心存在一定的偏差，从而导致工具坐标系标定结果存在一定偏差。通过结果发现，几何法与距离约束法的标定结果基本相同，原因可能在于激光跟踪仪本身的测量精度较高，因此工具坐标系标定的误差主要是由于工业机器人本身所引起的，因此两种方法得到的工具坐标系标定精度基本相同。 5 结　论 1 本文通过分析工业机器人的法兰盘结构，基于法兰盘上各点的几何关系，提出一种基于几何法原理机器人工具坐标系快速标定方法。该方法不需要控制运动，只需要利用激光跟踪仪测量机器人末端的6个点，就能实现工具坐标系的位置标定，标定过程简单且不受机器人运动范围的限制。之后，任意更换机器人法兰末端的工具只需加测一点就可以实现工具坐标系的位置标定。而传统的标定方法在更换机器人末端工具后，要重新进行工具坐标系标定。本文提出的标定方法标定效率大大提高，在实际机器人工具坐标系标定中更具适用性。同时，和基于距离约束的工具坐标系标定方法进行精度对比分析发现二者的精度基本一致。实验结果证明：基于几何法的工具坐标系标定后机器人的定位均方根误差为0.692 mm，说明该方法具有较高的精度，能够满足机器人高精度作业任务的要求。 参考文献 武燕 ， 陈静雯 . 中国工业机器人产业发展对策研究 ［J］. 对外经贸 ， 2020 ，（ 5 ）： 43 - 46 . 中国工业机器人产业发展对策研究 WU Y ， CHEN J W . Research on the development strategy of industrial robot industry in China ［J］. Foreign Economic Relations & Trade ， 2020 ，（ 5 ）： 43 - 46 . （in Chinese） Research on the development strategy of industrial robot industry in China 罗连发 ， 储梦洁 ， 刘俊俊 . 机器人的发展：中国与国际的比较 ［J］. 宏观质量研究 ， 2019 ， 7 （ 3 ）： 38 - 50 . 机器人的发展：中国与国际的比较 LUO L F ， CHU M J ， LIU J J . The development of robots： comparison between China and the world ［J］. Journal of Macro-quality Research ， 2019 ， 7 （ 3 ）： 38 - 50 . （in Chinese） The development of robots： comparison between China and the world PENG J ， DING Y ， ZHANG G ， et al . An enhanced kinematic model for calibration of robotic machining systems with parallelogram mechanisms ［J］. Robotics and Computer-Integrated Manufacturing ， 2019 ， 59 ： 92 - 103 . An enhanced kinematic model for calibration of robotic machining systems with parallelogram mechanisms JI W ， WANG L . Industrial robotic machining： a review ［J］. The International Journal of Advanced Manufacturing Technology ， 2019 ， 103 （ 1 ）： 1239 - 1255 . Industrial robotic machining： a review WU J ， ZHANG D ， LIU J ， et al . A moment approach to positioning accuracy reliability analysis for industrial robots ［J］. IEEE Transactions on Reliability ， 2019 ， 69 （ 2 ）： 699 - 714 . A moment approach to positioning accuracy reliability analysis for industrial robots 杜亮 ， 张铁 ， 戴孝亮 . 激光跟踪仪测量距离误差的机器人运动学参数补偿 ［J］. 红外与激光工程 ， 2015 ， 44 （ 8 ）： 2351 - 2357 . 激光跟踪仪测量距离误差的机器人运动学参数补偿 DU L ， ZHANG T ， DAI X L . Robot kinematic parameters compensation by measuring distance error using laser tracker system ［J］. Infrared and Laser Engineering ， 2015 ， 44 （ 8 ）： 2351 - 2357 . （in Chinese） Robot kinematic parameters compensation by measuring distance error using laser tracker system 温秀兰 ， 康传帅 ， 宋爱国 ， 等 . 基于全位姿测量优化的机器人精度研究 ［J］. 仪器仪表学报 ， 2019 ， 40 （ 7 ）： 81 - 89 . 基于全位姿测量优化的机器人精度研究 WEN X L ， KANG C S ， SONG A G ， et al . Study on robot accuracy based on full pose measurement and optimization ［J］. Chinese Journal of Scientific Instrument ， 2019 ， 40 （ 7 ）： 81 - 89 . （in Chinese） Study on robot accuracy based on full pose measurement and optimization YIN S ， GUO Y ， REN Y ， et al . A novel TCF calibration method for robotic visual measurement system ［J］. Optik ， 2014 ， 125 （ 23 ）： 6920 - 6925 . A novel TCF calibration method for robotic visual measurement system HE R ， ZHAO Y ， YANG S ， et al . Kinematic-parameter identification for serial-robot calibration based on POE formula ［J］. IEEE Transactions on Robotics ， 2010 ， 26 （ 3 ）： 411 - 423 . Kinematic-parameter identification for serial-robot calibration based on POE formula 刘涛 ， 尹仕斌 ， 任永杰 ， 等 . 机器人工具坐标系自动校准 ［J］. 光学 精密工程 ， 2019 ， 27 （ 3 ）： 661 - 670 . 机器人工具坐标系自动校准 LIU T ， YIN S B ， REN Y J . Automatic calibration of robot tool center frame robot tool center frame ［J］. Opt. Precision Eng. ， 2019 ， 27 （ 3 ）： 661 - 670 . （in Chinese） Automatic calibration of robot tool center frame robot tool center frame 陆艺 ， 葛文琦 ， 郭斌 . 基于标准球距离约束的工业机器人参数标定 ［J］. 计量学报 ， 2020 ， 41 （ 9 ）： 1048 - 1054 . 基于标准球距离约束的工业机器人参数标定 LU Y ， GE W Q ， GUO B . Calibration of industrial robot parameters based on standard ball distance constraints ［J］. Acta Metrologica Sinica ， 2020 ， 41 （ 9 ）： 1048 - 1054 . （in Chinese） Calibration of industrial robot parameters based on standard ball distance constraints 罗豪龙 ， 王力 ， 向奉卓 ， 等 . 基于最小二乘的工具坐标系标定方法 ［J］. 电子测量技术 ， 2020 ， 43 （ 2 ）： 6 - 9 . 基于最小二乘的工具坐标系标定方法 LUO H L ， WANG L ， XIANG F Z ， et al . Calibration method of tool coordinate system based on least squares ［J］. Electronic Measurement Technology ， 2020 ， 43 （ 2 ）： 6 - 9 . （in Chinese） Calibration method of tool coordinate system based on least squares 韩奉林 ， 江晓磊 ， 严宏志 . 基于可测距平板工具的机器人TCP标定方法 ［J］. 计算机工程与应用 ， 2016 ， 52 （ 18 ）： 18 - 23 . 基于可测距平板工具的机器人TCP标定方法 HAN F L ， JIANG X L ， YAN H Z . Robotic TCP calibration method based on plane toolbar with dis-tance measure function ［J］. Computer Engineering and Applications ， 2016 ， 52 （ 18 ）： 18 - 23 . （in Chinese） Robotic TCP calibration method based on plane toolbar with dis-tance measure function 牛雪娟 ， 刘景泰 . 基于奇异值分解的机器人工具坐标系标定 ［J］. 自动化与仪表 ， 2008 ，（ 3 ）： 1 - 4 . 基于奇异值分解的机器人工具坐标系标定 NIU X J ， LIU J T . Robot tool control frame calibration based on SVD .［J］. Automation & Instrumentation ， 2008 ，（ 3 ）： 1 - 4 . （in Chinese） Robot tool control frame calibration based on SVD 刘成业 ， 李文广 ， 马世国 ， 等 . 一种机器人工具坐标系标定方法 ［J］. 山东科学 ， 2012 ， 25 （ 1 ）： 69 - 74 . 一种机器人工具坐标系标定方法 LIU C Y ， LI W G ， MA S G ， et al . A robot tool frame calibration method ［J］. Shandong Science ， 2012 ， 25 （ 1 ）： 69 - 74 . （in Chinese） A robot tool frame calibration method 杨聚庆 ， 王大勇 ， 董登峰 ， 等 . 激光测量标定机器人坐标系位姿变换的正交化解算 ［J］. 光学 精密工程 ， 2018 ， 26 （ 8 ）： 1985 - 1993 . 激光测量标定机器人坐标系位姿变换的正交化解算 YANG J Q ， WANG D Y ， DONG D F ， et al . Laser measurement based evaluation for orthogonal transformation calibration of robot pose ［J］. Opt. Precision Eng. ， 2018 ， 26 （ 8 ）： 1985 - 1993 . （in Chinese） Laser measurement based evaluation for orthogonal transformation calibration of robot pose NUBIOLA A ， BONEV I A . Absolute calibration of an ABB IRB 1600 robot using a laser tracker ［J］. Robotics and Computer-Integrated Manufacturing ， 2013 ， 29 （ 1 ）： 236 - 245 . Absolute calibration of an ABB IRB 1600 robot using a laser tracker 周祥 . 工业机器人工具及工件坐标系的标定研究 ［D］. 南京 ： 南京理工大学 ， 2015 . 工业机器人工具及工件坐标系的标定研究 ZHOU X . Research on Calibration of Industrial Robot Tools and Workpiece Coordinate System ［D］. Nanjing ： Nanjing University of Science and Technology ， 2015 . （in Chinese） Research on Calibration of Industrial Robot Tools and Workpiece Coordinate System 孙义林 ， 樊成 ， 陈国栋 ， 等 . 基于激光跟踪仪的机器人抛光工具系统标定 ［J］. 制造业自动化 ， 2014 ，（ 24 ）： 105 - 108 . 基于激光跟踪仪的机器人抛光工具系统标定 SUN Y L ， FAN C ， CHEN G D ， et al . Tool calibration for the robotic polishing system based on the laser tracker ［J］. Manufacturing Automation ， 2014 ，（ 24 ）： 105 - 108 . （in Chinese） Tool calibration for the robotic polishing system based on the laser tracker 齐立哲 ， 陈磊 ， 王伟 ， 等 . 基于激光跟踪仪的机器人误差测量系统标定 ［J］. 制造技术与机床 ， 2012 ，（ 10 ）： 90 - 94 . 基于激光跟踪仪的机器人误差测量系统标定 QI L Z ， CHEN L ， WANG W ， et al . The calibration for industrial robot's position error measuring system based on the laser tracker ［J］. Manufacturing Technology & Machine Tool ， 2012 ，（ 10 ）： 90 - 94 . The calibration for industrial robot's position error measuring system based on the laser tracker 张博 ， 魏振忠 ， 张广军 . 机器人坐标系与激光跟踪仪坐标系的快速转换方法 ［J］. 仪器仪表学报 ， 2010 ， 31 （ 9 ）： 1986 - 1990 . 机器人坐标系与激光跟踪仪坐标系的快速转换方法 Zhang B ， WEI ZH ZH ， ZHANG G J . Rapid coordinate transformation between a robot and a laser tracker ［J］. Chinese Journal of Scientific Instrument ， 2010 ， 31 （ 9 ）： 1986 - 1990 . （in Chinese） Rapid coordinate transformation between a robot and a laser tracker 范百兴 ， 李广云 ， 周维虎 ， 等 . 激光跟踪仪空间联合平差模型及精度分析 ［J］. 武汉大学学报（信息科学版） ， 2018 ， 43 （ 1 ）： 120 - 126 . 激光跟踪仪空间联合平差模型及精度分析 FAN B X ， LI G Y ， ZHOU W H ， et al . Precision analysis of the unified spatial metrology network adjustment model ［J］. Geomatics and Information Science of Wuhan University ， 2018 ， 43 （ 1 ）： 120 - 126 . （in Chinese） Precision analysis of the unified spatial metrology network adjustment model 向民志 ， 范百兴 ， 李祥云 ， 等 . 激光跟踪仪与机器人坐标系转换方法研究 ［J］. 航空制造技术 ， 2018 ， 61 （ 1 ）： 98 - 101 . 激光跟踪仪与机器人坐标系转换方法研究 XIANG M Z ， FAN B X ， LI X Y ， et al . Study on Coordinate Transformation Between a Laser Tracker and a Robot ［J］. Aeronautical Manufacturing Technology ， 2018 ， 61 （ 1 ）： 98 - 101 . （in Chinese） Study on Coordinate Transformation Between a Laser Tracker and a Robot 10.37188/OPE.20212906.1375 TP242 A 1004-924X（2021）06-1375-12 李广云,罗豪龙,王力.机器人工具坐标系的快速标定方法[J].光学精密工程,2021,29(06):1375-1386. 李广云,罗豪龙,王力.机器人工具坐标系的快速标定方法[J].光学精密工程,2021,29(06):1375-1386. LI Guang-yun,LUO Hao-long,WANG Li.Fast calibration method of robot tool coordinate[J].Optics and Precision Engineering,2021,29(06):1375-1386. LI Guang-yun,LUO Hao-long,WANG Li.Fast calibration method of robot tool coordinate[J].Optics and Precision Engineering,2021,29(06):1375-1386. 国家重点研发计划资助项目（No.2017YFF0206001）；国家自然基金项目资助项目（No.41501491） 2020-10-21 2020-12-07 2021-06-15 2021-07-12 光学精密工程 6 29