机器人技术基础 课件 第3章 机器人运动学分析

第3章

机器人运动学分析机器人技术基础图为一个6轴机械臂，末端安装了一个夹爪。工作台上放了一块方形的物料，机械臂拟将该物料抓起，并将其放到左侧的传送带上。问题导入第2章我们学过，机械臂的每一个关节上都安装有电机，我们可以通过电机来控制各个关节的转角。如果已知各关节的转角，如何确定机器人末端所处的位置和姿态？或者反过来问，为了保证夹爪水平地抓起该物料，机械臂的6个关节分别应该转多少角度？问题导入机器人运动学主要研究的就是机器人末端执行器（夹爪）位姿与关节变量之间的关系。在正式进入本章的学习之前，请先思考一下，如果让你来回答上面两个问题，你的大致思路是什么？问题导入本章学习目标1.理解平移变换、旋转变换和复合变换的定义和特点；2.理解齐次坐标变换矩阵的构成及其代表的含义；3.能够针对不同情况合理选择齐次变换的左乘或右乘；4.能够为串联机器人建立D-H坐标系并确定其D-H参数；5.能够根据给定关节变量计算机器人的末端位姿，即会求运动学正解；6.理解逆向运动学求解思路；7.了解反解的存在性、反解的个数与机械臂结构之间的关系；8.熟悉利用MATLAB建立仿真机械臂并求运动学正逆解的基本方法。目录contants3.1机器人位姿和坐标变换3.2串联机器人位姿分析3.3串联机器人运动学分析第3章

机器人运动学分析3.1机器人位姿和坐标变换3.1.1位置与姿态3.1.2坐标变换3.1.3齐次坐标变换3.1.4左乘与右乘规则3.1机器人位姿和坐标变换机器人的机座一般是固定的，而各个关节均可转动或移动。不论哪个关节运动，都可能影响其末端执行器的位置和姿态，从而改变其相对工件的空间关系。为了清晰描述机械臂在空间的运动及其与工件之间的空间关系，必须建立合适的坐标系。固定的机器人机座适合用静坐标系来描述，而各机械臂各连杆是运动的，就需要建立固连于其上的运动坐标系，该坐标系的位置和方向随运动构件的运动而变化。3.1.1位置与姿态要完全确定一个物体在3维空间中的状态，需要有3个位置自由度和3个姿态自由度，前者用于描述物体的位置，后者用于描述物体的指向，即姿态。我们将物体6个自由度的状态称为物体的位姿。机械臂的形态各异，且由多个运动关节构成，如何才能跳出具体的形态，找到普遍适用且形式统一的位姿的表达方式？这需要引入坐标系和坐标变换概念。第3章

机器人运动学分析3.1机器人位姿和坐标变换3.1.1位置与姿态3.1.2坐标变换3.1.3齐次坐标变换3.1.4左乘与右乘规则3.1.2坐标变换图示机械臂可沿竖直轨道上下移动。图(a)展示了机械臂的初始位置，坐标系{A}固连于机座上，图(b)中坐标系{B}固连于水平机械臂上与其一起运动，机械臂上移一段距离，夹爪中心点则随之从P移动到P’的位置。如果已知P在{A}坐标系的的坐标，那么P’点在{A}中的坐标该如何求得？这就是平移变换要解决的问题。（1）平移变换(a)初始位姿

(b)平移后位姿机械臂的平移3.1.2坐标变换平移变换的特点是，坐标系在空间的姿态不变，只是两坐标系原点发生变化。上图中的平移仅在Y轴方向上发生，我们可以将其扩展到X,Y,Z三个方向上，如右图所示，根据向量加法的定义有（1）平移变换平移变换示意图3.1.2坐标变换（1）平移变换平移变换示意图上式也可以表达为以下两种形式P’点在B坐标系的位置矢量和坐标OB点在A坐标系的位置矢量和坐标P’点在A坐标系的位置矢量和坐标3.1.2坐标变换如图所示，机械臂可以绕Z轴旋转。图(1)展示了机械臂的初始位置，坐标系{A}固连于机座上。图(b)中坐标系{B}固连于旋转机械臂上与其一起运动，机械臂旋转θ角后，机械臂末端一点P随之转到了P’点。那么P’点在{A}中的坐标该如何求得？这就是旋转变换要解决的问题。（2）旋转变换（a）初始位姿

