第4章 机器人运动学

1、第第1节节 概述概述第第2节节 机器人运动方程的表示机器人运动方程的表示第第3节节 机器人运动方程的求解机器人运动方程的求解第第4节节 PUMA560机器人运动方程机器人运动方程第四章 机器人运动学第第1节节 概述概述第第2节节 机器人运动方程的表示机器人运动方程的表示第第3节节 机器人运动方程的求解机器人运动方程的求解第第4节节 PUMA560机器人运动方程机器人运动方程第四章 机器人运动学机器人研究所3第第1节节 概述概述常见的机器人运动学问题可归纳如下：常见的机器人运动学问题可归纳如下：1对一给定的机器人，已知杆件几何参数和关对一给定的机器人，已知杆件几何参数和关节角矢量求机器人末端执行

2、器相对于参考坐标节角矢量求机器人末端执行器相对于参考坐标系的位置和姿态。系的位置和姿态。2已知机器人杆件的几何参数，给定机器人末已知机器人杆件的几何参数，给定机器人末端执行器相对于参考坐标系的期望位置和姿态端执行器相对于参考坐标系的期望位置和姿态 (位姿位姿)，机器人能否使其末端执行器达到这个预，机器人能否使其末端执行器达到这个预期的位姿？如能达到，那么机器人有几种不同期的位姿？如能达到，那么机器人有几种不同形态可满足同样的条件形态可满足同样的条件? 机器人研究所4第第1节节 概述概述常见的机器人运动学问题可归纳如下：常见的机器人运动学问题可归纳如下：第一个问题常称为运动学正问题（第一个问题常

3、称为运动学正问题（正解正解）;第二个问题常称为运动学逆问题（第二个问题常称为运动学逆问题（反解反解）。）。这两个问题是机器人运动学中的基本问题。这两个问题是机器人运动学中的基本问题。 yXYZOxzoi机器人研究所5第第1节节 概述概述举例说明举例说明图为图为2 2自由度机器人手部的自由度机器人手部的连杆机构连杆机构处理这个手指位置（处理这个手指位置（P点位点位置置）与关节变量（）与关节变量（1、2）的关系称为的关系称为运动学分析运动学分析。L1、L2是大小不变的结构参数。机器人研究所6第第1节节 概述概述举例说明举例说明我们引入向量分别表示手爪我们引入向量分别表示手爪位置和关节变量：位置和关

4、节变量：手爪位置的各分量，按几何手爪位置的各分量，按几何学可表示为：学可表示为：xry 1211212coscos()xLL11212sinsin()yLL(x, y)机器人研究所7第第1节节 概述概述举例说明举例说明已知机器人的关节变量，求其手爪位置的运动已知机器人的关节变量，求其手爪位置的运动学问题称为正运动学（学问题称为正运动学（direct kinematics）。该）。该公式被称为运动方程式。公式被称为运动方程式。如果给定机器人的手爪位置，求为了到达这个如果给定机器人的手爪位置，求为了到达这个预定的位置，机器人的关节变量的运动学问题预定的位置，机器人的关节变量的运动学问题称为逆运动学

5、（称为逆运动学（inverse kinematics）。）。第第1节节 概述概述第第3节节 机器人运动方程的求解机器人运动方程的求解第第4节节 PUMA560机器人运动方程机器人运动方程第四章 机器人运动学机器人研究所9第第2节节 机器人运动方程的表示机器人运动方程的表示1. A矩阵和矩阵和T矩阵矩阵机械手可以看成由一系列关节连接起来的连杆机械手可以看成由一系列关节连接起来的连杆组构成。用组构成。用A矩阵矩阵描述连杆坐标系间相对平移和描述连杆坐标系间相对平移和旋转的齐次变换旋转的齐次变换.nA1表示第一连杆对基坐标的位姿表示第一连杆对基坐标的位姿nA2表示第二连杆对第一连杆位姿表示第二连杆对第

6、一连杆位姿T矩阵矩阵描述连杆对基坐标的齐次变换。描述连杆对基坐标的齐次变换。则第二连杆对基坐标的位姿为则第二连杆对基坐标的位姿为212TA A机器人研究所10第第2节节 机器人运动方程的表示机器人运动方程的表示1. A矩阵和矩阵和T矩阵矩阵如此类推，对于一个六连杆机器人，有如此类推，对于一个六连杆机器人，有6123456TA A A A A A机器人研究所11第第2节节 机器人运动方程的表示机器人运动方程的表示2. 运动姿态和方向角的描述运动姿态和方向角的描述1）夹持器（手抓）运动方向）夹持器（手抓）运动方向接近矢量接近矢量a：夹持器进入物体的方向；：夹持器进入物体的方向；Z轴轴方向矢量方向矢

