机器人机构学的数学基础 第3版 课件 第5章李群与刚体运动

机器人运动学基础D-H参数法与串联机器人正向运动学D-H参数法的由来美国西北大学的Denavit和Hartenberg于1955年提出的建立空间机构连杆坐标系的方法，后用于机械臂的运动学建模中机械臂各连杆两端有两个关节，设该连杆坐标系的z轴沿某关节轴线方向每个连杆坐标系建立在后向关节（远离基座）上，则称为后置坐标系，这也是原创的D-H参数法D-H参数法JacquesDenavit(1930-2012)RichardHartenberg(1907-1997)后置坐标系HartenbergRS,DenavitJ.Kinematicsynthesisoflinkages．NewYork:McGraw-Hill,1964Waldron和Paul进行了修正，但仍为后置坐标系方式，为教科书中所广泛应用，后人称为“标准D-H参数法”2D-H参数法后置坐标系前置坐标系如果连杆坐标系建立在前向关节上，则称为前置坐标系CraigKhalil和Dombre为教科书中所广泛应用，后人称为“改进的D-H参数法”D-H参数法的由来3D-H参数|前置坐标系

连杆参数

4

连杆参数

D-H参数|前置坐标系4个D-H参数中，ai-1和

i-1描述的是连杆本身，di和

i描述的是相邻连杆之间的位姿关系。对于旋转关节，关节角

i是变量，其他3个参数是结构参数；对于移动关节，偏距di是变量，其他3个参数则是结构参数。机器人的结构参数是由机器人本体结构特征决定的50号连杆（基座）的参数定义

n号连杆（末端杆）的参数定义

D-H参数|前置坐标系6

连杆i-1的固连坐标系定义在关节i-1上连杆坐标系定义——中间连杆

D-H参数|前置坐标系7连杆坐标系定义——中间连杆

D-H参数|前置坐标系如果轴线si与轴线si+1平行或重合，则原点Oi取在偏距di+1=0处，即使xi与xi+1共线8

基坐标系{0}D-H参数|前置坐标系9D-H参数|前置坐标系

工具坐标系{T}末端杆坐标系{n}设定工具坐标系{T}与末端杆坐标系{n}的原点不重合，但相应的各坐标轴保持一致，即两者之间只存在一个偏距（多数反映的是末端杆的长度）。10D-H参数|前置坐标系

建立连杆坐标系的一般步骤（1）找出各关节轴；（2）确定各中间连杆坐标系：按前述规则选取；（3）确定基坐标系{0}：按前述规则选取；（4）确定末端杆坐标系{n}：按前述规则选取；（5）确定工具坐标系{T}：按前述规则选取。

利用连杆坐标系确定D-H参数的一般步骤（1）ai-1：沿xi-1轴，从zi-1移动到zi的距离（只为正）（2）

i-1：绕xi-1轴，从zi-1旋转到zi的角度（可正可负）（3）di：沿zi轴，从xi-1移动到xi的距离（可正可负）（4）

i：绕zi轴，从xi-1旋转到xi的角度（可正可负）D-H参数11连杆坐标系定义——实例1标出右图所示3自由度平面机械臂（平面3R机器人）的连杆坐标系，给出其前置D-H参数连杆坐标系D-H参数y0x1x2x3x0y1y2y3D-H参数|前置坐标系xTyT12连杆坐标系定义——实例2标出右图所示空间3R机器人的连杆坐标系，给出其前置D-H参数连杆坐标系D-H参数z1z2x2z3x1x3z0x0y0y1y2y3z4x4y4D-H参数|前置坐标系13连杆坐标系定义——实例2标出右图所示空间3R机器人的连杆坐标系，给出其前置D-H参数连杆坐标系D-H参数z1z2x2z3x1x3z0y0y1y2y3z4x4y4D-H参数|前置坐标系解法2：x014

连杆坐标系{i-1}到坐标系{i}的齐次变换以上四步都是相对动坐标系描述，矩阵“从左到右”相乘：

D-H矩阵|前置坐标系15连杆坐标系{i-1}到坐标系{i}的齐次变换其中：前置D-H矩阵：D-H矩阵|前置坐标系16前置D-H矩阵——实例1建立下图所示平面3R机器人对应各连杆坐标系的前置D-H矩阵对应各连杆坐标系的D-H矩阵：D-H矩阵|前置坐标系17前置D-H矩阵——实例2建立下图所示空间3R机器人对应各连杆坐标系的前置D-H矩阵对应各连杆坐标系的D-H矩阵：D-H矩阵|前置坐标系18位姿求解|前置D-H参数法串联机器人位姿正解