（b）旋转后位姿机械臂的旋转3.1.2坐标变换旋转变换的特点是坐标原点的位置不变，但坐标轴的方向发生改变。右图清晰展示了旋转后的矢量OP’相对{A}和{B}两个坐标系的几何关系，通过简单的三角运算可以得到P’点相对于{A}的坐标。（2）旋转变换旋转变换示意图3.1.2坐标变换（2）旋转变换旋转变换示意图写成紧凑的矩阵形式，则有或其中R为旋转矩阵。3.1.2坐标变换（2）旋转变换同理可以推导出绕X轴和Y轴的旋转矩阵。请观察绕X,Y,Z轴的旋转的异同。XYZXYZXYZ3.1.2坐标变换（2）旋转变换下面我们来分析旋转矩阵R特点。以Rz为例，将其三列分别用矢量n，o，a来表达，则有noa计算这三个向量的模可见，n，o，a均为单位向量。3.1.2坐标变换（2）旋转变换再计算其点积n，o，a中任意两向量点积为0，说明它们两两正交，其所构成的矩阵R则为正交矩阵。正交矩阵有个非常好的性质：R-1=RT，即其逆矩阵即为其转置矩阵，这个特点可以极大地方便旋转矩阵的求逆运算。3.1.2坐标变换（2）旋转变换思考：下式中，旋转矩阵R有9个变量，其中独立变量有几个？为什么？3.1.2坐标变换如图所示，机械臂首先沿竖直轨道上移，然后又绕其转轴旋转了θ角，此时机械爪中心点P在{A}中的位置矢量

该如何求得？这就是复合变换要解决的问题，复合变换的特点是两坐标原点的位置和坐标轴的方向均发生改变。（3）复合变换（a）初始位姿

（b）平移后位姿

（c）旋转后位姿3.1.2坐标变换复合变换可以分解为平移和旋转变换加以解决，首先通过平移公式求出坐标原点OB相对于{A}的坐标，再通过旋转变换公式求出P’’相对过渡坐标系X'Y'Z'的坐标，二者相加即可，所以有：（3）复合变换（a）初始位姿

（b）平移后位姿

（c）旋转后位姿3.1.2坐标变换（3）复合变换绕Z轴旋转则可具体写为或展开为

第3章

机器人运动学分析3.1机器人位姿和坐标变换3.1.1位置与姿态3.1.2坐标变换3.1.3齐次坐标变换3.1.4左乘与右乘规则3.1.3齐次坐标变换上一节通过简单案例展现了空间3维矢量的平移、旋转以及复合变换，其中平移变换用3维矢量加法表达，旋转变换用3维矩阵乘法表达，而复合变换则是两者的和。机器人往往有多个关节，必然包含多个运动坐标系，涉及的坐标变换会十分复杂，如果每一次坐标变换还要讨论用加法还是用乘法就非常麻烦。那么，是否可能为平移、旋转和复合等不同的坐标变换找到统一的数学表达呢？3.1.3齐次坐标变换爱因斯坦说：“同一层面的问题，不可能在同一层面解决，只有在高于它的层面才能解决。”于是齐次坐标闪亮登场！所谓齐次坐标，就是将原本的n维矢量用n+1维矢量表示，它可以将代表平移的矢量加法和代表旋转的矩阵乘法巧妙整合为统一的矩阵乘积形式。3.1.3齐次坐标变换空间3维矢量的齐次坐标则为4维矢量，相应的，代表其坐标变换的矩阵维数也由3×3变成4×4。请观察下面的矩阵运算：上式中的4×4矩阵称为齐次变换矩阵，将其展开则有式中前三个公式与上一节讲到的复合变换式完全一致，第四个公式为恒等式可以忽略。3.1.3齐次坐标变换该式也可以简单写为：P点在坐标系{A}中的坐标P点在坐标系{B}中的坐标齐次变换矩阵齐次变换矩阵T分为四大块，其中左上角的代表旋转变换，右上角的代表平移变换，左下1×3恒为[000]，右下恒为1。即3.1.3齐次坐标变换下面我们验证一下平移变换是否可以用齐次变换矩阵变换表达。对于单纯平移变换，三个坐标轴方向均不变，因此其对应的旋转矩阵R应该是3×3的单位阵，其右上角3×1向量仍为平移向量，所以很明显，该式的前三个公式与上一节讲到的平移变换式完全一致。3.1.3齐次坐标变换我们再来验证一下旋转变换是否可以用齐次变换矩阵变换表达。仍假设坐标系{B}绕坐标系{A}的Z轴旋转θ角，因此其对应的旋转矩阵是又因该变换只有转动没有平移，所以{B}坐标系原点OB在{A}坐标系下的坐标仍为

