第03章机器人的运动学和动力学

打南册拉大修教案首页课程名称农业机器人任课教师李玉柱第3章机器人运动学和动力学计划学时3教学目的和要求：概述，齐次坐标与动系位姿矩阵，了解平移和旋转的齐次变换；机器人的运动学方程的建立与求解*机器人的动力学*重点：机器人操作机运动学方程的建立及求解；工业机器人运动学方程机器人动力学难点：机器人动力学方程及雅可比矩阵基本原理思考题：简述齐次坐标与动系位姿矩阵基本原理。连杆参数及连杆坐标系如何建立？机器人动力学方程及雅可比矩阵基本原理是什么？第3章机器人运动学和动力学教学主要内容：3.2齐次坐标与动系位姿矩阵3.3齐次变换3.4机器操作机运动学方程的建立与求解3.5机器人运动学方程3.6机器人动力学本章将主要讨论机器人运动学和动力学基本问题。先后引入了齐次坐标与动系位姿矩阵、齐次变换，通过对机器人的位姿分析，介绍了机器人运动学方程；在此基础上有对机器人运动学方程进行了较为深入的探讨。3.1概述机器人，尤其是关节型机器人最有代表性。关节型机器人实质上是由一系列关节连接而成的空间连杆开式链机构，要研究关节型机器人，必须对运动学和动力学知识有一个基本的了解。分析机器人连杆的位置和姿态与关节角之间的关系，理论称为运动学，而研究机器人运动和受力之间的关系的理论那么是动力学。3.2齐次坐标与动系位姿矩阵点的位置描述在关节型机器人的位姿控制中，首先要精确描述各连杆的位置。为此，先定义一个固定的坐标系，其原点为机器人处于初始状态的正下方地面上的那个点，如图3-1(a)所示。记该坐标系为世界坐标系。闾凹图世界坐标系与点的位置描述在选定的直角坐标系｛A｝中，空间任一点P的位置可以用3X1的位置向量AP表示，其左上标表示选定的坐标系｛A｝,此时有_P~AP=pPLZ」式中：P、P、P一点P在坐标系｛A｝中的三个位置坐标XYZ分量，如图3-1(b)。齐次坐标将一个n维空间的点用n+1维坐标表示，那么该n+1维坐标即为n维坐标的齐次坐标。一般情况下o称为该齐次坐••••标中的比例因子，当取o=1时，其表示方法称为齐次坐标的规格化形式，即式中a=①P；b=①P；c=①P坐标轴方向的描述如图3-2所示，i、j、k分别直角坐标系中X>Y,Z坐标轴的单位矢量，用齐次坐标表示之，那么有：'=1000'Y=0100'Z=0010/2j由上述可知，若规定：4X1图3-2坐标轴方向的描述列阵［abc。0中第四个元素为零，且满足a2+b2+c2=1，那么［abc。］中的a、b、c表示某轴的方向。4X1列阵［abc。0中第四个元素不为零，那么［abco］表示空间某点的位置。图3-2中矢量u的方向用4X1列可表达为：u=［abc0］t其中，a=cosa,b=cosP,c=cosy。图3-2中矢量u的起点0为坐标原点，用4X1列阵可表达为：0=[0001P【例3-1】用齐次坐标表示图3-3中所示的矢量u,v,w的坐标方向。间GT=90挪=3b+r=^y(b)CC=3。麟小：，瑚"间a=的10'祯'F寸图3T用不同方向角表示方向矢鼠、！/、化解矢量：u:cosa=0,cos。=0.866,cosy=0.5u=[00.8660.50hv:cosa-0.866,cosP=0,cosy=0.5v-[0.86600.50hw:cosa-0.866,cosP-0.5,cosy-0w-[0.8660.500h动系的位姿表示在机器人的坐标系中，运动时相对于连杆不懂的坐标系称为静坐标系，简称静系；跟随连杆运动的坐标系称为动坐标系，简称动系。动系位置与姿态的描述是对动系原点位置及各坐标轴方向的描述。连杆的位姿描述

设有一个机器人的连杆，若给定了了连杆PL上某点的位置和该连杆在空间的姿态，那么称该连杆在空间是完全确定的。如图3-4所示，°为连杆上一点，OXYZ为与连杆固接的一个动坐标系，即为动系。的位置可用一齐次坐标表示为X0Y0Z01图3-4拉杆的位姿描述连杆PL在固定坐标系中（3-4）连杆的姿态可由动系的坐标轴方向表示。令鸟"分别为X，、Y，、Z'坐标轴的单位矢量，各单位方向矢量在静系上的分量为动系各坐标轴的方向余弦，以齐次坐标形式分别表示为n=[nnn0]ro=[ooo0》a=[aaa0pXYZXYZXYZ图3-4拉杆的位姿描述连杆PL在固定坐标系中（3-4）（3-5）T=\nnT=\nnoaX一XXX0p]=nYoYaYY0noaZZZZ0_0001」（3-6）显然，连杆的位姿表示就是对固连于连杆上的动系的位姿表示。图3书连杆的坐标系18｝位姿描述【例3-2】图3-5表示固连于连杆上的坐标系｛B｝位于OB点，乂广2,七=1，Zb=0。在XOY平面内，坐标系｛B｝相对于固定坐标系｛A｝有一个30。的偏转，试写出表示连杆位姿的坐标系｛B｝图3书连杆的坐标系18｝位姿描述解XB的方向列阵n=[cos30°cos60ocos90°0h=【0.8660.500]yb的方向列阵o=[cos120°cos30°cos90°0]t=[-0.50.86600]zb的方向列阵a=[0010]t坐标系｛B｝的位置阵列P=[2101]r那么动坐标系｛B｝的4X4矩阵表达式为TOC\o"1-5"\h\z\o"CurrentDocument"0.866-0.5020.50.86601T=00.86610\o"CurrentDocument"0001

