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

第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}同向或