CN111796558A - 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质 - Google Patents

多轴联动实时动态前瞻轨迹规划方法、装置及存储介质 Download PDF

Info

Publication number
CN111796558A
CN111796558A CN202010510910.XA CN202010510910A CN111796558A CN 111796558 A CN111796558 A CN 111796558A CN 202010510910 A CN202010510910 A CN 202010510910A CN 111796558 A CN111796558 A CN 111796558A
Authority
CN
China
Prior art keywords
speed
transition
planning
real
look
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010510910.XA
Other languages
English (en)
Other versions
CN111796558B (zh
Inventor
梁艳阳
吴伟
姚超智
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Mianyang Sibo Intelligent Technology Co.,Ltd.
Original Assignee
Wuyi University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuyi University filed Critical Wuyi University
Priority to CN202010510910.XA priority Critical patent/CN111796558B/zh
Publication of CN111796558A publication Critical patent/CN111796558A/zh
Application granted granted Critical
Publication of CN111796558B publication Critical patent/CN111796558B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34169—Coarse interpolator, path calculator delivers position, speed, acceleration blocks

Landscapes

  • Engineering & Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了多轴联动实时动态前瞻轨迹规划方法、装置及存储介质，其中方法包括：接收输入数据；进行前瞻轨迹规划；接收输入指令；根据输入指令执行基于前瞻轨迹规划的实时变速或停止方法进行重新规划；将采样第一段和过渡区的轨迹得到的插补点输入S型数字滤波器；删除第一段和过渡区的数据后构建一个新段，重复上述步骤。解决了多轴联动机器人的运动轨迹不平滑、运动不稳定的问题，使多轴联动机器人运动轨迹更平滑，运动更流畅、稳定。

Description

多轴联动实时动态前瞻轨迹规划方法、装置及存储介质
技术领域
本发明涉及多轴联动机器人控制领域，特别是多轴联动实时动态前瞻轨迹规划方法、装置及存储介质。
背景技术
多轴联动机器人在工业生产中得到广泛的应用。前瞻轨迹规划可对多段路径中相邻过渡段进行位置、速度及加速度规划，使机器人末端轴按照预定轨迹运行。高效的前瞻轨迹规划算法可使多轴联动机器人高速、平稳地完成过渡段的运动，从而提高工作效率，因此前瞻规划算法的优劣也可视为衡量机器人性能的隐性指标。
但多轴联动机器人仍然存在运动轨迹不平滑、运动不稳定等问题。
发明内容
本发明的目的在于至少解决现有技术中存在的技术问题之一，提供多轴联动实时动态前瞻轨迹规划方法、装置及存储介质。
本发明解决其问题所采用的技术方案是：
本发明的第一方面，多轴联动实时动态前瞻轨迹规划方法，包括以下步骤：
步骤S1、接收输入数据，所述输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，其中n个轴是联动的；
步骤S2、根据所述输入数据进行前瞻轨迹规划；
步骤S3、接收输入指令；
步骤S4、若所述输入指令为变速指令，执行基于所述前瞻轨迹规划的实时变速方法进行重新规划；若所述输入指令为停止指令，执行基于所述前瞻轨迹规划的实时停止方法进行重新规划；
步骤S5、对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点，将所述插补点输入用于将梯形速度曲线转化为S型速度曲线的S型数字滤波器；
步骤S6、对第一段的轨迹和过渡区的轨迹采样完成后，删除第一段的数据和过渡区的数据；
步骤S7、获取新的点位信息并根据新的点位信息构建一个新段，重复执行步骤S2至步骤S6；
其中，所述前瞻轨迹规划包括以下步骤：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
根据本发明的第一方面，在所述计算n个轴的所有的过渡区大小的步骤中，所述过渡区的大小为：
Figure BDA0002528210050000021
其中Ls是过渡区的大小，tm为过渡时间，Ap为过渡区的加速度，
Figure BDA0002528210050000031
为余弦比例向量。
根据本发明的第一方面，所述计算n个轴的所有的过渡区的最大允许过渡速度具体为，对于第i段，若所述过渡区的大小为0，则所述最大允许过渡速度为：
Figure BDA0002528210050000032
若所述过渡区的大小不为0，则所述最大允许过渡速度为：
Figure BDA0002528210050000033
若计算得到的所述最大允许过渡速度存在
Figure BDA0002528210050000034
则更新所述最大允许过渡速度为
Figure BDA0002528210050000035
并更新所述过渡区的大小为：
Figure BDA0002528210050000036
其中Vmax,i为第i段的最大速度，
Figure BDA0002528210050000037
为第i段的进给速度，1≤i≤m-1。
根据本发明的第一方面，所述反向速度前瞻规划具体为：对于第i段，若vs≥vt，则保持所述过渡区的大小和所述最大允许过渡速度；若vs<vt，则计算
Figure BDA0002528210050000038
若v't≥vt，则保持所述过渡区的大小和所述最大允许过渡速度，否则更新所述过渡区的大小为Lsn＝(1-a)Ls和更新所述最大允许过渡速度为
Figure BDA0002528210050000039
其中a为所述过渡区的减小比例，
Figure BDA00025282100500000310
Amax是最大加速度，vs是第i段的前一段的vtn，1≤i≤m-1。
根据本发明的第一方面，所述正向速度规划具体为：计算临界速度为
Figure BDA00025282100500000311
若vs≥vt且vc≤VF，则将速度从vs加速到vc，再从vc减速到vt；若vs≥vt且vc>VF，则将速度从vs加速至vF并保持一段时间，再将速度从vF减速至vt；若vs≥vt且vs＝vc，将速度从vc减速至vt；若vs＜vt、
Figure BDA0002528210050000041
且vc≤VF，则速度从vs加速至vt，再从vc减速至vt；若vs<vt、
Figure BDA0002528210050000042
且vc>VF，则将速度从vs提速至vF并保持一段时间，再将速度从vF减速至vt；若vs<vt、
Figure BDA0002528210050000043
且Ls＝0，将速度提升至最大允许过渡速度
Figure BDA0002528210050000044
若vs<vt、
Figure BDA0002528210050000045
且Ls≠0，则减小过渡区后，再将速度提升至最大允许过渡速度
Figure BDA0002528210050000046
根据本发明的第一方面，所述过渡区的轨迹规划具体为：计算所述过渡区的速度为
Figure BDA0002528210050000047
所述过渡区的位置为
Figure BDA0002528210050000048
根据本发明的第一方面，所述基于前瞻轨迹规划的实时变速方法包括实时升速和实时降速；所述实时升速具体为：更新所有段的进给速度，再重新进行所述前瞻轨迹规划；所述实时降速具体为：更新所有段的进给速度，更新首个过渡区的大小为Lsn＝aLc+Ls和首个过渡区的最大允许过渡速度为
Figure BDA0002528210050000049
再重新进行所述前瞻轨迹规划。
根据本发明的第一方面，所述基于前瞻轨迹规划的实时停止方法具体为：对于第i段，第i段的长度为L＝Lc+Ls，计算
Figure BDA00025282100500000410
若L≤L，则将第i段的速度降至0；若L'>L，则调整所述过渡区的大小为
Figure BDA0002528210050000051
其中
Figure BDA0002528210050000052
将速度降速至vtn；若第i段的速度vtn不为0，则调整第(i+1)段的所述过渡区的大小和速度直至该段的速度vtn为0。
本发明的第二方面，多轴联动实时动态前瞻轨迹规划装置，包括：
数据输入模块，用于接收输入数据，所述输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，以及用于接收输入的新的点位信息；
第一规划模块，用于根据所述输入数据进行前瞻轨迹规划；
指令输入模块，用于接收输入指令；
第二规划模块，用于执行以下步骤：若所述输入指令为变速指令，执行基于所述前瞻轨迹规划的实时变速方法进行重新规划；若所述输入指令为停止指令，执行基于所述前瞻轨迹规划的实时停止方法进行重新规划；
采样模块，用于对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点；
S型数字滤波器，用于根据所述插补点将梯形速度曲线转化为S型速度曲线；
数据删除模块，用于删除数据；
其中，所述前瞻轨迹规划包括：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
本发明的第三方面，存储介质，存储有可执行指令，可执行指令能被计算机执行，使所述计算机执行如本发明第一方面所述的多轴联动实时动态前瞻轨迹规划方法。
上述方案至少具有以下的有益效果：根据动态给定的位置序列、前瞻段数、轴的速度和加速度范围，实时进行多轴联动前瞻轨迹规划，并通过S型数字滤波器将规划的梯形速度曲线转化为S型速度曲线；解决了多轴联动机器人的运动轨迹不平滑、运动不稳定的问题，使多轴联动机器人运动轨迹更平滑，运动更流畅、稳定；并能进一步推广应用到多轴联动工业机器人的任务空间位置和姿态同步轨迹前瞻规划上。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
下面结合附图和实例对本发明作进一步说明。
图1是本发明实施例多轴联动实时动态前瞻轨迹规划方法的流程图；
图2是本发明实施例多轴联动实时动态前瞻轨迹规划装置的结构图；
图3是相邻两段的过渡轨迹示意图；
图4是反向速度前瞻规划的轨迹示意图；
图5是正向速度规划的轨迹示意图；
图6是正向速度规划的第一情况的速度曲线图；
图7是正向速度规划的第二情况的速度曲线图；
图8是正向速度规划的第三情况的速度曲线图；
图9是正向速度规划的第四情况的速度曲线图；
图10是正向速度规划的第五情况的速度曲线图；
图11是正向速度规划的第六情况的速度曲线图；
图12是正向速度规划的第七情况的过渡区调整示意图；
图13是过渡区轨迹规划的轨迹示意图；
图14是实时降速的轨迹示意图；
图15是实时降速的过渡区的调整示意图；
图16是实时停止的轨迹示意图；
图17是六自由度的工业机器人的六个轴的原始位置曲线图；
图18是六自由度的工业机器人的六个轴的原始速度曲线图；
图19是六自由度的工业机器人的六个轴的原始加速度曲线图；
图20是六自由度的工业机器人的六个轴的规划后的位置曲线图；
图21是六自由度的工业机器人的六个轴的规划后的速度曲线图；
图22是六自由度的工业机器人的六个轴的规划后的加速度曲线图；
图23是六自由度的工业机器人的六个轴的实时变速后的位置曲线图；
图24是六自由度的工业机器人的六个轴的实时变速后的速度曲线图；
图25是六自由度的工业机器人的六个轴的实时变速后的加速度曲线图。
具体实施方式
本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。
在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
在本发明的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。
本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
参照图1，本发明的某些实施例，提供了多轴联动实时动态前瞻轨迹规划方法，包括以下步骤：
步骤S1、接收输入数据，输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，其中n个轴是联动的；
步骤S2、根据输入数据进行前瞻轨迹规划；
步骤S3、接收输入指令；
步骤S4、若输入指令为变速指令，执行基于前瞻轨迹规划的实时变速方法进行重新规划；若输入指令为停止指令，执行基于前瞻轨迹规划的实时停止方法进行重新规划；需要说明的是，停止状态可以通过设置停止状态标志进行标记；
步骤S5、对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点，将插补点输入用于将梯形速度曲线转化为S型速度曲线的S型数字滤波器；
步骤S6、对第一段的轨迹和过渡区的轨迹采样完成后，删除第一段的数据和过渡区的数据；另外，若处于停止状态且存在尚未完成采样的轨迹，则返回步骤S5继续采样；若处于停止状态且已完成所有轨迹的采样，则终止，不执行步骤S7；
步骤S7、获取新的点位信息并根据新的点位信息构建一个新段，重复执行步骤S2至步骤S6；另外，若不存在新的点位信息且已完成所有轨迹的采样，则终止；
其中，前瞻轨迹规划包括以下步骤：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
在该实施例中，根据动态给定的位置序列、前瞻段数、轴的速度和加速度范围，实时进行多轴联动前瞻轨迹规划，并通过S型数字滤波器将规划的梯形速度曲线转化为S型速度曲线；解决了多轴联动机器人的运动轨迹不平滑、运动不稳定的问题，使多轴联动机器人运动轨迹更平滑，运动更流畅、稳定；并能进一步推广应用到多轴联动工业机器人的任务空间位置和姿态同步轨迹前瞻规划上。
进一步，对于输入数据，m个点位序列表示为
Figure BDA0002528210050000101
点位序列中的每个位置点是一个n为向量，
Figure BDA0002528210050000102
第i段的进给速度为
Figure BDA0002528210050000103
第
Figure BDA0002528210050000104
和
Figure BDA0002528210050000105
构成第i段，1≤i≤m-1。
在计算n个轴的所有的过渡区大小的步骤中，
参照图3，对于由三个位置点相邻的两段，第i段和第i+1段，有第i段的长度为
Figure BDA0002528210050000111
第i+1段的长度为
Figure BDA0002528210050000112
过渡区的大小为
Figure BDA0002528210050000113
则第i段各轴的余弦比例为：
Figure BDA0002528210050000114
第i段的最大速度为：
Figure BDA0002528210050000115
第i段的最大加速度为：
Figure BDA0002528210050000116
进而相邻的两段的余弦比例差值为
Figure BDA0002528210050000117
归一化为
Figure BDA0002528210050000118
过渡区的加速度为
Figure BDA0002528210050000119
过渡区的加速度向量为
Figure BDA00025282100500001110
令过渡时间为tm，则由过渡区首末点处的速度不变，有
Figure BDA00025282100500001111
即
Figure BDA00025282100500001112
从而有
Figure BDA00025282100500001113
过渡曲线的轨迹方程为：
Figure BDA00025282100500001114
进而有
Figure BDA0002528210050000121
可得到
Figure BDA0002528210050000122
其中Ls是过渡区的大小，tm为过渡时间，Ap为过渡区的加速度，
Figure BDA0002528210050000123
为余弦比例向量。
进一步，在计算n个轴的所有的过渡区的最大允许过渡速度的步骤中，
对于第i段，若过渡区的大小为0，即不存在过渡区，则最大允许过渡速度为：
Figure BDA0002528210050000124
若过渡区的大小不为0，给定Ls＝1/2min(Li,Li+1)，有过渡时间为
Figure BDA0002528210050000125
则最大允许过渡速度为：
Figure BDA0002528210050000126
若计算得到的最大允许过渡速度存在
Figure BDA0002528210050000127
则更新最大允许过渡速度为
Figure BDA0002528210050000128
并更新过渡区的大小为：
Figure BDA0002528210050000129
其中Vmax,i为第i段的最大速度，
Figure BDA00025282100500001210
为第i段的进给速度，1≤i≤m-1。
参照图4，进一步，反向速度前瞻规划具体为：对于除首段外的第i段，若vs≥vt，即vt是可达到的，则保持过渡区的大小和最大允许过渡速度。若vs<vt，则需要判断vt是否能达到，则计算
Figure BDA00025282100500001211
若v't≥vt，则vt是可达到的，则保持过渡区的大小和最大允许过渡速度，否则更新过渡区的大小为Lsn＝(1-a)Ls和更新最大允许过渡速度为
Figure BDA0002528210050000131
其中a为过渡区的减小比例，
Figure BDA0002528210050000132
Amax是最大加速度，vs是第i段的前一段的vtn，1≤i≤m-1。
按照上述方法继续进行反向速度前瞻，直到第一段为止。
需要说明的是，根据
Figure BDA0002528210050000133
且
Figure BDA0002528210050000134
进而得到
Figure BDA0002528210050000135
可得到过渡区的大小为Lsn＝(1-a)Ls和最大允许过渡速度为
Figure BDA0002528210050000136
参照图5，进一步，正向速度规划具体为：根据
Figure BDA0002528210050000137
计算临界速度为
Figure BDA0002528210050000138
第一情况：若vs≥vt且vc≤VF，则将速度从vs加速到vc，再从vc减速到vt，参照图6。第二情况：若vs≥vt且vc>VF，则将速度从vs加速至vF并保持一段时间，再将速度从vF减速至vt，参照图7。第三情况：若vs≥vt且vs＝vc，将速度从vc减速至vt，参照图8。第四情况：若vs<vt、
Figure BDA0002528210050000139
且vc≤VF，则速度从vs加速至vt，再从vc减速至vt，参照图9。第五情况：若vs<vt、
Figure BDA00025282100500001310
且vc>VF，则将速度从vs提速至vF并保持一段时间，再将速度从vF减速至vt，参照图10。第六情况：若vs<vt、
Figure BDA00025282100500001311
且Ls＝0，此时无法提速至vt，将速度提升至最大允许过渡速度
Figure BDA00025282100500001312
参照图11。第七情况：若vs<vt、
Figure BDA00025282100500001313
且Ls≠0，则减小过渡区，过渡区的减小比例为a，此时
Figure BDA0002528210050000141
参照图12；将速度提升至最大允许过渡速度
Figure BDA0002528210050000142
且有Lcn＝Lc+aLs，Lsn＝(1-a)Ls。
参照图13，进一步，过渡区如果存在(最后一段没有过渡段，相邻两段如果方向余弦向量完全相等也不存在过渡段)，即Ls≠0，则需要进行过渡区轨迹规划。因为过渡区的速度不变，所以只需要计算出过渡区的速度曲线和位置曲线。
令
Figure BDA0002528210050000143
和
Figure BDA0002528210050000144
分别为起点和终点位置，有速度为vt，过渡区长度为Ls，加速度为Ap，加速度向量为
Figure BDA0002528210050000145
过渡区的运行时间为tm＝2Ls/vt，过渡区的轨迹规划具体为：计算过渡区的速度为
Figure BDA0002528210050000146
进而得到过渡区的位置为
Figure BDA0002528210050000147
进一步，基于前瞻轨迹规划进行实时变速能提升动态运行性能和灵活性，是实现动态跟踪功能(如视觉跟踪、焊缝跟踪或者其他引导功能等)的基础。
基于前瞻轨迹规划的实时变速方法包括实时升速和实时降速；
实时升速具体为：更新所有段的进给速度，再重新进行前瞻轨迹规划。
参照图14，对于实时降速，令当前变速度段的最大加速度绝对值为Am，过渡区的最大加速度绝对值为Ap，当前变速度段的长度为Lc，首速度为vs，允许的过渡速度为vt和过渡长度为Ls，当前变速度段和下段的进给速度分别为
Figure BDA0002528210050000151
和
Figure BDA0002528210050000152
前后两段的余弦比例分别为
Figure BDA0002528210050000153
和
Figure BDA0002528210050000154
更新所有段的进给速度
Figure BDA0002528210050000155
又有
Figure BDA0002528210050000156
进而有
Figure BDA0002528210050000157
以及Lcn＝Lc+Ls-Lsn。
如果vs≤vtm，则保持数据不变，否则计算
Figure BDA0002528210050000158
如果v’tm≤vtm，则只需要更新vtn＝vtm,VF＝vtn。
如果v'tm>vtm，则需要调整Lc和Ls，并计算新的过渡速度。参照图15，由于无法降速到vtm，所以需要减小Lc，令减小比例为a，那么有
Figure BDA0002528210050000159
以及
Figure BDA00025282100500001510
进而有
Figure BDA00025282100500001511
更新首个过渡区的大小为Lsn＝aLc+Ls且有Lcn＝(1-a)Lc和首个过渡区的最大允许过渡速度为
Figure BDA00025282100500001512
且有VF＝vtn，再重新进行前瞻轨迹规划。
参照图16，进一步，基于前瞻轨迹规划的实时停止方法具体为：对于第i段，第i段的长度为L＝Lc+Ls，计算
Figure BDA00025282100500001518
若L’≤L，则将第i段的速度降至0；若L’>L，则调整过渡区的大小为
Figure BDA00025282100500001514
另外此时
Figure BDA00025282100500001515
其中
Figure BDA00025282100500001516
且有
Figure BDA00025282100500001517
将速度降速至vtn；若第i段的速度vtn不为0，则调整下一段的过渡区的大小和速度直至存在一段的速度vtn为0为止。
另外，S型数字滤波器基于数字卷积原理，由两个线性滤波器串联实现。
其中线性滤波器的离散传递函数为
Figure BDA0002528210050000161
则S型数字滤波器的传递函数为
Figure BDA0002528210050000162
令输入信号为Vi(k),k＝1,2,....,Vi(k)|k≤0＝0，则S型数字滤波器的输出为：
Figure BDA0002528210050000163
其中
Figure BDA0002528210050000164
现采用该多轴联动实时动态前瞻轨迹规划对六自由度的工业机器人控制，该工业机器人有六个关节，每个关节对应一个运动轴，即有六个轴，六个轴的速度范围为[-4,4]弧度/秒，加速度范围为[-30,30]弧度/秒2。
首先在不考虑每个轴的速度和加速度限制的情况下，让机器人末端沿着一段圆弧路径以S型速度曲线运动，以2毫秒作为采样周期采集得到位置点，然后通过反解总共可得到202组六个轴的位置点。对这202组位置点进行差分计算得到相应的速度和加速度。从图17、图18和图19可以看出，每个轴的速度和加速度都超过了给定范围。
以这202组位置点作为输入的点位序列，以这202组位置点的差分速度作为输入的进给速度，前瞻段数(m-1)为100，以0.5毫秒作为精插补周期，采用该多轴联动实时动态前瞻轨迹规划方法进行轨迹规划。参照图20、图21和图22,可见，各轴的速度和加速度都在给定范围之内，且加速度曲线非常平滑。
如果在第400个采样点处(第200毫秒)将给定速度降低为原始进给速度的1/10，并在第1500个采样点处(第750毫秒)将速度提升到当前的10倍，则得到相应的精插补的位置、速度和加速度曲线。参照图23、图24和图25，可见在规划中实时动态的调整进给速度，采用该多轴联动实时动态前瞻轨迹规划方法能够及时响应并作出正确的处理。
参照图2，本发明的某些实施例，提供了多轴联动实时动态前瞻轨迹规划装置，装置应用了如方法实施例所述的多轴联动实时动态前瞻轨迹规划方法。
多轴联动实时动态前瞻轨迹规划装置包括：
数据输入模,10，用于接收输入数据，输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，以及用于接收输入的新的点位信息；
第一规划模块20，用于根据输入数据进行前瞻轨迹规划；
指令输入模块30，用于接收输入指令；
第二规划模块40，用于执行以下步骤：若输入指令为变速指令，执行基于前瞻轨迹规划的实时变速方法进行重新规划；若输入指令为停止指令，执行基于前瞻轨迹规划的实时停止方法进行重新规划；
采样模块50，用于对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点；
S型数字滤波器60，用于根据插补点将梯形速度曲线转化为S型速度曲线；
数据删除模块70，用于删除数据；
其中，前瞻轨迹规划包括：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
在该装置实施例中，多轴联动实时动态前瞻轨迹规划装置应用如方法实施例所述的多轴联动实时动态前瞻轨迹规划方法，经各个模块的配合，能执行多轴联动实时动态前瞻轨迹规划方法的各个步骤，具有和多轴联动实时动态前瞻轨迹规划方法相同的技术效果，在此不再详述。
本发明的某些实施例，存储介质，存储有可执行指令，可执行指令能被计算机执行，使计算机执行如方法实施例所述的多轴联动实时动态前瞻轨迹规划方法。
存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，都应属于本发明的保护范围。

Claims (10)

1.多轴联动实时动态前瞻轨迹规划方法，其特征在于，包括以下步骤：
步骤S1、接收输入数据，所述输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，其中n个轴是联动的；
步骤S2、根据所述输入数据进行前瞻轨迹规划；
步骤S3、接收输入指令；
步骤S4、若所述输入指令为变速指令，执行基于所述前瞻轨迹规划的实时变速方法进行重新规划；若所述输入指令为停止指令，执行基于所述前瞻轨迹规划的实时停止方法进行重新规划；
步骤S5、对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点，将所述插补点输入用于将梯形速度曲线转化为S型速度曲线的S型数字滤波器；
步骤S6、对第一段的轨迹和过渡区的轨迹采样完成后，删除第一段的数据和过渡区的数据；
步骤S7、获取新的点位信息并根据新的点位信息构建一个新段，重复执行步骤S2至步骤S6；
其中，所述前瞻轨迹规划包括以下步骤：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
2.根据权利要求1所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，在所述计算n个轴的所有的过渡区大小的步骤中，所述过渡区的大小为：
Figure FDA0002528210040000021
其中Ls是过渡区的大小，tm为过渡时间，Ap为过渡区的加速度，
Figure FDA0002528210040000022
为余弦比例向量。
3.根据权利要求2所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述计算n个轴的所有的过渡区的最大允许过渡速度具体为，对于第i段，若所述过渡区的大小为0，则所述最大允许过渡速度为：
Figure FDA0002528210040000023
若所述过渡区的大小不为0，则所述最大允许过渡速度为：
Figure FDA0002528210040000024
若计算得到的所述最大允许过渡速度存在
Figure FDA0002528210040000025
则更新所述最大允许过渡速度为
Figure FDA0002528210040000026
并更新所述过渡区的大小为：
Figure FDA0002528210040000027
其中Vmax,i为第i段的最大速度，
Figure FDA0002528210040000028
为第i段的进给速度，1≤i≤m-1。
4.根据权利要求1所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述反向速度前瞻规划具体为：对于第i段，若vs≥vt，则保持所述过渡区的大小和所述最大允许过渡速度；若vs<vt，则计算
Figure FDA0002528210040000031
若v′t≥vt，则保持所述过渡区的大小和所述最大允许过渡速度，否则更新所述过渡区的大小为Lsn＝(1-a)Ls和更新所述最大允许过渡速度为
Figure FDA0002528210040000032
其中a为所述过渡区的减小比例，
Figure FDA0002528210040000033
Amax是最大加速度，vs是第i段的前一段的vtn，1≤i≤m-1。
5.根据权利要求1所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述正向速度规划具体为：计算临界速度为
Figure FDA0002528210040000034
若vs≥vt且vc≤VF，则将速度从vs加速到vc，再从vc减速到vt；若vs≥vt且vc>VF，则将速度从vs加速至vF并保持一段时间，再将速度从vF减速至vt；若vs≥vt且vs＝vc，将速度从vc减速至vt；若vs<vt、
Figure FDA0002528210040000035
且vc≤VF，则速度从vs加速至vt，再从vc减速至vt；若vs<vt、
Figure FDA0002528210040000036
且vc>VF，则将速度从vs提速至vF并保持一段时间，再将速度从vF减速至vt；若vs<vt、
Figure FDA0002528210040000037
且Ls＝0，将速度提升至最大允许过渡速度
Figure FDA0002528210040000038
若vs<vt、
Figure FDA0002528210040000039
且Ls≠0，则减小过渡区后，再将速度提升至最大允许过渡速度
Figure FDA00025282100400000310
6.根据权利要求1所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述过渡区的轨迹规划具体为：计算所述过渡区的速度为
Figure FDA00025282100400000311
所述过渡区的位置为
Figure FDA00025282100400000312
7.根据权利要求1所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述基于前瞻轨迹规划的实时变速方法包括实时升速和实时降速；所述实时升速具体为：更新所有段的进给速度，再重新进行所述前瞻轨迹规划；所述实时降速具体为：更新所有段的进给速度，更新首个过渡区的大小为Lsn＝aLc+Ls和首个过渡区的最大允许过渡速度为
Figure FDA0002528210040000041
再重新进行所述前瞻轨迹规划。
8.根据权利要求7所述的多轴联动实时动态前瞻轨迹规划方法，其特征在于，所述基于前瞻轨迹规划的实时停止方法具体为：对于第i段，第i段的长度为L＝Lc+Ls，计算
Figure FDA0002528210040000045
若L’≤L，则将第i段的速度降至0；若L’>L，则调整所述过渡区的大小为
Figure FDA0002528210040000043
其中
Figure FDA0002528210040000044
将速度降速至vtn；若第i段的速度vtn不为0，则调整第(i+1)段的所述过渡区的大小和速度直至该段的速度vtn为0。
9.多轴联动实时动态前瞻轨迹规划装置，其特征在于，包括：
数据输入模块，用于接收输入数据，所述输入数据包括长度为(m-1)段的n个轴的m个点位序列、每个点位的进给速度、每个轴的最大速度和每个轴的加速度，以及用于接收输入的新的点位信息；
第一规划模块，用于根据所述输入数据进行前瞻轨迹规划；
指令输入模块，用于接收输入指令；
第二规划模块，用于执行以下步骤：若所述输入指令为变速指令，执行基于所述前瞻轨迹规划的实时变速方法进行重新规划；若所述输入指令为停止指令，执行基于所述前瞻轨迹规划的实时停止方法进行重新规划；
采样模块，用于对第一段的轨迹和过渡区的轨迹进行定周期的位置采样得到插补点；
S型数字滤波器，用于根据所述插补点将梯形速度曲线转化为S型速度曲线；
数据删除模块，用于删除数据；
其中，所述前瞻轨迹规划包括：
计算n个轴的所有的过渡区的大小以及最大允许过渡速度；
令末点速度为0，从第(m-1)段起至第1段进行反向速度前瞻规划，进而调整n个轴的过渡区大小以及最大允许过渡速度；
从第1段起至第(m-1)段进行正向速度规划以及每两段之间的过渡区的轨迹规划。
10.存储介质，所述存储介质存储有可执行指令，可执行指令能被计算机执行，使所述计算机执行如权利要求1至8任一项所述的多轴联动实时动态前瞻轨迹规划方法。
CN202010510910.XA 2020-06-08 2020-06-08 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质 Active CN111796558B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010510910.XA CN111796558B (zh) 2020-06-08 2020-06-08 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010510910.XA CN111796558B (zh) 2020-06-08 2020-06-08 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN111796558A true CN111796558A (zh) 2020-10-20
CN111796558B CN111796558B (zh) 2021-06-08

Family

ID=72802961

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010510910.XA Active CN111796558B (zh) 2020-06-08 2020-06-08 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN111796558B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112518744A (zh) * 2020-11-24 2021-03-19 五邑大学 机器人轨迹规划方法、装置、机器人
CN112720472A (zh) * 2020-12-18 2021-04-30 珠海格力电器股份有限公司 一种机器人轨迹规划方法、装置、存储介质及机器人
CN113848809A (zh) * 2021-09-24 2021-12-28 中建三局集团有限公司 基于数值滤波的轨迹规划方法
CN114035513A (zh) * 2021-09-28 2022-02-11 苏州谋迅智能科技有限公司 S形速度曲线前瞻规划方法和装置、存储介质和计算设备
CN114296404A (zh) * 2021-12-18 2022-04-08 成都乐创自动化技术股份有限公司 一种计算五轴点胶机加工轨迹速度的方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101510087A (zh) * 2009-01-21 2009-08-19 西安交通大学 微小线段高速加工的前瞻自适应速度控制方法
CN106444637A (zh) * 2015-08-04 2017-02-22 深圳市雷赛智能控制股份有限公司 一种微小线段动态前瞻控制方法和装置
JP2018065241A (ja) * 2016-09-05 2018-04-26 ゼネラル・エレクトリック・カンパニイ 電気的機械加工装置および方法、ならびにハイブリッド機械加工システムおよび方法
CN108319228A (zh) * 2018-02-09 2018-07-24 苏州科技大学 一种数控系统轨迹规划中加减速控制方法
CN109426151A (zh) * 2017-09-01 2019-03-05 沈阳高精数控智能技术股份有限公司 基于自适应前瞻和预测校正的实时柔性加减速控制算法
CN109683615A (zh) * 2018-12-25 2019-04-26 上海新时达机器人有限公司 机器人连续运动的路径的速度前瞻方法及机器人控制器
US20200004228A1 (en) * 2017-09-11 2020-01-02 Dalian University Of Technology A feedrate scheduling method for five-axis dual-spline curve interpolation

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101510087A (zh) * 2009-01-21 2009-08-19 西安交通大学 微小线段高速加工的前瞻自适应速度控制方法
CN106444637A (zh) * 2015-08-04 2017-02-22 深圳市雷赛智能控制股份有限公司 一种微小线段动态前瞻控制方法和装置
JP2018065241A (ja) * 2016-09-05 2018-04-26 ゼネラル・エレクトリック・カンパニイ 電気的機械加工装置および方法、ならびにハイブリッド機械加工システムおよび方法
CN109426151A (zh) * 2017-09-01 2019-03-05 沈阳高精数控智能技术股份有限公司 基于自适应前瞻和预测校正的实时柔性加减速控制算法
US20200004228A1 (en) * 2017-09-11 2020-01-02 Dalian University Of Technology A feedrate scheduling method for five-axis dual-spline curve interpolation
CN108319228A (zh) * 2018-02-09 2018-07-24 苏州科技大学 一种数控系统轨迹规划中加减速控制方法
CN109683615A (zh) * 2018-12-25 2019-04-26 上海新时达机器人有限公司 机器人连续运动的路径的速度前瞻方法及机器人控制器

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
史中权等: "多轴联动条件下插补速度实时可调的前瞻控制算法", 《航空学报》 *
张平: "工业机器人自适应轨迹平滑过渡算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
赖永林等: "基于三次非均匀B样条曲线的机器人轨迹规划算法研究", 《科学技术与工程》 *
金永乔: "微小线段高速加工的轨迹优化建模及前瞻插补技术研究", 《中国博士学位论文全文数据库 工程科技Ⅰ辑》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112518744A (zh) * 2020-11-24 2021-03-19 五邑大学 机器人轨迹规划方法、装置、机器人
CN112720472A (zh) * 2020-12-18 2021-04-30 珠海格力电器股份有限公司 一种机器人轨迹规划方法、装置、存储介质及机器人
CN113848809A (zh) * 2021-09-24 2021-12-28 中建三局集团有限公司 基于数值滤波的轨迹规划方法
CN114035513A (zh) * 2021-09-28 2022-02-11 苏州谋迅智能科技有限公司 S形速度曲线前瞻规划方法和装置、存储介质和计算设备
CN114296404A (zh) * 2021-12-18 2022-04-08 成都乐创自动化技术股份有限公司 一种计算五轴点胶机加工轨迹速度的方法

Also Published As

Publication number Publication date
CN111796558B (zh) 2021-06-08

Similar Documents

Publication Publication Date Title
CN111796558B (zh) 多轴联动实时动态前瞻轨迹规划方法、装置及存储介质
EP2022608B1 (en) Improved blending algorithm for trajectory planning
CN111791236B (zh) 一种工业机器人笛卡尔空间轨迹过渡方法
CN110497411B (zh) 一种工业机器人协同运动控制方法
JP2604698B2 (ja) 角加速度制御方法
US8774965B2 (en) Method and device for controlling a manipulator
CN109074067B (zh) 机器人运动控制方法及相关装置
CN105500354A (zh) 一种工业机器人应用的过渡轨迹规划方法
CN113190021B (zh) 一种工业机器人小线段轨迹规划方法
EP3486612B1 (en) Method for generating a trajectory
CN109426151B (zh) 基于自适应前瞻和预测校正的实时柔性加减速控制算法
CN112486101B (zh) Nurbs曲线自适应前瞻插补方法
CN111633668B (zh) 一种用于机器人加工三维自由曲面的运动控制方法
JP2019204354A (ja) 数値制御装置
CN112965443A (zh) 一种裁床拐角轨迹跟踪高精度插值控制方法
US20030125830A1 (en) Polynomial and spline interpolation of machine tool orientations
CN111880473A (zh) 一种运动控制方法、系统、装置、机器人和存储介质
JP5461980B2 (ja) ワーク設置誤差補正手段を有する５軸加工機を制御する数値制御装置
JP2790643B2 (ja) 数値制御装置
JP2005044230A (ja) ロボット制御装置
JPH0764622A (ja) ロボットの軌道補間装置
US5327523A (en) Robot controlling method and apparatus
CN113276116B (zh) 一种误差可控的机器人轨迹同步过渡方法
JPH07210233A (ja) ロボットの軌道の補間方法と補間装置
US20210260761A1 (en) Method And Control System For Controlling An Industrial Actuator

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20231027

Address after: No. 32, Section 1, Xianren Road, Youxian District, Mianyang City, Sichuan Province, 621000 (Mianyang Vocational and Technical College University Science Park)

Patentee after: Mianyang Sibo Intelligent Technology Co.,Ltd.

Address before: No.22, Dongcheng village, Pengjiang district, Jiangmen City, Guangdong Province

Patentee before: WUYI University