专利名称：一种控制多轴线自动装置的运动部分位移的方法和装置的制作方法
技术领域：
本发明涉及一种用于控制多轴线自动装置运动部分沿着路径的位移的方法和装置。
在控制多轴线自动装置的领域中，人们已经知道，可以通过作用在位置、速度和电流上的PID(比例-积分-微分)型伺服控制回路来控制多轴线自动装置，以使得可以携带工具或把手的自动装置的终端部分沿着具有预定形状和速度分布的路径运动。人们还已经知道，可以通过降低各个轴线上的伺服控制的硬度来控制这种自动装置，以使得臂的位置作为其运动部分与其周围之间的相互作用力的函数而变化。特别地，例如US-A-5 742 138教导，在直角坐标系中利用参数来设置多轴线自动装置臂的硬度，以测量位置误差、计算力以及校正自动装置的运动部分的位置，从而获得其终端部件一定量的挠性。这种挠性可以用于执行把手部分的操作，例如抛光、除去毛刺或者抵抗压力，其中，接触力可以与为自动装置设计的运动相反。
人们还已经知道，在直角坐标系中可以提供设定点力，以沿着路径在受控力级别实现操作，例如用于抛光或除去毛刺。
在已知的系统中，有时提供路径编程界面以能够指定运动部分沿着其路径的参数，例如速度或者加速度，所述参数决定了作为时间函数的自动装置臂的终端部分沿着其路径的位置。
从US-A-4 874 997中还可以知道，可以利用脉宽调制在数值上控制自动装置的接合处的无刷电动机。
在这些已知的系统中，难以对多轴线自动装置进行编程，以便一方面可以通过速度和加速度来控制其终端部分的位移路径，另一方面可以控制在运动部分及其周围之间的接触点处与路径相切的力，同时符合与自动装置的机械结构相容的速度值和加速度值。
EP-A-0 349 291还公开了使自动装置从已选择的路径转向，以符合作为待处理部分的实际形状的函数的力的设定点。在该示例中，力的控制可以不遵循路径，而这一点不适合于某些必须精确遵循路径的应用。
本发明旨在通过提供一种方法来克服上述缺陷，其中，可以精确地控制周围与自动装置的运动部分(尤其是其携带的部件或工具)之间的相互作用。
根据这一精神，本发明涉及一种控制多轴线自动装置的运动部分沿着路径的位移的方法，其中包括以下步骤向路径生成器提供运动指令，所述运动指令至少包括与所述路径的形状和力的设定点相关的信息；计算外力信号，所述外力信号表示所述运动部分施加到其周围的力的至少一个分量；在预定的采样频率下向所述路径生成器提供所述外力信号；利用所述路径发生器，在预定的采用频率下，采用使作用于路径切线的外力的投影与作用于所述切线的所述设定点的投影之差最小的方式，计算沿着所述路径的运动设定点；以及将所述运动设定点发送到能够将所述自动装置的至少一条轴线的运动设置成符合所述运动设定点的伺服控制装置。
从而，所述切向力伺服控制能够在使运动部分的速度适合所述路径的同时，保持所述路径的形状。
根据有利的方面，一种控制多轴线自动装置的运动部分的位移的方法可以包含以下特征中的一个或多个特征根据表示在所述自动装置的至少一个致动器中流动的电流的信息，来计算所述外力信号。
包括在计算所述外力信号时利用所述自动装置的动态模型的步骤。
包括在计算所述运动设定点时，向所述路径发生器提供至少一个速度极限值和/或至少一个加速度极限值，从而使所述设定点符合所述极限值的步骤。
本发明还提供了一种能够实现上述方法的装置，更具体地说，所述装置包括
路径发生器，用于计算作为运动指令的函数的运动设定点，所述运动指令至少包括与所述路径的形状和力的设定点相关的信息；以及力估计器，用于产生外力信号，所述外力信号表示所述运动部分作用于其周围的力的至少一个分量，所述力估计器还用于在预定的采样频率下将所述信号发送到所述路径发生器，其中，所述路径发生器用于在预定的采样频率下、以使得作用于路径切线的外力的投影与作用于所述切线的力的设定点的投影之差最小的方式、计算沿着所述路径的所述运动设定点，将所述运动设定点发送到能够对所述自动装置的至少一条轴线的运动进行设置的伺服控制装置。
有利地，所述装置包括程序解释部件，用于执行包含运动指令的程序，所述运动指令至少能够使得所述路径的形状和力的设定点被指定。
通过下面对符合本发明原理的方法的实现的描述，本发明将更加易于理解，而其他的优点也更加显而易见，这些描述仅以实施例的方式并参照以下附图给出
图1是示出在实现根据本发明的方法时，沿着路径移动铣刀的多轴线自动装置的原理图；图2是示出图1的自动装置的运动部分处的力的分解的原理图；图3是示出本发明的一般操作的方框图；图4是示出位置设定点发送到各个电动机的相应的伺服控制系统的方框图；图5是示出各个电动机的伺服控制的方框图；图6示出了用于估计外力的方法；以及图7示出了路径发生器用来产生运动设定点的算法。
图1中示出的自动装置R是具有六个接合处的多轴线自动装置，其能够以六个自由度运动。可以采用笛卡尔模式来控制自动装置R，其中，其自由度可以是沿着X、Y、Z三个轴的方向平移的三个自由度，以及绕所述轴转动RX、RY、RZ的三个自由度。还可以采用接合处模式来控制所述自动装置，其中，其自由度可以是绕接合处的六个铰链轴X1、X2、X3、X4、X5和X6的六个转动R1、R2、R3、R4、R5和R6。
所述自动装置与控制单元U相关，控制单元U在训练和使用期间控制所述自动装置的操作。
在训练期间也可以使用人工控制台B。
在示出的实施例中，自动装置R携带工具O，例如沿着路径T放置的铣刀。
在图2中可以更具体地看出，认为工具O是由自动装置R沿着待铣削部分的边缘B1放置的。路径T通常平行于所述边缘。工具O对边缘B1施加的力记作F。与路径T相切的该力的分量记作FT，而垂直于路径T的所述力的分量记作FN。
根据本发明，在控制工具O的位移的方法中使用切向分量FT的值。
在下面的描述中，广泛采用F，并且其代表自动装置臂的接合处中的转矩的向量，或者代表包括笛卡尔力FX、FY、FZ以及笛卡尔转矩MX、MY、MZ的外力的非共面直线对。
如图3所示，程序设计平台100使所述系统的用户能够以使得自动装置执行期望的操作的方式对自动装置进行编程。这些操作由程序来描述。所述程序设计平台使待训练的自动装置可以到达连续的位置，并能够编写用于描述运动顺序的程序、调试程序、运行程序、终止程序等。
为了执行所述程序，所述程序设计平台使用程序解释器200。该程序解释器执行使自动装置执行任务的程序。除了包含其他内容之外，被执行的程序尤其包含公知为运动指令的指令300。指令300描述了自动装置待执行的运动的类型(直线，曲线，……，等)以及表征运动的各种参数(目标点，速度，力，……)。
运动指令300被发送到路径发生器400。所述路径发生器用于执行所述运动指令，即，其用于有规律地(通常每隔4毫秒(ms)一次)产生作为运动类型和运动参数的函数的运动设定点500。这些运动设定点500代表了自动装置在各个时刻所要遵循的路径。伺服控制工业自动装置600包括具有六个轴线的工业臂和伺服控制系统。设计的伺服控制系统可以控制自动装置的六个电动机(一个电动机用于一条轴线)，从而使所述自动装置实际遵循的路径尽可能地接近由运动设定点500限定的路径。
在某些应用中，工业自动装置600的终端部件与其周围环境(例如，其正在铣削的部分)机械地相互作用。外力估计器700构成了用于获取被称为外力800的信号的装置，所述外力代表了自动装置施加到环境的力的数值和方向。该信号被实时地(通常每隔4毫秒(ms)一次)发送到路径发生器400。所述路径发生器利用该返回信息来修改运动设定点，以调节相互作用力。
所述程序设计平台包括使自动装置能够以交互模式位移的控制台，从而能够训练自动装置要到达的各种位置。所述控制台还用作编写和调试程序的程序设计界面。用于编写这种程序的语言为文本语言，但是其他类型的语言(例如图形语言)也可以用于本发明的上下文中。
所使用的语言包含了自动装置程序设计语言中的常用指令，所述指令用于指定不同类型的运动、控制运动的速度和加速度、以及控制如何安排所述指令的顺序。
下面通过实施例来描述与本发明有关的两种其他的指令。
指令MOVEJF(位置，工具，参数，力)指定自动装置必须使得其所携带的并且由参数“工具”限定的工具，从先前运动的最终位置移位到由“位置”参数限定的位置。以接合处模式执行位移，即，在运动过程中，各个轴的位移之间存在比例常数。参数“参数”指定运动过程中不能超过的速度和加速度。所述参数还指定了运动是否将继续进入之后的运动。参数“力”指定了在运动过程中与路径相切的力的期望值。只要未达到由“参数”限定的速度极限和加速度极限，所述自动装置就应当适应其沿着路径的位移，以保持切向力在数值上尽可能地接近由“力”参数所指定的值。
指令MOVELF(位置，工具，参数，力)与MOVEJF指令类似。二者唯一的区别在于，所述运动需要沿着直线执行。
可以很容易地想象类似的用于限定圆形路径、或由花键限定的路径、或其它路径的指令。
对于本发明来说，重要的是，这些指令首先限定了自动装置将要遵循的路径的形状，其次限定了自动装置在位移过程中将要施加的切向力的数值。
程序解释器200利用开发环境来执行所生成的程序。为了执行所述运动指令，所述程序解释器验证用户指定的参数的有效性，然后将运动指令300发送到路径生成器400。运动指令300包含来自于程序中指定的数值的信息，即310-路径形状的限定标志位表明路径类型(接合处或者直线)。如果类型为“接合处”，则给出起始点的接合处坐标q0和目标点的接合处坐标q1。如果类型为“直线”，则给出起始点的笛卡尔坐标X0和目标点的笛卡尔坐标X1；320-切向力的设定点这是由自动装置端部作用于周围的笛卡尔力的范数的期望值des；330-速度极限自动装置端部的笛卡尔速度的范数的极限值vmax；以及340-加速度极限自动装置端部的笛卡尔加速度的范数的极限值amax。
在本文描述的实施方案中，运动命令部件500由六个电动机的角位置qdes组成，每隔4ms由所述路径发生器对其更新一次。如图4所示，六个位置设定点qdes1、qdes2、……、qdes6中的每一个被发送到电动机601、602、……、606中的每一个的相应的伺服控制系统。
每个电动机都安装有位置和速度传感器。每个电动机的伺服控制系统都是比例-微分-积分(PID)型伺服系统。在图5中，“qdes，j”是轴线j上的位置设定点，“qmes，j”是测得的位置，“Kp，j”、“Ki，j”以及“Kd，j”是所述PID的各自的增益，“Ides，j”是电流设定点，“Imes，j”是测得的电流，而p是拉普拉斯变量。
其他的位置伺服控制技术也可以用于本发明的上下文中。例如，可以通过去耦技术和线性化技术来执行伺服控制。在这种情况下，不再逐轴线地执行所述伺服控制。在本实施例中，运动设定点500是各个电动机的期望的角位置。还可以采用如在笛卡尔坐标系中所表示的所述终端部件的期望的位置和速度。所述运动设定点还可以是连续的位置增量，或者是在各个电动机在各个时刻的期望的速度。对于本发明来说重要的一点是，所述路径发生器400实时地运转，以计算所述自动装置所要遵循的路径，并且伺服控制系统保证了所述自动装置尽可能接近地遵循所述路径。
在本发明的上下文中，可以利用多种装置来估计所述自动装置施加到外部的力。最简单的方法包括将力传感器设置在所述自动装置的端部。在这种情况下，采用笛卡尔坐标来表示外力800。还可以采用设置在所述自动装置上的多个位置的力传感器或者转矩传感器。
在图6提出和表示的实现中，希望提供一种能够利用位置伺服控制变量来估计所述外力的方案。
所采用的方法需要代表在各个电动机中流动的电流I的数值701。在本实施例中，所述数值等于在伺服控制系统中所使用的测得的电流Imes。
I＝Imes＝[Imes，1 Imes，2…Imes，6]同样可以利用电流控制信号，即I＝Ides＝[Ides，1 Ides，2…Ides，6]或者代表在电动机中流动的电流的任何其他测得的变量或计算的变量。
所采用电动机是无刷电动机。用于这种类型的电动机的最简单的模型702在于认为发送的转矩Cm 703与流过所述电动机的电流是成比例的，即Cm＝Kt.I在该表达式中，I和Cm是包含与各个轴线有关的信息的向量，Kt是对角矩阵。可以将更加复杂的模型用作模型702。
另外，所采用的方法需要代表自动装置711运动的信号，例如，分别由向量q，dq/dt，以及d2q/dt2表示的各个电动机的位置、速度、以及加速度。在本例中，这些值是根据伺服控制系统所采用的传感器的测量结果及其导数推导出的。
q＝qmesdq/dt＝d(qmes)/dtd2q/dt2＝d2(qmes)/dt2还可以利用所述位置设定点及其导数qdes，d(qdes)/dt以及d2(qdes)/dt2。
在该信息以及所述自动装置臂的动态模型(例如利用牛顿-欧拉公式建立的模型)712的基础上，计算向量Cd，假定所述自动装置未施加任何外力713，则所述向量Cd代表了所述自动装置的各个电动机在理论上应当施加的力矩。
利用在720处产生的差值，推导得到向量Cext 800，其构成了与自动装置施加到其周围的力800相对应的电动机转矩的估计值Cext＝Cm-Cd图7示出了所述路径发生器所采用的算法如何被分成四部分起始于切向力的设定点310、外力800以及曲线的形状320，在步骤410计算标量误差变量420。在步骤430，将速度极限330和加速度极限340投影到曲线320上。在投影的速度极限和加速度极限470的基础上，以及在所述误差变量420的基础上，调整器430计算沿着曲线440的期望的运动。然后根据曲线320的限定的形状以及沿着曲线440的限定的运动，在步骤450计算所述自动装置的运动设定点500。
下面描述四个部分中的每个部分A)计算误差变量(410-420)直线运动下面描述直线运动中误差变量420的计算。下面给定的计算包括计算力的设定点310和外力800之差在曲线320上的投影。
在直线运动过程中，可以采用如下的参量形式来表示所述自动装置的笛卡尔位置x(向量)x＝x0+s.(x1-x0)其中，x0是运动起始点的笛卡尔位置，x1是笛卡尔目标位置，而s是在0到1范围内的标量参数。可以采用以下形式重写上述公式x＝f(s) (公式1)对该公式进行微分，得到以下公式
v＝dx/dt＝df/ds.ds/dt (公式2)在该公式中，df/ds是与所述曲线相切的笛卡尔向量。所述笛卡尔向量u＝(df/ds)/‖df/ds‖是与所述曲线相切的一元向量。
在本实施例中，力设定点300是所述笛卡尔切向力的范数。从而给出期望的笛卡尔力向量Fdes＝fdes.u(公式3)由所述接合处转矩Cext的向量给出外力800。可以利用以下公式将该值变换到笛卡尔坐标系Fext＝J-t.Cext (公式4)其中J-t是所述自动装置的运动矩阵的转置矩阵的逆。
所选的命令变量420记为eps，其为期望的切向力与投影到所述路径的切线上的外力之差，即eps＝ut.(Fdes-Fext)不管采用什么样的函数f(圆，花键，等等)，只要可以对其进行微分，也就意味着在所有的点都可以定义它的切线，那么该计算都是有效的。
该计算是在力的设定点300是笛卡尔力向量的范数以及外力800是在接合处坐标系中给出的特殊情况下进行的。利用适当的坐标变换公式，例如，代换公式3和公式4，可以处理在任何坐标系中给出的情况。
B)计算误差变量(410-420)接合处运动当处理接合处运动时，可以采用如下的参量形式将所述自动装置的接合处位置q记为q＝q0+s.(q1-q0)其中，q0是运动起始时的接合处位置，q1是目标接合处的位置，而s是0到1之间的标量参数。可以采用以下形式将上述公式改写为q＝g(s)给出相应的笛卡尔位置
x＝k(q(s))其中，k是所述自动装置的直接的几何模型。
因此，可以通过选择函数f，像处理所述笛卡尔运动一样来处理所述接合处运动，例如f(s)＝k(q(s))C)将速度和加速度极限沿着曲线投影(460)在本实施例中，在笛卡尔坐标系中给出所述速度极限。换句话说，希望所述自动装置的速度v的范数应当小于给定的速度vmax，并且笛卡尔加速度dv/dt的范数应当小于给定的加速度amax。
定义以下值(ds/dt)max＝min(vmax/‖df/ds‖)；c.sqrt(amax/‖df/ds‖) (公式5)(d2s/dt2)max＝(amax-‖df/ds‖.(ds/dt)max)/‖d2f/ds2‖ (公式6)其中，c为常数，满足0＜c＜1。
sqrt()表示平方根函数。
假定以下式子|ds/dt|≤(ds/dt)max|d2s/dt2｜≤(d2s/dt2)max然后，根据公式2，可以得到以下式子‖v‖＝‖df/ds.ds/dt‖＝‖df/ds‖.‖ds/dt‖≤‖df/ds‖.(ds/dt)max从而，根据公式5，可以得到以下式子‖V‖≤vmax此外，通过对公式2进行微分，可以得到以下式子a＝df/ds.d2/dt2+d2f/ds2.(ds/dt)2从而‖a‖＝‖df/ds.d2s/dt2+d2f/ds2.(ds/dt)2‖≤‖df/ds‖.|d2s/dt2|+‖d2f/ds2‖.|ds/dt|2≤‖df/ds‖.(d2s/dt2)max|+‖d2f/ds2‖.(ds/dt)max2利用公式5和公式6的定义，可以得到以下式子
‖a‖≤amax从而，数值(ds/dt)max和(d2s/dt2)max限定了对s的导数的约束，其保证了可以满足所述笛卡尔速度极限和加速度极限。从而可以认为，这些数值是所述笛卡尔速度极限和加速度极限在s坐标上的投影。
类似地，通过利用函数h(s)＝k-1(f(s))而不是上面计算中的函数f，可以得到接合处速度极限和/或加速度极限。
D)调节器(430)调节器430的作用是生成沿着曲线440的运动，从而尽可能使所述误差变量420接近于零。
所采用的调节器是积分型调节器。可以采用以下形式来表示“传统的”积分调节器d(sdes)/dt＝K.epssdes＝∫d(sdes)/dt.dt其中，sdes是所述自动装置沿着公式1中以参数s为变量表示的曲线的期望位置，K为常数。在下面的方框图中示出了这一点，其中p表示拉普拉斯变量。
该图等价于 为了将所述速度极限和加速度极限包含进去，对上图修改如下
方框“clac(ds/dt)max”和“calc(d2s/dt2)max”计算作为由公式5和公式6(对于s＝sdes，估算df/dt和d2f/dt2的值)给出的sdes的函数的投影的加速度极限的速度。方框“satVel”使得作为(ds/dt)max的函数的输入速度z饱和。换句话说，利用了以下公式 类似地，satAccel使得作为(d2s/dt2)max的函数的加速度u饱和，即 在任何情况下，该方案保证了|d(sdes)/dt|＜(ds/dt)max从而保证了沿着所述路径的期望的笛卡尔速度的范数小于vmax。
当未达到最大速度时，则z＝d(sdes)/dt从而w＝d2(sdes)/dt2以及|d2(sdes)/dt2|＜(d2s/dt2)max其保证了所述笛卡尔加速度的范数小于amax。
在总的上下文中，所描述的算法保证了只要未达到最大的笛卡尔速度，则总是可以满足所述笛卡尔速度极限和所述笛卡尔加速度极限。
对于直线运动来说，当达到所述速度极限时，加速度为零，因此也满足了所述加速度极限。
E)计算(430)运动设定点(500)在本例中，所述运动设定点500是所述自动装置接合处坐标qdes。沿着所述曲线sdes，从期望的位置开始，可以利用如公式1所给出的曲线的参量公式来计算相应的笛卡尔位置设定点xdes＝f(sdes)此后，利用记为k-1的自动装置的几何模型的逆，可以得到所述接合处坐标qdes＝k-1(xdes)如果运动设定点为速度，则可以利用关于速度的相应的坐标变换(公式2以及自动装置运动矩阵的逆)。
权利要求
1.一种控制多轴线自动装置的运动部分沿着路径的位移的方法，其特征在于，所述方法包含以下步骤向路径生成器(400)提供运动指令(300)，所述运动指令至少包括与所述路径(320)的形状和力的设定点(310)相关的信息；计算外力信号(800)，所述外力信号表示所述运动部分(O)施加到其周围的力(F)的至少一个分量；在预定的采样频率下向所述路径生成器(400)提供所述外力信号(800)；利用所述路径发生器(400)，在预定的采用频率下，采用使作用于路径切线(T)的外力的投影(FT)与作用于所述切线的所述设定点的投影之差最小的方式，计算沿着所述路径(320)的运动设定点(500)；以及将所述运动设定点(500)发送到能够将所述自动装置(600)的至少一条轴线的运动设置成符合所述运动设定点的伺服控制装置(601-606)。
2.如权利要求1所述的方法，其特征在于，根据表示在所述自动装置(600)的至少一个致动器(601-606)中流动的电流的信息，来计算所述外力信号(800)。
3.如前述权利要求中的任意一项所述的方法，其特征在于，所述方法包括在计算所述外力信号(800)时利用所述自动装置(600)的动态模型(712)的步骤。
4.如前述权利要求中的任意一项所述的方法，其特征在于，所述方法包括在计算所述运动设定点(500)时，向所述路径发生器(400)提供至少一个速度极限值(330)和/或至少一个加速度极限值(340)，从而使所述设定点符合所述极限值的步骤。
5.一种控制多轴线自动装置的运动部分沿着路径的位移的装置，其特征在于，所述装置包括路径发生器(400)，用于计算作为运动指令(300)的函数的运动设定点(500)，所述运动指令至少包括与所述路径(320)的形状和力的设定点(310)相关的信息；以及力估计器(700)，用于产生外力信号(800)，所述外力信号表示所述运动部分(O)作用于其周围的力(F)的至少一个分量，所述力估计器还用于在预定的采样频率下将所述信号发送到所述路径发生器，其中，所述路径发生器用于在预定的采样频率下、以使得作用于路径切线(T)的外力的投影(FT)与作用于所述切线的力的设定点的投影之差最小的方式、计算沿着所述路径(320)的所述运动设定点(500)，将所述运动设定点(500)发送到能够对所述自动装置(600)的至少一条轴线的运动进行设置的伺服控制装置(601-606)。
6.如权利要求5所述的装置，其特征在于，所述装置包括程序解释部件(200)，用于执行包含运动指令(300)的程序，所述运动指令至少能够使得所述路径(320)的形状和力的设定点(310)被指定。
全文摘要
本发明方法包括向路径生成器(400)提供运动指令(300)，所述运动指令至少包括与路径(320)的形状和装入指令(310)相关的信息；计算有关的装入信号(800)；将所述装入信号(800)发送到所述路径生成器(400)；计算沿着所述路径的运动指令(500)，以这种方式来使得作用于路径切线的负荷的投影与作用于所述切线的指令的投影之差最小；并且将所述运动指令(500)发送到用于启动自动装置(600)的装置。还公开了一种包含用于实现所述控制的部件(200，400，700)的装置。
文档编号G05B19/19GK1894640SQ200480037380
公开日2007年1月10日 申请日期2004年12月17日 优先权日2003年12月19日
发明者露珂·兆丽 申请人:施托布利法韦日公司