基于奇异值分解的机器人工具坐标系标定

文章编号：１００１—９９４４（２００８）０３－０００１—０４　基于奇异值分解的机器人工具坐标系标定　牛雪娟，刘景泰　（南开大学机器人与信息自动化研究所，天津３０００７１）　

摘要：机器人通过在末端安装不同的操作工具来完成各种作业任务，而工具参数的准确度直　接影响着机器人操作精度，因此研究一种准确、快速的工具坐标系标定方法具有重要的意　义。该文提出一种称为三点五步法的机器人工具坐标系标定方法，并基于矩阵的奇异值分解　理论对该标定方法进行了严密的数学推理，证明该标定方法理论上的可行性。采用该算法进　行工具坐标系标定具有操作简单、标定精度高、可靠性强和算法易于程序实现的特点。　关键词：奇异值分解；工具坐标系：标定　中图分类号：ＴＰ２４２．６　文献标志码：Ａ　

Ｒｏｂｏｔ　Ｔｏｏｌ　Ｃｏｎｔｒｏｌ　Ｆｒａｍｅ　Ｃａｌｉｂｒａｔｉｏｎ　Ｂａｓｅｄ　ｏｎ　ＳＶＤ　ＮＩＵ　Ｘｕｅ－ｊｕａｎ．ＬＩＵ　Ｊｉｎｇ—ｔａｉ　（Ｉｎｓｔｉｔｕｔｅ　ｏｆ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ｉｎｆｏｒｍａｔｉｏｎ　Ａｕｔｏｍａｔｉｃ　Ｓｙｓｔｅｍ，Ｎａｎｋａｉ　Ｕｎｉｖｅｒｓｉｔｙ，Ｔｉａｎｊｉｎ　３０００７１，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｂｙ　ｆｉｘｉｎｇ　ｖａｒｉｏｕｓ　ｔｏｏｌｓ　ｏｎ　ｔｈｅ　ｅｎｄ—ｅｆｆｅｃｔｏｒｓ，ｒｏｂｏｔｓ　ｃａｎ　ｃｏｍｐｌｅｔｅ　ｖａｒｉｏｕｓ　ｔａｓｋｓ．Ｔｈｅ　ａｃｃｕｒａｔｅ　ｏｆ　ｔｈｅ　ｔｏｏｌ’Ｓ　ｐａｒａｍｅｔｅｒｓ　ｐｒｏｄｕｃｅ　ｇｒｅａｔ　ｅｆｆｅｃｔ　ｏｎ　ｔｈｅ　ａｃｃｕｒａｔｅ　ｏｆ　ｔｈｅ　ｒｏｂｏｔ’Ｓ　ｏｐｅｒａｔｉｏｎ．Ｉｔ　ｉｓ　ｖｅｒｙ　ｉｍｐｏｒｔａｎｔ　ｔｏ　ｓｔｕｄｙ　ａ　ｋｉｎｄ　ｏｆ　ＴＣＦ　ｃａｌｉｂｒａｔｉｏｎ　ｍｅｔｈｏｄ，ｗｈｉｃｈ　ｉｓ　ｏｆ　ｔｈｅ　ｐｒｏｐｅｒｔｙ　ｏｆ　ｈｉｇｈ　ｐｒｅｃｉｓｉｏｎ　ａｎｄ　ｑｕｉｃｋｎｅｓｓ．Ｔｈｉｓ　ｐａｐｅｒ　ｃｏｎｓｉｄｅｒｓ　ａ　ｐｒａｃｔｉｃａｌ　ｍｅｔｈｏｄ　ｆｏｒ　ＴＣＦ　ｃａｌｉｂｒａｔｉｏｎ，ｃａｌｌｅｄ　ｔｈｒｅｅ—ｐｏｉｎｔｓ　ａｎｄ　ｆｉｖｅ—ｓｔｅｐｓ　ｍｅｔｈｏｄ，ｗｈｉｃｈ　ｉｓ　ｐｒｏｖｅｄ　ｂｙ　ｔｈｅ　ａｌｇｏｒｉｔｈｍ　ｂａｓｅｄ　ｏｎ　ＳＶＤ，　ＴｈｒｏｕＩｓｈ　ｔｈｅ　ｓｔｒｉｃｔ　ｒｅａｓｏｎｉｎｇ，ｃａｌｌｅｄ　ｉｔ　ｓｈｏｗｓ　ｔｈｅ　ｆｅａｓｉｂｉｌｉｔｙ　ｏｆ　ｔｈｉｓ　ｍｅｔｈｏｄ．Ｓｉｍｐｌｅ　ｏｐｅｒａｔｉｏｎ，ｈｉｓｈ　ａｃｃｕｒａｔｅ，ｓｔｒｏｎｇ　ｒｅｌｉ—　ａｂｉｌｉｔｙ　ａｎｄ　ｅａｓｙ　ｐｒｏｇｒａｍｍｉｎｇ　ｍａｋｅ　ｔｈｅ　ｍｅｔｈｏｄ　ｖｅｒｙ　ａｐｐｌｉｅｄ．　Ｋｅｙ　ｗｏｒｄｓ：ｓｉｎｇｕｌａｒ　ｖａｌｕｅ　ｄｅｃｏｍｐｏｓｉＩｉｏｎ（ＳＶＤ）；ｔｏｏｌ　ｃｏｎｔｒｏｌ　ｆｒａｍｅ（ＴＣＦ）；ｃａｌｉｂｒａｔｉｏｎ　

