机器人位置运动学(上课用)课件

机器人位置运动学(上课用)46、法律有权打破平静。——马·格林47、在一千磅法律里，没有一盎司仁爱。——英国48、法律一多，公正就少。——托·富勒49、犯罪总是以惩罚相补偿；只有处罚才能使犯罪得到偿还。——达雷尔50、弱者比强者更能得到法律的保护。——威·厄尔机器人位置运动学(上课用)机器人位置运动学(上课用)46、法律有权打破平静。——马·格林47、在一千磅法律里，没有一盎司仁爱。——英国48、法律一多，公正就少。——托·富勒49、犯罪总是以惩罚相补偿；只有处罚才能使犯罪得到偿还。——达雷尔50、弱者比强者更能得到法律的保护。——威·厄尔第2章机器人运动学§2.1引言§2.2机器人机构§2.3机器人运动学的矩阵表示§2.4齐次变换矩阵§2.5变换的表示§2.6变换矩阵的逆§2.7机器人的正逆运动学§2.8机器人正运动学方程的D-H表示法§2.9机器人的逆运动学解§2.10机器人的逆运动学编程2§2.1引言位置运动学正运动学∶逆运动学∶关节变量位姿位姿关节变量3但实际上，为了使机器人能在三维空间运动，机器人通常具有多个自由度，并且有三维开环链式机构。而对于开环控制系统来说，由于没有反馈，如果关节和连杆有丝毫的偏差，该关节之后的所有关节的位置都会改变，而且没有反馈。所以，对于一个实际的机器人来说，即使设定所有的关节，也不能确保机器人的手准确地处于给定的位置。只有不断测量所有关节和连杆的参数，或者监控系统的末端才能知道机器人手的运动位置。看这样两幅图:6在第一个式中，如果连杆AB偏移，则O1A也会相应移动，但是，在等式右边，因为O1O2设定后是不变的的，所以只需测出AB和O1A的变化，O2B就可测得了。而在第二个式中，如果左式中的AB变化了，显然，我们是无法预测O1C的变化的，除非AB，O1A和BC的变化都被测得。二者本质区别:左图是一个闭环机构，而右图是一个开环机构。让我们分别列出两个机构的向量方程，用来表示这种区别。7该怎样弥补开环机器人的缺陷呢？通过运动学分析，调高控制准确度；借助摄像机等装置来构成闭环系统;增加连杆和关节强度来减少偏移。8§2.3机器人运动学的矩阵表示矩阵表示的范围：点、向量、坐标系、平移、旋转以及其他变换，还可以表示坐标系中的物体和其他运动元件。§2.3.1空间点的表示我们可以这样来表示P=axi+byj+czk其中ax,by,cz是参考坐标系中表示该点的坐标。显然，也可以用其他坐标来表示空间点的位置。∧∧∧9§2.3.2空间向量的表示向量可用三个起始和终止的坐标来表示。如果一个向量起始于A，终止于B，那么它可以表示为PAB=（Bx-Ax)i+(By-Ay)j+(Bz-Az)k如果一个向量的起点是原点，则上式就变成了点的表示形式，则有：P=axi+byj+czk其中ax,by,cz是该向量在参考坐标系中的分量。以上是我们比较熟悉的表示方法，下面我们来介绍一种矩阵表达的形式。∧∧∧∧∧∧10上述向量也可表示为这种表示法也可以稍作变化:我们加入一个比例因子w,如果x,y,z各除以w,则得到ax,by,cz。于是，这时向量可以写为：其中，ax=x/w,by=y/w等等11随着w的变化，向量大小也随之发生变化，这类似于计算机图形学中对图片的放大或缩小。让我们来讨论一下w的取值：w>1时，向量的所有分量都变小w=1时，各分量大小保持不变w<1时，向量的所有分量都变大w=0时，向量长度无穷大，方向由x,y,z确定注意：这就是矩阵表示法中方向向量的表示方法。接下来我们看这样一个例子:12例2.1有一个向量P=3i+5j+2k,按如下要求将其表示成矩阵形式：（1）比例因子为2（2）将它表示为方向的单位向量解：该向量可以表示为比例因子为2的矩阵形式，当比例因子为0时，则可以表示为方向向量，结果如下：∧∧∧和13接下来我们将方向向量变为单位向量。我们只需把每一个分量都除以三个分量平方和的开方，最终的答案是：14

