CN102510793A

CN102510793A - 加速度连续的机器人轨迹生成系统和方法

Info

Publication number: CN102510793A
Application number: CN2010800037434A
Authority: CN
Inventors: 景奉水; 谭民; 李恩; 梁自泽; 侯增广; 杨德刚; 张凯良; 强艳辉
Original assignee: Institute of Automation of Chinese Academy of Science
Current assignee: Institute of Automation of Chinese Academy of Science
Priority date: 2010-06-04
Filing date: 2010-06-04
Publication date: 2012-06-20
Anticipated expiration: 2030-06-04
Also published as: US20120029699A1; CN102510793B; US8600554B2; WO2011150534A1

Abstract

加速度连续的机器人轨迹生成方法，从运动命令接口接收来自用户的运动命令，并将用户的运动命令送到笛卡尔空间轨迹生成器；将用户命令转换成笛卡尔空间下机器人末端的轨迹节点数据，然后转化成机器人关节空间下轨迹节点数据；计算每个运动伺服周期的关节位置、速度和加速度；本发明可以有效解决因加速度不连续，不可控而引起的运动机构的过冲或振动问题。

Description

加速度连续的机器人轨迹生成系统和方法技术领域

本发明涉及机器人，尤其是机器人轨迹生成系统和方法。背景技术

工业机器人依靠末端工具在笛卡尔坐标系空间的运动来完成作业任务，但机器人末端工具的运动不能被直接控制，而是通过控制机器人的关节运动来实现。在任一时间，机器人各个关节的位置、速度和加速度都决定了工具的位姿、速度和加速度，也就是决定了工具的运动轨迹。因此，为了控制机器人末端工具在每一时刻的位姿、速度和加速度，就必须转化成控制机器人关节在每一伺服周期的位置、速度和加速度，这就是机器人轨迹生成的目的。

机器人末端工具的在笛卡尔空间运动轨迹是可用一组彼此间隔一定时间的离散点（通常包括位姿、速度和加速度）来表示。如上所述，要控制机器人末端工具的运动，就要将末端工具的在笛卡尔空间运动轨迹上的离散时间点的位姿、速度和加速度转化成对应的关节空间运动轨迹上位置、速度和加速度。位姿的转换通过机器人运动学的逆计算实现。速度的转化通过机器人雅克比矩阵的逆计算实现。但是雅克比矩阵的逆计算有时会失败：当一个 6自由度机器人的某两个关节的轴线重合，即其处于奇异位姿时，机器人末端工具在笛卡尔空间就会丧失在某些方向上运动能力，在这些方向上速度要求也就不可能满足。此时，机器人雅克比矩阵为奇异矩阵，其逆不存在，因而不能计算出机器人在关节空间的速度。而关节空间的加速度计算是以关节空间的速度为基础的，所以机器人奇异位姿处也无法计算加速度。这就是说，在奇异位姿处，不能由机器人在笛卡尔空间的速度和加速度直接解出机器人在关^ Ϊ空间的速度和加速度。

关节空间运动轨迹上这些离散点的时间间隔往往远大于关节驱动器的伺服周期（典型的运动伺服周期为 5毫秒），为此需要在这些轨迹离散点之间插补更多的点，以满足关节驱动器的伺服周期要求。可以选择不同的插值方式，例如梯形速度曲线插值和各种型样条曲线插值方法。如美国 Delta Tau公司生产的 PMAC多轴运动控制卡，就提供了 SPLINEl和 PVT两种 3次样条曲线插值方法。

SPLINEl样条插值方法只需要知道节点的位移（首尾节点速度为零）即可，因而不需处理机器人的关节速度求解问题，但是轨迹速度的控制精度较差；而 PVT方法除了需要输入各个节点的位移和速度，控制轨迹速度的精准度要比 SPLINEl样条插值法高，但用于机器人的运动控制，必须解决在奇异位姿时的关节速度求解问题。美国专利（US4663726) " Robot control utilizing cubic spline interpolation", 针对机器人关节控制提出了一种 3次样条插值方法，可以适应于节点速度己知和未知两种情况。

然而，上述各种样条曲线插值方法，都没有考虑节点的加速度。而对于运动控制来说，加速度不连续控易引发运动过冲或系统振动问题。发明内容

本发明的目的是提供一种加速度连续的机器人轨迹生成系统和方法。

按照本发明的一方面，一种加速度连续的机器人轨迹生成方法，包括步骤: 从运动命令接口接收来自用户的运动命令，并将用户的运动命令送到笛卡尔空间轨迹生成器；将用户命令转化成笛卡尔空间下机器人末端工具的轨迹节点数据；把笛卡尔空间下机器人末端工具的轨迹节点数据转化成机器人关节空间下轨迹节点数据；

