3自由度工程机器人.doc

1、第 2 章 工程机器人自主作业系统总体构架吉林大学机械电子工程实验室的3 自由度工程机器人吉林大学机械电子工程实验室的 3 自由度工程机器人如图 1-1 所示。它是在液压挖掘机工作装置的基础上，用单自由度的末端执行器替换挖掘机铲斗，再将回转体及相应部件安装在基座上，从而构成了一个 3 自由度串联关节型机械手。末端执行器由一对啮合齿轮、抓手、四连杆机构和驱动液压缸组成。通过驱动液压缸活塞杆的伸缩行控制抓手的开合。液压缸活塞杆带动四连杆机构并驱使主动齿轮啮合被动齿轮，以实现抓手的开合。这样的结构设计可保证抓手两边的抓取力相等。连杆 1连杆 0关节 1关节 2连杆 3关节 3连杆 3关节 4连杆 4

2、图 1-1 工程机器人机器人的连杆和关节3 自由度串联关节型机械手结构简图3 自由度串联关节型机械手结构简图如图 1-2 所示。机械各个关节的自由度是由液压缸来改变的，从机构学的角度看，它们都是一个曲柄滑块机构。液压缸采用电液比例阀控制。图 1-2 3 自由度串联关节型机械手结构简图1小臂；2抓手；3大臂；4回转臂；5基座6位移传感器；7电液比例阀；8压力传感器第 2 章 工程机器人自主作业系统总体构架3 自由度串联关节型机械手液压系统工程机器人以液压驱动为主，分别由四组直线液压缸来控制串联关节型机械手的四个自由度（包括手部自由度） 。整个液压系统由液压站、油路、电控比例换向阀、比例换向阀控制

3、器和液压缸等组成，如图 13 所示。图 1-3 工程机器人电液控制系统原理图1-油箱 2-粗滤油器 3-电动机 4-定量泵 5、10- 压力表6、9、11-表用管接头 7-单向阀 8-电控溢流阀 12-蓄能器13-细滤油器 14-压力开关 1518-例换向阀 1922-液压缸941235106117 813 14M19 21 21 2215 16 17 18123 自由度工程机器人自主作业系统3 自由度工程机器人自主作业系统基本构成如图 14 所示。3 自由度工程机器人自主作业系统结构3 自由度工程机器人自主作业系统结构如图 15 所示。主控计算机 工程机器人三目摄像机目标物体图像处理系统图

4、1-4 3 自由度工程机器人自主作业系统基本构成路径规划网络连接三维信息A/D 卡阀控制信号图 1-5 3 自由度工程机器人自主作业系统结构遥操作工程机器人 位移传感器压力传感器工程机器人自主作业控制系统目标物体D/A 卡三目摄像机 图像处理系统数据通信系统抓取检测传感器信号电液比例换向阀控制器第 2 章 工程机器人自主作业系统总体构架3 自由度工程机器人坐标系3 自由度工程机器人自主作业系统结构如图 16 所示。图 1-1 所示工程机器人的连杆参数见表 1-1表 11 3 自由度连杆机器人的连杆参数关节 i i() ai (mm) i() di (mm) cosi sini1 1 L1=35

5、0 90 0 0 12 2 L2=2014 0 0 1 03 3 L3=1400 0 0 1 04 4 L4=620 0 0 1 0其中： i 称为两条连杆之间的关节角；ai 代表连杆 i 的长度；i 称为两轴线 zi 与 zi+1 的交错角，即连杆 Li 的扭角；di 称为两条连杆之间的偏置；y0（b）前臂22222243022110压力量化值（a）前臂液压缸有杆腔压力曲线t/s0z2z12a1a2a3回转关节 大臂关节小臂关节抓手O0腕部关节x1x4y1y4341z0x0y2a4x2x3y3z3z4图 1-6 3 自由度工程机器人坐标系O1O2O3O4第 2 章 工程机器人自主作业系统总体

6、构架第 2 章 工程机器人自主作业系统本章系统阐述工程机器人自主作业控制系统的工作原理与总体结构，并介绍各系统的功能及相互关系。2.1 工程机器人自主作业系统原理图以立体视觉为基础，建立的工程机器人自主作业系统，其原理图如图 2-1 所示。摄像机在控制装置的作用下，像眼睛一样搜索目标物体，并抓拍目标物体的图像。图像处理系统根据目标物体的图像来计算目标物体的轮廓形状及相应的位置信息，并将这些信息规范化处理后送给工程机器人控制机。工程机器人将传送的物体坐标进行空间坐标变换，把物体坐标由摄象机空间坐标系转换为工程机器人坐标系，然后再进行运动学反解，计算出各关节角，最后控制工程机器人按路径规划靠近目标