在上一节中我们得知，每一个向量都可由它们所在参考坐标系中的三个分量表示，我们不妨用三个相互垂直的单位向量来表示一个中心位于参考坐标系原点的坐标系，分别为n,o,a，依次表示法线(normal)，指向(oritentation)，和接近(approach)。这样，坐标系就可以由三个向量以矩阵的形式表示为§2.3.3坐标系在固定参考坐标系原点的表示15§2.3.4坐标系在固定参考坐标系中的表示如果一个坐标系不在固定参考坐标系的原点，那么该坐标系的原点相对于参考坐标系该怎样表示呢？可以在该坐标系的原点与参考坐标系原点之间做一个向量P，而这个向量P由上节中提到的参考坐标系的三个坐标向量表示。这样，这个坐标系就可以由三个表示方向的单位向量以及第四个位置向量来表示。16在上式中，前三个向量是w=0的方向向量，表示该坐标系三个单位向量n,o,a的方向，而第四个w=1的向量表示该坐标系原点相对于参考坐标系的位置。与单位向量不同，向量P的长度十分重要，因而使用比例因子为1。想一想，右图中的F坐标系该怎样表示呢？（它位于参考坐标系的3，6，7的位置。n轴与x轴平行，o轴相对于y轴角度45°，a轴相对于z轴角度45°）17在上式中，前三个向量是w=0的方向向量，表示该坐标系三个单位向量n,o,a的方向，而第四个w=1的向量表示该坐标系原点相对于参考坐标系的位置。与单位向量不同，向量P的长度十分重要，因而使用比例因子为1。想一想，右图中的F坐标系该怎样表示呢？（它位于参考坐标系的3，6，7的位置。n轴与x轴平行，o轴相对于y轴角度45°，a轴相对于z轴角度45°）18§2.3.5刚体的表示我们该怎样对一个物体进行空间表示呢？通常的做法是：首先在它上面固连一个坐标系，再将该固连的坐标系在空间表示出来。因为物体一直和该坐标系固连在一起，所以相对于该坐标系的位姿是已知的，而这个坐标系又可以通过参考坐标系来表示。我们可以用矩阵表示这个位姿，其中坐标原点和相对于参考坐标系的表示该坐标姿态的三个向量同样可以用该矩阵表示出来。19这样以来，一个刚体在空间有几个自由度？正确的答案是：六个因为刚体除了有沿三条参考坐标轴移动的三个自由度外，它自身还可以绕这三个轴旋转。所以如果要全面地定义一物体，就需要6条独立的信息来描述物体原点在参考坐标系中的位置，以及物体关于这三个坐标轴的姿态。如下图所示。20这个矩阵中有几条信息？12条信息。因为最后一行的比例因子没有附加信息，所以我们将其排除。在剩下的12条中，9条为姿态信息，3条为位置信息。如果我们利用一些约束条件，是不是可以将上述信息减少到6条呢？答案是肯定的。但是，这需要利用6个必定存在的约束条件。这些条件来自于目前尚未利用的已知坐标系特性。21首先，三个向量n,o,a是相互垂直的其次，每个单位向量的长度必须为1我们可以将其转换为以下六个约束方程：n·o=0︱n︱=1n·a=0︱o︱=1a·o=0︱a︱=1对于左边的三个方程，我们也可以这样表示：n×o=a

若向量a=(a1,b1,c1)，向量b=(a2,b2,c2)，则a·b=a1a2+b1b2+c1c2；a×b

=|ijk||a1b1c1||a2b2c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)（i、j、k分别为空间中相互垂直的三条坐标轴的单位向量）。