7、量o：指尖互相指向；：指尖互相指向；Y轴轴法线矢量法线矢量n：X轴轴0001Txxxxyyyyzzzznoapnoapnoap机器人研究所12第第2节节 机器人运动方程的表示机器人运动方程的表示2. 运动姿态和方向角的描述运动姿态和方向角的描述2）用欧拉角表示运动姿态）用欧拉角表示运动姿态欧拉角欧拉角:绕绕Z轴转轴转,再绕新再绕新Y轴转轴转,绕最新绕最新Z轴转轴转.( , ,)( , )( , )( ,)000000000100000010000010000100010001EulerRot zRot yRot zcscscsscscsc 机器人研究所13第第2节节 机器人运动方程的表示机器人

8、运动方程的表示2. 运动姿态和方向角的描述运动姿态和方向角的描述3）用滚）用滚仰仰偏转表示运动姿态偏转表示运动姿态n横滚横滚: :绕绕Z Z轴转轴转, ,n俯仰俯仰: :绕绕Y Y轴转轴转, ,n偏转偏转: :绕绕X X轴转轴转. .100000000001100000001000100001000000),(),(),(),(cssccssccsscxRotyRotzRotRPY机器人研究所14第第2节节 机器人运动方程的表示机器人运动方程的表示3. 运动位置和坐标的描述运动位置和坐标的描述1）用柱面坐标表示末端运动位置）用柱面坐标表示末端运动位置n从基础坐标系出发变换的顺序为：沿从基础坐标

9、系出发变换的顺序为：沿x轴平移轴平移r，接着，接着绕绕z轴旋转轴旋转最后沿最后沿z轴平移轴平移z;n相对于参考坐标系的变换，位置和姿态都有变化，变相对于参考坐标系的变换，位置和姿态都有变化，变换矩阵为：换矩阵为：100010000100001000010001100001000000100010000100001)0 , 0 ,(),(), 0 , 0(),(zrscsrcscrcssczrTranszRotzTransrzCyl机器人研究所15第第2节节 机器人运动方程的表示机器人运动方程的表示3. 运动位置和坐标的描述运动位置和坐标的描述1）用柱面坐标表示末端运动位置）用柱面坐标表示末端运

10、动位置n由于上述绕由于上述绕Z轴的旋转，使末端执行器的姿态出现变轴的旋转，使末端执行器的姿态出现变化，若要执行器姿态不变，需将其绕化，若要执行器姿态不变，需将其绕执行器执行器Z轴轴反向反向旋转旋转：( , , )(,)000000001001000010001BCyl zr Rot zcsrccsscrsscz 机器人研究所16第第2节节 机器人运动方程的表示机器人运动方程的表示3. 运动位置和坐标的描述运动位置和坐标的描述2）用球面坐标表示末端运动位置）用球面坐标表示末端运动位置n沿沿Z平移平移r,绕绕Y轴转轴转,绕绕Z轴转轴转.1000010001000010000110000000100

11、0100001000000),0,0(),(),(),(rccssrsssccssrcccsccrcssccsscrTransyRotzRotrSph机器人研究所17第第2节节 机器人运动方程的表示机器人运动方程的表示3. 运动位置和坐标的描述运动位置和坐标的描述2）用球面坐标表示末端运动位置）用球面坐标表示末端运动位置n由于上述两个旋转，使执行器姿态发生变化，为保持由于上述两个旋转，使执行器姿态发生变化，为保持执行器姿态，需要绕其自身执行器姿态，需要绕其自身Y和和Z轴反向旋转。轴反向旋转。( , , )(,)(,)1000100010001BBSphr Rot yRot zrc srs sr

12、c 机器人研究所18第第2节节 机器人运动方程的表示机器人运动方程的表示3. 运动位置和坐标的描述运动位置和坐标的描述选择选择直角坐标直角坐标，柱面坐标柱面坐标还是还是球面坐标球面坐标是根据是根据机器人的结果来选择的。机器人的结果来选择的。机器人研究所19第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述已知杆件几何参数和关节角矢量求机器人末端已知杆件几何参数和关节角矢量求机器人末端相对于参考坐标系的位置和姿态相对于参考坐标系的位置和姿态称为称为正运动学正运动学分析分析。下面分析下面分析广义连杆广义连杆参数。参数。n广义连杆：连接两个关节的杆件。广义连杆：连接两

13、个关节的杆件。yXYZOxzo16123,.L L L机器人研究所20第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述描述一个连杆的两个参数描述一个连杆的两个参数: 1）Link length 连杆长度连杆长度ai-1 关节轴关节轴i-1和关节轴和关节轴i之间的公垂之间的公垂线的长度线的长度ai-1 2）Link twist 连杆转角连杆转角i-1 假设作一个平面假设作一个平面,并使该平面与两关节轴之间的公垂线垂直并使该平面与两关节轴之间的公垂线垂直,然后把然后把关节轴关节轴i-1和关节轴和关节轴i投影到该平面上投影到该平面上,在平面内轴在平面内轴i-1按照右

14、手法则绕按照右手法则绕ai-1转向轴转向轴i,测量两轴角之间的夹角为测量两轴角之间的夹角为i-1.机器人研究所21第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述描述连杆连接的两个参数描述连杆连接的两个参数: 1） link offset 连杆偏距连杆偏距di 相邻两个连杆之间有一个公共相邻两个连杆之间有一个公共的关节轴，沿着两个相邻连杆公共的关节轴，沿着两个相邻连杆公共轴线方向的距离可以用一个参数描轴线方向的距离可以用一个参数描述为连杆偏距述为连杆偏距di. 当当i为移动关节时为移动关节时,连杆偏距为一变量连杆偏距为一变量. 2） joint angle 关

15、节角关节角i 描述两个相邻连杆绕公共轴线旋描述两个相邻连杆绕公共轴线旋转的夹角转的夹角i. 当当i为转动关节时，关节角为一变量为转动关节时，关节角为一变量.机器人研究所22第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述连杆参数（连杆参数（ ai-1 ，i-1 ， di ， i ）n对于转动关节对于转动关节,i为关节变量为关节变量,其他三个参数固定不变其他三个参数固定不变;对于移动关节对于移动关节, di为关节变量为关节变量,其他三个参数固定不变其他三个参数固定不变;这种用连杆参数描述机构运动关系的规则称为这种用连杆参数描述机构运动关系的规则称为Denavit

16、-Hartenberg参数法，简称参数法，简称D-H法法.机器人研究所23第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述连杆附加坐标系的规定：连杆附加坐标系的规定：n为了描述每个连杆和相邻连杆之间的相对位置关系为了描述每个连杆和相邻连杆之间的相对位置关系,需需要在每个连杆上定义一个固连坐标系要在每个连杆上定义一个固连坐标系.1）连杆中的中间连杆）连杆中的中间连杆 坐标系i的Z轴称为Zi,与关节轴i重合; 坐标系i的原点位于公垂线ai与关节轴i的交点处. Xi轴沿ai方向由关节i指向关节i+1(若: ai =0,则Xi垂直于Zi和Zi+1所在的平面) Yi轴由

17、右手定则确定机器人研究所24第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述2）连杆中的首尾连杆）连杆中的首尾连杆坐标系坐标系0 通常规定通常规定: Z0轴沿着关节轴轴沿着关节轴1的方向的方向,当坐标系当坐标系1的关节变量为的关节变量为0时时,设定参考设定参考坐标系坐标系0与与1重合重合.且且a0=0, 0=0,当关节当关节1为转动关节为转动关节,d1=0;当关节当关节1为移动关节为移动关节, 1.=0.坐标系坐标系n 通常规定通常规定: 对于转动关节对于转动关节 n,设定设定n=0, 此时此时Xn和和Xn-1轴的方向相同轴的方向相同,选取坐标选取坐标系系n

18、的原点位置的原点位置,使之满足使之满足dn=0; 对于移动关节对于移动关节 n, 设定设定Xn轴的方向轴的方向使之满足使之满足n=0,当当dn=0时时,选取坐选取坐标系标系n 的原点位于的原点位于Xn-1轴与关节轴轴与关节轴n的交点位置的交点位置.机器人研究所25第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述：沿：沿Xi-1轴轴,从从Zi-1移动到移动到Zi的距离的距离;：绕：绕Xi-1轴轴,从从Zi-1旋转到旋转到Zi的角度的角度;：沿：沿Zi轴轴,从从Xi-1移动到移动到Xi的距离的距离;：绕：绕Zi轴轴,从从Xi-1旋转到旋转到Xi的角度的角度;1ai

19、1iidi通常规定通常规定ai 0,其余其余可正可负可正可负.3）在连杆坐标系中对连杆参数的解释）在连杆坐标系中对连杆参数的解释ii-1机器人研究所26第第2节节 机器人运动方程的表示机器人运动方程的表示4. 连杆参数描述连杆参数描述确定关节轴，并画出轴的延长线。找出关节轴 i 和 i+1的公垂线或交点，作为坐标系 i 的原点。规定Zi的指向是沿着第 i 个关节轴。规定Xi轴得指向是沿着轴i和i+1的公垂线的方向，如果关节轴i和i+1相交，则Xi轴垂直于关节轴i和i+1所在的平面。Yi 轴的方向由右手定则确定。当第一个关节变量为0时，规定坐标系0和1 重合，对于坐标系N，尽量选择坐标系使得连杆

20、参数为0.4）建立连杆坐标系的步骤）建立连杆坐标系的步骤机器人研究所27第第2节节 机器人运动方程的表示机器人运动方程的表示例题例题10机器人研究所28第第2节节 机器人运动方程的表示机器人运动方程的表示4.两杆之间的位姿矩阵两杆之间的位姿矩阵OiXidiZi-1Oi-1Xi-1 iZii-1ai-111111(,)(,a)( ,)( ,)iiiiiiiiiiARot xTrans xRot zTrans z d机器人研究所29第第2节节 机器人运动方程的表示机器人运动方程的表示4.两杆之间的位姿矩阵两杆之间的位姿矩阵从坐标系从坐标系Oi-1-xi-1yi-1zi-1到坐标系到坐标系Oi-xi

21、yizi的变换矩阵为的变换矩阵为111111111(,)(a, 0, 0)(,)(0, 0,)1000100acossin0010000cossin00100sincos0001000sincos000100010001000100010001iiiiiiiiiiiiiiiiiARot xTransRot zTransdd1111111110001cossin0acossincoscossinsinsinsinsincoscoscos0001iiiiiiiiiiiiiiiiiidd机器人研究所30第第2节节 机器人运动方程的表示机器人运动方程的表示5.连续连杆变换连续连杆变换定义了连杆坐标系和

22、相应得连杆参数，就能建定义了连杆坐标系和相应得连杆参数，就能建立运动学方程，坐标系立运动学方程，坐标系N相对于坐标系相对于坐标系0的变的变换矩阵为：换矩阵为：n变换矩阵变换矩阵 是关于是关于n个关节变量的函数，这些变个关节变量的函数，这些变量可以通过放置在关节上的传感器测得，则机器量可以通过放置在关节上的传感器测得，则机器人末端连杆再基坐标系中的位置和姿态就能描述人末端连杆再基坐标系中的位置和姿态就能描述出来。出来。00121123NNNTA A AA0NT机器人研究所31第第2节节 机器人运动方程的表示机器人运动方程的表示6.机器人运动学方程机器人运动学方程已知已知t时刻机器人关节变量时刻机

23、器人关节变量qi(t)(i=1,2)，计算末，计算末端操作器的位姿。即建立运动方程式：端操作器的位姿。即建立运动方程式：D-H参数中除关节变量之外的连杆参数都是固定参数中除关节变量之外的连杆参数都是固定值，由机器人物理结构确定。值，由机器人物理结构确定。iiiqd对转动关节对移动关节00111122()()()TAAAnnnnqqq机器人研究所32第第2节节 机器人运动方程的表示机器人运动方程的表示例题例题1：如图所示的三杆平面机器人，设已知手臂长如图所示的三杆平面机器人，设已知手臂长l1，l2和和l3，关节变量，关节变量q1，q2和和q3，试求末端操作器的，试求末端操作器的位姿矩阵。位姿矩阵。机器人研究所33第第2节节 机器人运动方程的表示机器人运动方程的表示例题例题1：解：解： 1、建立机器人各杆的坐标系平面三自由度机器人平面三自由度机器人X0Y0 x1y1x2y2x3y3l1l2l3123x4y4机器人研究所34第第2节节 机器人运动方程的表示机器人运动方程的表示2、确定各连杆D-H参数和关节变量平面三自由度机器人平面三自由度机器人X0Y0 x1y1x2y2x3y3l1l2l