机器人技术第三章机器人运动学及其数学基础.ppt

I. 机器人学 机器人学 机械电子工程 Dr. Kevin Craig I. 机器人学 IEEE International Conference on Robotics and Automation (ICRA) 2010 安克雷奇 文章： 856/2034 分会场： 154 国家： 47 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2009 圣路易斯 文章： 936/1599 分会场： 192 国家： 53 I. 机器人学 Technical Session的主要内容 Human robot interaction Medical robotics Sensor fusion Legged robots Underwater robots Manipulator motion planning Camera calibration Intelligent transportation systems SLAM: Features and landmarks Humanoid robot body motion Microrobots Biologically-inspired robotic devices Rehabilitation robotics Field robotics Grasping Nanorobotic manipulation Fish-like robot Parallel robot 第二章 机器人运动学及其数学基础 参考教材 美 付京逊 机器人学 中南大学 蔡自兴 机器人学 美 理查德 鲍尔 机器人操作手 数 学 编程与控制 参考教材 美 付京逊 机器人学 美籍华人 普渡大学（ Purdue University）电机工程专业著 名教授 4部著作、 400多篇论文 第一任国际模式识别学会会长，被誉为自动模 式识别之父 1985年去世 参考教材 中南大学 蔡自兴 中南大学教授，我国人工 智能和机器人领域著名专 家 中国人工智能学会智能机 器人专委会理事长 曾与付京逊教授一起工作 过 第一节 引言 串联机器人可以用一个开环关节链来建模 由数个驱动器驱动的 转动 或 移动 关节串联而成 一端固定在基座上，另一端是自由的，安装工具（末端 执行器），用以操纵物体，或完成各种任务 i n o a 关节的 相对运动 导致杆件的运动， 使末端执行器定位于所需要的方 位上 在一般机器人应用问题中，人们 感兴趣的是：末端执行器相对于 固定参考坐标数的 空间几何描述 ， 也就是机器人的运动学问题 机器人的运动学即是研究机器人 手臂 末端执行器位置和姿态 与 关 节变量空间 之间的关系 运动学研究的问题 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 运动学正问题 运动学逆问题 哈佛大学 Roger Brockett建立的指数积公 式 运动学 滚动接触 非完整控制 数学基础 -刚体运动 参考文献： 机器人操作的数学导论 作者：理查德 摩雷 李泽湘 夏卡恩 萨斯特里 翻译：徐卫良 钱瑞明（东南大学） 研究运动学的方法 1955年丹纳维特（ Denavit）和哈顿伯格（ Hartenberg）提 出了一种 采用矩阵代数方法 解决机器人的运动学问题 D-H 方法，其数学基础即是 齐次变换 具有直观的几何意义 能表达动力学、计算机视觉和 比例变换问题 为以后的比例变换、透视变换 等打下基础 1000 p p p T z yyy xxx zzz y x w w w 第二节 数学基础 齐次坐标和齐次变换 2.1 点和面的齐次坐标 2.1.1 点的齐次坐标 一般来说， n维空间的齐次坐标表示是一个（ n+1）维空间实体。有一 个特定的投影附加于 n维空间，也可以把它看作一个附加于每个矢量的 特定坐标 比例系数。 引入齐次坐标的目的是为了表示几何变换的旋转、平移和缩放 kcjbiav zy x Tw w z y x V 式中 i, j, k为 x, y, z 轴上的单位矢量， a= , b= , c= ， w为比例系数 w x w y w z 显然，齐次坐标表达 并不是唯一 的，随 w值的不同而不同。在计算机图学中， w 作为通用比例因子，它可取任意正值，但 在机器人的运动分析中，总是取 w=1 。 列矩阵 一个点矢： 例 1： kjiV 543 可以表示为： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T 齐次坐标与三维直角坐标的区别 V点在 O XYZ坐标系中表 示是 唯一 的（ a、 b、 c） 而在齐次坐标中表示可 以是多值的。 不同的表 示方法代表的 V点在空间 位置上不变。 x y z z z x V 图 2 - 2 o 几个特定意义的齐次坐标： 0 0 0 nT 坐标原点矢量的齐次坐标， n为任 意非零比例系数 1 0 0 0T 指向无穷远处的 OX轴 0 1 0 0T 指向无穷远处的 OY轴 0 0 1 0T 指向无穷远处的 OZ轴 0 0 0 0T 没有意义 2个常用的公式： zzyyxx babababa kbabajbabaibaba bbb aaa kji ba xyyxzxxzyzzy zyx zyx )()()( 点乘 : 叉乘 : 2.1.2 平面的齐次坐标 平面齐次坐标由 行矩阵 P=a b c d 来表示 当点 v=x y z wT处于平面 P内时，矩阵乘积 PV=0，或记为 0 dwczbyax w z y x dcbaPV 与点矢 相仿，平面 也没有意义 T0000 0000 点和平面间的位置关系 设一个平行于 x、 y轴 ， 且在 z轴上的坐标为单位距离的平面 P可以表示为： 或 有： PV= 1100 P 2200 P v0 v0 v0 点在平面下方 点在平面上 点在平面上方 例如：点 V=10 20 1 1T 必定处于此平面内 ， 而点 V=0 0 2 1T 处于平 P 的上方 ， 点 V=0 0 0 1T处于 P平面下方 ， 因为： 0 1 1 20 10 101000 0 1 1 2 0 0 1100 0 -1 1 0 0 0 1-100 2.2 旋转矩阵及旋转齐次变换 2.2.1 旋转矩阵 设固定参考坐标系直角坐标为 Oxyz， 动坐标系为 Ouvw， 研究旋转变换情况 。 x y z w v u P o ( O ) 图 2 - 3 初始位置时，动静坐标系重合， O、 O 重合，如图。各轴对 应重合，设 P点是动坐标系 Ouvw中的一点，且固定不变。则 P 点在 Ouvw中可表示为： wwvvuuu v w kPjPiPP 、 、 为坐标系 Ouvw的单位矢量 ， 则 P点在 oxyz中可表示为： ui vj wk zzyyxxxyz kPjPiPP xyzu v w PP 当动坐标系 Ouvw绕 O点回转时，求 P点在固定坐标系 oxyz 中的位置 y z x o ( O ) u v w P Pw Pv Pu 图 2 - 4 已知： P点在 Ouvw中是不变的仍然成 立，由于 Ouvw回转，则： wwvvuuu v w kPjPiPP xwwvvuuxu v wx ikPjPiPiP )(P ywwvvuuyu v wy jkPjPiPjP )(P zwwvvuuzu v wz kkPjPiPkP )(P 用矩阵表示为 : w v wzvzz wvyy wxvxx z y x P P P kkjkik kjjjij kijiii P P P y （ 2-7） u v wxyz wzvzz wvyy wxvxx PRp kkjkik kjjjij kijiii :R y 则旋转矩阵为：定义 反过来： x y zu v w PRP 1 R RR de t *1 T1 RR Rd e t 是正交矩阵，的行列式，为的伴随矩阵，为 RRRR 2.2.2 旋转齐次变换 用齐次坐标变换来表示式（ 2-7） 11000 0 0 0 1 w v u z y x P P P R P P P 11000 0 0 0 1 1 z y x w v u P P P R P P P 2.2.3 三个基本旋转矩阵和合成旋转矩阵 三个基本旋转矩阵 ),( xR 即动坐标系 求 的旋转矩阵 ， 也就是 求出坐标系 中各轴单位矢量 在固定坐标系 中各轴的投影分量 ， 很容易得到在两个坐标系重合时 ， 有： 角，轴转动绕， XOvwO vwO wv kji , Oxyz ),( xR 100 010 001 R wzvzz wvyy wxvxx kkjkik kjjjij kijiii y)R ( x , x y z o u v w U V W O 图 2 - 5 ss in0 s inc o s0 001 co ii ux 方向余弦阵 同理： c o s0s in 010 s in0c o s )y,R （ 100 0c o ss in 0s in-c o s )z,R （ ss in0 s inc o s0 001 )R ( x , co 三个基本旋转矩阵 : x y z o u v w U W O x y z o u v w U W O v 合成旋转矩阵 : 例 1：在动坐标中有一固定点 ，相对固定参 考坐标系 做如下运动： R（ x, 90 ）； R(z, 90 )； R(y,90 )。求运动后点 在固定参考坐标系 下的位置。 Tu v wPo 1321 Oxyz uvwPo Oxyz 解 1：用画图的简单方法 解 2：用分步计算的方法 R（ x, 90 ） R（ z, 90 ） R（ y, 90 ） 1 2 3 1 1 3 2 1 1000 0010 01-00 0001 P 1 2 1 3 1 2 3 1 1000 0100 0001 001-0 P 1 3 1 2 1 2 1 3 1000 0001- 0010 0100 P （ 2-14） （ 2-15） （ 2-16） 上述计算方法非常繁琐，可以通过一系列计算得到上述 结果。将式（ 2-14）（ 2-15）（ 2-16）联写为如下形式： 110001 33 w v u z y x P P P R P P P R3x3为二者之间的关系矩阵，我们令： ),(),(),RR 33 xRzRy（ 定义 1： 当动坐标系 绕固定坐标系 各坐标轴顺序有限次 转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序 左乘 。 注意： 旋转矩阵间 不可以交换 uvwO Oxyz 平移齐次变换矩阵 1000 100 010 001 c) b (a T r a n sH c b a 注意： 平移矩阵间可以交换， 平移和旋转矩阵间不可以交换 z y x o o w u v a b c 2.2.4 相对变换 举例说明： 例 1： 动坐标系 0起始位置与固定参考坐标系 0重合 ,动坐标系 0做如下运动： R(Z,90) R（ y,90） Trans(4， -3, 7)， 求合成矩阵 解 1：用画图的方法： o z y x 7 4 - 3 o w u v v u w z y x o o(o ) x y z u v w z y x u w o ( o ) v 解 2：用计算的方法 根据定义 1， 我们有： 1000 7010 3001 4100 )R ( Z ,90 )90 R ( y , 7) ,3- ,T r a n s ( 4T 以上均以固定坐标系多轴为变换基准 ， 因此矩阵左乘 。 如果我们做如下变换，也可以得到相同的结果： 例 2： 先平移 Trans (4,-3,7)； 绕当前 轴转动 90； 绕当前 轴转动 90；求合成旋转矩阵 。 v w （ 2-20） 解 1：用画图的方法 z y x o( o ) v w u z y x o o w u v o z y x o w v u x y z o o w u v 解 2：用计算的方法 1000 7010 3001 4100 )R ( Z ,9 0 )90 R ( y , 7) ,3- ,T r a n s ( 4T oo （ 2-21） 式 （ 2-20） 和式 （ 2-21） 无论在形式上 ， 还是在结果上都是 一致的 。 因此我们有如下的结论： 动坐标系在固定坐标系中的齐次变换有 2种情况： 定义 1： 如果所有的变换都是 相对于固定坐标系 中各坐标轴旋 转或平移 ， 则依次 左乘 ， 称为 绝对变换 。 定义 2： 如果动坐标系 相对于自身坐标系的当前坐标轴 旋转或 平移，则齐次变换为依次 右乘 ，称为 相对变换 。 结果均为动坐标系在固定坐标中的位姿 （ 位置 +姿态 ） 。 相 对于固定坐标系 ， 轴。轴相当于轴，轴相对于轴，轴相当于 ZYX wv 也就是说，动坐标系绕自身坐标轴做齐次变换， 要达到绕固 定坐标系相等的结果，就应该用相反的顺序。 右乘的意义： 机器人用到相对变换的 时候比较多 例如机械手抓一个杯子， 如右图所示，手爪需要 转动一个角度才抓的牢， 相对于固定坐标系表达 太麻烦，可以直接根据 手爪的坐标系表示 但也要知道在 O中的位 姿，就用右乘的概念。 x y z o H 2.2.5 绕通过原点的任意轴旋转的齐次变换 有时动坐标系 O可能绕过原点 O的分量分别为 rx、 ry、 rz的 任意单位矢量 r 转动 角。 研究这种转动的好处是可用 O绕某轴 r 的一次转动代替绕 O各坐标轴的数次转动 为推导此旋转矩阵，可作下述 5步变换： 1. 绕 X 轴转 角， 使 r 轴处于 XZ平面内 2. 绕 Y 轴转 - 角，使 r 轴与 OZ轴重合 3. 绕 OZ轴转动 角 4. 绕 Y 轴转 角 5. 绕 X 轴转 - 角 X Y Z r x r y r z A B C D B O 5 1 2 43 r A 由上图容易求出： 2 z 2 y y rr rs in 2 z 2 y z rr rc o s x x r r r r OCs in 2 z 2 y 2 z 2 y rr r rr OB CBc o s 由定义 1和定义 2，上述 5次旋转的合成旋转矩阵为： c o ss i n0 s i n-c o s0 001 c o s0s i n 010 s i n-0c o s 100 0c o ss i n 0s i n-c o s c o s0s i n- 010 s i n0c o s c o ss i n0 s i nc o s0 001 RRRRRR ,x,y,z,y,x,r （ 2-25） X Y Z r x r y r z A B C D B O 5 1 2 43 r A 带入式 （ 2-25），得 c os)c os(1r s i nr)c os(1rr s i nr)c os(1rr s i nr)c os(1rrs i nr)c os(1rr c os)c os(1rs i nr)c os(1rr s i nr)c os(1rrc os)c os(1r R 2 z xzy yzx xzyyzx 2 yzyx zyx 2 x ,r 由该式可以推出 3个基本旋转矩阵 2.2.6 齐次变换矩阵的几何意义 设，有一个手爪，即动坐标系 O，已知， 初始位置 重合，那么 O在 O中的齐次坐标变换为： ，如果手爪转了一个角度， 则： 111 cbao 1000 100 010 001 T 1 1 1 1 c b a 1000 p p p T z yyy xxx zzz y x w w w T反映了 O在 O中的位置和姿态 ， 即表示了该坐标系原点 和各坐标轴单位矢量在固定坐标系中的位置和姿态 。 该矩阵可以由 4个子矩阵组成 ， 写成如下形式： 比例系数透视矩阵 位置矢量旋转矩阵 1131 1333 wf PR T zzz yyy xxx w w w 33R 为 姿态矩阵（旋转矩阵） ，表示动坐标系 O在固定参考坐标系 O中的姿态，即表示 O各坐标轴单位矢量在 O各轴上的投影 为 位置矢量矩阵 ，代表动坐标系 O坐标原点 在固定参考坐标系 O中的位置 Tzyx ppp P 13 为 透视变换矩阵 ，在视觉中进行图像计算，一 般置为 0 00031 f 为 比例系数 1 11 w 如果需要求解 O在 O中的位置和姿态，此时的齐次变换矩 阵为 ，即求逆矩阵： 1T 1000 - -R - T T T1- 33 T 1 pw pv p ）（ ）（ ）（ kpjpipp zyx kji zyx kvjvivv zyx kwjwiww zyx 其中： 这些式子以后经常遇到， 在机器人计算中，所要 求的就是齐次变换矩阵 2.2.7 透镜成像的齐次变换 p p : P x 1 P P x 1 1 () 1 pp pp T T yz yz y p zp zp f y p zp zp y p f y p f zp x p y p f zp x p y p y p f y p f y p f y p y p y p f y p y p f f xp xp y 以光心为原点O，光轴与y轴重合，P 为物点， 用齐次坐标表示 求 的齐次坐标，即求 根据三角形相似原理： 注意 是负值， 是正值，所以实际上为相减关系 又有 设 11 1 0 0 0 0 1 0 0 0 0 1 0 11 1 0 0 0 1 y p zp y p zp p y p zp f f f xp x p x p yp y p y p Tfzp zp zp yp f 用矩阵表示： z y P y p f o z p f p z P p y p p : P x 1 P P x 1 1 () 1 pp pp T T yz yz y p zp zp f y p zp zp y p f y p f zp x p y p f zp x p y p y p f y p f y p f y p y p y p f y p y p f f xp xp y 以光心为原点O，光轴与y轴重合，P 为物点， 用齐次坐标表示 求 的齐次坐标，即求 根据三角形相似原理： 注意 是负值， 是正值，所以实际上为相减关系 又有 设 11 1 0 0 0 0 1 0 0 0 0 1 0 11 1 0 0 0 1 y p zp y p zp p y p zp f f f xp x p x p yp y p y p Tfzp zp zp yp f 用矩阵表示： 因此，进行机器人运动学计算时，不能省略透视矩阵，有 摄像头时，透视矩阵为 0 - 0，没有摄像头时为 0 0 0 。 f 1 10 1 0 0100 0010 0001 T 1 T 11 p f z y x f y z y x z y x f p p p f p p p p p p 用矩阵表示： 知识点： 1. 点和面的齐次坐标和齐次变换 2. 三个基本旋转矩阵 3. 绝对变换：如果所有的变换都是相对于固定坐标系中各坐 标轴旋转或平移 ， 则依次左乘 ， 称为绝对变换 。 4. 相对变换：如果动坐标系相对于自身坐标系的当前坐标轴 旋转或平移 ， 则齐次变换为依次右乘 ， 称为相对变换 。 5. 绕任意轴旋转： 5步顺序 6. 透视变换 知识点： 三个基本旋 转矩阵 c o s0s in 010 s in0c o s )y,R （ 100 0c o ss in 0s in-c o s )z,R （ ss in0 s inc o s0 001 )R ( x , co 例题 1： O与 O初始重合， O作如下运动：绕 Z轴转动 30 ； 绕 X轴转动 60 ；绕 Y轴转动 90 。求 T。 1000 0100 0030c o s30s in 0030s in30c o s R 1 1000 060c os60s in0 060s in60c os0 0001 2 R 1000 090c o s090s in 0010 090s in090c o s 3 R 1000 002/12/3 02/34/34/1 02/14/34/3 123 RRRT 例题 2： O与 O初始重合， O作如下运动：绕 X轴转动 90；绕 w轴转动 90；绕 Y轴转动 90。求 T；改变旋转顺序，如何 旋转才能获得相同的结果。 1000 090c os90s in0 090s in-09c os0 0001 R 1 1000 0100 0090c o s90s in 0090s in90c o s 2 R 1000 090c o s090s in 0010 090s in090c o s 3 R 1000 0010 0100 0001 213 RRRT 解： 解 ： 绕 Z（ w）轴转动 90； 绕 X轴转动 90； 绕 Y轴转动 90。 例题 3： 矢量 在 O中表示为 ， O相对于 O的 奇次变换为： P kjip 2230 1000 1100 20001 10010 T oo 中的矢量在求此时 ，轴平移的沿轴转的绕当 中的矢量在求 的位置和姿态在画出：求 0 20X0 90Y0 0)3 0 2) 00 1) O 00 p pp 解： 1） z x y u w v o o 解： 2） 1 3 23 8 T 00 pp o 解： 3） ， ， 1000 090c o s090s in 0010 090s in090c o s R 1 1000 0100 0010 20001 T 1r 1000 10010 20001 21100 TRT 1 1 or T 1 8 23 23 1 2 2 3 1000 10010 20001 21101 00 pTp 例题 4： 如图所示， 1）写出 、 、 、 ； 2）求 1T 21T 32T 43T 40 T 1000 11-00 301-0 3.5-001 T 1 1000 101-0 3001- 0100 T 21 1000 0001 5 5 3 5 4 -0 0 5 4 5 3 0 T 32 1000 01-00 0010 3.5001- T 43 解： 1） o 0 x 0 y 0 z 4 3 3 . 5 1 1 o 1 x 1 y 1 z 2 o 2 x 2 y 2 z 3 o 3 x 3 y 3 z 4 o 4 x 4 y 4 z 解 2） ：根据定义 2， 绕自身旋转 ， 右乘 1000 50.6-0.8-0 00.8-0.60 0001- T T T TT 433221140 习题 1： O与 O初始重合， O作如下运动：绕 z轴转动 90；绕 v轴转动 90；绕 x轴转动 90。求 T；改变旋转顺序，如 何旋转才能获得相同的结果。 习题 2： 已知齐次变换矩阵 要求 R（ f,) , 求 f和 值 1000 0001 0100 0010 H 第三章 机器人运动学 机器人运动学主要是把机器人 相对于固定参考 系 的运动作为 时间的函数 进行分析研究，而不 考虑引起这些运动的力和力矩 也就是要把机器人的 空间位移 解析地表示为 时 间的函数 ，特别是研究机器人 关节变量空间和 机器人末端执行器位置和姿态之间 的关系 本章将讨论机器人运动学几个具有实际意义的 基本问题。 3.1 机器人运动学所讨论的问题 3.1.1 研究的对象 机器人在基本机构形式上分为两种，一种是关节式串 联机器人，另外一种是并联机器人，如图： PUMA560 Hexapod Fanuc manipulator 1972 Victor Scheinman在 Unimation公司为通用； 1980Westinghouse收购； 1988Stubli收购； Nokia Robotics在 80年代卖出 1500余台 PUMA系统； Nokia的 Robotics division1990年卖出。 运动学研究的问题 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 运动学正问题 运动学逆问题 研究的问题 : 运动学正问题 -已知杆件几何参数和关节角矢量 ， 求操 作机末端执行器相对于固定参考作标的位置和姿态 （ 齐 次变换问题 ） 。 运动学逆问题 -已知操作机杆件的几何参数 ， 给定操作 机末端执行器相对于参考坐标系的期望位置和姿态 （ 位 姿 ） ， 操作机能否使其末端执行器达到这个预期的位姿 ？ 如能达到 ， 那么操作机有几种不同形态可以满足同样的 条件 ？ 运动学正问题关节角 杆件参数 末端执行器 运动学正问题关节角 杆件参数 逆 3.2 机器人杆件，关节和它们的参数 3.2.1 杆件 ， 关节 操作机由一串用转动或平移（棱 柱形）关节连接的刚体（杆件） 组成 每一对关节杆件构成一个 关节 自由度 ，因此 N个自由度的操作 机就有 N对关节 -杆件。 0号杆件（一般不把它当作机器 人的一部分）固联在机座上，通 常在这里建立一个固定参考坐标 系，最后一个杆件与工具相连 关节和杆件均由底座向外顺序排 列，每个杆件最多和另外两个杆 件相联，不构成闭环。 关 节 杆 件 末端操作手 机座 两自 由度 关节： 一般说来，两个杆件间是用 低付 相联的 只可能有 6种低付关节： 旋转 （转动）、 棱柱 （移动）、 圆柱形 、 球形 、 螺旋 和 平面 ，其中只有 旋转和棱柱形 关 节是串联机器人操作机常见的，各种低副形状如下图所 示： 旋转 棱柱形 柱形 球形 螺旋形 平面 3.2.2 杆件参数的设定 条件 关节串联 每个杆件 最多 与 2个杆件相连 ， 如 Ai与 Ai-1和 Ai+1相连 。 第 i 关节的关节轴 Ai 位于 2个杆 件相连接处 ， 如图所示 ， i-1关节和 i+1关节 也各有一个关节轴 Ai-1 和 Ai+1。 Ai Ai+1 Ai-1 杆件参数的定义 和 li 关节 Ai轴和 Ai+1轴线 公法线的长度 关节 i轴线与 i+1轴线 在垂直于 li 平面内的夹 角，有方向性 ,由 Ai转 向 Ai+1， 由右手定则决 定正负 由运动学的观点来看 ， 杆件的作用仅在于它能保 持其两端关节间的 结构形态 不变 。 这种形态由两 个参数决定 ， 一是杆件的长度 li， 一个是杆件的 扭转角 i Ai Ai+1 i il i il i 杆件参数的定义 和 Li和 Li-1在 Ai轴线上 的交点之间的距离 Li和 Li-1之间的夹 角，由 Li-1转向 Li，由 右手定则决定正负， 对于旋转关节它是个 变量 确定杆件 相对位置关系 ， 由另外 2个参数决定 ， 一个是杆 件的偏移量 ， 一个是杆件的回转角 iid i id i Ai Ai+1 i il id 1il i Ai-1 id 移动关节杆件参数的定义 确定杆件的结构形态的 2个参数 Li与 i与旋转关节是一样的。 确定杆件相对位置关系的 2个参数则相反。这里 i为常数， di为变量。 上述 4个参数，就确定了杆件的结构形态和相邻杆件相对位 置关系，在转动关节中， Li, i, di是固定值， i是变量。在 移动关节中， Li, i, i是固定值， di 是变量。 3.3 机器人关节坐标系的建立 oO o n-1nX 对于每个杆件都可以在关节轴处建立一个正规的笛卡儿 坐标系（ xi, yi, zi），（ i=1, 2, , n ）， n是自由度数，再 加上基座坐标系，一共有（ n+1）个坐标系。 基座坐标系 定义为 0号坐标系（ x0, y0, z0） ,它也是机 器人的惯性坐标系， 0号坐标系在基座上的位置和方向可 任选，但 轴线必须与关节 1的轴线重合，位置和方向可 任选； 最后一个坐标系（ n关节），可以设在手的任意部位，但 必须保证 与 垂直。 机器人关节坐标系的建立主要是为了描述机器人各杆件和终 端之间的相对运动，对建立运动方程和动力学研究是基础性 的工作。 为了描述机器人各杆件和终端之间转动或移动关系， Denavit 和 Hartenberg于 1955年提出了一种为运动链中每个杆件建立 附体坐标系的矩阵方法 （ D-H方法） ，建立原则如下： D-H关节坐标系建立原则 右手坐标系 原点 Oi：设在 Li与 Ai+1轴线的交点上 Zi轴 ： 与 Ai+1关节轴重合，指向任意 Xi轴 ： 与公法线 Li重合，指向沿 Li由 Ai轴线指向 Ai+1轴线 Yi轴 ： 按右手定则 关节坐标系的建立原则 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 原点 Oi：设在 Li与 Ai+1轴线的交点上 Zi轴：与 Ai+1关节轴 重合，指向任意 Xi轴：与公法线 Li 重合，指向沿 Li由 Ai轴线指向 Ai+1轴线 Yi轴：按右手定则 杆件长度 Li 沿 xi 轴 ， zi-1 轴与 xi 轴交点到 0i 的距离 杆件扭转角 i 绕 xi 轴 ， 由 zi-1 转向 zi 杆件偏移量 di 沿 zi-1 轴 ， zi-1 轴和 xi 交点至 0i 1 坐标系原点的距离 杆件回转角 i 绕 zi-1 轴 ， 由 xi-1转向 xi 两种特殊情况 两轴相交，怎么建立坐 标系？ 0iAi与 Ai+1关节轴线的交 点； ZiAi+1轴线； XiZi和 Zi-1构成的平面的 法线 ； Yi右手定则； i-1 i A i A i + 1 o i z i - 1 z i x i y i 两轴平行，怎么建立坐标系 (Ai与 Ai+1平行 )？ 先建立 0i-1 然后建立 0i+1 最后建立 0i i-1O D 注意： 由于 Ai和 Ai+1平行 ， 所以公法线任意点 在 A点位置； 按照先前的定义 ， di为 Oi-1点和 A点之间的距离 ， di+1为 B点和 C点间 的距离 ， 这样设定可以的 ， 但我们可以变更一下 ， 将 0i点放在 C点 ， 定义 Oi在 Li+1和 Ai+1轴的交点上 ， 这样使 di+1=0使计算简便 ， 此时 di= A i - 1 A i A i + 1 A i + 2 l i - 1 o i - 1 x i - 1 y i - 1 z i - 1 A B D C o i （ x i ） （ y i ） z i x i y i o i + 1 x i + 1 y i + 1 z i + 1 d i + 1 l i + 1 d i 相邻 关节坐标系间的齐次变换过程 机器人运动学正解 1. 将 xi-1轴绕 zi-1 轴转 i 角度，将其与 xi轴平行； 2. 沿 zi-1轴平移距离 di ， 使 xi-1 轴与 xi 轴重合； 3. 沿 xi 轴平移距离 Li， 使两坐标系原点及 x轴 重合； 4. 绕 xi 轴转 i 角度，两 坐标系完全重合 ),(),(),(),(A 111 iiiir a n siir a n siiii xRlxTdZTZR 根据上述坐标系建立原则，用下列旋转和位移我们 可以建立相邻的 Oi-1 和 Oi 坐标系之间的关系 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 机器人的运动学正解方程 0 0 1 1 12 i iiT A A A D-H变换矩阵 ii A1 1000 100 0010 0001 id 100 0100 00c oss in 0s inc os ii ii 1000 0100 0010 001 il 1000 0c oss in0 0s inc os0 0001 ii ii 1000 c oss i n0 s i nc oss i nc osc oss i n c oss i ns i ns i nc osc os iii iiiiiii iiiiiii d l l = = 机械手的坐标变换图如图所示，机械手的末端（即连杆坐标系 i） 相对于基座坐标系 0的描述用 oTi 表示，即： 0 z A1 A2 A3 A4 A5 A6 0 E X 0T6 1T6 2T6 3T6 4T6 5T6 机械手的坐标变换图 机器人的运动学正解方程 0 0 1 1 12 i iiT A A A 举例： Stanford机器人 A1 A2 A3 A4 A5 A6 d1 z1 x1 y 1 O1 d2 z2 x2 y2 O2 z3 y3 x3 O3 y4 z4 x4 O4 z5 y5 x5 O5 345 45 , 0 o o o dd 重 合 d3 z6 x6 y6 O6 d6 z0 y0 x0 O0 为右手坐标系 原点 Oi： Ai与 Ai+1 关节轴线的交点 Zi轴：与 Ai+1关节轴 重合，指向任意 Xi轴： Zi和 Zi-1构成 的面的法线 Yi轴：按右手定则 Li 沿 xi 轴 ， zi-1 轴与 xi 轴交点到 0i 的距离 i 绕 xi 轴 ， 由 zi-1 转向 zi di 沿 zi-1 轴 ， zi-1 轴和 xi 交点至 0i 1 坐标系原 点的距离 i 绕 zi-1 轴 ， 由 xi-1转向 xi 解： 3.4 例题 试求立方体中心在机座坐标系 0中的位置 该手爪从上方把物体抓起，同时手爪的开合方向与物体的 Y轴同向， 那么，求手爪相对于 0的姿态是什么？ 在机器人工作台上加装一电视摄像机，摄像机可见到固联 着 6DOF关节机器人的机座坐标系原点，它也可以见到被操作 物体（立方体）的中心，如果在物体中心建一局部坐标系，则 摄像机所见到的这个物体可由齐次变换矩阵 T1来表示，如果摄 像机所见到的机座坐标系为矩阵 T2表示。 1000 101-00 2001-0 10-001 T 1000 91-00 10001 1010 T 21 x y z 解 1： T T 21 物机机摄物摄 求，已知 TTT TT 11-2 ）（有： 物摄摄机物机 TTT 1000 91-00 10001 1010 1000 101-00 2001-0 10001 1000 1100 10001- 11010 O 物 根据 T 1 画出 O 机 根据 T 2 画出 因此物体位于机座坐标系的（ 11， 10， 1） T 处，它的 X， Y， Z轴分别与机座坐标系的 -Y， X， Z轴平行。 x y z y 机 z 物 y 物 x 物 z 机 o O 机 O 物 解 2： 向重合手爪开合方向与物体 ya : Ts 001有 方向相反方向物体的从上向下抓，指出手爪 zab : Ta 100 则有 Tkji kji asnc 01000 100 001: 1-00 001 010 因此：姿态矩阵为 重合时 与物体中心 当手爪中心 1000 11-00 10001 11010 T 物 机 O s n a y z x X机 手爪 机实际要求 T pzazsznz pyaysyny pxaxsxnx 1000 工作空间 工作空间 : 末端操作手可以到达的空间位置集合 如何获得工作空间 : 利用正运动学模型 ,改变关节 变量值 可达空间 : 末端操作手可以至少以一个姿态到达的 空间位置集合 灵活空间 : 末端操作手可以以任何姿态到达的空间 位置集合 如何确定可达空间 ?首先， 令 3变 化 示例 : 平面 3连杆机器人 l2 l3 l1 1 2 3 1 1 2 1 2 3 1 2 3 1 1 2 1 2 3 1 2 3 1 2 3 1 2 3 1 2 3 c os c os c os si n si n si n , x l l l y l l l l l l l l l 然后 2变化 最终， 变化 1 3.5 机器人末端操作器位姿的其它 描述方法 用矩阵表示刚性体的转动简化了许多运算， 但它需要 9个元素来完全描述旋转刚体的姿 态，因此矩阵并不直接得出一组完备的广 义坐标。 一组广义坐标应能描述转动刚体相对于参 考坐标的方向，被称为欧拉角的三个角度， 、 、 就是这种广义坐标。 有几种不同的欧拉角表示方法，它们均可 描述刚体相对于固定参考系的姿态。三种 最常见的欧拉角类型列在表中 3种最常见的欧拉角类型 步 1 步 2 步 3 类型 1 绕 OZ轴转 角 绕当前 OU 轴转 角 绕当前 OW轴转 角 类型 2 绕 OZ轴转 角 绕当前 OV 轴转 角 绕当前 OW轴转 角 类型 3 绕 OX轴转 角 绕 OY轴转 角 绕 OZ轴转 角 u v w x(u) y (v) z (w) o u v w u v W ),( ZR ),( R ),( wR N0 T 100 0 0 0 0 001 100 0 0 cs sc cs sccs sc ccsss sccccssscccs ssccsscscscc 类型 1：表示法通常用于陀螺运动 类型 2： 所得的转动矩阵为右乘 100 0 0 c0s- 010 s0c 100 0 0 ),(),v(),(R cs sc cs sc wRRZR 1000 pz pyR px T ccsss ssccscsscccs sccssccssccc 类型 2 绕 OZ轴转 角 绕当前 OV 轴转 角 绕当前 OW轴转 角 类型 3：一般称此转动的欧拉角 为偏航角、俯仰和横滚， （这 种方法也叫做 偏航、俯仰和横滚 角表示方法）这种形 式主要用 于航空工程中分析飞行器的运动， 其旋转矩阵为 ccscs sccssccssscs sscsccsssccc cs sc cs sc cs sc xRyRz 0 0 001 0 010 0 100 0 0 ),(),(),RR （ 类型 3 绕 OX轴转 角 绕 OY轴转 角 绕 OZ轴转 角 正运动学问题 : 已知关节角度或位移，计算 末端操作手的对应位姿 . 逆运动学问题 : 已知 末端操作手的位姿，求 解对应的关节变量 . 为 什么逆运动学问题更困难 ? 可能存在多解或无解 通常需多次求解非线性超越方程 3.6 运动学逆问题 解的存在性 目标 点应位于工作空间内 可能存在多解，如何选择最合适的解？ 存在双解 ! 求解方法 如果各关节可用某算法获得，一个机械手是 有解的 . 算法应包含所有可能解 . 封闭形式解（解析解） 数值解 方法 我们对封闭形式的解法更感兴趣 代数方法 几何方法 可解性的重要结论是： 所有具有转动和移动关节的系统，在一个单一串联中 总共有 6个（或小于 6个）自由度时，是可解的，其通 解一般是数值解，它不是解析表达式，而是利用数值 迭代原理求解，它的计算量要比解析解大。 但在某些特殊情况下，如若干个关节轴线相交和或多 个关节轴线等于 0 或 90 的情况下，具有 6个自由度 的机器人可得到解析解。 为使机器人有解析解，一般设计时，使工业机器人足 够简单，尽量满足这些特殊条件。 对于给定的机器人 ， 能否求得它的运动学逆解的解析式 （ 也叫封闭解 ） 。 运动学逆问题的可解性 运动学逆问题的多解性 机器人运动问题为解三角方程 ， 解反三角函数方程时会 产生多解 .显然对于真实的机器人 ， 只有一组解与实际情 况最相对应 ， 因此必须作出判断 ， 以选择合适的解 。 通常采用如下方法剔除多余解： 01 40i 0002 2 2 01 8 040 i 若该关节运动空间为 ，则应选 。 100 040i 1 根据关节运动空间选取合适的解 。 例如求得机器 人某关节角的两个解为 2 选择一个与前一采样时间最接近的解 ， 例如： 01 40i 0002 2 2 01 8 040 i 若该关节运动空间为 ， 且 ， 则应选 250 01 160i 0220i 3 根据避障要求 ， 选择合适的解 4 逐级剔除多余解 对于具有 n个关节的机器人 ， 其全部解将构成树形结构 。 为简化起见 ， 应逐级剔除多余解 。 这样可以避免在树形解中 选择合适的解 。 迭代法 计算量大 几何法 适用于自由度较少的情况 反变换法 运动学逆问题解法 用未知的逆变换逐次左乘，由乘得的矩阵方程 的元素决定未知数，即用逆变换把一个未知数 由矩阵方程的右边移到左边 考察方程式左、右两端对应元素相等，以产生 一个有效方程式，理论上可得到 12个方程。 然后求这个三角函数方程式，以求解未知数 把下一个未知数移到左边 重复上述过程，直到解出所有解 缺点：无法由数种可能的解中直接得出合适的解， 需要通过人为的选择 运动学逆问题解法 反变换法 Paul 等人提出的方法 (1981年，也叫求逆的方 法，是解析解