22求解所缺元素的值，并用矩阵来表示这个坐标系。解：应用六个约束条件经过计算我们会得到：nx=±0.707，nz=0，oy=0，oz=1，ax=±0.707，ay=-0.707为什么会出现多组解呢？这是因为利用给出的参数我们得到了两组在相反方向相互垂直的向量。除此之外，nx与ax必须同号（由图上可知）。最终我们得到了如下两个矩阵：23除了这种解法，我们也可以利用n×o=a来求解。24§2.4齐次变换矩阵

变换矩阵应写成方阵形式理由：1.计算方型矩阵的逆要比计算长方形矩阵的逆容易的多。2.为使两矩阵相乘，它们的维数必须匹配。由于要以不同顺序将许多矩阵乘在一起来得到机器人运动方程，因此应采用方阵进行计算。具体做法：1.加入比例因子使之成为4x4矩阵，既表示姿态又表示位置。2.只表示姿态，去掉比例因子得到3x3矩阵。3.加入第四列全为零的位置数据以保持矩阵为方阵。这种形式的矩阵称为齐次矩阵，它们写为：25§2.5变换的表示

当空间的一个坐标系（一个向量、一个物体或一个运动坐标系）相对于固定的参考坐标系运动时，这一运动可以用类似于表示坐标系的方式来表示。这是因为变换本身就是坐标系状态的变化（表示坐标系位姿的变化），因此变换可以用坐标系来表示。变换常为如下几种形式中的一种：

1.纯平移2.绕一个轴的纯旋转3.平移与旋转的结合26§2.5.1纯平移变换的表示如果一坐标系（它也可能表示一个物体）在空间以不变的姿态运动，那么该变换就是纯平移。在这种情况下，它的表示方向的单位向量保持同一个方向不变。只需改变运动坐标系原点相对于参考坐标系的变换。27相对于固定参考坐标系的新的坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量求得。若用矩阵形式，新坐标系的表示可以通过坐标系左乘变换矩阵得到。由于在纯平移中方向向量不改变，变换矩阵T可以简单地表示为：其中dx,dy和dz是纯平移向量d相对于参考坐标系x,y和z轴的三个分量。可以看到，矩阵的前三列表示没有旋转运动（单位阵），而最后一列表示平移运动。28新的坐标系位置为：这个方程也可以用符号写为Fnew=Trans(dx,dy,dz)×Fold这种方法对于所有的变换都成立。可以注意到，方向向量经过纯平移后保持不变。但是，新的坐标系位置是d和p向量相加的结果，即d+p。最后应该注意到，齐次变换矩阵与矩阵乘法的关系使得到的新矩阵的维数和变换前相同。29§2.5.2绕轴纯旋转变换的表示

为简化绕轴旋转的推导，首先假设运动坐标系位于参考坐标系的原点并且与之平行，之后将结果推广到其他的旋转以及旋转的组合。假设坐标系（n,o,a）位于参考坐标系（x,y,z）的原点，

坐标系（n,o,a）绕参考坐标系的x轴旋转一个角度θ（逆时针方向为正），

再假设旋转坐标系（n,o,a）上有一点P相对于参考坐标系的坐标为Px,Py和Pz，相对于运动坐标系的坐标为Pn,Po和Pa。当坐标系绕x轴旋转时，坐标系上的点P也随坐标系一起旋转。旋转前后，P点坐标有何变化？30如图所示，在旋转之前，P点在两个坐标系中的坐标是相同的（两个坐标系位置相同，并且相互平行）。旋转后，该点坐标Pn,Po和Pa在旋转坐标系（n,o,a）中保持不变，但在参考坐标系中Px,Py和Pz却改变了。31从x轴来观察在二维平面上的同一点的坐标。结论：可以看出，Px不随坐标系统x轴的转动而改变，而Py和Pz却改变了。可以证明：32可见，为了得到在参考坐标系中的坐标，旋转坐标系中的点P（或向量P）的坐标必须左乘旋转矩阵。这个旋转矩阵只适用于绕参考坐标系的x轴做纯旋转变换的情况，它可表示为：Pxyz=Rot(x,)×Pnoa注意：旋转矩阵的第一行表示相对于x轴的位置，其值为1，0，0，它表示沿x轴的坐标没有改变。声明：为简化书写，习惯用符号Cθ表示cosθ以及用Sθ表示sinθ