对于具有n个关节的串联机器人，其位姿求解的一般计算公式为正向位姿求解——实例1利用前置D-H参数法对平面3R机器人进行正向位姿求解19正向位姿求解——实例3利用前置D-H参数法对PUMA560机器人进行正向位姿求解aonApproachOrientationz1x1z2x2z3x3z4x4z5x5z6x6位姿求解|前置D-H参数法20正向位姿求解——实例3利用前置D-H参数法对PUMA560机器人进行正向位姿求解连杆i变量θiαi-1ai-1di变量范围1θ10°00-160°~160°2θ2

90°0d2-225°~45°3θ30°a20-45°~225°4θ4

90°a3d4-110°~170°5θ590°00-100°~100°6θ6

90°00-266°~266°位姿求解|前置D-H参数法21正向位姿求解——实例3利用前置D-H参数法对PUMA560机器人进行正向位姿求解机器人的位姿正运动学模型：根据关节变量求解末端位姿的过程称为位姿解析正解位姿求解|前置D-H参数法22

符号定义

基坐标系{0}与末端杆坐标系{n}的选取原则类同于前置坐标系的情况D-H参数|后置坐标系23特点

D-H参数|后置坐标系24相邻坐标系之间的变换矩阵从连杆坐标系{i-1}到坐标系{i}的齐次变换需要4步：

相对动坐标系变换，遵循矩阵“从左到右”相乘的原则：D-H矩阵|后置坐标系25正向位姿求解——实例利用后置D-H参数法对平面3R机器人进行正向位姿求解位姿求解|后置D-H参数法正向位姿求解后置D-H参数后置D-H矩阵：26主讲教师：于靖军机器人运动学基础主讲教师：于靖军指数积公式的两种表示形式回顾：李群李代数与机器人29R.BrockettS.S.SastryJerroldMarsden哈佛大学的R.Brockett开始尝试采用微分流形和李群理论建立机器人学、控制、机构学和制造学科的统一理论，建立了机器人的指数建模方法，即通常所说的指数积公式（POEformula）。之后，Murray、Hervé、Selig、Park、李泽湘等对李群与李代数理论在机器人和机构学领域的应用进行了广泛而深入的研究。BrocketR,“Roboticmanipulatorsandtheproductofexponentialsformula,”InP.A.Fuhrman,editor,MathematicalTheoryofNetworksandSystems,120-129.Springer-Verlag,1984POE公式刚体运动（单自由度转动刚体）2自由度平面2R机器人正向运动学的求解首先将转动副1固定不动只转动关节2，然后将转动副2固定不动只转动关节1，根据刚体运动的叠加原理可以得到选择运动副的顺序与前面相反，即首先转动关节1，并保证关节2固定不动然后转动关节2，这时第二个连杆将绕新的轴线转动或（1）POE公式2自由度平面2R机器人正向运动学的求解然后转动关节2，这时第二个连杆将绕新的轴线转动或根据矩阵指数的性质根据刚体运动的叠加原理可以得到（2）式（1）与式（2）结果完全一样，因此可以得出结论机器人的运动学公式与运动副的顺序选择无关3自由度平面3R机器人正向运动学的求解零位（初始位形）：若将

1和

2置于初始位形（即前两个关节不动，只有第3个关节转动）。这时，对于关节3相对{S}系的单位运动旋量可以写成对应的螺旋运动的矩阵指数为POE公式|第一种形式或再假定

1=0以及固定

3（任意值），这时关节2的旋转可以看作是将一螺旋运动施加在一刚化系统上（杆2与杆3连接成一体）POE公式|第一种形式3自由度平面3R机器人正向运动学的求解或最后，保持

2和

3固定，关节1的旋转可看作是将一螺旋运动施加在整个刚化的三杆系统上。这时，对于任意的(

1,

2,

3)，满足POE公式|第一种形式3自由度平面3R机器人正向运动学的求解或POE公式|第一种形式3自由度平面3R机器人正向运动学的求解POE公式|第一种形式3自由度平面3R机器人正向运动学的求解第

i