计算每个运动伺服周期的关节位置、速度和加速度；

将关节空间轨迹插补器产生的关节的位置、速度和加速度与机器人参数数据库中存储的各机器人关节的速度、加速度极限值进行比较。

按照本发明的另一方面，一种加速度连续的机器人轨迹生成系统，包括：轨迹协调器，从运动命令接口接收来自用户的运动命令，并将用户的运动命令送到笛卡尔空间轨迹生成器；

笛卡尔空间轨迹生成器，将用户命令转化成笛卡尔空间下机器人末端工具的轨迹节点数据；

关节空间轨迹转化器，把笛卡尔空间下机器人末端工具的轨迹节点数据转化成机器人关节空间下轨迹节点数据；

关节空间轨迹插补器，计算每个运动伺服周期的关节的位置、速度和加速度；

轨迹协调器最后将关节空间轨迹插补器产生的关节的位置、速度和加速度与机器人参数数据库中存储的各机器人关节的速度、加速度极限值进行比较。

本发明适应性广，既可用于在线规划，更可用于离线规划；既可用于节点速度信息未知（取代 SPLINE1轨迹规划方法）情况，又可用于已知全部节点速度信息（取代 PVT轨迹规划方法）。本发明的轨迹在节点处的加速度连续可控，可有效解决因加速度不连续不可控而引起的运动机构的过冲或振动问题。附图说明

图 1 是多关节工业机器人示意图；

图 2 是机器人轨迹生成系统结构图；

图 3 是机器人关节运动轨迹节点示意图；

图 4 是关节轨迹插补函数选择流程图；

图 5 是关节轨迹伺服周期插补流程图。具体实施方式

下面结合附图和实施例对本发明进一步说明。

图 1是多关节工业机器人示意图。机器人有多个关节，每个关节是个运动副，即包括一个固定件和一个活动件，活动件在驱动器（图中未标出）的作用下可做相对固定件的一维运动。关节的位置用表示，速度用表示，加速度用#表示。关节 21的固定件固定在机器人基座上，而基座固定在大地上，大地坐标系为 0-xyz。关节 21的活动件通过构件与关节 22固定件连接；关节 22活动件通过构件和关节 23固定件连接；关节 23活动件通过构件和关节 24固定件连接；关节 24 活动件通过构件和关节 25固定件连接；关节 25活动件通过构件和关节 26固定件连接；关节 26活动件通过构件和机器人末端工具 27连接。

末端工具 27上附有一个坐标系 Ot-xyz 28,称为工具坐标系，其原点和称作 TCP 的末端工具作用点相重合。工具坐标系 28随末端工具 27—同运动，可用来表示末端工具的在笛卡尔空间坐标系，即大地坐标系 0- xyz 20下的位姿、速度和加速度。除了前面所述坐标系外，机器人还有工具坐标系还有一伴随坐标系（图中未标出）。伴随坐标系每时每刻都和工具坐标系重合，但是却相对大地坐标系静止。伴随坐标系得引入，是为了表示速度和加速度的方便。

一种常用的机器人末端工具位姿表示方式是齐次位姿矩阵：

ux vx wx lx

uy vy wy ly

uz vz wz Iz

0 0 0 1