手部位姿的描述TOC\o"1-5"\h\z机器人手部的位姿7厂、.如图3-6所示，可用固连於于手部的坐标系｛B｝的位口―一竺姿来表示。坐标系｛B｝由/m原点位置和三个单位矢4'图35亍部位豪的捕近量唯一确定如下。.原点取手部中心点为原点OB。.接近矢量关节轴方向的单位矢量«。.姿态矢量手指连线方向的单位矢量。。.法向矢量n为法向单位矢量，同时垂直于a、o矢量，即n=ax0。手部位姿矢量为从固定参考坐标系OXYZ原点指向于手部坐标系｛B｝原点的矢量P。手部的位姿可由4X4矩阵表示:T=^noa(3-7)noaXnoaYnoaZ0001【例3-3】图3-7表示手部抓握物体Q,T=^noa(3-7)解因为物体Q形心与手部坐标系OXYZ'的坐标原点O'，所以手部位置的4X1列阵为

p=11111]t4图3-7抓握物体Q的手部手部坐标系X，轴的方向可以用单位矢量"来表示：4图3-7抓握物体Q的手部n:a=90。，。=180。，Y=90。n=cosa=0n=cosP=-1n=cosy=0同理，手部坐标系Y'轴与Z'轴的方向分别用单位矢量。和a表示:o:o=一1,o=0,o=0a:a=0,a=0,a=—1根据式（3-7）可知，手部位姿可用矩阵表达为:TOC\o"1-5"\h\z\o"CurrentDocument"0—101T=[n\o"CurrentDocument"p]=-1001P00-110001（3）目标物位姿的描述T=[n任何一个物体在空间的位姿和姿态都可以用齐次矩阵来表示。【例3-4】如图3-8所示。楔块Q在图（a）的情况下可用6个点描述，矩阵表达式为:

1-1-100000211111-1044200111若让其绕Z1-1-100000211111-1044200111(4,0,0),那么楔块成为图(b)位姿，其齐次矩阵表达式为~4466441-1-111-1000044111111可见，用符号表示目标物的变换方式，不但可以记录物体移动的过程，也便于矩阵的运算。3)移动前的位置(b)移动后的位置3)移动前的位置(b)移动后的位置图3-8楔块Q位姿的齐次明阵表示3.3齐次变换在机器人中，手臂、手腕等被视为（连杆）刚体。而刚体的运动一般可以包括平移运动、旋转运动和平移加旋转运动。把刚体每次简单的运动用一个变换矩阵来表示，那么，多次运动即可用多个变换矩阵的积来表示，表示这个积的矩阵称为齐次变换矩阵。这样，用连杆的初始位姿矩阵乘以齐次变换矩阵，即可得到经过多次变换后该连杆的最终位姿矩阵。通过多个连杆位姿的传递，可以得到机器人末端操作器的位姿，即进行机器人正运动学的讨论。平移的齐次变换首先，介绍点在空间直角坐标系中的平移。如图3-9所示，空间某一点A，坐标为（X,X,X）当它平移至点A后，坐标ABC为(X',X',X')其中：ABCX'=X+AX(3-10)Y=/+AYZ=Z+AZA或写成如下形式：(3-10)AY，AZA1也可简写成：A=Trans(AX,AY,AZ)A(3-11)式中Trans（AX,AY,AZ）表示齐次坐标变换的平移算子，且100AX/、010Ar(3-12)Trans(AX,AY,AZ)二001AZ0001(3-12)式中，第四列元素AX、Ar、AZ分别表示沿坐标轴x、r、z的移动量。若算子左乘，表示坐标变换是相对于固定坐标系进行的，加入相对动坐标系机型坐标变换，那么算子应该右乘。由上述推导可以看出，平移变换的数学实质是求两个矢量的和，丁为平移变换矩阵。对于二维情况，它是3X3单位方阵。它是由一个2X2单位矩阵，和所求点的列矢量以及满足齐次坐标表达式而增加的一个行矢量"0n组成。对于三维情况，它是4X4单位方阵。完成平移变换的关键在于要构造一个变换矩阵T。平移的齐次变换公式（3-12）同样适用于坐标系、物体等的变换。【例3-5】如图3-10所示坐标系与物体的平移变换给出了下面三种情况：动坐标系｛A｝相对于固定坐标系的X0、匕、Z0轴做（-1，2,2）平移后到｛△'｝；动坐标系｛A｝相对于自身坐标系的X、r、Z轴分别作（-1，2,2）平移后到｛A"｝；物体Q相对于固定坐标系作（2,6,0）平移后到必。已知-0-101--1-1-111-1--1001000022A二,Q二00-11001100_0001__111111_

写出坐标系｛A'｝、｛A〃｝以及物体Q的矩阵表达式。xrz'r-1.5向坐标系山)物体侦£-.向宁站派系阡i位置距113-10物体的平移变换解动坐标系｛A｝的两个齐次坐标变换平移算子均为一100-1一Trans(AX,AY,AZ)=010200120001｛A'｝坐标系是动系｛A｝沿固定坐标系平移变换得来的，故算子左乘，｛A｝的矩阵表达式为一100-「一0-101_一0-100一A'=Trans(-1,2,2)A=0102-1001=-1003001200-1100-13000100010001一0-101]一100-「一0-10-1A〃=ATrans(-1,2,2)=-10010102=-100200-11001200-1-1000100010001物体Q的齐次坐标变换平移算子为一1002一0106Trans(AX,AY,AZ)=00100001