7、物体，并根据任务的要求，将目标物体放到指定位置，整个过程实现工程机器人对目标物体的自主抓取。在此过程中，本论文对图像处理系统不做研究，除此之外还需解决的关键问题如下：1. 及时准确的将物体空间坐标由图像控制机传送到工程机器人控制机。2. 根据工程机器人本体结构进行运动学反解计算，求出各关节角。3. 设计新的算法控制器，使工程机器人能及时、准确、平稳的自主作业。4. 设计工程机器人控制系统，实现主控计算机对工程机器人的控制。主控计算机 工程机器人三目摄像机目标物体图像处理系统图 2-1 工程器人自主作业系统原理图路径规划网络连接三维信息2.2 工程机器人自主作业系统基本构成本文中的工程机器人主要

8、有三大部分系统组成，主要包括工程机器人本体、控制系统、传感检测系统，其中工程机器人本体包括工程机器人结构系统与驱动系统；传感检测系统包括位移、压力传感器、视觉传感器等，为控制系统获取有用的内、外部环境状态信息；而控制系统则包含图像处理系统、工程机器人自主作业控制统以及由环境所构成的交互系统等。工程机器人自主作业控制系统结构图如图 2-2 所示。2.2.1 工程机器人本系统的工程机器人是在液压挖掘机动臂的基础上，用单自由度的末端执行器替换挖掘机铲斗，从而构成了一个 3 自由度串联关节型机械手，如图2-3 所示。末端执行器由一对啮合齿轮、抓手和四连杆机构组成，通过驱动该自由度液压缸活塞杆的伸缩进行

9、控制，液压缸活塞杆带动四连杆机构并驱使主动齿轮啮合被动齿轮，以实现抓手的开合。这样的结构设计可保证抓手两边的抓取力相等。位移传感器平行固定在液压缸上，可随活塞杆的伸缩测量出液压缸位移的大小，实现位置闭环控制。串联关节型机械手作为工程机器人的主体，在伺服控制过程中，液压阀与液压缸的布置较困难。在一般的A/D 卡阀控制信号图 2-2 工程机器人自主作业控制系统结构图遥操作工程机器人 位移传感器压力传感器工程机器人自主作业控制系统目标物体D/A 卡三目摄像机 图像处理系统数据通信系统抓取检测传感器信号电液比例换向阀控制器第 2 章 工程机器人自主作业系统总体构架液压伺服系统设计时，液压阀与油缸是直接

10、相连的，但两者之间不允许有柔性存在。如果把液压阀装在串联型关节机械手臂上，则机械手臂的重量和转动惯量增大，如果把同液压阀直接连接的驱动部分放在工程机器人的基座上，通过钢丝绳等传递动力，由于钢丝的弹性以及传动机构的间隙等也对工程机器人的特性不利。考虑到各元件的重量分配和结构刚性，工程机器人机械手臂驱动部分的合理结构布置是把执行元件液压缸放在手臂上，把液压阀放在基座上，两者之间再以挠性连接。工程机器人机械手臂采用电液比例阀作为伺服控制元件，虽然电液伺服阀是响应特性最好的控制阀，但其制造成本和维护费用高。电液比例阀除了中位有死区外，其稳态特性与伺服阀不相上下，而制造成本和维护费用要低得多，因此得到越

11、来越广泛的应用。为了提高工程机器人在作业时的安全性，对原有的基座进行了改进设计。原有的基座的上部是一个利用槽钢焊接而成的一个支撑架，下部是四个刚度较大的支柱，下部用地脚螺栓固定在地面上，上下部分通过螺栓把紧，这样整个基座是一个固定的刚性支撑，结构如图 2-4 所示。工程机器人在作业工图 2-3 四自由度串联关节型机械手结构简图1小臂；2抓手；3大臂；4回转臂；5基座6位移传感器；7电液比例阀；8压力传感器程中，可能出现由于人为的误操作使抓手触地而没有及时关闭电液比例阀的现象，如果此类现象出现基座将受到系统的强大压力冲击，这种冲击可能使基座的上下连接处断裂而造成重大事故。为了避免此类事故发生，将