上式中，向量 (/χ，/;，/_ζ 表示工具坐标系 28的原点，即末端工具的 TCP，在大地坐标系 0- xyz 20中的位置坐标； (ux, uy, uz)^T、 (ν：， ν »，νζ)^Γ禾口 (WX，M WZ) 表示工具坐标系 28的 x，y和 ζ轴的单位向量分别在大地坐标系 0- xyz 20 x, y， z轴上的投影。

一种常用的机器人末端工具速度表示方式是 6维向量：

[ωχ, ωγ^ ωζ₉ vx， vy, νζ]^Γ

其中， [^,^ ζΓ表示末端工具 27相对于工具坐标系 28的伴随坐标系的 χ， y, z 轴的角速度， |νχ,_ν_ν,_νζ;Τ表示末端工具 27相对于工具坐标系 28的伴随坐标系的 X, y, z轴的线速度。

一种常用的机器人末端工具加速度表示方式是 6维向量：

其中， [ ，表示末端工具 27相对于工具坐标系 28的伴随坐标系的 x, y, z 轴的角加速度， [^,^,αζΤ表示末端工具 27相对于工具坐标系 28的伴随坐标系的 X， y， z轴的线加速度。

图 2是机器人轨迹生成系统的结构图。机器人轨迹生成系统由运动命令接口 10、轨迹协调器 11、笛卡尔空间轨迹生成器 12、关节空间轨迹转化器 14、关节空间轨迹插补器 15、运动伺服接口 16和机器人参数数据库 13构成。机器人轨迹生成系统通过轨迹协调器 11与其它 6个部分连接并形成一个整体。

' 轨迹协调器 11调用运动命令接口 10接受来自用户的运动命令。最常见的用户命令有机器人末端工具保持一定位姿和速度下，沿直线或者圆弧运动，同时在运动的起始阶段和结束阶段还需要有加速和减速要求。轨迹协调器 11将返回的用户运动命令发送到笛卡尔空间轨迹生成器 12，生成笛卡尔空间下轨迹节点数据：

位姿

ux vx wx lx

uy vy wy ly

uz vz wz Iz

0 0 0 1」,

速度

和加速度

[ Λ:， ξγ₉ ξζ, αχ₉ ay₉ αζ]^Τ . 式中， i=l，2，3,…… ,η.表示轨迹节点索引； η是轨迹节点的数目。

轨迹协调器 11将返回的笛卡尔空间下轨迹节点数据发送到关节空间轨迹转化器 14，生成机器人各关节空间下轨迹节点数据

位置

[q_],q₂,...q_J...,q_N'\_i，

速度加速度

式中，、和#，表示机器人第 j个关节的位置、速度和加速度； j=l,2,3,-,N, 为关节编号；机器人基座处关节编号为 1，顺序递增。 N为机器人关节数目。 i的定义同上。

机器人关节的位置可以用其逆运动学来计算。在机器人奇异位姿处，机器人关节的速度和加速度的值都设置为

[INVALID VALUE,..., INVALIDVALUE]. 式中， INVALIDVALUE表示无效值，可以用一个超出关节有效运动范围的数值来表示，如 100000000或者 -100000000。

在机器人非奇异位姿处，机器人关节的速度可以用下式计算

式中， J,-'表示机器人轨迹上第 i个节点对应的逆雅可比矩阵，其它符号的定义同前。在机器人非奇异位姿处，机器人的关节加速度 [ 可以用以下加速度递推公式求出

丄

式中，和分别表示机器人第 j个关节轴线向量及其导数，其它符号定义同前。

为了下一步在机器人关节空间的轨迹插补运算，机器人关节轨迹插补器 15需要计算节点间的时间间隔 7：，表示从第 i个节点运动到下一个节点所需要的时间。当 i=n，即 i是轨迹上最后一个节点时，可设置 7；值为零；否则可以用下式计算 7；，

' (^vx, + ^VXM )² + (vy, + vy_M f + (νζ,. + νζ_Μ )²

最后生成的机器人所有关节轨迹单节点数据形如

注意到同一各笛卡尔空间轨迹节点所对应的 N个关节轨迹节点的时间间隔值是相等的，都是 7：。

轨迹协调器 11发送机器人关节空间下轨迹节点数据到关节空间轨迹插补器 15，返回每个伺服周期的关节运动命令（位置、速度、加速度）和关节运动轨迹的最大速度及最大加速度；轨迹协调器 11调用机器人参数数据库 13中存储的各机器人关节的速度、加速度极限值，将其与关节空间轨迹插补器 15产生的最大速度、最大加速度比较，如果没有超限，则将关节空间轨迹插补器 15产生的每个伺服周期的关节运动命令发送到伺服接口 16，否则将超限错误信息发送到运动命令接口 10。

图 3 是机器人单关节运动轨迹节点示意图。图 3中关节轨迹节点有四个：节点 31、节点 32、节点 33和节点 34。

节点 31为首节点，其位置^ ,为15，单位可以根据实际需要确定，如码盘计数值 counts ; 速度为 0,单位可以根据实际需要确定，如 counts/s _; 加速度为

0，单位可以根据实际需要确定，如 coimts/s² ；节点 31到下一节点 32的时间间隔 1为5，单位可以根据实际需要确定，如3。节点 32为中间节点，其位置^.₂为15，单位可以为码盘计数值 counts ;速度 ₂ 为 0,单位可以根据实际需要确定，如 counts/s ; 加速度 ₂为0，单位可以根据实际需要确定，如 counts/s² ；节点 32到下一节点 33的时间间隔 T₂为 9，单位可以根据实际需要确定，如5。

节点 33为中间节点，其位置^ ₂为8，单位可以为码盘计数值 counts ; 速度 ₃ 为无效值 INVALIDVALUE, 可以用一个超出关节有效运动范围的数值来表示，如 100000000或者- 100000000; 加速度无效值 INVALIDVALUE , 可以用一个超出关节有效运动范围的数值来表示，如 100000000或者- 100000000; 节点 33到下一节点 34的时间间隔 T₃为 7，单位可以根据实际需要确定，如5。

节点 34为末节点，其位置^ ₄为12，单位可以为码盘计数值 counts ;速度 ₄为

0，单位可以根据实际需要确定，如 counts/s; 加速度为 0，单位可以根据实际需要确定，如 counts/s² ；节点 34到下一节点 32的时间间隔 L为 0，表示当前节点为末节点。

图 4是关节轨迹插补函数选择流程图和图 5是关节轨迹伺服周期插补流程图，共同描述了图 2中关节轨迹插补器 15功能的一种实施方案。前者为后者提供每个轨迹节点区间的插补函数的参数。后者者每隔 1个伺服周期 CT，计算 1次插补函数位置、速度和加速度数值，为运动伺服控制器提供命令。

在图 4中，关节轨迹插补器 15首先调用模块 41初始化数据公共区 A 和 B区。数据公共区 A区用于存储关节轨迹节点数据，数据公共区 B区用于存储分段插值函数的参数。此外，还要设置系统速度和加速度无效值。可以用一个超出关节有效运动范围的数值来表示，如 100000000或者- 100000000。接着调用模块 42从公共数据区读入2个节点构成数据组[ 1^1，&1，11) , (_P2, v2, a2， T2) ]。 (pi,vi,ai,Ti)和（ , ， 7；), 表示的数据一致。

根据第 2个节点（p2， v2, a2, T2)特征，可以分成 4种情况处理：

情况 1: 如果第 2个节点的加速度 a2有效（模块 81)，则调用模块 43，选择一个 5次多项式函数来表示位置，如 eq.1; 选择一个 4次多项式函数来表示速度，如 eq.2_; 选择一个 3次多项式来表示加速度，如 eq.3。各式中的 u为时间 t的归一化变量，其定义见 eq. 10。 eq.2是 eq.1关于 u的导数，而 eq.3又是 eq. 2关于 u 的导数。 eq.1-3各式的系数（(：5 4，(3,(2，(1,(0)，分别依 .4-9计算。调用速度、加速度校验模块 85，从 eq.2和 eq.3解出在区间 [0, 1]内的速度最大值 _Vmai和加速度最大值 a ，同机器人参数数据库 13中的关节的速度极限值及加速度极限值比较，如果 _V»_M超出速度极限值或者 a_nM超出加速度极限值，则插补结束，并将错误信息返回到用户命令接口 10。如果无错误发生，则调用模块 90把计算结果 (c5, c4, c3, c2, cl, cO)和 Tl作为一个单元写入公共数据 B区队列。接着调用模块 94重建数据组 [(pi, vl，al，Tl)]，让（pl， vl,al,Tl)= (p2, v2, a2, T2)„ 如果数据组第 2个节点的时间 T2等于 0，那么选择分段插值函数的流程结束。否则，调用模块 42从公共数据 Α区读入下一个节点，然后进行新一轮的运算。

p(u) = c5-u⁵ + c4-u⁴ +c3-u³ +c2-u² +c\-u + c0 ； (eq.1) v(u) = -(cl + 2-c2-u + 3-c3-u²+4-a4-u³+5-c5-u⁴)/Tl (eq.2) a{u) = (2 · c2 + 6 · c3 · w + 12 · c4 · w2 + 20 · c5 · «3) / Π² (eq.3) ί5 = (12· 1-12·ρ2 + 6·Π·ν1 + 6·Π·ν2 + Π²·α1-Π²·α2)/2 (eq.4) ί4 = (-30· 1 + 30· 2-14·Π·ν1-16·Γ1·ν2-2·Π²·α1 + 3·Π²·α2)/2 (eq.5) c3 = (20 /7l-20 /72 + 8-n-vl + 12-n-v2 + n².al-3-n²-a2)/2 (eq.6) c2 = (T\² -a2)/2 (eq.7) (eq.8)

(eq.9) (eq.10) 情况 2: 如果第 2个节点的加速度 a2无效但是速度 v2有效（模块 82)，则调用模块 44，选择一个 4次多项式函数来表示位置，如 eq. 11；选择一个 3次多项式函数来表示速度，如 eq. 12；选择一个 2次多项式来表示加速度，如 eq.13。各式中的 u为时间 t的归一化变量，其定义见 eq.10。 eq.12是 eq.11关于 u的导数，而 eq.13又是 eq.12关于 u的导数。 eq. U-13各式的系数（c4， c3, c2， cl， cO)，分别依 eq.14-18计算。按 eq. 19计算 a2并替换 (p2, v2， a2， T2)中的无效值。调用速度、加速度校验模块 86，从 eq.12和 eq.13解出在区间 [0，1]内的速度最大值 _V„_ax 和加速度最大值 ₈,，同机器人参数数据库 13中的关节的速度极限值及加速度极限值比较，如果超出速度极限值或者 _Μ超出加速度极限值，则插补结束，并将错误信息返回到用户命令接口 10。如果无错误发生，接着调用模块 91把计算结果（c5=INVALIDVALUE， c4, c3, c2, cl, cO)和 Tl作为一个单元写入公共数据 B区队列。接着调用模块 95重建数据组 [ (pi, vl, al, Tl) ]，让 (pi, vl, al, Tl)： (p2, v2, a2, T2) ₀ 调用模块 42从公共数据 A区读入下一个节点，然后进行新一轮的运算。