。因此，旋转矩阵也可写为：同理可得绕y轴旋转，绕z轴旋转的转换矩阵如下：3334也可写为习惯的形式，以便于理解不同坐标系间的关系，为此可将该变换表示为(读做坐标系R相对于坐标系U的变换，将Pnoa表示为(点P相对于坐标系R)，将Pxyz表示为

（P相对于坐标系U）。则上式可简化为：

下面看一道例题：35例:旋转坐标系中有一点P（2，3，4），此坐标系绕参考坐标系x轴旋转90度。求旋转后该点相对于参考坐标系的坐标，并且用图解法检验结果。T36例:旋转坐标系中有一点P（2，3，4），此坐标系绕参考坐标系x轴旋转90度。求旋转后该点相对于参考坐标系的坐标，并且用图解法检验结果。T解：由于点P固连在旋转坐标系中，因此点P相对于旋转坐标系的坐标在旋转前后保持不变。该点相对于参考坐标系的坐标为：37用图解法解下面这道题可以得到相对于参考坐标系的坐标为2,-4,3例:旋转坐标系中有一点P（2，3，4），此坐标系绕参考坐标系x轴旋转90度。求旋转后该点相对于参考坐标系的坐标，并且用图解法检验结果。38§2.5.3复合变换复合变换是有固定参考坐标系和当前运动坐标系的一系列沿轴平移和绕轴旋转变换所组成的。任何变换都可以分解为按一定顺序的一组平移和旋转变换。例如，为了完成所要求的变换，可以先绕X轴旋转，再沿x,y,z轴平移，最后绕y轴旋转。

变换顺序很重要，如果颠倒两个依次变换的顺序，结果将会完全不同。为了探讨如何处理复合变换，假定坐标系（n,o,a）相对于参考坐标系（x,y,z）依次进行了下面三个变换：（1）绕x轴旋转度度（2）接着平移[L1,L2,L3]（分别相对于x,y,z轴）；（3）最后绕y轴旋转度。39比如点Pnoa固定在旋转坐标系,起初，旋转坐标系的原点与参考坐标系的原点重合。随着坐标系（n,o,a）相对于参考坐标系旋转或者平移，坐标系中的P点相对于参考坐标系也跟着改变。

第一次变换后，P点相对于参考坐标系的坐标可用下列方程进行计算:P1,xyz=Rot(x,a)×Pnoa其中，P1,xyz是第一次变换后该点相对于参考坐标系的坐标。

第二次变换后，该点相对于参考坐标系的坐标是：

P2,xyz=Trans(l1,l2,l3)×P1,xyz=Trans(l1,l2,l3)×Rot(x,a)×Pnoa40第三次变换后，该点相对于参考坐标系的坐标为：可见，每次变换后该点相对于参考坐标系的坐标都是通过用每个变换矩阵左乘该点的坐标得到的。当然，矩阵的顺序不能改变。矩阵书写的顺序和进行变换的顺序正好相反。例2.6固连在坐标系（n,o,a）上的点P（7,3,2）经历如下变换，求出变换后该点相对于参考坐标系的坐标。(1)绕z轴旋转90度；(2)接着绕y轴旋转90度；(3)接着再平移[4,-3,7]。41例2.6固连在坐标系（n,o,a）上的点P（7,3,2）经历如下变换，求出变换后该点相对于参考坐标系的坐标。(1)绕z轴旋转90度；(2)接着绕y轴旋转90度；(3)接着再平移[4,-3,7]。解：表示该变换的矩阵方程为：Pxyz=Trans(4,-3,7)Rot(y,90)Rot(z,90)Pnoa42下面这幅图，（n,o,a）坐标系首先绕z轴旋转90度，接着绕y轴旋转90度，最后相对于参考坐标系的x,y,z轴平移。坐标系中的P点相对于n,o,a轴的位置如图所示，最后该点在x,y,z轴上的坐标分别为2+4=6，7-3=4，3+7=10。（点P初位置（7,3,2）

）(1)(2)(3)原图43例2.7根据上例，假定(n,o,a)坐标系上的点P（7,3,2）经历相同变换，但变换按如下不同顺序进行，求出变换后该点相对于参考坐标系的坐标。（1）绕z轴旋转90度；（2）接着平移[4,-3,7]；（3）接着再绕y轴旋转90度。44例2.7根据上例，假定(n,o,a)坐标系上的点P（7,3,2）经历相同变换，但变换按如下不同顺序进行，求出变换后该点相对于参考坐标系的坐标。（1）绕z轴旋转90度；（2）接着平移[4,-3,7]；（3）接着再绕y轴旋转90度。解：表示该变换的矩阵方程为：Pxyz=Rot(y,90)Trans(4,-3,7)Rot(z,90)Pnoa=45例2.7(n,o,a)坐标系上的点P（7,3,2）经历相同变换，但变换按如下不同顺序进行，求出变换后该点相对于参考坐标系的坐标。（1）绕z轴旋转90度；（2）接着平移[4,-3,7]；（3）接着再绕y轴旋转90度。46§2.5.4

相对于旋转坐标系的变换以上所讨论的变换都是相对于固定参考坐标系。然而事实上，也有可能做相对于运动坐标系即当前坐标系的轴的变换。为计算当前坐标系中的点的坐标相对于参考坐标系的变化，这时需要右乘变换矩阵而不是左乘。由于运动坐标系中的点或物体的位置总是相对于运动坐标系测量的，所以总是右乘描述该点或物体的位置矩阵。47例2.8假设P（7,3,2）现在进行相同的变换，但所有变换都是相对于当前的运动坐标系，具体变换出如下。求出变换完成后该点相对于参考坐标系的坐标。（1）绕a轴旋转90度；（2）然后沿n,o,a轴平移[4,-3,7]；（3）接着绕o轴旋转90度。解：在本例中，因为所作变换是相对于当前坐标系的，因此右乘每个变换矩阵，可得表示该坐标的方程为：TB=Rot(a,90)Trans(4,-3,7)Rot(o,90)Pxyz=TB*Pnoa=48P（n=7,o=3,a=2）

结果与其他各例完全不同。下面的图展示了这一结果。同时应注意，在当前坐标系中p点的坐标7，3，2,变换后得到相对于参考坐标系的坐标0，6，0。（1）绕a轴旋转90度（2）然后沿n,o,a轴平移[4,-3,7]（3）接着绕o轴旋转90度49例2.9坐标系B绕x轴旋转90°，然后沿当前坐标系a轴做了3英寸的平移，然后在绕z轴旋转90，最后沿当前坐标系o轴做5英寸的平移。(a)写出描述该运动的方程。(b)求坐标系中的点p（1,5,4）相对于参考坐标系的最终位置。50例2.9坐标系B绕x轴旋转90°，然后沿当前坐标系a轴做了3英寸的平移，然后在绕z轴旋转90，最后沿当前坐标系o轴做5英寸的平移。(a)写出描述该运动的方程。(b)求坐标系中的点p（1,5,4）相对于参考坐标系的最终位置。解：在本例中，相对于参考坐标系以及当前坐标系的运动是交替进行的。(a)相应地左乘或右乘每个运动矩阵，得到：TB=Rot（z,90)Rot(x,90)Trans(0,0,3)Trans(0,5,0)(b)带入具体的矩阵并将它们相乘，得到：512.6变换矩阵的逆1.逆矩阵的应用：在机器人分通过析中有很多地方要用到矩阵的逆，在下面的例子中可以看到一种涉及变换矩阵的情况。图中，假设机器人要在零件p上钻孔而须向零件p处移动。机器人基座相对于参考坐标系u的位置用坐标系R来描述，机器人手用坐标系H来描述，末端执行器（即钻头的末端）用坐标系E来描述，零件的位置用坐标系P来描述。钻孔的点的位置于参考坐标系U可以通过两个独立的路径发生联系：一个是通过该零件的路径，另一个是通过机器人的路径。52这就是说，该零件中点E的位置可以通过从U变换到P，并从P变换到E来完成，或者从U变换到R，从R变换到H，再从H变换到E。因此，可以写出下面的方程：53由于机器人的基座位置在安装时就是已知的，因此变换(坐标系R相对于坐标系U的变换)是已知的。由于用于末端执行器的器械是已知的，其尺寸和结构也是已知的，所以（机器人末端执行器相对于机器人手的变换）也是已知的。此外，（零件相对于全局坐标系的变换）也是已知的。该位置可以通过视觉系统、传感器或其他类似仪器来确定。54加工就需要知道零件上钻孔的位置，所以也是已知的。此时，唯一未知的变换就是（机器人手相对于机器人基座的变换）。因此，必须找出机器人的关节变量（机器人旋转关节的角度以及滑动关节的连杆长度），以便将末端执行器定位在要钻孔的位置上。这就需要用到合适的矩阵的逆并通过左乘或右乘来求解。因此有：55该方程的正确性可以通过认为与相同来加以检验。因此，该方程可重写为：显然为了对机器人运动学进行分析，需要能够计算变换矩阵的逆。2.逆矩阵的计算：我们来看看关于x轴的简单旋转矩阵的求逆计算情况。关于x轴的旋转矩阵是：化简后得到56必须采用一下的步骤来计算矩阵的逆：计算矩阵的行列式；将矩阵转置；将转置矩阵的每个元素用它的子行列式（伴随矩阵）代替；用转换后的矩阵除以行列式。将上面的步骤用到该旋转，得到：57现在计算每一个子行列式（伴随矩阵）。例如，元素2.2的子行列式是C-0=C，元素1.1的子行列式是。可以注意到，这里的每一个元素的子行列式与其本身相同，因此有：由于原旋转矩阵的行列式为1，因此用矩阵除以行列式仍得出相同的结果。因此，关于x轴的旋转矩阵的逆的行列式与它的转置矩阵相同，即：58