12、原来固定的基座增加一个转动自由度，使基座在受到压力冲击时可以绕一轴线转动。考虑到节约成本，在基座上部不变的情况下，去掉基座下部的四个支柱，重新设计四个支腿。用槽钢作成的两前支腿，并将其与上部支架焊接在一起，但前支腿不与地面固定，再用槽钢、钢板、万向节做成后支腿。将后支腿焊接在基座上部并与地面把紧，结构如图 2-5 所示。2.2.2 工程机器人电液控制系统工程机器人以液压驱动为主，分别由四组直线液压缸来控制串联关节型机械手的四个自由度。整个液压系统由液压站、油路、电控比例换向阀、比例换向阀控制器和液压缸组成，如图 26 所示。由图 2-6 可知，电液比例换向阀及其控制器是电液控制子系统的核心，由

13、它实现对液压缸的流量控制即速度控制和换向控制。电液比例控制系统的优点是充分利用液压控制与电器控制的长处，即在功率传递方面利用液压传动大功率的优点，在信号处理方面利用电气信号的处理运算便捷的优点。考虑到工程机器人现场工作环境比较恶劣，以及成本要求，采用电液比例换向阀作为液压控制元件。电液比例换向阀是一种具有液流方向控制和流量控制功能的复合阀，主要用于控制负载的运动方向和速度，除中位有死区以及频图 2-4 工程机器人基座原结构图图 2-5 工程机器人基座改进后结构图 图 2-4 工程机器人基座原结构图第 2 章 工程机器人自主作业系统总体构架率响应低于电液伺服阀外，其他特性越来越接近于电液伺服阀，

14、完全能够满足本系统的要求。在节流口压差恒定的条件下，通过它的液压油流量与输入的电流信号成比例，而流动的方向则取决于比例电磁铁是否受到激励。使用比例换向阀的回路，可省去节流调速元件，能迅速、准确地实现工作循环，避免负载引起的压力尖峰以及满足切换性能的要求，延长元件和机器的使用寿命。2.2.3 基于立体视觉的图像处理系统立体视觉是计算机视觉领域的一个重要课题，它的目的在于重构场景的三维几何信息。立体视觉的研究具有重要的应用价值，其应用包括移动机器人的自主导航系统，航空及遥感测量，工业自动化系统等。立体视觉的主要研究目标可总结为两点，其一是模拟人眼的功能建立立体视觉系统，完成各种视觉任务，获取目标物

15、体的深度图，实现三维物体重构；其二是将该研究作为探索人脑视觉工作原理的手段，进一步加深对人脑视觉的掌握和理解。图 2-6 工程机器人电液控制系统原理图1-油箱 2-粗滤油器 3-电动机 4-定量泵 5、10- 压力表6、9、11-表用管接头 7-单向阀 8-电控溢流阀 12-蓄能器13-细滤油器 14-压力开关 1518-例换向阀 1922-液压缸941235106117 813 14M19 21 21 2215 16 17 1812本文采用 DIGI-HIBW-60 立体成像系统采集图像，该立体成像系统的摄像机模型采用的是加拿大 Point Grey Research Inc.生产的 Dig

16、iclops 三目摄像机，如图 2-7 所示。通过三个 Sony 1/3“ progressive scan CCD 摄像头，IEEE 1394 接口，API 软件接口函数，DIGI-HIBW-60 立体成像系统使用户能够利用数字立体图像技术开发定制自己的应用，诸如跟踪、测量、VRML 模型的建立、人机交互、自主移动机器人等方面。本文搭建的图像处理系统由DIGI-HIBW-60 摄像机模型、 IEEE-1394 PCI 卡（如图 2-8 所示） 、图像处理计算机三部分组成。2.2.4 数据通信系统TCP/IP 作为 Internet 上最重要的一个协议族，其应用的广泛程度已经大大地超出了人们最

17、初设计它时想象的范围。TCP/IP 协议是一组包括 TCP 协议和 IP 协议，UDP 协议、ICMP 协议和其他一些协议的协议组。TCP/IP 协议并不完全符合 OSI 的七层参考模型。传统的开放式系统互连参考模型，是一种通信协议的 7 层抽象的参考模型,其中每一层执行某一特定任务。该模型的目的是使各种硬件在相同的层次上相互通信。这 7 层是:物理层、数据链路层、网路层、传输层、话路层、表示层和应用层。而 TCP/IP 通讯协议采用了 4 层的层级结构，每一层都呼叫它的下一层所提供的网络来完成自己的需求。这 4 层分别为：应用层、传输层、互连网络层、网络接口层。其中图 2-7 DigiClo