p(u) = c4 + c3 · M³ +c2 -u² +c\ u + c0 (eq.11) _v(u) = -(cl + 2-c2-u + 3-c3-u²+4-a4-u³)/Tl (eq.12) a( ) = (2- c2 + 6-c3-u + 12- c4-u2)/Tl² (eq.13) c4 = (6- l-6- 2 + 4-n-vl + 2-n-v2 + n²-al)/2 (eq.14)

3 = -8· 1 + 8· 2-5·Π·ν1-3·Π·ν2-Π²·ο1 (eq.15) c2 = (\2-pl-l2-p2 + 6-T\-v\ + 6-T\-v2 + T\² -a\)/2 (eq.16) d = -(H v2) (eq.17) cO = p2 (eq.18) ο2 = -(-12·/?1 + 12·/72-6·Π·ν1-6·Π·ν2-Π²·α1)/Π² (eq.19)

情况 3: 第 2个节点的加速度速度 v2无效（模块 83)，则读入第 3个节点的数据，构成数据组[( 1^1，&1，1 )， 2 2，_&2，下2)， (p3, v3, a3,T3)], v3有效。调用模块 45，选择一个 4次多项式函数，如 eq. 11, 来表示连接第 1节点和第 2节点轨迹曲线的位置；选择一个 3次多项式函数，如 eq. 12，来表示连接第 1节点和第 2 节点轨迹曲线的速度；选择一个 2次多项式函数，如 eq. 13, 来表示连接第 1节点和第 2节点轨迹曲线的加速度。各式中的 u为时间 t的归一化变量，其定义见 eq. 10。 eq.12是 eq.11关于 u的导数，而 eq.13又是 eq.12关于 u的导数。 eq. 11-13 各式的系数 ^4,03 2,01，（：0),分别依69. 20-24计算。按 eq. 25计算 v2按 eq.26 计算 a2并替换（p2，v2，a2，T2)中的无效值。调用速度、加速度校验模块 87，从 eq.12和 eq.13解出在区间 [0， 1]内的速度最大值 v ^和加速度最大值 _ax，同机器人参数数据库 13中的关节的速度极限值及加速度极限值比较，如果 _Vmax超出速度极限值或者超出加速度极限值，则插补结束，并将错误信息返回到用户命令接口 10 。如果无错误发生，则调用模块 92 把计算结果 (c5=INVALIDVALUE, c4， c3， c2， cl, cO)和 Tl作为一个单元写入公共数据 B区队列。调用模块 96重建数据组， [(pl，vl，al，Tl), (p2, v2, a2, T2) ] ，并使 (pi, vl,al,Tl) = (p2, v2, a2, T2) , (ρ2, ν2, a2, Τ2) = (ρ3, ν3, a3, T2) ₀ 这样，原来数据组的第 2点，就变成了第 1点；第 3点，就变成了第 2点。调用模块 81进行新一轮的运算。 4 = -(-6·Π·Γ2·/7ΐ-3·Γ2²· 1 + 3·Π²·/72 + 6·Π·Γ2· 2 + 3·Γ2²·^2-

3 · Π² · ;?3— 4 · Π² · Γ2 · vl— 3 · Π · Γ2² · vl + ΤΫ ·Γ2·ν3- Π³ · Τ2-α\- (eq.20)

Τ - Τ2² - ol) / (Γ2■ (2 · Π + 3 · Τ2))