[000]T，于是该变换可以表达为很明显，该式的前三个公式与上一节讲到的旋转变换公式完全一致。3.1.3齐次坐标变换思考：4×4的齐次变换矩阵T共有16个元素，其中独立的有几个？从上面的分析可以看到，一个齐次变换矩阵中同时包含了所有平移、旋转和复合变换的信息，因此它可以完全定义两个坐标系之间的空间关系。3.1.3齐次坐标变换在机器人运动学分析中，齐次变换矩阵是一个非常重要而有效的数学工具，因此我们必须熟练掌握。为了帮助大家从不同角度理解变换矩阵T，我们先来讲一个看似无关的有趣话题。一位中国留学生遍历意大利之后大为震撼，对他的意大利教授感叹道：“意大利随处可见几百年前的古典建筑，而在中国这是非常少见的，这说明意大利的传统文化得到了更好地保护和传承。”教授想了想说：“我并不同意的你的观点，中国古代建筑通常是木制的，相比意大利的石制建筑确实不易保存。但中国的文化是通过独特的汉字来传承的，绝大部分今天的中国人仍能欣赏千年前的诗词歌赋，而如今能读懂拉丁语的意大利人却是凤毛麟角。”3.1.3齐次坐标变换确实，中华文字博大精深且精妙绝伦，我们从文字的多义性中简单感受一下。食，作名词指食物，民以食为天；作动词指吃，何不食肉糜。树，作名词指木，树欲静而风不止；作动词指栽培，十年树木百年树人。其实，数学也有类似的美妙之处，比如齐次变换矩阵T，我们也可以认为它有不同的词性。T作“名词”，代表一个坐标系，其中包含三个坐标轴的方向信息（R）和原点坐标信息（P）。T作“动词”，代表一个坐标系到另一个坐标系所经历的变换，其中包含旋转变换信息（R）和平移变换信息（P）。3.1.3齐次坐标变换下面几道关于齐次变换矩阵例题帮助我们理解T的内涵，注意体会将T看作“名词”或“动词”带来的不同感悟。例3.1已知{A}坐标系经过某种几何变换后成为{B}坐标系，对应的齐次变换矩阵T如下，请说明从{A}到{B}经过了何种变换，并将{B}坐标系绘制出来。例3.13.1.3齐次坐标变换旋转R：绕XA轴逆时针旋转300平移P：{B}的原点从点[000]移动到了点[021]例3.13.1.3齐次坐标变换例3.1拓展练习：借助装有RoboticsToolbox工具箱的MATLAB软件可以非常方便地绘制齐次变换矩阵对应的坐标系。读者可以尝试自己编写代码，或者扫描二维码获得该程序及其讲解。3.1.3齐次坐标变换例3.2已知{A}坐标系沿XA轴平移5个单位再绕YA轴旋转300再后成为{B}坐标系，请写出相应的齐次坐标变换矩阵。已知{B}坐标系中一点坐标为[123]T，求该点在{A}坐标系中的坐标。例3.2

所以齐次变换矩阵为3.1.3齐次坐标变换例3.2

已知{B}坐标系中一点的坐标为[123]T，则其在{A}坐标系中的坐标为3.1.3齐次坐标变换例3.2

拓展练习：借助装有RoboticsToolbox工具箱的MATLAB软件，完成该题非常简单，读者可以尝试自己编写代码，或者扫描二维码获得该程序及其讲解。3.1.3齐次坐标变换例3.3坐标系{A}和{B}的关系如例3.2所述，已知{A}坐标系下一点的坐标为[123]T，求其在{B}坐标系下的坐标。例3.3

