第 30 卷第 4 期 辽宁工程技术大学学报（自然科学版） 2011 年 8 月 Vol.30 No.4 Journal of Liaoning Technical University（Natural Science） Aug . 2011 文章编号：1008-0562(2011)04-0562-04 DOI: CNKI:21-1379/N.20110826.2131.009 4 自由度串联式机械手轨迹运动学的分析 石 宁 1，黄 勇 1，李晓豁 2 （1.新疆工业高等专科学校 机械工程系，新疆 乌鲁木齐 830091； 2.辽宁工程技术大学 机械工程学院，辽宁 阜新 123000） 摘 要：为了实现 4 自由度串联式机械手的轨迹控制，为实验室设计一种 4 自由度串联式机械手的结构，基于 D-H 坐标变换理论，建立了该机械手位置运动学模型，研究了该模型的正、逆解问题。通过计算机仿真，验证 了该模型及其正解的正确性。以此为基础，分析了该 4 自由度串联式机械手的工作空间，并导出该模型逆解的解 析式，为实现该机械手的轨迹的精确控制奠定了基础。 关键词：4 自由度机械手；运动学模型；正解；逆解；工作空间 中图分类号：TP 242 文献标志码：A Position kinematics of 4-freedom serial manipulator SHI Ning1，HUANG Yong1，LI Xiaohuo2 (1.Department of Mechanical engineering, Xinjiang Ploytechnical College, Urumuqi 830091,China； 2. Mechanical Engineering College of Liaoning Technical University, Fuxin 123000,China) Abstract: In order to achieve a trajectory control for a 4-freedom serial manipulator designed for our laboratory, a kinematics model of the manipulator is established based on D-H coordinate transformation theory according to its structure in the paper, problems of positive solution and of inverse solution for the model is studied, correctness of the kinematics model and the positive solution are proved through computer simulation. The workspace of 4 freedom manipulator is analyzed on the basis; the expressions of inverse solution are derived, which lay the foundation for controlling accurately the trajectory of the manipulator. Keywords: 4-freedom manipulator; kinematics model; forward kinematics; inverse kinematics; workspace 0 引 言 机械手是机器人的一种重要形式，为了实现机 械手的控制，首先要建立机械手的位置运动学模 型。位置运动学研究的问题有两类：已知机构运动 参数求解机械手末端执行器相对于参考坐标系的 位置和姿态，这一类问题为正解位置问题。另一类 问题是给定机械手的位置和姿态，计算各运动副的 参数，这类问题称之为逆解位置问题[1]。本文建立 了一种 4 自由度机械手的位置运动学模型。通过对 模型的仿真和求解，确定其工作空间，为后续实现 机械手的轨迹控制奠定良好基础。 1 基于 D-H 坐标的位置运动学模型 建立该 4 自由度串联式机械手的三维模型如图 1。 图 1 机械手的三维模型 Fig.1 3D model of manipulator D-H 坐标变换法是由 Denavit 和 Hartenberg 提 出的一种在关节链中建立各杆件坐标系，以研究相 连杆件位置、姿态关系的方法，该方法是建立串联 式机械手运动学模型的比较成熟和有效的方法，本 文将利用该方法研究 4 自由度串联式机械手的位置 运动学。 收稿日期：2011-06-30 网络出版时间：2011-8-26 21:31:24 网络出版地址：http://www.cnki.net/kcms/detail/21.1379.N.20110826.2131.009.html 基金项目：新疆维吾尔自治区高等学校重点科研计划项目（XJEDU2008I46）；新疆维吾尔自治区高等学校科研计划项目（XJEDU2008I49）； 中国煤炭工业科技计划项目（MTKJ2009-254）；辽宁省大型工矿装备重点实验室（辽宁省第二批科学技术计划项目,2008403010） 作者简介：石 宁（ 1962-），男，黑龙江 哈尔滨人，副教授，主要从事矿山机电设备、工程机械的设计与制造方向研究。本文编校：焦 丽 中国煤炭期刊网 www.chinacaj.net