c3 = -(32-T\ T2-pl + l2-T2² -pl-32-T\-T2-p2-\2-T2² -p2 + \S-T\² ·ρ3 +

20·Π²·7^,2·ν1 + 12·Γ1·Γ2²·ν1-6·Π²·Γ2·ν3 + 4·Π³·Γ2·α1 + 3·Π²·Γ2²·α1) (eq.21) /(2·Γ2·(2·Π + 3·Γ2)) c2 = -(-\2 T2²-p\-6-TV-p2 + 6-TV-p3-6-T\-T2 -v\-2-TV-T2-v3-

(eq.22) Π². Τ2². α\) /(2·Γ2·(2·Π + 3· Γ2)) ' 1 = 2·(12·Γ2· 1-9·Π· 2-12·Γ2·/72 + 9·Π·/?3 + 6·Π·Γ2·ν1-

(eq.23) 3·Π Γ2·ν3 + Π²·Γ2·ο1)/(Π·Γ2·(2·Π + 3·Γ2)) cO = ρ2 (eq.24) ν2 = -(12-Γ2²■ pl + 6-Tl² ·ρ2-6-Τ1² ·ρ3 + 6-Τ1-Τ2² -vl+2-Tl² ·Τ2·ν3 +

(eq.25), Π² ·Γ2² Α1)/(2.·Π·Γ2·(2·Π + 3·Γ2)) 2 = -(-12 n r2- 7l + 9 n²-p2 + 9-n²-/?3-6-n²- 2-vl + 3-n² r2-v3-

(eq.26) Π³. Γ2 · αΐ) /(Γ2-(2·Π + 3· Γ2)) 情况 4: 第 2个节点速度 v2无效，读入第 3个节点的数据后构成数据组 [(pi, vl,al,Tl), (p2, v2, a2, T2) , (p3， v3， a3， T3) ]，且 v3无效。调用模块 46, 选择一个 4次多项式函数，如 eq. 11，来表示连接第 1节点和第 2节点轨迹曲线的位置；选择一个 3次多项式函数，如 eq. 12，来表示连接第 1节点和第 2节点轨迹曲线的速度；选择一个 2次多项式函数，如 eq. 13，来表示连接第 1节点和第 2节点轨迹曲线的加速度。各式中的 u为时间 t的归一化变量，其定义见 eq. 10。 eq.12 是 eq.11关于 u的导数，而 eq.13又是 eq.12关于 u的导数。 eq. 11-13各式的系数 (c4, c3, c2, cl, cO)，分别依 eq.27- 31计算。按 eq.32计算 v2按 eq.33计算 a2并替换 (p2， v2，a2,T2)中的无效值。调用速度、加速度校验模块 88，从 eq.12和 eq.13解出在区间 [0, 1]内的速度最大值 V ^和加速度最大值 „，同机器人参数数据库 13 中的关节的速度极限值及加速度极限值比较，如果超出速度极限值或者 a ^超出加速度极限值，则插补结束，并将错误信息返回到用户命令接口 10。如果无错误发生，则调用模块 93把计算结果（c5=INVALIDVALUE， c4, c3, c2, cl， cO)和 Tl 作为一个单元写入公共数据 B区队列。调用模块 97重建数据组， [(pi, vl,al,Tl), (p2, v2, a2,T2)] ，让 (pi, vl, al, Tl) = (p2, v2, a2, T2) ， (p2, v2, a2, T2) = (p3, v3, a3, T2)。调用模块 81进行新一轮的运算。