现在是已知左侧，求右侧。很容易求得T的行列式为1≠0，所以存在。两边同时左乘T-1则有解：在例3.2中我们已经求得T，且有下列公式3.1.3齐次坐标变换思考：例3.3的关键在于求出T-1，而逆矩阵的求解比较复杂。请仔细观察T和T-1这两个矩阵，考虑是否有简单巧妙的办法求出？例3.3

T3.1.3齐次坐标变换例3.3

拓展练习：借助装有RoboticsToolbox工具箱的MATLAB软件，完成该题非常简单，读者可以尝试自己编写代码，或者扫描二维码获得该程序及其讲解。3.1.3齐次坐标变换例3.4已知{A}坐标系经过旋转变换后成为{B}坐标系（无平移），{A}坐标系的基向量为i，j，k，{B}坐标系的基向量为n，o，a。如图所示，n与i，j，k的夹角分别为αn，βn，γn，o与i，j，k的夹角分别为αo，βo，γo，a与i，j，k的夹角分别为αa，βa，γa。（1）请写出坐标系{A}到{B}的齐次变换矩阵T。（2）已知点P在{B}坐标系的坐标为[123]，求该点在{A}坐标系的坐标（写出公式即可）。例3.4

3.1.3齐次坐标变换解：（1）旋转矩阵R三列代表{B}坐标系的基向量n，o，a在XA，YA，ZA轴上的分量，所以只要把这些分量计算出来即可得到旋转矩阵R。已知n，o，a与i，j，k的夹角，所以容易求得向量n在XA轴上的分量：nx=cos(αn)向量n在YA轴上的分量：ny=cos(βn)向量n在ZA轴上的分量：nz=cos(γn)因此旋转矩阵R的第一列为：例3.4

同理3.1.3齐次坐标变换又因{A}到{B}没有平移，所以代表平移变换的例3.4

综上，齐次变换矩阵（2）点P在{A}坐标系下的坐标为3.1.3齐次坐标变换思考：一个向量的三个方向余弦指该向量与三个坐标轴之间的角度的余弦，请问这三个方向余弦有几个是独立的？例3.4

第3章

机器人运动学分析3.1机器人位姿和坐标变换3.1.1位置与姿态3.1.2坐标变换3.1.3齐次坐标变换3.1.4左乘与右乘规则3.1.4左乘与右乘规则3.1.4左乘与右乘规则3.1.4左乘与右乘规则包括绕Z轴旋转900，沿X轴平移10个单位。那么问题来了，该变换是相对哪个坐标系进行的？

3.1.4左乘与右乘规则

坐标系{1}相对当前坐标系做变换坐标系{1}相对固定坐标系做变换3.1.4左乘与右乘规则可见，右乘结果与坐标系{1}相对当前坐标系做变换结果一致；左乘结果与坐标系{1}相对固定坐标系做变换结果一致；3.1.4左乘与右乘规则总结以上分析我们可以得到下面非常重要的结论：3.1.4左乘与右乘规则拓展练习：借助装有RoboticsToolbox工具箱的MATLAB软件，可以非常方便地绘制出以上坐标系，并通过图形窗口提供的旋转功能从不同角度观察坐标系之间的关系。3.1.4左乘与右乘规则思考：如果T1和T2均只包含平移没有旋转，T1，T2左乘和右乘的结果是否一致？3.1.4左乘与右乘规则例3.5下图所示为平面两连杆机械臂，两杆长均为1。图中坐标系{0}为参考坐标系，杆1绕垂直于纸面向外的Z0轴旋转，其末端固连坐标系{1}随杆1一起转动，杆1转角为θ1，杆2的转轴固连于杆1末端，杆2末端固连坐标系{2}随杆2一起转动，杆2相对于杆1的转角为θ2。求杆2相对于参考坐标系的位姿。例3.5

3.1.4左乘与右乘规则例3.5

3.1.4左乘与右乘规则例3.5

杆2的姿态：坐标系{2}的方向相对Z0轴旋转了杆2的位置：坐标系{2}的原点O2在参考坐标系中的坐标3.1.4左乘与右乘规则例3.5