18、ps 三目摄像机外观图 图 2-8 IEEE 1394 PCI 卡第 2 章 工程机器人自主作业系统总体构架网络层和传输层与网络通讯程序的开发密切相关，模型如图 2-9 所示，TCP/IP 参考模型详解如图 2-10 所示40。UDP 是一个无连接协议，传输数据之前源端和终端不建立连接，当它想传送时就简单地去抓取来自应用程序的数据，并尽可能快地把它扔到网络上。由于传输数据不建立连接，因此也就不需要维护连接状态。UDP 信息包的标题很短，只有 8 个字节，相对于 TCP 的 20 个字节信息包的额外开销很小。另外它的吞吐量不受拥挤控制算法的调节，只受应用软件生成数据的速率、传输带宽、源端和终端主

19、机性能的限制。而 TCP 是面向连接的，可提供可靠的传输服务，但由于其底层 IP 协议是一种无连接的投递机制，为保证数据包传输时的连续性和正确性，其控制要比 UDP 复杂许多，故在实时系统中均会造成一定的时间开销，从而影响系统的实时性。本文所进行的工程机器人自主控制过程，是保证工程机器人控制系统能够及时读取由立体视觉系统计算得来的目标物体空间坐标，整个作业过程，目标物体的坐标的实时性较可靠性具有更重要的意义，因此选择 UDP 协议作为通信协议。UDP 信息包由 UDP 标题和数据组成。UDP 的标题结构如图 2-11 所示,它由 5 个域组成：源端端口(SourcePort)、目的地端口(De

20、stinationPort)、用户数据包的长度(Length)和检查和（Checksum)。其中，前 4 个域组成图 2-11 UDP 协议的标题结构应用层OSI 参考模型图 2-9 TCP/TP 参考模型TCP/TP 参考模型数据段数据包数据帧比特图 2-10 TCP/IP 参考模型详解应用程序 应用程序TCP UDPICMP IP IGMPARP RARP主机到网络层网络接口层表示层会话层传输层网络层数据链路层物理层传输层网络互连层应用层UDP 标题(UDP header)，每个域由 4 个字节组成；检查和域占据 2 个字节，它用来检测传输过程中是否出现了错误；用户数据包的长度包括所有 5

21、 个域的字节数。这样便解决了本章开始所提到四个关键问题中的第一个问题。2.2.5 工程机器人自主作业控制系统工程机器人自主作业控制统是整个工程机器人自主作业系统的核心。通过数据通信系统，它与图像处理系统控制机取得联系，并接收由图像处理系统控制机送来的目标物体空间坐标，将物体坐标由摄象机空间坐标系转换为工程机器人坐标系，再进行运动学反解，计算出各关节角。通过电液比例换向阀控制器，它控制液比例换向阀的开口大小及阀芯移动方向来控制液压缸的伸缩快慢，从而控制工程机器人的作业。同时读取位移、压力传感器的传感器信号，经处理后作为反馈量同目标量进行比较运算，再将算的阀控信号送往电液比例换向阀控制器，如此反复

22、运算直到整个运算结束，这就是工程机器人自主作业过程。工程机器人自主作业控制系统要做控制系统的算法研究，及控制系统的软硬件设计。其中工程机器人自主作业控制系统的算法研究我们将在第四章做详细介绍，控制系统的软硬件设计将在第五章做介绍。2.3 本章小结本章设计了工程机器人自主作业控制系统，系统阐述了工程机器人自主作业控制系统的工作原理与总体构成，并介绍了各子系统的功能、相互关系及试验中使用的传感器和设备。第 3 章 工程机器人机器人运动学分析第 3 章 工程机器人运动学分析3.1 引言关节坐标的计算是新兴的工业机器人控制过程中程序设计的基础。开发比较高级的机器人程序设计语言，要求具有按照笛卡尔坐标系