c4 = -(~6 Τ\-Τ2· ρ\-6·Τ2 - ρ\ + 6·Τ\·Τ2- ρ2 + 6·Τ2²■ p2-2-TV■ _P3- 4·Π²·Γ2·ν1-6·Π·Γ2²·ν1-Π³·Γ2·α1-2·Π²·Γ2²·α1) (eq.27) /(2.·Γ2·(Π + 3·Γ2))

3 = -(16·Π·Γ2·/7ΐ + 12·Γ2² ·/7ΐ-6·Π²· 2-16·Π·Γ2· 2-12·Γ2²· 2 +

6·Π²· 3 + 10·Π²·Γ2·ν1 + 12·Π·7^,2²·ν1 + 2·Π³·Γ2·α1 + 3·Π²·Γ2²·α1) (eq.28) /(2.·Γ2·(Π + 3·Γ2)) ί2 = (Π·(12·Γ2· 1-6·Π·/72 + 6·Π·Γ2·ν1 + Π²·Γ2·α1))/(2.·Γ2·(Π + 3·Γ2)) (eq.29) d = (一 12 · Τ2² ·ρ\-2- Tl² -_P2 + +2-Tl²-p3-6-T\- Τ2² · vl - Π² · Γ2². al)

(eq.30) /(2.·Γ2·(Π + 3·Γ2)) c0 = p2 (eq.31) v2 = -(\2-T2² -p\ + 2-T\² -p2-2-T\² -p3 + 6-T\-T2² -v\ + T\² -T2² -a\)

