景奉水 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
TAN, Min (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
谭民 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
LI, En (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
李恩 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
LIANG, Zize (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
梁自泽 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
中国科学院自动化研究所 (中国北京市海淀区中关村东路95号, Beijing 0, 100190, CN)
JING, Fengshui (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
景奉水 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
TAN, Min (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
谭民 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
LI, En (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
李恩 (中国北京市海淀区中关村东路95号自动化大厦906房间, Beijing 0, 100190, CN)
LIANG, Zize (Room 906, Building of AutomationNo.95 Zhongguancun East Road,Haidian District, Beijing 0, 100190, CN)
| 1. 一种加速度连续的机器人轨迹生成方法， 包括步骤： 从运动命令接口接收来自用户的运动命令， 并将用户的运动命令送到笛卡 尔空间轨迹生成器； 将用户命令转化成笛卡尔空间下机器人末端工具的轨迹节点数据； 把笛卡尔空间下机器人末端工具的轨迹节点数据转化成机器人关节空间下 轨迹节点数据； 计算每个运动伺服周期的关节位置、 速度和加速度； 将关节空间轨迹插补器产生的关节的位置、 速度和加速度与机器人参数数 据库中存储的各机器人关节的速度、 加速度极限值进行比较。 2. 根据权利要求 1所述的方法， 其特征在于还包括步骤： 如果比较结果没有超限， 则将产生的每个伺服周期的关节运动命令发送到 伺服接口。 3. 根据权利要求 1所述的方法， 其特征在于所述关节空间下的轨迹节点位 置包括奇异位姿处和非奇异位姿处。 4. 根据权利要求 3所述的方法， 其特征在于， 在奇异位姿处， 设置机器人 关节速度为无效值， 则在非奇异位姿处， 按下式计算机器人关节的速度- 式中， J,-'表示机器人轨迹上第 i个节点对应的逆雅可比矩阵， [^^,.. ···^ 表示机器人轨迹上第 i个节点对应的关节空间速度向量-， ί.表示机器人轨迹上第 i个节点对应的笛卡尔空间速度向量。 5. 根据权利要求 3所述的方法， 其特征在于， 在奇异位姿处， 设置机器人 关节加速度为无效值， 则在非奇异位姿处， 按下式计算机器人关节的加速度： 式中， 下标 i表示机器人轨迹上第 i个节点； 和 分别表示机器人第 j个关 节轴线向量及其导数； 和#分别表示机器人第 j个关节的关节空间速度和加速 度； [ ， ,^^，《z;r 表示第 j个关节的笛卡尔空间加速度向量； 6. 根据权利要求 1所述的方法， 其特征在于按下式计算节点之间的时间间 隔： (/χ,·+1― Ix + (lyM - ly + (lzi+、 - Iz (vx, + vxi+. )2 + (V + vy,.+1 )2 + (νζ,. + νζ,+ι ) 式中， 7表示机器人轨迹上第 i个节点到第 i+1个节点的时间间隔；（/x,.,/_y,.，fe,)和 (/^ y,+ fe,+1)分别表示机器人轨迹上第 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-u2 +4-a4-u3+5-c5-u4)/Tl， 其中， u为时间 t的归一化 10. 根据权利要求 7所述的方法， 其特征在于所述 3次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2 + 20-c5-u3)/T\2 ,其中， U为时间 t的归一化变量。 11. 根据权利要求 1所述的方法， 其特征在于所述计算关节的位置、 速度和 加速度包括： 如果第二个节点的加速度无效但速度有效， 则 选择一个 4次多项式函数来表示位置； 选择一个 3次多项式函数来表示速度； 选择一个 2次多项式来表示加速度， 并以此式计算第二节点的加速度以替换 原来的无效值。 12. 根据权利要求 11所述的方法， 其特征在于所述 4次多项式函数为： p(u) = c4-u4 +C3-U3 +c2-u2 +c\-u + c0 , 其中， U为时间 t的归一化变量。 13. 根据权利要求 11所述的方法， 其特征在于所述 3次多项式函数为： v(u) = -(cl+2-c2-u + 3-c3-u2+4-a4-us)/Tl , 其中， U为时间 t的归一化变量。 14. 根据权利要求 11所述的方法， 其特征在于所述 2次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2)/T\2 , 其中， 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-u4 +c3-u3 +c2-u2 +c\-u + c , 其中， U为时间 t的归一化变量。 17. 根据权利要求 15所述的方法， 其特征在于所述 3次多项式函数为- v(u) = -(c\ + 2-c2-u + 3-c3-u2 +4-a4-u3)/T\ , 其中， U为时间 t的归一化变量。 18. 根据权利要求 15所述的方法， 其特征在于所述 2次多项式函数为： a(u) = (2-c2 + 6-c3-u + \2-c4-u2)/T\2 , 其中， 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-W4+C3-M3 +C2-M2+C1-M + C0, 其中， u为时间 t的归一化变量。 21. 根据权利要求 19所述的方法， 其特征在于所述 3次多项式函数为： v(u) = -(cl + 2-c2-u + 3-c3-u2 + 4-a4-u3)/T\， 其中， U为时间 t的归一化变量。 22. 根据权利要求 19所述的方法， 其特征在于所述 2次多项式函数为： a(u)^(2-c2 + 6-c3-u + \2-c4-u2)/T\2 , 其中， U为时间 t的归一化变量。 23. 一种加速度连续的机器人轨迹生成系统， 包括： 轨迹协调器， 从运动命令接口接收来自用户的运动命令， 并将用户的运动 命令送到笛卡尔空间轨迹生成器； 笛卡尔空间轨迹生成器， 将用户命令转化成笛卡尔空间下机器人末端工具 的轨迹节点数据； 关节空间轨迹转化器， 把笛卡尔空间下机器人末端工具的轨迹节点数据转 化成机器人 ¾节空间下轨迹节点数据； 关节空间轨迹插补器， 计算每个运动伺服周期的关节的位置、 速度和加速 度； 轨迹协调器最后将关节空间轨迹插补器产生的关节的位置、 速度和加速度 与机器人参数数据库中存储的各机器人关节的速度、 加速度极限值进行比较。 24. 根据权利要求 23所述的系统， 其特征在于还包括： 如果轨迹协调器的比较结果没有超限， 则将关节空间轨迹插补器产生的每 个伺服周期的关节运动命令发送到伺服接口。 20 更正页 （细则第 91条) |
本发明涉及机器人， 尤其是机器人轨迹生成系统和方法。 背景技术
工业机器人依靠末端工具在笛卡尔坐标系空间 的运动来完成作业任务， 但机 器人末端工具的运动不能被直接控制， 而是通过控制机器人的关节运动来实现。 在任一时间， 机器人各个关节的位置、 速度和加速度都决定了工具的位姿、 速 度和加速度， 也就是决定了工具的运动轨迹。 因此， 为了控制机器人末端工具 在每一时刻的位姿、 速度和加速度， 就必须转化成控制机器人关节在每一伺服 周期的位置、 速度和加速度， 这就是机器人轨迹生成的目的。
机器人末端工具的在笛卡尔空间运动轨迹是可 用一组彼此间隔一定时间的 离散点 （通常包括位姿、 速度和加速度） 来表示。 如上所述， 要控制机器人末 端工具的运动， 就要将末端工具的在笛卡尔空间运动轨迹上的 离散时间点的位 姿、 速度和加速度转化成对应的关节空间运动轨迹 上位置、 速度和加速度。 位 姿的转换通过机器人运动学的逆计算实现。 速度的转化通过机器人雅克比矩阵 的逆计算实现。 但是雅克比矩阵的逆计算有时会失败： 当一个 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维向量：
[ωχ, ωγ^ ωζ 9 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」,
速度
和加速度
[ Λ:， ξγ 9 ξζ, αχ 9 ay 9 αζ] Τ . 式中， i=l，2，3,…… ,η.表示轨迹节点索引； η是轨迹节点的数目。
轨迹协调器 11将返回的笛卡尔空间下轨迹节点数据发送到 节空间轨迹转 化器 14， 生成机器人各关节空间下轨迹节点数据
位置
[q ] ,q 2 ,...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 , + VX M ) 2 + (vy, + vy M f + (νζ,. + νζ Μ ) 2
最后生成的机器人所有关节轨迹单节点数据形 如
注意到同一各笛卡尔空间轨迹节点所对应的 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 2 ； 节点 31到下一节点 32的时间间 隔 1为5， 单位可以根据实际需要确定， 如3。 节点 32为中间节点，其位置^. 2 为15，单位可以为码盘计数值 counts ;速度 2 为 0,单位可以根据实际需要确定， 如 counts/s ; 加速度 2 为0， 单位可以根据实 际需要确定， 如 counts/s 2 ； 节点 32到下一节点 33的时间间隔 T 2 为 9， 单位可以 根据实际需要确定， 如5。
节点 33为中间节点， 其位置^ 2 为8， 单位可以为码盘计数值 counts ; 速度 3 为无效值 INVALIDVALUE, 可以用一个超出关节有效运动范围的数值来表 示， 如 100000000或者- 100000000; 加速度 无效值 INVALIDVALUE , 可以用一个超出 关节有效运动范围的数值来表示， 如 100000000或者- 100000000; 节点 33到下一 节点 34的时间间隔 T 3 为 7， 单位可以根据实际需要确定， 如5。
节点 34为末节点，其位置^ 4 为12，单位可以为码盘计数值 counts ;速度 4 为
0，单位可以根据实际需要确定， 如 counts/s; 加速度 为 0， 单位可以根据实际 需要确定， 如 counts/s 2 ； 节点 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) , ( P 2, 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 5 + c4-u 4 +c3-u 3 +c2-u 2 +c\-u + c0 ； (eq.1) v(u) = -(cl + 2-c2-u + 3-c3-u 2 +4-a4-u 3 +5-c5-u 4 )/Tl (eq.2) a{u) = (2 · c2 + 6 · c3 · w + 12 · c4 · w2 + 20 · c5 · «3) / Π 2 (eq.3) ί5 = (12· 1-12·ρ2 + 6·Π·ν1 + 6·Π·ν2 + Π 2 ·α1-Π 2 ·α2)/2 (eq.4) ί4 = (-30· 1 + 30· 2-14·Π·ν1-16·Γ1·ν2-2·Π 2 ·α1 + 3·Π 2 ·α2)/2 (eq.5) c3 = (20 /7l-20 /72 + 8-n-vl + 12-n-v2 + n 2 .al-3-n 2 -a2)/2 (eq.6) c2 = (T\ 2 -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 和加速度最大值 8 ,， 同机器人参数数据库 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) 0 调用模块 42从公共数据 A区读入下一个节点， 然后进行新一轮 的运算。
p(u) = c4 + c3 · M 3 +c2 -u 2 +c\ u + c0 (eq.11) v (u) = -(cl + 2-c2-u + 3-c3-u 2 +4-a4-u 3 )/Tl (eq.12) a( ) = (2- c2 + 6-c3-u + 12- c4-u2)/Tl 2 (eq.13) c4 = (6- l-6- 2 + 4-n-vl + 2-n-v2 + n 2 -al)/2 (eq.14)
3 = -8· 1 + 8· 2-5·Π·ν1-3·Π·ν2-Π 2 ·ο1 (eq.15) c2 = (\2-pl-l2-p2 + 6-T\-v\ + 6-T\-v2 + T\ 2 -a\)/2 (eq.16) d = -(H v2) (eq.17) cO = p2 (eq.18) ο2 = -(-12·/?1 + 12·/72-6·Π·ν1-6·Π·ν2-Π 2 ·α1)/Π 2 (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) 0 这样， 原来 数据组的第 2点， 就变成了第 1点； 第 3点， 就变成了第 2点。 调用模块 81进行新 一轮的运算。 4 = -(-6·Π·Γ2·/7ΐ-3·Γ2 2 · 1 + 3·Π 2 ·/72 + 6·Π·Γ2· 2 + 3·Γ2 2 ·^2-
3 · Π 2 · ;?3— 4 · Π 2 · Γ2 · vl— 3 · Π · Γ2 2 · vl + ΤΫ ·Γ2·ν3- Π 3 · Τ2-α\- (eq.20)
Τ - Τ2 2 - ol) / (Γ2■ (2 · Π + 3 · Τ2))
c3 = -(32-T\ T2-pl + l2-T2 2 -pl-32-T\-T2-p2-\2-T2 2 -p2 + \S-T\ 2 ·ρ3 +
20·Π 2 ·7 , 2·ν1 + 12·Γ1·Γ2 2 ·ν1-6·Π 2 ·Γ2·ν3 + 4·Π 3 ·Γ2·α1 + 3·Π 2 ·Γ2 2 ·α1) (eq.21) /(2·Γ2·(2·Π + 3·Γ2)) c2 = -(-\2 T2 2 -p\-6-TV-p2 + 6-TV-p3-6-T\-T2 -v\-2-TV-T2-v3-
(eq.22) Π 2 . Τ2 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 ·Γ2·ο1)/(Π·Γ2·(2·Π + 3·Γ2)) cO = ρ2 (eq.24) ν2 = -(12-Γ2 2 ■ pl + 6-Tl 2 ·ρ2-6-Τ1 2 ·ρ3 + 6-Τ1-Τ2 2 -vl+2-Tl 2 ·Τ2·ν3 +
(eq.25), Π 2 ·Γ2 2 Α1)/(2.·Π·Γ2·(2·Π + 3·Γ2)) 2 = -(-12 n r2- 7l + 9 n 2 -p2 + 9-n 2 -/?3-6-n 2 - 2-vl + 3-n 2 r2-v3-
(eq.26) Π 3 . Γ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 2 ■ p2-2-TV■ P 3- 4·Π 2 ·Γ2·ν1-6·Π·Γ2 2 ·ν1-Π 3 ·Γ2·α1-2·Π 2 ·Γ2 2 ·α1) (eq.27) /(2.·Γ2·(Π + 3·Γ2))
3 = -(16·Π·Γ2·/7ΐ + 12·Γ2 2 ·/7ΐ-6·Π 2 · 2-16·Π·Γ2· 2-12·Γ2 2 · 2 +
6·Π 2 · 3 + 10·Π 2 ·Γ2·ν1 + 12·Π·7 , 2 2 ·ν1 + 2·Π 3 ·Γ2·α1 + 3·Π 2 ·Γ2 2 ·α1) (eq.28) /(2.·Γ2·(Π + 3·Γ2)) ί2 = (Π·(12·Γ2· 1-6·Π·/72 + 6·Π·Γ2·ν1 + Π 2 ·Γ2·α1))/(2.·Γ2·(Π + 3·Γ2)) (eq.29) d = (一 12 · Τ2 2 ·ρ\-2- Tl 2 - P 2 + +2-Tl 2 -p3-6-T\- Τ2 2 · vl - Π 2 · Γ2 2 . al)
(eq.30) /(2.·Γ2·(Π + 3·Γ2)) c0 = p2 (eq.31) v2 = -(\2-T2 2 -p\ + 2-T\ 2 -p2-2-T\ 2 -p3 + 6-T\-T2 2 -v\ + T\ 2 -T2 2 -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继续用当前插值函数计算插值。 如果将提供给数控系统的运动伺服程序 用。 在数控系统伺服周期到来时， 运动伺服程序读入当前时刻对应的轨迹多项 式曲线的系数， 计算出当前伺服周期运动轨迹上的位置、 速度和加速度， 提供 给机器人伺服控制器作为命令输入。
Next Patent: QUICK- CONNECTING TYPE OUTDOOR WATERPROOF ELECTRIC CONNECTOR