23、规定工作任务的能力。空间中的目标物体在工作空间内的位置以及机器人操作臂的位置，都是以某个确定的坐标系来描述的；而工作任务则是以某个中间坐标系（如附于操作臂端部的坐标系）来规定的。由笛卡尔坐标系来描述工作任务时，必须把上述规定变换为一系列能够由操作臂驱动的关节位置。确定操作臂位置和姿态的各关节位置的解答，就是运动方程的求解41 。本章将研究利用坐标变换的方法，通过设置主从手各连杆坐标系，确定各连杆的齐次变换矩阵D-H(Denavit-Hartenberg)坐标变换矩阵 42，从而建立从手的运动学和动力学描述方程。这是进一步从事工程机器人自主作业控制技术研究工作的数学基础。本章着重介绍了 4 自由

24、度工程机器人的运动学模型。在计算机控制过程中，工程机器人抓手根据位置和关节角来进行轨迹规划，如抓手的位置是直接和关节轴的角位移变量相对应的。反过来，这些相对于关节轴的角位移变量也是由各关节液压缸的长度决定的。这些变量之间的数学表达式是通过工程机器人各关节的运动学关系来确定的。3.2 工程机器人的正向运动学方程工程机器人运动学研究的是操作臂各连杆间的位移关系、速度关系和加速度关系。机器人操作臂可以看作为一个开式运动链，它由一系列连杆通过转动或移动关节串联而成的。开链的一端固定在基座上，另一端是自由的，安装着末端执行器，用以操作物体，完成各种作业。关节由驱动器驱动，关节的相对运动导致连杆的运动，使

25、抓手到达所需的位姿。为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系间的关系。Denavit 和 Hartenberg 提出一种通用的方法，用一 44 的齐次变换矩阵来描述相邻两连杆的空间关系，从而推导出“手抓坐标系”相对“参考系”的等价齐次变换矩阵，建立操作臂的运动学方程 43。3.2.1 工程机器人连杆的几何关系本文所使用的工程机器人工程机器人是由四个连杆和四个转动关节组成的，每个关节有一个自由度。基座称为连杆 0，不包含在这四个连杆之内。连杆 1 与基座由关节 1 相连接；连杆 2 与连杆 1 通过关节 2 相连接，依此类推，抓手与连杆 4 固接，基座

26、固定不动，如图 3-1 所示。其操作臂可以看作为一个开式运动链，它由一系列连杆通过转动或移动关节串联而成的。开链的一端固定在基座上，另一端是自由的，装有末端执行器，用以操作物体，完成各种作业。关节由驱动器驱动，关节的相对运动导致连杆的运动，使抓手到达所需的位姿。图 3-1 所示工程机器人的连杆参数见表 3-1。连杆 1连杆 0关节 1关节 2连杆 3关节 3连杆 3关节 4连杆 4图 3-1 工程机器人机器人的连杆和关节第 3 章 工程机器人机器人运动学分析表 31 3 自由度连杆机器人的连杆参数关节 i i() ai (mm) i() di (mm) cosi sini1 1 L1=350

27、90 0 0 12 2 L2=2014 0 0 1 03 3 L3=1400 0 0 1 04 4 L4=620 0 0 1 0其中： i 称为两条连杆之间的关节角；ai 代表连杆 i 的长度；i 称为两轴线 zi 与 zi+1 的交错角，即连杆 Li 的扭角；di 称为两条连杆之间的偏置；3.2.2 工程机器人的连杆坐标系的确定连杆的功能在于保持其两端的关节轴线具有固定的几何关系，为研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系间的关系。为此采用将杆件坐标系固定在每个杆件的上关节处的办法，与基座固接的坐标系记为0，第 i 杆件的坐标系i 设置于 i+1 号关

28、节上，并固定与 i 杆件上，坐标系i 与杆件 i 无相对运动。i 杆件坐标系i 的 oixiyizi系固定在 i 杆件的上关节 i+1 处，它的原点在关节 i+1 的轴线上；o izi 轴与关节 i+1 的轴线重合； oixi 轴是杆件长度的延长线；o iyi 轴的方向由右手坐标系的原则决定。同样，我们也将杆件 i-1 的坐标系i-1 设置在杆件 i-1 的上关节i 的轴线处 41。工程机器人坐标系的确定如图 3-2 所示 44。一旦对全部连杆规定坐标系之后，我们就可以按照下列顺序由两个旋转和两个平移来建立相邻两连杆 i-1 与 i 之间的相对关系，即坐标系i -1经过如下变换就可以得到坐标系

