六自由度工业机器人标定

本科生毕业设计 毕 业 设 计 题 目 工业机器人标定 学 生 姓 名 专 业 机 械 设 计 制 造 及 其 自 动 化 班 级 指 导 教 师 完 成 日 期 2 0 1 4 年 05 月 30 日 六自由度工业机器人标定 蒋涛 摘 要 运动学标定是提高机器人精度的关键技术，也是机器人学的重要内容，在机器人空前发展的今天有十分重要的理论和现实意义。机器人 运动学标定以运动学建模为基础，几何误差参数辨识为目的，为机器人的误差补偿提供依据。 现今机器人厂家生产的机器人其重复定位精度比较高，而绝对定位精度却很低。伴随着机器人越来越广泛的运用，提高机器人绝对定位精度已成为其中一关键技术问题。本文采用一种运动学标定方法，应用先进的激光跟踪测量系统和基于模型的参数辨识方法识别出一种 6R 机器人模型的准确参数，提高了该机器人的绝对定位精度。 针对工业机器人标定问题，首先结合机器人的实际机构特点，运用 D-H 方法建立了机器人的连杆坐标系，在此基础上进行了机器人 运动学正逆解和雅可比矩阵的详细推导及求解，并运用 Matlab 语言进行运动学模型的编程求解，通过与机器人控制器中位姿数据对比，验证了所建立的连杆坐标系统的正确性。 针对工业机器人的机构特点，分析了影响机器人末端绝对定位精度的误差来源，采用修正的运动学连杆参数模型，基于微分变换法推导了用于机器人标定的误差模型，并基于 Matlab 软件系统编制了机器人运动学误差模型的最小二乘算法，通过对误差模型进行模拟求解，验证了机器人标定误差模型的可行性。 关键词：工业机器人 ; 运动学 ; 定位精度 ; 标定 ; 误差模型 ;连杆参数。 六自由度工业机器人标定 蒋涛 六自由度工业机器人标定 蒋涛 II Abstract Kinematic calibration is the key technology to improve the accuracy of robot, is also the important content of robotics, an unprecedented development in robot today have very important theoretical and practical significance. The robot kinematics calibration modeling based on kinematics, geometric error parameter identification for the purpose, to provide basis for error compensation of robot. The robot manufacturers robot its repetitive positioning precision is higher, but the absolute positioning accuracy is very low. With the use of robots are more and more widely, improving the robot absolute positioning accuracy has become a key technology problem which. This paper uses a kinematic calibration method, the application of advanced laser tracking measurement system based on parameter identification method and identification model of accurate parameters of a 6R robot model, improves the accuracy of the robot absolute positioning. Aiming at the industrial robot calibration, the actual mechanism firstly with the robot, the robot is established by D-H method of pole coordinates, based on the detailed derivation and solution of robot kinematics and Jacobi matrix, programming and kinematics model using Matlab language, with the attitude data comparison of robot controller, verified the correctness of the established link coordinate system. According to the mechanism of industrial robot, analyzes the impact of absolute location error precision of the robot, the kinematics model, based on differential transform method is derived for the error model calibration of robots, and based on the Matlab software system of least square algorithm for robot kinematics error model, through the simulation to solve the error model, validation the feasibility of robot calibration error model. Keywords: industrial robot; kinematics; positioning accuracy; calibration; error model; link parameters. 六自由度工业机器人标定 蒋涛 III 目录 摘 要 . 2 Abstract . II 第一章 绪论 . 1 1.1 引言 . 1 1.2 工业机器人运动学标定技术的背景和意义 . 1 1.3 机器人标定技术的研究现状 . 3 第二章 机器人运动学 . 5 2.1 机器 人运动学模型的建立 . 5 2.2 正向运动学求解 . 9 2.3 逆向运动学求解 . 10 2.4 微分运动学模型 . 13 2.5 本章小 结 . 17 第三章 SR06 型机器人的标定技术 . 18 3.1 标定用运动学模型的建立 . 18 3.1.1 直线的无极点表示法 . 19 3.1.2 CPC 模型的建立 . 20 3.2 机器人的标定方法 . 24 3.2.1 几何误差的来源 . 24 3.2.2 连杆参数的线性求解方法 . 25 3.3 本章小结 . 30 第四章 标定实验及结论 . 32 六自由度工业机器人标定 蒋涛 IV 4.1 原始数据采集 . 32 4.2 数据处理 . 33 4.2.1 齐次坐标变换矩阵与绕任意轴的旋转矩阵之间的关系 33 4.2.2 方程 RA Rx= RxRb的求解 . 36 4.3 标定结果 . 错误 !未定义书签。 4.4 本章小结 . 38 第五章 总结与展望 . 39 致谢 . 41 参考文献 . 42 附录 . 46 六自由度工业机器人标定 蒋涛 1 第一章 绪论 1.1 引言 工业机器人是一种具有自动控制的操作和移动功能，能够完成各种作业的可编程操作机器，工业机器人一般是由机械执行机构、伺服驱动装置、检测传感装置和控制系统等部分组成。自从 1959 年美国发明家乔治德沃尔和约瑟夫英格伯格制造出世界上第一台工业机器人以来，工业机器人作为机器人的一个主要分支，其五十多年来的发展历程使得工业机器人技术及产品广泛应用于各类工业生产中。工业机器人技术及其产品和相关配套技术发展迅速，已经成为自动化工厂（ Factory Automation, FA）和柔性制造系统（ Flexible Manufacturing System, FMS）实现自动化的重要工具。 工业机器人延伸了人的手足和大脑功能，可以代替人从事危险、有害、有毒、低温和高热等恶劣环境中的工作，代替人完成繁重、简单重复的枯燥劳动，提高劳动生产率，保证产品的生产质量。与计算机技术、网络技术对世界的生活生产方式产生巨大改变类似，工业机器人在自动化生产线上的广泛应用正在逐渐对全世界的生产方式产生变革。 1.2 工业机器人运动学标定技术的背景和意义 伴随着工业自动 化的需要和发展，工业机器人技术及产业规模都飞速发展起来。在工业生产中，弧焊机器人、点焊机器人、分配机器人、装配机器人、喷漆机器人及搬运机器人等工业机器人都已被大量采用。预计至 2015年，中国工业机器人年销售将达到 3.5 万台，工业机器人被广泛应用于汽车及汽车零部件制造业、机械加工行业、电子电气行业、橡胶及塑料工业、食品工业、木材与家具制造业等领域中，尤其是汽车行业、机械制造业。 技术融合是机器人发展的主要趋势。从近几年世界机器人推出的产品来看，工业机器人技术正在向智能机器和智能系统的方向发展，其发展趋势主要为 结构的模块化和可重构化，控制技术的开放化、 PC 化和网络化，伺服驱动技术的数字化和分散化，多传感器融合技术的实用化，工作环境设计的优化和作业的柔性化，以及系统的网络化和智能化等方面。机器人系统由硬件系统和软件系统组成，在硬件系统条件一定的情况下，机器人实用功能的灵活性 六自由度工业机器人标定 蒋涛 2 和智能程度在很大程度上取决于机器人的编程能力。机器人编程有在线编程(On-Line Programming)和离线编程 (Off-Line Programming) 两种形式。在机器人所要完成的作业不很复杂，以及示教时间相对工作时间比较短的情况下， 在线示教编程是切实可行的。随着企业对柔性加工要求的提高和计算机的发展，出现了机器人离线编程技术。机器人离线编程系统是机器人编程语言的推广，它利用计算机图形学的成果，建立机器人及其工作环境的模型，再利用一些规划算法，通过对图形的控制和操作，在不使用实际机器人的情况下进行轨迹规划，进而产生机器人程序。离线编程系统中的仿真模型 (理想模型 )和实际机器人模型存在有误差，产生误差的因素主要有机器人本体、工作环境以及离线编程系统等因素。因此如何有效地消除误差，提高机器人的精度成为离线编程系统实用化的关键技术之一。一般地， 影响机器人精度的因素可以归结为以下 4 类： (1) 参数因素：机器人运动学模型参数，包括杆件长度、相邻轴线的倾斜、关节的零位偏差等，机器人基础坐标系相对与参考坐标系的坐标转换关系误差等。 (2) 负载因素：机器人杆件弹性变形，齿轮传动误差，关节间隙等。 (3) 数字误差：包括机器人关节码盘读数精度，机器人控制器的控制分辨能力，编程分辨能力，计算机舍入误差等。 (4) 环境因索：温度影响，湿度影响，电噪声以及随机误差等。 对于不同结构、不同用途的机器人，以上各因素的影响不尽相同，一般来讲，在参 数因素中几何结构参数偏差引起的误差占机器人总误差的 80%左右。 结合机器人设计、加工、装配、编程和控制，一般认为提高机器人精度主要有两种方法： (1)误差预防法。即减少误差源，这种方法是通过改进设计方法和制造工艺，最大限度地提高机器人零部件的设计、加工精度，以降低加工误差；尽可能的提高装配精度，以降低装配过程造成的误差；另外还有采用高性能控制器提高机器人的控制精度等。但是这种方法本身具有较大的局限性。该方法的实现需要高精度的加工技术与条件，价格昂贵，而且这种方法对于机器人机械磨损、元件性能降低以及构件自 身动态特性等因素带来的误差无能为力。 六自由度工业机器人标定 蒋涛 3 (2)误差补偿法，即运动学标定，所谓标定就是应用先进的测量手段和基于模型的参数识别方法辨识出机器人模型的准确参数，通过采用附加控制算法或修改原控制算法来补偿机器人误差，从而提高机器人绝对精度的过程。该方法采用软件方法提高机器人精度，由于误差源与机器人误差之间的函数规律复杂，该方法只能补偿某些误差源 (主要是杆件几何偏差和杆件静态变形 )引起的误差。但是标定仍然是机器人误差补偿的良好途径，并且具有成本低的优点。 机器人标定在自动化制造、计算机集成制造装配系统中起 到非常重要的作用。机器人标定不仅可以提高精度，而且可以大幅降低制造成本。下面为机器人运动学标定的典型应用： （ 1）离线编程和机器人任务仿真的基础。机器人标定在计算机集成制造 (CIM)系统中的必要性已经被广泛接受。通过标定，机器人作业可以不通过示教来编程，而通过仿真进行离线编程，这样可以大幅降低成本。为了使离线生成的程序能够在机器人控制系统中有效执行，真实模型和设计模型要良好匹配，否则，位姿误差将会增大，使正常作业无法进行。通常运动学标定可以将绝对精度提高 10 倍。 （ 2）机器人制造的评价手段。许多物理参数 在机器人制造及装配后无法直接测量，而标定则是一种行之有效的间接测量方法。准确测量一定数目的机器人位姿，就可以较为准确地识别机器人的物理参数。 （ 3）机器人辅助手术中的应用。 Goswami、 Kinzle 以及 Tull 等把机器人系统应用到手术中，以提高手术质量。手术通常对绝对精度要求严格，故研究有效的补偿方法以保证绝对精度非常必要。 1.3 机器人标定技术的研究现状 在机器人应用范围扩大、所完成任务的复杂程度提高的情况下，特别是在 CAD/CAM/机器人 /数控设备的一体化环境中，示教再现型机器人难以提供环境所要求 的灵活性。机器人离线编程系统的迅速发展，成为解决实际生产问题的智能化手段。对于采用离线编程方式工作的机器人而言，机器人的绝对精度成为关键指标。而一般来说绝对精度只有厘米数量级，精度较低。于是需要进行机器人运动学标定的研究来提高机器人的绝对精度。机器人标定 六自由度工业机器人标定 蒋涛 4 的结果是一系列被识别的机器人参数，这些参数可以提供给制造厂家作为产品质量检验指标，也可以提供给用户来提高机器人的绝对精度，是离线编程和机器人任务仿真的基础。 在很多应用场合，机器人负载较小，此时造成位姿误差的主要原因在于机器人控制器中的机 器人理论运动学模型与机器人的实际物理模型之间存在的微小偏差，这个偏差主要是在机器人的制造及安装过程中产生的。 机器人运动学标定是离线编程技术实用化的关键技术之一，它是一个集运动学建模以及运动学参数辨识于一体的过程。机器人运动学标定根据标定方法的不同可以分成以下两类：基于运动学模型的参数标定以及基于神经网络的参数标定。本文主要讨论基于运动学模型的参数标定方法，它一般包括四个步骤： （ 1） 建立准确的运动学模型； （ 2） 用已知精度的测量装置测量出机器人末端操作器的位姿； （ 3） 引入算法辨识几何参数； （ 4） 对原有机器人运动学模型进行修 正。其中，选择合适的运动学模型和测量方法是标定的基础，辨识出几何参数的真实值并校正是标定的目的。 六自由度工业机器人标定 蒋涛 5 第二章 机器人运动学 2.1 机器人运动学模型的建立 SR06 型机器人是一台串联 6 自由度的旋转机器人，其控制柜中内建的运动模型为 D-H 模型，该模型是由 Denavit 和 Hartenberg 两人在 1956 年提出的一种方法，以列表的形式完成变换矩阵 A 的填写，使用起来十分简便，因此获得了广泛应用。 在建立机 器人运动学模型之前先要对各个关节和连杆进行编号：基座为杆 0，从基座起依次向上为杆 1、杆 2；关节 i 连接杆 i-1 和 i，即杆 i 离基座近的一端（简称近端）有关节 i，而离基座远的一端（简称远端）有关节 i+1。 D-H 方法严格定义了杆件坐标系，遵循的基本原则如下： 第一步：确定各坐标系的 z 轴。 基本原则是：选取 Zi轴沿关节 i+1 的轴向（指向可以任选，但通常都将各平行的 z 轴均取为相同的指向）。这里需要说明的是： 1. 当关节 i+1 是移动关节 时，其轴线指向已知，但位置不确定，这时选取 Zi轴与 Zi+1轴相交（若还有移动关节 i+2，则取 Zi轴和 Zi+1轴都与 Zi+2轴相交）。 2. 机器人杆 n 远端没有关节 n+1，这时可选取 Zn轴与 Zn-1轴重合。 第二步：确定各坐标系的原点。 基本原则是：选取原点 Oi在过 Zi-1轴与 Zi轴的公法线上（即 Oi为此公法线与 Zi轴的交点）。这里需要说明的是： 1. 当 Zi-1 轴与 Zi 轴平行时，经过两轴的公法线不唯一。确定方法是：若 Zi-1轴与 Zi轴重合，取 Oi=Oi-1，若 Zi-1轴与 Zi轴平行且不重合，过 Oi-1点作 Zi-1轴和 Zi轴的公法线，取此公法线与 Zi轴的交点为 Oi。 2. 由于没有 Zi-1轴，故无法按上述基本原则选取 O0，这时确定 O0的方法是：若 Z0与 Z1相交时，取 O0= O1，若 Z0与 Z1不相交时， O0在 Z0与 Z1的公法线上。 第三步：确定坐标系的 X 轴。 六自由度工业机器人标定 蒋涛 6 基本原则是选取 Xi轴沿过 Zi-1轴和 Zi轴的公法线，方向从 Zi-1轴指向 Zi轴。 这里要说明的是： 1.当 Zi-1轴与 Zi轴重合时，（这时 Oi=Oi-1）选取 Xi轴满足在初始条件位置时， Xi轴与 Xi-1轴重合。 2.当 Zi-1轴与 Zi轴相交且不重合时，选择 Xi =（ Zi-1 Zi，通常使所有平行的 X 轴均有相同的指向。 3.当 i=0 时，由上所述知，这时 O0=0，或 O0在 Z0轴与 Z1轴的公法线上，选取在初始位置时 X0轴与 X1轴重合。 第四步：确定各坐标系的 y 轴 基本原则是：使 yi= zi xi，即构成右手坐标系。 这 样 ， 便 能 建 立 起 如 图 2-1 所 示 的 坐 标 系 分 布 。 图 2-1 D-H 模型坐标系分布示意图 六自由度工业机器人标定 蒋涛 7 一旦对全部连杆规定坐标系之后，就能够确定用于指示系 i-1 和系 i 间相对位置和方位的 4 个参数。 杆件长度 ai：定义为从 zi-1轴到 zi轴的距离。沿 xi轴指向为正。 杆件扭角 i ：定义为从 zi-1轴到 zi轴的转角。绕 xi轴正向转动为正，且规定 i , )。 关节距离 di: 定义为从 xi-1轴到 xi轴的距离。沿 zi-1轴指向为正。 关节转角 i ：定义为从 xi-1 轴到 xi 轴的转角。绕 zi-1 轴正向转向为正，且规定 i , )。 参数 ai， i ， di和 i 的意义如图 2-2。这些参数被称为 D-H 参数，又常被称为机器人运动参数或几何参数。这里要说明的是： 1. 杆 i 的两端分别有 zi-1 轴和 zi 轴， ai 和 i 分别描述了从 zi-1 轴到 zi轴的距离和转角，关节转角 i 的轴向 zi-1是 xi-1 轴和 xi 轴的公法线；di和 i 分别描述了从 xi-1轴到 xi 轴的距离和转角。 2. ai和 i 由杆的结构确定，是常数。而 di和 i 与关节 i 的类型有关，其中一个是常数，另一个是变量。当关节 i 是转动关节时， di是常数， i 是变量；当关节 i 是移动关节时， di是变量， i 是常数。 图 2-2 D-H 模型参数示意图 所以，机 器人的每个杆件都有 4个参数 ai， i ， di 和 i 。它们决定 六自由度工业机器人标定 蒋涛 8 了机器人手臂每一个杆件的运动学形态。由此可以确定联系 i-1坐标系和 i坐标系之间的齐次变换矩阵，即从 i-1坐标系变换到 i 坐标系，只需完成下述变换： 1 沿 zi-1 轴平移距离 di，使 xi-1 轴和 xi 轴相交； 2 将 xi-1 轴绕 zi-1 轴转 i 角，使它同 xi 轴对准； 3 沿 xi 轴移动距离 ai，使 i 坐标系和 i-1坐标系的原点及 x 轴重合； 4 绕 xi 轴转 i 角，使 i 坐标系和 i-1坐标系完全重合。 表 2-1 D-H 模型连杆参数名义值 序号 i ( ) di（ mm） ai（ mm） i ( ) 1 0 0 0 0 2 -90 0 180 -90 3 0 54 635 0 4 -90 515.7 0 0 5 90 0 0 180 6 -90 0 0 0 将这四步过程用基本的齐次转动和齐次平移矩阵表示并连乘，可得相邻坐标系 i-1 和 i 之间的合成齐次变换矩阵 Aii-1，即 Aii-1=Transz（ di） Rotz（ i） Transz（ ai） Rotz（ i） =100010000100001id 1000010000c o ss in00s inc o siiii1000100001001 ia10000c o ss in00s inc o s00001iiii = 1000c o ss i n0s i nc o ss i nc o sc o ss i nc o ss i ns i ns i nc o sc o siiiiiiiiiiiiiiiiidaa（ 2-1） 由上式，要充分表示相邻连杆坐标系之间的位置关系，每个连杆至少由 4 个参数进行描述，其中连杆长度和扭角描述连杆本身，另外两个描述连杆和相邻连杆的连接关系。对于 6 自由度机器人，用 18 个参数可完全描述 六自由度工业机器人标定 蒋涛 9 它的运动学的固定参数部分，而其它 6 个关节变量则是机器人运动方程中的变量部分。本文所研究的机器人各关节均为转动关节，故 1 到 6 为关节变量。 2.2 正向运动学求解 当机器人的结构参数已确定，并给出各个关节角度，就可以计算机器人末端执行器在基坐标系中所处的位置和姿态，这就是求解机器人运动学的正问题。 对于 SR06 型机器人，在根据 D-H 原则建立起坐标系之 后，就可以根据式 (2-1)得到 6 个转换矩阵： A1=1000010s inc o s0s inc o ss in0c o s111111111daa， A2=10000100c o s0s inc o ss in0c o ss in22222222aaA3=10000010s inc o s0s inc o ss in0c o s33333333aa， A4=10000100c o s0s in0s in0c o s44444dA5=100000100c o s0s in0s in0c o s5555， A6= 100010000c o ss in00s inc o s66666d令 T=A1A2A3A4A5A6 (2-2) 则 T 表示了机器人末端法兰盘坐标系相对于机器人基坐标系的位置与姿态，所以 T 矩阵就是机器人正向运动学的解。 需要注意的是，为了增强机械强度， SR06 型机器人包含有一个四杆机构在内的闭环结构，这将导致 z2 轴与 z1 轴的联动现象。一旦 z1 轴相对零位发 生旋转，即使没有令 z2 轴单独转动，此时 z2 轴也会发生旋转，所以在计算时要考虑到这种影响，即从第三根轴的转角中减去第二根轴的转角。反过来的情况却不同， z2 轴的转动不会给 z1 轴产生任何影响。 六自由度工业机器人标定 蒋涛 10 正向运动学求解： 图 2-3 显示了机器人正向运动学求解的计算结果。 2.3 逆向运动学求解 当机器人末端执行器的位置和姿态给定时，去求解相应的关节转角是逆向运动学问题。由于求解逆问题的非线性方程组不一定总有封闭解、可能存在多个解、解与机器人实际结构相违背等可能，使得求解起来较为复杂。 根据机器人运动学理论，如果一台 6 自由度机器人的 3 个连续关节的轴线相交于一点，那么该机器人就有逆向运动的封闭解。对于 SR06 型机器人，它 的第 4、 5、 6 关节轴线相交于一点，因此就可以运用代数法得到封闭解。 在这里采用递推逆变换法求解一组非线性超越方程组。将一组逆矩阵A-11， A-12， A-13 连续左乘式 (2-2)两端，可得到若干矩阵方程，每个矩阵有 12 个方程式，在这些关系式中可选择只包含一个或不多于两个待求运动参数的关系式。然后递推求解，一般递推过程不一定全部作完，就可利用等式两端矩阵中所包含对应元素相等的关系式，求得所需的全部待求运动参数。 设 T=1000zzzzyyyyxxxxpaonpaonpaon(2-3) 则 T 中 12 个有效元素为： nx=c1s23(s4s6-c4c5c6)-c23s5c6-s1(s4c5c6+c4s6) ny=s1s23(s4s6-c4c5c6)-c23s5c6+c1(s4c5c6+c4s6) 六自由度工业机器人标定 蒋涛 11 nz=-c23(c4c5c6-s4s6)+s23s5c6 Ox=c1s23(c4c5s6+s4c6)+c23s5s6+s1(s4c5c6-c4c6) Oy=s1s23(c4c5s6+s4c6)+c23s5s6-c1(s4c5s6-c4c6) Oz=c23(c4c5s6+s4c6)-s23s5s6 ax=-c1(c4s23s5+c23c5)-s1s4s5 ay=-s1(c4s23s5-c23c5)+c1s4s5 az=-c4c23s5-s23c5 Px=-c1d6(c4s23s5-c23c5)-d4c23-a3s23-a2s2-a1-s1s4s5d6 Py=-s1d6(c4s23s5-c23c5)-d4c23-a3s23-a2s2-a1+c1s4s5d6 pz=d6(-c4c23s5-s23c5)-d4s23+a3c23+a2c2+d1 其中， si = sin i， ci = cos i， s23= sin( 2 + 3),c23 = cos( 2 + 3) 将式 (2-2)做简单变换，得到 A -12A-11T61A-16=A3A4A5 (2-4) 令该式左右两边的 (3,4)元素相等，化简得到 s1(axd6-px)=c1(ayd6-py),当 cos 0，即 k + /2（ k 为整数）时，有 tan 1=xxyy pda pda 66 由此可求得 1=arctan（xxyy pda pda 66 ）。 令式 (2-4)两边的 (1,4)和 (2,4)对应元素分别相等，得 -s3d4+a3c3=-d6c2(c1ax+s1ay)-s2az+c2(c1px+s1py-a1)-s2(pz-d1)-a2 (2-5) C3d4+a3s3=-d6-s2(c1ax+s1ay)-c2az-s2(c1px+s1py-a1)-s2(pz-d1) (2-6) 先将式 (2-5)中右边的 a2项移到左边，再将该方程组两边平方相加，得： 左边 =d24+a23+a22-2d4a2s3+2a3a2s3 右边 =-2d6f1f2+f3az+d26f21+azz+f22+f33 这里 f1=c1ax+s1ay ,f2=c1px+s1py-a1 ,f3=pz-d1 左式 =右式，那么 2d4a2s3-2a3a2c3=d24+a23+a22+2d6f1f2+f3az-d26f21+a2z-f22-f23 , 六自由度工业机器人标定 蒋涛 12 再令 p1 = 2 d4a2， p2 = 2 a3a2， P3=d24+a23+a22+2d6f1f2+f3az-d26f21+a2z-f22-f23 经过三角变换可以的得到 3=arctan(2322213pppp)-arctan(12pp ),上姿态时取正号，下姿态时取负号。 令 g1=d6( c1ax+s1ay)-c1px-s1py+a1 ,g2=d6az-pz+d1 ,g3 = c3d4+a3s3 ,那么式 (2-6)可以写成 s2g1+c2g2=g3，经过三角变换，得到 2=arctan(2322213gggg)-arctan(12gg ),上姿态时取正号，下姿态时取负号。 令式（ 2-4）两边（ 1,3）和（ 2,3）元素分别对应相等，有C3c4s5+s3c5=c2（ c1ax+s1ay） -s2az （ 2-7） S3c4s5-c3c5=-s2（ c1ax+s1ay） -c2az （ 2-8） 式（ 2-7)x s3-式（ 2-8） x c3 ，整理后得： C5=s32（ c1ax+s1ay） +c32az ,代入 1 、 2 、 3 ，即可求得 5 。 令式 (2-4)两边的 (3,3)元素相等，得 -s4s5=-s1ax+c1ay ,代入 5 即可求得4 。令式 (2-4)两边的 (3,2)元素相等，得 C4 = s6s1nx+c1ny+c6s1ox+c1oy，做三角变换，并代入 1 、 4 ，可以求得 6 。 按照上述方法求解关节变量时，都存在多解的问题，这时可根据 robtarget 数据类型的 robconf 来确定到底取哪个解。另外，当末端执行器由前一个点位向后一个点位运动时，达到后一点位的位姿有多解时，可选择最“接近”前一点位的解，即选择关节变量解最靠近前一点的关节变量值。 需要指出的是， SR06 型机器人的姿态并不是用 3 3 的旋转矩阵表示的，而是用一个四维向量 q 来表示的，它们之间 的关系为： q1=2 1 zyx aon, q2=2 1 zyx aon, signq2=sign(oz-ay), 六自由度工业机器人标定 蒋涛 13 q3=2 1 zxy ano, signq3=sign(ax-nz), q4=2 1 yxz ona,signq4=sign(ny-ox). 所以在求逆向解时要先将此四维向量转换成旋转矩阵。 运动学逆解： 图 2-4 显示了机器人逆向运动学求解的计算结果。 2.4 微分运动学模型 机器人的微分运动是指当关节坐标变量（包括关节变量和连杆参数）产生微小变化而引起臂端（手部）位姿的某一微小变化。 设机器人运动链中某一杆件对于固定坐标系的位姿为 T，经过微运动后该杆件对固定系的位姿变为 T+dT，若这个微运动是相对于固定系进行的，总可以用微小的平移和旋转来表示，即 T + dT=Trans( dx,dy,dz)Rot(k,d )T dT = Trans(dx,dy,dz)Rot(k,d )TT= Trans(dx,dy,dz)Rot(k ,d )IT (2-9) 根据齐 次变换的相对性，求微分运动是对于某个杆系 i 进行的，则 T+dT 可以表示为： T + dT=TTrans( dx,dy,dz)Rot(k,d ) dT=TTrans(dx,dy,dz)Rot(k,d )T= TTrans (dx,dy,dz)Rot(k,d )I (2-10) 其中 =Trans ( dx,dy,dz)Rot(k ,d )I 称为微分变换矩阵。 于是，式 (2-9)变为 dT = 0T，式 (2-10)变为 dT = T i，此处 的下标 六自由度工业机器人标定 蒋涛 14 不同是相对不同的坐标系，若变换 T 是若干变量 qi (i=1,2, ,n)的函数，则 dT= niii dqqT1(2-11) 微分平移与一般平移变换一样，其变换矩阵为： Trans(dx ,dy ,dz )=1000100010001zyxddd(2-12) 从几何意义上讲， 旋转矩阵可以表示为遵循右手法则绕轴 K= Tzyx k k k 做角的旋转，展开后如式 (2-13)所示： Rot(k, )=c o ss i ns i ns i nc o ss i ns i ns i nc o sv e r skkkv e r skkkv e r skkkv e r skkv e r skkkv e r skkkv e r skkkv e r skkv e r skkzzxyzyzxxyxyyzyxyzxzyxx