个关节在初始位形下相对惯性坐标系的单位运动旋量坐标其中，（移动副）（转动副）通用公式（指数积公式的空间表示形式或第一种形式）POE公式上面所得的结论完全可以推广到具有n个关节的串联机器人正向运动学的求解。机器人的正向运动学可以表示成矩阵指数积的形式，每一项对应一个螺旋运动。该方法的最大优点在于正向运动学表达式中只涉及两个坐标系{S}（惯性坐标系）和{T}（工具坐标系）或{B}（物体坐标系）。初始位形时惯性坐标系与工具坐标系的变换为解：【例】利用指数积公式对SCARA机器人进行正向运动学求解POE公式解：【例】利用指数积公式对SCARA机器人进行正向运动学求解POE公式解：【例】利用指数积公式对SCARA机器人进行正向运动学求解POE公式惯性坐标系与初始位形的选择一般情况下，机器人的惯性坐标系取在机器人的基座上。不过，这种选取并不是唯一的，可以根据实际情况选取惯性坐标系的位置。为了简化计算，一种典型的选取方法是将惯性坐标系取在与初始位形时与工具坐标系重合的位置。在描述机器人正向运动学时，初始位形选取的自由度更大。由于各个关节的运动旋量坐标取决于初始位形（以及惯性坐标系）的选择，因此在选取初始位形时应遵循使运动分析尽量简单的原则。POE公式主讲教师：于靖军机器人运动学基础主讲教师：于靖军D-H参数法与POE公式之间的关系回顾：两种经典的机器人运动学公式指数积公式D-H参数法连杆坐标系D-H参数与D-H矩阵只涉及两个坐标系{S}（惯性坐标系）和{T}（工具坐标系）或{B}（物体坐标系）第

i

个关节在初始位形下相对惯性坐标系的单位运动旋量坐标及对应的矩阵指数【问题】两者之间是否存在联系？44D-H参数法与POE公式之间的关系？是否存在一一映射的关系在机器人的关节D

H参数与其对应的运动旋量坐标之间并不存在着一一映射的关系对于单个关节而言，完全描述其D

H参数需要4个参数，而用旋量坐标需要6个参数）每个运动副的旋量坐标都是相对惯性坐标系来描述的，它不能反映相邻杆件之间的相对运动45D-H参数法与POE公式之间的关系46根据矩阵指数

的性质得D-H参数法与POE公式之间的关系47第i个关节在初始位形下相对惯性坐标系的单位运动旋量坐标D-H参数法与POE公式之间的关系一种根据D

H参数来求解关节运动旋量坐标的方法1.一旦D

H参数给定，则

已知，根据求得2.根据求得48主讲教师：于靖军机器人运动学基础主讲教师：于靖军串联机器人逆运动学的指数积公式逆运动学（InverseKinematic）惯性系{S}工具{T}逆运动学问题

1

2

3

4

5

6

求解关节变量[

1,

2,

,

n]T逆运动学模型——从末端构件位姿矩阵到关节变量的映射机器人应用的基础——应用时，通常给定末端工具位姿，需要求解关节变量，然后通过控制关节变量到指定值，使得末端工具到达给定位姿51关节变量

i之间相互耦合，造成求解方程的非线性，导致或者存在封闭解，或者只能进行数值求解位移反解一般为多解，不具有唯一性。相比串联机器人位移正解求解而言，其逆运动学问题要复杂得多。运动学方程通常为非线性，可能导致无封闭解或解析解，只有数值解可能存在多解，视运动学方程的最高次数而定可能存在无穷多个解。比如运动学冗余的情况可能不存在可行解，比如运动学奇异的情况由机器人的特殊结构特征所导致位移反解的特点52反解过程中需遵循的三个原则（iii）姿态保持不变原则（移动）（i）位置保持不变原则（转动）（ii）距离保持不变原则（转动）53反解过程中需遵循的三个原则54反解过程中需遵循的三个原则55反解过程中需遵循的三个原则56位移反解简化思路直接分解法：直接消去POE公式中更多的变量，使得对POE公式的求解问题变成对运动子链（kinematicsubchain）的求解问题解：【实例】已知6自由度PRRRRR机器人，特点是后三个转动关节交于一点q57变量消元法：只能消去POE公式中的一个变量【实例】考察空间RRR机器人解：位移反解简化思路58Paden-Kahan子问题59Paden-Kahan子问题60Paden-Kahan子问题61位移反解求解示例【实例】求6自由度的RRRRRR机器人的运动学反解（RRR表示汇交于一点）解：62【实例】求6自由度的RRRRRR机器人的运动学反解（RRR表示汇交于一点）解（续）：位移反解求解示例63【实例】求SCARA机器人的运动学反解解：位移反解求解示例64主讲教师：于靖军机器人运动学基础主讲教师：于靖军速度雅可比的概念与求解雅可比的定义67利用多元函数求导法则，可得到上述xi关于q的全微分，即设向量X=(x1,…,xm)T中的每一个元素xi(i=1,2,…,m)，都是向量q=(q1,…,qn)T中元素的函数：