29、i 。（1）绕 zi-1 轴旋转 i 角，使 xi-1 轴转到与 xi 轴同一平面内。（2）沿 zi-1 轴平移一距离 di，把 xi-1 移到与 xi 同一直线上。（3）沿 xi 轴平移一距离 i，把连杆 i-1 的坐标系移到使其原点与连杆 i的坐标系原点重合的地方。（4）绕 xi-1 轴旋转 i 角，使 zi-1 轴转到与 zi 轴同一直线上。这种关系可由表示连杆 i 对连杆 i-1 相对位置的四个齐次坐标变换来描述，此关系式为 43：(3-1) 100cossinsinicosi coic ),()0,(),0(),(11 iii iiiii daxRtTransdraszRtA式中：T

30、rans(p x，p y，p z)平移齐次坐标变换矩阵，平移矢量为p x，p y，p zT；Rot(k，)旋转齐次坐标变换矩阵，k 表示旋转方向的单位矢量， 为旋转角度。根据表 3-1 所示的工程机器人的各杆件的参数，由式(3-1)可得到各杆件的 A 矩阵：(3-2)101110SLCSy0（b）前臂22222243022110压力量化值（a）前臂液压缸有杆腔压力曲线t/s0z2z12a1a2a3回转关节 大臂关节小臂关节抓手O0腕部关节x1x4y1y4341z0x0y2a4x2x3y3z3z4图 3-2 四自由度工程机器人坐标系O1O2O3O4第 3 章 工程机器人机器人运动学分析(3-3)

31、102221SLCSA(3-4)103332SLS(3-5)104443SLCSA3.2.3 工程机器人的正向运动学方程对于工程机器人的自主作业控制，抓手能够到达一个指定的方向和位置是有必要的。抓手的位置可以通过改变各关节液压缸的长度来实现，并由此调整关节角以使其达到所需的大小。这种能够描述抓手到关节轴的位置、方向以及液压缸长度的数学表达式被称为运动学方程。由此可以看出，当液压缸的长度或关节角变量被给定了以后，抓手的位置可以通过运动学方程的正解得到；而抓手的位置和方向一旦被确定了，相应于这个位置和液压缸长度的关节角可以通过运动学方程的逆解来得到。由图 3-2 可以看出，工程机器人的位置可以根据

32、抓手运动过程中其旋转轴来确定，即腕部关节坐标系的原点 O3 的坐标。对于和坐标原点 O3 相邻的两个连杆的位置向量 ip 和 i+1p，有如下的表达式：ip = Aii+1i+1p (3-6)为了得到点 O3 相对于基座坐标系下的坐标，方程（ 3-6）可以采取递归的形式来进行求解，即有：p0O3=（A 01A12A23）p 3O3= A03 p3O3 (3-7)式中：(3-8)1)(2231130SLCSpO同理，抓手的指尖点的坐标相对于基座坐标系的坐标同样也可以通过方程（3-9 ）表达出来，即有p0O4=（A 04）p 4O4 (3-9)式中，P4O4 是抓手指尖点相对于 O4x4y4z4

33、坐标系的空间坐标，P 4O4=0 0 0 1T；其中 p4O4 是相对于 O4 为坐标原点的空间坐标。(3-10) 100 )(234232234234 11114 SLSLCS CA当已知关节角变量 i（i=1，4），图 3-2 中抓手之间位置 O4 相对于基座坐标系的坐标可以计算出来。式中，角 的正弦和余弦简化表示形式如下：Si=sini (i=1，2，3， 4)，Ci =cosi (i=1，2，3，4)，Cij=Ci CjS iSjcos（ i j）( i=1，2，3，4； j=1，2，3，4)，Sij= CiSj SiCjsin（ i j）(i =1，2，3，4；j=1，2，3，4)，

34、C234C 2C34S 2S34，S234S 2C34+C2S34 式(3-9)描述了末端连杆坐标系4 相对基坐标系0 的位姿，是工程机器人运动分析和综合的基础。第 3 章 工程机器人机器人运动学分析图 3-3 所示为工程机器人的一个抓手。把所描述的坐标系的原点置于手爪指尖的中心，此原点由矢量 P 表示。描述抓手三个单位矢量的指向如下：z 向矢量处于夹手进入物体的方向上，称为接近矢量 a，y 向矢量的方向从一个指尖指向另外一个指尖，处于规定夹手方向上，称为方向矢量 o；最后一个矢量叫做法线矢量 n，它与方向矢量 o 和接近矢量 a 构成个一个右手矢量集合，是 o 和 a 的矢量积，其方向和大小

35、由 n=oa 所确定 45。因此，变换T4 具有下列元素：(3-11)1040zzyyxxOpaonp当工程机器人结构的运动方程被确定后，根据已知的 p0O4 来求各关节变量的解，是工程机器人轨迹规划中最为重要的问题。4.2.4 工程机器人关节角与液压缸伸长量的关系在以液压驱动的工程机器人系统中，在对工程机器人进行自主作业控制中，需要将各关节的角位移转换为液压缸的直线位移。因此，本系统 3 自由度工程机器人各关节转角与液压缸伸长量之间的关系如图 3-43-5 所示。O1O1l1 b1a111图 3-4 回转自由度(a)E维模 U制器模(a)E维模 U制器模糊控制器OZXY aonp图 3-3

36、工程机器人手爪位姿坐标系定义（1）对于回转自由度假定初始位置时， 10 为工程机器人处于初始位置时 a1、b 1 的夹角，此时对应的回转液压缸的长度（从其转轴 算起）为 l10，l 10 的长度可1o以根据三角形的余弦定理得到。当有回转动作时，假定回转角为 1，则此时液压缸的伸长变换量 l 1 可以由下面的表达式获得：(3-12)1010121 )cos(lbal 式中： 为按图 34 规定坐标系的回转自由度关节转角， 1-70 ，55 。1根据所获得的液压缸的伸长量，将其量化为 AD 卡所允许的数字量，并将其作为控制量的给定值。（2）对于大臂自由度对于大臂自由度而言，l 20 为工程机器人处

37、于初始位置时大臂自由度液压缸的长度（从其转轴 算起） ，当液压缸长度发生变化时，并假定此时大臂2o液压缸伸长变化量为l 2，根据图 3-5 的几何关系，则有(3-13)20/222 )cos(lbal 式中： ，其中 数值上是固定不变的，而 为按照图 3-520218 0规定坐标系的大臂自由度关节转角， 2-4 ，52 。（3）对于前臂自由度同理，我们可以根据三角形的几何关系来确定前臂液压缸的伸长变化量。O3a3l3b330330O33图 3-6 前臂自由度2022oaYXZOO2 l2a2 b2图 3-5 大臂自由度4m第 3 章 工程机器人机器人运动学分析已知 为工程机器人处于初始位置时3

38、0l(3-14) 30/323 )cos(lbal 式中： 3=360+30 30 3，且 30、30 为固定值，可以根据三角几几何何关系来确定。（4）对于抓手自由度由于抓手自由度的结构相对复杂，很难通过简单几何关系计算出抓手自由度液压缸伸长变化量l 4 与抓手自由度关节转角 4 的关系，因此采用曲线拟合的办法来确定二者关系。根据图 3-7，在三角形 O43O40O44 中，由于O 43O40O4 的值很小，因此可近似认为 O43、O 41 、O 40 在一条直线上，令 1420d2343120O则有：43418（3-15 ）其中：2244434arcosblhra210418（3-16 ）其

39、中：O41O42O40440O43 O44l4a4b4e4 c4d4图 3-7 抓手自由度34044rh4 1h10h40 f24442142224440cosarcosarcosenbdendmf根据式（3-15）和式（3-16） ，当给定一个 l4 时，便可通过计算得到一个与其对应的抓手自由度关节转角 4。在控制过程中通过测量、记录得到多组l 4 与 4 的数据对后，选择 4 为因变量进行曲线拟合，拟合结果如图 3-8 所示，表达式为l 4=0.004442-5.14064+299.6571，其中 4-27 ，65 。3.3 工程机器人运动学方程的逆解46在已知工程机器人末端执行器的位姿

40、p0O4 的基础上，通过运动学方程求解关节变量 qq 1 q2 q3 qnT 的过程称之为工程机器人位置的逆运动学问题，关节变量 q 的维数由机器人的自由度数 n 来决定。工程机器人逆运动学问题实际上是一个非线性超越方程组的求解问题，解的存在性主要取决于所研究的工程机器人的结构。逆运动学解的存在性问题即至少存在一组能够满足运动学方程的关节变量使工程机器人能够产生期望的手部位姿；当关节-30 -10 10 30 50 7050150250350450-50图 3-8 关节角 4 与抓手液压缸伸长量的关系第 3 章 工程机器人机器人运动学分析运动变量值只是一组解时，则解是唯一的，否则则称为有多组解

41、。当存在多组解时，需要在实际约束条件下对所有关节变量值进行校验，以确定它们是否满足实际要求。工程机器人在空间的轨迹是由工程机器人的作业任务来规定的，轨迹上的各点分别对应于它们所要求的关节变量 q。由计算得到的关节变量 q 去控制工程机器人各执行机构的运动，从而完成对所规划的轨迹的跟踪。由表 3-1 可知，本文所采用的工程机器人的关节变量 q 1 2 3 4T。在关节变量求解的过程中，采用变量分离的方法。对于工程机器人自主作业控制，在进行作业时，通过控制关节角的变化驱动液压缸来实现转手对物体的搬运。由式（3-7）可以得到，p1O3=A10p0O3 (3-17)式中：A 10 A01-1，且 (3

42、-18)101101CSL现假定点 O3 在基座坐标系下的坐标为 p0O3= p0XO3 p0YO3 p0ZO3 1T，则可以得到相对应的关节角 1， 2， 3， 4。展开式（3-17）可以得到(3-19) 110 3030130130130301131 OYXZOOZYXO pCSLpCSLp再由式（3-17）可以得到p1O3=A10（A 03）p 3O3=A12A23p3O3 (3-20)式中，p 3O3=0 0 0 1T。由式（3-20 ）可得（A 12） -1p1O3= A23p3O3 (3-21)即有 100110 333030130130122 SLCSpCSLCSLOYXZO(3

43、-22)由（3-22 ）可以得到(3-23)031301OYXpCS(3-24)）（ 30-1tanX根据（3-22 ） ，还有 3230213013012 CLpSLpSCOzOY）（(3-25)(3-26)- SSzX）（对式（3-25 ）和式（3-26 ）平方相加得2130130122230213013012 )(）（）（ LpSCpLCpSL OYXOzOzOYX (3-27)令 r ，r302sinOzp ）（ 13013012cos LpSOYX则 2/13013012302 )(4 ）（ LpSCLOYXz 因此有 ）（ 130130130/tanpYOXz由此可以得到(3-28

44、)/t 1301301301 ）（ LSCOYz 则式（3-27 ）根据三角函数变换可以写成(3-29)rLpSCpOYXOz /)()(cos 21301301232302 ）（ 第 3 章 工程机器人机器人运动学分析在获得了 1， 2 的基础上，根据式 (3-25)和式(3-26)则可以得到关节角 3(3-30)(tan 23013013012302-3 LpSpCSOXYOXOz 此外，工程机器人实际运动中，为了使得到的关节角能够更加合理，根据各关节之间的关系，可以用下列条件 1-55 ，70 ， 2-4 ，52 ， 3214 ，318 ， 4-27 ，62 加以判断。3.4 基于 UG

45、 环境的工程机器人运动学仿真分析为了验证所获得的机器人运动学模型的正确性，利用三维实体造型软件UG 创建了机器人实体模型，并进行了仿真分析，如图 3-9 所示。UG 是 Unigraphics Solutions 公司主要的 CAD 产品，UG 面向过程驱动的技术是虚拟产品开发的关键技术,在面向过程驱动技术的环境中，用户的全部产品以及精确的数据模型能够在产品开发全过程的各个环节保持相关，从而有效地实现了并行工程。该软件不仅具有强大的实体造型、曲面造型、虚拟装配和产生工程图等设计功能；而且，在设计过程中可进行有限元分析、机构运动分析、动力学分析和仿真模拟，提高设计的可靠性。图 3-9 基于 UG 环境的工程机器人运动学仿真分析3.5 本章小结本章利用坐标变换的方法，通过设置工程机器人各连杆坐标系，确定各连杆的齐次变换矩阵，建立了 3 自由度工程机器人的运动学方程，确定了统一的空间坐标系下工程机器人各关节转角与其抓手末端位置。运动学方程的正解以及关节液压缸摆角和伸长量之间的关系是进行工程机器人作业控制的理论基础。运动学反解使工程机器人自主作业控制成为可能。本章解决了第二章中提到的四个关键问题中的第二个问题。