随着工业的发展，机器人的应用越来越广泛。实　际应用中．机器人是通过在末端安装不同的操作工　具来完成各种作业任务ｌＩ】。工具参数的准确度直接　影响着机器人轨迹精度，所以准确、快速的标定方法　对机器人的现场应用具有重要意义。国内外都有工　具参数标定的相关研究，其中文献【１，２］提到的标定　方法只能实现位置参数标定；文献【３］提到的标定算　法虽然能实现位姿标定。但是过程要复杂得多。　对于离线编程，必须对加工单元中的非理想状　态的工件设备进行标定。没有精确的工具坐标系标　定，即便是精度极高的机器人也无法实现对轨迹误　差要求高的离线编程任务。对于在线操作，机器人的　工具坐标系的标定也很重要，它可以使得路径点的　规划更直接、更准确。对于各种机器人避碰算法，机　器人的工具坐标系标定也是必须的。　机器人上的坐标系可以分成四类：机器人基坐　

收稿日期：２００７—０４—２５：修订日期：２ｏｏ７—１０—２４　．　基金项目：国家ｃａ然科学基金资助项目（６０５７５０４８）；天津市ｃａ然科学基金项目（０７ＪＣＹＢＪＣ０５５００）　作者简介：牛雪娟（１９７８一），女，博士研究生，研究方向为机器人学、控制理论与控制工程；刘景泰（１９６４一），男，博士生导师，　研究领域为机器人学、控制理论与控制工程、自动化控制。　

ｓ动化５仪表２００８（，３）　■　

维普资讯 http://www.cqvip.com ｊｉ　标系（Ｂａｓｅ　Ｆｒａｍｅ）、末端执行器坐标系（Ｅｎｄ—ｅｆｆｅｃｔｏｒ　Ｆｒａｍｅ）、Ｔ具坐标系（Ｔｏｏｌ　Ｆｒａｍｅ）、用户坐标系（Ｕｓｅｒ　Ｆｒａｍｅ）。机器人基坐标系是一个不可设置的缺省的　坐标系，固结在机器人机座上，它是机器人运动学分　析的基本参考系。同时，机器人基坐标系还可用作通　用坐标系。作为用户坐标系的参考位置。末端执行器　坐标系。即与机器人最后一个连杆固结的坐标系，原　点位于机器人末端法兰盘中心。丁具坐标系用来定　义ＴＣＰ（ｔｏｏｌ　ｃｅｎｔｅｒ　ｐｏｉｎｔ）的位置及１二具的姿态。用　户坐标系是用户在Ｔ作空间中自定义的坐标系，是　程序中记录的所有位置的参考坐标系。本文所说的　工具坐标系标定指的是Ｔ具坐标系相对于末端坐标　系的位姿参数的确定，即坐标变换矩阵的确定。　奇异值分解ＳＶＤ广泛应用于信号的处理和图　像处理中，它作为一种通用＿厂具，在解决与视觉有关　的几何问题时常用。ＳＶＤ是一种强大的在最ｚｌ，２．乘　法意义下求解线性方程的线性代数方法，可以确定　在最小二乘法意义下线性方程最可能接近解　

