第8章-机器人控制技术运动轨迹-机器人原理及控制技术-教学课件

8.1引言8.2目标物体的描述

8.3任务的描述8.4视觉8.5程序8.6传送带跟踪8.7位置之间的运动8.8关节运动8.9笛卡尔运动8.10本章小结第八章运动轨迹ChapterⅧMotionTrajectories8.1引言8.2

本章是机器人运动控制的基础，它分为四个主要部分:

第一部分：利用齐次坐标变换构造任务

第二部分：基于时间坐标轨迹的运动控制描述

第三部分：关节坐标运动的描述

第四部分：笛卡儿运动控制描述8.1引言（Introduction）本章是机器人运动控制的基础，它分为四个主要部分

任何刚性物体都能够用一个与该物体固定相联的坐标系来描述，给出该物体的图形表示及其坐标系统。只要说明该坐标系统的位置和方向，就足以在任何位置和方位上复现这个物体。如图8.1中的销钉，它的轴位于z轴上，半径为0.5，长度为6。R=0.5xzy6图8.1销钉的描述8.2目标物体的描述（ObjectDescription）任何刚性物体都能够用一个与该物体固定相联的坐

利用齐次变换来描述一个任务。任务内容是抓取如图8.1所示的一些销钉，然后把它们插入一个装配部件的孔中(见图8.2)。图8.2任务的描述8.3任务的描述（TaskDescription）z利用齐次变换来描述一个任务。任务内容是抓取如图规定机械手末端执行器（手爪）的一系列位置Pn(见图8.3)，就能把这一任务描述为相应于这些编号位置的机械手运动和动作的序列。图8.3末端执行器的位置MOVEP1接近销钉MOVEP2移动到销钉的位置GRASP抓住销钉MOVEP3垂直提起销钉MOVEP4按一定角度接近孔眼MOVEP5接触到孔眼时停止MOVEP6调整销钉的位置MOVEP7插入销钉RELEASE松开销钉MOVEP8离开规定机械手末端执行器（手爪）的一系列位置Pn(见图8

下面通过规定机械手的结构来确定任务结构。我们用三个变换的乘积描述机械手，从而任务描述中的位置就由下式取代

MOVEpn=MOVEZT6E

(8.1)其中Z：表示机械手相对于任务坐标系的位置；T6：表示机械手末端相对于机械手坐标的位置；E：表示末端执行器（手爪）相对于机械手末端坐标的位置。下面通过规定机械手的结构来确定任务结构。按照上述描述，机械手的位置由Z来确定，任务的执行就是改变抓手的位置。现在利用下列符号来描述任务的变化:

P销钉在基坐标中的位置；

H带有两孔眼的金属块在基坐标中的位置；

HHRi

金属块上第i个孔相对H坐标系的位置；

PPG

抓取销钉的抓手相对于销钉的位置；

PPA

抓手接近销钉；

PPD抓手提起销钉；

HRPHA

销钉接近第i个孔眼；

HRPCH

销钉接触孔眼；

HRPAL

销钉开始插入；

HRPN

插入后的销钉。按照上述描述，机械手的位置由Z来确现在，任务可由一系列变换式来描述，由此解出机械手的控制输入T6，这些变换式如下:

P1:ZT6E=PPA接近销钉

P2:ZT6E=PPG到达抓取销钉的位置

GRASP抓取销钉

P3:ZT6E=PPDPG提起销钉

P4:ZT6E=HHRiPHAPG接近第i个孔眼

P5:ZT6E=HHRiPCHPG接触第i个孔眼

P6:ZT6E=HHRiPALPG插入销钉

P7:ZT6E=HHRiPNPG插入完成

RELEASE松开手爪

P8:ZT6E=HHRiPNPA回到起始位置手爪相对于销钉的位置P手爪相对于第i个孔眼的位置HRi现在，任务可由一系列变换式来任务位置变换图如图8.4所示。

尽管这样表示可能显得复杂,但是说明了任务的基本结构。而且每一个变换表示了一个独立的情况。

图8.4任务位置变换图HRiHRiHRiHRiHRiPPAPGPGPGPGPGPGPAT6T6T6T6T6T6T6T6HPNEZHPALEZHHAEZPEZHPNEPDEZZHEEPCHZPZ任务位置变换图如图8.4所示。图8.4相应的P,H和Z坐标系如图8.5所示。由图8.5可知，Z为机械手坐标系，它定位在肩关节上,因此工作坐标系（基坐标系）位于机械手坐标的位置为

T6pz=-50由于机械手不能到达它自己的基座，所以我们把它放在基坐标系原点的后面，这样T6px=30。使T6py=0,并且保持两个坐标系的平行。

图8.5任务坐标系P,H和Z

相应的P,H和Z坐标系如图8.

由图8.5可知:(8.2)由图8.5可知:(8.2)

下面通过相对于机械手末端的变换来定义末端执行器,我们沿着这样的表示习惯:末端执行器的z轴指向执行任务的方向，而y轴表示手爪的开合方向，于是如图8.6所示的抓手就可描述为图8.6手爪变换(8.3)下面通过相对于机械手末端的变换来定义末端执行器,我们已经在图8.1中描述了销钉，现在再看一下带有两个孔眼的金属块H。H的正视图如图8.7所示，借助于变换矩阵HRi（i＝1、2，是孔眼的序号）来描述它的特征。HR1=

HR2=(8.4)(8.5)图8.7带有两个孔眼的金属块

HR[1]HR[2]yxxxzy10510H注意：式（8.4）和式（8.5）是分别沿H坐标的x轴旋转－90°再平移后得到。我们已经在图8.1中描述了销钉，

最重要的变换是销钉插入一个孔眼(见图8.8)。销钉的z轴必须与孔眼的轴一致。由于销钉具有圆柱的对称性，x、y轴的方向就可任意了。最后一个变换必须按照手爪在销钉上的部位来确定（见图8.9）。xyHRxzyzPN图8.8销钉插入孔眼

xxzzPyyPG5图8.9手爪在销钉上的位置(8.6)(8.7)最重要的变换是销钉插入一个孔眼(见图8.8)。现在我们通过示教方式利用机械手来确定前面的变换关系。将末端执行器放在销钉上面，处于它的抓取位置（图8.3中的P2），可得到下列变换式。

ZT6E=PPG（P2）(8.8)

上式可确定P

P=ZT6EPG-1(8.9)

手爪返回到靠近销钉的位置P1，于是有

ZT6E=PPA（P1）(8.10)

从而确定了PA

PA=P-1ZT6E(8.11)关于P的起始点可这样确定：把手爪中的销钉提起，移到起始位置P3，于是有

ZT6E=PPDPG（P3）(8.12)由上式确定了PD

PD=P-1ZT6EPG-1(8.13)现在我们通过示教方式利用机械手来确定前面的变换金属块H的位置由下式确定ZT6E=HHR1PNPG（P7）（8.14）解出HH=ZT6E（HR1PNPG）-1（8.15）销钉开始插入，PAL（见图5.10）由下式确定ZT6E=HHR1

PALPG（P6）(8.16）从而PAL=（HHR1）-1ZT6EPG-1（8.17）接触孔眼的销钉位置PCH以及接近触点的点PHA可确定为PCH=（HHR1）-1ZT6EPG-1（P5）(8.18）PHA=（HHR1）-1ZT6EPG-1（P4）（8.19）这样就定义所有的变换。PHAPCHPALXyzHR图8.10接近接触和开始插入

问题：式（8.17）~（8.19）三个表达式完全相同，为什么能描述三个不同的工位点？金属块H的位置由下式确定PHAPCHPALXyzHR图8.1

在求取任务执行过程中的销钉位置时，我们是把机械手放在销钉的上方，处于正确的抓取位置，然后求解变换式P。在实际系统中，我们还可以通过视觉获得销钉等物体的位置。假如有一个位置固定的摄像机，它能把销钉的方位表示为相对于它自身的坐标系的一个齐次变换,比如说PC，那么销钉相对于基坐标的位置就可表示为P=CAMPC(8.20)其中CAM是一个变换，表示摄像机坐标系在基坐标中的位置。在通过机械手确定P之后，CAM的值能通过摄取一幅图象来得到PC，从而解得CAM=PPC-1(8.21)摄像机的输入通过READ读取得到，READ(CAMERA,PC)P:=CAM+PC而P是要被确定的，我们用标识符来表示程序中的变换，“+”号表示矩阵相乘，“－”号表示与逆阵相乘。8.4视觉（Vision）在求取任务执行过程中的销钉位置时，我们是把

现在可以利用求解T6的变换式(见图8.4)来确定任务程序中从P1到P8的每个位置。为了把机械手移到P1,要求ZT6E=PPA(8.22)这就意味着T6=Z-1PPAE-1(8.23)要到达P2则需ZT6E=PPG(8.24)如此等等。8.5程序（Program）现在可以利用求解T6的变换式(在我们用变换表达式代替程序中P1—P8的各个位置时，先定义两个变量COORD和TOOL,它分别表示工作坐标系的一般表达式和抓手坐标系的一般表达式。然后,把所有的位置表达式写成如下形式T6TOOL=COORDPOS(8.25)对于第一步运动,COORD和TOOL定义为

COORD:=-Z+P;

TOOL:=E;因而第一步运动就是MOVEPA;现在的位置是相对于销钉来确定的,在销钉被抓取之后,我们把TOOL规定为

TOOL:=E–PG上式起到这样的作用:把机械手的末端重新规定为销钉的末端,插入两个销钉的程序就变为在我们用变换表达式代替程序中P1—TOOL:=E;固定抓手FORI:=1,2DOBEGINREAD(CAMERA,PC)销钉位置P:=CAM+PC;建立P

COORD:=-Z+P；相对于销钉的位置MOVEPA;接近MOVEPG;移至销钉上方GRASP;抓取

TOOL:=E–PC;抓手在销钉末端MOVEPD;起始点

HT:=HR[I];孔眼的位置

COORD:=-Z+H+HT;相对于孔眼的坐标MOVEPHA;接近孔眼MOVEPCH;接触孔眼MOVEPAL;调整销钉MOVEPN;插入销钉RELEASE;松开销钉

COORD:=-Z+H+HT+PN；相对于销钉的坐标

TOOL:=EMOVEPA脱离销钉ENDTOOL:=E;当销钉放在传送带上执行任务时，需要建立一个传送带与工作坐标的变换CONV(S)，它是传送带位置S的函数。然后根据传送带的坐标变换CONV(S)和销钉相对传送带的位置CONVP来确定销钉在工作坐标系中的位置PP=CONV(S)

CONVP（8.26）这样，所有的位置都可描述为相对于传送带的齐次变换。如果物体OBJ放在传送带上，就可以利用传送带变换写出它的变换表达式ZT6E=CONV(S)OBJFG（8.27）其中

CONV某个标量S的函数,描述传送带的位置;

CONVOBJ

相对于传送带描述物体;

OBJF描述装配部件方向的特征;

FG利用装配部件特征描述末端执行器。8.6传送带跟踪（Conveyortracking）当销钉放在传送带上执行任务时，需如果传送带上的物体停止在某个位置,而末端执行器处于抓取位置,OBJ就可确定为

OBJ=CONV-1(S)ZT6E(FG)-1(8.28)在任务执行过程中,T6变成传送带变量的函数T6=Z-1CONV(S)OBJFGE-1(8.29)如果不断地对T6求值,并转换成关节角度（解逆运动方程）,同时机械手跟随这些角度,那么机械手就会跟踪运动着的物体。如果传送带停止运动，机械手也会停止动作。这样,执行任务的程序就可写成如果传送带上的物体停止在某个位置,TOOL:=E固定抓手FORI:=1,2DOBEGINREAD(CAMERA,PC);插入销钉位置P:=-CONV[S]+CAM+P;建立P

COORD:=-Z+CONV[S]+P;相对于销钉的坐标MOVEPA;接近MOVEPC;在销钉上方GRASP;抓取销钉

TOOL:=E–PG;抓手在销钉末端MOVEPD;起始位置HT:=HR[I];孔眼的位置COORD:=-Z+H+HT;相对于孔眼的坐标MOVEPHA;接近孔眼MOVEPCH;接触孔眼MOVEPAI;调整销钉MOVEPN:插入RELEASE;松开销钉COORD:=-Z+H+HT+PN;相对于销钉的坐标TOOL:=E;MOVEPA;离开销钉END;TOOL:=E机械手的任务位置可以表示为一般形式T6TOOL=COORDPOS(8.30)其中T6表示机械手六个关节的变换;

TOOL变换表达式，描述手爪端点，或运动受到控制的物体;

COORD变换表达式，表示工作坐标系;

POS其余的变换表达式，描述所期望的手爪端点或物体的位置；为了到达所确定的机械手任务的任何位置,机械手的一系列位置运动可由下列方程确定T6TOOL1=COORD1

POS1T6TOOL2=COORD2

POS2(8.31)

T6TOOL3=COORD3POS38.7位置之间的运动（MotionBetweenPositions）机械手的任务位置可以表示为一般形式虽然任务位置的序列确定了任务，但并没有说明机械手是如何从一个位置移动到另一个位置的。利用下一个坐标系统和抓手，另外再定义一个POS变换，就可以完成上述工作。在位置1有

1T6TOOL1

=COORD1(s|t=0)1POS1

（8.32）下面利用终点位置TOOL2和COORD2再写一个变换表达式

1T6TOOL2

=COORD2(s|t=0)2POS1（8.33）由这两个方程我们能解得利用COORD2和TOOL2定义的对于位置1的变换2POS12POS1=COORD2-1(s|t=0)COORD1(s|t=0)1POS1TOOL1-1TOOL2

（8.34）于是，任何两点i和i+1之间的运动就是从T6=COORD2(s|t=0)2POS1TOOL2-1

（8.35）到T6=COORD2(s|t=0)2POS2TOOL2-1

（8.36）的运动。注意：在这些方程中，COORDi+1是运动坐标系统变量（S）的函数。

虽然任务位置的序列确定了任务，但

当然，要把机械手从一个位置移到下一个位置可以有许多方法。但是，每一个实际系统必须提供连续的位置和连续的速度。为了防止振荡和蠕动，还要求连续的加速度。以从位置到位置的运动时间Ti为基准，我们可以画出直线坐标、角坐标或者关节坐标，作为时间的函数。图8.11是一个典型的坐标图。由于并不需要在每一个中间点停留。可以先对这条轨迹进行线性近似，如图8.12所示。但是，由于速度和加速度在所有的轨迹定义点上都是不连续的。因而我们并不能实现这样一条线性轨迹。t图8.11起动和停止运动θt图8.12分段线性近似θ当然，要把机械手从一个位置移到下一个位置可以有许多

在从一段轨迹过渡到另一条轨迹时为了保证速度、位置和加速度的连续性，可以在区间-tacc<t<tacc上的位置定义成一种合适的时间函数f（t）。在一段轨迹终了之前，就开始从这段轨迹向下一个tacc过渡，而在新的轨迹终了之后，就完成了tacc的过渡。考察图8.13所示轨迹A-BT1T2CDCttacctaccBtacctacc0图8.13路径过渡

θ在从一段轨迹过渡到另一条轨迹时为了保证速度、位置和

在对轨迹路径段的时间进行估计时，我们假设在时间tacc内把机械手从静止状态加速度到最大速度。在路径段之间的过渡时间为2tacc。如果把f（t）选择为一个多项式，由于过渡阶段的对称性，实际上只需要一个四次多项式：

q=a4t4+a3t3+a2t2+a1t+a0(8.37)其中q表示广义的位置：关节的直线位移或角位移。通过求导并运用约束条件，求出式（8.37）中各项系数，可以得到表示过渡期间的位置q，速度

以及加速度的函数。(8.38)(8.39)(8.40)约束条件在对轨迹路径段的时间进行估计其中

ΔC=C–B(8.41)ΔB=A–B(8.42)(8.43)在t=tacc时刻过渡结束。已知值由下列各式给定

q=ΔC

h+B(8.44)

(8.45)(8.46)(8.47)其中

根据确定的加速时间tacc,我们可以得到使整个机械手能实现的坐标轨迹（见图8.14）。规划机械手的全部运动并非必要，我们只需要提前考虑一个位置。当时间t=T1–tacc时，就根据下列赋值句，以便开始向下一路径段过渡。T1：=T2；A：=X；当前位置B：=C；C：=D；ΔC：=C–B；ΔB：=A–B；t:=-TACC;重新启动时刻时间图8.14平滑轨迹运动

θ根据确定的加速时间tacc,我们可以得到使

本节，我们要考察机械手的关节运动，此时，运动变量q、和由式（8.38）—（8.47）给定，它们表示关节坐标。在旋转关节的情况下，表示关节角度θi，而在滑动关节的情况下，则表示关节位移di

。在一次过渡开始时（如在图8.13的A点）来描述这种类型的运动，并且假设：在t=T1–tacc时刻的当前关节坐标为J。当前路径终点C的相应坐标为JC。如果D点用一个运动坐标系统COORD（S）来表示，那么就令S=S1，对COORD（S）求值，S1为S在当前时刻t＝T1–tacc的值。从而JD=Solve(COORD(S1)POSTOOL-1)(8.48)其中，Solve为求解关节坐标（即解逆运动方程）的函数，它是以一个变换表达式为自变量。

8.8关节运动（JointMotion）本节，我们要考察机械手的关节运动于是，在向D点运动的过程中，每个关节的运动时间就由下式求得：ti=|JDi–JCi|/vi(8.49)其中，Vi为关节i的最大速度，路径段的时间取为T2=max(ti,2tacc)(8.50)然后,对完整的路径段进行赋值T1:=T2;JA:=J;当前位置JB:=JC;JC:=JD;

ΔJC:=JC–JB;ΔJB:=JA–JB;T:=-Tacc;重新起动时刻再利用式（8.38）—（8.48）,把JA,JB,…ΔJB代替A,B,…ΔB,就可求解机械手关节的定位点。于是，在向D点运动的过程中，每个关节的运动时间就由

然而,我们还没有考虑由于运动坐标系COORD(S)的影响，为此,可再对相对于终点D的关节坐标进行求值。我们称所求的关节坐标为JC2。此时t=T1，在这一时刻，我们把运动坐标变量S估计为S2=S|t=T1JC2=Solve(COORD(S2)POSTOOL-1)(8.51)计算诸定位点J时,如果简单地给J附加一个修正项以说明C点的运动,那么在机械手达到C点时,我们就会得到追踪C点的运动。J：=J+(JC2–JC)*((S–S1)/(S2–S1))说明C点运动的这一修正变化导致了一个比较次要的问题：在过渡的开始点A，我们利用下式不连续地改变了速度其中，S0为S|t=0。对于B点，可将表达式修改为JB：=JB+(JC2–JC)*((S0–S1)/(S2–S1))如果初始条件保持不变,机械手仍然会象原先一样向B点运动。如果初始条件不能保持，那我们还是要规定速度的连续性。然而,我们还没有考虑由于运动坐标系COORD因为机械手要追踪C点的运动，所以在修改B点的值以后，还必须重新计算ΔJB和ΔJC。最后，在t=T1–tacc时刻接近C点时，我们处于定位点J，对C点求值得到JC：=JC+(JC2–JC)*((S3-S1)/(S2-S1))其中，最终的路径段赋值句就变为JA：=J；当前位置JB：=JC+(JC2–JC)*((S3–S1)/(S2–S1));JC：=Solve(COORD(S1)+PDS-TOOL

)；T1：=T2；JC2：=Solve(COORD(S2)+PDS-TOOL)；JB：=JB–(JC2–JC)*((S0–S1)/(S2–S1));ΔJC:=JC–JB;ΔJB:=JA–JB;t:=-TACC重新起动时刻因为机械手要追踪C点的运动，所以笛卡尔运动可以非常简单地推广到圆柱坐标、球坐标以及其他正交坐标系统。如同关节坐标运动，轨迹线段是在由齐次变换式描述的位置之间定义的。笛卡尔运动与关节坐标运动的差别在于

笛卡尔运动：运动在笛卡尔坐标中是直观的。

关节坐标运动：运动在关节坐标中是线性的。笛卡尔运动的优点：轨迹段端点之间的运动很直观，容易定义，因而特别适合于最初和最终的轨迹线段。笛卡尔运动的缺点：它需要对机械手的定位点进行不断的求值，把它变换成各个关节坐标的运动。8.9笛卡尔运动（CartesianMotion）笛卡尔运动可以非常简单地推广到圆柱坐标、球坐标现在我们来研究机械手通过一次平移和两次转动时的情况。第一次转动是使抓手对准目标的方向。第二次转动则控制抓手具有合适的抓取姿态（定向）。利用两次转动，关节运动的范围就比较简单，而且也可以预测。

从点1到点2的运动可以用一个驱动变换D(γ)来表示，这是一个与γ有关的运动函数

T6=COORD2(S)2POS1D(γ)

TOOL2-1(8.52)

其中γ=t/T，t是从运动开始起算的时间，而T是整个运动的总时间。当开始运动时，γ=0，我们要求D(0)＝I，I为单位变换，在运动终了时，γ=1，我们要求

2POS2=2POS1D(1)(8.53)8.9.1位置之间的运动（MotionBetweenPositions）现在我们来研究机械手通过一次平移和两次转动时的从而D(1)=2POS1-12POS2(8.54)

如果令2POS1=P1，而2POS2=P2，并且把P1和P2的各列表示为向量P1n，P10，P1a，P1p以及P2n，P2a，P2p就有(8.55)(8.56)从而(8.55)(8.56)

然后对P1进行符号求逆，再右乘P2，得到（8.57）平移沿着连接P1和P2的直线进行并且由变换T(γ)来表示，第一次转动的作用是将抓手指向接近矢量P1a，再从P1旋转到P2位置上的接近矢量P2a，这次旋转是绕矢量k进行的。而k是通过P1和y轴围绕Z轴旋转一个角度Ψ而取得的，因此矢量P1k就由下列给出

(8.58)这一旋转可表示成Ra(γ)。而由式（2.70）给出，其中k由上式确定。然后对P1进行符号求逆，再右乘P2，得到（8.57）

第二次旋转是把表示抓手定向矢量P10，从P1旋转成P2位置上的定向矢量P20，即为Ro(γ)，于是，D(γ)就表示为D(γ)=T(γ)Ra(γ)Ro(γ)（8.59）

其中T、Ra和Ro具有下列形式：

式中x，y和z表示从P1到P2的平移分量。

(8.60)

第二次旋转是把表示抓手定向矢量P10，从P1(8.62)(8.63)(8.64)式中Ra(γ)表示绕P1k的角度为θ的旋转（见式8.58）。

(8.61)(8.62)(8.63)(8.64)式中Ra(γ)表示绕P

绕接近矢量的旋转Ro(γ)只是绕Z轴的一次旋转，可由式（2.34）给定Ro(γ)表示绕抓手的接近矢量角度为θ的旋转。式中(8.65)(8.67)(8.66)绕接近矢量的旋转Ro(γ)只是绕Z轴的一次旋转，可由D(γ)的右边三列为

令γ=1，我们就可按下列步骤求解x，y，z，θ，Ψ和Ф，用Ro(1)-1Ra(1)-1同时右乘式8.59的两边，使右边一列的元素与式8.57的元素相等。我们就得到平移变换T的x，y，z的值

D(1)

Ro(1)-1Ra(1)-1=T(1)(8.69)

x=p1n·(p2p–p1p)(8.70)

y=p1o·(p2p–p1p)(8.71)

z=

p1a·(p2p–p1p)(8.72)

（8.68）D(γ)的右边三列为D(1)Ro(1)-

用Ro(1)-1右乘式（8.59）的两边，然后用T(1)左乘两边，使第二、三、四列的元素与式（8.57）的元素相等。我们就可求解θ和Ψ

从而有(8.77)T(1)-1D(1)Ro(1)-1=Ra(1)

(8.73)

(8.74)(8.76)(8.75)用Ro(1)-1右乘式（8.59）的两边，然后用及（8.78）即为(8.80)最后，先用T(1)-1，再用Ra(1)-1左乘式（8.59）的两边，得到

Ra(1)-1

T(1)-1D(1)=Ro(1)(8.79)及（8.78）即为(8.80)最后，先用T(1)-1，再用R再使上式两边的两个矩阵中第二行第一列、第二行第二列的元素对应相等，就可得到

（8.81）（8.82）再使上式两边的两个矩阵中第二行第一列、第二行第

在上小节中，我们利用驱动变换D(γ)控制的一次平移和两次旋转来描述路径与运动

本节要描述路径段之间的过渡，描述方式与在关节坐标中路径过渡的情况（8.8节）相似。这就是说，假定机械手在T1–tacc时刻处于由JA描述的位置，向JB运动。在T1时刻达到JB，再向位置POSC运动，POSC由下列描述T6CTOOL=COORD(S)POSC

(8.84)

其中T6C是当机械手位于POSC时T6的值。T6=COORD2(S)2POS1

D(γ)TOOL2-1

(8.83)

8.9.2路径段之间的过渡（TransitionsBetweenPathsSegments）在上小节中，我们利用驱动变换D(γ)控制的一次平移

首先，先求出分别由JA和JB表示T6的两个值。T6A和T6B然后解出相应的位置变换POSA和POSB

POSA=COORD(S|t=-tacc

)-1T6ATOOL(8.85)

POSB=COORD(S|t=0)-1T6BTOOL

(8.86)

从B点到C点的运动可用一个驱动变换DBC来描述DBC(1)=POSB-1POSC(8.87)

T6=COORD(S)

POSBDBC(γ)TOOLC-1(8.88)

用另一个驱动变换DBA描述从B点返回A点的运动DBA(1)=POSB-1POSA(8.89)首先，先求出分别由JA和JB表示T6的两个值。

根据上述两个驱动变换表达式，描述从A点经过B点到达C点的运动可这样进行：把D(γ)的γx,γy,γz,γθ和γФ从xA、yA、zA、θA和ФA经过零点到达xC、yC、zC、θC和ФC

。这样就可以把坐标画成时间的函数，如图8.15所示。

tT1taccA-tacc∆BC∆CBq图8.15笛卡尔路径过渡

根据上述两个驱动变换表达式，描述从A

现在，我们可以定义ΔB和ΔC，并利用式（8.38）—（8.47）来描述过渡问题以及向着C点的直线运动。驱动变换的坐标由下式给定(8.91)(8.93)(8.92)(8.90)现在，我们可以定义ΔB和ΔC，并利用式（8.3在t=tacc时刻，过渡结束，各个定位点由下式给定(8.98)(8.94)(8.95)(8.96)(8.97)如果Ψ对于两个驱动变换并不相同，那么就必须在过渡期间把它由ΨA变成ΨC，我们可以作一线性变化在t=tacc时刻，过渡结束，各个定位点由下式给定(8.

【例5.1】这个例子说明机械手从点POS1经过POS2到达POS3的运动，其中如图8.16所示，在从点POS1到POS2的运动过程中，我们利用式(8.70)—(8.82)来确定x、y、z、Ψ、θ和φ。

x=0y=0z=0图8.16笛卡尔运动的例子yzxyzxpos1zxypos2yzxpos3【例5.1】这个例子说明机械手从点POS1经过POS

我们假定机械手正在从POS1接近POS2，而γ＝0.9，驱动变换于是就由式(8.68)确定

而机械手位置为

这样，机械手的运动就是从当前位置开始，经过POS2，到达POS3分别相应于第8.9.2节中的位置A，B，C我们假定机械手正在从POS1接近POS2，而γ＝0

驱动变换DBA通过从B到A的运动来确定驱动变换DBA通过从B到A的运动来确定

驱动参数xA、yA、zA、ΨA、θA和ФA可由式(8.70)—(8.82)求得，其中B=POS1，A=POS2

xA=1.00yA=0zA=0

从B点到C点的运动由驱动变换DBC描述，这时

驱动参数xA、yA、zA、ΨA、θA和ФA可由式(

再一次使用(8.70)—(8.82)，我们得到

xC=0yC=0zC=0,我们给加上，考虑

由于的符号，得到,我们给加上，考虑由于的符号，得到

xA=1.00yA=0zA=0

现在，我们能根据从A到C的过渡，求得其中任何一点的运动情况（图8.17）。T1T1T1T1T1-τacc10yz图8.17笛卡尔运动变量-τacc-τacc-τacc-τaccx1xA=1.00yA=0z

例如，当t=0，T1=10tacc，由式(5.94)确定为0.5，那么式(5.91)就给出各个分量的值当t=tacc，根据式(5.98)，γ=0.1，运动变量则由式(8.94)确定为例如，当t=0，T1=10tacc，由当t=T1-tacc,γ=0.9时，有当t=T1-tacc,γ=0.9时，有按照这种运动方式，机械手实际上并不经过轨迹上任何一点，而是从这些点的附近经过（见图8.18中的B点），机械手仅当停止在某一轨迹点时，它才经过该点。0-tacctaccBAB=CCDtx图8.18经过B的点运动按照这种运动方式，机械手实际上并不经过轨迹上任如果试图使机械手以非零速度经过一些中间轨迹点，那么，在所有的轨迹极值点处，机械手都会偏离轨迹点。这是因为机械手在经过极值点的前后，速度的正负号都必须改变（图8.19）。要解决这个问题，就要在每个极值点处，速度都为零。当然，这样就与前面所述的方法效果相当了。tx图8.19超调运动如果试图使机械手以非零速度经过一些中间轨迹点，

为了驱动机械手，我们将以某个固定的采样速度fsolve来完成这些运算。在这一节就要涉及对任何机械手进行fsolve的估计问题。

串链机械手是一种极其复杂的机械结构，对于一个机械手进行粗略的简单描述，是把它看作一种具有谐振频率fstructural和阻尼系数的结构，假定计算一个解J要花费的时间为1/fsolve，而使fsolve处的加速度成为脉冲，为了使这个脉冲不激起机械手的任何机构谐振，应该保证:

fsolve>10

fstructural(8.99)8.9.3求解计算周期（SolutionEvaluationPeriod）为了驱动机械手，我们将以某个固定的采样速度fs图5.20跟踪精度R/2R/2路径∆X路径∆y∆X图8.20跟踪精度如图8.20所示，假定机械手由一个半径为R的连杆构成，它位于距运动物体R/2之处，该物体以恒定的速度沿y轴前进。图5.20跟踪精度R/2R/2路径∆X路径

在这种情况下我们可以说明，最大跟踪误差∆x是两个求解点之间的距离∆y的函数

如果被跟踪物体以恒定速度VC运动，那么误差与求解频率的关系为

因此，对于斯坦福手，如果R=1m，VC=10cm/s，那么∆x=5×10－4mm，这是一个非常小的数字。(8.100)(8.101)在这种情况下我们可以说明，最大跟踪误差∆x是两个求

一个关节伺服系统应当具有大约为15fstructural的采样速率，就斯坦福手来说，这个速率高达300Hz。每隔1/fsolve得到一个解的定位点，在每个定位点之间进行插值计算，就可得到上述这些中间伺服定位点，假定求取一个解的时间为1/fsolve时刻就开始求解，这样的做法在运动坐标系统突然停止的情况下会带来一些问题，即机械手会超越正常的位置(8.102)对于斯坦福手来说，跟踪一个运动速度为10cm/s的传送带，超调量就是4mm。

8.9.4插值（Interpolation）一个关节伺服系统应当具有大约为15fstruct本章我们介绍了三个主要内容：任务的描述，适宜于中间路径运动的关节运动控制方法，以及适宜于起始端运动控制的笛卡尔运动控制方法。控制系统通过中间点不停地按式(8.38)—(8.47)计算和确定下一步运动，从而能连续地完成笛卡尔运动。笛卡尔坐标运动是由驱动变换D(γ)控制(参看式8.68)。关节坐标运动把运动学与动力学分离开来，它要求坐标系统之间的变换仅仅以运动学条件为根据，而不象笛卡尔运动系统那样，要考虑伺服速度，通过使笛卡尔坐标和关节坐标之间的变换速率与外形组态变化（而不是加速度的变化）发生一定的关系，就能很有意义地提高计算频率。8.10本章小结（Summary）本章我们介绍了三个主要内容：任务的描述，适宜于8.1引言8.2目标物体的描述

8.3任务的描述8.4视觉8.5程序8.6传送带跟踪8.7位置之间的运动8.8关节运动8.9笛卡尔运动8.10本章小结第八章运动轨迹ChapterⅧMotionTrajectories8.1引言8.2

本章是机器人运动控制的基础，它分为四个主要部分:

第一部分：利用齐次坐标变换构造任务

第二部分：基于时间坐标轨迹的运动控制描述

第三部分：关节坐标运动的描述

第四部分：笛卡儿运动控制描述8.1引言（Introduction）本章是机器人运动控制的基础，它分为四个主要部分

任何刚性物体都能够用一个与该物体固定相联的坐标系来描述，给出该物体的图形表示及其坐标系统。只要说明该坐标系统的位置和方向，就足以在任何位置和方位上复现这个物体。如图8.1中的销钉，它的轴位于z轴上，半径为0.5，长度为6。R=0.5xzy6图8.1销钉的描述8.2目标物体的描述（ObjectDescription）任何刚性物体都能够用一个与该物体固定相联的坐

利用齐次变换来描述一个任务。任务内容是抓取如图8.1所示的一些销钉，然后把它们插入一个装配部件的孔中(见图8.2)。图8.2任务的描述8.3任务的描述（TaskDescription）z利用齐次变换来描述一个任务。任务内容是抓取如图规定机械手末端执行器（手爪）的一系列位置Pn(见图8.3)，就能把这一任务描述为相应于这些编号位置的机械手运动和动作的序列。图8.3末端执行器的位置MOVEP1接近销钉MOVEP2移动到销钉的位置GRASP抓住销钉MOVEP3垂直提起销钉MOVEP4按一定角度接近孔眼MOVEP5接触到孔眼时停止MOVEP6调整销钉的位置MOVEP7插入销钉RELEASE松开销钉MOVEP8离开规定机械手末端执行器（手爪）的一系列位置Pn(见图8

下面通过规定机械手的结构来确定任务结构。我们用三个变换的乘积描述机械手，从而任务描述中的位置就由下式取代

MOVEpn=MOVEZT6E

(8.1)其中Z：表示机械手相对于任务坐标系的位置；T6：表示机械手末端相对于机械手坐标的位置；E：表示末端执行器（手爪）相对于机械手末端坐标的位置。下面通过规定机械手的结构来确定任务结构。按照上述描述，机械手的位置由Z来确定，任务的执行就是改变抓手的位置。现在利用下列符号来描述任务的变化:

P销钉在基坐标中的位置；

H带有两孔眼的金属块在基坐标中的位置；

HHRi

金属块上第i个孔相对H坐标系的位置；

PPG

抓取销钉的抓手相对于销钉的位置；

PPA

抓手接近销钉；

PPD抓手提起销钉；

HRPHA

销钉接近第i个孔眼；

HRPCH

销钉接触孔眼；

HRPAL

销钉开始插入；

HRPN

插入后的销钉。按照上述描述，机械手的位置由Z来确现在，任务可由一系列变换式来描述，由此解出机械手的控制输入T6，这些变换式如下:

P1:ZT6E=PPA接近销钉

P2:ZT6E=PPG到达抓取销钉的位置

GRASP抓取销钉

P3:ZT6E=PPDPG提起销钉

P4:ZT6E=HHRiPHAPG接近第i个孔眼

P5:ZT6E=HHRiPCHPG接触第i个孔眼

P6:ZT6E=HHRiPALPG插入销钉

P7:ZT6E=HHRiPNPG插入完成

RELEASE松开手爪

P8:ZT6E=HHRiPNPA回到起始位置手爪相对于销钉的位置P手爪相对于第i个孔眼的位置HRi现在，任务可由一系列变换式来任务位置变换图如图8.4所示。

尽管这样表示可能显得复杂,但是说明了任务的基本结构。而且每一个变换表示了一个独立的情况。

图8.4任务位置变换图HRiHRiHRiHRiHRiPPAPGPGPGPGPGPGPAT6T6T6T6T6T6T6T6HPNEZHPALEZHHAEZPEZHPNEPDEZZHEEPCHZPZ任务位置变换图如图8.4所示。图8.4相应的P,H和Z坐标系如图8.5所示。由图8.5可知，Z为机械手坐标系，它定位在肩关节上,因此工作坐标系（基坐标系）位于机械手坐标的位置为

T6pz=-50由于机械手不能到达它自己的基座，所以我们把它放在基坐标系原点的后面，这样T6px=30。使T6py=0,并且保持两个坐标系的平行。

图8