(eq.32) /(2.·Π·Γ2·(Π + 3·Γ2)) α2 = -(-12·Γ2· 1 + 6·Π·/?2-6·Π· 3-6·Π·7^,2·ν1-Π ·Γ2·α1)

(eq.33) /(Π·Γ2·(Π + 3·Γ2)) 在图 5中，关节轨迹插补器 15首先调用模块 51初始化设置伺服周期 CT值。接着调用模块 52从 B区队列读取一组 (C5，C4，C3，C2，C1,C0,T1)数据。调用模块 53，根据 Π的值是否为 0，来判断当前数据是否是轨迹插补的最后一组数据，若是则退出。若 Π不等于 0，则设置插值的初始化步长 step和 u值。调用模块 55判断 C5 是否是无效值；若是调用模块 56b,依据 eq.ll- 13计算 [p(_U),_V(_U),_a(u)];若不是，则调用模块 56a依据 eq.1-3计算 [p(_U)，_V(ii),a(_U)]。调用模块 57将计算结果输出到机器人伺服控制器。接着调用模块 58计算下一周期的 u值。调用模块 59判断 u值是否小于 0; 如果小于 0，则调用模块 52读取新的插值函数参数；否则调用模块 55继续用当前插值函数计算插值。如果将提供给数控系统的运动伺服程序用。在数控系统伺服周期到来时，运动伺服程序读入当前时刻对应的轨迹多项式曲线的系数，计算出当前伺服周期运动轨迹上的位置、速度和加速度，提供给机器人伺服控制器作为命令输入。

Claims

1. 一种加速度连续的机器人轨迹生成方法，包括步骤：

从运动命令接口接收来自用户的运动命令，并将用户的运动命令送到笛卡尔空间轨迹生成器；

将用户命令转化成笛卡尔空间下机器人末端工具的轨迹节点数据；把笛卡尔空间下机器人末端工具的轨迹节点数据转化成机器人关节空间下轨迹节点数据；

计算每个运动伺服周期的关节位置、速度和加速度；

2. 根据权利要求 1所述的方法，其特征在于还包括步骤：

如果比较结果没有超限，则将产生的每个伺服周期的关节运动命令发送到伺服接口。

3. 根据权利要求 1所述的方法，其特征在于所述关节空间下的轨迹节点位置包括奇异位姿处和非奇异位姿处。

4. 根据权利要求 3所述的方法，其特征在于，在奇异位姿处，设置机器人关节速度为无效值，则在非奇异位姿处，按下式计算机器人关节的速度-

式中， J,-'表示机器人轨迹上第 i个节点对应的逆雅可比矩阵， [^^,.. ···^ 表示机器人轨迹上第 i个节点对应的关节空间速度向量-，

ί.表示机器人轨迹上第 i个节点对应的笛卡尔空间速度向量。

5. 根据权利要求 3所述的方法，其特征在于，在奇异位姿处，设置机器人关节加速度为无效值，则在非奇异位姿处，按下式计算机器人关节的加速度：

式中，下标 i表示机器人轨迹上第 i个节点；和分别表示机器人第 j个关节轴线向量及其导数；和#分别表示机器人第 j个关节的关节空间速度和加速度； [ ， ,^^，《_z;r 表示第 j个关节的笛卡尔空间加速度向量；

6. 根据权利要求 1所述的方法，其特征在于按下式计算节点之间的时间间隔：

(/χ,·₊₁― Ix + (ly_M - ly + (lz_i+、 - Iz

(vx, + vx_i+. )² + (V + vy,.₊₁ )² + (νζ,. + νζ,_+ι ) 式中， 7表示机器人轨迹上第 i个节点到第 i+1个节点的时间间隔；（/x,.,/_y,.，fe,)和 (/^ y,₊ fe,₊₁)分别表示机器人轨迹上第 i和第 i+1个节点对应的机器人 TCP的位置； ,,_νλ，_νζ,)和 (_νχ,._+ι，_νΛ+1 , _ν )分别表示机器人轨迹上第 i和第 i+1个节点对应的机器人 TCP的速度。

7. 根据权利要求 1所述的方法，其特征在于所述计算关节的位置、速度和加速度包括：如果第二个节点的加速度有效，则选择一个 5次多项式函数来表示位置；

选择一个 4次多项式函数来表示速度；

选择一个 3次多项式来表示加速度。

8. 根据权利要求 7所述的方法，其特征在于所述 5次多项式函数为：

其中， u为时间 t的归一化变量。

9. 根据权利要求 7所述的方法，其特征在于所述 4次多项式函数为：

v(u) = -(c\ + 2-c2-u + 3-c3-u² +4-a4-u³+5-c5-u⁴)/Tl，其中， u为时间 t的归一化

10. 根据权利要求 7所述的方法，其特征在于所述 3次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2 + 20-c5-u3)/T\² ,其中， U为时间 t的归一化变量。