第 4 期 石 宁，等：4 自由度串联式机械手轨迹运动学的分析 为实验室设计的机械手拥有 4 个转动自由度， 见图 2。根据关节的类型和位置，针对每个关节给 出该关节所连接的两个杆件的 D-H 参数（见表 1）。 其中转角为机械手的关节转角，其取值范围为： 1α =0°~360°， 2α =0°~90°， 3α =-90°~90°， 4α =0°~180°。 z1 x1 z2 x2 z3 x3 z0 x0 图 2 机械手 D-H 坐标 Fig.2 D-H coordinates system of manipulator 表 1 4 自由度串联式机械手 D-H 坐标参数 Tab.1 parameters in D-H coordinate system for the 4 DOF manipulator 连杆 杆长/mm 偏距/mm 偏角/(°) 转角/(°) 1 2 3 4 0 340 335.5 0 260 0 0 0 90 0 0 90 1α 2α 3α 4α 根据表 1 参数，依次写出相邻杆件的 D-H 齐次 变换矩阵[2] T 01 = Ca 1 Sa 1 0 0 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ 0 0 1 0 Sa 1 Ca − 1 0 0 0 0 260 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ T 12 = Ca 2 Sa 2 0 0 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ 2 Sa − Ca 2 0 0 Ca 0 340 2 Sa 0 340 2 0 1 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ ， （ 1） ， （ 2） ， （3） T 23 = Ca 3 Sa 3 0 0 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Sa − 3 Ca 3 0 0 Ca 3 Sa 3 0 335.5 0 335.5 0 1 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ T 34 = 4 Ca 4 Sa 0 0 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ 0 0 1 0 Sa 4 Ca − 4 0 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ 563 。 （4） 将末端执行器的参考点设定在执行器腕部坐 标系沿 z 轴方向 210 mm 处，则建立的矩阵 T = 0 0 0 0 0 0 0 0 0 0 0 210 0 0 0 0 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ 。 （5） 根据以上矩阵，得到 4 自由度串联式机械手的 位置运动学模型 T T T T E 01 12 23 34 = ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ x y x l m n x l m n y l m n z z 0 0 z 0 y y x p p p z 1 ， （ 6） ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ 式中，l 、m 、n 分别为末端执行器坐标轴 x 、 y 、 z 的单位向量， xp 、 yp 、 zp 分别为末端执行器坐 标原点在参考坐标系下的坐标。 综上可见，式（6）是一个含有 12 个方程的超 越方程组。 2 运动学模型的正解问题与仿真 已知机械手各关节转角求取机械手末端执行 器的位姿，为运动学模型的正解位置问题。如果将 一些特殊关节转角带入模型，就可检查末端执行器 的位置和姿态是否为预期值。因此，通过正解问题 可以验证所建立位置运动学模型的正确性。该方法 虽然可行但并不直观。 Robotics Toolbox 是基于 MATLAB 环境下的一 种针对串联式机器人研究的工具箱，它集成了大量 函数，对机器人的开发和研究有重要意义。利用它 可以检验所建立 4 自由度串联式关节机械手位置运 动学模型的正确性[3]。具体过程如下：(1)利用 link 函数和 robot 函数建立机械手模型；(2)运用 drivebot 函数生成模型三维图（见图 3）；(3)驱动关节标尺中 的滑块，观察末端执行器的位姿。通过与期望位姿 作对比检验模型的正确性。 中国煤炭期刊网 www.chinacaj.net