具有这种特征的矩阵称为酉矩阵，也就是说所有的旋转矩阵都是酉矩阵。因此，计算旋转矩阵的逆就是将该矩阵转置。可以证明，关于y轴和z轴的旋转矩阵同样也是酉矩阵。应注意，只有旋转矩阵才是酉矩阵。如果一个矩阵不是一个简单的旋转矩阵，那么它也许就不是酉矩阵。59

以上结论只对简单的不表示位置的3×3旋转矩阵成立。对一个齐次的4×4变换矩阵而言，它的求逆可以将矩阵分为两部分。矩阵的旋转部分仍是酉矩阵，只需简单的转置；矩阵的位置部分是向量P分别与n，o，a向量点积的负值，其结果为：T=nxoxaxPxnyoyayPynzozazPz0001T=nxnynz-P.n0xoyoz-P.oaxayaz-P.a000160例2.10计算表示Rot(x,40°）的矩阵解：绕x轴旋转40°的矩阵为：-1Rot(x,40)=00000.766-0.643000.6430.76600001其矩阵的逆是：Rot(x,40)=00000.7660.64300-0.6430.76600001-1需注意的是，由于矩阵的位置向量为0，它与n,o,a向量的点积也为零。61解：根据先前的计算，变换矩阵的逆是：T=T=0.500.86630.8660-0.5201050001-10.50.8660-3.232001-50.866-0.50-1.5980001可以证明TT是单位阵。-1例2.11计算如下变换矩阵的逆62§2.7正逆运动学

坐标变换的最终目的是确定机器人各杆件之间的相互位置关系，通过关节角的值计算末端操作器在空间位置（正运动学）或预算出末端操作器在要求位置的各个关节角（逆运动学）。

主要包括以下内容：1)相对杆件的坐标系的确定；2)建立各连杆的模型矩阵A；3)正运动学算法；4)逆运动学算法。63由于机器人的基座位置在安装时就是已知的，因此变换是已知的。由于用于末端执行器的器械是已知的，其尺寸和结构也是已知的，所以也是已知的。此外，也是已知的。该位置可以通过视觉系统、传感器或其他类似仪器来确定。加工就需要知道零件上钻孔的位置，所以也是已知的。此时，唯一未知的变换就是。因此，必须找出机器人的关节变量（机器人旋转关节的角度以及滑动关节的连杆长度），以便将末端执行器定位在要钻孔的位置上。例如：64§2.8D-H表示法学习目标：1.理解D-H法原理2.学会用D-H法对机器人建模学习重点：1.给关节指定参考坐标系2.制定D-H参数表3.利用参数表计算转移矩阵65背景简介：