11. 根据权利要求 1所述的方法，其特征在于所述计算关节的位置、速度和加速度包括：如果第二个节点的加速度无效但速度有效，则

选择一个 4次多项式函数来表示位置；

选择一个 3次多项式函数来表示速度；

选择一个 2次多项式来表示加速度，并以此式计算第二节点的加速度以替换原来的无效值。

12. 根据权利要求 11所述的方法，其特征在于所述 4次多项式函数为： p(u) = c4-u⁴ +C3-U³ +c2-u² +c\-u + c0 , 其中， U为时间 t的归一化变量。

13. 根据权利要求 11所述的方法，其特征在于所述 3次多项式函数为： v(u) = -(cl+2-c2-u + 3-c3-u²+4-a4-u^s)/Tl , 其中， U为时间 t的归一化变量。

14. 根据权利要求 11所述的方法，其特征在于所述 2次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2)/T\² , 其中， U为时间 t的归一化变量。

15. 根据权利要求 1所述的方法，其特征在于所述计算关节的位置、速度和加速度包括：如果第二个节点的速度无效，读入第 3个节点的数据，构成数据组 [(pl,vl,al,Tl), (p2, v2, a2, T2) , (p3, v3, a3, T3) ] , 第 3个节点的速度有效，则选择一个 4次多项式函数来表示连接第 1节点和第 2节点轨迹曲线的位置；选择一个 3次多项式函数来表示连接第 1节点和第 2节点轨迹曲线的速度；选择一个 2次多项式来表示连接第 1节点和第 2节点轨迹曲线的加速度。

16. 根据权利要求 15所述的方法，其特征在于所述 4次多项式函数为： p(u) = c4-u⁴ +c3-u³ +c2-u² +c\-u + c , 其中， U为时间 t的归一化变量。

17. 根据权利要求 15所述的方法，其特征在于所述 3次多项式函数为- v(u) = -(c\ + 2-c2-u + 3-c3-u² +4-a4-u³)/T\ , 其中， U为时间 t的归一化变量。

18. 根据权利要求 15所述的方法，其特征在于所述 2次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2)/T\² , 其中， U为时间 t的归一化变量。

19. 根据权利要求 1所述的方法，其特征在于所述计算关节的位置、速度和加速度包括：如果第二个节点的速度无效，读入第 3个节点的数据，构成数据组 [(pl,vl,al,Tl), (p2, v2, a2, T2) , (p3， v3， a3, T3) ]，第 3个节点的速度有效，则选择一个 4次多项式函数来表示连接第 1节点和第 2节点轨迹曲线的位置；选择一个 3次多项式函数来表示连接第 1节点和第 2节点轨迹曲线的速度，并以此式计算第 2节点的速度以替换原来的无效值；

选择一个 2次多项式来表示连接第 1节点和第 2节点轨迹曲线的加速度，并以此式计算第 2节点的加速度以替换原来的无效值。

20. 根据权利要求 19所述的方法，其特征在于所述 4次多项式函数为： (_W) = C4-_W ⁴+C3-M³ +C2-_M ²+C1-M + C0, 其中， u为时间 t的归一化变量。

21. 根据权利要求 19所述的方法，其特征在于所述 3次多项式函数为： v(u) = -(cl + 2-c2-u + 3-c3-u² + 4-a4-u³)/T\，其中， U为时间 t的归一化变量。

22. 根据权利要求 19所述的方法，其特征在于所述 2次多项式函数为： a(u)^(2-c2 + 6-c3-u + \2-c4-u2)/T\² , 其中， U为时间 t的归一化变量。

23. 一种加速度连续的机器人轨迹生成系统，包括：

轨迹协调器，从运动命令接口接收来自用户的运动命令，并将用户的运动命令送到笛卡尔空间轨迹生成器；

关节空间轨迹转化器，把笛卡尔空间下机器人末端工具的轨迹节点数据转化成机器人 ¾节空间下轨迹节点数据；

24. 根据权利要求 23所述的系统，其特征在于还包括：

如果轨迹协调器的比较结果没有超限，则将关节空间轨迹插补器产生的每个伺服周期的关节运动命令发送到伺服接口。

20

更正页（细则第 91条)