564 辽宁工程技术大学学报（自然科学版） 第 30 卷 z 0 500 1000 -500 -1000 1000 500 0 y -500 -1000 -1000 -500 0 x 1000 500 图 3 robotics toolbox 生成的机械手模型 Fig.3 manipulator model produced by robotics toolbox 1200 1000 800 600 400 200 0 z 轴 -200 -800 -600 k 轴 -400 -200 0 x 轴 200 400 600 800 图 4 机械手工作空间截面 Fig.4 section of manipulator workspace 3 机械手的工作空间 工作空间是指工业机器人正常运动时，末端执 行器参考点能在空间活动的最大范围。机器人工作 空间的求解方法主要有解析法、图解法及数值方 法。解析法求解繁琐且不直观，图解法虽然直观但 对于多自由度的复杂机械手需要分组处理，过程比 较复杂。 由于 4 自由度机械手的运动学模型正解问题已 经解决。利用运动学模型正解问题可以求解出机械 手末端执行器参考点的空间坐标即空间位置。如果 可以求解出末端执行器所有能够到达空间点的坐 标，那么这个点集就是机械手的工作空间[4]。但是 所求集合是无限集，所以只能用有限点来逼近工作 空间的点集。以上思路为数值方法。这种方法建立 在运动学模型正解问题的基础上，所求工作空间也 可以反过来验证运动学正解问题是否正确。基于以 上思路，按照如下步骤编制程序：(1)按一定步距将 每个关节转角取值空间离散（步距的大小决定了逼 近的精度），即每个关节转角取一组特定值；(2)将 4 个关节转角的取值做组合；(3)将所有关节转角的组 合代入⑹式中与位置相关的 3 个方程，得到一组末 端执行器的位置坐标；(4)利用 MATLAB 的图形显 示功能将所有坐标对应的点显示出来形成一个点 集“云图”； 为了清晰显示机械手工作空间的内部情况，先 赋予与杆 1 相关的转角 1α 一个常量值，比如 0°（相 当于约束杆 1 旋转自由度），其它关节取值空间的 离散步距为 0.1π，代入式（6）。最后利用 MATLAB 显示计算出的点集（见图 4）。由图 4 可见，点集中 最外侧点的包络线围成的区域就是 4 自由度串联式 机械手的工作空间截面。该截面可以清晰显示机械 手工作空间的内部情况。如将截面图形绕 k 轴旋转 360°，就可得到机械手的工作空间。 4 运动学模型的逆解与求解实例 若已知机械手的位姿反求各关节转角，这便是 运动学模型的逆解位置问题。显然逆解问题是控制 机械手需要解决的关键问题。通过观察式（6）不 难发现直接求解非常困难。所以需要对式（6）做 等效变换[5]。具体如下： 对式（6）同时左乘 − TT 1 1 − 01 12 ，得 TTETT 1 1 − − 23 01 12 = 34 x y l ⎡ ⎢ l ⎢ l ⎢ z ⎢ 0 ⎣ y x m m m z 0 y x n n n z 0 y x p p p z 1 ， （7 ） ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ 对式（7）的方程组依次消元，得到模型逆解 的解析式 a 1 = arctan p p y x ⎞ ⎟⎟ ⎠ ， （ 8） ⎛ ⎜⎜ ⎝ ， v 1 v 2 a 2 = = = p x p z − − arcsin( n y n p 210 210 + + x y n 210 260 ， − v 2972 5. 2 − − − 2 2 ca v 680 2 + 1 1 ca 1 v 2 2 z ) − arctan( vca 11 v 2 ) 中国煤炭期刊网 www.chinacaj.net