1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出了这一方法，后成为表示机器人以及对机器人建模的标准方法，应用广泛。

总体思想：

首先给每个关节指定坐标系，然后确定从一个关节到下一个关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化，将所有变化结合起来，就确定了末端关节与基座之间的总变化，从而建立运动学方程，进一步对其求解。661.第一个关节指定为关节n,第二个关节为n+1,其余关节以此类推。坐标系的确定2.Z轴确定规则：如果关节是旋转的，Z轴位于按右手规则旋转的方向，转角为关节变量。如果关节是滑动的，Z轴为沿直线运动的方向，连杆长度d为关节变量。关节n处Z轴下标为n-1。673.X轴确定规则情况1：两关节Z轴既不平行也不相交取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。情况2：两关节Z轴平行此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的一条公垂线。情况3：两关节Z轴相交取两条Z轴的叉积方向作为X轴。4.Y轴确定原则取X轴、Z轴叉积方向作为Y轴方向。（右手）5.变量选择原则用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量的距离;an+1表示关节偏移，an+1是从Zn到Zn+1沿Xn+1测量的距离;角α表示关节扭转,αn+1是从Zn到Zn+1绕Xn+1旋转的角度。

通常情况下，只有θ和d是关节变量。68例1：PUMA560运动学方程（六个自由度，全部是旋转关节）θ1θ2θ3θ4θ5θ6关节变量都是θ691.θn是从Xn-1到Xn绕Zn-1旋转的角度；2.dn是从Xn-1到Xn沿Zn-1测量的距离；3.an是从Zn-1到Zn沿Xn测量的距离；4.αn是从Zn-1到Zn绕Xn旋转的角度。701.θn是从Xn-1到Xn绕Zn-1旋转的角度；2.dn是从Xn-1到Xn沿Zn-1测量的距离；3.an是从Zn-1到Zn沿Xn测量的距离；4.αn是从Zn-1到Zn绕Xn旋转的角度。角度顺时针为负。71※到达下一坐标系的标准运动可以通过以下几个运动，将一个参考坐标系变换到下一个参考坐标系。1.绕Zn轴旋转，使得Xn和Xn+1互相平行。2.沿Zn轴平移dn+1距离，使得Xn和Xn+1共线1.θn+1是从Xn到Xn+1绕Zn旋转的角度；2.dn+1是从Xn到Xn+1沿Zn测量的距离；3.an+1是从Zn到Zn+1沿Xn+1测量的距离；4.αn+1是从Zn到Zn+1绕Xn+1旋转的角度。723沿Xn轴平移an+1的距离，使Zn和Zn+1的原点重合。1.θn+1是从Xn到Xn+1绕Zn旋转的角度；2.dn+1是从Xn到Xn+1沿Zn测量的距离；3.an+1是从Zn到Zn+1沿Xn+1测量的距离；4.αn+1是从Zn到Zn+1绕Xn+1旋转的角度。4将Zn轴绕Xn+1（Xn和Xn+1共线）轴旋转，使得Zn轴与Zn+1轴对准。这样就实现了从一个坐标系变换下一个坐标系73列出变换矩阵