其中，m和n分别为向量X、q的广义坐标数雅可比矩阵雅可比矩阵J函数关系微分关系矩阵表达标量表达标量表达与矩阵表达68雅可比的定义定义式中，J(q)是向量值函数X对向量变量q的全微分式，被称为雅可比矩阵69雅可比矩阵

CarlGustavJacobJacobi（1804-1851）——普鲁士数学家，提出了椭圆方程和多元自变量与多元应变量的微分描述—雅可比矩阵描述自变量为向量、应变量也为向量的向量函数的微分映射机器人末端位姿与关节变量的关系，就是一个典型的向量——向量函数关系机器人末端位姿与关节变量的微分关系——速度映射，就是机器人的速度雅可比矩阵70直接微分法——概述根据速度雅可比矩阵的定义，通过求末端位姿矩阵的时间导数，直接获得串联机器人的速度雅可比。n个关节的串联机器人的末端位姿矩阵，其中的元素ri,j和pi是关节向量

的函数：上式中

对进行全微分即可得到末端相对于惯性坐标系{0}的速度矢量

的表达式，以及速度雅可比

71直接微分法——实例1求图示平面2R机器人的速度雅可比矩阵根据D-H法，可得机器人正运动学模型（末端位姿矩阵）末端位置末端姿态末端位置向量72直接微分法——实例1线速度的矩阵表达线速度求解求导7374直接微分法——实例1角速度求解代入角速度的矩阵表达74直接微分法——实例1合并线速度和角速度平面3R机器人的雅可比思考：对于空间机器人，如何求解与角速度相关的雅可比行？能否对旋转矩阵求导？线速度相关的行角速度相关的行75直接微分法——实例2求图所示空间3R机器人的速度雅可比矩阵由D-H法得到机器人正运动学模型（末端位姿矩阵）末端位置末端姿态末端位置向量76直接微分法——实例2姿态矩阵线速度项代入角速度项77直接微分法——实例2合并线速度和角速度空间3R机器人的雅可比线速度相关的行角速度相关的行78直接微分法——推广建立n自由度串联机器人的关节速度矢量与末端速度矢量的线性映射左上标0表示相对于基坐标系，q表示关节矢量79直接微分法——推广对位姿矩阵第四列的线速度项0pTORG求导，得到0vT的表达式：式中，0Jv(q)是3

n阶矩阵，反映了关节速度矢量与末端线速度矢量的映射关系求解线速度矢量80直接微分法——推广把旋转矩阵中的对应项代入下式（广义角速度的求解公式）进行全微分展开，得到0ωT的表达式：式中，0Jω(q)是3

n阶矩阵，反映了关节速度矢量与末端角速度矢量的映射关系求解角速度矢量81直接微分法——推广速度雅可比的意义完整的速度雅可比表达式合并和，即得到了完整的末端速度矢量和雅可比表达式：

（3）雅可比0J(q)：反映了机器人关节空间速度向操作空间速度传递的广义传动比——线性映射

（4）完整的J(q)有6个行向量，前3行与线速度矢量相关，后3行与角速度矢量相关（6）J(q)中的各元素与D-H参数中的结构参数和关节参数都有关，说明J(q)随机器人位形改变而发生变化。J(q)内含了机器人的位形信息，可以反映机器人的工作性能（5）J(q)的列数等于机器人主动关节数n，第i列反应了第i个关节的速度对末端速度的贡献82矢量积法思路充分利用速度雅可比列向量的物理意义串联机器人速度雅可比0J的第i列，对应着其他关节速度为零时，关节i的速度对末端速度的影响串联机器人的关节通常不是移动副就是转动副，分析单个关节对末端速度的作用，可以分别得到0J的各列直接利用速度矢量积公式来获得末端速度83矢量积法移动关节的作用移动关节i的速度大小为，其他关节速度均为零末端执行器的线速度与关节i

相等，角速度则为零，即

式中，为关节i的z轴在{0}中的表达，它是旋转矩阵的第3列84矢量积法移动关节的作用与移动关节i相关的末端速度矢量速度雅可比的第i列85矢量积法旋转关节的作用关节i为旋转关节，速度大小为末端执行器角速度与关节i相等，线速度由矢量积公式得到，即86矢量积法旋转关节的作用关节i为旋转关节，速度大小为末端执行器角速度与关节i相等，线速度由矢量积公式得到，即上式中，矢量0piT是末端坐标系{T}原点相对于{i}原点的位置矢量ipT在{0}中的表达

式中，是在位姿矩阵的第4列前三个元素组成的矢量87矢量积法旋转关节的作用与旋转关节i相关的末端速度矢量:速度雅可比的第i列:注意：利用矢量积法推导雅可比