拓展练习：借助装有符号工具箱的MATLAB软件，可以帮助我们完成以上推导过程。读者可以尝试自己编写代码，或者扫描二维码获得该程序及其讲解。目录contants3.1机器人位姿和坐标变换3.2串联机器人位姿分析3.3串联机器人运动学分析3.2串联机器人位姿分析串联机器人由多根连杆通过关节首尾相连而成，各关节或能转动或能移动。要研究每根连杆的位姿，首先要建立固连于各杆并随其运动的坐标系，然后借助齐次变换矩阵这个强大的数学工具，就能计算出各杆相对于参考坐标系的位姿。每根连杆都有6个自由度，其中3个确定位置，3个确定姿态。因此我们自然可以想到，连杆坐标系的参数也应该有6个。然而，Denavit和Hartenberg两位美国教授于1956年提出，仅用4个参数就可以清晰而明确地表达连杆的位姿，这种坐标系构建方法就以其姓氏首字母命名，称为D-H坐标系或D-H参数法。那么问题来了：明明有6个自由度，怎么可能只用4个参数来表达？3.2串联机器人位姿分析众所周知，两点决定一条直线，而空间一点有(X,Y,Z)3个参数，因此确定一条直线共需6个参数。然而，如果要求该直线与另一条直线垂直，那么只需要确定另外一点的3个参数即可唯一地确定这条直线。虽然感觉上只用到了3个参数，但另外3个自由度其实是被垂直这个几何约束完全确定了。(x1,y1,z1)(x2,y2,z2)(x3,y3,z3)3.2串联机器人位姿分析D-H坐标系用4个参数确定6个自由度的道理与此类似，采用该法确定各连杆坐标系时，相邻坐标系的坐标轴之间有严格的几何约束关系，而绝非任意的。由于D-H参数法将坐标系参数由6个减少到4个，简化了机器人坐标系的表达，大大减少了机器人位姿分析的计算量，因此在全世界得到广泛使用，该方法称为标准D-H（StandardD-H）参数法，后来在此基础上还发展出修正D-H参数（ModifiedD-H）法，二者大同小异。本书以标准D-H坐标系及其参数为基础进行机器人的位姿分析和运动学分析。第3章

机器人运动学分析3.2串联机器人位姿分析3.2.1坐标系的建立3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.3运动方程3.2.1坐标系的建立串联机器人可以看作一系列连杆通过关节串联而成的运动链。绝大部分工业机器人的关节均为转动关节，也有部分机器人具有移动关节。下面以转动关节为例介绍D-H坐标系的建立过程。（1）连杆坐标系的确定3.2.1坐标系的建立①连杆和关节的编号为了便于分析和研究，首先按照从机座到末端执行器的顺序，由低到高依次为各关节和连杆编号。固定机座记为连杆0，与机座相连的为杆1，与杆1相连的为杆2，依次类推，直到最后一根连杆N。机座与杆1的关节编号为关节1，杆1与杆2的关节编号为关节2，依次类推，直到最后一个关节N。然后画出各关节的轴线延长线。（1）连杆坐标系的确定连杆和关节的编号3.2.1坐标系的建立②中间各连杆坐标系Z轴的确定规定中间各连杆i的Z轴（Zi）沿着下一关节（i+1）的轴线方向。如图所示，杆1的Z轴沿着关节2轴线的方向，杆2的Z轴在沿着关节3轴线的方向，依次类推。Z轴正负方向可以任意选择，通常取向上为正。（1）连杆坐标系的确定中间连杆坐标系Z轴的确定3.2.1坐标系的建立③中间连杆坐标系X轴的确定找到当前杆i的Z轴（Zi）与下一杆件的Z轴（Zi+1）的公垂线，当前杆的X轴（Xi）即沿着该公垂线方向。如图所示，X1沿着Z1和Z2的公垂线，X2沿着Z2和Z3的公垂线，通常取X轴的正向指向下一杆件。Z轴和X轴确定后，坐标原点即为其交点，如图所示，坐标系{1}的原点为O1，{2}的原点为O2。（1）连杆坐标系的确定中间连杆坐标系X轴的确定需要注意的是，图中各Z轴在空间是交叉关系，因此其公垂线是唯一的。如果相邻坐标系Z轴互相平行，则其有无数条相互平行的公垂线，此时X轴的选择并不唯一，但应遵循一个原则，即尽量让连杆D-H参数为0，后面还会详细介绍。3.2.1坐标系的建立④中间连杆坐标系Y轴的确定按照右手定则即可确定Y轴方向。由于在D-H参数确定中用不到Y轴，图中可以不画出。⑤首尾连杆坐标系的确定机座记为连杆0，理论上其坐标系可以任意确定，但为了便于计算，尽量使连杆D-H参数为0，通常设定坐标系{0}与坐标系{1}同向或重合。最后一根连杆N的坐标系也可以任意选取，同理，通常会选择与{N-1}坐标系同向或重合。这点在后例3.6和3.7中会进一步介绍。移动关节连杆D-H坐标系的确定与转动关节的十分类似，唯一不同是，规定移动关节的Z轴沿着其移动的方向，例3.7中也会讲到。（1）连杆坐标系的确定3.2.1坐标系的建立机器人最后一根连杆的末端（腕部）通常会安装机械手完成各种作业，为了了解机械手相对参考坐标系或者工件坐标系的位置，必须为其确定手部(Hand)坐标系{H}。手部坐标系也称为工具坐标系，通常将其原点定义在手指的中间，如图所示。理论上XH,YH,ZH三坐标轴的方向可任取，但通常取接近工件的方向为ZH轴方向，所以称ZH轴的单位向量a为接近(approach)向量；两手指的连线为YH轴，指向可任意确定，称Y轴的单位向量o为姿态或方向(orientation)向量；XH轴应与YH，ZH轴垂直(normal)，所以有n=o×a，指向符合右手定则。（2）手部坐标系的确定手部坐标系的确定第3章