由于所有的运动都是相对于当前坐标系而言的。因此，总的变换矩阵A等于各变换矩阵右乘。从而得到的结果如下：以此类推，总的变换矩阵为：1.θn+1是从Xn到Xn+1绕Zn旋转的角度；2.dn+1是从Xn到Xn+1沿Zn测量的距离；3.an+1是从Zn到Zn+1沿Xn+1测量的距离；4.αn+1是从Zn到Zn+1绕Xn+1旋转的角度。末端到基座的变换矩阵74零位校验：零位校验：零位校验：75零位校验：零位校验：零位校验：76注：令：77D-H参数表通过原理图确定各参数，制定D-H参数表如下：将各参数带入矩阵方程即可得到运动学方程，进一步求解。78例题对下图所示简单机器人，根据D-H法，建立必要坐标系及参数表。79第一步：根据D-H法建立坐标系的规则建立坐标系80第二步：将做好的坐标系简化为我们熟悉的线图形式81第三步：根据建立好的坐标系，确定各参数，并写入D-H参数表8283第四步：将参数代入A矩阵，可得到84第5步求出总变化矩阵85思考题86依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次变换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐标系中的空间描述，即已知q1,q2,…,qn，求，称为运动学正解；已知，求q1,q2,…,qn，称为运动学反解。上式称为运动方程。综上：87正解反解88§2.9机器人的逆运动学解给定机器人终端位姿，求各关节变量，称求机器人运动学逆解。让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步骤。前面例子最后方程为：求逆运动学方程的解89