１　工具坐标系标定三点五步法　１．１　三点五步法的实验步骤　该标定方法共需要五步操作，在进行标定之前．　在机器人工作空问内选择一个机器人丁具末端比较　容易接近且表面平整的位置，在其上倒放一个锥体。　该锥体要有比较尖的顶部，最好固定在某个不动体　上，这有助于提高标定精度。该标定方法中的五步操　作都是在该锥体的顶部为参照对象．标定方法如图　１所示，标定具体步骤如下　

点　步骤５　；　

步骤２　

步　

步　～兰　

图１　三点五步法标定示意图　Ｆｉｇ．１　Ｃｈａｒｔ　ｏｆ　ｔｈｒｅｅ—ＤＯｉｎｔｓ—ｆｉｖｅ—ｓｔｅｐｓ　ｃａｌｉｂｒａｔｉｏｎ　ｍｅｔｈｏｄ　第１、２、３步操纵机器人六个关节使得工具分　别从三个不同的方向指向锥体，使工具末端ＴＣＰ　（ｔｏｏｌ　ｅｅＦｌｔｅＦ　ｐｏｉｎｔ）尽量接近锥体顶部Ｐ点。调整机　器人４－６关节，使得第三个姿态下的工具系Ｚ轴与　

日　

基座坐标系Ｚ轴基本一致，工具系Ｘ轴与基座坐标　系Ｘ轴方向基本一致。分别记下在这三个位姿下，　机器人末端坐标系ｆ６｝相对于机器人基座坐标系｛０｝　

的坐标变换阵，分别记为：　，：　，：　。　第４步将机器人的示教坐标系变换为机器人　基坐标系，这里记机器人基座坐标系为ｆ０｝，操纵　机器人使机器人末端沿着ｆ０｝的Ｘ轴正向平移一段　距离（记为　），并记录当前ｆ６｝相对Ｔ｛ｏ｝的坐标变　

换阵：　。　第５步操纵机器人使机器人末端沿着ｆ０｝的Ｚ　轴正向平移一段距离（记为　），并记录当前ｆ６｝相对　

于（０｝的坐标变换阵：　。　１－２三点五步法的理论基础　工具坐标系标定就是精确确定工具系ｆ　相对　

于机器人末端固结坐标系｛６｝的坐标变换矩阵　。　

７’＝　＝　

ｌ　ｑ　１　ｑ　ｌ　ｑ　０　０　ｍ１３　Ｐ　ｍ２３　Ｐ，　ｍ３３　Ｐ：　０　１　￡　瓯￡９　￡　０　１　ｒ　ｄ　／＇ｉ，ｄ　ｒ　ｄ　

０　１　其中，　０　和０　分别代表第　步时｛６｝和｛　相对于｛０｝　的坐标变换矩阵。由｛０｝、｛６｝和｛　问的变换关系，可　得变换方程：０　６　０　，　｛１，２，３，４，５｝，即：　・　

￡　￡西　￡　０　ｌ　

“ｐ　Ｗｉｙ　ｐ　Ｗ／ｚ　ｐ　０　１　

ｍｌ３　ｑ　ｍ２３　ｑ　ｍ３３　ｑ　０　１　

Ａｕｔｏｍａｔｉｏｎ＆Ｉｎｓｔｒｕｍｅｎｔａｔｉｏｎ　２ＯＯ８（３１　

维普资讯 http://www.cqvip.com 获取工具坐标系的位置信息：由三点五步法获　取数据的方法可知１￣３步Ｐ点重合，即ｐｌ－ｐ：　，。标　记Ｐ点在｛０｝］　的坐标为（ｐ　Ｐ　ｐ　）　。由式（１），知　Ｒ，ｑ－－ｐ　；（ｉ＝１￣３），可以列写方程：　

［　Ｒ　２３—－　Ｒ　ｌ１］ｌ；　ｌ＝ｔ　１－　ｔ２，］　ｃ２　

其中：　Ｒ　ｎ　ｏｌｘ　ｎｉｔ　ｏｉｒ％　ｎ　０　０　；　＝

［　］；ｐ＝［三　］；ｇ＝［　］　