机器人运动学分析3.2串联机器人位姿分析3.2.1坐标系的建立3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.3运动方程3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.2D-H参数及连杆坐标系变换矩阵的确定思考：上述子变换的顺序是否可以互换？比如步骤1，2可换吗？步骤2，3可换吗？步骤3，4可换吗？步骤1，4可换吗？3.2.2D-H参数及连杆坐标系变换矩阵的确定12343.2.2D-H参数及连杆坐标系变换矩阵的确定第3章

机器人运动学分析3.2串联机器人位姿分析3.2.1坐标系的建立3.2.2D-H参数及连杆坐标系变换矩阵的确定3.2.3运动方程3.2.3运动方程3.2.3运动方程感谢大家观看化学工业出版社第3章

机器人运动学分析机器人技术基础目录contants3.1机器人位姿和坐标变换3.2串联机器人位姿分析3.3串联机器人运动学分析第3章

机器人运动学分析3.33.3串联机器人运动学分析3.3.1正向运动学3.3.2逆向运动学3.3.3关于反解的讨论3.3串联机器人运动学分析串联机器人是由若干杆件和关节组成首尾不相连的开式运动链。机器人运动学不考虑力、质量、时间等因素的影响，仅在几何学范畴来研究机器人的运动。机器人运动学问题分为两类：①正向运动学：已知机器人各连杆几何参数和关节变量，求解其末端执行器位姿的过程。②逆向运动学：已知机器人各连杆几何参数和末端位姿，求解各关节变量的过程。运动学分析的目的是建立各运动参数与机器人末端位姿的关系，为机器人运动学、动力学、轨迹规划以及控制研究提供基础。3.3.1正向运动学本节通过两个例子讨论正向运动学求解问题，即从关节变量到手部位姿。例3.6的GLUON桌面级机械臂有6个转动关节，在工业机器人中具有典型性和代表性；例3.7的斯坦福机械手有5个转动关节和一个移动关节，帮助我们拓展认知宽度。3.3.1正向运动学例3.63.3.1正向运动学例3.6解：（1）

D-H坐标系的建立根据3.2.1中介绍的步骤确立各连杆坐标系，标注连杆和关节的编号，并绘制各关节轴线的延长线，如图（a）所示。3.3.1正向运动学例3.6解：（1）

