第2章 工业机器人运动学.docx
《第2章 工业机器人运动学.docx》由会员分享，可在线阅读，更多相关《第2章 工业机器人运动学.docx（18页珍藏版）》请在冰点文库上搜索。
第2章工业机器人运动学
第2章工业机器人运动学之宇文皓月创作
章节题目：
第2章工业机器人运动学
[教学内容]
2-1齐次坐标及对象物的描述
2-2齐次变换及运算
2-3工业机器人连杆参数及其齐次变换矩阵
2-4工业机器人运动学方程
[教学安插]
第2章安插6学时，其中介绍点的位置描述10分钟，齐次坐标10分钟，坐标轴方向的描述10分钟，动坐标系位姿的描述20分钟，目标物齐次矩阵暗示10分钟，平移的齐次变换30分钟，旋转的齐次变换30分钟，平移加旋转的齐次变换15分钟，连杆参数及连杆坐标系的建立20分钟，连杆坐标系之间的变换矩阵15分钟，机器人运动学方程10分钟，正向运动学及实例45分钟，反向运动学及实例30分钟，X=X（Q）形式运动学方程15分钟。
通过多媒体课件结合板书的方式，采取课堂讲授和课堂讨论相结合的方法，介绍齐次坐标的概念及各种对象的齐次坐标方法，进而向学生讲述齐次变换及运算方法，通过上述内容的讲解，进一步让学生掌握连杆参数及其齐次变换矩阵，最终引出工业机器人运动学方程。
[知识点及其基本要求]
1、点的位置描述（掌握）
2、齐次坐标（掌握）
3、坐标轴方向的描述（掌握）
4、动坐标系的描述（掌握）
5、齐次变换（重点掌握）
6、连杆参数及其齐次变换矩阵（掌握）
7、运动学方程（掌握）
[重点和难点]
重点：
1、对象的齐次坐标暗示
2、齐次变换
3、机器人运动学方程
难点：
1、连杆参数
2、机器人运动学方程
[教学法设计]
引入新课：
机器人实际上可认为是由一系列关节连接起来的连杆所组成。
我们把坐标系固连在机器人的每一个连杆关节上，可以用齐次变换来描述这些坐标系之间的相对位置和方向。
齐次变换具有较直观的几何意义，而且可描述各连杆之间的关系，所以经常使用于解决运动学问题。
新课讲解：
第一次课
第2章工业机器人运动学
2-1齐次坐标及对象物的描述
一、点的位置描述
在选定的直角坐标系{A}中，空间任一点P的位置可用3×1的位置矢量Ap暗示，其左上标代表选定的参考坐标系：
。
二、齐次坐标
如用四个数组成的（4×1）列阵
暗示三维空间直角坐标系{A}中点P，则列阵[pxpypz1]T称为三维空间点P的齐次坐标。
必须注意，齐次坐标的暗示不是唯一的，将其各元素同乘一非零因子w后，仍然代表同一点P，即
。
三、坐标轴方向的描述
i，j，k分别暗示直角坐标系中X、Y、Z坐标轴的单位向量。
若用齐次坐标来描述X、Y、Z轴的方向，则
。
从上可知，规定：
（4×1）列阵[abc0]T中第四个元素为零，且a2+b2+c2=1，则暗示某轴（某矢量）的方向；
（4×1）列阵[abcw]T中第四个元素不为零，则暗示空间某点的位置。
四、动坐标系位姿的描述
动坐标系位姿的描述就是对动坐标系原点位置的描述以及对动坐标系各坐标轴方向的描述。
1、刚体位置和姿态的描述
机器人的一个连杆可以看做一个刚体。
若给定了刚体上某一点的位置和该刚体在空间的姿态，则这个刚体在空间上是完全确定的。
O’为刚体上任一点，O’X’Y’Z’为与刚体固连的一个坐标系，称为动坐标系。
刚体Q在固定坐标系OXYZ中的位置可用齐次坐标形式的一个（4×1）列阵暗示：
。
刚体的姿态可由动坐标系的坐标轴方向来暗示。
令n、o、a分别为X’、Y’、Z’坐标轴的单位方向矢量，每个单位方向矢量在固定坐标系上的分量为动坐标系各坐标轴的方向余弦，用齐次坐标形式的（4×1）列阵分别暗示为：
n=[nxnynz0]T，o=[oxoyoz0]T，a=[axayaz0]T。
因此，刚体的位姿可用下面（4×4）矩阵来描述：
。
2、手部位置和姿态的暗示
机器人手部的位置和姿态也可以用固连于手部的坐标系{B}的位姿来暗示。
坐标系{B}可以这样来确定：
取手部的中心点OB；关节周为ZB轴，ZB轴的单位方向矢量a称为接近矢量，指向朝外；二手指的连线为YB轴，YB轴的单位方向矢量o称为姿态矢量，指向可任意选定；XB轴与YB轴及ZB轴垂直，XB轴的单位方向矢量n称为法向矢量，且n=o×a，指向符合右手法则。
手部的位置矢量为固定参考系原点指向手部坐标系{B}原点的矢量p，手部的方向矢量为n、o、a。
于是手部的位姿可用（4×4）矩阵暗示为：
。
五、目标物齐次矩阵暗示
2-2齐次变换及运算
刚体的运动是由转动和平移组成的。
为了能用同一矩阵暗示转动和平移，有需要引入（4×4）的齐次坐标变换矩阵。
一、平移的齐次变换
空间某一点A，坐标为（x，y，z），当它平移至A’点后，坐标为（x’，y’，z’），以及
，或写成如下形式：
，也可以简写为a’=Trans（Δx，Δy，Δz）a，其中，Trans（Δx，Δy，Δz）暗示齐次坐标变换的平移算子。
且
，其中第四列元素分别暗示沿坐标轴X，Y，Z的移动量。
若算子左乘，暗示坐标变换是相对固定坐标系进行的；假如相对动坐标系进行坐标变换，则算子应该右乘。
第二次课
二、旋转的齐次变换
空间某一点A，坐标为（x，y，z），当它绕Z轴旋转θ角后至A’点，坐标为（x’,y’,z’），A’点和A点的坐标关系为：
，或用矩阵暗示为：
。
A’点和A点的齐次坐标分别为[x’y’z’1]T和[xyz1]T，因此A点的旋转齐次变换过程为：
，也可简写为：
a’=Rot（z,θ）a，其中，Rot（z,θ）暗示齐次坐标变换时绕Z轴的旋转算子，算子左乘暗示相对于固定坐标系进行变换，算子的内容为：
。
同理，可写出绕X轴旋转的算子和绕Y轴旋转的算子分别为：
，
。
点A绕任意过原点的单位矢量k旋转θ角时，kx，ky，kz分别为k矢量在固定参考系坐标轴X、Y、Z上的三个分量，且kx2+ky2+kz2=1。
可以证明，绕任意过原点的单位矢量k转θ角的旋转齐次变换公式为：
式中，versθ=（1-cosθ）。
上式称为一般旋转齐次变换通式，它概括了绕X轴、Y轴、Z轴进行旋转齐次变换的各种特殊情况。
反之，若给出某个旋转齐次变换矩阵
，则可根据变换通式求出其等效转轴矢量k及等效转角θ：
式中，当θ取0°到180°之间的值时，式中的符号取﹢号；当转角θ很小时，公式很难确定转轴；当θ接近0°或180°时，转轴完全不确定。
和平移变换一样，旋转变换算子公式以及一般旋转变换算子公式，不但仅适用于点的旋转变换，而且也适用于矢量、坐标系、物体等旋转变换计算。
若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算子右乘。
三、平移加旋转的齐次变换
平移变换和旋转变换可以组合在一个齐次变换中。
2-3工业机器人连杆参数及其齐次变换矩阵
一、连杆参数及连杆坐标系的建立
连杆两端有关节n和n+1。
该连杆尺寸可以用两个量来描述：
一个是两个关节轴线沿公垂线的距离an称为连杆长度；另一个是垂直于an的平面内两个轴线的夹角αn，称为连杆扭角。
这两个参数为连杆的尺寸参数。
再考虑连杆n与相邻连杆n-1的关系，若它们通过关节相连，其相对位置可用两个参数dn和θn来确定，其中dn是沿关节n轴线两个公垂线的距离，θn是垂直于关节n轴线的平面内两个公垂线的夹角。
这是表达相邻杆件关系的两个参数。
这样，每个连杆可以由四个参数所描述：
其中两个描述连杆尺寸，另外两个描述连杆与相邻杆件的连接关系。
对于旋转关节，θn是关节变量，其它三个参数固定不变；对于移动关节，dn是关节变量，其它三个参数固定不变。
连杆坐标系的建立按下面的规则进行：
连杆n坐标系（简称n系）的坐标原点设在关节n的轴线和关节n+1的轴线的公垂线与关节n+1的轴线相交之处，n系的Z轴与关节n+1的轴线重合，X轴与上述公垂线重合，且方向从关节n指向关节n+1，Y轴则按右手系确定。
二、连杆坐标系之间的变换矩阵
建立了各连杆坐标系后，n-1系与n系之间的变换关系可以用坐标系的平移、旋转来实现。
从n-1系到n系的变换，可先令n-1系绕Zn-1轴旋转θn角，再沿Zn-1轴平移dn，然后沿Xn轴平移an，最后绕Xn轴旋转αn角，使得n-1系与n系重合。
用一个变换矩阵An来综合暗示上述四次变换时应注意到坐标系在每次旋转或平移后发生了变动，后一次变换都是相对动系进行的，因此在运算中变换算子应该右乘。
于是连杆n的齐次变换矩阵为：
2-4工业机器人运动学方程
一、机器人运动学方程
为机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。
通常把描述一个连杆坐标系与下一个连杆坐标系间相对关系的齐次变换矩阵叫做A变换矩阵或A矩阵。
如果A1矩阵暗示第一个连杆坐标系相对于固定坐标系的位姿，A2矩阵暗示第二个连杆坐标系相对于第一个连杆坐标系的位姿，那么第二个连杆坐标系在固定坐标系中的位姿可用A1和A2的乘积来暗示：
T2=A1A2。
同理，若A3矩阵暗示第三个连杆坐标系相对于第二个连杆坐标系的位姿，则有T3=A1A2A3，如此类推，对于六连杆机器人，有下列T6矩阵：
T6=A1A2A3A4A5A6。
此式右边暗示了从固定参考系到手部坐标系的各连杆坐标系之间的变换矩阵的连乘，左边T6暗示这些变换矩阵的乘积，也就是手部坐标系相对于固定参考系的位姿，称上式为机器人运动学方程，计算结果T6是一个如下的（4×4）矩阵：
，式中，前三列暗示手部的姿态，第四列暗示手部的位置。
第三次课
二、正向运动学及实例
正向运动学主要解决机器人运动学方程的建立及手部位姿的求解问题。
1、平面关节型机器人的运动学方程
具有一个肩关节、一个肘关节和一个腕关节的SCARA机器人的机械结构特点是三个关节轴线是平行的。
固定坐标系{0}和连杆1、连杆2、连杆3的坐标系{1}、{2}、{3}坐落在关节1、关节2、关节3和手部中心。
坐标系{3}也就是手部坐标系。
连杆参数中θ为变量，其余参数d、a、α均为常量。
考虑到关节轴线平行，而且连杆都在一个平面内的特点，列出SCARA机器人连杆的参数如下表所示。
连杆
转角（变量）θ
两连杆间距离d
连杆长度a
连杆扭角α
连杆1
θ1
d1=0
a1=l1=100
α1=0
连杆2
θ2
d2=0
a2=l2=100
α2=0
连杆3
θ3
d3=0
a3=l3=20
α3=0
该平面关节型机器人的运动学方程为T3=A1A2A3，式中A1暗示连杆1的坐标系{1}相对于固定坐标系{0}的齐次变换矩阵；A2暗示连杆2的坐标系{2}相对于连杆1的坐标系{1}的齐次变换矩阵；A3暗示连杆3的坐标系即手部坐标系{3}相对于连杆2的坐标系{2}的齐次变换矩阵。
于是有：
。
即
因此，可写出：
T3是A1、A2、A3连乘的结果，暗示手部坐标系{3}（即手部）的位置和姿态。
可写出手部位置（4×1）列阵为：
。
暗示手部姿态的方向矢量n、o、a分别为：
2、斯坦福机器人的运动学方程
杆号
关节转角θ
扭角α
杆长a
距离d
1
2
3
4
5
6
θ1
θ2
0
θ4
θ5
θ6
-90°
90°
0°
-90°
90°
0°
0
0
0
0
0
0
0
d2
d3
0
0
H
上表给出了斯坦福机器人各连杆的参数。
现在根据各连杆坐标系的关系写出齐次变换矩阵Ai。
{1}系与{0}系是旋转关节连接。
坐标系{1}相对于固定坐标系{0}的Z0轴的旋转为变量θ1，然后绕自身坐标系X1轴作α1的旋转变换，α1==90°。
所以
{2}系与{1}系是旋转关节连接，连杆距离为d2。
坐标系{2}相对于坐标系{1}的Z1轴的旋转为变量θ2，然后绕自身坐标系Z2轴正向作d2距离的平移变换及绕X2轴作α2的旋转坐标变换，α2=90°。
所以
{3}系与{2}系是移动关节连接。
坐标系{3}相对于坐标系{2}的Z2轴德平移为变量d3。
所以
斯坦福机器人手腕三个关节都是转动关节，关节变量为θ4，θ5及θ6，而且三个关节的中心重合。
系{4}对系{3}的旋转变量为θ4，然后绕自身坐标轴X4作α4的旋转变换，α4=-90°。
所以
系{5}对系{4}的旋转变量为θ5，然后绕自身坐标轴X5作α5的旋转变换，α5=90°。
所以
系{6}相对于系{5}的旋转变量为θ6，并移动距离H。
所以
这样，所有杆的A矩阵已建立。
如果要知道非相邻杆件间的关系，只要用相应的A矩阵连乘即可。
如：
则斯坦福机器人的运动学方程为
。
方程右边的结果就是最后一个坐标系{6}的位置和姿态矩阵，各元素均为θ和d的函数，当θ和d给出后，可以计算出斯坦福机器人手部坐标系{6}的位置p和姿态n、o、a。
这就是斯坦福机器人手部位姿的解，这个求解过程叫做斯坦福机器人运动学正解。
三、反向运动学实例
在机器人控制中，往往在已知手部要到达的目标位姿的情况下如何求出关节变量，以驱动各关节的马达，使手部的位姿得到满足，这就是反向运动学问题，也称求运动学逆解。
以斯坦福机器人为例介绍反向求解的一种方法。
假设H=0，即坐标系{6}与坐标系{5}原点重合。
已知斯坦福机器人的运动学方程为：
T6=A1A2A3A4A5A6，给出T6矩阵及各杆的参数a、α、d，求关节变量θ1~θ6，其中θ3=d3。
（1）求θ1：
用A1-1左乘运动学方程，得：
1T6=A1-1T6=A2A3A4A5A6，左右展开得：
取上式左、右两边第三行第四列相等，即：
-pxs1+pyc1=d2，引入中间变量r及φ，令px=rcosφ，py=rcosφ，r=（px2+py2）1/2，φ=arctg（py/px），则该式化为：
cosθ1sinφ-cosφsinθ1=d2/r。
利用和差公式，上式又可化为：
sin（φ-θ1）=d2/r。
这里0，所以
。
这里，“+”号对应右肩位姿，“-”号对应左肩位姿。
（2）求θ2：
取上面矩阵等式左、右两边第一行第四列相等和第二行第四列相等，即：
故：
（3）求θ3：
在斯坦福机器人中，θ3=d3，有
（2）中等式可解得：
（4）求θ4：
由于3T6=A4A5A6，所以A4-13T6=A5A6，左右两边展开后取其左、右两边第三行第三列相等，得：
，所以
及
（5）求θ5：
取（4）中矩阵等式展开左、右两边第一行第三列相等及第二行第三列相等，有：
所以：
（6）求θ6：
采取方程A5-14T6=A6，展开并取其左、右两边第一行第二列相等及第二行第二列相等，有：
所以：
。
至此，θ1、θ2、d3、θ4、θ5、θ6全部求出。
从以上解的过程看出，这种方法就是将一个未知数由矩阵方程的右边移向左边，使其与其它未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数。
所以这种方法也叫变量分离法。
还应注意到机器人运动学逆解问题的求解存在如下三个问题：
（1）解可能不存在。
机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。
（2）解的多重性。
机器人的逆运动学问题可能出现多解。
在多解情况下，一定有一个最接近解，即最接近起始点的解。
（3）求解方法的多样性。
机器人逆运动学求解有多种方法，一般分为两类：
封闭解和数值解。
应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。
四、X=X（Q）形式运动学方程
“角度设定法”是采取相对参考坐标系或相对运动坐标系作三次连续转动来规定姿态的方法。
机器人手部位姿可用一个6维列矢量来暗示：
X=[pxpypzφxφyφz]T，或写成X=[xyzφxφyφz]T。
式中，x、y、z暗示手部位置，φx、φy、φz分别是用角度设定法来规定手部姿态时绕X轴、Y轴和Z轴的转角。
1、RPY角法和欧拉角法
RPY角法和欧拉角法是角度设定法中经常使用的方法。
RPY角法是手部相对参考坐标系轴作三次连续转动获得规定的姿态：
先绕X轴转动φx角，称为偏转，再绕Y轴转动φy角，称为俯仰，最后绕Z轴转动φz角，称为翻滚，得到相应的旋转矩阵为：
RPY（φx，φy，φz）=Rot（Z，φx）Rot（Y，φy）Rot（X，φx），该式也称为“x—y—z”RPY角设定法。
欧拉角法是手部相对运动坐标系轴作三次连续转动获得规定的姿态：
如果转动顺序为z—y—x，则相应的旋转矩阵为：
Euler（φz，φy，φx）=Rot（Z，φx）Rot（Y，φy）Rot（X，φx），该式也称为“z—y—x”欧拉角设定法。
以上两式结果恰巧完全相同。
如果用其它顺序进行欧拉角三次连续转动，结果便不相同了。
不管用什么角度设定法来规定手部姿态，姿态的实现，事实上是由关节变量作决定的。
知道旋转矩阵则可由以上两式逆解出手部姿态的规定角φx、φy、φz，而且φx=φx（q）、φy=φy（q）、φz==φz（q），q为广义关节变量，q=[q1q2…qn]T。
2、运动学方程X=X（q）
用A矩阵确定T6，可写成T6=A1（q）A2（q）A3（q）A4（q）A5（q）A6（q），或写成T6=T（q）。
该式暗示机器人手部位姿（n，o，a，p）与关节变量q之间的关系。
[应用]
第一次课
第二次课
1、点矢量v为[10.0020.0030.00]T，相对参考系作如下齐次变换，写出变换后点矢量v的表达式。
并说明是什么性质的变换，写出Rot（?
，?
），Tran（?
，?
，?
）。
2、有一旋转变换，先绕固定坐标系Z0轴转45°，再绕其X0轴转30°，最后绕其Y0轴转60°，试求该齐次变换矩阵。
3、坐标系{B}起初与固定坐标系{0}相重合，现坐标系{B}绕ZB轴旋转30°，然后绕旋转后的动坐标系XB轴旋转45°，试写出该坐标系{B}的起始矩阵表达式和最后矩阵表达式。
4、坐标系{A}及{B}在固定坐标系{0}中的矩阵表达式如下，画出它们在{0}坐标系中的位置和姿态。
5、写出齐次变换矩阵
，它暗示坐标系{B}连续相对固定坐标系{A}作以下变换：
（1）绕ZA轴旋转90°；
（2）绕XA轴旋转-90°；（3）移动[3，7，9]T。
6、写出齐次变换矩阵
，它暗示坐标系{B}连续相对自身运动坐标系{B}作以下变换：
（1）移动[3，7，9]T；
（2）绕XB轴旋转-90°；（3）绕ZB轴旋转90°。
7、图2-28（a）暗示两个楔形物体，试用两个变换序列分别暗示两个楔形物体的变换过程，使最后的状态如图（b）所示。
第三次课
8、如图2-29所示二自由度平面机械手，关节1为转动关节，关节变量为θ1；关节2为移动关节，关节变量为d2。
（1）建立关节坐标系，并写出该机械手的运动方程式。
（2）按下列关节变量参数，求出手部中心的位置值。
θ1
0°
30°
60°
90°
d2/m
0.50
0.80
1.00
0.70
9、如图2-29所示二自由度平面机械手，已知手部中心坐标值为X、Y。
求该机械手运动方程的逆解θ1及d2。
10、三自由度机械手如图2-30所示，臂长为l1和l2，手部中心离手腕中心的距离为H，转角为θ1、θ2、θ3，试建立杆件坐标系，并推导出该机械手的运动学方程。
11、图2-31为一个二自由度的机械手，两连杆长度均为1m，试建立各杆件坐标系，求出A1、A2及该机械手的运动学逆解。
12、什么是机器人运动学逆解的多重性？
[板书设计]
第一次课
第2章工业机器人运动学
2-1齐次坐标及对象物的描述
一、点的位置描述
二、齐次坐标
三、坐标轴方向的描述
四、动坐标系位姿的描述
1、刚体位置和姿态的描述
2、手部位置和姿态的暗示
五、目标物齐次矩阵暗示
2-2齐次变换及运算
一、平移的齐次变换
第二次课
二、旋转的齐次变换
三、平移加旋转的齐次变换
2-3工业机器人连杆参数及其齐次变换矩阵
一、连杆参数及连杆坐标系的建立
二、连杆坐标系之间的变换矩阵
2-4工业机器人运动学方程
一、机器人运动学方程
第三次课
二、正向运动学及实例
1、平面关节型机器人的运动学方程
2、斯坦福机器人的运动学方程
三、反向运动学实例
（1）求θ1：
（2）求θ2：
（3）求θ3：
（4）求θ4：
（5）求θ5：
（6）求θ6：
（1）解可能不存在
（2）解的多重性
（3）求解方法的多样性
四、X=X（Q）形式运动学方程
1、RPY角法和欧拉角法
2、运动学方程X=X（q）
[小结]
通过本章的教学，使学生熟练掌握齐次变更的运算，熟悉工业机器人运动学方程，通过练习，让学生巩固机器人运动学的知识。
[教学后记]
[教学资料弥补]
升级会员 