式（２）中共含有６个方程，３个未知量，为超定　方程。对于形如Ａｘ＝ｂ的超定方程，由于其不定性，　用通常的线性代数手段是无法求解的，需借助于最　小二乘方法，获取最小二乘意义下Ｐ点在｛０｝系下的　坐标ｐ：（ｐ　Ｐ　ｐ　）　，及ＴＣＰ点相对于｛６｝系的坐标ｇ＝　（ｑ　ｑ　ｇ　）　。　获取工具坐标系的姿态信息：由于第３—５步中　工具坐标系指向与基座坐标系一致，因此，沿基坐标　系平移相当于沿工具坐标系平移。对式（１）而言。可　在方程两边同时右乘平移矩阵，来描述机器人的相　对运动。由第３—５步的动作方式，可列写方程：　ｆ　．　¨ｍ　１：６　（３）Ａ　【ｘＲ　Ｒ　¨ｍ，Ｊ　

其中：　ｍＦ［ｍ“　ｍ五　ｍ　ｒ；Ａｘ＝ｔ缸一ｔ　；Ａｚ＝ｔ　一ｔ　；　６＝『１　０　０△　０　Ｔｒｏ　式（３）中共６个方程、６个变量、为恰定方程，具　有精确解，即旋转阵的第１、３列（ｍ。和ｍ，）。利用旋　转阵正交性，即ｍ：＝ｍ　ｘｍ，，可以求出旋转阵的第２　列（ｍ２）。　

２基于奇异值分解标定工具坐标系　２．１　奇异值分解的基本理论　在奇异值理论中［４１，任何ｍｘｎ阶的矩阵　（其中　ｍ＞ｎ）的奇异值分解表示为　

＝　（４）　式中：　和　分别是ｍｘｍ阶和ｎ￣ｎ阶列正交矩阵；　Ｄ＝ｄｉａｇ（ａｌ，　２，　３，…，　）是对角矩阵，其对角元素为　的奇异值，并按降序排列。　不管矩阵　是否奇异，ＳＶＤ分解都能做到。而　且它几乎是唯一的［４１。当　为ｎ￣ｎ方阵，且Ｒａｎｋ（Ａ）　

自动化ｓ仪表２ｏｏｓ（３）　

：ｎ时，所求解为准确解，否则为最小二乘解。在实际　应用中由于噪声的存在，求取　的最小二乘解是具　有很强的实用意义的。　２．２基于ＳＶＤ标定工具坐标系　对式（２），令　＝（　二　：］，对　进行ｓＶ。分解，　

如式（４），『　，Ｄ，ＶＩ＝ｓｖｄ（Ａ）；然后利用　、Ｄ和　求　取　的广义逆矩阵　十＝ＶＤｔＵｙ。其中Ｄ十：　

｛　）≠０晰利　求取方程最小二　

乘解ｑ－－Ａｔｘ｝　｝。因为Ａ为６ｘ３矩阵，所以求出的　【ｔｌ－ｔ３　Ｊ　

ｑ为工具坐标系原点，即ＴＣＰ点在机器人末端坐标　系｛６｝下坐标值的最小二乘估计值。　同理，对式（３）采用奇异值分解，可以获得工具　坐标系相对于｛６｝系的旋转矩阵。因为方程（３）系数　矩阵为满秩方阵，故所求解为工具坐标系旋转阵的　准确解。　２．３标定算法的Ｍａｔｌａｂ实现　Ｍａｔｌａｂ中有专门对矩阵进行奇异值分解的函数　『　，Ｓ，ＶＩ＝ｓｖｄ（Ｘ），该方法虽比ＬＵ分解法、ＱＲ分解　法及迭代法等耗时，但ＳＶＤ是最可靠、实用面最广　的分线性方程组求解方法。在Ｍａｔｌａｂ下实现基于　ＳＶＤ的工具坐标系标定，需要如下几个步骤：　首先将在三点五步法中获取的数据存为一个文　本文件，每一点对应于文件中的一行，每行包含描述　机器人末端坐标系｛６｝系的位姿的所有信息。　然后，利用Ｍａｔｌａｂ的程序编制功能，将文本中　的数据信息读人Ｍａｔｌａｂ，并对这些数据进行转换，转　换为旋转矩阵和平移矢量的格式。　最后，再根据式（２）和式（３），利用Ｍａｔｌａｂ的　ＳＶＤ命令求解出工具系的平移矢量和旋转矩阵。