D-H坐标系的建立确定中间各连杆坐标系Z轴，如图（b）所示，注意Zi轴是沿着i+1关节的轴线的。3.3.1正向运动学例3.61）确定中间连杆坐标系X轴如图（c）所示。①确定X1：它是Z0与Z1的公垂线，Z0与Z1相交所以有唯一公垂线，正向可以任意确定，不妨确定为如图所示方向；②确定X2：它是Z1与Z2的公垂线，Z1与Z2平行所以有无数公垂线，为了使尽量多参数为0，取沿着杆3中心的方向为X2，正向指向下一杆；③确定X3：它是Z2与Z3的公垂线，Z2与Z3平行所以有无数公垂线，为了使尽量多参数为0，取沿着杆4中心的方向为X3，正向指与X2一致；3.3.1正向运动学例3.61）确定中间连杆坐标系X轴④确定X4：它是Z3与Z4的公垂线，Z3与Z4相交所以有唯一公垂线，为了使尽量多参数为0，取X4与X1同向；⑤确定X5：它是Z4与Z5的公垂线，Z4与Z5相交所以有唯一公垂线，为了使尽量多参数为0，取X5与X4同向。3.3.1正向运动学例3.62）中间连杆坐标系Y轴的确定按照右手定则即可唯一确定各坐标系Y轴。因为在后面的参数确定中用不到Y轴，图中仅标出Y0和Y6。3）首尾连杆坐标系的确定如图（d）所示，坐标系{0}的Z0轴的方向已经确定，为了使尽量多参数为0，我们取X0与X1重合且同向。坐标系{6}理论上可以任意确定，为了使尽量多参数为0，我们选择与坐标系{5}完全重合。你也可以选择让坐标系{6}的原点位于腕部中心的位置。3.3.1正向运动学例3.6(2)确定各连杆的D-H参数和关节变量1)确定关节1参数：即确定坐标系{0}到{1}四个子变换的参数。X0与X1同向，所以变量θ1初始值θ1=00；由于{0}和{1}的原点重合，所以连杆距离d和连杆长度a均为0；最后考察Z0和Z1，Z0需要绕X1旋转-900才会与Z1重合，所以关节扭角α=-900；该关节为转动关节，机器人生产商会给出关节变量θ1的转动范围-1400~1400。以上信息即为下表的第一行。3.3.1正向运动学例3.6(2)确定各连杆的D-H参数和关节变量2)确定关节2参数：即坐标系{1}到{2}四个子变换的参数。X1需要绕Z1轴转-900才能与X2同向，所以变量θ2初始值θ2=-900；为了让两坐标系原点重合，{1}首先要沿着Z1移动79.2，接着再沿X2移动173，所以连杆距离d2=79.2，连杆长度a2=173；Z1和Z2平行，所以关节扭角α=00；该关节为转动关节，关节角θ2的转动范围为-900~900。3.3.1正向运动学例3.6(2)确定各连杆的D-H参数和关节变量3)确定关节3参数：即坐标系{2}到{3}四个子变换的参数。X2与X3平行，所以变量θ3初始值θ3=00；为了让两坐标系原点重合，{2}首先要沿着Z2移动-79.2，接着再沿X3移动173，所以连杆距离d3=-79.2，连杆长度a3=173；Z2和Z3平行，所以关节扭角α=00；该关节为转动关节，关节角θ3的转动范围为-1400~1400。3.3.1正向运动学例3.6(2)确定各连杆的D-H参数和关节变量用类似的方法可以确定关节4,5,6的参数，如下表所示。3.3.1正向运动学例3.6将上表中的各参数带入上式

有3.3.1正向运动学例3.63.3.1正向运动学例3.63.3.1正向运动学例3.63.3.1正向运动学例3.6③调节6个转角，观察仿真机械臂的位姿，哪些关节角决定了末端位置，哪些关节角会影响末端姿态？3.3.1正向运动学例3.63.3.1正向运动学例3.7解：(1)D-H坐标系的建立3.3.1正向运动学例3.7解：(1)D-H坐标系的建立3.3.1正向运动学例3.7(2)确定各连杆的D-H参数和关节变量本列为厂家给出注意：关节3是移动轴，因此d3就是关节变量q3，而θ3是不变的常量。3.3.1正向运动学例3.7将上表中的各参数带入上式

有变量常量3.3.1正向运动学例3.73.3.1正向运动学例3.7拓展练习：①借助MATLAB符号工具箱，协助完成上面的推导和计算过程；3.3.1正向运动学例3.7拓展练习：②借助RoboticsToolbox工具箱，根据D-H参数建立斯坦福机械手仿真；③调节6个关节变量，观察仿真机械臂的位姿，哪些关节变量决定了末端位置，哪些关节变量会影响末端姿态？3.3.1正向运动学例3.7拓展练习：④随机给定一组关节变量，通过即可求出对应的机械臂末端位姿；如果随机给定足够多组关节变量，则可绘制出该机械臂的工作空间。第3章