故有Qf=Trans(2,6,0)Q100001000010000100001026011001-1001-101111-10221001113116660011113316881001113-11点的旋转变挽或用矩阵表示为:X':Y':3-11点的旋转变挽或用矩阵表示为:X':Y':Z'L:」cos0sin00-sin0

cos00Xv:YZL:」经过平移坐标变换后，坐标系｛A'｝、｛A"｝以及物体q，的实际情况如图3-10所示。旋转的齐次变换（1）点在空间直接坐标系中绕坐标轴的旋转变换如图3-11所示，空间某一点A，坐标为（X.,Xb,X。），当它绕Z轴旋转0角后移至A'点，坐标为（x：,X；,X；）。A'与A点的关系为X'=Xcos0-Ysin0Y=Xsin0—Ycos0z:=Z*:'与：的齐次坐标分别为[x:匕z：1^t和[x：y：z：1^t，因此A点的旋转齐次变换过程为

X']cos0-sin000一XAAY'sin0cos0X']cos0-sin000一XAAY'sin0cos000YA二AZ0011ZAA1J_0000_Li(3-14)也可简写为A'=Rot(Z,0)A(3-15)式中，Rot(Z,0)表示齐次坐标变换时绕Z轴转动的齐次变换矩阵，又称旋转算子，旋转算子左乘表示相对于固定坐标系进行变换，旋转算子的内容为:c0-S000由c00000100001Rot(Z,0)=(3-16)式中，c0=cos0;s0=sin0;下同。同理，可写出绕X同理，可写出绕X轴转动的旋转算子和绕Y轴转动的旋10000c0-由00由c000001Rot(X,0)c00s000100-由0c000001Rot(匕0)

（2）点在空间直角坐标系绕过原点任意轴的一般旋转变换图3-12所示为点A绕任意过原点的单位矢量k旋转0（2）点在空间直角坐标系绕过原点任意轴的一般旋转变换图3-12所示为点A绕任意过原点的单位矢量k旋转0角的情况。kx、kY、kz分别为k矢量在固定参考系坐标轴x、图3TZ一般旋转变换Y、Z上的三个分量，且XYZ。可以证得，绕任意过原点的单位矢量k转0角的旋转算子为:Rot(k,0)=kkverS3+c0kkverS3+k50Rot(k,0)=kkverS3+c0kkverS3+k50kkverS3-k500kkverS3-k50kkverS3+c0YYkkverS3+k500kkverS3+k50kkverS3-k50kkverS3+c000001（3-19）式（3-19）称为一般旋转齐次变换通式，它概括了绕X轴、Y轴及Z轴进行旋转齐次变换的各种特殊情况，例如：当kx=1,即kY=kz=0时，那么由式（3-19）可得到式（3-17）当kY=1,即kx=kz=0时，那么由式（3-19）可得到式当kZ=L即kX=kY=0时，那么由式（3-19）可得到式（3-16）反之，若给出某个旋转算子noa0XXXnoa0R=YYYnoa0ZZZ0001那么可根据式（3-19）求出其等效转轴矢量k及等效角转角•A1.7:_7:_7r-sin。=±_；(o-a)2+(a-n)2+(n-o)22、’ZYXZYX0为：(o-a)2+(a-n)2+(n-O)2tan0=±xzyxn+o+a—1k=°Z-：YX2sin0k=a—%Y2sin0n-o

kz=（3）算子左右乘规那么若相对固定坐标系进行变换，那么算子左乘，若对动坐标系进行变换，那么算子右乘。3.4机器人操作机运动学方程的建立及求解描述机器人操作机上每一活动杆件在空间相对于绝对坐标系或相对于机座坐标系的位置及姿态的方程，称为机器人操作机的运动学方程。

