机器人控制技术运动轨迹

8.1引言8.2目标物体的描述8.3任务的描述8.4视觉8.5程序8.6传送带跟踪8.7位置之间的运动8.8关节运动8.9笛卡尔运动8.10本章小结,第八章运动轨迹ChapterMotionTrajectories,本章是机器人运动控制的基础，它分为四个主要部分:第一部分：利用齐次坐标变换构造任务第二部分：基于时间坐标轨迹的运动控制描述第三部分：关节坐标运动的描述第四部分：笛卡儿运动控制描述,8.1引言（Introduction）,任何刚性物体都能够用一个与该物体固定相联的坐标系来描述，给出该物体的图形表示及其坐标系统。只要说明该坐标系统的位置和方向，就足以在任何位置和方位上复现这个物体。如图8.1中的销钉，它的轴位于z轴上，半径为0.5，长度为6。,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离开,下面通过规定机械手的结构来确定任务结构。我们用三个变换的乘积描述机械手，从而任务描述中的位置就由下式取代MOVEpn=MOVEZT6E(8.1)其中Z：表示机械手相对于任务坐标系的位置；T6：表示机械手末端相对于机械手坐标的位置；E：表示末端执行器（手爪）相对于机械手末端坐标的位置。,按照上述描述，机械手的位置由Z来确定，任务的执行就是改变抓手的位置。现在利用下列符号来描述任务的变化:P销钉在基坐标中的位置；H带有两孔眼的金属块在基坐标中的位置；HHRi金属块上第i个孔相对H坐标系的位置；PPG抓取销钉的抓手相对于销钉的位置；PPA抓手接近销钉；PPD抓手提起销钉；HRPHA销钉接近第i个孔眼；HRPCH销钉接触孔眼；HRPAL销钉开始插入；HRPN插入后的销钉。,现在，任务可由一系列变换式来描述，由此解出机械手的控制输入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回到起始位置,任务位置变换图如图8.4所示。尽管这样表示可能显得复杂,但是说明了任务的基本结构。而且每一个变换表示了一个独立的情况。,相应的P,H和Z坐标系如图8.5所示。由图8.5可知，Z为机械手坐标系，它定位在肩关节上,因此工作坐标系（基坐标系）位于机械手坐标的位置为T6pz=-50由于机械手不能到达它自己的基座，所以我们把它放在基坐标系原点的后面，这样T6px=30。使T6py=0,并且保持两个坐标系的平行。,图8.5任务坐标系P,H和Z,由图8.5可知:,(8.2),下面通过相对于机械手末端的变换来定义末端执行器,我们沿着这样的表示习惯:末端执行器的z轴指向执行任务的方向，而y轴表示手爪的开合方向，于是如图8.6所示的抓手就可描述为,图8.6手爪变换,(8.3),我们已经在图8.1中描述了销钉，现在再看一下带有两个孔眼的金属块H。H的正视图如图8.7所示，借助于变换矩阵HRi（i1、2，是孔眼的序号）来描述它的特征。,HR1=,HR2=,(8.4),(8.5),注意：式（8.4）和式（8.5）是分别沿H坐标的x轴旋转90再平移后得到。,最重要的变换是销钉插入一个孔眼(见图8.8)。销钉的z轴必须与孔眼的轴一致。由于销钉具有圆柱的对称性，x、y轴的方向就可任意了。最后一个变换必须按照手爪在销钉上的部位来确定（见图8.9）。,(8.6),(8.7),现在我们通过示教方式利用机械手来确定前面的变换关系。将末端执行器放在销钉上面，处于它的抓取位置（图8.3中的P2），可得到下列变换式。,ZT6E=PPG（P2）(8.8)上式可确定PP=ZT6EPG-1(8.9)手爪返回到靠近销钉的位置P1，于是有ZT6E=PPA（P1）(8.10)从而确定了PAPA=P-1ZT6E(8.11)关于P的起始点可这样确定：把手爪中的销钉提起，移到起始位置P3，于是有ZT6E=PPDPG（P3）(8.12)由上式确定了PDPD=P-1ZT6EPG-1(8.13),金属块H的位置由下式确定ZT6E=HHR1PNPG（P7）（8.14）解出HH=ZT6E（HR1PNPG）-1（8.15）销钉开始插入，PAL（见图5.10）由下式确定ZT6E=HHR1PALPG（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）这样就定义所有的变换。,问题：式（8.17）（8.19）三个表达式完全相同，为什么能描述三个不同的工位点？,在求取任务执行过程中的销钉位置时，我们是把机械手放在销钉的上方，处于正确的抓取位置，然后求解变换式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）,在我们用变换表达式代替程序中P1P8的各个位置时，先定义两个变量COORD和TOOL,它分别表示工作坐标系的一般表达式和抓手坐标系的一般表达式。然后,把所有的位置表达式写成如下形式T6TOOL=COORDPOS(8.25)对于第一步运动,COORD和TOOL定义为COORD:=-Z+P;TOOL:=E;因而第一步运动就是MOVEPA;现在的位置是相对于销钉来确定的,在销钉被抓取之后,我们把TOOL规定为TOOL:=EPG上式起到这样的作用:把机械手的末端重新规定为销钉的末端,插入两个销钉的程序就变为,TOOL:=E;固定抓手FORI:=1,2DOBEGINREAD(CAMERA,PC)销钉位置P:=CAM+PC;建立PCOORD:=-Z+P；相对于销钉的位置MOVEPA;接近MOVEPG;移至销钉上方GRASP;抓取TOOL:=EPC;抓手在销钉末端MOVEPD;起始点,HT:=HRI;孔眼的位置COORD:=-Z+H+HT;相对于孔眼的坐标MOVEPHA;接近孔眼MOVEPCH;接触孔眼MOVEPAL;调整销钉MOVEPN;插入销钉RELEASE;松开销钉COORD:=-Z+H+HT+PN；相对于销钉的坐标TOOL:=EMOVEPA脱离销钉END,当销钉放在传送带上执行任务时，需要建立一个传送带与工作坐标的变换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:=-CONVS+CAM+P;建立PCOORD:=-Z+CONVS+P;相对于销钉的坐标MOVEPA;接近MOVEPC;在销钉上方GRASP;抓取销钉TOOL:=EPG;抓手在销钉末端MOVEPD;起始位置,HT:=HRI;孔眼的位置COORD:=-Z+H+HT;相对于孔眼的坐标MOVEPHA;接近孔眼MOVEPCH;接触孔眼MOVEPAI;调整销钉MOVEPN:插入RELEASE;松开销钉COORD:=-Z+H+HT+PN;相对于销钉的坐标TOOL:=E;MOVEPA;离开销钉END;,机械手的任务位置可以表示为一般形式T6TOOL=COORDPOS(8.30)其中T6表示机械手六个关节的变换;TOOL变换表达式，描述手爪端点，或运动受到控制的物体;COORD变换表达式，表示工作坐标系;POS其余的变换表达式，描述所期望的手爪端点或物体的位置；为了到达所确定的机械手任务的任何位置,机械手的一系列位置运动可由下列方程确定T6TOOL1=COORD1POS1T6TOOL2=COORD2POS2(8.31)T6TOOL3=COORD3POS3,8.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所示。但是，由于速度和加速度在所有的轨迹定义点上都是不连续的。因而我们并不能实现这样一条线性轨迹。,在从一段轨迹过渡到另一条轨迹时为了保证速度、位置和加速度的连续性，可以在区间-tacct10fstructural(8.99),8.9.3求解计算周期（SolutionEvaluationPeriod）,如图8.20所示，假定机械手由一个半径为R的连杆构成，它位于距运动物体R/2之处，该物体以恒定的速度沿y轴前进。,在这种情况下我们可以说明，最大跟踪误差x是两个求解点之间的距离y的函数如果被跟踪物体以恒定速度VC运动，那么误差与求解频率的关系为因此，对于斯坦福手，如果R=1m，VC=10cm/s，那么x=5104mm，这是一个非常小的数字。,(8.100),(8.101),一个关节伺服系统应当具有大约为15fstructural的采样速率，就斯坦福手来说，这个速率高达300Hz。每隔1/fsolve得到一个解的定位点，在每个定位点之间进行插值计算，就可得到上述这些中间伺服定位点，假定求取一个解的时间为1/fsolve时刻就开始求解，这样的做法在运动坐标系统突然停止的情况下会带来一些问题，即机械手会超越正常的位置,(8.102),对于斯坦福手来说，跟踪一个运动速度为10cm/s的传送带，超调量就是4mm。,8.9.4插值（Interpo