机器人运动学分析3.33.3串联机器人运动学分析3.3.1正向运动学3.3.2逆向运动学3.3.3关于反解的讨论3.3.2逆向运动学3.3.2逆向运动学3.3.2逆向运动学3.3.2逆向运动学3.3.2逆向运动学例3.8在例3.7中已经求得所以有3.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.8其中3.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.8所以有3.3.2逆向运动学例3.83.3.2逆向运动学例3.8前面已经求出θ4，现在只剩下θ5一个未知数。3.3.2逆向运动学例3.8思考：为什么不通过第三个方程直接用arccos(x)的形式求出q5?3.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.83.3.2逆向运动学例3.8本程序难度较大，程序较长，供参考。3.3.2逆向运动学例3.8任意给定一组关节变量q，比如q=[305060105040]，调用正解子程序求运动学正解T06=fkine_stf(q)，以上面求出T06为输入，调用反解子程序求运动学反解q_inv=ikine_stf(T06)。可以发现求出了两组反解，其中第一组与原来给定的q一致，另一组不一致。3.3.2逆向运动学例3.8为了观察两组解的关系，将两组解均绘制出来。对比两图可以发现，虽然各关节变量取值不同（注意图左侧q1,q2，...，q6的取值），但两组解得到的机械臂末端的位姿完全一致（注意两图中X,Y,Z和R,P,Y的取值）。以上说明对于一个末端位姿T06，可能有多组关节变量q与之对应。（a）从左侧抵达目标点

（b）从右侧抵达目标点

3.3.2逆向运动学例3.93.3.2逆向运动学例3.9常数3.3.2逆向运动学例3.93.3.2逆向运动学例3.9所以3.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.9在（5）中求得方程组：3.3.2逆向运动学例3.93.3.2逆向运动学例3.93.3.2逆向运动学例3.9图为某位姿对应的四组解，分别从右下、右上、左下、左上四个方位到达要求的末端位姿。（a）右下

（b）右上（c）左下（d）左上第3章

机器人运动学分析3.33.3串联机器人运动学分析3.3.1正向运动学3.3.2逆向运动学3.3.3关于反解的讨论3.3.3关于反解的讨论想象一下你摘葡萄的场景。假设你伸展手臂可以达到的最大高度是2.2米，那么对于高度位于2.3米的葡萄，不借助其他工具你根本够不着，更不必说将其摘下；对于高度位于2.2米的葡萄，你的指尖可以触碰到它，但仍无法将其摘下；对于高度位于2.18米的葡萄，你可以通过中指和食指配合，勉强将其夹住硬拉下来；而对于高度位于2.1米以下的葡萄，你的手就能从任意方向到达它，这时就可以十分轻松灵活地将其摘下了。3.3.3关于反解的讨论求机械臂的运动学反解时，与摘葡萄遇到的问题类似。当我们给定一个末端位姿，有可能机械臂无论如何都是无法达到，即反解不存在，也可能有多种抵达方式，即有多个反解。下面我们从以下三个方面进行讨论。3.3.3关于反解的讨论工作空间是机械臂末端或手腕中心能够到达的空间范围，即其可到达的目标点的集合。工作空间可以分为以下两种。①灵活（工作）空间：指机械臂末端能以任意方位到达的目标点的集合（类比2.1米以下的葡萄）。②可达（工作）空间：指机械臂末端至少能以一个方位到达的目标点集合（类比2.2米、2.18米处的葡萄）。显然，灵活空间是可达空间的子集。若给定的末端位姿位于工作空间之内，则反解是存在的，否则反解不存在。（1）工作空间和解的存在性3.3.3关于反解的讨论当末端处于机械臂的工作空间之内时，遇到的另一个问题是，反解并不唯一。从例3.8和3.9可以看出，给定一个末端位姿，能够求出2个甚至4个反解。机械臂运动学反解的个数取决于关节的数目、连杆参数和关节变量的活动范围。一般来说，非零连杆参数越多，到达目标点的方式越多，反解数目也就越多