操作机运动学研究机器人末端执行器相对于参考系的位置、速度和角速度以及加速度和角加速度。但是不考虑引起运动的力和力矩。机器人操作机末端执行器的位置和姿态问题，通常可分为两类基本问题。一类是运动学正问题，已知机器人操作机各运动副的参数和杆件的结构参数，求末端执行器相对于参考坐标系的位置和姿态。另一类是运动学逆问题，根据已给定的满足工作要求时末端执行器相对于参考坐标系的位置和姿态以及杆件的结构参数，求各运动副的运动参数。这是机器人设计中对其进行控制的关键，因为只有使各关键运动到逆解中求得的值，才能使末端执行器到达工作要求的位置和姿态。机器人运动学的研究重点是研究手部的位姿和运动，而手部位姿是机器人各杆件的尺寸、运动副类型及杆间的相互关系直接关联。因此杆件坐标系建立的研究十分重要。连杆参数及连杆坐标系的建立（1）坐标系连杆号的分配方法机器人的各连杆通过图机器人坐标系连杆的分配关节连接在一起，关节有移动副和转动副两种，按从机座到末端执行器的顺序，由低到高依次为各关节和各连杆编号，如图3-16所示。机座的编号为杆件0,与机座相连的杆件编号为杆件1，依次类推。机座与连杆1的关节编号为关节1，连杆1与连杆2的关节编号为关节2，依次类推，各连杆的坐标系Z轴方向与关节轴线重合（对于移动关节，Z轴线沿此关节移动方向）。图机器人坐标系连杆的分配（2）各坐标系的方位的确定机器人机械手是由一系w；芹-林列连接在一起的连杆（杆件）一:f构成的。需要用两个参数来描述一个连杆，即公法线距--，.一图连杆尺寸参数叫"离i，如图3-17所示。连杆两端有关节i和i+1。该连杆尺寸可以用两个量来描述：一个是两个关节轴线沿公垂线的距离匕，称为连杆长度；一个是垂直于匕的平面内两个轴线的夹角a,，称为连杆扭角。这两个参数为连杆的尺寸参数。机器人机械手坐标系的配置取决于机械手连杆连接的类型。有两种连接一一转动关节和棱柱联轴节。如图3-18，图3-19所示。其中七为两连杆的距离，。•为两个连杆的夹角。对于转动关节，6•为关节变量。连杆•的坐标系原点位于关节•和•+1的公法线与关节i+1轴线的交点上。如果两相邻连杆的轴线相交于一点，那么原点就在这一交点上。如果两轴线平行，那么就选择原点使对下一杆（其坐标原点已确定）的距离d.+1为零。连杆i的Z轴与关节i+1的轴线在一直线上，而X轴那么在连杆i和i+1的公法线上，其方向从'指向i+1，如图3-18所示。当两关节轴线相交时，X轴的方向与两矢量的交积Z._1xZ平行或反向平行，X轴的方向总是沿着公法线从转轴'指向i+1。当两轴x和乂平行且同向时，第'个转动关节的七为零。关节E关节计1图3T8转动关节连杆坐标系壶立示意图连杆坐标系间的变换矩阵（1）连杆坐标系间的齐次变换矩阵表示方法用An_1表示机器人连杆n坐标系的坐标变换成连杆n-1坐标系的坐标的齐次坐标变换矩阵，通常上把上标省略，写成气。对于n个关节的机器人，前一个关节向后一个关节的坐标齐次变换矩阵分别为An_1,An-2,...,A0也就是An,An_「...，A1其中，A"A1）表示杆件1上的1号坐标系到机座的0号坐标系的齐次坐标变换矩阵。（2）连杆坐标系间变化矩阵的确定