根据第3行第4列元素对应相等可得到依次用左乘上面两个矩阵，得到：90根据1，4元素和2，4元素，可得到：将上面两个方程两边平方相加，并利用和差化积公式得到91已知于是可得到：依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到92接下来再一次利用式由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：93最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：θ1θ2θ3θ4θ5θ6关节变量都是θ94§2.10机器人的运动学编程在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯消去法等相对繁琐的算法。正确的算法是：95§2.11设计项目

利用本书中所介绍的四自由度机器人，结合本章所学的知识进行四自由度机器人的正逆运动学分析。SCARA型机器人的运动学模型的建立，包括机器人运动学方程的表示，以及运动学正解、逆解等，这些是研究机器人控制的重要基础，也是开放式机器人系统轨迹规划的重要基础。为了描述SCARA型机器人各连杆之间的数学关系，采用D-H法。SCARA型机器人操作臂可以看作是一个开式运动链。它是由一系列连杆通过转动或移动关节串联而成的。为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。96SCARA（SelectiveComplianceAssemblyRobotArm装配机器人臂）机器人坐标系的建立

1.SCARA机器人坐标系建立原则根据D-H坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则(1)轴沿着第n个关节的运动轴;基坐标系的选择为:当第一关节变量为零时，零坐标系与一坐标系重合。(2)轴垂直于轴并指向离开轴的方向。(3)轴的方向按右手定则确定。2.构件参数的确定根据D-H构件坐标系表示法，构件本身的结构参数、和相对位置参数、可由以下的方法确定:(1)为绕轴(按右手定则)由轴到轴的关节角。(2)为沿轴，将轴平移至轴的距离。(3)为沿轴从量至轴的距离。(4)为绕轴(按右手定则)由轴到轴的偏转角。973.变换矩阵的建立全部的连杆规定坐标系之后，就可以按照下列的顺序来建立相邻两连杆n-1和n之间的相对关系:(1)绕轴转角。(2)沿轴移动。(3)绕轴转角。(4)沿轴移动。这种关系可由表示连杆n对连杆n-1相对位置齐次变换来表征。即：展开上式得

98由于描述第n个连杆相对于第n-1连杆的位姿，对于SCARA教学机器人(四个自由度