x ( − + u = 1 ca p ( 2 u = 2 sa p ( 210 ) x n n sa ca 2 1 260 210 ) − z n p − z ca ca p ( x − 210 ) x − 2 − − 2 z z 2 n 1 260 210 ) 340 u 1 u 2 nsa 1 arctan( a = 3 nca 1 ca + 2 2 x + ca sa p ( 1 − 210 ) y n + y ); （ 10） 性。假定给出一组关节转角，利用模型正解得到一 组机械手的位姿。再将位姿代入式（8）~式（11） 的模型逆解，反求出关节转角，通过对比给出的关 节转角和逆解求出的关节转角可以验证逆解的正 确性[6-11]。基于以上思路编制程序验证结果，由于 文章篇幅限制，只列出一组解的计算结果（见表 2）。 以上过程验证了该 4 自由度关节机械手的逆解是正 确的。 第 4 期 石 宁，等：4 自由度串联式机械手轨迹运动学的分析 565 + sa sa 1 2 ( − p y + 210 ) y n + 4 a = ca 。 （11） arcsin( 式（8）~式（11）就是机械手位置运动学模型 的逆解。通过代入实际数值，可以验证逆解的正确 nsa 2 − + a ) 3 y z 表 2 一组逆解问题的实例 Tab.2 an example about inverse kinematics 给定的关节转角（弧度） 末端执行器位姿 逆解求出的关节角（弧度） a 1 a 3 = = ;0 .1 2 a .0 = a 0996 ; ; 3146 .0 47124 = 4 p l x m x n p 575.58; = x y -0.30902;l = y 0; 1; = = − n 0.95106; = p 0; = = z l 0; = = z m 0; = z n 0; = m y x y = 0.30902; z 761.43; 0.95106; a 1 a 3 = = ;0 .1 2 a .0 = a 0996 ; ; 3146 .0 47120 = 4 由于三角函数的周期性，模型的逆解存在多 解。在具体选用时要通过代入模型正解验证，来删 除增根。另外，如果几个解都满足条件，就要根据 具体情况比如路径最短、速度最光滑等约束条件求 最优解。 5 结 论 基于 D-H 坐标变换理论建立了 4 自由度串联式 机械手位置运动学模型，并研究了模型的正、逆解 和机械手工作空间问题，使机械手的运动学特性有 了重要参考依据，对进一步做机械手机构的优化设 计奠定了基础。特别是位置运动学逆问题的解决对 后期实现机械手轨迹控制有重要意义。另外，文章 所提出的利用仿真的手段研究机械手运动学模型 和工作空间的方法对机械手的设计有一定的参考 价值。 参考文献： [1] 蔡自兴.机器人学[M].北京:清华大学出版社,2000. [2] 傅则绍.机构设计学[M].成都:成都科技大学出版社,1988. [3] 罗家佳,胡国清.基于 MATLAB 的机器人运动仿真研究[J].厦门大学 学报:自然科学版,2005,44(5):640-644. [4] 梁喜凤,王永维,苗香雯,等.番茄收获机械手工作空间分析与仿真[J]. 浙江大学学报:农业与生命科学版 2005,31(6):807-811. [5] 陈 平 ,刘国海.MOTOMAN-UPJ 型机械手运动学改进算法研究[J]. 机械传动,2006,30(4):23-27. [6] 倪振松,廖启征,魏世民,等.空间一般 6R 机械手位置反解的新方法[J]. 北京邮电大学学报,2009,32(2):29-33. [7] Tsai L W,Morgan A. Solving the kinematics of the most general six and five-degree-of-freedom manipulators bycontinuation methods[J].Mechanism and Machine Theory, 1985, 20 (2) :189-200 . [8] CLIFFORD W K. Application of Grassman s extensive algebra[J] .Amer J of Math, 1878(1) :350-358. [9] Li H, Hestenes D, Rockwood A. Generalized homogeneous coordinates for computational geometry .Geometric Computing with Clifford Algebras. Berlin, Heidelberg: Springer-Verlag, 2001(1):27-60 . [10] Lee H Y, Liang C G. Displacement analysis of the general spatial 7-link 7R mechanism.Mechanism and Machine Theory,1988,23(3) :219-226 . [11] Dinesh Manocha, John F Canny. Efficient inverse kinematics for general 6R manipulators .IEEE Transactions on Robotics and Automation, 1994,10 (5) :648-657 . 中国煤炭期刊网 www.chinacaj.net