如图3-18和3-19所示，一旦对全部连杆规定坐标系后，就能按照下列的步骤建立相邻两连杆1和i+1之间的相对关系。绕Z-轴旋转e「角，使X-轴转到与X,同一平面内。沿Z轴平移一距离d，把X移到与X同一直线i-1ii-1i上。沿Xj轴平移一距离％，把连杆i-1的坐标系移动到使其原点与连杆'坐标系原点重合的地方。绕Xj旋转ai角，使Z^转到与Z「同一直线上。连杆i-1的坐标系经过上述变换与连杆'的坐标系重合。如果把相邻连杆对空间的关系成为A矩阵，那么根据上述变换步骤，从连杆'到连杆i-1的坐标系重合。如果把表相邻连杆相对于空间关系的矩阵称为A矩阵，那么根据上述变换步骤，从连杆'到连杆I-1的坐标变换矩阵A.为:iA=Rot(Z,0)Trans(0,0,d)Rot(XA=Rot(Z,0)Trans(0,0,d)Rot(X,a)icoseisin0i00一sin0icos0i000000i00011000i00100100ai0di110000cosa00i00一sinad000'「cos0一sin0cosasin0cos0cosa0sinai00sin0sina一cos0sinacosai0acos0asin0di1(3-21)同理，对联轴节的齐次坐标变换矩阵有cos0isincos0isin0i0-sin0cosacos0cosasina

i0sin0sina-cos0sinacosai0sin0sina-cos0sinad11i1实际上很多机器人在设计时，常常使某些连杆参数取特别值，如使a.=0或90°，也有的使d=0或a.=0，从而可以简化变换矩阵A的计算，这样也可简化控制。i3.5机器人运动学方程机器人运动学方程对机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。通常把描••••述一个连杆坐标系下与下一个连杆坐标系间相对关系的齐次变换矩阵，叫做A变换矩阵或A矩阵。•••如果A1矩阵表示第一连杆坐标系相对于固定坐标系的齐次变换，那么第一连杆坐标系相对于固定坐标系的位姿广1为TOC\o"1-5"\h\zT=AT

110如果A2矩阵表示第二连杆坐标系相对于第一连杆坐标系的齐次变换，那么第二连杆坐标系在固定坐标系中的位姿t可用A和A的成积来表示，并且A应该右乘，即2212T=AA212同理，若A3矩阵表示第三连杆坐标系相对于第二连杆坐标系的齐次变换，那么有

T=AAA3123如此类推，对于六连杆机器人，有下列矩阵(3-23)T=AAAAAA6123456(3-23)该等式称为机器人运动学方程，此式右边表示了从固定参考系到手部坐标系的各连杆坐标系之家的变换矩阵的连乘，左边T表示这些变换矩阵的乘积，也就是手部坐标系相6对于固定参考系的位姿。式(3-23)计算结果孔是一个4X4的矩阵，即TOC\o"1-5"\h\z%°XaxPxT—OYaYPY\o"CurrentDocument"6七ozaP/0001式中，前三列表示手部的姿态；第四列表示手部的位置。正向运动学的实例正向运动学主要解决机器人运动学方程的建立及手部位姿求解问题，下面给出建立机器人运动学方程的方法及实例。tai斯电福巩器人四梯功美节(c}f邮动套F圉『约斯坦推tai斯电福巩器人四梯功美节(c}f邮动套F圉『约斯坦推机器人及耗连忏的坐株案坦福机器人的手臂有两个转动关节（关节1和关节2）且两个转动关节的轴线相交于一点，一个移动关节（关节3）,共三个自由度：杆1绕固定坐标系的Z0轴旋转七；杆2绕杆1坐标系的Z轴旋转0；杆3绕杆2坐标系的Z轴平移d。手1223腕有三个转动关节，与转动关节的轴线相交于一点，共三个自由度：杆4绕杆3坐标系的Z轴旋转0；杆5绕杆4坐标34系的Z轴旋转0；杆6绕杆5坐标系的Z轴旋转0；XYZ4556666为手部坐标系，原点位于手部两手爪的中心，离手腕中心的距离为H，当夹持工件时，需确定它与被夹持工件上固连坐标系的相对位置关系和相对姿态关系。表3-1斯坦福机器人的各连杆参数杆件号关节转角0扭角a杆长〃距离d10-90°0020290°0d3030°0d404-90°0050590°006060°0H根据表3-1所示的斯坦福机器人各连杆的参数和齐次变换矩阵公式，可求得a:i

c90-s00c90s00「1000-1122s90c90知0-c900100A=11,A=22,A=10-1002010d3001d23000100010001c90-s00c90s00c9-s000一445566知0c90s90-c90知c900A=44,A=55,A=6640-1005010060010_0001__0001_0001T=MAT=MAMA=6123456nXnYnZ0oaPXXXoaPYYYoaPZZZ001式中:nXnY=c1=s1c(cccnXnY=c1=s1c(ccc-ss)-sss-245646256」c(ccc-ss)-sss245646256」-s(scc+cs)145646+c(scc+cs)145646nZoXoY=-s(ccc-ss)-csc245646256=c-c(ccc-ss)+sss-s(-scc+cs)1L245646256」145646=s1c(ccc-ss)-sss245646256」oZaXaYaZPXPYP=Z=s(ccc-sc)+csc245646256=c(ccc+sc)-sss124525145=s(ccc+sc)+css124525145=-scc+cs24525ccsH-s(cH-d)ccsH-s(cH-d)]+c(ssH+d)253」1452scsH+c(cH-d)]245253=c1L245=s1L245-[-s(ssH+d)1452设斯坦福机器人的起始位置为零位，如图3-21所示。现已知关节变量为:e=90。,e=90。,d=300mm,0=90。,0=90。,，0=90。123456机器人结构参数为：d2=100mm,H=50mm。并设H=0,那么n、o、a三个方向矢量不变，而位置矢量的分量PX、%弓分别为：P=csd-sdX12312P=ssd+cdY12312P=cd代人本例给出的已知参数值和变量值，求得数值解为一00-1-150010300T=610000001该4X4矩阵即为斯坦福机器人在题目给定情况下手部的位姿矩阵，即运动学正解。I君3-为斯坦福机器人手部及各祁件状态逆向运动学及实例反向运动学解决的问题是已知手部的位姿，求各个关节

的变量。在机器人的控制中，往往已知手部到达的目标位姿，需要求出关节变量，以驱动各关节的电机，使手部的位姿得到满足，这就是运动学的反向问题，也称逆向运动学。运动（3-25）学逆解如式（3-25）所示。（3-25）noaPXXXXnoaPYYYYnoaPZZZZ0001=123456图3-22所示为一个2连杆机器人，对于一个给定的位置和姿态，它具有两组解。虚线和实线各代表一组解，它们都能满足给定的位置和姿态。这就是多解性。多解性是由于解反三角函数方程产生的。显然，对于一个真实的机器人，只有一组解与实际情况相对应。为此，必须做出判断，以选择合适的解。通常采用剔除多余解的方法：根据关节运动空间来选择合适的解；选择一个最接近的解；根据避障要求选择合适的解；逐级剔除多余解。能否求得机器人运动学逆解的解析式是机器人的可解性问题。逆向运动学的求解是用一系列变换矩阵的逆A-1左乘，然后找出右端为常数的元素，并令这些元素与左端元素相等，这样就可以得出一个可以求解的三角函数方程式。

对于有解析解的机器人，求得它的解是运动学中最重要，而又是最困难的事情。现仍以斯坦福机器人为例来介绍反向求解的一种方法。【例3-9】如图3-20（b）所示，以6自由度斯坦福机器人为例，其连杆坐标系如图3-23所示。图卜23G白由度斯坦福机器人连朴坐标系解设坐标系｛6｝与坐标系｛5｝原点重合，其运动学方程T=AAAAAA=T=AAAAAA=6123456nXnYnZ0oaPXXXoaPYYYoaPZZZ001（3-26）现给出T6矩阵及各杆参数a、a、d，求关节变量。e，其中9=d。其中，A为坐标系｛1｝，相对于固定16331坐标系｛O｝的Z轴旋转e，然后绕自身坐标系X轴做a的旋O0111转变换，a1=90°，所以4=Rot(Z0,9)Rot(X,a)=cos9isin90000-10-sin91cos91000001(3-27)只要列出A-1在式(3-26)两边分别左乘运动学方程，即可得（1）A-1T=AAAAA=T1i623方程(3-28)456左端为A-1T二-s100-1000001nXnYnZ0oXoYoZ0aXaYaZ0PXPYPZ1f1(n)

f(n)

12f3(n)0f11(o)

f12(o)

f"0f1(a)f12(a)f3(a)f11(P)f\P)f\P)1式中，fj是缩写，其中f(i)=ci+sif\i)=-^2f3(i)=-sixi=n,o,aT1=AAAAA

623456c(ccc-ss

45646s(ccc-ss)+css=45646256s4c4c6

0-c(ccc+ss)+sss245646256-s(ccc+ss)-sss245646256-scs+cc456460ccs+sc24525ccs—sc2452ss0sd—cdd21（3-30）式（3-30）中第3行、第4列元素为常数，把式（3-29）对应的元素等同起来，可得（3-31）f3(P)=d2-电P+电Py=d2采用三角代换PX=pcos^,5=PsinT（3-32）式中，p=』p；+号伸二arctan[「/p]进行三角代换后可得:sin(中-0)=1d—2P,C0S(中-0)=±j1-[*]1NIpJsin(中-0)=1d—2P中-0=arctan2(3-33)0=arctan2(P,P)一arctan2d,+IP+P-d21YXL2yXY2式中，正、负号对应的两个解对应于01的两个可能解。（2）求02根据根据前述原那么，用A-1左乘方程式（3-28）得：A-1A-1T=AAAA（3-34）2163456查找右边的元素，这些元素是各关节的函数，计算矩阵后可知，第1行、第4列和第2行、第4列是院2d3的函数，因此可得s0d=c0P+s0P231X1Y-c0d=—P（3-35）（3-36）由于d3大于零（棱形导轨的伸展大于零），所以02有唯一解0=arctan电P+电P2PZ（3-37）（3）求d3用a-1左乘方程式（3-34）得:A-1A-1A-1T=AAA

3216456(3-38)因已经求得6，6，故由、d0、由、书的值为已知。计算式121122（3-38），令第3行、第4列元素相等，可以得到d3的方程式：d=由（c0P+50P）+c0P（3-39）321X1Y2Z（4）求04用A-1左乘式（3-38）得：A-1A-1A-1A-1T=AA（3-40）44321656计算矩阵式，因右端第3行、第3列元素为0，令左、右第3行、第3列元素相等，有:一50[c0(c一50[c0(c0a+50a)一50a]+c0(一50a+c0a)=021X1Y2Y41Y1Y解得：0=arctan2[-50a+c0a,c0(c0a+50a)一50a]41X1Y21X1Y(5)求05用A-1左乘式(3-40)得：A-1A-1A-1A-1A-1T=A5432166根据(3-43)左右两边对应的元素，可以得到505、即50=c0[c0(c0a+50a)一50a]+50(一50a5421X1Y2Y41Yc0=50(c0a+50a)+c0a521X1Y2Y解得(3-43)的方程，C05+c6a)（3-45）（3-41）（3-42）(3-44)0=arctan2(c0[c0(c0a+50a)一50a]+50(-50a+c0a),5421X1Y2Y41Y1Y.、50(c0a+50a)+c0a}(3-46)21X1Y2Y根据c0的表6(3-43)左右两边对应的元素，可以得到由亍50=-c0([c0[c0(c0O+50O)一50O]+法*65421X1Y2Y•50(-50O+c0O)}+50[50(c0O+50O)+c0O41X1Y521X1Y2Yc0=一50[c0(c0O+50O)一50O]+c0(-50O+c0O6421X1Y2Y41X1Y)（3-47）（3-48）解得：0=arctan[50/c0](3-49)3.6机器人动力学机器人动力学主要研究机器人运动与受力之间的关系，目的是对机器人进行控制、优化设计和仿真。机器人动力学主要解决动力学正问题和逆问题两类问题。动力学正问题是根据各关节的驱动力（力矩），求解机器人的运动（关节位移、速度和加速度），主要用于机器人的仿真；动力学逆问题是已知机器人关节的位移、速度和加速度，求解所需要的关节力（力矩），是实时控制的需要。其中雅克比矩阵式机器人速度和静力分析的基础。机器人雅克比矩阵机器人雅克比矩阵（简称雅克比）揭示了操作空间与关节空间映射关系。雅克比不仅表示操作空间与关节空间的速度映射关系，也表示二者之间力的传递关系，为确定机器人的静态关节力矩以及不同坐标系间的速度、加速度和静力的变换提供了便捷的方法。（1）机器人速度雅克比矩阵在数学上，雅克比矩阵是一个多元函数的偏导矩阵。假设有六个函数，每个函数有六个变量，即：

=f(X,X,X,X,X,X)TOC\o"1-5"\h\z1123456=f(X,X,X,X,X,X)2123456.=f(X,X,X,X,X,X)66123456可简写成Y=F(X).将其微分得dFdFdFd《=XdX1+X~dX2+••,+录dX6126dY=空LdX+匚dX+..・+坦dXdY=4dX+4dX+..・+4dX6dX1dX2dX6dY=4dX+4dX+..・+4dX6dX1dX2dX6dX1dX2dX6(3-50)(3-51)图3-24二白由度平面关节型机器人筒图,、,dF,可简写成：dY=--dX图3-24二白由度平面关节型机器人筒图一dF一，—式中，6X6矩阵一dX称为雅克比矩阵。机器人学中，雅克比是一个把关节速度矢量d变换手爪相对基坐标的广义速度矢量U的变换矩阵。【例3-10】图3-24所示为二自由度平面关节型机器人(2R机器人)，端点位置X、Y与关节0.02的关系为：X=lC0+1cY=lSo1+1s12(3-52)11212

x=x(e,e)即/"(e；e：)(3-53)将其微分得I叔axdX-广ae1dY=%de+ae11de+彳de1ae22aYiLde面22'axax一rdx〔aeaede==i2i[dY\aYa将其微分得Ide+彳de1ae22aYiLde面22'axax一rdx〔aeaede==i2i[dY\aYaeaYaedeL2」(3-54)axaeiaYaeiaxaeaY商2」若对式（3-55）进行运算，克比可写为:Jj"电_l2S若对式（3-55）进行运算，克比可写为:Jj"电_l2S12

[ice+ic一dX一de式中：dX=dY，d。=deL2」J称为图3-24所示2R机器人的速度雅克比，它反映了关节空间微小运动de与手部作业空间微小位移dX的关系。那么图3-24所示2R机器人的雅_l2%一侦12」（3-57）从J中元素的组成可见，J阵的值是关于ef02的函数。推而广之，杜宇口自由度机器人，关节变量可用广义关节变量q表示，q二风，q2.・・％]T，当关节为转动关节时q=e)；当关节为移动关节时，q=d，dq=[dq1，dq2.・・dqn]，反映了空间的微笑运动。机器人末端在操作空间的位置和方位可用末端的手爪的位姿x表示，它是关节变量的函数，X=X(q)，并且是一个6维列矢量。dX=[dX,dY,dZ,场X,胸y,胸z]T反映了操作空间的微笑运动，它是机器人末端微小线位移和微笑角位移(微笑转动)组成。因此，式(3-56)可写为dX=J(q)dq(3-58)式中，J(q)是6Xn维偏导数矩阵，称为n自由度机器人速度雅克比。(2)机器人速度分析利用机器人速度雅克比矩阵对机器人进行速度分析。对式(3-58)左右两边各除以dt得：dXdq,、—=J(q)(3-59)dtdr或表示为v=X=J(q)q式中v一机器人末端在操作空间中的广义速度；q—机器人关节在关节空间中的关节速度；J(q)一确定关节空间速度q与操作速度v之间的关系的雅克比矩阵。对于图3-24所示的2R机器人而言，J(q)是式(3-57)

所示的2X2矩阵。若令七J2分别为式（3-57）所示雅克比的第1列矢量和第2列矢量，那么式（3-60）可写为v=JB1+J202式中，右边第一项表示仅由第一个关节运动引起的端点速度；右边第二项表示仅由第二个关节引起的端点速度；总的端点速度为这两个速度矢量的合成。因此，机器人速度雅克比的每一列表示其他关节不动而某一关节运动产生的端点速度。图3-24所示二自由度机器人手部的速度为vv=Xv-12S212C2」0011-2-1Y-ls0-1slCO+1c-(1sO+1s)0-1s0ii212i2122(1c0+1c)0+1c01-ii212i2122-1-12S212C2」0011-2-1假如已知的1、02是时间的函数，即0广f®,02=f°，那么可求出该机器人手部在某一时刻的速度v=f°，即手部瞬时速度。反之，假如给定机器人手部速度，可由式（3-60）解出相应的关节速度为：q=J-1v【例3-11】如图3-25所示的二自由度机械手，手部沿固定坐标系x0轴正向以LOm/s的速度移动，杆长1广12=0.5m。设在某瞬间0广30。,02=60。，求相应瞬时的关节速度。式（3-57）知，二自由度机械手速度雅可比为-12"12lC12因此，逆雅可比为J-1=J-1=1lls0-lC0-1122由式（3-61）可知c11212l2"12-lS0-l"11212」,0=j-1U且u=Q,0]k，即;1■2-14"12-l"0-ls1;1■2-14"12-l"0-ls11212」l2C12C112121ll由-IC0-1TOC\o"1-5"\h\z122Lrad/s=-2rad/s0=°1,2=—质^0.5C0crad/s=-2rad/s0=_i-12=4rad/sTit恒因此，在该瞬时两关节的位置分别为01=300,02=-60°；速度分别为01=-2rad/s,02=4rad/s；手部瞬时速度为1m/s。机器人力雅克比矩阵静力计算机器人作业时与外界环境的接触会在机器人与环境之

间引起相互作用力和力矩。机器人各关节的驱动装置提供关节力或力矩，通过连杆传递到末端操作器，克服外界作用力和力矩。各关节的驱动力或力矩与末端操作器施加的力（广义力包括力和力矩）之间的关系，是机器人操作臂力控制的基础。本节讨论操作臂在静力状态下力的平衡关系。假定各关节“锁住”，机器人称为一个结构。这种“锁定用”的关节力矩与手部所支持的载荷或受到的外界环境作用的力取得静力平衡。求解这种“锁定用”的关节力矩，或求解在已知驱动力矩作用下手部的输出力就是对机器人操作臂的静力计算。（1）机器人力雅克比矩阵t=JtF假定关节无摩擦，并忽略各杆件的重力，那么广义关节力矩Tt=JtF式中T一广义关节力矩;F一机器人手部端点力Jt—nx6机器人力雅克比矩阵或力雅克比，并且是机器人速度雅克比J的转置矩阵。式（3-63）可用虚功原理证明。【例3-12】如图3-26图326点端执行器及名关节的虚仲移

所示，各个关节的虚位移组成机器人关节虚位移矢量5q.i末端操作器的虚位移矢量为5X理证明。【例3-12】如图3-26图326点端执行器及名关节的虚仲移d55q=[d55q=[5q5q=[dXdd5中5中5中h(3-64)Z...5qhn(3-65)设发生上述虚位移时，各关节力伟T‘，（，=1,2,...〃），环境作用在机器人手部端点上的力和力矩分别为-fnn+1和nn,n+l，由上述力和力矩所作的虚功可以由下式求出：5W=T5q+T5q+...T5q一fd一n5(3-66)1122nnn,n+1n,n+1(3-67)或写成5W=TT5q一Ft5X(3-67)根据虚位移原理，机器人处于平衡状态的充分必要条件是对任意的符合几何约束的虚位移有5W=0，又因dX—Jdq,代入得5W—Tt5q-Ft5X—Tt5q-FtJ5q—(T-JtF)5q(3—68)式中，5q表示几何上允许位移的关节独立变量，对任意的5q，欲使5W=0成立，必有T—JtF，式中，Jt与手部端点力和广义关节力矩之间的力传递有关，称为机器人力雅克比。机器人力雅克比正好是速度雅克比的转置矩阵。（2）机器人静力计算从操作臂手部端点力f与广义关节力矩T之间的关系可知，操作臂静力计算可分为两类问题:

已知外界环境对机器人手部的作用力尸，（即手部端点力F—F），利用式（3-63）求相应满足静力条件的关节驱动力矩'。已知关节驱动力矩T，确定机器人手部对外界环境的作用力和负载的质量。第二类问题是第一类问题的逆解。逆解的关系式为F=（Jt）-it（3-69）满秩，那么可利用最小二乘法求机器人的自由度不是6时，例如〃〉6时，力雅克比矩阵就不是方阵，那么Jt就没有逆解。所以，对第二类问题的求解就困难的多，一般情况不一定能得到惟一的解。如果F的维数比T的维数低，且J得F的估计值。满秩，那么可利用最小二乘法求【例3-13】图3-27所囹32?尹部端点.力F与关节力矩『示为一个二自由度平面关节机械手，已知手部端点力F=岭,F，忽略摩擦，q=0囹32?尹部端点.力F与关节力矩『解由式（3-57），该机械手的速度雅克比为J=V—4_〈电+12七

那么该机械手的力雅克比为—